Cherry-picked imuFixed differences
							parent
							
								
									19f823a1fa
								
							
						
					
					
						commit
						a881e8d3ee
					
				
							
								
								
									
										114
									
								
								.cproject
								
								
								
								
							
							
						
						
									
										114
									
								
								.cproject
								
								
								
								
							|  | @ -592,7 +592,6 @@ | |||
| 			</target> | ||||
| 			<target name="tests/testBayesTree.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments/> | ||||
| 				<buildTarget>tests/testBayesTree.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>false</useDefaultCommand> | ||||
|  | @ -600,7 +599,6 @@ | |||
| 			</target> | ||||
| 			<target name="testBinaryBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments/> | ||||
| 				<buildTarget>testBinaryBayesNet.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>false</useDefaultCommand> | ||||
|  | @ -648,7 +646,6 @@ | |||
| 			</target> | ||||
| 			<target name="testSymbolicBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments/> | ||||
| 				<buildTarget>testSymbolicBayesNet.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>false</useDefaultCommand> | ||||
|  | @ -656,7 +653,6 @@ | |||
| 			</target> | ||||
| 			<target name="tests/testSymbolicFactor.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments/> | ||||
| 				<buildTarget>tests/testSymbolicFactor.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>false</useDefaultCommand> | ||||
|  | @ -664,7 +660,6 @@ | |||
| 			</target> | ||||
| 			<target name="testSymbolicFactorGraph.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments/> | ||||
| 				<buildTarget>testSymbolicFactorGraph.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>false</useDefaultCommand> | ||||
|  | @ -680,7 +675,6 @@ | |||
| 			</target> | ||||
| 			<target name="tests/testBayesTree" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments/> | ||||
| 				<buildTarget>tests/testBayesTree</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>false</useDefaultCommand> | ||||
|  | @ -1136,7 +1130,6 @@ | |||
| 			</target> | ||||
| 			<target name="testErrors.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments/> | ||||
| 				<buildTarget>testErrors.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>false</useDefaultCommand> | ||||
|  | @ -1366,46 +1359,6 @@ | |||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="testBTree.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j5</buildArguments> | ||||
| 				<buildTarget>testBTree.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="testDSF.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j5</buildArguments> | ||||
| 				<buildTarget>testDSF.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="testDSFMap.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j5</buildArguments> | ||||
| 				<buildTarget>testDSFMap.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="testDSFVector.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j5</buildArguments> | ||||
| 				<buildTarget>testDSFVector.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="testFixedVector.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j5</buildArguments> | ||||
| 				<buildTarget>testFixedVector.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="all" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j2</buildArguments> | ||||
|  | @ -1488,6 +1441,7 @@ | |||
| 			</target> | ||||
| 			<target name="testSimulated2DOriented.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments/> | ||||
| 				<buildTarget>testSimulated2DOriented.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>false</useDefaultCommand> | ||||
|  | @ -1527,6 +1481,7 @@ | |||
| 			</target> | ||||
| 			<target name="testSimulated2D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments/> | ||||
| 				<buildTarget>testSimulated2D.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>false</useDefaultCommand> | ||||
|  | @ -1534,6 +1489,7 @@ | |||
| 			</target> | ||||
| 			<target name="testSimulated3D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments/> | ||||
| 				<buildTarget>testSimulated3D.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>false</useDefaultCommand> | ||||
|  | @ -1547,6 +1503,46 @@ | |||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="testBTree.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j5</buildArguments> | ||||
| 				<buildTarget>testBTree.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="testDSF.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j5</buildArguments> | ||||
| 				<buildTarget>testDSF.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="testDSFMap.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j5</buildArguments> | ||||
| 				<buildTarget>testDSFMap.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="testDSFVector.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j5</buildArguments> | ||||
| 				<buildTarget>testDSFVector.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="testFixedVector.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j5</buildArguments> | ||||
| 				<buildTarget>testFixedVector.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="testEliminationTree.run" path="build/gtsam/inference/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j5</buildArguments> | ||||
|  | @ -1804,7 +1800,6 @@ | |||
| 			</target> | ||||
| 			<target name="Generate DEB Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>cpack</buildCommand> | ||||
| 				<buildArguments/> | ||||
| 				<buildTarget>-G DEB</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>false</useDefaultCommand> | ||||
|  | @ -1812,7 +1807,6 @@ | |||
| 			</target> | ||||
| 			<target name="Generate RPM Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>cpack</buildCommand> | ||||
| 				<buildArguments/> | ||||
| 				<buildTarget>-G RPM</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>false</useDefaultCommand> | ||||
|  | @ -1820,7 +1814,6 @@ | |||
| 			</target> | ||||
| 			<target name="Generate TGZ Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>cpack</buildCommand> | ||||
| 				<buildArguments/> | ||||
| 				<buildTarget>-G TGZ</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>false</useDefaultCommand> | ||||
|  | @ -1828,7 +1821,6 @@ | |||
| 			</target> | ||||
| 			<target name="Generate TGZ Source Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>cpack</buildCommand> | ||||
| 				<buildArguments/> | ||||
| 				<buildTarget>--config CPackSourceConfig.cmake</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>false</useDefaultCommand> | ||||
|  | @ -2002,6 +1994,14 @@ | |||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="check.navigation" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j2 VERBOSE=1</buildArguments> | ||||
| 				<buildTarget>check.navigation</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>false</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="check" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j2</buildArguments> | ||||
|  | @ -2683,7 +2683,6 @@ | |||
| 			</target> | ||||
| 			<target name="testGraph.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments/> | ||||
| 				<buildTarget>testGraph.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>false</useDefaultCommand> | ||||
|  | @ -2691,7 +2690,6 @@ | |||
| 			</target> | ||||
| 			<target name="testJunctionTree.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments/> | ||||
| 				<buildTarget>testJunctionTree.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>false</useDefaultCommand> | ||||
|  | @ -2699,7 +2697,6 @@ | |||
| 			</target> | ||||
| 			<target name="testSymbolicBayesNetB.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments/> | ||||
| 				<buildTarget>testSymbolicBayesNetB.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>false</useDefaultCommand> | ||||
|  | @ -2809,14 +2806,6 @@ | |||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="testBasisDecompositions.run" path="build/gtsam_unstable/nonlinear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j4</buildArguments> | ||||
| 				<buildTarget>testBasisDecompositions.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="testCustomChartExpression.run" path="build/gtsam_unstable/nonlinear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j4</buildArguments> | ||||
|  | @ -3251,6 +3240,7 @@ | |||
| 			</target> | ||||
| 			<target name="tests/testGaussianISAM2" path="build/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments/> | ||||
| 				<buildTarget>tests/testGaussianISAM2</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>false</useDefaultCommand> | ||||
|  |  | |||
							
								
								
									
										126
									
								
								gtsam.h
								
								
								
								
							
							
						
						
									
										126
									
								
								gtsam.h
								
								
								
								
							|  | @ -2404,25 +2404,24 @@ class ConstantBias { | |||
| }///\namespace imuBias
 | ||||
| 
 | ||||
| #include <gtsam/navigation/ImuFactor.h> | ||||
| class PoseVelocity{ | ||||
|     PoseVelocity(const gtsam::Pose3& pose, Vector velocity); | ||||
| class PoseVelocityBias{ | ||||
|     PoseVelocityBias(const gtsam::Pose3& pose, Vector velocity, const gtsam::imuBias::ConstantBias& bias); | ||||
|   }; | ||||
| class ImuFactorPreintegratedMeasurements { | ||||
|   // Standard Constructor
 | ||||
|   ImuFactorPreintegratedMeasurements(const gtsam::imuBias::ConstantBias& bias, Matrix measuredAccCovariance,Matrix measuredOmegaCovariance, Matrix integrationErrorCovariance, bool use2ndOrderIntegration); | ||||
|   ImuFactorPreintegratedMeasurements(const gtsam::imuBias::ConstantBias& bias, Matrix measuredAccCovariance,Matrix measuredOmegaCovariance, Matrix integrationErrorCovariance); | ||||
|   ImuFactorPreintegratedMeasurements(const gtsam::ImuFactorPreintegratedMeasurements& rhs); | ||||
|   // ImuFactorPreintegratedMeasurements(const gtsam::ImuFactorPreintegratedMeasurements& rhs);
 | ||||
| 
 | ||||
|   // Testable
 | ||||
|   void print(string s) const; | ||||
|   bool equals(const gtsam::ImuFactorPreintegratedMeasurements& expected, double tol); | ||||
| 
 | ||||
|   Matrix measurementCovariance() const; | ||||
|   Matrix deltaRij() const; | ||||
|   double deltaTij() const; | ||||
|   Matrix deltaRij() const; | ||||
|   Vector deltaPij() const; | ||||
|   Vector deltaVij() const; | ||||
|   Vector biasHat() const; | ||||
|   Vector biasHatVector() const; | ||||
|   Matrix delPdelBiasAcc() const; | ||||
|   Matrix delPdelBiasOmega() const; | ||||
|   Matrix delVdelBiasAcc() const; | ||||
|  | @ -2430,10 +2429,11 @@ class ImuFactorPreintegratedMeasurements { | |||
|   Matrix delRdelBiasOmega() const; | ||||
|   Matrix preintMeasCov() const; | ||||
| 
 | ||||
| 
 | ||||
|   // Standard Interface
 | ||||
|   void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT); | ||||
|   void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT, const gtsam::Pose3& body_P_sensor); | ||||
|   gtsam::PoseVelocityBias predict(const gtsam::Pose3& pose_i, Vector vel_i, const gtsam::imuBias::ConstantBias& bias, | ||||
|       Vector gravity, Vector omegaCoriolis) const; | ||||
| }; | ||||
| 
 | ||||
| virtual class ImuFactor : gtsam::NonlinearFactor { | ||||
|  | @ -2444,11 +2444,60 @@ virtual class ImuFactor : gtsam::NonlinearFactor { | |||
|       const gtsam::Pose3& body_P_sensor); | ||||
|   // Standard Interface
 | ||||
|   gtsam::ImuFactorPreintegratedMeasurements preintegratedMeasurements() const; | ||||
|   gtsam::PoseVelocity Predict(const gtsam::Pose3& pose_i, Vector vel_i, const gtsam::imuBias::ConstantBias& bias, | ||||
|       const gtsam::ImuFactorPreintegratedMeasurements& preintegratedMeasurements, | ||||
| }; | ||||
| 
 | ||||
| #include <gtsam/navigation/CombinedImuFactor.h> | ||||
| class CombinedImuFactorPreintegratedMeasurements { | ||||
|   // Standard Constructor
 | ||||
|   CombinedImuFactorPreintegratedMeasurements( | ||||
|       const gtsam::imuBias::ConstantBias& bias, | ||||
|       Matrix measuredAccCovariance, | ||||
|       Matrix measuredOmegaCovariance, | ||||
|       Matrix integrationErrorCovariance, | ||||
|       Matrix biasAccCovariance, | ||||
|       Matrix biasOmegaCovariance, | ||||
|       Matrix biasAccOmegaInit); | ||||
|   CombinedImuFactorPreintegratedMeasurements( | ||||
|       const gtsam::imuBias::ConstantBias& bias, | ||||
|       Matrix measuredAccCovariance, | ||||
|       Matrix measuredOmegaCovariance, | ||||
|       Matrix integrationErrorCovariance, | ||||
|       Matrix biasAccCovariance, | ||||
|       Matrix biasOmegaCovariance, | ||||
|       Matrix biasAccOmegaInit, | ||||
|       bool use2ndOrderIntegration); | ||||
|   // CombinedImuFactorPreintegratedMeasurements(const gtsam::CombinedImuFactorPreintegratedMeasurements& rhs);
 | ||||
| 
 | ||||
|   // Testable
 | ||||
|   void print(string s) const; | ||||
|   bool equals(const gtsam::CombinedImuFactorPreintegratedMeasurements& expected, double tol); | ||||
| 
 | ||||
|   double deltaTij() const; | ||||
|   Matrix deltaRij() const; | ||||
|   Vector deltaPij() const; | ||||
|   Vector deltaVij() const; | ||||
|   Vector biasHatVector() const; | ||||
|   Matrix delPdelBiasAcc() const; | ||||
|   Matrix delPdelBiasOmega() const; | ||||
|   Matrix delVdelBiasAcc() const; | ||||
|   Matrix delVdelBiasOmega() const; | ||||
|   Matrix delRdelBiasOmega() const; | ||||
|   Matrix preintMeasCov() const; | ||||
| 
 | ||||
|   // Standard Interface
 | ||||
|   void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT); | ||||
|   void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT, const gtsam::Pose3& body_P_sensor); | ||||
|   gtsam::PoseVelocityBias predict(const gtsam::Pose3& pose_i, Vector vel_i, const gtsam::imuBias::ConstantBias& bias, | ||||
|       Vector gravity, Vector omegaCoriolis) const; | ||||
| }; | ||||
| 
 | ||||
| virtual class CombinedImuFactor : gtsam::NonlinearFactor { | ||||
|   CombinedImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, size_t bias_i, size_t bias_j, | ||||
|       const gtsam::CombinedImuFactorPreintegratedMeasurements& CombinedPreintegratedMeasurements, Vector gravity, Vector omegaCoriolis); | ||||
|   // Standard Interface
 | ||||
|   gtsam::CombinedImuFactorPreintegratedMeasurements preintegratedMeasurements() const; | ||||
| }; | ||||
| 
 | ||||
| #include <gtsam/navigation/AHRSFactor.h> | ||||
| class AHRSFactorPreintegratedMeasurements { | ||||
|   // Standard Constructor
 | ||||
|  | @ -2461,7 +2510,6 @@ class AHRSFactorPreintegratedMeasurements { | |||
|   bool equals(const gtsam::AHRSFactorPreintegratedMeasurements& expected, double tol); | ||||
| 
 | ||||
|   // get Data
 | ||||
|   Matrix measurementCovariance() const; | ||||
|   Matrix deltaRij() const; | ||||
|   double deltaTij() const; | ||||
|   Vector biasHat() const; | ||||
|  | @ -2488,64 +2536,6 @@ virtual class AHRSFactor : gtsam::NonlinearFactor { | |||
|       Vector omegaCoriolis) const; | ||||
| }; | ||||
| 
 | ||||
| #include <gtsam/navigation/CombinedImuFactor.h> | ||||
| class PoseVelocityBias{ | ||||
|     PoseVelocityBias(const gtsam::Pose3& pose, Vector velocity, const gtsam::imuBias::ConstantBias& bias); | ||||
|   }; | ||||
| class CombinedImuFactorPreintegratedMeasurements { | ||||
|   // Standard Constructor
 | ||||
|   CombinedImuFactorPreintegratedMeasurements( | ||||
|       const gtsam::imuBias::ConstantBias& bias, | ||||
|       Matrix measuredAccCovariance, | ||||
|       Matrix measuredOmegaCovariance, | ||||
|       Matrix integrationErrorCovariance, | ||||
|       Matrix biasAccCovariance, | ||||
|       Matrix biasOmegaCovariance, | ||||
|       Matrix biasAccOmegaInit); | ||||
|   CombinedImuFactorPreintegratedMeasurements( | ||||
|       const gtsam::imuBias::ConstantBias& bias, | ||||
|       Matrix measuredAccCovariance, | ||||
|       Matrix measuredOmegaCovariance, | ||||
|       Matrix integrationErrorCovariance, | ||||
|       Matrix biasAccCovariance, | ||||
|       Matrix biasOmegaCovariance, | ||||
|       Matrix biasAccOmegaInit, | ||||
|       bool use2ndOrderIntegration); | ||||
|   CombinedImuFactorPreintegratedMeasurements(const gtsam::CombinedImuFactorPreintegratedMeasurements& rhs); | ||||
| 
 | ||||
|   // Testable
 | ||||
|   void print(string s) const; | ||||
|   bool equals(const gtsam::CombinedImuFactorPreintegratedMeasurements& expected, double tol); | ||||
| 
 | ||||
|   // Standard Interface
 | ||||
|   void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT); | ||||
|   void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT, const gtsam::Pose3& body_P_sensor); | ||||
| 
 | ||||
|   Matrix measurementCovariance() const; | ||||
|   Matrix deltaRij() const; | ||||
|   double deltaTij() const; | ||||
|   Vector deltaPij() const; | ||||
|   Vector deltaVij() const; | ||||
|   Vector biasHat() const; | ||||
|   Matrix delPdelBiasAcc() const; | ||||
|   Matrix delPdelBiasOmega() const; | ||||
|   Matrix delVdelBiasAcc() const; | ||||
|   Matrix delVdelBiasOmega() const; | ||||
|   Matrix delRdelBiasOmega() const; | ||||
|   Matrix PreintMeasCov() const; | ||||
| }; | ||||
| 
 | ||||
| virtual class CombinedImuFactor : gtsam::NonlinearFactor { | ||||
|   CombinedImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, size_t bias_i, size_t bias_j, | ||||
|       const gtsam::CombinedImuFactorPreintegratedMeasurements& CombinedPreintegratedMeasurements, Vector gravity, Vector omegaCoriolis); | ||||
| 
 | ||||
|   // Standard Interface
 | ||||
|   gtsam::CombinedImuFactorPreintegratedMeasurements preintegratedMeasurements() const; | ||||
|   gtsam::PoseVelocityBias Predict(const gtsam::Pose3& pose_i, Vector vel_i, const gtsam::imuBias::ConstantBias& bias_i, | ||||
|       const gtsam::CombinedImuFactorPreintegratedMeasurements& preintegratedMeasurements, | ||||
|       Vector gravity, Vector omegaCoriolis); | ||||
| }; | ||||
| 
 | ||||
| #include <gtsam/navigation/AttitudeFactor.h> | ||||
| //virtual class AttitudeFactor : gtsam::NonlinearFactor {
 | ||||
| //  AttitudeFactor(const Unit3& nZ, const Unit3& bRef);
 | ||||
|  |  | |||
|  | @ -18,9 +18,9 @@ | |||
|  **/ | ||||
| 
 | ||||
| #include <gtsam/navigation/AHRSFactor.h> | ||||
| #include <iostream> | ||||
| 
 | ||||
| /* External or standard includes */ | ||||
| #include <ostream> | ||||
| using namespace std; | ||||
| 
 | ||||
| namespace gtsam { | ||||
| 
 | ||||
|  | @ -29,47 +29,35 @@ namespace gtsam { | |||
| //------------------------------------------------------------------------------
 | ||||
| AHRSFactor::PreintegratedMeasurements::PreintegratedMeasurements( | ||||
|     const Vector3& bias, const Matrix3& measuredOmegaCovariance) : | ||||
|     biasHat_(bias), deltaTij_(0.0) { | ||||
|   measurementCovariance_ << measuredOmegaCovariance; | ||||
|   delRdelBiasOmega_.setZero(); | ||||
|     PreintegratedRotation(measuredOmegaCovariance), biasHat_(bias) | ||||
| { | ||||
|   preintMeasCov_.setZero(); | ||||
| } | ||||
| 
 | ||||
| //------------------------------------------------------------------------------
 | ||||
| AHRSFactor::PreintegratedMeasurements::PreintegratedMeasurements() : | ||||
|     biasHat_(Vector3()), deltaTij_(0.0) { | ||||
|   measurementCovariance_.setZero(); | ||||
|   delRdelBiasOmega_.setZero(); | ||||
|   delRdelBiasOmega_.setZero(); | ||||
|     PreintegratedRotation(I_3x3), biasHat_(Vector3()) | ||||
| { | ||||
|   preintMeasCov_.setZero(); | ||||
| } | ||||
| 
 | ||||
| //------------------------------------------------------------------------------
 | ||||
| void AHRSFactor::PreintegratedMeasurements::print(const std::string& s) const { | ||||
|   std::cout << s << std::endl; | ||||
|   std::cout << "biasHat [" << biasHat_.transpose() << "]" << std::endl; | ||||
|   deltaRij_.print(" deltaRij "); | ||||
|   std::cout << " measurementCovariance [" << measurementCovariance_ << " ]" | ||||
|       << std::endl; | ||||
|   std::cout << " PreintMeasCov [ " << preintMeasCov_ << " ]" << std::endl; | ||||
| void AHRSFactor::PreintegratedMeasurements::print(const string& s) const { | ||||
|   PreintegratedRotation::print(s); | ||||
|   cout << "biasHat [" << biasHat_.transpose() << "]" << endl; | ||||
|   cout << " PreintMeasCov [ " << preintMeasCov_ << " ]" << endl; | ||||
| } | ||||
| 
 | ||||
| //------------------------------------------------------------------------------
 | ||||
| bool AHRSFactor::PreintegratedMeasurements::equals( | ||||
|     const PreintegratedMeasurements& other, double tol) const { | ||||
|   return equal_with_abs_tol(biasHat_, other.biasHat_, tol) | ||||
|       && equal_with_abs_tol(measurementCovariance_, | ||||
|           other.measurementCovariance_, tol) | ||||
|       && deltaRij_.equals(other.deltaRij_, tol) | ||||
|       && std::fabs(deltaTij_ - other.deltaTij_) < tol | ||||
|       && equal_with_abs_tol(delRdelBiasOmega_, other.delRdelBiasOmega_, tol); | ||||
|   return PreintegratedRotation::equals(other, tol) | ||||
|       && equal_with_abs_tol(biasHat_, other.biasHat_, tol); | ||||
| } | ||||
| 
 | ||||
| //------------------------------------------------------------------------------
 | ||||
| void AHRSFactor::PreintegratedMeasurements::resetIntegration() { | ||||
|   deltaRij_ = Rot3(); | ||||
|   deltaTij_ = 0.0; | ||||
|   delRdelBiasOmega_.setZero(); | ||||
|   PreintegratedRotation::resetIntegration(); | ||||
|   preintMeasCov_.setZero(); | ||||
| } | ||||
| 
 | ||||
|  | @ -78,7 +66,6 @@ void AHRSFactor::PreintegratedMeasurements::integrateMeasurement( | |||
|     const Vector3& measuredOmega, double deltaT, | ||||
|     boost::optional<const Pose3&> body_P_sensor) { | ||||
| 
 | ||||
|   // NOTE: order is important here because each update uses old values.
 | ||||
|   // First we compensate the measurements for the bias
 | ||||
|   Vector3 correctedOmega = measuredOmega - biasHat_; | ||||
| 
 | ||||
|  | @ -93,64 +80,27 @@ void AHRSFactor::PreintegratedMeasurements::integrateMeasurement( | |||
|   // rotation vector describing rotation increment computed from the
 | ||||
|   // current rotation rate measurement
 | ||||
|   const Vector3 theta_incr = correctedOmega * deltaT; | ||||
|   Matrix3 D_Rincr_integratedOmega; | ||||
|   const Rot3 incrR = Rot3::Expmap(theta_incr, D_Rincr_integratedOmega); // expensive !!
 | ||||
| 
 | ||||
|   // rotation increment computed from the current rotation rate measurement
 | ||||
|   const Rot3 incrR = Rot3::Expmap(theta_incr); | ||||
|   const Matrix3 incrRt = incrR.transpose(); | ||||
|   // Update Jacobian
 | ||||
|   update_delRdelBiasOmega(D_Rincr_integratedOmega, incrR, deltaT); | ||||
| 
 | ||||
|   // Right Jacobian computed at theta_incr
 | ||||
|   const Matrix3 Jr_theta_incr = Rot3::ExpmapDerivative(theta_incr); | ||||
| 
 | ||||
|   // Update Jacobians
 | ||||
|   // ---------------------------------------------------------------------------
 | ||||
|   delRdelBiasOmega_ = incrRt * delRdelBiasOmega_ - Jr_theta_incr * deltaT; | ||||
| 
 | ||||
|   // Update preintegrated measurements covariance
 | ||||
|   // ---------------------------------------------------------------------------
 | ||||
|   const Vector3 theta_i = Rot3::Logmap(deltaRij_); // Parameterization of so(3)
 | ||||
|   const Matrix3 Jr_theta_i = Rot3::LogmapDerivative(theta_i); | ||||
| 
 | ||||
|   Rot3 Rot_j = deltaRij_ * incrR; | ||||
|   const Vector3 theta_j = Rot3::Logmap(Rot_j); // Parameterization of so(3)
 | ||||
|   const Matrix3 Jrinv_theta_j = Rot3::LogmapDerivative(theta_j); | ||||
| 
 | ||||
|   // Update preintegrated measurements covariance: as in [2] we consider a first
 | ||||
|   // order propagation that can be seen as a prediction phase in an EKF framework
 | ||||
|   Matrix3 H_angles_angles = Jrinv_theta_j * incrRt * Jr_theta_i; | ||||
|   // analytic expression corresponding to the following numerical derivative
 | ||||
|   // Matrix H_angles_angles = numericalDerivative11<LieVector, LieVector>
 | ||||
|   // (boost::bind(&DeltaAngles, correctedOmega, deltaT, _1), thetaij);
 | ||||
| 
 | ||||
|   // overall Jacobian wrpt preintegrated measurements (df/dx)
 | ||||
|   const Matrix3& F = H_angles_angles; | ||||
|   // Update rotation and deltaTij.
 | ||||
|   Matrix3 Fr; // Jacobian of the update
 | ||||
|   updateIntegratedRotationAndDeltaT(incrR, deltaT, Fr); | ||||
| 
 | ||||
|   // first order uncertainty propagation
 | ||||
|   // the deltaT allows to pass from continuous time noise to discrete time noise
 | ||||
|   preintMeasCov_ = F * preintMeasCov_ * F.transpose() | ||||
|       + measurementCovariance_ * deltaT; | ||||
| 
 | ||||
|   // Update preintegrated measurements
 | ||||
|   // ---------------------------------------------------------------------------
 | ||||
|   deltaRij_ = deltaRij_ * incrR; | ||||
|   deltaTij_ += deltaT; | ||||
|   preintMeasCov_ = Fr * preintMeasCov_ * Fr.transpose() | ||||
|       + gyroscopeCovariance() * deltaT; | ||||
| } | ||||
| 
 | ||||
| //------------------------------------------------------------------------------
 | ||||
| Vector3 AHRSFactor::PreintegratedMeasurements::predict(const Vector3& bias, | ||||
|     boost::optional<Matrix&> H) const { | ||||
|   const Vector3 biasOmegaIncr = bias - biasHat_; | ||||
|   Vector3 delRdelBiasOmega_biasOmegaIncr = delRdelBiasOmega_ * biasOmegaIncr; | ||||
|   const Rot3 deltaRij_biascorrected = deltaRij_.retract( | ||||
|       delRdelBiasOmega_biasOmegaIncr, Rot3::EXPMAP); | ||||
|   const Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected); | ||||
|   if (H) { | ||||
|     const Matrix3 Jrinv_theta_bc = //
 | ||||
|         Rot3::LogmapDerivative(theta_biascorrected); | ||||
|     const Matrix3 Jr_JbiasOmegaIncr = //
 | ||||
|         Rot3::ExpmapDerivative(delRdelBiasOmega_biasOmegaIncr); | ||||
|     (*H) = Jrinv_theta_bc * Jr_JbiasOmegaIncr * delRdelBiasOmega_; | ||||
|   } | ||||
|   return theta_biascorrected; | ||||
|   return biascorrectedThetaRij(biasOmegaIncr, H); | ||||
| } | ||||
| //------------------------------------------------------------------------------
 | ||||
| Vector AHRSFactor::PreintegratedMeasurements::DeltaAngles( | ||||
|  | @ -172,7 +122,7 @@ Vector AHRSFactor::PreintegratedMeasurements::DeltaAngles( | |||
| // AHRSFactor methods
 | ||||
| //------------------------------------------------------------------------------
 | ||||
| AHRSFactor::AHRSFactor() : | ||||
|     preintegratedMeasurements_(Vector3(), Matrix3::Zero()) { | ||||
|     _PIM_(Vector3(), Z_3x3) { | ||||
| } | ||||
| 
 | ||||
| AHRSFactor::AHRSFactor(Key rot_i, Key rot_j, Key bias, | ||||
|  | @ -180,7 +130,7 @@ AHRSFactor::AHRSFactor(Key rot_i, Key rot_j, Key bias, | |||
|     const Vector3& omegaCoriolis, boost::optional<const Pose3&> body_P_sensor) : | ||||
|     Base( | ||||
|         noiseModel::Gaussian::Covariance( | ||||
|             preintegratedMeasurements.preintMeasCov_), rot_i, rot_j, bias), preintegratedMeasurements_( | ||||
|             preintegratedMeasurements.preintMeasCov_), rot_i, rot_j, bias), _PIM_( | ||||
|         preintegratedMeasurements), omegaCoriolis_(omegaCoriolis), body_P_sensor_( | ||||
|         body_P_sensor) { | ||||
| } | ||||
|  | @ -192,13 +142,12 @@ gtsam::NonlinearFactor::shared_ptr AHRSFactor::clone() const { | |||
| } | ||||
| 
 | ||||
| //------------------------------------------------------------------------------
 | ||||
| void AHRSFactor::print(const std::string& s, | ||||
| void AHRSFactor::print(const string& s, | ||||
|     const KeyFormatter& keyFormatter) const { | ||||
|   std::cout << s << "AHRSFactor(" << keyFormatter(this->key1()) << "," | ||||
|   cout << s << "AHRSFactor(" << keyFormatter(this->key1()) << "," | ||||
|       << keyFormatter(this->key2()) << "," << keyFormatter(this->key3()) << ","; | ||||
|   preintegratedMeasurements_.print("  preintegrated measurements:"); | ||||
|   std::cout << "  omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]" | ||||
|       << std::endl; | ||||
|   _PIM_.print("  preintegrated measurements:"); | ||||
|   cout << "  omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]" << endl; | ||||
|   noiseModel_->print("  noise model: "); | ||||
|   if (body_P_sensor_) | ||||
|     body_P_sensor_->print("  sensor pose in body frame: "); | ||||
|  | @ -207,8 +156,7 @@ void AHRSFactor::print(const std::string& s, | |||
| //------------------------------------------------------------------------------
 | ||||
| bool AHRSFactor::equals(const NonlinearFactor& other, double tol) const { | ||||
|   const This *e = dynamic_cast<const This*>(&other); | ||||
|   return e != NULL && Base::equals(*e, tol) | ||||
|       && preintegratedMeasurements_.equals(e->preintegratedMeasurements_, tol) | ||||
|   return e != NULL && Base::equals(*e, tol) && _PIM_.equals(e->_PIM_, tol) | ||||
|       && equal_with_abs_tol(omegaCoriolis_, e->omegaCoriolis_, tol) | ||||
|       && ((!body_P_sensor_ && !e->body_P_sensor_) | ||||
|           || (body_P_sensor_ && e->body_P_sensor_ | ||||
|  | @ -216,50 +164,49 @@ bool AHRSFactor::equals(const NonlinearFactor& other, double tol) const { | |||
| } | ||||
| 
 | ||||
| //------------------------------------------------------------------------------
 | ||||
| Vector AHRSFactor::evaluateError(const Rot3& rot_i, const Rot3& rot_j, | ||||
| Vector AHRSFactor::evaluateError(const Rot3& Ri, const Rot3& Rj, | ||||
|     const Vector3& bias, boost::optional<Matrix&> H1, | ||||
|     boost::optional<Matrix&> H2, boost::optional<Matrix&> H3) const { | ||||
| 
 | ||||
|   // Do bias correction, if (H3) will contain 3*3 derivative used below
 | ||||
|   const Vector3 theta_biascorrected = //
 | ||||
|       preintegratedMeasurements_.predict(bias, H3); | ||||
|   const Vector3 biascorrectedOmega = _PIM_.predict(bias, H3); | ||||
| 
 | ||||
|   // Coriolis term
 | ||||
|   const Vector3 coriolis = //
 | ||||
|       preintegratedMeasurements_.integrateCoriolis(rot_i, omegaCoriolis_); | ||||
|   const Vector3 theta_corrected = theta_biascorrected - coriolis; | ||||
|   const Vector3 coriolis = _PIM_.integrateCoriolis(Ri, omegaCoriolis_); | ||||
|   const Matrix3 coriolisHat = skewSymmetric(coriolis); | ||||
|   const Vector3 correctedOmega = biascorrectedOmega - coriolis; | ||||
| 
 | ||||
|   // Prediction
 | ||||
|   const Rot3 deltaRij_corrected = Rot3::Expmap(theta_corrected); | ||||
|   const Rot3 correctedDeltaRij = Rot3::Expmap(correctedOmega); | ||||
| 
 | ||||
|   // Get error between actual and prediction
 | ||||
|   const Rot3 actualRij = rot_i.between(rot_j); | ||||
|   const Rot3 fRhat = deltaRij_corrected.between(actualRij); | ||||
|   Vector3 fR = Rot3::Logmap(fRhat); | ||||
|   const Rot3 actualRij = Ri.between(Rj); | ||||
|   const Rot3 fRrot = correctedDeltaRij.between(actualRij); | ||||
|   Vector3 fR = Rot3::Logmap(fRrot); | ||||
| 
 | ||||
|   // Terms common to derivatives
 | ||||
|   const Matrix3 Jr_theta_bcc = Rot3::ExpmapDerivative(theta_corrected); | ||||
|   const Matrix3 Jrinv_fRhat = Rot3::LogmapDerivative(fR); | ||||
|   const Matrix3 D_cDeltaRij_cOmega = Rot3::ExpmapDerivative(correctedOmega); | ||||
|   const Matrix3 D_fR_fRrot = Rot3::LogmapDerivative(fR); | ||||
| 
 | ||||
|   if (H1) { | ||||
|     // dfR/dRi
 | ||||
|     H1->resize(3, 3); | ||||
|     Matrix3 Jtheta = -Jr_theta_bcc * skewSymmetric(coriolis); | ||||
|     Matrix3 D_coriolis = -D_cDeltaRij_cOmega * coriolisHat; | ||||
|     (*H1) | ||||
|         << Jrinv_fRhat * (-actualRij.transpose() - fRhat.transpose() * Jtheta); | ||||
|         << D_fR_fRrot * (-actualRij.transpose() - fRrot.transpose() * D_coriolis); | ||||
|   } | ||||
| 
 | ||||
|   if (H2) { | ||||
|     // dfR/dPosej
 | ||||
|     H2->resize(3, 3); | ||||
|     (*H2) << Jrinv_fRhat * Matrix3::Identity(); | ||||
|     (*H2) << D_fR_fRrot * Matrix3::Identity(); | ||||
|   } | ||||
| 
 | ||||
|   if (H3) { | ||||
|     // dfR/dBias, note H3 contains derivative of predict
 | ||||
|     const Matrix3 JbiasOmega = Jr_theta_bcc * (*H3); | ||||
|     const Matrix3 JbiasOmega = D_cDeltaRij_cOmega * (*H3); | ||||
|     H3->resize(3, 3); | ||||
|     (*H3) << Jrinv_fRhat * (-fRhat.transpose() * JbiasOmega); | ||||
|     (*H3) << D_fR_fRrot * (-fRrot.transpose() * JbiasOmega); | ||||
|   } | ||||
| 
 | ||||
|   Vector error(3); | ||||
|  | @ -272,16 +219,16 @@ Rot3 AHRSFactor::predict(const Rot3& rot_i, const Vector3& bias, | |||
|     const PreintegratedMeasurements preintegratedMeasurements, | ||||
|     const Vector3& omegaCoriolis, boost::optional<const Pose3&> body_P_sensor) { | ||||
| 
 | ||||
|   const Vector3 theta_biascorrected = preintegratedMeasurements.predict(bias); | ||||
|   const Vector3 biascorrectedOmega = preintegratedMeasurements.predict(bias); | ||||
| 
 | ||||
|   // Coriolis term
 | ||||
|   const Vector3 coriolis = //
 | ||||
|       preintegratedMeasurements.integrateCoriolis(rot_i, omegaCoriolis); | ||||
| 
 | ||||
|   const Vector3 theta_corrected = theta_biascorrected - coriolis; | ||||
|   const Rot3 deltaRij_corrected = Rot3::Expmap(theta_corrected); | ||||
|   const Vector3 correctedOmega = biascorrectedOmega - coriolis; | ||||
|   const Rot3 correctedDeltaRij = Rot3::Expmap(correctedOmega); | ||||
| 
 | ||||
|   return rot_i.compose(deltaRij_corrected); | ||||
|   return rot_i.compose(correctedDeltaRij); | ||||
| } | ||||
| 
 | ||||
| } //namespace gtsam
 | ||||
|  |  | |||
|  | @ -20,6 +20,7 @@ | |||
| #pragma once | ||||
| 
 | ||||
| /* GTSAM includes */ | ||||
| #include <gtsam/navigation/PreintegratedRotation.h> | ||||
| #include <gtsam/nonlinear/NonlinearFactor.h> | ||||
| #include <gtsam/geometry/Pose3.h> | ||||
| 
 | ||||
|  | @ -35,17 +36,12 @@ public: | |||
|    * Can be built incrementally so as to avoid costly integration at time of | ||||
|    * factor construction. | ||||
|    */ | ||||
|   class PreintegratedMeasurements { | ||||
|   class PreintegratedMeasurements : public PreintegratedRotation { | ||||
| 
 | ||||
|     friend class AHRSFactor; | ||||
| 
 | ||||
|   protected: | ||||
|     Vector3 biasHat_; ///< Acceleration and angular rate bias values used during preintegration. Note that we won't be using the accelerometer
 | ||||
|     Matrix3 measurementCovariance_; ///< (Raw measurements uncertainty) Covariance of the vector [measuredOmega] in R^(3X3)
 | ||||
| 
 | ||||
|     Rot3 deltaRij_; ///< Preintegrated relative orientation (in frame i)
 | ||||
|     double deltaTij_; ///< Time interval from i to j
 | ||||
|     Matrix3 delRdelBiasOmega_; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias
 | ||||
|     Matrix3 preintMeasCov_; ///< Covariance matrix of the preintegrated measurements (first-order propagation from *measurementCovariance*)
 | ||||
| 
 | ||||
|   public: | ||||
|  | @ -61,31 +57,19 @@ public: | |||
|     PreintegratedMeasurements(const Vector3& bias, | ||||
|         const Matrix3& measuredOmegaCovariance); | ||||
| 
 | ||||
|     Vector3 biasHat() const { | ||||
|       return biasHat_; | ||||
|     } | ||||
|     const Matrix3& preintMeasCov() const { | ||||
|       return preintMeasCov_; | ||||
|     } | ||||
| 
 | ||||
|     /// print
 | ||||
|     void print(const std::string& s = "Preintegrated Measurements: ") const; | ||||
| 
 | ||||
|     /// equals
 | ||||
|     bool equals(const PreintegratedMeasurements&, double tol = 1e-9) const; | ||||
| 
 | ||||
|     const Matrix3& measurementCovariance() const { | ||||
|       return measurementCovariance_; | ||||
|     } | ||||
|     Matrix3 deltaRij() const { | ||||
|       return deltaRij_.matrix(); | ||||
|     } | ||||
|     double deltaTij() const { | ||||
|       return deltaTij_; | ||||
|     } | ||||
|     Vector3 biasHat() const { | ||||
|       return biasHat_; | ||||
|     } | ||||
|     const Matrix3& delRdelBiasOmega() const { | ||||
|       return delRdelBiasOmega_; | ||||
|     } | ||||
|     const Matrix3& preintMeasCov() const { | ||||
|       return preintMeasCov_; | ||||
|     } | ||||
| 
 | ||||
|     /// TODO: Document
 | ||||
|     void resetIntegration(); | ||||
| 
 | ||||
|  | @ -103,12 +87,6 @@ public: | |||
|     Vector3 predict(const Vector3& bias, boost::optional<Matrix&> H = | ||||
|         boost::none) const; | ||||
| 
 | ||||
|     /// Integrate coriolis correction in body frame rot_i
 | ||||
|     Vector3 integrateCoriolis(const Rot3& rot_i, | ||||
|         const Vector3& omegaCoriolis) const { | ||||
|       return rot_i.transpose() * omegaCoriolis * deltaTij_; | ||||
|     } | ||||
| 
 | ||||
|     // This function is only used for test purposes
 | ||||
|     // (compare numerical derivatives wrt analytic ones)
 | ||||
|     static Vector DeltaAngles(const Vector& msr_gyro_t, const double msr_dt, | ||||
|  | @ -120,11 +98,8 @@ public: | |||
|     friend class boost::serialization::access; | ||||
|     template<class ARCHIVE> | ||||
|     void serialize(ARCHIVE & ar, const unsigned int version) { | ||||
|       ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegratedRotation); | ||||
|       ar & BOOST_SERIALIZATION_NVP(biasHat_); | ||||
|       ar & BOOST_SERIALIZATION_NVP(measurementCovariance_); | ||||
|       ar & BOOST_SERIALIZATION_NVP(deltaRij_); | ||||
|       ar & BOOST_SERIALIZATION_NVP(deltaTij_); | ||||
|       ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega_); | ||||
|     } | ||||
|   }; | ||||
| 
 | ||||
|  | @ -132,7 +107,7 @@ private: | |||
|   typedef AHRSFactor This; | ||||
|   typedef NoiseModelFactor3<Rot3, Rot3, Vector3> Base; | ||||
| 
 | ||||
|   PreintegratedMeasurements preintegratedMeasurements_; | ||||
|   PreintegratedMeasurements _PIM_; | ||||
|   Vector3 gravity_; | ||||
|   Vector3 omegaCoriolis_; ///< Controls whether higher order terms are included when calculating the Coriolis Effect
 | ||||
|   boost::optional<Pose3> body_P_sensor_; ///< The pose of the sensor in the body frame
 | ||||
|  | @ -178,7 +153,7 @@ public: | |||
| 
 | ||||
|   /// Access the preintegrated measurements.
 | ||||
|   const PreintegratedMeasurements& preintegratedMeasurements() const { | ||||
|     return preintegratedMeasurements_; | ||||
|     return _PIM_; | ||||
|   } | ||||
| 
 | ||||
|   const Vector3& omegaCoriolis() const { | ||||
|  | @ -208,7 +183,7 @@ private: | |||
|     ar | ||||
|         & boost::serialization::make_nvp("NoiseModelFactor3", | ||||
|             boost::serialization::base_object<Base>(*this)); | ||||
|     ar & BOOST_SERIALIZATION_NVP(preintegratedMeasurements_); | ||||
|     ar & BOOST_SERIALIZATION_NVP(_PIM_); | ||||
|     ar & BOOST_SERIALIZATION_NVP(omegaCoriolis_); | ||||
|     ar & BOOST_SERIALIZATION_NVP(body_P_sensor_); | ||||
|   } | ||||
|  |  | |||
|  | @ -36,197 +36,136 @@ CombinedImuFactor::CombinedPreintegratedMeasurements::CombinedPreintegratedMeasu | |||
|     const Matrix3& measuredOmegaCovariance, const Matrix3& integrationErrorCovariance, | ||||
|     const Matrix3& biasAccCovariance, const Matrix3& biasOmegaCovariance, | ||||
|     const Matrix& biasAccOmegaInit, const bool use2ndOrderIntegration) : | ||||
|             biasHat_(bias), deltaPij_(Vector3::Zero()), deltaVij_(Vector3::Zero()), | ||||
|             deltaRij_(Rot3()), deltaTij_(0.0), | ||||
|             delPdelBiasAcc_(Z_3x3), delPdelBiasOmega_(Z_3x3), | ||||
|             delVdelBiasAcc_(Z_3x3), delVdelBiasOmega_(Z_3x3), | ||||
|             delRdelBiasOmega_(Z_3x3), use2ndOrderIntegration_(use2ndOrderIntegration) | ||||
|         PreintegrationBase(bias, measuredAccCovariance, measuredOmegaCovariance, | ||||
|         integrationErrorCovariance, use2ndOrderIntegration), | ||||
|         biasAccCovariance_(biasAccCovariance), biasOmegaCovariance_(biasOmegaCovariance), | ||||
|         biasAccOmegaInit_(biasAccOmegaInit) | ||||
| { | ||||
|   measurementCovariance_.setZero(); | ||||
|   measurementCovariance_.block<3,3>(0,0) = integrationErrorCovariance; | ||||
|   measurementCovariance_.block<3,3>(3,3) = measuredAccCovariance; | ||||
|   measurementCovariance_.block<3,3>(6,6) = measuredOmegaCovariance; | ||||
|   measurementCovariance_.block<3,3>(9,9) = biasAccCovariance; | ||||
|   measurementCovariance_.block<3,3>(12,12) = biasOmegaCovariance; | ||||
|   measurementCovariance_.block<6,6>(15,15) = biasAccOmegaInit; | ||||
|   PreintMeasCov_.setZero(); | ||||
|   preintMeasCov_.setZero(); | ||||
| } | ||||
| 
 | ||||
| //------------------------------------------------------------------------------
 | ||||
| void CombinedImuFactor::CombinedPreintegratedMeasurements::print(const string& s) const{ | ||||
|   cout << s << endl; | ||||
|   biasHat_.print("  biasHat"); | ||||
|   cout << "  deltaTij " << deltaTij_ << endl; | ||||
|   cout << "  deltaPij [ " << deltaPij_.transpose() << " ]" << endl; | ||||
|   cout << "  deltaVij [ " << deltaVij_.transpose() << " ]" << endl; | ||||
|   deltaRij_.print("  deltaRij "); | ||||
|   cout << "  measurementCovariance [ " << measurementCovariance_ << " ]" << endl; | ||||
|   cout << "  PreintMeasCov [ " << PreintMeasCov_ << " ]" << endl; | ||||
|   PreintegrationBase::print(s); | ||||
|   cout << "  biasAccCovariance [ " << biasAccCovariance_ << " ]" << endl; | ||||
|   cout << "  biasOmegaCovariance [ " << biasOmegaCovariance_ << " ]" << endl; | ||||
|   cout << "  biasAccOmegaInit [ " << biasAccOmegaInit_ << " ]" << endl; | ||||
|   cout << "  preintMeasCov [ " << preintMeasCov_ << " ]" << endl; | ||||
| } | ||||
| 
 | ||||
| //------------------------------------------------------------------------------
 | ||||
| bool CombinedImuFactor::CombinedPreintegratedMeasurements::equals(const CombinedPreintegratedMeasurements& expected, double tol) const{ | ||||
|   return biasHat_.equals(expected.biasHat_, tol) | ||||
|   && equal_with_abs_tol(measurementCovariance_, expected.measurementCovariance_, tol) | ||||
|   && equal_with_abs_tol(deltaPij_, expected.deltaPij_, tol) | ||||
|   && equal_with_abs_tol(deltaVij_, expected.deltaVij_, tol) | ||||
|   && deltaRij_.equals(expected.deltaRij_, tol) | ||||
|   && fabs(deltaTij_ - expected.deltaTij_) < tol | ||||
|   && equal_with_abs_tol(delPdelBiasAcc_, expected.delPdelBiasAcc_, tol) | ||||
|   && equal_with_abs_tol(delPdelBiasOmega_, expected.delPdelBiasOmega_, tol) | ||||
|   && equal_with_abs_tol(delVdelBiasAcc_, expected.delVdelBiasAcc_, tol) | ||||
|   && equal_with_abs_tol(delVdelBiasOmega_, expected.delVdelBiasOmega_, tol) | ||||
|   && equal_with_abs_tol(delRdelBiasOmega_, expected.delRdelBiasOmega_, tol); | ||||
|   return equal_with_abs_tol(biasAccCovariance_, expected.biasAccCovariance_, tol) | ||||
|       && equal_with_abs_tol(biasOmegaCovariance_, expected.biasOmegaCovariance_, tol) | ||||
|       &&equal_with_abs_tol(biasAccOmegaInit_, expected.biasAccOmegaInit_, tol) | ||||
|       && equal_with_abs_tol(preintMeasCov_, expected.preintMeasCov_, tol) | ||||
|       && PreintegrationBase::equals(expected, tol); | ||||
| } | ||||
| 
 | ||||
| //------------------------------------------------------------------------------
 | ||||
| void CombinedImuFactor::CombinedPreintegratedMeasurements::resetIntegration(){ | ||||
|   deltaPij_ = Vector3::Zero(); | ||||
|   deltaVij_ = Vector3::Zero(); | ||||
|   deltaRij_ = Rot3(); | ||||
|   deltaTij_ = 0.0; | ||||
|   delPdelBiasAcc_ = Z_3x3; | ||||
|   delPdelBiasOmega_ = Z_3x3; | ||||
|   delVdelBiasAcc_ = Z_3x3; | ||||
|   delVdelBiasOmega_ = Z_3x3; | ||||
|   delRdelBiasOmega_ = Z_3x3; | ||||
|   PreintMeasCov_.setZero(); | ||||
|   PreintegrationBase::resetIntegration(); | ||||
|   preintMeasCov_.setZero(); | ||||
| } | ||||
| 
 | ||||
| //------------------------------------------------------------------------------
 | ||||
| void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement( | ||||
|     const Vector3& measuredAcc, const Vector3& measuredOmega, | ||||
|     double deltaT, boost::optional<const Pose3&> body_P_sensor) { | ||||
|     double deltaT, boost::optional<const Pose3&> body_P_sensor, | ||||
|     boost::optional<Matrix&> F_test, boost::optional<Matrix&> G_test) { | ||||
| 
 | ||||
|   // NOTE: order is important here because each update uses old values, e.g., velocity and position updates are based on previous rotation estimate.
 | ||||
|   // (i.e., we have to update jacobians and covariances before updating preintegrated measurements).
 | ||||
| 
 | ||||
|   // First we compensate the measurements for the bias: since we have only an estimate of the bias, the covariance includes the corresponding uncertainty
 | ||||
|   Vector3 correctedAcc = biasHat_.correctAccelerometer(measuredAcc); | ||||
|   Vector3 correctedOmega = biasHat_.correctGyroscope(measuredOmega); | ||||
|   Vector3 correctedAcc, correctedOmega; | ||||
|   correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega, correctedAcc, correctedOmega, body_P_sensor); | ||||
| 
 | ||||
|   // Then compensate for sensor-body displacement: we express the quantities (originally in the IMU frame) into the body frame
 | ||||
|   if(body_P_sensor){ | ||||
|     Matrix3 body_R_sensor = body_P_sensor->rotation().matrix(); | ||||
|     correctedOmega = body_R_sensor * correctedOmega; // rotation rate vector in the body frame
 | ||||
|     Matrix3 body_omega_body__cross = skewSymmetric(correctedOmega); | ||||
|     correctedAcc = body_R_sensor * correctedAcc - body_omega_body__cross * body_omega_body__cross * body_P_sensor->translation().vector(); | ||||
|     // linear acceleration vector in the body frame
 | ||||
|   } | ||||
| 
 | ||||
|   const Vector3 theta_incr = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement
 | ||||
|   const Rot3 Rincr = Rot3::Expmap(theta_incr); // rotation increment computed from the current rotation rate measurement
 | ||||
|   const Matrix3 Jr_theta_incr = Rot3::ExpmapDerivative(theta_incr); // Right jacobian computed at theta_incr
 | ||||
|   const Vector3 integratedOmega = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement
 | ||||
|   Matrix3 D_Rincr_integratedOmega; // Right jacobian computed at theta_incr
 | ||||
|   const Rot3 Rincr = Rot3::Expmap(integratedOmega, D_Rincr_integratedOmega); // rotation increment computed from the current rotation rate measurement
 | ||||
| 
 | ||||
|   // Update Jacobians
 | ||||
|   /* ----------------------------------------------------------------------------------------------------------------------- */ | ||||
|   if(!use2ndOrderIntegration_){ | ||||
|     delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT; | ||||
|     delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT; | ||||
|   }else{ | ||||
|     delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT - 0.5 * deltaRij_.matrix() * deltaT*deltaT; | ||||
|     delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT - 0.5 * deltaRij_.matrix() | ||||
|                                             * skewSymmetric(biasHat_.correctAccelerometer(measuredAcc)) * deltaT*deltaT * delRdelBiasOmega_; | ||||
|   } | ||||
| 
 | ||||
|   delVdelBiasAcc_ += -deltaRij_.matrix() * deltaT; | ||||
|   delVdelBiasOmega_ += -deltaRij_.matrix() * skewSymmetric(correctedAcc) * deltaT * delRdelBiasOmega_; | ||||
|   delRdelBiasOmega_ = Rincr.inverse().matrix() * delRdelBiasOmega_ - Jr_theta_incr  * deltaT; | ||||
|   updatePreintegratedJacobians(correctedAcc, D_Rincr_integratedOmega, Rincr, deltaT); | ||||
| 
 | ||||
|   // Update preintegrated measurements covariance: as in [2] we consider a first order propagation that
 | ||||
|   // can be seen as a prediction phase in an EKF framework. In this implementation, contrarily to [2] we
 | ||||
|   // consider the uncertainty of the bias selection and we keep correlation between biases and preintegrated measurements
 | ||||
|   /* ----------------------------------------------------------------------------------------------------------------------- */ | ||||
|   const Vector3 theta_i = Rot3::Logmap(deltaRij_); // parametrization of so(3)
 | ||||
|   const Matrix3 Jr_theta_i = Rot3::ExpmapDerivative(theta_i); | ||||
| 
 | ||||
|   Rot3 Rot_j = deltaRij_ * Rincr; | ||||
|   const Vector3 theta_j = Rot3::Logmap(Rot_j); // parametrization of so(3)
 | ||||
|   const Matrix3 Jrinv_theta_j = Rot3::LogmapDerivative(theta_j); | ||||
|   const Matrix3 R_i = deltaRij(); // store this
 | ||||
|   // Update preintegrated measurements. TODO Frank moved from end of this function !!!
 | ||||
|   Matrix9 F_9x9; | ||||
|   updatePreintegratedMeasurements(correctedAcc, Rincr, deltaT, F_9x9); | ||||
| 
 | ||||
|   // Single Jacobians to propagate covariance
 | ||||
|   Matrix3 H_pos_pos    = I_3x3; | ||||
|   Matrix3 H_pos_vel    = I_3x3 * deltaT; | ||||
|   Matrix3 H_pos_angles = Z_3x3; | ||||
| 
 | ||||
|   Matrix3 H_vel_pos    = Z_3x3; | ||||
|   Matrix3 H_vel_vel    = I_3x3; | ||||
|   Matrix3 H_vel_angles = - deltaRij_.matrix() * skewSymmetric(correctedAcc) * Jr_theta_i * deltaT; | ||||
|   // analytic expression corresponding to the following numerical derivative
 | ||||
|   // Matrix H_vel_angles = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_vel, correctedOmega, correctedAcc, deltaT, _1, deltaVij), theta_i);
 | ||||
|   Matrix3 H_vel_biasacc = - deltaRij_.matrix() * deltaT; | ||||
| 
 | ||||
|   Matrix3 H_angles_pos   = Z_3x3; | ||||
|   Matrix3 H_angles_vel    = Z_3x3; | ||||
|   Matrix3 H_angles_angles = Jrinv_theta_j * Rincr.inverse().matrix() * Jr_theta_i; | ||||
|   Matrix3 H_angles_biasomega =- Jrinv_theta_j * Jr_theta_incr * deltaT; | ||||
|   // analytic expression corresponding to the following numerical derivative
 | ||||
|   // Matrix H_angles_angles = numericalDerivative11<Vector3, Vector3>(boost::bind(&PreIntegrateIMUObservations_delta_angles, correctedOmega, deltaT, _1), thetaij);
 | ||||
|   Matrix3 H_vel_biasacc = - R_i * deltaT; | ||||
|   Matrix3 H_angles_biasomega =- D_Rincr_integratedOmega * deltaT; | ||||
| 
 | ||||
|   // overall Jacobian wrt preintegrated measurements (df/dx)
 | ||||
|   Matrix F(15,15); | ||||
|   F << H_pos_pos,    H_pos_vel,     H_pos_angles,          Z_3x3,                     Z_3x3, | ||||
|       H_vel_pos,     H_vel_vel,     H_vel_angles,      H_vel_biasacc,              Z_3x3, | ||||
|       H_angles_pos,  H_angles_vel,  H_angles_angles,   Z_3x3,                         H_angles_biasomega, | ||||
|       Z_3x3,         Z_3x3,         Z_3x3,             I_3x3,                         Z_3x3, | ||||
|       Z_3x3,         Z_3x3,         Z_3x3,             Z_3x3,                         I_3x3; | ||||
|   // for documentation:
 | ||||
|   //  F << I_3x3,    I_3x3 * deltaT,   Z_3x3,             Z_3x3,            Z_3x3,
 | ||||
|   //       Z_3x3,    I_3x3,            H_vel_angles,      H_vel_biasacc,    Z_3x3,
 | ||||
|   //       Z_3x3,    Z_3x3,            H_angles_angles,   Z_3x3,            H_angles_biasomega,
 | ||||
|   //       Z_3x3,    Z_3x3,            Z_3x3,             I_3x3,            Z_3x3,
 | ||||
|   //       Z_3x3,    Z_3x3,            Z_3x3,             Z_3x3,            I_3x3;
 | ||||
|   F.setZero(); | ||||
|   F.block<9,9>(0,0)  = F_9x9; | ||||
|   F.block<6,6>(9,9)  = I_6x6; | ||||
|   F.block<3,3>(3,9)  = H_vel_biasacc; | ||||
|   F.block<3,3>(6,12) = H_angles_biasomega; | ||||
| 
 | ||||
|   // first order uncertainty propagation
 | ||||
|   // Optimized matrix multiplication   (1/deltaT) * G * measurementCovariance * G.transpose()
 | ||||
| 
 | ||||
|   Matrix G_measCov_Gt = Matrix::Zero(15,15); | ||||
|   // BLOCK DIAGONAL TERMS
 | ||||
|   G_measCov_Gt.block<3,3>(0,0) = deltaT * measurementCovariance_.block<3,3>(0,0); | ||||
| 
 | ||||
| //   BLOCK DIAGONAL TERMS
 | ||||
|   G_measCov_Gt.block<3,3>(0,0) = deltaT * integrationCovariance(); | ||||
|   G_measCov_Gt.block<3,3>(3,3) = (1/deltaT) * (H_vel_biasacc)  * | ||||
|       (measurementCovariance_.block<3,3>(3,3)  +  measurementCovariance_.block<3,3>(15,15) ) * | ||||
|       (accelerometerCovariance()  +  biasAccOmegaInit_.block<3,3>(0,0) ) * | ||||
|       (H_vel_biasacc.transpose()); | ||||
| 
 | ||||
|   G_measCov_Gt.block<3,3>(6,6) = (1/deltaT) *  (H_angles_biasomega) * | ||||
|       (measurementCovariance_.block<3,3>(6,6)  +  measurementCovariance_.block<3,3>(18,18) ) * | ||||
|       (gyroscopeCovariance()  +  biasAccOmegaInit_.block<3,3>(3,3) ) * | ||||
|       (H_angles_biasomega.transpose()); | ||||
| 
 | ||||
|   G_measCov_Gt.block<3,3>(9,9) = deltaT * measurementCovariance_.block<3,3>(9,9); | ||||
| 
 | ||||
|   G_measCov_Gt.block<3,3>(12,12) = deltaT * measurementCovariance_.block<3,3>(12,12); | ||||
| 
 | ||||
|   // NEW OFF BLOCK DIAGONAL TERMS
 | ||||
|   Matrix3 block23 = H_vel_biasacc * measurementCovariance_.block<3,3>(18,15) *  H_angles_biasomega.transpose(); | ||||
|   G_measCov_Gt.block<3,3>(9,9) = (1/deltaT) * biasAccCovariance_; | ||||
|   G_measCov_Gt.block<3,3>(12,12) = (1/deltaT) * biasOmegaCovariance_; | ||||
|   // OFF BLOCK DIAGONAL TERMS
 | ||||
|   Matrix3 block23 = H_vel_biasacc * biasAccOmegaInit_.block<3,3>(3,0) *  H_angles_biasomega.transpose(); | ||||
|   G_measCov_Gt.block<3,3>(3,6) = block23; | ||||
|   G_measCov_Gt.block<3,3>(6,3) = block23.transpose(); | ||||
|   preintMeasCov_ = F * preintMeasCov_ * F.transpose() + G_measCov_Gt; | ||||
| 
 | ||||
|   PreintMeasCov_ = F * PreintMeasCov_ * F.transpose() + G_measCov_Gt; | ||||
| 
 | ||||
|   // Update preintegrated measurements
 | ||||
|   /* ----------------------------------------------------------------------------------------------------------------------- */ | ||||
|   if(!use2ndOrderIntegration_){ | ||||
|     deltaPij_ += deltaVij_ * deltaT; | ||||
|   }else{ | ||||
|     deltaPij_ += deltaVij_ * deltaT + 0.5 * deltaRij_.matrix() * biasHat_.correctAccelerometer(measuredAcc) * deltaT*deltaT; | ||||
|   // F_test and G_test are used for testing purposes and are not needed by the factor
 | ||||
|   if(F_test){ | ||||
|     F_test->resize(15,15); | ||||
|     (*F_test) << F; | ||||
|   } | ||||
|   if(G_test){ | ||||
|     G_test->resize(15,21); | ||||
|     // This is for testing & documentation
 | ||||
|     ///< measurementCovariance_ : cov[integrationError measuredAcc measuredOmega biasAccRandomWalk biasOmegaRandomWalk biasAccInit biasOmegaInit] in R^(21 x 21)
 | ||||
|     (*G_test) << I_3x3 * deltaT,  Z_3x3,          Z_3x3,              Z_3x3,              Z_3x3,           Z_3x3,              Z_3x3, | ||||
|                  Z_3x3,          -H_vel_biasacc,  Z_3x3,              Z_3x3,              Z_3x3,           H_vel_biasacc,      Z_3x3, | ||||
|                  Z_3x3,           Z_3x3,         -H_angles_biasomega, Z_3x3,              Z_3x3,           Z_3x3,              H_angles_biasomega, | ||||
|                  Z_3x3,           Z_3x3,          Z_3x3,              I_3x3,              Z_3x3,           Z_3x3,              Z_3x3, | ||||
|                  Z_3x3,           Z_3x3,          Z_3x3,              Z_3x3,              I_3x3,           Z_3x3,              Z_3x3; | ||||
|   } | ||||
|   deltaVij_ += deltaRij_.matrix() * correctedAcc * deltaT; | ||||
|   deltaRij_ = deltaRij_ * Rincr; | ||||
|   deltaTij_ += deltaT; | ||||
| } | ||||
| 
 | ||||
| //------------------------------------------------------------------------------
 | ||||
| // CombinedImuFactor methods
 | ||||
| //------------------------------------------------------------------------------
 | ||||
| CombinedImuFactor::CombinedImuFactor() : | ||||
|     preintegratedMeasurements_(imuBias::ConstantBias(), Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Matrix::Zero(6,6)) {} | ||||
|     ImuFactorBase(), _PIM_(imuBias::ConstantBias(), Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_6x6) {} | ||||
| 
 | ||||
| //------------------------------------------------------------------------------
 | ||||
| CombinedImuFactor::CombinedImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i, Key bias_j, | ||||
|     const CombinedPreintegratedMeasurements& preintegratedMeasurements, | ||||
|     const Vector3& gravity, const Vector3& omegaCoriolis, | ||||
|     boost::optional<const Pose3&> body_P_sensor, const bool use2ndOrderCoriolis) : | ||||
|           Base(noiseModel::Gaussian::Covariance(preintegratedMeasurements.PreintMeasCov_), pose_i, vel_i, pose_j, vel_j, bias_i, bias_j), | ||||
|           preintegratedMeasurements_(preintegratedMeasurements), | ||||
|           gravity_(gravity), | ||||
|           omegaCoriolis_(omegaCoriolis), | ||||
|           body_P_sensor_(body_P_sensor), | ||||
|           use2ndOrderCoriolis_(use2ndOrderCoriolis){ | ||||
| } | ||||
|           Base(noiseModel::Gaussian::Covariance(preintegratedMeasurements.preintMeasCov_), pose_i, vel_i, pose_j, vel_j, bias_i, bias_j), | ||||
|           ImuFactorBase(gravity, omegaCoriolis, body_P_sensor, use2ndOrderCoriolis), | ||||
|           _PIM_(preintegratedMeasurements) {} | ||||
| 
 | ||||
| //------------------------------------------------------------------------------
 | ||||
| gtsam::NonlinearFactor::shared_ptr CombinedImuFactor::clone() const { | ||||
|  | @ -243,22 +182,17 @@ void CombinedImuFactor::print(const string& s, const KeyFormatter& keyFormatter) | |||
|       << keyFormatter(this->key4()) << "," | ||||
|       << keyFormatter(this->key5()) << "," | ||||
|       << keyFormatter(this->key6()) << ")\n"; | ||||
|   preintegratedMeasurements_.print("  preintegrated measurements:"); | ||||
|   cout << "  gravity: [ " << gravity_.transpose() << " ]" << endl; | ||||
|   cout << "  omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]" << endl; | ||||
|   ImuFactorBase::print(""); | ||||
|   _PIM_.print("  preintegrated measurements:"); | ||||
|   this->noiseModel_->print("  noise model: "); | ||||
|   if(this->body_P_sensor_) | ||||
|     this->body_P_sensor_->print("  sensor pose in body frame: "); | ||||
| } | ||||
| 
 | ||||
| //------------------------------------------------------------------------------
 | ||||
| bool CombinedImuFactor::equals(const NonlinearFactor& expected, double tol) const { | ||||
|   const This *e =  dynamic_cast<const This*> (&expected); | ||||
|   return e != NULL && Base::equals(*e, tol) | ||||
|   && preintegratedMeasurements_.equals(e->preintegratedMeasurements_, tol) | ||||
|   && equal_with_abs_tol(gravity_, e->gravity_, tol) | ||||
|   && equal_with_abs_tol(omegaCoriolis_, e->omegaCoriolis_, tol) | ||||
|   && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_))); | ||||
|   && _PIM_.equals(e->_PIM_, tol) | ||||
|   && ImuFactorBase::equals(*e, tol); | ||||
| } | ||||
| 
 | ||||
| //------------------------------------------------------------------------------
 | ||||
|  | @ -268,230 +202,69 @@ Vector CombinedImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_ | |||
|     boost::optional<Matrix&> H3, boost::optional<Matrix&> H4, | ||||
|     boost::optional<Matrix&> H5, boost::optional<Matrix&> H6) const { | ||||
| 
 | ||||
|   const double& deltaTij = preintegratedMeasurements_.deltaTij_; | ||||
|   const Vector3 biasAccIncr = bias_i.accelerometer() - preintegratedMeasurements_.biasHat_.accelerometer(); | ||||
|   const Vector3 biasOmegaIncr = bias_i.gyroscope() - preintegratedMeasurements_.biasHat_.gyroscope(); | ||||
|   // if we need the jacobians
 | ||||
|   if(H1 || H2 || H3 || H4 || H5 || H6){ | ||||
|     Matrix H1_pvR, H2_pvR, H3_pvR, H4_pvR, H5_pvR, Hbias_i, Hbias_j; // pvR = mnemonic: position (p), velocity (v), rotation (R)
 | ||||
| 
 | ||||
|   // we give some shorter name to rotations and translations
 | ||||
|   const Rot3 Rot_i = pose_i.rotation(); | ||||
|   const Rot3 Rot_j = pose_j.rotation(); | ||||
|   const Vector3 pos_i = pose_i.translation().vector(); | ||||
|   const Vector3 pos_j = pose_j.translation().vector(); | ||||
|     // error wrt preintegrated measurements
 | ||||
|     Vector r_pvR(9); | ||||
|     r_pvR = _PIM_.computeErrorAndJacobians(pose_i, vel_i, pose_j, vel_j, bias_i, | ||||
|         gravity_, omegaCoriolis_, use2ndOrderCoriolis_, //
 | ||||
|         H1_pvR, H2_pvR, H3_pvR, H4_pvR, H5_pvR); | ||||
| 
 | ||||
|   // We compute factor's Jacobians, according to [3]
 | ||||
|   /* ---------------------------------------------------------------------------------------------------- */ | ||||
|   const Rot3 deltaRij_biascorrected = preintegratedMeasurements_.deltaRij_.retract(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr, Rot3::EXPMAP); | ||||
|   // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr)
 | ||||
|     // error wrt bias evolution model (random walk)
 | ||||
|     Vector6 fbias = bias_j.between(bias_i, Hbias_j, Hbias_i).vector(); // [bias_j.acc - bias_i.acc; bias_j.gyr - bias_i.gyr]
 | ||||
| 
 | ||||
|   Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected); | ||||
| 
 | ||||
|   Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected  - | ||||
|       Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij; // Coriolis term
 | ||||
| 
 | ||||
|   const Rot3 deltaRij_biascorrected_corioliscorrected = | ||||
|       Rot3::Expmap( theta_biascorrected_corioliscorrected ); | ||||
| 
 | ||||
|   const Rot3 fRhat = deltaRij_biascorrected_corioliscorrected.between(Rot_i.between(Rot_j)); | ||||
| 
 | ||||
|   const Matrix3 Jr_theta_bcc = Rot3::ExpmapDerivative(theta_biascorrected_corioliscorrected); | ||||
| 
 | ||||
|   const Matrix3 Jtheta = -Jr_theta_bcc  * skewSymmetric(Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij); | ||||
| 
 | ||||
|   const Matrix3 Jrinv_fRhat = Rot3::LogmapDerivative(Rot3::Logmap(fRhat)); | ||||
| 
 | ||||
|   if(H1) { | ||||
|     H1->resize(15,6); | ||||
| 
 | ||||
|     Matrix3 dfPdPi; | ||||
|     Matrix3 dfVdPi; | ||||
|     if(use2ndOrderCoriolis_){ | ||||
|       dfPdPi = - Rot_i.matrix() + 0.5 * skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij*deltaTij; | ||||
|       dfVdPi = skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij; | ||||
|     if(H1) { | ||||
|       H1->resize(15,6); | ||||
|       H1->block<9,6>(0,0) = H1_pvR; | ||||
|       // adding: [dBiasAcc/dPi ; dBiasOmega/dPi]
 | ||||
|       H1->block<6,6>(9,0) = Z_6x6; | ||||
|     } | ||||
|     else{ | ||||
|       dfPdPi = - Rot_i.matrix(); | ||||
|       dfVdPi = Z_3x3; | ||||
|     if(H2) { | ||||
|       H2->resize(15,3); | ||||
|       H2->block<9,3>(0,0) = H2_pvR; | ||||
|       // adding: [dBiasAcc/dVi ; dBiasOmega/dVi]
 | ||||
|       H2->block<6,3>(9,0) = Matrix::Zero(6,3); | ||||
|     } | ||||
| 
 | ||||
|     (*H1) << | ||||
|         // dfP/dRi
 | ||||
|         Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij_ | ||||
|         + preintegratedMeasurements_.delPdelBiasOmega_ * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc_ * biasAccIncr), | ||||
|         // dfP/dPi
 | ||||
|         dfPdPi, | ||||
|         // dfV/dRi
 | ||||
|         Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij_ | ||||
|         + preintegratedMeasurements_.delVdelBiasOmega_ * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc_ * biasAccIncr), | ||||
|         // dfV/dPi
 | ||||
|         dfVdPi, | ||||
|         // dfR/dRi
 | ||||
|         Jrinv_fRhat *  (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta), | ||||
|         // dfR/dPi
 | ||||
|         Z_3x3, | ||||
|         //dBiasAcc/dPi
 | ||||
|         Z_3x3, Z_3x3, | ||||
|         //dBiasOmega/dPi
 | ||||
|         Z_3x3, Z_3x3; | ||||
|     if(H3) { | ||||
|       H3->resize(15,6); | ||||
|       H3->block<9,6>(0,0) = H3_pvR; | ||||
|       // adding: [dBiasAcc/dPj ; dBiasOmega/dPj]
 | ||||
|       H3->block<6,6>(9,0) = Z_6x6; | ||||
|     } | ||||
|     if(H4) { | ||||
|       H4->resize(15,3); | ||||
|       H4->block<9,3>(0,0) = H4_pvR; | ||||
|       // adding: [dBiasAcc/dVi ; dBiasOmega/dVi]
 | ||||
|       H4->block<6,3>(9,0) = Matrix::Zero(6,3); | ||||
|     } | ||||
|     if(H5) { | ||||
|       H5->resize(15,6); | ||||
|       H5->block<9,6>(0,0) = H5_pvR; | ||||
|       // adding: [dBiasAcc/dBias_i ; dBiasOmega/dBias_i]
 | ||||
|       H5->block<6,6>(9,0) = Hbias_i; | ||||
|     } | ||||
|     if(H6) { | ||||
|       H6->resize(15,6); | ||||
|       H6->block<9,6>(0,0) = Matrix::Zero(9,6); | ||||
|       // adding: [dBiasAcc/dBias_j ; dBiasOmega/dBias_j]
 | ||||
|       H6->block<6,6>(9,0) = Hbias_j; | ||||
|     } | ||||
|     Vector r(15); r << r_pvR, fbias; // vector of size 15
 | ||||
|     return r; | ||||
|   } | ||||
| 
 | ||||
|   if(H2) { | ||||
|     H2->resize(15,3); | ||||
|     (*H2) << | ||||
|         // dfP/dVi
 | ||||
|         - I_3x3 * deltaTij | ||||
|         + skewSymmetric(omegaCoriolis_) * deltaTij * deltaTij,  // Coriolis term - we got rid of the 2 wrt ins paper
 | ||||
|         // dfV/dVi
 | ||||
|         - I_3x3 | ||||
|         + 2 * skewSymmetric(omegaCoriolis_) * deltaTij, // Coriolis term
 | ||||
|         // dfR/dVi
 | ||||
|         Z_3x3, | ||||
|         //dBiasAcc/dVi
 | ||||
|         Z_3x3, | ||||
|         //dBiasOmega/dVi
 | ||||
|         Z_3x3; | ||||
|   } | ||||
| 
 | ||||
|   if(H3) { | ||||
|     H3->resize(15,6); | ||||
|     (*H3) << | ||||
|         // dfP/dPosej
 | ||||
|         Z_3x3, Rot_j.matrix(), | ||||
|         // dfV/dPosej
 | ||||
|         Matrix::Zero(3,6), | ||||
|         // dfR/dPosej
 | ||||
|         Jrinv_fRhat *  ( I_3x3 ), Z_3x3, | ||||
|         //dBiasAcc/dPosej
 | ||||
|         Z_3x3, Z_3x3, | ||||
|         //dBiasOmega/dPosej
 | ||||
|         Z_3x3, Z_3x3; | ||||
|   } | ||||
| 
 | ||||
|   if(H4) { | ||||
|     H4->resize(15,3); | ||||
|     (*H4) << | ||||
|         // dfP/dVj
 | ||||
|         Z_3x3, | ||||
|         // dfV/dVj
 | ||||
|         I_3x3, | ||||
|         // dfR/dVj
 | ||||
|         Z_3x3, | ||||
|         //dBiasAcc/dVj
 | ||||
|         Z_3x3, | ||||
|         //dBiasOmega/dVj
 | ||||
|         Z_3x3; | ||||
|   } | ||||
| 
 | ||||
|   if(H5) { | ||||
|     const Matrix3 Jrinv_theta_bc = Rot3::LogmapDerivative(theta_biascorrected); | ||||
|     const Matrix3 Jr_JbiasOmegaIncr = Rot3::ExpmapDerivative(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr); | ||||
|     const Matrix3 JbiasOmega = Jr_theta_bcc * Jrinv_theta_bc * Jr_JbiasOmegaIncr * preintegratedMeasurements_.delRdelBiasOmega_; | ||||
| 
 | ||||
|     H5->resize(15,6); | ||||
|     (*H5) << | ||||
|         // dfP/dBias_i
 | ||||
|         - Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasAcc_, | ||||
|         - Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasOmega_, | ||||
|         // dfV/dBias_i
 | ||||
|         - Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasAcc_, | ||||
|         - Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasOmega_, | ||||
|         // dfR/dBias_i
 | ||||
|         Matrix::Zero(3,3), | ||||
|         Jrinv_fRhat * ( - fRhat.inverse().matrix() * JbiasOmega), | ||||
|         //dBiasAcc/dBias_i
 | ||||
|         -I_3x3, Z_3x3, | ||||
|         //dBiasOmega/dBias_i
 | ||||
|         Z_3x3, -I_3x3; | ||||
|   } | ||||
| 
 | ||||
|   if(H6) { | ||||
|     H6->resize(15,6); | ||||
|     (*H6) << | ||||
|         // dfP/dBias_j
 | ||||
|         Z_3x3, Z_3x3, | ||||
|         // dfV/dBias_j
 | ||||
|         Z_3x3, Z_3x3, | ||||
|         // dfR/dBias_j
 | ||||
|         Z_3x3, Z_3x3, | ||||
|         //dBiasAcc/dBias_j
 | ||||
|         I_3x3, Z_3x3, | ||||
|         //dBiasOmega/dBias_j
 | ||||
|         Z_3x3, I_3x3; | ||||
|   } | ||||
| 
 | ||||
|   // Evaluate residual error, according to [3]
 | ||||
|   /* ---------------------------------------------------------------------------------------------------- */ | ||||
|   const Vector3 fp = | ||||
|       pos_j - pos_i | ||||
|       - Rot_i.matrix() * (preintegratedMeasurements_.deltaPij_ | ||||
|           + preintegratedMeasurements_.delPdelBiasAcc_ * biasAccIncr | ||||
|           + preintegratedMeasurements_.delPdelBiasOmega_ * biasOmegaIncr) | ||||
|           - vel_i * deltaTij | ||||
|           + skewSymmetric(omegaCoriolis_) * vel_i * deltaTij*deltaTij  // Coriolis term - we got rid of the 2 wrt ins paper
 | ||||
|           - 0.5 * gravity_ * deltaTij*deltaTij; | ||||
| 
 | ||||
|   const Vector3 fv = | ||||
|       vel_j - vel_i - Rot_i.matrix() * (preintegratedMeasurements_.deltaVij_ | ||||
|           + preintegratedMeasurements_.delVdelBiasAcc_ * biasAccIncr | ||||
|           + preintegratedMeasurements_.delVdelBiasOmega_ * biasOmegaIncr) | ||||
|           + 2 * skewSymmetric(omegaCoriolis_) * vel_i * deltaTij  // Coriolis term
 | ||||
|           - gravity_ * deltaTij; | ||||
| 
 | ||||
|   const Vector3 fR = Rot3::Logmap(fRhat); | ||||
| 
 | ||||
|   const Vector3 fbiasAcc = bias_j.accelerometer() - bias_i.accelerometer(); | ||||
| 
 | ||||
|   const Vector3 fbiasOmega = bias_j.gyroscope() - bias_i.gyroscope(); | ||||
| 
 | ||||
|   Vector r(15); r << fp, fv, fR, fbiasAcc, fbiasOmega; // vector of size 15
 | ||||
| 
 | ||||
|   // else, only compute the error vector:
 | ||||
|   // error wrt preintegrated measurements
 | ||||
|   Vector r_pvR(9); | ||||
|   r_pvR = _PIM_.computeErrorAndJacobians(pose_i, vel_i, pose_j, vel_j, bias_i, | ||||
|       gravity_, omegaCoriolis_, use2ndOrderCoriolis_, //
 | ||||
|       boost::none, boost::none, boost::none, boost::none, boost::none); | ||||
|   // error wrt bias evolution model (random walk)
 | ||||
|   Vector6 fbias = bias_j.between(bias_i).vector(); // [bias_j.acc - bias_i.acc; bias_j.gyr - bias_i.gyr]
 | ||||
|   // overall error
 | ||||
|   Vector r(15); r << r_pvR, fbias; // vector of size 15
 | ||||
|   return r; | ||||
| } | ||||
| 
 | ||||
| //------------------------------------------------------------------------------
 | ||||
| PoseVelocityBias CombinedImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i, | ||||
|     const imuBias::ConstantBias& bias_i, | ||||
|     const CombinedPreintegratedMeasurements& preintegratedMeasurements, | ||||
|     const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis){ | ||||
| 
 | ||||
|   const double& deltaTij = preintegratedMeasurements.deltaTij_; | ||||
|   const Vector3 biasAccIncr = bias_i.accelerometer() - preintegratedMeasurements.biasHat_.accelerometer(); | ||||
|   const Vector3 biasOmegaIncr = bias_i.gyroscope() - preintegratedMeasurements.biasHat_.gyroscope(); | ||||
| 
 | ||||
|   const Rot3 Rot_i = pose_i.rotation(); | ||||
|   const Vector3 pos_i = pose_i.translation().vector(); | ||||
| 
 | ||||
|   // Predict state at time j
 | ||||
|   /* ---------------------------------------------------------------------------------------------------- */ | ||||
|   Vector3 pos_j =  pos_i + Rot_i.matrix() * (preintegratedMeasurements.deltaPij_ | ||||
|       + preintegratedMeasurements.delPdelBiasAcc_ * biasAccIncr | ||||
|       + preintegratedMeasurements.delPdelBiasOmega_ * biasOmegaIncr) | ||||
|       + vel_i * deltaTij | ||||
|       - skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij  // Coriolis term - we got rid of the 2 wrt ins paper
 | ||||
|       + 0.5 * gravity * deltaTij*deltaTij; | ||||
| 
 | ||||
|   Vector3 vel_j = Vector3(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij_ | ||||
|       + preintegratedMeasurements.delVdelBiasAcc_ * biasAccIncr | ||||
|       + preintegratedMeasurements.delVdelBiasOmega_ * biasOmegaIncr) | ||||
|       - 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij  // Coriolis term
 | ||||
|       + gravity * deltaTij); | ||||
| 
 | ||||
|   if(use2ndOrderCoriolis){ | ||||
|     pos_j += - 0.5 * skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij*deltaTij;  // 2nd order coriolis term for position
 | ||||
|     vel_j += - skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij; // 2nd order term for velocity
 | ||||
|   } | ||||
| 
 | ||||
|   const Rot3 deltaRij_biascorrected = preintegratedMeasurements.deltaRij_.retract(preintegratedMeasurements.delRdelBiasOmega_ * biasOmegaIncr, Rot3::EXPMAP); | ||||
|   // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr)
 | ||||
|   Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected); | ||||
|   Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected  - | ||||
|       Rot_i.inverse().matrix() * omegaCoriolis * deltaTij; // Coriolis term
 | ||||
|   const Rot3 deltaRij_biascorrected_corioliscorrected = | ||||
|       Rot3::Expmap( theta_biascorrected_corioliscorrected ); | ||||
|   const Rot3 Rot_j = Rot_i.compose( deltaRij_biascorrected_corioliscorrected  ); | ||||
| 
 | ||||
|   Pose3 pose_j = Pose3( Rot_j, Point3(pos_j) ); | ||||
| 
 | ||||
|   return PoseVelocityBias(pose_j, vel_j, bias_i); | ||||
| } | ||||
| 
 | ||||
| } /// namespace gtsam
 | ||||
|  |  | |||
|  | @ -23,7 +23,8 @@ | |||
| 
 | ||||
| /* GTSAM includes */ | ||||
| #include <gtsam/nonlinear/NonlinearFactor.h> | ||||
| #include <gtsam/navigation/ImuBias.h> | ||||
| #include <gtsam/navigation/PreintegrationBase.h> | ||||
| #include <gtsam/navigation/ImuFactorBase.h> | ||||
| #include <gtsam/base/debug.h> | ||||
| 
 | ||||
| namespace gtsam { | ||||
|  | @ -33,78 +34,63 @@ namespace gtsam { | |||
|  * @addtogroup SLAM | ||||
|  * | ||||
|  * If you are using the factor, please cite: | ||||
|  * L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating conditionally | ||||
|  * independent sets in factor graphs: a unifying perspective based on smart factors, | ||||
|  * Int. Conf. on Robotics and Automation (ICRA), 2014. | ||||
|  * L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating | ||||
|  * conditionally independent sets in factor graphs: a unifying perspective based | ||||
|  * on smart factors, Int. Conf. on Robotics and Automation (ICRA), 2014. | ||||
|  * | ||||
|  * REFERENCES: | ||||
|  * [1] G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008. | ||||
|  * [2] T. Lupton and S.Sukkarieh, "Visual-Inertial-Aided Navigation for High-Dynamic Motion in Built | ||||
|  * Environments Without Initial Conditions", TRO, 28(1):61-76, 2012. | ||||
|  * [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor: Computation of the Jacobian Matrices", Tech. Report, 2013. | ||||
|  ** REFERENCES: | ||||
|  * [1] G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", | ||||
|  *     Volume 2, 2008. | ||||
|  * [2] T. Lupton and S.Sukkarieh, "Visual-Inertial-Aided Navigation for | ||||
|  *     High-Dynamic Motion in Built Environments Without Initial Conditions", | ||||
|  *     TRO, 28(1):61-76, 2012. | ||||
|  * [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor: | ||||
|  *     Computation of the Jacobian Matrices", Tech. Report, 2013. | ||||
|  */ | ||||
| 
 | ||||
| /**
 | ||||
|  * Struct to hold all state variables of CombinedImuFactor returned by Predict function | ||||
|  * CombinedImuFactor is a 6-ways factor involving previous state (pose and | ||||
|  * velocity of the vehicle, as well as bias at previous time step), and current | ||||
|  * state (pose, velocity, bias at current time step). Following the pre- | ||||
|  * integration scheme proposed in [2], the CombinedImuFactor includes many IMU | ||||
|  * measurements, which are "summarized" using the CombinedPreintegratedMeasurements | ||||
|  * class. There are 3 main differences wrpt the ImuFactor class: | ||||
|  * 1) The factor is 6-ways, meaning that it also involves both biases (previous | ||||
|  *    and current time step).Therefore, the factor internally imposes the biases | ||||
|  *    to be slowly varying; in particular, the matrices "biasAccCovariance" and | ||||
|  *    "biasOmegaCovariance" described the random walk that models bias evolution. | ||||
|  * 2) The preintegration covariance takes into account the noise in the bias | ||||
|  *    estimate used for integration. | ||||
|  * 3) The covariance matrix of the CombinedPreintegratedMeasurements preserves | ||||
|  *    the correlation between the bias uncertainty and the preintegrated | ||||
|  *    measurements uncertainty. | ||||
|  */ | ||||
| struct PoseVelocityBias { | ||||
|   Pose3 pose; | ||||
|   Vector3 velocity; | ||||
|   imuBias::ConstantBias bias; | ||||
| 
 | ||||
|   PoseVelocityBias(const Pose3& _pose, const Vector3& _velocity, | ||||
|       const imuBias::ConstantBias _bias) : | ||||
|         pose(_pose), velocity(_velocity), bias(_bias) { | ||||
|   } | ||||
| }; | ||||
| 
 | ||||
| /**
 | ||||
|  * CombinedImuFactor is a 6-ways factor involving previous state (pose and velocity of the vehicle, as well as bias | ||||
|  * at previous time step), and current state (pose, velocity, bias at current time step). According to the | ||||
|  * preintegration scheme proposed in [2], the CombinedImuFactor includes many IMU measurements, which are | ||||
|  * "summarized" using the CombinedPreintegratedMeasurements class. There are 3 main differences wrt ImuFactor: | ||||
|  * 1) The factor is 6-ways, meaning that it also involves both biases (previous and current time step). | ||||
|  * Therefore, the factor internally imposes the biases to be slowly varying; in particular, the matrices | ||||
|  * "biasAccCovariance" and "biasOmegaCovariance" described the random walk that models bias evolution. | ||||
|  * 2) The preintegration covariance takes into account the noise in the bias estimate used for integration. | ||||
|  * 3) The covariance matrix of the CombinedPreintegratedMeasurements preserves the correlation between the bias uncertainty | ||||
|  * and the preintegrated measurements uncertainty. | ||||
|  */ | ||||
| class CombinedImuFactor: public NoiseModelFactor6<Pose3,Vector3,Pose3,Vector3,imuBias::ConstantBias,imuBias::ConstantBias> { | ||||
| class CombinedImuFactor: public NoiseModelFactor6<Pose3,Vector3,Pose3,Vector3,imuBias::ConstantBias,imuBias::ConstantBias>, public ImuFactorBase{ | ||||
| public: | ||||
| 
 | ||||
|   /** CombinedPreintegratedMeasurements accumulates (integrates) the IMU measurements (rotation rates and accelerations)
 | ||||
|    * and the corresponding covariance matrix. The measurements are then used to build the CombinedPreintegrated IMU factor (CombinedImuFactor). | ||||
|    * Integration is done incrementally (ideally, one integrates the measurement as soon as it is received | ||||
|    * from the IMU) so as to avoid costly integration at time of factor construction. | ||||
|   /**
 | ||||
|    * CombinedPreintegratedMeasurements integrates the IMU measurements | ||||
|    * (rotation rates and accelerations) and the corresponding covariance matrix. | ||||
|    * The measurements are then used to build the CombinedImuFactor. Integration | ||||
|    * is done incrementally (ideally, one integrates the measurement as soon as | ||||
|    * it is received from the IMU) so as to avoid costly integration at time of | ||||
|    * factor construction. | ||||
|    */ | ||||
|   class CombinedPreintegratedMeasurements { | ||||
|   class CombinedPreintegratedMeasurements: public PreintegrationBase { | ||||
| 
 | ||||
|     friend class CombinedImuFactor; | ||||
| 
 | ||||
|   protected: | ||||
|     imuBias::ConstantBias biasHat_; ///< Acceleration and angular rate bias values used during preintegration
 | ||||
|     Eigen::Matrix<double,21,21> measurementCovariance_; ///< (Raw measurements uncertainty) Covariance of the vector
 | ||||
|     ///< [integrationError measuredAcc measuredOmega biasAccRandomWalk biasOmegaRandomWalk biasAccInit biasOmegaInit] in R^(21 x 21)
 | ||||
| 
 | ||||
|     Vector3 deltaPij_; ///< Preintegrated relative position (does not take into account velocity at time i, see deltap+, in [2]) (in frame i)
 | ||||
|     Vector3 deltaVij_; ///< Preintegrated relative velocity (in global frame)
 | ||||
|     Rot3 deltaRij_;    ///< Preintegrated relative orientation (in frame i)
 | ||||
|     double deltaTij_;  ///< Time interval from i to j
 | ||||
|     Matrix3 biasAccCovariance_;   ///< continuous-time "Covariance" describing accelerometer bias random walk
 | ||||
|     Matrix3 biasOmegaCovariance_; ///< continuous-time "Covariance" describing gyroscope bias random walk
 | ||||
|     Matrix6 biasAccOmegaInit_;    ///< covariance of bias used for pre-integration
 | ||||
| 
 | ||||
|     Matrix3 delPdelBiasAcc_;   ///< Jacobian of preintegrated position w.r.t. acceleration bias
 | ||||
|     Matrix3 delPdelBiasOmega_; ///< Jacobian of preintegrated position w.r.t. angular rate bias
 | ||||
|     Matrix3 delVdelBiasAcc_;   ///< Jacobian of preintegrated velocity w.r.t. acceleration bias
 | ||||
|     Matrix3 delVdelBiasOmega_; ///< Jacobian of preintegrated velocity w.r.t. angular rate bias
 | ||||
|     Matrix3 delRdelBiasOmega_; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias
 | ||||
| 
 | ||||
|     Eigen::Matrix<double,15,15> PreintMeasCov_; ///< Covariance matrix of the preintegrated measurements
 | ||||
|     Eigen::Matrix<double,15,15> preintMeasCov_; ///< Covariance matrix of the preintegrated measurements
 | ||||
|     ///< COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION BiasAcc BiasOmega]
 | ||||
|     ///< (first-order propagation from *measurementCovariance*). CombinedPreintegratedMeasurements also include the biases and keep the correlation
 | ||||
|     ///< between the preintegrated measurements and the biases
 | ||||
| 
 | ||||
|     bool use2ndOrderIntegration_; ///< Controls the order of integration
 | ||||
| 
 | ||||
|   public: | ||||
| 
 | ||||
|     /**
 | ||||
|  | @ -141,60 +127,20 @@ public: | |||
|      * @param body_P_sensor Optional sensor frame (pose of the IMU in the body frame) | ||||
|      */ | ||||
|     void integrateMeasurement(const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT, | ||||
|         boost::optional<const Pose3&> body_P_sensor = boost::none); | ||||
|         boost::optional<const Pose3&> body_P_sensor = boost::none, | ||||
|         boost::optional<Matrix&> F_test = boost::none, boost::optional<Matrix&> G_test = boost::none); | ||||
| 
 | ||||
|     /// methods to access class variables
 | ||||
|     Matrix measurementCovariance() const {return measurementCovariance_;} | ||||
|     Matrix deltaRij() const {return deltaRij_.matrix();} | ||||
|     double deltaTij() const{return deltaTij_;} | ||||
|     Vector deltaPij() const {return deltaPij_;} | ||||
|     Vector deltaVij() const {return deltaVij_;} | ||||
|     Vector biasHat() const { return biasHat_.vector();} | ||||
|     Matrix delPdelBiasAcc() const { return delPdelBiasAcc_;} | ||||
|     Matrix delPdelBiasOmega() const { return delPdelBiasOmega_;} | ||||
|     Matrix delVdelBiasAcc() const { return delVdelBiasAcc_;} | ||||
|     Matrix delVdelBiasOmega() const { return delVdelBiasOmega_;} | ||||
|     Matrix delRdelBiasOmega() const{ return delRdelBiasOmega_;} | ||||
|     Matrix PreintMeasCov() const { return PreintMeasCov_;} | ||||
| 
 | ||||
|     /* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */ | ||||
|     // This function is only used for test purposes (compare numerical derivatives wrt analytic ones)
 | ||||
|     static inline Vector PreIntegrateIMUObservations_delta_vel(const Vector& msr_gyro_t, const Vector& msr_acc_t, const double msr_dt, | ||||
|         const Vector3& delta_angles, const Vector& delta_vel_in_t0){ | ||||
|       // Note: all delta terms refer to an IMU\sensor system at t0
 | ||||
|       Vector body_t_a_body = msr_acc_t; | ||||
|       Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles); | ||||
|       return delta_vel_in_t0 + R_t_to_t0.matrix() * body_t_a_body * msr_dt; | ||||
|     } | ||||
| 
 | ||||
|     // This function is only used for test purposes (compare numerical derivatives wrt analytic ones)
 | ||||
|     static inline Vector PreIntegrateIMUObservations_delta_angles(const Vector& msr_gyro_t, const double msr_dt, | ||||
|         const Vector3& delta_angles){ | ||||
|       // Note: all delta terms refer to an IMU\sensor system at t0
 | ||||
|       // Calculate the corrected measurements using the Bias object
 | ||||
|       Vector body_t_omega_body= msr_gyro_t; | ||||
|       Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles); | ||||
|       R_t_to_t0    = R_t_to_t0 * Rot3::Expmap( body_t_omega_body*msr_dt ); | ||||
|       return Rot3::Logmap(R_t_to_t0); | ||||
|     } | ||||
|     /* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */ | ||||
|     Matrix preintMeasCov() const { return preintMeasCov_;} | ||||
| 
 | ||||
|   private: | ||||
|     /** Serialization function */ | ||||
| 
 | ||||
|     /// Serialization function
 | ||||
|     friend class boost::serialization::access; | ||||
|     template<class ARCHIVE> | ||||
|     void serialize(ARCHIVE & ar, const unsigned int version) { | ||||
|       ar & BOOST_SERIALIZATION_NVP(biasHat_); | ||||
|       ar & BOOST_SERIALIZATION_NVP(measurementCovariance_); | ||||
|       ar & BOOST_SERIALIZATION_NVP(deltaPij_); | ||||
|       ar & BOOST_SERIALIZATION_NVP(deltaVij_); | ||||
|       ar & BOOST_SERIALIZATION_NVP(deltaRij_); | ||||
|       ar & BOOST_SERIALIZATION_NVP(deltaTij_); | ||||
|       ar & BOOST_SERIALIZATION_NVP(delPdelBiasAcc_); | ||||
|       ar & BOOST_SERIALIZATION_NVP(delPdelBiasOmega_); | ||||
|       ar & BOOST_SERIALIZATION_NVP(delVdelBiasAcc_); | ||||
|       ar & BOOST_SERIALIZATION_NVP(delVdelBiasOmega_); | ||||
|       ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega_); | ||||
|       ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationBase); | ||||
|       ar & BOOST_SERIALIZATION_NVP(preintMeasCov_); | ||||
|     } | ||||
|   }; | ||||
| 
 | ||||
|  | @ -203,12 +149,7 @@ private: | |||
|   typedef CombinedImuFactor This; | ||||
|   typedef NoiseModelFactor6<Pose3,Vector3,Pose3,Vector3,imuBias::ConstantBias,imuBias::ConstantBias> Base; | ||||
| 
 | ||||
|   CombinedPreintegratedMeasurements preintegratedMeasurements_; | ||||
|   Vector3 gravity_; | ||||
|   Vector3 omegaCoriolis_; | ||||
|   boost::optional<Pose3> body_P_sensor_; ///< The pose of the sensor in the body frame
 | ||||
| 
 | ||||
|   bool use2ndOrderCoriolis_; ///< Controls whether higher order terms are included when calculating the Coriolis Effect
 | ||||
|   CombinedPreintegratedMeasurements _PIM_; | ||||
| 
 | ||||
| public: | ||||
| 
 | ||||
|  | @ -257,11 +198,7 @@ public: | |||
|   /** Access the preintegrated measurements. */ | ||||
| 
 | ||||
|   const CombinedPreintegratedMeasurements& preintegratedMeasurements() const { | ||||
|     return preintegratedMeasurements_; } | ||||
| 
 | ||||
|   const Vector3& gravity() const { return gravity_; } | ||||
| 
 | ||||
|   const Vector3& omegaCoriolis() const { return omegaCoriolis_; } | ||||
|     return _PIM_; } | ||||
| 
 | ||||
|   /** implement functions needed to derive from Factor */ | ||||
| 
 | ||||
|  | @ -275,12 +212,6 @@ public: | |||
|       boost::optional<Matrix&> H5 = boost::none, | ||||
|       boost::optional<Matrix&> H6 = boost::none) const; | ||||
| 
 | ||||
|   /// predicted states from IMU
 | ||||
|   static PoseVelocityBias Predict(const Pose3& pose_i, const Vector3& vel_i, | ||||
|       const imuBias::ConstantBias& bias_i, | ||||
|       const CombinedPreintegratedMeasurements& preintegratedMeasurements, | ||||
|       const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false); | ||||
| 
 | ||||
| private: | ||||
| 
 | ||||
|   /** Serialization function */ | ||||
|  | @ -289,7 +220,7 @@ private: | |||
|   void serialize(ARCHIVE & ar, const unsigned int version) { | ||||
|     ar & boost::serialization::make_nvp("NoiseModelFactor6", | ||||
|         boost::serialization::base_object<Base>(*this)); | ||||
|     ar & BOOST_SERIALIZATION_NVP(preintegratedMeasurements_); | ||||
|     ar & BOOST_SERIALIZATION_NVP(_PIM_); | ||||
|     ar & BOOST_SERIALIZATION_NVP(gravity_); | ||||
|     ar & BOOST_SERIALIZATION_NVP(omegaCoriolis_); | ||||
|     ar & BOOST_SERIALIZATION_NVP(body_P_sensor_); | ||||
|  |  | |||
|  | @ -35,165 +35,82 @@ ImuFactor::PreintegratedMeasurements::PreintegratedMeasurements( | |||
|     const imuBias::ConstantBias& bias, const Matrix3& measuredAccCovariance, | ||||
|     const Matrix3& measuredOmegaCovariance, const Matrix3& integrationErrorCovariance, | ||||
|     const bool use2ndOrderIntegration) : | ||||
|                   biasHat_(bias), deltaPij_(Vector3::Zero()), deltaVij_(Vector3::Zero()), | ||||
|                   deltaRij_(Rot3()), deltaTij_(0.0), | ||||
|                   delPdelBiasAcc_(Z_3x3), delPdelBiasOmega_(Z_3x3), | ||||
|                   delVdelBiasAcc_(Z_3x3), delVdelBiasOmega_(Z_3x3), | ||||
|                   delRdelBiasOmega_(Z_3x3), use2ndOrderIntegration_(use2ndOrderIntegration) | ||||
|             PreintegrationBase(bias, | ||||
|             measuredAccCovariance, measuredOmegaCovariance, | ||||
|             integrationErrorCovariance, use2ndOrderIntegration) | ||||
| { | ||||
|   measurementCovariance_.setZero(); | ||||
|   measurementCovariance_.block<3,3>(0,0) = integrationErrorCovariance; | ||||
|   measurementCovariance_.block<3,3>(3,3) = measuredAccCovariance; | ||||
|   measurementCovariance_.block<3,3>(6,6) = measuredOmegaCovariance; | ||||
|   PreintMeasCov_.setZero(9,9); | ||||
|   preintMeasCov_.setZero(); | ||||
| } | ||||
| 
 | ||||
| //------------------------------------------------------------------------------
 | ||||
| void ImuFactor::PreintegratedMeasurements::print(const string& s) const { | ||||
|   cout << s << endl; | ||||
|   biasHat_.print("  biasHat"); | ||||
|   cout << "  deltaTij " << deltaTij_ << endl; | ||||
|   cout << "  deltaPij [ " << deltaPij_.transpose() << " ]" << endl; | ||||
|   cout << "  deltaVij [ " << deltaVij_.transpose() << " ]" << endl; | ||||
|   deltaRij_.print("  deltaRij "); | ||||
|   cout << "  measurementCovariance = \n [ " << measurementCovariance_ << " ]" << endl; | ||||
|   cout << "  PreintMeasCov = \n [ " << PreintMeasCov_ << " ]" << endl; | ||||
|   PreintegrationBase::print(s); | ||||
|   cout << "  preintMeasCov = \n [ " << preintMeasCov_ << " ]" << endl; | ||||
| } | ||||
| 
 | ||||
| //------------------------------------------------------------------------------
 | ||||
| bool ImuFactor::PreintegratedMeasurements::equals(const PreintegratedMeasurements& expected, double tol) const { | ||||
|   return biasHat_.equals(expected.biasHat_, tol) | ||||
|   && equal_with_abs_tol(measurementCovariance_, expected.measurementCovariance_, tol) | ||||
|   && equal_with_abs_tol(deltaPij_, expected.deltaPij_, tol) | ||||
|   && equal_with_abs_tol(deltaVij_, expected.deltaVij_, tol) | ||||
|   && deltaRij_.equals(expected.deltaRij_, tol) | ||||
|   && fabs(deltaTij_ - expected.deltaTij_) < tol | ||||
|   && equal_with_abs_tol(delPdelBiasAcc_, expected.delPdelBiasAcc_, tol) | ||||
|   && equal_with_abs_tol(delPdelBiasOmega_, expected.delPdelBiasOmega_, tol) | ||||
|   && equal_with_abs_tol(delVdelBiasAcc_, expected.delVdelBiasAcc_, tol) | ||||
|   && equal_with_abs_tol(delVdelBiasOmega_, expected.delVdelBiasOmega_, tol) | ||||
|   && equal_with_abs_tol(delRdelBiasOmega_, expected.delRdelBiasOmega_, tol); | ||||
|   return equal_with_abs_tol(preintMeasCov_, expected.preintMeasCov_, tol) | ||||
|   && PreintegrationBase::equals(expected, tol); | ||||
| } | ||||
| 
 | ||||
| //------------------------------------------------------------------------------
 | ||||
| void ImuFactor::PreintegratedMeasurements::resetIntegration(){ | ||||
|   deltaPij_ = Vector3::Zero(); | ||||
|   deltaVij_ = Vector3::Zero(); | ||||
|   deltaRij_ = Rot3(); | ||||
|   deltaTij_ = 0.0; | ||||
|   delPdelBiasAcc_ = Z_3x3; | ||||
|   delPdelBiasOmega_ = Z_3x3; | ||||
|   delVdelBiasAcc_ = Z_3x3; | ||||
|   delVdelBiasOmega_ = Z_3x3; | ||||
|   delRdelBiasOmega_ = Z_3x3; | ||||
|   PreintMeasCov_.setZero(); | ||||
|   PreintegrationBase::resetIntegration(); | ||||
|   preintMeasCov_.setZero(); | ||||
| } | ||||
| 
 | ||||
| //------------------------------------------------------------------------------
 | ||||
| void ImuFactor::PreintegratedMeasurements::integrateMeasurement( | ||||
|     const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT, | ||||
|     boost::optional<const Pose3&> body_P_sensor) { | ||||
|     boost::optional<const Pose3&> body_P_sensor, | ||||
|     OptionalJacobian<9, 9> F_test, OptionalJacobian<9, 9> G_test) { | ||||
| 
 | ||||
|   // NOTE: order is important here because each update uses old values (i.e., we have to update
 | ||||
|   // jacobians and covariances before updating preintegrated measurements).
 | ||||
|   Vector3 correctedAcc, correctedOmega; | ||||
|   correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega, correctedAcc, correctedOmega, body_P_sensor); | ||||
| 
 | ||||
|   // First we compensate the measurements for the bias
 | ||||
|   Vector3 correctedAcc = biasHat_.correctAccelerometer(measuredAcc); | ||||
|   Vector3 correctedOmega = biasHat_.correctGyroscope(measuredOmega); | ||||
| 
 | ||||
|   // Then compensate for sensor-body displacement: we express the quantities (originally in the IMU frame) into the body frame
 | ||||
|   if(body_P_sensor){ | ||||
|     Matrix3 body_R_sensor = body_P_sensor->rotation().matrix(); | ||||
|     correctedOmega = body_R_sensor * correctedOmega; // rotation rate vector in the body frame
 | ||||
|     Matrix3 body_omega_body__cross = skewSymmetric(correctedOmega); | ||||
|     correctedAcc = body_R_sensor * correctedAcc - body_omega_body__cross * body_omega_body__cross * body_P_sensor->translation().vector(); | ||||
|     // linear acceleration vector in the body frame
 | ||||
|   } | ||||
| 
 | ||||
|   const Vector3 theta_incr = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement
 | ||||
|   const Rot3 Rincr = Rot3::Expmap(theta_incr); // rotation increment computed from the current rotation rate measurement
 | ||||
| 
 | ||||
|   const Matrix3 Jr_theta_incr = Rot3::ExpmapDerivative(theta_incr); // Right jacobian computed at theta_incr
 | ||||
|   const Vector3 integratedOmega = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement
 | ||||
|   Matrix3 D_Rincr_integratedOmega; // Right jacobian computed at theta_incr
 | ||||
|   const Rot3 Rincr = Rot3::Expmap(integratedOmega, D_Rincr_integratedOmega); // rotation increment computed from the current rotation rate measurement
 | ||||
| 
 | ||||
|   // Update Jacobians
 | ||||
|   /* ----------------------------------------------------------------------------------------------------------------------- */ | ||||
|   if(!use2ndOrderIntegration_){ | ||||
|     delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT; | ||||
|     delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT; | ||||
|   }else{ | ||||
|     delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT - 0.5 * deltaRij_.matrix() * deltaT*deltaT; | ||||
|     delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT - 0.5 * deltaRij_.matrix() | ||||
|                                             * skewSymmetric(biasHat_.correctAccelerometer(measuredAcc)) * deltaT*deltaT * delRdelBiasOmega_; | ||||
|   } | ||||
|   delVdelBiasAcc_ += -deltaRij_.matrix() * deltaT; | ||||
|   delVdelBiasOmega_ += -deltaRij_.matrix() * skewSymmetric(correctedAcc) * deltaT * delRdelBiasOmega_; | ||||
|   delRdelBiasOmega_ = Rincr.inverse().matrix() * delRdelBiasOmega_ - Jr_theta_incr  * deltaT; | ||||
|   updatePreintegratedJacobians(correctedAcc, D_Rincr_integratedOmega, Rincr, deltaT); | ||||
| 
 | ||||
|   // Update preintegrated measurements covariance
 | ||||
|   // Update preintegrated measurements (also get Jacobian)
 | ||||
|   const Matrix3 R_i = deltaRij(); // store this, which is useful to compute G_test
 | ||||
|   Matrix9 F; // overall Jacobian wrt preintegrated measurements (df/dx)
 | ||||
|   updatePreintegratedMeasurements(correctedAcc, Rincr, deltaT, F); | ||||
| 
 | ||||
|   // first order covariance propagation:
 | ||||
|   // as in [2] we consider a first order propagation that can be seen as a prediction phase in an EKF framework
 | ||||
|   /* ----------------------------------------------------------------------------------------------------------------------- */ | ||||
|   const Vector3 theta_i = Rot3::Logmap(deltaRij_); // parametrization of so(3)
 | ||||
|   const Matrix3 Jr_theta_i = Rot3::ExpmapDerivative(theta_i); | ||||
| 
 | ||||
|   Rot3 Rot_j = deltaRij_ * Rincr; | ||||
|   const Vector3 theta_j = Rot3::Logmap(Rot_j); // parametrization of so(3)
 | ||||
|   const Matrix3 Jrinv_theta_j = Rot3::LogmapDerivative(theta_j); | ||||
| 
 | ||||
|   Matrix H_pos_pos    = I_3x3; | ||||
|   Matrix H_pos_vel    = I_3x3 * deltaT; | ||||
|   Matrix H_pos_angles = Z_3x3; | ||||
| 
 | ||||
|   Matrix H_vel_pos    = Z_3x3; | ||||
|   Matrix H_vel_vel    = I_3x3; | ||||
|   Matrix H_vel_angles = - deltaRij_.matrix() * skewSymmetric(correctedAcc) * Jr_theta_i * deltaT; | ||||
|   // analytic expression corresponding to the following numerical derivative
 | ||||
|   // Matrix H_vel_angles = numericalDerivative11<Vector3, Vector3>(boost::bind(&PreIntegrateIMUObservations_delta_vel, correctedOmega, correctedAcc, deltaT, _1, deltaVij), theta_i);
 | ||||
| 
 | ||||
|   Matrix H_angles_pos   = Z_3x3; | ||||
|   Matrix H_angles_vel    = Z_3x3; | ||||
|   Matrix H_angles_angles = Jrinv_theta_j * Rincr.inverse().matrix() * Jr_theta_i; | ||||
|   // analytic expression corresponding to the following numerical derivative
 | ||||
|   // Matrix H_angles_angles = numericalDerivative11<Vector3, Vector3>(boost::bind(&PreIntegrateIMUObservations_delta_angles, correctedOmega, deltaT, _1), thetaij);
 | ||||
| 
 | ||||
|   // overall Jacobian wrt preintegrated measurements (df/dx)
 | ||||
|   Matrix F(9,9); | ||||
|   F << H_pos_pos, H_pos_vel,  H_pos_angles, | ||||
|       H_vel_pos, H_vel_vel, H_vel_angles, | ||||
|       H_angles_pos, H_angles_vel, H_angles_angles; | ||||
| 
 | ||||
|   // first order uncertainty propagation:
 | ||||
|   // the deltaT allows to pass from continuous time noise to discrete time noise
 | ||||
|   // preintMeasCov = F * preintMeasCov * F.transpose() + G * (1/deltaT) * measurementCovariance * G.transpose();
 | ||||
|   // NOTE 1: (1/deltaT) allows to pass from continuous time noise to discrete time noise
 | ||||
|   // measurementCovariance_discrete = measurementCovariance_contTime * (1/deltaT)
 | ||||
|   // Gt * Qt * G =(approx)= measurementCovariance_discrete * deltaT^2 = measurementCovariance_contTime * deltaT
 | ||||
|   PreintMeasCov_ = F * PreintMeasCov_ * F.transpose() + measurementCovariance_ * deltaT ; | ||||
|   // NOTE 2: the computation of G * (1/deltaT) * measurementCovariance * G.transpose() is done blockwise,
 | ||||
|   // as G and measurementCovariance are blockdiagonal matrices
 | ||||
|   preintMeasCov_ = F * preintMeasCov_ * F.transpose(); | ||||
|   preintMeasCov_.block<3,3>(0,0) += integrationCovariance() * deltaT; | ||||
|   preintMeasCov_.block<3,3>(3,3) += R_i * accelerometerCovariance() * R_i.transpose() * deltaT; | ||||
|   preintMeasCov_.block<3,3>(6,6) += D_Rincr_integratedOmega * gyroscopeCovariance() * D_Rincr_integratedOmega.transpose() * deltaT; | ||||
| 
 | ||||
|   // Extended version, without approximation: Gt * Qt * G =(approx)= measurementCovariance_contTime * deltaT
 | ||||
|   // This in only kept for documentation.
 | ||||
|   //
 | ||||
|   // Matrix G(9,9);
 | ||||
|   // G << I_3x3 * deltaT, Z_3x3,  Z_3x3,
 | ||||
|   //      Z_3x3, deltaRij.matrix() * deltaT, Z_3x3,
 | ||||
|   //      Z_3x3, Z_3x3, Jrinv_theta_j * Jr_theta_incr * deltaT;
 | ||||
|   //
 | ||||
|   // PreintMeasCov = F * PreintMeasCov * F.transpose() + G * (1/deltaT) * measurementCovariance * G.transpose();
 | ||||
| 
 | ||||
|   // Update preintegrated measurements (this has to be done after the update of covariances and jacobians!)
 | ||||
|   /* ----------------------------------------------------------------------------------------------------------------------- */ | ||||
|   if(!use2ndOrderIntegration_){ | ||||
|     deltaPij_ += deltaVij_ * deltaT; | ||||
|   }else{ | ||||
|     deltaPij_ += deltaVij_ * deltaT + 0.5 * deltaRij_.matrix() * biasHat_.correctAccelerometer(measuredAcc) * deltaT*deltaT; | ||||
|   // F_test and G_test are given as output for testing purposes and are not needed by the factor
 | ||||
|   if(F_test){ // This in only for testing
 | ||||
|     (*F_test) << F; | ||||
|   } | ||||
|   if(G_test){ // This in only for testing & documentation, while the actual computation is done block-wise
 | ||||
|     //           intNoise         accNoise      omegaNoise
 | ||||
|     (*G_test) << I_3x3 * deltaT,   Z_3x3,        Z_3x3,                                 // pos
 | ||||
|                  Z_3x3,            R_i * deltaT, Z_3x3,                                 // vel
 | ||||
|                  Z_3x3,            Z_3x3,        D_Rincr_integratedOmega * deltaT;      // angle
 | ||||
|   } | ||||
|   deltaVij_ += deltaRij_.matrix() * correctedAcc * deltaT; | ||||
|   deltaRij_ = deltaRij_ * Rincr; | ||||
|   deltaTij_ += deltaT; | ||||
| } | ||||
| 
 | ||||
| //------------------------------------------------------------------------------
 | ||||
| // ImuFactor methods
 | ||||
| //------------------------------------------------------------------------------
 | ||||
| ImuFactor::ImuFactor() : | ||||
|     preintegratedMeasurements_(imuBias::ConstantBias(), Z_3x3, Z_3x3, Z_3x3), use2ndOrderCoriolis_(false){} | ||||
|     ImuFactorBase(), _PIM_(imuBias::ConstantBias(), Z_3x3, Z_3x3, Z_3x3) {} | ||||
| 
 | ||||
| //------------------------------------------------------------------------------
 | ||||
| ImuFactor::ImuFactor( | ||||
|  | @ -202,13 +119,10 @@ ImuFactor::ImuFactor( | |||
|     const Vector3& gravity, const Vector3& omegaCoriolis, | ||||
|     boost::optional<const Pose3&> body_P_sensor, | ||||
|     const bool use2ndOrderCoriolis) : | ||||
|         Base(noiseModel::Gaussian::Covariance(preintegratedMeasurements.PreintMeasCov_), pose_i, vel_i, pose_j, vel_j, bias), | ||||
|         preintegratedMeasurements_(preintegratedMeasurements), | ||||
|         gravity_(gravity), | ||||
|         omegaCoriolis_(omegaCoriolis), | ||||
|         body_P_sensor_(body_P_sensor), | ||||
|         use2ndOrderCoriolis_(use2ndOrderCoriolis){ | ||||
| } | ||||
|         Base(noiseModel::Gaussian::Covariance(preintegratedMeasurements.preintMeasCov_), | ||||
|             pose_i, vel_i, pose_j, vel_j, bias), | ||||
|         ImuFactorBase(gravity, omegaCoriolis, body_P_sensor, use2ndOrderCoriolis), | ||||
|         _PIM_(preintegratedMeasurements) {} | ||||
| 
 | ||||
| //------------------------------------------------------------------------------
 | ||||
| gtsam::NonlinearFactor::shared_ptr ImuFactor::clone() const { | ||||
|  | @ -224,215 +138,28 @@ void ImuFactor::print(const string& s, const KeyFormatter& keyFormatter) const { | |||
|       << keyFormatter(this->key3()) << "," | ||||
|       << keyFormatter(this->key4()) << "," | ||||
|       << keyFormatter(this->key5()) << ")\n"; | ||||
|   preintegratedMeasurements_.print("  preintegrated measurements:"); | ||||
|   cout << "  gravity: [ " << gravity_.transpose() << " ]" << endl; | ||||
|   cout << "  omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]" << endl; | ||||
|   ImuFactorBase::print(""); | ||||
|   _PIM_.print("  preintegrated measurements:"); | ||||
|   this->noiseModel_->print("  noise model: "); | ||||
|   if(this->body_P_sensor_) | ||||
|     this->body_P_sensor_->print("  sensor pose in body frame: "); | ||||
| } | ||||
| 
 | ||||
| //------------------------------------------------------------------------------
 | ||||
| bool ImuFactor::equals(const NonlinearFactor& expected, double tol) const { | ||||
|   const This *e =  dynamic_cast<const This*> (&expected); | ||||
|   return e != NULL && Base::equals(*e, tol) | ||||
|   && preintegratedMeasurements_.equals(e->preintegratedMeasurements_, tol) | ||||
|   && equal_with_abs_tol(gravity_, e->gravity_, tol) | ||||
|   && equal_with_abs_tol(omegaCoriolis_, e->omegaCoriolis_, tol) | ||||
|   && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_))); | ||||
|   && _PIM_.equals(e->_PIM_, tol) | ||||
|   && ImuFactorBase::equals(*e, tol); | ||||
| } | ||||
| 
 | ||||
| //------------------------------------------------------------------------------
 | ||||
| Vector ImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j, | ||||
|     const imuBias::ConstantBias& bias, | ||||
|     boost::optional<Matrix&> H1,  boost::optional<Matrix&> H2, | ||||
|     boost::optional<Matrix&> H3,  boost::optional<Matrix&> H4, | ||||
|     boost::optional<Matrix&> H5) const | ||||
| { | ||||
| Vector ImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_i, | ||||
|     const Pose3& pose_j, const Vector3& vel_j, | ||||
|     const imuBias::ConstantBias& bias_i, boost::optional<Matrix&> H1, | ||||
|     boost::optional<Matrix&> H2, boost::optional<Matrix&> H3, | ||||
|     boost::optional<Matrix&> H4, boost::optional<Matrix&> H5) const { | ||||
| 
 | ||||
|   const double& deltaTij = preintegratedMeasurements_.deltaTij_; | ||||
|   const Vector3 biasAccIncr = bias.accelerometer() - preintegratedMeasurements_.biasHat_.accelerometer(); | ||||
|   const Vector3 biasOmegaIncr = bias.gyroscope() - preintegratedMeasurements_.biasHat_.gyroscope(); | ||||
| 
 | ||||
|   // we give some shorter name to rotations and translations
 | ||||
|   const Rot3 Rot_i = pose_i.rotation(); | ||||
|   const Rot3 Rot_j = pose_j.rotation(); | ||||
|   const Vector3 pos_i = pose_i.translation().vector(); | ||||
|   const Vector3 pos_j = pose_j.translation().vector(); | ||||
| 
 | ||||
|   // We compute factor's Jacobians
 | ||||
|   /* ---------------------------------------------------------------------------------------------------- */ | ||||
|   const Rot3 deltaRij_biascorrected = preintegratedMeasurements_.deltaRij_.retract(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr, Rot3::EXPMAP); | ||||
|   // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr)
 | ||||
| 
 | ||||
|   Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected); | ||||
| 
 | ||||
|   Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected  - | ||||
|       Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij; // Coriolis term
 | ||||
| 
 | ||||
|   const Rot3 deltaRij_biascorrected_corioliscorrected = | ||||
|       Rot3::Expmap( theta_biascorrected_corioliscorrected ); | ||||
| 
 | ||||
|   const Rot3 fRhat = deltaRij_biascorrected_corioliscorrected.between(Rot_i.between(Rot_j)); | ||||
| 
 | ||||
|   const Matrix3 Jr_theta_bcc = Rot3::ExpmapDerivative(theta_biascorrected_corioliscorrected); | ||||
| 
 | ||||
|   const Matrix3 Jtheta = -Jr_theta_bcc  * skewSymmetric(Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij); | ||||
| 
 | ||||
|   const Matrix3 Jrinv_fRhat = Rot3::LogmapDerivative(Rot3::Logmap(fRhat)); | ||||
| 
 | ||||
|   if(H1) { | ||||
|     H1->resize(9,6); | ||||
| 
 | ||||
|     Matrix3 dfPdPi; | ||||
|     Matrix3 dfVdPi; | ||||
|     if(use2ndOrderCoriolis_){ | ||||
|       dfPdPi = - Rot_i.matrix() + 0.5 * skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij*deltaTij; | ||||
|       dfVdPi = skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij; | ||||
|     } | ||||
|     else{ | ||||
|       dfPdPi = - Rot_i.matrix(); | ||||
|       dfVdPi = Z_3x3; | ||||
|     } | ||||
| 
 | ||||
|     (*H1) << | ||||
|         // dfP/dRi
 | ||||
|         Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij_ | ||||
|         + preintegratedMeasurements_.delPdelBiasOmega_ * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc_ * biasAccIncr), | ||||
|         // dfP/dPi
 | ||||
|         dfPdPi, | ||||
|         // dfV/dRi
 | ||||
|         Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij_ | ||||
|         + preintegratedMeasurements_.delVdelBiasOmega_ * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc_ * biasAccIncr), | ||||
|         // dfV/dPi
 | ||||
|         dfVdPi, | ||||
|         // dfR/dRi
 | ||||
|         Jrinv_fRhat *  (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta), | ||||
|         // dfR/dPi
 | ||||
|         Z_3x3; | ||||
|   } | ||||
| 
 | ||||
|   if(H2) { | ||||
|     H2->resize(9,3); | ||||
|     (*H2) << | ||||
|         // dfP/dVi
 | ||||
|         - I_3x3 * deltaTij | ||||
|         + skewSymmetric(omegaCoriolis_) * deltaTij * deltaTij,  // Coriolis term - we got rid of the 2 wrt ins paper
 | ||||
|         // dfV/dVi
 | ||||
|         - I_3x3 | ||||
|         + 2 * skewSymmetric(omegaCoriolis_) * deltaTij, // Coriolis term
 | ||||
|         // dfR/dVi
 | ||||
|         Z_3x3; | ||||
|   } | ||||
| 
 | ||||
|   if(H3) { | ||||
|     H3->resize(9,6); | ||||
|     (*H3) << | ||||
|         // dfP/dPosej
 | ||||
|         Z_3x3, Rot_j.matrix(), | ||||
|         // dfV/dPosej
 | ||||
|         Matrix::Zero(3,6), | ||||
|         // dfR/dPosej
 | ||||
|         Jrinv_fRhat *  ( I_3x3 ), Z_3x3; | ||||
|   } | ||||
| 
 | ||||
|   if(H4) { | ||||
|     H4->resize(9,3); | ||||
|     (*H4) << | ||||
|         // dfP/dVj
 | ||||
|         Z_3x3, | ||||
|         // dfV/dVj
 | ||||
|         I_3x3, | ||||
|         // dfR/dVj
 | ||||
|         Z_3x3; | ||||
|   } | ||||
| 
 | ||||
|   if(H5) { | ||||
|     const Matrix3 Jrinv_theta_bc = Rot3::LogmapDerivative(theta_biascorrected); | ||||
|     const Matrix3 Jr_JbiasOmegaIncr = Rot3::ExpmapDerivative(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr); | ||||
|     const Matrix3 JbiasOmega = Jr_theta_bcc * Jrinv_theta_bc * Jr_JbiasOmegaIncr * preintegratedMeasurements_.delRdelBiasOmega_; | ||||
| 
 | ||||
|     H5->resize(9,6); | ||||
|     (*H5) << | ||||
|         // dfP/dBias
 | ||||
|         - Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasAcc_, | ||||
|         - Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasOmega_, | ||||
|         // dfV/dBias
 | ||||
|         - Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasAcc_, | ||||
|         - Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasOmega_, | ||||
|         // dfR/dBias
 | ||||
|         Matrix::Zero(3,3), | ||||
|         Jrinv_fRhat * ( - fRhat.inverse().matrix() * JbiasOmega); | ||||
|   } | ||||
| 
 | ||||
|   // Evaluate residual error, according to [3]
 | ||||
|   /* ---------------------------------------------------------------------------------------------------- */ | ||||
|   const Vector3 fp = | ||||
|       pos_j - pos_i | ||||
|       - Rot_i.matrix() * (preintegratedMeasurements_.deltaPij_ | ||||
|           + preintegratedMeasurements_.delPdelBiasAcc_ * biasAccIncr | ||||
|           + preintegratedMeasurements_.delPdelBiasOmega_ * biasOmegaIncr) | ||||
|           - vel_i * deltaTij | ||||
|           + skewSymmetric(omegaCoriolis_) * vel_i * deltaTij*deltaTij  // Coriolis term - we got rid of the 2 wrt ins paper
 | ||||
|           - 0.5 * gravity_ * deltaTij*deltaTij; | ||||
| 
 | ||||
|   const Vector3 fv = | ||||
|       vel_j - vel_i - Rot_i.matrix() * (preintegratedMeasurements_.deltaVij_ | ||||
|           + preintegratedMeasurements_.delVdelBiasAcc_ * biasAccIncr | ||||
|           + preintegratedMeasurements_.delVdelBiasOmega_ * biasOmegaIncr) | ||||
|           + 2 * skewSymmetric(omegaCoriolis_) * vel_i * deltaTij  // Coriolis term
 | ||||
|           - gravity_ * deltaTij; | ||||
| 
 | ||||
|   const Vector3 fR = Rot3::Logmap(fRhat); | ||||
| 
 | ||||
|   Vector r(9); r << fp, fv, fR; | ||||
|   return r; | ||||
| } | ||||
| 
 | ||||
| //------------------------------------------------------------------------------
 | ||||
| PoseVelocity ImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i, | ||||
|     const imuBias::ConstantBias& bias, const PreintegratedMeasurements preintegratedMeasurements, | ||||
|     const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis) | ||||
| { | ||||
| 
 | ||||
|   const double& deltaTij = preintegratedMeasurements.deltaTij_; | ||||
|   const Vector3 biasAccIncr = bias.accelerometer() - preintegratedMeasurements.biasHat_.accelerometer(); | ||||
|   const Vector3 biasOmegaIncr = bias.gyroscope() - preintegratedMeasurements.biasHat_.gyroscope(); | ||||
| 
 | ||||
|   const Rot3 Rot_i = pose_i.rotation(); | ||||
|   const Vector3 pos_i = pose_i.translation().vector(); | ||||
| 
 | ||||
|   // Predict state at time j
 | ||||
|   /* ---------------------------------------------------------------------------------------------------- */ | ||||
|   Vector3 pos_j =  pos_i + Rot_i.matrix() * (preintegratedMeasurements.deltaPij_ | ||||
|       + preintegratedMeasurements.delPdelBiasAcc_ * biasAccIncr | ||||
|       + preintegratedMeasurements.delPdelBiasOmega_ * biasOmegaIncr) | ||||
|       + vel_i * deltaTij | ||||
|       - skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij  // Coriolis term - we got rid of the 2 wrt ins paper
 | ||||
|       + 0.5 * gravity * deltaTij*deltaTij; | ||||
| 
 | ||||
|   Vector3 vel_j = Vector3(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij_ | ||||
|       + preintegratedMeasurements.delVdelBiasAcc_ * biasAccIncr | ||||
|       + preintegratedMeasurements.delVdelBiasOmega_ * biasOmegaIncr) | ||||
|       - 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij  // Coriolis term
 | ||||
|       + gravity * deltaTij); | ||||
| 
 | ||||
|   if(use2ndOrderCoriolis){ | ||||
|     pos_j += - 0.5 * skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij*deltaTij;  // 2nd order coriolis term for position
 | ||||
|     vel_j += - skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij; // 2nd order term for velocity
 | ||||
|   } | ||||
| 
 | ||||
|   const Rot3 deltaRij_biascorrected = preintegratedMeasurements.deltaRij_.retract(preintegratedMeasurements.delRdelBiasOmega_ * biasOmegaIncr, Rot3::EXPMAP); | ||||
|   // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr)
 | ||||
|   Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected); | ||||
|   Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected  - | ||||
|       Rot_i.inverse().matrix() * omegaCoriolis * deltaTij; // Coriolis term
 | ||||
|   const Rot3 deltaRij_biascorrected_corioliscorrected = | ||||
|       Rot3::Expmap( theta_biascorrected_corioliscorrected ); | ||||
|   const Rot3 Rot_j = Rot_i.compose( deltaRij_biascorrected_corioliscorrected  ); | ||||
| 
 | ||||
|   Pose3 pose_j = Pose3( Rot_j, Point3(pos_j) ); | ||||
|   return PoseVelocity(pose_j, vel_j); | ||||
|   return _PIM_.computeErrorAndJacobians(pose_i, vel_i, pose_j, vel_j, bias_i, | ||||
|       gravity_, omegaCoriolis_, use2ndOrderCoriolis_, H1, H2, H3, H4, H5); | ||||
| } | ||||
| 
 | ||||
| } /// namespace gtsam
 | ||||
|  |  | |||
|  | @ -23,7 +23,8 @@ | |||
| 
 | ||||
| /* GTSAM includes */ | ||||
| #include <gtsam/nonlinear/NonlinearFactor.h> | ||||
| #include <gtsam/navigation/ImuBias.h> | ||||
| #include <gtsam/navigation/PreintegrationBase.h> | ||||
| #include <gtsam/navigation/ImuFactorBase.h> | ||||
| #include <gtsam/base/debug.h> | ||||
| 
 | ||||
| namespace gtsam { | ||||
|  | @ -38,66 +39,46 @@ namespace gtsam { | |||
|  * Int. Conf. on Robotics and Automation (ICRA), 2014. | ||||
|  * | ||||
|  ** REFERENCES: | ||||
|  * [1] G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008. | ||||
|  * [2] T. Lupton and S.Sukkarieh, "Visual-Inertial-Aided Navigation for High-Dynamic Motion in Built | ||||
|  * Environments Without Initial Conditions", TRO, 28(1):61-76, 2012. | ||||
|  * [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor: Computation of the Jacobian Matrices", Tech. Report, 2013. | ||||
|  * [1] G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", | ||||
|  *     Volume 2, 2008. | ||||
|  * [2] T. Lupton and S.Sukkarieh, "Visual-Inertial-Aided Navigation for | ||||
|  *     High-Dynamic Motion in Built Environments Without Initial Conditions", | ||||
|  *     TRO, 28(1):61-76, 2012. | ||||
|  * [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor: | ||||
|  *     Computation of the Jacobian Matrices", Tech. Report, 2013. | ||||
|  */ | ||||
| 
 | ||||
| /**
 | ||||
|  * Struct to hold return variables by the Predict Function | ||||
|  * ImuFactor is a 5-ways factor involving previous state (pose and velocity of | ||||
|  * the vehicle at previous time step), current state (pose and velocity at | ||||
|  * current time step), and the bias estimate. Following the preintegration | ||||
|  * scheme proposed in [2], the ImuFactor includes many IMU measurements, which | ||||
|  * are "summarized" using the PreintegratedMeasurements class. | ||||
|  * Note that this factor does not model "temporal consistency" of the biases | ||||
|  * (which are usually slowly varying quantities), which is up to the caller. | ||||
|  * See also CombinedImuFactor for a class that does this for you. | ||||
|  */ | ||||
| struct PoseVelocity { | ||||
|   Pose3 pose; | ||||
|   Vector3 velocity; | ||||
|   PoseVelocity(const Pose3& _pose, const Vector3& _velocity) : | ||||
|     pose(_pose), velocity(_velocity) { | ||||
|   } | ||||
| }; | ||||
| 
 | ||||
| /**
 | ||||
|  * ImuFactor is a 5-ways factor involving previous state (pose and velocity of the vehicle at previous time step), | ||||
|  * current state (pose and velocity at current time step), and the bias estimate. According to the | ||||
|  * preintegration scheme proposed in [2], the ImuFactor includes many IMU measurements, which are | ||||
|  * "summarized" using the PreintegratedMeasurements class. | ||||
|  * Note that this factor does not force "temporal consistency" of the biases (which are usually | ||||
|  * slowly varying quantities), see also CombinedImuFactor for more details. | ||||
|  */ | ||||
| class ImuFactor: public NoiseModelFactor5<Pose3,Vector3,Pose3,Vector3,imuBias::ConstantBias> { | ||||
| class ImuFactor: public NoiseModelFactor5<Pose3,Vector3,Pose3,Vector3,imuBias::ConstantBias>, public ImuFactorBase { | ||||
| public: | ||||
| 
 | ||||
|   /**
 | ||||
|    * PreintegratedMeasurements accumulates (integrates) the IMU measurements | ||||
|    * (rotation rates and accelerations) and the corresponding covariance matrix. | ||||
|    * The measurements are then used to build the Preintegrated IMU factor (ImuFactor). | ||||
|    * Integration is done incrementally (ideally, one integrates the measurement as soon as it is received | ||||
|    * from the IMU) so as to avoid costly integration at time of factor construction. | ||||
|    * The measurements are then used to build the Preintegrated IMU factor. | ||||
|    * Integration is done incrementally (ideally, one integrates the measurement | ||||
|    * as soon as it is received from the IMU) so as to avoid costly integration | ||||
|    * at time of factor construction. | ||||
|    */ | ||||
|   class PreintegratedMeasurements { | ||||
|   class PreintegratedMeasurements: public PreintegrationBase { | ||||
| 
 | ||||
|     friend class ImuFactor; | ||||
| 
 | ||||
|   protected: | ||||
|     imuBias::ConstantBias biasHat_; ///< Acceleration and angular rate bias values used during preintegration
 | ||||
|     Eigen::Matrix<double,9,9> measurementCovariance_; ///< (continuous-time uncertainty) "Covariance" of the vector [integrationError measuredAcc measuredOmega] in R^(9X9)
 | ||||
| 
 | ||||
|     Vector3 deltaPij_; ///< Preintegrated relative position (does not take into account velocity at time i, see deltap+, in [2]) (in frame i)
 | ||||
|     Vector3 deltaVij_; ///< Preintegrated relative velocity (in global frame)
 | ||||
|     Rot3 deltaRij_;    ///< Preintegrated relative orientation (in frame i)
 | ||||
|     double deltaTij_;  ///< Time interval from i to j
 | ||||
| 
 | ||||
|     Matrix3 delPdelBiasAcc_;   ///< Jacobian of preintegrated position w.r.t. acceleration bias
 | ||||
|     Matrix3 delPdelBiasOmega_; ///< Jacobian of preintegrated position w.r.t. angular rate bias
 | ||||
|     Matrix3 delVdelBiasAcc_;   ///< Jacobian of preintegrated velocity w.r.t. acceleration bias
 | ||||
|     Matrix3 delVdelBiasOmega_; ///< Jacobian of preintegrated velocity w.r.t. angular rate bias
 | ||||
|     Matrix3 delRdelBiasOmega_; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias
 | ||||
| 
 | ||||
|     Eigen::Matrix<double,9,9> PreintMeasCov_; ///< COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION]
 | ||||
|     Eigen::Matrix<double,9,9> preintMeasCov_; ///< COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION]
 | ||||
|     ///< (first-order propagation from *measurementCovariance*).
 | ||||
| 
 | ||||
|     bool use2ndOrderIntegration_; ///< Controls the order of integration
 | ||||
| 
 | ||||
|     public: | ||||
|   public: | ||||
| 
 | ||||
|     /**
 | ||||
|      *  Default constructor, initializes the class with no measurements | ||||
|  | @ -127,160 +108,107 @@ public: | |||
|      * @param measuredOmega Measured angular velocity (as given by the sensor) | ||||
|      * @param deltaT Time interval between two consecutive IMU measurements | ||||
|      * @param body_P_sensor Optional sensor frame (pose of the IMU in the body frame) | ||||
|      * @param Fout, Gout Jacobians used internally (only needed for testing) | ||||
|      */ | ||||
|     void integrateMeasurement(const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT, | ||||
|         boost::optional<const Pose3&> body_P_sensor = boost::none); | ||||
|         boost::optional<const Pose3&> body_P_sensor = boost::none, | ||||
|         OptionalJacobian<9, 9> Fout = boost::none, OptionalJacobian<9, 9> Gout = boost::none); | ||||
| 
 | ||||
|     /// methods to access class variables
 | ||||
|     Matrix measurementCovariance() const {return measurementCovariance_;} | ||||
|     Matrix deltaRij() const {return deltaRij_.matrix();} | ||||
|     double deltaTij() const{return deltaTij_;} | ||||
|     Vector deltaPij() const {return deltaPij_;} | ||||
|     Vector deltaVij() const {return deltaVij_;} | ||||
|     Vector biasHat() const { return biasHat_.vector();} | ||||
|     Matrix delPdelBiasAcc() const { return delPdelBiasAcc_;} | ||||
|     Matrix delPdelBiasOmega() const { return delPdelBiasOmega_;} | ||||
|     Matrix delVdelBiasAcc() const { return delVdelBiasAcc_;} | ||||
|     Matrix delVdelBiasOmega() const { return delVdelBiasOmega_;} | ||||
|     Matrix delRdelBiasOmega() const{ return delRdelBiasOmega_;} | ||||
|     Matrix preintMeasCov() const { return PreintMeasCov_;} | ||||
| 
 | ||||
|     /* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */ | ||||
|     // This function is only used for test purposes (compare numerical derivatives wrt analytic ones)
 | ||||
|     static inline Vector PreIntegrateIMUObservations_delta_vel(const Vector& msr_gyro_t, const Vector& msr_acc_t, const double msr_dt, | ||||
|         const Vector3& delta_angles, const Vector& delta_vel_in_t0){ | ||||
|       // Note: all delta terms refer to an IMU\sensor system at t0
 | ||||
|       Vector body_t_a_body = msr_acc_t; | ||||
|       Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles); | ||||
|       return delta_vel_in_t0 + R_t_to_t0.matrix() * body_t_a_body * msr_dt; | ||||
|     } | ||||
| 
 | ||||
|     // This function is only used for test purposes (compare numerical derivatives wrt analytic ones)
 | ||||
|     static inline Vector PreIntegrateIMUObservations_delta_angles(const Vector& msr_gyro_t, const double msr_dt, | ||||
|         const Vector3& delta_angles){ | ||||
|       // Note: all delta terms refer to an IMU\sensor system at t0
 | ||||
|       // Calculate the corrected measurements using the Bias object
 | ||||
|       Vector body_t_omega_body= msr_gyro_t; | ||||
|       Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles); | ||||
|       R_t_to_t0    = R_t_to_t0 * Rot3::Expmap( body_t_omega_body*msr_dt ); | ||||
|       return Rot3::Logmap(R_t_to_t0); | ||||
|     } | ||||
|     /* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */ | ||||
| 
 | ||||
|     private: | ||||
|       /** Serialization function */ | ||||
|       friend class boost::serialization::access; | ||||
|       template<class ARCHIVE> | ||||
|       void serialize(ARCHIVE & ar, const unsigned int version) { | ||||
|         ar & BOOST_SERIALIZATION_NVP(biasHat_); | ||||
|         ar & BOOST_SERIALIZATION_NVP(measurementCovariance_); | ||||
|         ar & BOOST_SERIALIZATION_NVP(deltaPij_); | ||||
|         ar & BOOST_SERIALIZATION_NVP(deltaVij_); | ||||
|         ar & BOOST_SERIALIZATION_NVP(deltaRij_); | ||||
|         ar & BOOST_SERIALIZATION_NVP(deltaTij_); | ||||
|         ar & BOOST_SERIALIZATION_NVP(delPdelBiasAcc_); | ||||
|         ar & BOOST_SERIALIZATION_NVP(delPdelBiasOmega_); | ||||
|         ar & BOOST_SERIALIZATION_NVP(delVdelBiasAcc_); | ||||
|         ar & BOOST_SERIALIZATION_NVP(delVdelBiasOmega_); | ||||
|         ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega_); | ||||
|       } | ||||
|     }; | ||||
|     Matrix preintMeasCov() const { return preintMeasCov_;} | ||||
| 
 | ||||
|   private: | ||||
| 
 | ||||
|     typedef ImuFactor This; | ||||
|     typedef NoiseModelFactor5<Pose3,Vector3,Pose3,Vector3,imuBias::ConstantBias> Base; | ||||
| 
 | ||||
|     PreintegratedMeasurements preintegratedMeasurements_; | ||||
|     Vector3 gravity_; | ||||
|     Vector3 omegaCoriolis_; | ||||
|     boost::optional<Pose3> body_P_sensor_;        ///< The pose of the sensor in the body frame
 | ||||
| 
 | ||||
|     bool use2ndOrderCoriolis_; ///< Controls whether higher order terms are included when calculating the Coriolis Effect
 | ||||
| 
 | ||||
|   public: | ||||
| 
 | ||||
|     /** Shorthand for a smart pointer to a factor */ | ||||
| #if !defined(_MSC_VER) && __GNUC__ == 4 && __GNUC_MINOR__ > 5 | ||||
|     typedef typename boost::shared_ptr<ImuFactor> shared_ptr; | ||||
| #else | ||||
|     typedef boost::shared_ptr<ImuFactor> shared_ptr; | ||||
| #endif | ||||
| 
 | ||||
|     /** Default constructor - only use for serialization */ | ||||
|     ImuFactor(); | ||||
| 
 | ||||
|     /**
 | ||||
|      * Constructor | ||||
|      * @param pose_i Previous pose key | ||||
|      * @param vel_i  Previous velocity key | ||||
|      * @param pose_j Current pose key | ||||
|      * @param vel_j  Current velocity key | ||||
|      * @param bias   Previous bias key | ||||
|      * @param preintegratedMeasurements Preintegrated IMU measurements | ||||
|      * @param gravity Gravity vector expressed in the global frame | ||||
|      * @param omegaCoriolis Rotation rate of the global frame w.r.t. an inertial frame | ||||
|      * @param body_P_sensor Optional pose of the sensor frame in the body frame | ||||
|      * @param use2ndOrderCoriolis When true, the second-order term is used in the calculation of the Coriolis Effect | ||||
|      */ | ||||
|     ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias, | ||||
|         const PreintegratedMeasurements& preintegratedMeasurements, | ||||
|         const Vector3& gravity, const Vector3& omegaCoriolis, | ||||
|         boost::optional<const Pose3&> body_P_sensor = boost::none, const bool use2ndOrderCoriolis = false); | ||||
| 
 | ||||
|     virtual ~ImuFactor() {} | ||||
| 
 | ||||
|     /// @return a deep copy of this factor
 | ||||
|     virtual gtsam::NonlinearFactor::shared_ptr clone() const; | ||||
| 
 | ||||
|     /** implement functions needed for Testable */ | ||||
| 
 | ||||
|     /// print
 | ||||
|     virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; | ||||
| 
 | ||||
|     /// equals
 | ||||
|     virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const; | ||||
| 
 | ||||
|     /** Access the preintegrated measurements. */ | ||||
| 
 | ||||
|     const PreintegratedMeasurements& preintegratedMeasurements() const { | ||||
|       return preintegratedMeasurements_; } | ||||
| 
 | ||||
|     const Vector3& gravity() const { return gravity_; } | ||||
| 
 | ||||
|     const Vector3& omegaCoriolis() const { return omegaCoriolis_; } | ||||
| 
 | ||||
|     /** implement functions needed to derive from Factor */ | ||||
| 
 | ||||
|     /// vector of errors
 | ||||
|     Vector evaluateError(const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j, | ||||
|         const imuBias::ConstantBias& bias, | ||||
|         boost::optional<Matrix&> H1 = boost::none, | ||||
|         boost::optional<Matrix&> H2 = boost::none, | ||||
|         boost::optional<Matrix&> H3 = boost::none, | ||||
|         boost::optional<Matrix&> H4 = boost::none, | ||||
|         boost::optional<Matrix&> H5 = boost::none) const; | ||||
| 
 | ||||
|     /// predicted states from IMU
 | ||||
|     static PoseVelocity Predict(const Pose3& pose_i, const Vector3& vel_i, | ||||
|         const imuBias::ConstantBias& bias, const PreintegratedMeasurements preintegratedMeasurements, | ||||
|         const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false); | ||||
| 
 | ||||
|   private: | ||||
| 
 | ||||
|     /** Serialization function */ | ||||
|     /// Serialization function
 | ||||
|     friend class boost::serialization::access; | ||||
|     template<class ARCHIVE> | ||||
|     void serialize(ARCHIVE & ar, const unsigned int version) { | ||||
|       ar & boost::serialization::make_nvp("NoiseModelFactor5", | ||||
|           boost::serialization::base_object<Base>(*this)); | ||||
|       ar & BOOST_SERIALIZATION_NVP(preintegratedMeasurements_); | ||||
|       ar & BOOST_SERIALIZATION_NVP(gravity_); | ||||
|       ar & BOOST_SERIALIZATION_NVP(omegaCoriolis_); | ||||
|       ar & BOOST_SERIALIZATION_NVP(body_P_sensor_); | ||||
|       ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationBase); | ||||
|       ar & BOOST_SERIALIZATION_NVP(preintMeasCov_); | ||||
|     } | ||||
|   }; // class ImuFactor
 | ||||
|   }; | ||||
| 
 | ||||
|   typedef ImuFactor::PreintegratedMeasurements ImuFactorPreintegratedMeasurements; | ||||
|   private: | ||||
| 
 | ||||
|   typedef ImuFactor This; | ||||
|   typedef NoiseModelFactor5<Pose3,Vector3,Pose3,Vector3,imuBias::ConstantBias> Base; | ||||
| 
 | ||||
|   PreintegratedMeasurements _PIM_; | ||||
| 
 | ||||
|   public: | ||||
| 
 | ||||
|   /** Shorthand for a smart pointer to a factor */ | ||||
| #if !defined(_MSC_VER) && __GNUC__ == 4 && __GNUC_MINOR__ > 5 | ||||
|   typedef typename boost::shared_ptr<ImuFactor> shared_ptr; | ||||
| #else | ||||
|   typedef boost::shared_ptr<ImuFactor> shared_ptr; | ||||
| #endif | ||||
| 
 | ||||
|   /** Default constructor - only use for serialization */ | ||||
|   ImuFactor(); | ||||
| 
 | ||||
|   /**
 | ||||
|    * Constructor | ||||
|    * @param pose_i Previous pose key | ||||
|    * @param vel_i  Previous velocity key | ||||
|    * @param pose_j Current pose key | ||||
|    * @param vel_j  Current velocity key | ||||
|    * @param bias   Previous bias key | ||||
|    * @param preintegratedMeasurements Preintegrated IMU measurements | ||||
|    * @param gravity Gravity vector expressed in the global frame | ||||
|    * @param omegaCoriolis Rotation rate of the global frame w.r.t. an inertial frame | ||||
|    * @param body_P_sensor Optional pose of the sensor frame in the body frame | ||||
|    * @param use2ndOrderCoriolis When true, the second-order term is used in the calculation of the Coriolis Effect | ||||
|    */ | ||||
|   ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias, | ||||
|       const PreintegratedMeasurements& preintegratedMeasurements, | ||||
|       const Vector3& gravity, const Vector3& omegaCoriolis, | ||||
|       boost::optional<const Pose3&> body_P_sensor = boost::none, const bool use2ndOrderCoriolis = false); | ||||
| 
 | ||||
|   virtual ~ImuFactor() {} | ||||
| 
 | ||||
|   /// @return a deep copy of this factor
 | ||||
|   virtual gtsam::NonlinearFactor::shared_ptr clone() const; | ||||
| 
 | ||||
|   /** implement functions needed for Testable */ | ||||
| 
 | ||||
|   /// print
 | ||||
|   virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; | ||||
| 
 | ||||
|   /// equals
 | ||||
|   virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const; | ||||
| 
 | ||||
|   /** Access the preintegrated measurements. */ | ||||
| 
 | ||||
|   const PreintegratedMeasurements& preintegratedMeasurements() const { | ||||
|     return _PIM_; } | ||||
| 
 | ||||
|   /** implement functions needed to derive from Factor */ | ||||
| 
 | ||||
|   /// vector of errors
 | ||||
|   Vector evaluateError(const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j, | ||||
|       const imuBias::ConstantBias& bias, | ||||
|       boost::optional<Matrix&> H1 = boost::none, | ||||
|       boost::optional<Matrix&> H2 = boost::none, | ||||
|       boost::optional<Matrix&> H3 = boost::none, | ||||
|       boost::optional<Matrix&> H4 = boost::none, | ||||
|       boost::optional<Matrix&> H5 = boost::none) const; | ||||
| 
 | ||||
|   private: | ||||
| 
 | ||||
|   /** Serialization function */ | ||||
|   friend class boost::serialization::access; | ||||
|   template<class ARCHIVE> | ||||
|   void serialize(ARCHIVE & ar, const unsigned int version) { | ||||
|     ar & boost::serialization::make_nvp("NoiseModelFactor5", | ||||
|         boost::serialization::base_object<Base>(*this)); | ||||
|     ar & BOOST_SERIALIZATION_NVP(_PIM_); | ||||
|     ar & BOOST_SERIALIZATION_NVP(gravity_); | ||||
|     ar & BOOST_SERIALIZATION_NVP(omegaCoriolis_); | ||||
|     ar & BOOST_SERIALIZATION_NVP(body_P_sensor_); | ||||
|   } | ||||
| }; // class ImuFactor
 | ||||
| 
 | ||||
| typedef ImuFactor::PreintegratedMeasurements ImuFactorPreintegratedMeasurements; | ||||
| 
 | ||||
| } /// namespace gtsam
 | ||||
|  |  | |||
|  | @ -0,0 +1,84 @@ | |||
| /* ----------------------------------------------------------------------------
 | ||||
| 
 | ||||
|  * GTSAM Copyright 2010, Georgia Tech Research Corporation,  | ||||
|  * Atlanta, Georgia 30332-0415 | ||||
|  * All Rights Reserved | ||||
|  * Authors: Frank Dellaert, et al. (see THANKS for the full author list) | ||||
| 
 | ||||
|  * See LICENSE for the license information | ||||
| 
 | ||||
|  * -------------------------------------------------------------------------- */ | ||||
| 
 | ||||
| /**
 | ||||
|  *  @file  PreintegrationBase.h | ||||
|  *  @author Luca Carlone | ||||
|  *  @author Stephen Williams | ||||
|  *  @author Richard Roberts | ||||
|  *  @author Vadim Indelman | ||||
|  *  @author David Jensen | ||||
|  *  @author Frank Dellaert | ||||
|  **/ | ||||
| 
 | ||||
| #pragma once | ||||
| 
 | ||||
| /* GTSAM includes */ | ||||
| #include <gtsam/navigation/ImuBias.h> | ||||
| #include <gtsam/navigation/PreintegrationBase.h> | ||||
| 
 | ||||
| namespace gtsam { | ||||
| 
 | ||||
| class ImuFactorBase { | ||||
| 
 | ||||
| protected: | ||||
| 
 | ||||
|   Vector3 gravity_; | ||||
|   Vector3 omegaCoriolis_; | ||||
|   boost::optional<Pose3> body_P_sensor_;        ///< The pose of the sensor in the body frame
 | ||||
|   bool use2ndOrderCoriolis_; ///< Controls whether higher order terms are included when calculating the Coriolis Effect
 | ||||
| 
 | ||||
| public: | ||||
| 
 | ||||
|   /** Default constructor - only use for serialization */ | ||||
|   ImuFactorBase() : | ||||
|     gravity_(Vector3(0.0,0.0,9.81)), omegaCoriolis_(Vector3(0.0,0.0,0.0)), | ||||
|     body_P_sensor_(boost::none), use2ndOrderCoriolis_(false) {} | ||||
| 
 | ||||
|   /**
 | ||||
|    *  Default constructor, stores basic quantities required by the Imu factors | ||||
|    * @param gravity Gravity vector expressed in the global frame | ||||
|    * @param omegaCoriolis Rotation rate of the global frame w.r.t. an inertial frame | ||||
|    * @param body_P_sensor Optional pose of the sensor frame in the body frame | ||||
|    * @param use2ndOrderCoriolis When true, the second-order term is used in the calculation of the Coriolis Effect | ||||
|    */ | ||||
|   ImuFactorBase(const Vector3& gravity, const Vector3& omegaCoriolis, | ||||
|       boost::optional<const Pose3&> body_P_sensor = boost::none, const bool use2ndOrderCoriolis = false) : | ||||
|         gravity_(gravity), omegaCoriolis_(omegaCoriolis), | ||||
|         body_P_sensor_(body_P_sensor), use2ndOrderCoriolis_(use2ndOrderCoriolis) {} | ||||
| 
 | ||||
|   /// Methods to access class variables
 | ||||
|   const Vector3& gravity() const { return gravity_; } | ||||
|   const Vector3& omegaCoriolis() const { return omegaCoriolis_; } | ||||
| 
 | ||||
|   /// Needed for testable
 | ||||
|   //------------------------------------------------------------------------------
 | ||||
|   void print(const std::string& s) const { | ||||
|     std::cout << "  gravity: [ " << gravity_.transpose() << " ]" << std::endl; | ||||
|     std::cout << "  omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]" << std::endl; | ||||
|     std::cout << "  use2ndOrderCoriolis: [ " << use2ndOrderCoriolis_ << " ]" << std::endl; | ||||
|     if(this->body_P_sensor_) | ||||
|       this->body_P_sensor_->print("  sensor pose in body frame: "); | ||||
|   } | ||||
| 
 | ||||
|   /// Needed for testable
 | ||||
|   //------------------------------------------------------------------------------
 | ||||
|   bool equals(const ImuFactorBase& expected, double tol) const { | ||||
|     return equal_with_abs_tol(gravity_, expected.gravity_, tol) | ||||
|     && equal_with_abs_tol(omegaCoriolis_, expected.omegaCoriolis_, tol) | ||||
|     && (use2ndOrderCoriolis_ == expected.use2ndOrderCoriolis_) | ||||
|     && ((!body_P_sensor_ && !expected.body_P_sensor_) || | ||||
|         (body_P_sensor_ && expected.body_P_sensor_ && body_P_sensor_->equals(*expected.body_P_sensor_))); | ||||
|   } | ||||
| 
 | ||||
| }; | ||||
| 
 | ||||
| } /// namespace gtsam
 | ||||
|  | @ -0,0 +1,141 @@ | |||
| /* ----------------------------------------------------------------------------
 | ||||
| 
 | ||||
|  * GTSAM Copyright 2010, Georgia Tech Research Corporation,  | ||||
|  * Atlanta, Georgia 30332-0415 | ||||
|  * All Rights Reserved | ||||
|  * Authors: Frank Dellaert, et al. (see THANKS for the full author list) | ||||
| 
 | ||||
|  * See LICENSE for the license information | ||||
| 
 | ||||
|  * -------------------------------------------------------------------------- */ | ||||
| 
 | ||||
| /**
 | ||||
|  *  @file  PreintegratedRotation.h | ||||
|  *  @author Luca Carlone | ||||
|  *  @author Stephen Williams | ||||
|  *  @author Richard Roberts | ||||
|  *  @author Vadim Indelman | ||||
|  *  @author David Jensen | ||||
|  *  @author Frank Dellaert | ||||
|  **/ | ||||
| 
 | ||||
| #pragma once | ||||
| 
 | ||||
| #include <gtsam/geometry/Rot3.h> | ||||
| 
 | ||||
| namespace gtsam { | ||||
| 
 | ||||
| /**
 | ||||
|  * PreintegratedRotation is the base class for all PreintegratedMeasurements | ||||
|  * classes (in AHRSFactor, ImuFactor, and CombinedImuFactor). | ||||
|  * It includes the definitions of the preintegrated rotation. | ||||
|  */ | ||||
| class PreintegratedRotation { | ||||
| 
 | ||||
|   Rot3 deltaRij_;    ///< Preintegrated relative orientation (in frame i)
 | ||||
|   double deltaTij_;  ///< Time interval from i to j
 | ||||
| 
 | ||||
|   /// Jacobian of preintegrated rotation w.r.t. angular rate bias
 | ||||
|   Matrix3 delRdelBiasOmega_; | ||||
|   Matrix3 gyroscopeCovariance_;     ///< continuous-time "Covariance" of gyroscope measurements
 | ||||
| 
 | ||||
| public: | ||||
| 
 | ||||
|   /**
 | ||||
|    *  Default constructor, initializes the variables in the base class | ||||
|    */ | ||||
|   PreintegratedRotation(const Matrix3& measuredOmegaCovariance) : | ||||
|     deltaRij_(Rot3()), deltaTij_(0.0), | ||||
|     delRdelBiasOmega_(Z_3x3), gyroscopeCovariance_(measuredOmegaCovariance) {} | ||||
| 
 | ||||
|   /// methods to access class variables
 | ||||
|   Matrix3 deltaRij() const {return deltaRij_.matrix();} // expensive
 | ||||
|   Vector3 thetaRij(boost::optional<Matrix3&> H = boost::none) const {return Rot3::Logmap(deltaRij_, H);} // super-expensive
 | ||||
|   const double& deltaTij() const{return deltaTij_;} | ||||
|   const Matrix3& delRdelBiasOmega() const{ return delRdelBiasOmega_;} | ||||
|   const Matrix3& gyroscopeCovariance() const { return gyroscopeCovariance_;} | ||||
| 
 | ||||
|   /// Needed for testable
 | ||||
|   void print(const std::string& s) const { | ||||
|     std::cout << s << std::endl; | ||||
|     std::cout << "deltaTij [" << deltaTij_ << "]" << std::endl; | ||||
|     deltaRij_.print("  deltaRij "); | ||||
|     std::cout << "delRdelBiasOmega ["    << delRdelBiasOmega_ << "]" << std::endl; | ||||
|     std::cout << "gyroscopeCovariance [" << gyroscopeCovariance_ << "]" << std::endl; | ||||
|   } | ||||
| 
 | ||||
|   /// Needed for testable
 | ||||
|   bool equals(const PreintegratedRotation& expected, double tol) const { | ||||
|     return deltaRij_.equals(expected.deltaRij_, tol) | ||||
|     && fabs(deltaTij_ - expected.deltaTij_) < tol | ||||
|     && equal_with_abs_tol(delRdelBiasOmega_, expected.delRdelBiasOmega_, tol) | ||||
|     && equal_with_abs_tol(gyroscopeCovariance_, expected.gyroscopeCovariance_, tol); | ||||
|   } | ||||
| 
 | ||||
|   /// Re-initialize PreintegratedMeasurements
 | ||||
|   void resetIntegration(){ | ||||
|     deltaRij_ = Rot3(); | ||||
|     deltaTij_ = 0.0; | ||||
|     delRdelBiasOmega_ = Z_3x3; | ||||
|   } | ||||
| 
 | ||||
|   /// Update preintegrated measurements
 | ||||
|   void updateIntegratedRotationAndDeltaT(const Rot3& incrR, const double deltaT, | ||||
|       OptionalJacobian<3, 3> H = boost::none){ | ||||
|     deltaRij_ = deltaRij_.compose(incrR, H, boost::none); | ||||
|     deltaTij_ += deltaT; | ||||
|   } | ||||
| 
 | ||||
|   /**
 | ||||
|    *  Update Jacobians to be used during preintegration | ||||
|    *  TODO: explain arguments | ||||
|    */ | ||||
|   void update_delRdelBiasOmega(const Matrix3& D_Rincr_integratedOmega, const Rot3& incrR, | ||||
|       double deltaT) { | ||||
|     const Matrix3 incrRt = incrR.transpose(); | ||||
|     delRdelBiasOmega_ = incrRt * delRdelBiasOmega_ - D_Rincr_integratedOmega * deltaT; | ||||
|   } | ||||
| 
 | ||||
|   /// Return a bias corrected version of the integrated rotation - expensive
 | ||||
|   Rot3 biascorrectedDeltaRij(const Vector3& biasOmegaIncr) const { | ||||
|     return deltaRij_*Rot3::Expmap(delRdelBiasOmega_ * biasOmegaIncr); | ||||
|   } | ||||
| 
 | ||||
|   /// Get so<3> version of bias corrected rotation, with optional Jacobian
 | ||||
|   // Implements: log( deltaRij_ * expmap(delRdelBiasOmega_ * biasOmegaIncr) )
 | ||||
|   Vector3 biascorrectedThetaRij(const Vector3& biasOmegaIncr, | ||||
|       OptionalJacobian<3, 3> H = boost::none) const { | ||||
|     // First, we correct deltaRij using the biasOmegaIncr, rotated
 | ||||
|     const Rot3 deltaRij_biascorrected = biascorrectedDeltaRij(biasOmegaIncr); | ||||
| 
 | ||||
|     if (H) { | ||||
|       Matrix3 Jrinv_theta_bc; | ||||
|       // This was done via an expmap, now we go *back* to so<3>
 | ||||
|       const Vector3 biascorrectedOmega = Rot3::Logmap(deltaRij_biascorrected, Jrinv_theta_bc); | ||||
|       const Matrix3 Jr_JbiasOmegaIncr = //
 | ||||
|           Rot3::ExpmapDerivative(delRdelBiasOmega_ * biasOmegaIncr); | ||||
|       (*H) = Jrinv_theta_bc * Jr_JbiasOmegaIncr * delRdelBiasOmega_; | ||||
|       return biascorrectedOmega; | ||||
|     } | ||||
|     //else
 | ||||
|     return Rot3::Logmap(deltaRij_biascorrected); | ||||
|   } | ||||
| 
 | ||||
|   /// Integrate coriolis correction in body frame rot_i
 | ||||
|   Vector3 integrateCoriolis(const Rot3& rot_i, | ||||
|       const Vector3& omegaCoriolis) const { | ||||
|     return rot_i.transpose() * omegaCoriolis * deltaTij(); | ||||
|   } | ||||
| 
 | ||||
| private: | ||||
|   /** Serialization function */ | ||||
|   friend class boost::serialization::access; | ||||
|   template<class ARCHIVE> | ||||
|   void serialize(ARCHIVE & ar, const unsigned int version) { | ||||
|     ar & BOOST_SERIALIZATION_NVP(deltaRij_); | ||||
|     ar & BOOST_SERIALIZATION_NVP(deltaTij_); | ||||
|     ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega_); | ||||
|   } | ||||
| }; | ||||
| 
 | ||||
| } /// namespace gtsam
 | ||||
|  | @ -0,0 +1,425 @@ | |||
| /* ----------------------------------------------------------------------------
 | ||||
| 
 | ||||
|  * GTSAM Copyright 2010, Georgia Tech Research Corporation,  | ||||
|  * Atlanta, Georgia 30332-0415 | ||||
|  * All Rights Reserved | ||||
|  * Authors: Frank Dellaert, et al. (see THANKS for the full author list) | ||||
| 
 | ||||
|  * See LICENSE for the license information | ||||
| 
 | ||||
|  * -------------------------------------------------------------------------- */ | ||||
| 
 | ||||
| /**
 | ||||
|  *  @file  PreintegrationBase.h | ||||
|  *  @author Luca Carlone | ||||
|  *  @author Stephen Williams | ||||
|  *  @author Richard Roberts | ||||
|  *  @author Vadim Indelman | ||||
|  *  @author David Jensen | ||||
|  *  @author Frank Dellaert | ||||
|  **/ | ||||
| 
 | ||||
| #pragma once | ||||
| 
 | ||||
| #include <gtsam/navigation/PreintegratedRotation.h> | ||||
| #include <gtsam/navigation/ImuBias.h> | ||||
| 
 | ||||
| namespace gtsam { | ||||
| 
 | ||||
| /**
 | ||||
|  * Struct to hold all state variables of returned by Predict function | ||||
|  */ | ||||
| struct PoseVelocityBias { | ||||
|   Pose3 pose; | ||||
|   Vector3 velocity; | ||||
|   imuBias::ConstantBias bias; | ||||
| 
 | ||||
|   PoseVelocityBias(const Pose3& _pose, const Vector3& _velocity, | ||||
|       const imuBias::ConstantBias _bias) : | ||||
|         pose(_pose), velocity(_velocity), bias(_bias) { | ||||
|   } | ||||
| }; | ||||
| 
 | ||||
| /**
 | ||||
|  * PreintegrationBase is the base class for PreintegratedMeasurements | ||||
|  * (in ImuFactor) and CombinedPreintegratedMeasurements (in CombinedImuFactor). | ||||
|  * It includes the definitions of the preintegrated variables and the methods | ||||
|  * to access, print, and compare them. | ||||
|  */ | ||||
| class PreintegrationBase : public PreintegratedRotation { | ||||
| 
 | ||||
|   imuBias::ConstantBias biasHat_; ///< Acceleration and angular rate bias values used during preintegration
 | ||||
|   bool use2ndOrderIntegration_; ///< Controls the order of integration
 | ||||
| 
 | ||||
|   Vector3 deltaPij_; ///< Preintegrated relative position (does not take into account velocity at time i, see deltap+, in [2]) (in frame i)
 | ||||
|   Vector3 deltaVij_; ///< Preintegrated relative velocity (in global frame)
 | ||||
| 
 | ||||
|   Matrix3 delPdelBiasAcc_;   ///< Jacobian of preintegrated position w.r.t. acceleration bias
 | ||||
|   Matrix3 delPdelBiasOmega_; ///< Jacobian of preintegrated position w.r.t. angular rate bias
 | ||||
|   Matrix3 delVdelBiasAcc_;   ///< Jacobian of preintegrated velocity w.r.t. acceleration bias
 | ||||
|   Matrix3 delVdelBiasOmega_; ///< Jacobian of preintegrated velocity w.r.t. angular rate bias
 | ||||
| 
 | ||||
|   Matrix3 accelerometerCovariance_; ///< continuous-time "Covariance" of accelerometer measurements
 | ||||
|   Matrix3 integrationCovariance_;   ///< continuous-time "Covariance" describing integration uncertainty
 | ||||
|   /// (to compensate errors in Euler integration)
 | ||||
| 
 | ||||
| public: | ||||
| 
 | ||||
|   /**
 | ||||
|    *  Default constructor, initializes the variables in the base class | ||||
|    *  @param bias Current estimate of acceleration and rotation rate biases | ||||
|    *  @param use2ndOrderIntegration     Controls the order of integration | ||||
|    *  (if false: p(t+1) = p(t) + v(t) deltaT ; if true: p(t+1) = p(t) + v(t) deltaT + 0.5 * acc(t) deltaT^2) | ||||
|    */ | ||||
|   PreintegrationBase(const imuBias::ConstantBias& bias, | ||||
|       const Matrix3&  measuredAccCovariance, const Matrix3& measuredOmegaCovariance, | ||||
|       const Matrix3&integrationErrorCovariance, const bool use2ndOrderIntegration) : | ||||
|         PreintegratedRotation(measuredOmegaCovariance), | ||||
|     biasHat_(bias), use2ndOrderIntegration_(use2ndOrderIntegration), | ||||
|     deltaPij_(Vector3::Zero()), deltaVij_(Vector3::Zero()), | ||||
|     delPdelBiasAcc_(Z_3x3), delPdelBiasOmega_(Z_3x3), | ||||
|     delVdelBiasAcc_(Z_3x3), delVdelBiasOmega_(Z_3x3), | ||||
|     accelerometerCovariance_(measuredAccCovariance), | ||||
|     integrationCovariance_(integrationErrorCovariance) {} | ||||
| 
 | ||||
|   /// methods to access class variables
 | ||||
|   const Vector3& deltaPij() const {return deltaPij_;} | ||||
|   const Vector3& deltaVij() const {return deltaVij_;} | ||||
|   const imuBias::ConstantBias& biasHat() const { return biasHat_;} | ||||
|   Vector6 biasHatVector() const { return biasHat_.vector();} // expensive
 | ||||
|   const Matrix3& delPdelBiasAcc() const { return delPdelBiasAcc_;} | ||||
|   const Matrix3& delPdelBiasOmega() const { return delPdelBiasOmega_;} | ||||
|   const Matrix3& delVdelBiasAcc() const { return delVdelBiasAcc_;} | ||||
|   const Matrix3& delVdelBiasOmega() const { return delVdelBiasOmega_;} | ||||
| 
 | ||||
|   const Matrix3& accelerometerCovariance() const { return accelerometerCovariance_;} | ||||
|   const Matrix3& integrationCovariance() const { return integrationCovariance_;} | ||||
| 
 | ||||
|   /// Needed for testable
 | ||||
|   void print(const std::string& s) const { | ||||
|     PreintegratedRotation::print(s); | ||||
|     std::cout << "  accelerometerCovariance [ " << accelerometerCovariance_ << " ]" << std::endl; | ||||
|     std::cout << "  integrationCovariance [ " << integrationCovariance_ << " ]" << std::endl; | ||||
|     std::cout << "  deltaPij [ " << deltaPij_.transpose() << " ]" << std::endl; | ||||
|     std::cout << "  deltaVij [ " << deltaVij_.transpose() << " ]" << std::endl; | ||||
|     biasHat_.print("  biasHat"); | ||||
|   } | ||||
| 
 | ||||
|   /// Needed for testable
 | ||||
|   bool equals(const PreintegrationBase& expected, double tol) const { | ||||
|     return PreintegratedRotation::equals(expected, tol) | ||||
|     && biasHat_.equals(expected.biasHat_, tol) | ||||
|     && equal_with_abs_tol(deltaPij_, expected.deltaPij_, tol) | ||||
|     && equal_with_abs_tol(deltaVij_, expected.deltaVij_, tol) | ||||
|     && equal_with_abs_tol(delPdelBiasAcc_, expected.delPdelBiasAcc_, tol) | ||||
|     && equal_with_abs_tol(delPdelBiasOmega_, expected.delPdelBiasOmega_, tol) | ||||
|     && equal_with_abs_tol(delVdelBiasAcc_, expected.delVdelBiasAcc_, tol) | ||||
|     && equal_with_abs_tol(delVdelBiasOmega_, expected.delVdelBiasOmega_, tol) | ||||
|     && equal_with_abs_tol(accelerometerCovariance_, expected.accelerometerCovariance_, tol) | ||||
|     && equal_with_abs_tol(integrationCovariance_, expected.integrationCovariance_, tol); | ||||
|   } | ||||
| 
 | ||||
|   /// Re-initialize PreintegratedMeasurements
 | ||||
|   void resetIntegration(){ | ||||
|     PreintegratedRotation::resetIntegration(); | ||||
|     deltaPij_ = Vector3::Zero(); | ||||
|     deltaVij_ = Vector3::Zero(); | ||||
|     delPdelBiasAcc_ = Z_3x3; | ||||
|     delPdelBiasOmega_ = Z_3x3; | ||||
|     delVdelBiasAcc_ = Z_3x3; | ||||
|     delVdelBiasOmega_ = Z_3x3; | ||||
|   } | ||||
| 
 | ||||
|   /// Update preintegrated measurements
 | ||||
|   void updatePreintegratedMeasurements(const Vector3& correctedAcc, | ||||
|       const Rot3& incrR, const double deltaT,  OptionalJacobian<9, 9> F) { | ||||
| 
 | ||||
|     Matrix3 dRij = deltaRij(); // expensive
 | ||||
|     Vector3 temp = dRij * correctedAcc * deltaT; | ||||
|     if(!use2ndOrderIntegration_){ | ||||
|       deltaPij_ += deltaVij_ * deltaT; | ||||
|     }else{ | ||||
|       deltaPij_ += deltaVij_ * deltaT + 0.5 * temp * deltaT; | ||||
|     } | ||||
|     deltaVij_ += temp; | ||||
| 
 | ||||
|     Matrix3 R_i, F_angles_angles; | ||||
|     if (F) R_i = deltaRij(); // has to be executed before updateIntegratedRotationAndDeltaT as that updates deltaRij
 | ||||
|     updateIntegratedRotationAndDeltaT(incrR,deltaT, F ? &F_angles_angles : 0); | ||||
| 
 | ||||
|     if(F){ | ||||
|       Matrix3 F_vel_angles = - R_i * skewSymmetric(correctedAcc) * deltaT; | ||||
|       //    pos          vel              angle
 | ||||
|       *F << I_3x3,       I_3x3 * deltaT,  Z_3x3,          // pos
 | ||||
|             Z_3x3,       I_3x3,           F_vel_angles,   // vel
 | ||||
|             Z_3x3,       Z_3x3,           F_angles_angles;// angle
 | ||||
|     } | ||||
|   } | ||||
| 
 | ||||
|   /// Update Jacobians to be used during preintegration
 | ||||
|   void updatePreintegratedJacobians(const Vector3& correctedAcc, | ||||
|       const Matrix3& D_Rincr_integratedOmega, const Rot3& incrR, double deltaT){ | ||||
|     Matrix3 dRij = deltaRij(); // expensive
 | ||||
|     Matrix3 temp = -dRij * skewSymmetric(correctedAcc) * deltaT * delRdelBiasOmega(); | ||||
|     if (!use2ndOrderIntegration_) { | ||||
|       delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT; | ||||
|       delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT; | ||||
|     } else { | ||||
|       delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT - 0.5 * dRij * deltaT * deltaT; | ||||
|       delPdelBiasOmega_ += deltaT*(delVdelBiasOmega_ + temp * 0.5); | ||||
|     } | ||||
|     delVdelBiasAcc_ += -dRij * deltaT; | ||||
|     delVdelBiasOmega_ += temp; | ||||
|     update_delRdelBiasOmega(D_Rincr_integratedOmega,incrR,deltaT); | ||||
|   } | ||||
| 
 | ||||
|   void correctMeasurementsByBiasAndSensorPose(const Vector3& measuredAcc, | ||||
|       const Vector3& measuredOmega, Vector3& correctedAcc, | ||||
|       Vector3& correctedOmega, boost::optional<const Pose3&> body_P_sensor) { | ||||
|     correctedAcc = biasHat_.correctAccelerometer(measuredAcc); | ||||
|     correctedOmega = biasHat_.correctGyroscope(measuredOmega); | ||||
| 
 | ||||
|     // Then compensate for sensor-body displacement: we express the quantities
 | ||||
|     // (originally in the IMU frame) into the body frame
 | ||||
|     if(body_P_sensor){ | ||||
|       Matrix3 body_R_sensor = body_P_sensor->rotation().matrix(); | ||||
|       correctedOmega = body_R_sensor * correctedOmega; // rotation rate vector in the body frame
 | ||||
|       Matrix3 body_omega_body__cross = skewSymmetric(correctedOmega); | ||||
|       correctedAcc = body_R_sensor * correctedAcc - body_omega_body__cross * body_omega_body__cross * body_P_sensor->translation().vector(); | ||||
|       // linear acceleration vector in the body frame
 | ||||
|     } | ||||
|   } | ||||
| 
 | ||||
|   /// Predict state at time j
 | ||||
|   //------------------------------------------------------------------------------
 | ||||
|   PoseVelocityBias predict(const Pose3& pose_i, const Vector3& vel_i, | ||||
|       const imuBias::ConstantBias& bias_i, const Vector3& gravity, | ||||
|       const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false, | ||||
|       boost::optional<Vector3&> deltaPij_biascorrected_out = boost::none, | ||||
|       boost::optional<Vector3&> deltaVij_biascorrected_out = boost::none) const { | ||||
| 
 | ||||
|     const Vector3 biasAccIncr = bias_i.accelerometer() - biasHat().accelerometer(); | ||||
|     const Vector3 biasOmegaIncr = bias_i.gyroscope() - biasHat().gyroscope(); | ||||
| 
 | ||||
|     const Rot3& Rot_i = pose_i.rotation(); | ||||
|     const Vector3& pos_i = pose_i.translation().vector(); | ||||
| 
 | ||||
|     // Predict state at time j
 | ||||
|     /* ---------------------------------------------------------------------------------------------------- */ | ||||
|     Vector3 deltaPij_biascorrected = deltaPij() + delPdelBiasAcc() * biasAccIncr + delPdelBiasOmega() * biasOmegaIncr; | ||||
|     if(deltaPij_biascorrected_out)// if desired, store this
 | ||||
|       *deltaPij_biascorrected_out = deltaPij_biascorrected; | ||||
| 
 | ||||
|     Vector3 pos_j =  pos_i + Rot_i.matrix() * deltaPij_biascorrected | ||||
|         + vel_i * deltaTij() | ||||
|         - omegaCoriolis.cross(vel_i) * deltaTij()*deltaTij()  // Coriolis term - we got rid of the 2 wrt ins paper
 | ||||
|         + 0.5 * gravity * deltaTij()*deltaTij(); | ||||
| 
 | ||||
|     Vector3 deltaVij_biascorrected = deltaVij() + delVdelBiasAcc() * biasAccIncr + delVdelBiasOmega() * biasOmegaIncr; | ||||
|     if(deltaVij_biascorrected_out)// if desired, store this
 | ||||
|       *deltaVij_biascorrected_out = deltaVij_biascorrected; | ||||
| 
 | ||||
|     Vector3 vel_j = Vector3(vel_i + Rot_i.matrix() * deltaVij_biascorrected | ||||
|         - 2 * omegaCoriolis.cross(vel_i) * deltaTij()  // Coriolis term
 | ||||
|         + gravity * deltaTij()); | ||||
| 
 | ||||
|     if(use2ndOrderCoriolis){ | ||||
|       pos_j += - 0.5 * omegaCoriolis.cross(omegaCoriolis.cross(pos_i)) * deltaTij()*deltaTij();  // 2nd order coriolis term for position
 | ||||
|       vel_j += - omegaCoriolis.cross(omegaCoriolis.cross(pos_i)) * deltaTij(); // 2nd order term for velocity
 | ||||
|     } | ||||
| 
 | ||||
|     const Rot3 deltaRij_biascorrected = biascorrectedDeltaRij(biasOmegaIncr); | ||||
|     // deltaRij_biascorrected = deltaRij * expmap(delRdelBiasOmega * biasOmegaIncr)
 | ||||
| 
 | ||||
|     Vector3 biascorrectedOmega = Rot3::Logmap(deltaRij_biascorrected); | ||||
|     Vector3 correctedOmega = biascorrectedOmega  - | ||||
|         Rot_i.inverse().matrix() * omegaCoriolis * deltaTij(); // Coriolis term
 | ||||
|     const Rot3 correctedDeltaRij = | ||||
|         Rot3::Expmap( correctedOmega ); | ||||
|     const Rot3 Rot_j = Rot_i.compose( correctedDeltaRij  ); | ||||
| 
 | ||||
|     Pose3 pose_j = Pose3( Rot_j, Point3(pos_j) ); | ||||
|     return PoseVelocityBias(pose_j, vel_j, bias_i); // bias is predicted as a constant
 | ||||
|   } | ||||
| 
 | ||||
|   /// Compute errors w.r.t. preintegrated measurements and jacobians wrt pose_i, vel_i, bias_i, pose_j, bias_j
 | ||||
|   //------------------------------------------------------------------------------
 | ||||
|   Vector9 computeErrorAndJacobians(const Pose3& pose_i, const Vector3& vel_i, | ||||
|       const Pose3& pose_j, const Vector3& vel_j, | ||||
|       const imuBias::ConstantBias& bias_i, const Vector3& gravity, | ||||
|       const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis, | ||||
|       OptionalJacobian<9, 6> H1 = boost::none, | ||||
|       OptionalJacobian<9, 3> H2 = boost::none, | ||||
|       OptionalJacobian<9, 6> H3 = boost::none, | ||||
|       OptionalJacobian<9, 3> H4 = boost::none, | ||||
|       OptionalJacobian<9, 6> H5 = boost::none) const { | ||||
| 
 | ||||
|     // We need the mismatch w.r.t. the biases used for preintegration
 | ||||
|     // const Vector3 biasAccIncr = bias_i.accelerometer() - biasHat().accelerometer(); // this is not necessary
 | ||||
|     const Vector3 biasOmegaIncr = bias_i.gyroscope() - biasHat().gyroscope(); | ||||
| 
 | ||||
|     // we give some shorter name to rotations and translations
 | ||||
|     const Rot3& Ri = pose_i.rotation(); | ||||
|     const Rot3& Rj = pose_j.rotation(); | ||||
|     const Vector3& pos_j = pose_j.translation().vector(); | ||||
| 
 | ||||
|     // Evaluate residual error, according to [3]
 | ||||
|     /* ---------------------------------------------------------------------------------------------------- */ | ||||
|     Vector3 deltaPij_biascorrected, deltaVij_biascorrected; | ||||
|     PoseVelocityBias predictedState_j = predict(pose_i, vel_i, bias_i, gravity, | ||||
|         omegaCoriolis, use2ndOrderCoriolis, deltaPij_biascorrected, deltaVij_biascorrected); | ||||
| 
 | ||||
|     // Ri.transpose() is important here to preserve a model with *additive* Gaussian noise of correct covariance
 | ||||
|     const Vector3 fp = Ri.transpose() * ( pos_j - predictedState_j.pose.translation().vector() ); | ||||
| 
 | ||||
|     // Ri.transpose() is important here to preserve a model with *additive* Gaussian noise of correct covariance
 | ||||
|     const Vector3 fv = Ri.transpose() * ( vel_j - predictedState_j.velocity ); | ||||
| 
 | ||||
|     // fR will be computed later. Note: it is the same as: fR = (predictedState_j.pose.translation()).between(Rot_j)
 | ||||
| 
 | ||||
|     // Jacobian computation
 | ||||
|     /* ---------------------------------------------------------------------------------------------------- */ | ||||
|     // Get Get so<3> version of bias corrected rotation
 | ||||
|     // If H5 is asked for, we will need the Jacobian, which we store in H5
 | ||||
|     // H5 will then be corrected below to take into account the Coriolis effect
 | ||||
|     Matrix3 D_cThetaRij_biasOmegaIncr; | ||||
|     Vector3 biascorrectedOmega = biascorrectedThetaRij(biasOmegaIncr, H5 ? &D_cThetaRij_biasOmegaIncr : 0); | ||||
| 
 | ||||
|     // Coriolis term, note inconsistent with AHRS, where coriolisHat is *after* integration
 | ||||
|     const Matrix3 Ritranspose_omegaCoriolisHat = Ri.transpose() * skewSymmetric(omegaCoriolis); | ||||
|     const Vector3 coriolis = integrateCoriolis(Ri, omegaCoriolis); | ||||
|     Vector3 correctedOmega = biascorrectedOmega  - coriolis; | ||||
| 
 | ||||
|     Rot3 correctedDeltaRij, fRrot; | ||||
|     Vector3 fR; | ||||
| 
 | ||||
|     // Accessory matrix, used to build the jacobians
 | ||||
|     Matrix3 D_cDeltaRij_cOmega, D_coriolis, D_fR_fRrot; | ||||
| 
 | ||||
|     // This is done to save computation: we ask for the jacobians only when they are needed
 | ||||
|     if(H1 || H2 || H3 || H4 || H5){ | ||||
|       correctedDeltaRij = Rot3::Expmap( correctedOmega, D_cDeltaRij_cOmega); | ||||
|       // Residual rotation error
 | ||||
|       fRrot = correctedDeltaRij.between(Ri.between(Rj)); | ||||
|       fR = Rot3::Logmap(fRrot, D_fR_fRrot); | ||||
|       D_coriolis = -D_cDeltaRij_cOmega  * skewSymmetric(coriolis); | ||||
|     }else{ | ||||
|       correctedDeltaRij = Rot3::Expmap( correctedOmega); | ||||
|       // Residual rotation error
 | ||||
|       fRrot = correctedDeltaRij.between(Ri.between(Rj)); | ||||
|       fR = Rot3::Logmap(fRrot); | ||||
|     } | ||||
|     if(H1) { | ||||
|       H1->resize(9,6); | ||||
|       Matrix3 dfPdPi = -I_3x3; | ||||
|       Matrix3 dfVdPi = Z_3x3; | ||||
|       if(use2ndOrderCoriolis){ | ||||
|         // this is the same as: Ri.transpose() * omegaCoriolisHat * omegaCoriolisHat * Ri.matrix()
 | ||||
|         Matrix3 temp = Ritranspose_omegaCoriolisHat * (-Ritranspose_omegaCoriolisHat.transpose()); | ||||
|         dfPdPi += 0.5 * temp * deltaTij()*deltaTij(); | ||||
|         dfVdPi += temp * deltaTij(); | ||||
|       } | ||||
|       (*H1) << | ||||
|           // dfP/dRi
 | ||||
|           skewSymmetric(fp + deltaPij_biascorrected), | ||||
|           // dfP/dPi
 | ||||
|           dfPdPi, | ||||
|           // dfV/dRi
 | ||||
|           skewSymmetric(fv + deltaVij_biascorrected), | ||||
|           // dfV/dPi
 | ||||
|           dfVdPi, | ||||
|           // dfR/dRi
 | ||||
|           D_fR_fRrot *  (- Rj.between(Ri).matrix() - fRrot.inverse().matrix() * D_coriolis), | ||||
|           // dfR/dPi
 | ||||
|           Z_3x3; | ||||
|     } | ||||
|     if(H2) { | ||||
|       H2->resize(9,3); | ||||
|       (*H2) << | ||||
|           // dfP/dVi
 | ||||
|           - Ri.transpose() * deltaTij() | ||||
|           + Ritranspose_omegaCoriolisHat * deltaTij() * deltaTij(),  // Coriolis term - we got rid of the 2 wrt ins paper
 | ||||
|           // dfV/dVi
 | ||||
|           - Ri.transpose() | ||||
|           + 2 * Ritranspose_omegaCoriolisHat * deltaTij(), // Coriolis term
 | ||||
|           // dfR/dVi
 | ||||
|           Z_3x3; | ||||
|     } | ||||
|     if(H3) { | ||||
|       H3->resize(9,6); | ||||
|       (*H3) << | ||||
|           // dfP/dPosej
 | ||||
|           Z_3x3, Ri.transpose() * Rj.matrix(), | ||||
|           // dfV/dPosej
 | ||||
|           Matrix::Zero(3,6), | ||||
|           // dfR/dPosej
 | ||||
|           D_fR_fRrot *  ( I_3x3 ), Z_3x3; | ||||
|     } | ||||
|     if(H4) { | ||||
|       H4->resize(9,3); | ||||
|       (*H4) << | ||||
|           // dfP/dVj
 | ||||
|           Z_3x3, | ||||
|           // dfV/dVj
 | ||||
|           Ri.transpose(), | ||||
|           // dfR/dVj
 | ||||
|           Z_3x3; | ||||
|     } | ||||
|     if(H5) { | ||||
|       // H5 by this point already contains 3*3 biascorrectedThetaRij derivative
 | ||||
|       const Matrix3 JbiasOmega = D_cDeltaRij_cOmega * D_cThetaRij_biasOmegaIncr; | ||||
|       H5->resize(9,6); | ||||
|       (*H5) << | ||||
|           // dfP/dBias
 | ||||
|           - delPdelBiasAcc(),   - delPdelBiasOmega(), | ||||
|           // dfV/dBias
 | ||||
|           - delVdelBiasAcc(),   - delVdelBiasOmega(), | ||||
|           // dfR/dBias
 | ||||
|           Z_3x3,               D_fR_fRrot * ( - fRrot.inverse().matrix() * JbiasOmega); | ||||
|     } | ||||
|     Vector9 r; r << fp, fv, fR; | ||||
|     return r; | ||||
|   } | ||||
| 
 | ||||
| private: | ||||
|   /** Serialization function */ | ||||
|   friend class boost::serialization::access; | ||||
|   template<class ARCHIVE> | ||||
|   void serialize(ARCHIVE & ar, const unsigned int version) { | ||||
|     ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegratedRotation); | ||||
|     ar & BOOST_SERIALIZATION_NVP(biasHat_); | ||||
|     ar & BOOST_SERIALIZATION_NVP(deltaPij_); | ||||
|     ar & BOOST_SERIALIZATION_NVP(deltaVij_); | ||||
|     ar & BOOST_SERIALIZATION_NVP(delPdelBiasAcc_); | ||||
|     ar & BOOST_SERIALIZATION_NVP(delPdelBiasOmega_); | ||||
|     ar & BOOST_SERIALIZATION_NVP(delVdelBiasAcc_); | ||||
|     ar & BOOST_SERIALIZATION_NVP(delVdelBiasOmega_); | ||||
|   } | ||||
| }; | ||||
| 
 | ||||
| class ImuBase { | ||||
| 
 | ||||
| protected: | ||||
| 
 | ||||
|   Vector3 gravity_; | ||||
|   Vector3 omegaCoriolis_; | ||||
|   boost::optional<Pose3> body_P_sensor_;        ///< The pose of the sensor in the body frame
 | ||||
|   bool use2ndOrderCoriolis_; ///< Controls whether higher order terms are included when calculating the Coriolis Effect
 | ||||
| 
 | ||||
| public: | ||||
| 
 | ||||
|   ImuBase() : | ||||
|     gravity_(Vector3(0.0,0.0,9.81)), omegaCoriolis_(Vector3(0.0,0.0,0.0)), | ||||
|     body_P_sensor_(boost::none), use2ndOrderCoriolis_(false) {} | ||||
| 
 | ||||
|   ImuBase(const Vector3& gravity, const Vector3& omegaCoriolis, | ||||
|       boost::optional<const Pose3&> body_P_sensor = boost::none, const bool use2ndOrderCoriolis = false) : | ||||
|         gravity_(gravity), omegaCoriolis_(omegaCoriolis), | ||||
|         body_P_sensor_(body_P_sensor), use2ndOrderCoriolis_(use2ndOrderCoriolis) {} | ||||
| 
 | ||||
|   const Vector3& gravity() const { return gravity_; } | ||||
|   const Vector3& omegaCoriolis() const { return omegaCoriolis_; } | ||||
| 
 | ||||
| }; | ||||
| 
 | ||||
| } /// namespace gtsam
 | ||||
|  | @ -116,7 +116,7 @@ TEST( AHRSFactor, PreintegratedMeasurements ) { | |||
| /* ************************************************************************* */ | ||||
| TEST(AHRSFactor, Error) { | ||||
|   // Linearization point
 | ||||
|   Vector3 bias; // Bias
 | ||||
|   Vector3 bias(0.,0.,0.); // Bias
 | ||||
|   Rot3 x1(Rot3::RzRyRx(M_PI / 12.0, M_PI / 6.0, M_PI / 4.0)); | ||||
|   Rot3 x2(Rot3::RzRyRx(M_PI / 12.0 + M_PI / 100.0, M_PI / 6.0, M_PI / 4.0)); | ||||
| 
 | ||||
|  |  | |||
|  | @ -39,19 +39,55 @@ using symbol_shorthand::X; | |||
| using symbol_shorthand::V; | ||||
| using symbol_shorthand::B; | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| namespace { | ||||
| /* ************************************************************************* */ | ||||
| // Auxiliary functions to test Jacobians F and G used for
 | ||||
| // covariance propagation during preintegration
 | ||||
| /* ************************************************************************* */ | ||||
| Vector updatePreintegratedMeasurementsTest( | ||||
|     const Vector3 deltaPij_old, const Vector3& deltaVij_old, const Rot3& deltaRij_old, | ||||
|     const imuBias::ConstantBias& bias_old, | ||||
|     const Vector3& correctedAcc, const Vector3& correctedOmega, const double deltaT, | ||||
|     const bool use2ndOrderIntegration) { | ||||
| 
 | ||||
| ImuFactor::PreintegratedMeasurements evaluatePreintegratedMeasurements( | ||||
|   Matrix3 dRij = deltaRij_old.matrix(); | ||||
|   Vector3 temp = dRij * (correctedAcc - bias_old.accelerometer()) * deltaT; | ||||
|   Vector3 deltaPij_new; | ||||
|   if(!use2ndOrderIntegration){ | ||||
|     deltaPij_new = deltaPij_old + deltaVij_old * deltaT; | ||||
|   }else{ | ||||
|     deltaPij_new += deltaPij_old + deltaVij_old * deltaT + 0.5 * temp * deltaT; | ||||
|   } | ||||
|   Vector3 deltaVij_new = deltaVij_old + temp; | ||||
|   Rot3 deltaRij_new = deltaRij_old * Rot3::Expmap((correctedOmega-bias_old.gyroscope()) * deltaT); | ||||
|   Vector3 logDeltaRij_new = Rot3::Logmap(deltaRij_new); // not important any more
 | ||||
|   imuBias::ConstantBias bias_new(bias_old); | ||||
|   Vector result(15);  result << deltaPij_new,  deltaVij_new, logDeltaRij_new, bias_new.vector(); | ||||
|   return result; | ||||
| } | ||||
| 
 | ||||
| Rot3 updatePreintegratedMeasurementsRot( | ||||
|     const Vector3 deltaPij_old, const Vector3& deltaVij_old, const Rot3& deltaRij_old, | ||||
|     const imuBias::ConstantBias& bias_old, | ||||
|     const Vector3& correctedAcc, const Vector3& correctedOmega, const double deltaT, | ||||
|     const bool use2ndOrderIntegration){ | ||||
| 
 | ||||
|   Vector result = updatePreintegratedMeasurementsTest(deltaPij_old, deltaVij_old, deltaRij_old, | ||||
|       bias_old, correctedAcc, correctedOmega, deltaT, use2ndOrderIntegration); | ||||
| 
 | ||||
|   return Rot3::Expmap(result.segment<3>(6)); | ||||
| } | ||||
| 
 | ||||
| // Auxiliary functions to test preintegrated Jacobians
 | ||||
| // delPdelBiasAcc_ delPdelBiasOmega_ delVdelBiasAcc_ delVdelBiasOmega_ delRdelBiasOmega_
 | ||||
| /* ************************************************************************* */ | ||||
| CombinedImuFactor::CombinedPreintegratedMeasurements evaluatePreintegratedMeasurements( | ||||
|     const imuBias::ConstantBias& bias, | ||||
|     const list<Vector3>& measuredAccs, | ||||
|     const list<Vector3>& measuredOmegas, | ||||
|     const list<double>& deltaTs, | ||||
|     const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) | ||||
|     ) | ||||
| { | ||||
|   ImuFactor::PreintegratedMeasurements result(bias, Matrix3::Identity(), | ||||
|       Matrix3::Identity(), Matrix3::Identity()); | ||||
|     const list<double>& deltaTs){ | ||||
|   CombinedImuFactor::CombinedPreintegratedMeasurements result(bias, Matrix3::Identity(), | ||||
|       Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), Matrix::Identity(6,6), false); | ||||
| 
 | ||||
|   list<Vector3>::const_iterator itAcc = measuredAccs.begin(); | ||||
|   list<Vector3>::const_iterator itOmega = measuredOmegas.begin(); | ||||
|  | @ -59,7 +95,6 @@ ImuFactor::PreintegratedMeasurements evaluatePreintegratedMeasurements( | |||
|   for( ; itAcc != measuredAccs.end(); ++itAcc, ++itOmega, ++itDeltaT) { | ||||
|     result.integrateMeasurement(*itAcc, *itOmega, *itDeltaT); | ||||
|   } | ||||
| 
 | ||||
|   return result; | ||||
| } | ||||
| 
 | ||||
|  | @ -67,20 +102,16 @@ Vector3 evaluatePreintegratedMeasurementsPosition( | |||
|     const imuBias::ConstantBias& bias, | ||||
|     const list<Vector3>& measuredAccs, | ||||
|     const list<Vector3>& measuredOmegas, | ||||
|     const list<double>& deltaTs, | ||||
|     const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) ) | ||||
| { | ||||
|     const list<double>& deltaTs){ | ||||
|   return evaluatePreintegratedMeasurements(bias, | ||||
|       measuredAccs, measuredOmegas, deltaTs, initialRotationRate).deltaPij(); | ||||
|       measuredAccs, measuredOmegas, deltaTs).deltaPij(); | ||||
| } | ||||
| 
 | ||||
| Vector3 evaluatePreintegratedMeasurementsVelocity( | ||||
|     const imuBias::ConstantBias& bias, | ||||
|     const list<Vector3>& measuredAccs, | ||||
|     const list<Vector3>& measuredOmegas, | ||||
|     const list<double>& deltaTs, | ||||
|     const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) ) | ||||
| { | ||||
|     const list<double>& deltaTs){ | ||||
|   return evaluatePreintegratedMeasurements(bias, | ||||
|       measuredAccs, measuredOmegas, deltaTs).deltaVij(); | ||||
| } | ||||
|  | @ -89,9 +120,7 @@ Rot3 evaluatePreintegratedMeasurementsRotation( | |||
|     const imuBias::ConstantBias& bias, | ||||
|     const list<Vector3>& measuredAccs, | ||||
|     const list<Vector3>& measuredOmegas, | ||||
|     const list<double>& deltaTs, | ||||
|     const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) ) | ||||
| { | ||||
|     const list<double>& deltaTs){ | ||||
|   return Rot3(evaluatePreintegratedMeasurements(bias, | ||||
|       measuredAccs, measuredOmegas, deltaTs).deltaRij()); | ||||
| } | ||||
|  | @ -101,7 +130,6 @@ Rot3 evaluatePreintegratedMeasurementsRotation( | |||
| /* ************************************************************************* */ | ||||
| TEST( CombinedImuFactor, PreintegratedMeasurements ) | ||||
| { | ||||
|   //cout << "++++++++++++++++++++++++++++++ PreintegratedMeasurements +++++++++++++++++++++++++++++++++++++++ " << endl;
 | ||||
|   // Linearization point
 | ||||
|   imuBias::ConstantBias bias(Vector3(0,0,0), Vector3(0,0,0)); ///< Current estimate of acceleration and angular rate biases
 | ||||
| 
 | ||||
|  | @ -120,28 +148,17 @@ TEST( CombinedImuFactor, PreintegratedMeasurements ) | |||
|       Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), | ||||
|       Matrix3::Zero(), Matrix3::Zero(), Matrix::Zero(6,6)); | ||||
| 
 | ||||
| //           const imuBias::ConstantBias& bias, ///< Current estimate of acceleration and rotation rate biases
 | ||||
| //           const Matrix3& measuredAccCovariance, ///< Covariance matrix of measuredAcc
 | ||||
| //           const Matrix3& measuredOmegaCovariance, ///< Covariance matrix of measuredAcc
 | ||||
| //           const Matrix3& integrationErrorCovariance, ///< Covariance matrix of measuredAcc
 | ||||
| //           const Matrix3& biasAccCovariance, ///< Covariance matrix of biasAcc (random walk describing BIAS evolution)
 | ||||
| //           const Matrix3& biasOmegaCovariance, ///< Covariance matrix of biasOmega (random walk describing BIAS evolution)
 | ||||
| //           const Matrix& biasAccOmegaInit ///< Covariance of biasAcc & biasOmega when preintegrating measurements
 | ||||
| 
 | ||||
|   actual1.integrateMeasurement(measuredAcc, measuredOmega, deltaT); | ||||
| 
 | ||||
|   EXPECT(assert_equal(Vector(expected1.deltaPij()), Vector(actual1.deltaPij()), tol)); | ||||
| //  EXPECT(assert_equal(Vector(expected1.deltaVij), Vector(actual1.deltaVij), tol));
 | ||||
| //  EXPECT(assert_equal(expected1.deltaRij, actual1.deltaRij, tol));
 | ||||
| //  DOUBLES_EQUAL(expected1.deltaTij, actual1.deltaTij, tol);
 | ||||
|   EXPECT(assert_equal(Vector(expected1.deltaVij()), Vector(actual1.deltaVij()), tol)); | ||||
|   EXPECT(assert_equal(Matrix(expected1.deltaRij()), Matrix(actual1.deltaRij()), tol)); | ||||
|   DOUBLES_EQUAL(expected1.deltaTij(), actual1.deltaTij(), tol); | ||||
| } | ||||
| 
 | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| TEST( CombinedImuFactor, ErrorWithBiases ) | ||||
| { | ||||
|   //cout << "++++++++++++++++++++++++++++++ ErrorWithBiases +++++++++++++++++++++++++++++++++++++++ " << endl;
 | ||||
| 
 | ||||
|   imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot)
 | ||||
|   imuBias::ConstantBias bias2(Vector3(0.2, 0.2, 0), Vector3(1, 0, 0.3)); // Biases (acc, rot)
 | ||||
|   Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI/4.0)), Point3(5.0, 1.0, -50.0)); | ||||
|  | @ -157,50 +174,37 @@ TEST( CombinedImuFactor, ErrorWithBiases ) | |||
|   double deltaT = 1.0; | ||||
|   double tol = 1e-6; | ||||
| 
 | ||||
|   //           const imuBias::ConstantBias& bias, ///< Current estimate of acceleration and rotation rate biases
 | ||||
|   //           const Matrix3& measuredAccCovariance, ///< Covariance matrix of measuredAcc
 | ||||
|   //           const Matrix3& measuredOmegaCovariance, ///< Covariance matrix of measuredAcc
 | ||||
|   //           const Matrix3& integrationErrorCovariance, ///< Covariance matrix of measuredAcc
 | ||||
|   //           const Matrix3& biasAccCovariance, ///< Covariance matrix of biasAcc (random walk describing BIAS evolution)
 | ||||
|   //           const Matrix3& biasOmegaCovariance, ///< Covariance matrix of biasOmega (random walk describing BIAS evolution)
 | ||||
|   //           const Matrix& biasAccOmegaInit ///< Covariance of biasAcc & biasOmega when preintegrating measurements
 | ||||
| 
 | ||||
|   Matrix I6x6(6,6); | ||||
|   I6x6 = Matrix::Identity(6,6); | ||||
| 
 | ||||
| 
 | ||||
|   ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), | ||||
|       Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity()); | ||||
| 
 | ||||
|     pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); | ||||
|   pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); | ||||
| 
 | ||||
|    CombinedImuFactor::CombinedPreintegratedMeasurements Combined_pre_int_data( | ||||
|        imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0),  Vector3(0.0, 0.0, 0.0)), | ||||
|         Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), 2 * Matrix3::Identity(),  I6x6 ); | ||||
|   CombinedImuFactor::CombinedPreintegratedMeasurements Combined_pre_int_data( | ||||
|       imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0),  Vector3(0.0, 0.0, 0.0)), | ||||
|       Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), 2 * Matrix3::Identity(),  I6x6 ); | ||||
| 
 | ||||
|    Combined_pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); | ||||
|   Combined_pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); | ||||
| 
 | ||||
|   // Create factor
 | ||||
|   ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis); | ||||
| 
 | ||||
|     // Create factor
 | ||||
|     ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis); | ||||
|   noiseModel::Gaussian::shared_ptr Combinedmodel = noiseModel::Gaussian::Covariance(Combined_pre_int_data.preintMeasCov()); | ||||
|   CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), Combined_pre_int_data, gravity, omegaCoriolis); | ||||
| 
 | ||||
|     noiseModel::Gaussian::shared_ptr Combinedmodel = noiseModel::Gaussian::Covariance(Combined_pre_int_data.PreintMeasCov()); | ||||
|     CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), Combined_pre_int_data, gravity, omegaCoriolis); | ||||
|   Vector errorExpected = factor.evaluateError(x1, v1, x2, v2, bias); | ||||
| 
 | ||||
|   Vector errorActual = Combinedfactor.evaluateError(x1, v1, x2, v2, bias, bias2); | ||||
| 
 | ||||
|     Vector errorExpected = factor.evaluateError(x1, v1, x2, v2, bias); | ||||
|   EXPECT(assert_equal(errorExpected, errorActual.head(9), tol)); | ||||
| 
 | ||||
|     Vector errorActual = Combinedfactor.evaluateError(x1, v1, x2, v2, bias, bias2); | ||||
|   // Expected Jacobians
 | ||||
|   Matrix H1e, H2e, H3e, H4e, H5e; | ||||
|   (void) factor.evaluateError(x1, v1, x2, v2, bias, H1e, H2e, H3e, H4e, H5e); | ||||
| 
 | ||||
| 
 | ||||
|     EXPECT(assert_equal(errorExpected, errorActual.head(9), tol)); | ||||
| 
 | ||||
|     // Expected Jacobians
 | ||||
|     Matrix H1e, H2e, H3e, H4e, H5e; | ||||
|     (void) factor.evaluateError(x1, v1, x2, v2, bias, H1e, H2e, H3e, H4e, H5e); | ||||
| 
 | ||||
| 
 | ||||
|     // Actual Jacobians
 | ||||
|   // Actual Jacobians
 | ||||
|   Matrix H1a, H2a, H3a, H4a, H5a, H6a; | ||||
|   (void) Combinedfactor.evaluateError(x1, v1, x2, v2, bias, bias2, H1a, H2a, H3a, H4a, H5a, H6a); | ||||
| 
 | ||||
|  | @ -214,7 +218,6 @@ TEST( CombinedImuFactor, ErrorWithBiases ) | |||
| /* ************************************************************************* */ | ||||
| TEST( CombinedImuFactor, FirstOrderPreIntegratedMeasurements ) | ||||
| { | ||||
|   //cout << "++++++++++++++++++++++++++++++ FirstOrderPreIntegratedMeasurements +++++++++++++++++++++++++++++++++++++++ " << endl;
 | ||||
|   // Linearization point
 | ||||
|   imuBias::ConstantBias bias; ///< Current estimate of acceleration and rotation rate biases
 | ||||
| 
 | ||||
|  | @ -237,22 +240,22 @@ TEST( CombinedImuFactor, FirstOrderPreIntegratedMeasurements ) | |||
|   } | ||||
| 
 | ||||
|   // Actual preintegrated values
 | ||||
|   ImuFactor::PreintegratedMeasurements preintegrated = | ||||
|       evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)); | ||||
|   CombinedImuFactor::CombinedPreintegratedMeasurements preintegrated = | ||||
|       evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs); | ||||
| 
 | ||||
|   // Compute numerical derivatives
 | ||||
|   Matrix expectedDelPdelBias = numericalDerivative11<Vector,imuBias::ConstantBias>( | ||||
|       boost::bind(&evaluatePreintegratedMeasurementsPosition, _1, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)), bias); | ||||
|       boost::bind(&evaluatePreintegratedMeasurementsPosition, _1, measuredAccs, measuredOmegas, deltaTs), bias); | ||||
|   Matrix expectedDelPdelBiasAcc   = expectedDelPdelBias.leftCols(3); | ||||
|   Matrix expectedDelPdelBiasOmega = expectedDelPdelBias.rightCols(3); | ||||
| 
 | ||||
|   Matrix expectedDelVdelBias = numericalDerivative11<Vector,imuBias::ConstantBias>( | ||||
|       boost::bind(&evaluatePreintegratedMeasurementsVelocity, _1, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)), bias); | ||||
|       boost::bind(&evaluatePreintegratedMeasurementsVelocity, _1, measuredAccs, measuredOmegas, deltaTs), bias); | ||||
|   Matrix expectedDelVdelBiasAcc   = expectedDelVdelBias.leftCols(3); | ||||
|   Matrix expectedDelVdelBiasOmega = expectedDelVdelBias.rightCols(3); | ||||
| 
 | ||||
|   Matrix expectedDelRdelBias = numericalDerivative11<Rot3,imuBias::ConstantBias>( | ||||
|       boost::bind(&evaluatePreintegratedMeasurementsRotation, _1, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)), bias); | ||||
|       boost::bind(&evaluatePreintegratedMeasurementsRotation, _1, measuredAccs, measuredOmegas, deltaTs), bias); | ||||
|   Matrix expectedDelRdelBiasAcc   = expectedDelRdelBias.leftCols(3); | ||||
|   Matrix expectedDelRdelBiasOmega = expectedDelRdelBias.rightCols(3); | ||||
| 
 | ||||
|  | @ -265,6 +268,7 @@ TEST( CombinedImuFactor, FirstOrderPreIntegratedMeasurements ) | |||
|   EXPECT(assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega(), 1e-3)); // 1e-3 needs to be added only when using quaternions for rotations
 | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| TEST(CombinedImuFactor, PredictPositionAndVelocity){ | ||||
|   imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot)
 | ||||
| 
 | ||||
|  | @ -283,22 +287,21 @@ TEST(CombinedImuFactor, PredictPositionAndVelocity){ | |||
| 
 | ||||
|   for (int i = 0; i<1000; ++i)   Combined_pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); | ||||
| 
 | ||||
|     // Create factor
 | ||||
|     noiseModel::Gaussian::shared_ptr Combinedmodel = noiseModel::Gaussian::Covariance(Combined_pre_int_data.PreintMeasCov()); | ||||
|     CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), Combined_pre_int_data, gravity, omegaCoriolis); | ||||
| 
 | ||||
|     // Predict
 | ||||
|     Pose3 x1; | ||||
|     Vector3 v1(0, 0.0, 0.0); | ||||
|     PoseVelocityBias poseVelocityBias = Combinedfactor.Predict(x1, v1, bias, Combined_pre_int_data, gravity, omegaCoriolis); | ||||
|     Pose3 expectedPose(Rot3(), Point3(0, 0.5, 0)); | ||||
|     Vector3 expectedVelocity; expectedVelocity<<0,1,0; | ||||
|     EXPECT(assert_equal(expectedPose, poseVelocityBias.pose)); | ||||
|     EXPECT(assert_equal(Vector(expectedVelocity), Vector(poseVelocityBias.velocity))); | ||||
| 
 | ||||
|   // Create factor
 | ||||
|   noiseModel::Gaussian::shared_ptr Combinedmodel = noiseModel::Gaussian::Covariance(Combined_pre_int_data.preintMeasCov()); | ||||
|   CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), Combined_pre_int_data, gravity, omegaCoriolis); | ||||
| 
 | ||||
|   // Predict
 | ||||
|   Pose3 x1; | ||||
|   Vector3 v1(0, 0.0, 0.0); | ||||
|   PoseVelocityBias poseVelocityBias = Combined_pre_int_data.predict(x1, v1, bias, gravity, omegaCoriolis); | ||||
|   Pose3 expectedPose(Rot3(), Point3(0, 0.5, 0)); | ||||
|   Vector3 expectedVelocity; expectedVelocity<<0,1,0; | ||||
|   EXPECT(assert_equal(expectedPose, poseVelocityBias.pose)); | ||||
|   EXPECT(assert_equal(Vector(expectedVelocity), Vector(poseVelocityBias.velocity))); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| TEST(CombinedImuFactor, PredictRotation) { | ||||
|   imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot)
 | ||||
|   Matrix I6x6(6,6); | ||||
|  | @ -319,14 +322,152 @@ TEST(CombinedImuFactor, PredictRotation) { | |||
|   // Predict
 | ||||
|   Pose3 x(Rot3().ypr(0,0, 0), Point3(0,0,0)); | ||||
|   Vector3 v(0,0,0); | ||||
|   PoseVelocityBias poseVelocityBias = Combinedfactor.Predict(x,v,bias, Combined_pre_int_data, gravity, omegaCoriolis); | ||||
|   PoseVelocityBias poseVelocityBias = Combined_pre_int_data.predict(x,v,bias, gravity, omegaCoriolis); | ||||
|   Pose3 expectedPose(Rot3().ypr(M_PI/10, 0,0), Point3(0,0,0)); | ||||
|   EXPECT(assert_equal(expectedPose, poseVelocityBias.pose, tol)); | ||||
| } | ||||
| 
 | ||||
| #include <gtsam/linear/GaussianFactorGraph.h> | ||||
| /* ************************************************************************* */ | ||||
| TEST( CombinedImuFactor, JacobianPreintegratedCovariancePropagation ) | ||||
| { | ||||
|   // Linearization point
 | ||||
|   imuBias::ConstantBias bias_old = imuBias::ConstantBias(); ///< Current estimate of acceleration and rotation rate biases
 | ||||
|   Pose3 body_P_sensor = Pose3(); | ||||
| 
 | ||||
|   // Measurements
 | ||||
|   list<Vector3> measuredAccs, measuredOmegas; | ||||
|   list<double> deltaTs; | ||||
|   measuredAccs.push_back(Vector3(0.1, 0.0, 0.0)); | ||||
|   measuredOmegas.push_back(Vector3(M_PI/100.0, 0.0, 0.0)); | ||||
|   deltaTs.push_back(0.01); | ||||
|   measuredAccs.push_back(Vector3(0.1, 0.0, 0.0)); | ||||
|   measuredOmegas.push_back(Vector3(M_PI/100.0, 0.0, 0.0)); | ||||
|   deltaTs.push_back(0.01); | ||||
|   for(int i=1;i<100;i++) | ||||
|   { | ||||
|     measuredAccs.push_back(Vector3(0.05, 0.09, 0.01)); | ||||
|     measuredOmegas.push_back(Vector3(M_PI/100.0, M_PI/300.0, 2*M_PI/100.0)); | ||||
|     deltaTs.push_back(0.01); | ||||
|   } | ||||
|   // Actual preintegrated values
 | ||||
|   CombinedImuFactor::CombinedPreintegratedMeasurements preintegrated = | ||||
|       evaluatePreintegratedMeasurements(bias_old, measuredAccs, measuredOmegas, deltaTs); | ||||
| 
 | ||||
|   // so far we only created a nontrivial linearization point for the preintegrated measurements
 | ||||
|   // Now we add a new measurement and ask for Jacobians
 | ||||
|   const Vector3 newMeasuredAcc = Vector3(0.1, 0.0, 0.0); | ||||
|   const Vector3 newMeasuredOmega = Vector3(M_PI/100.0, 0.0, 0.0); | ||||
|   const double newDeltaT = 0.01; | ||||
|   const Rot3    deltaRij_old = preintegrated.deltaRij();    // before adding new measurement
 | ||||
|   const Vector3 deltaVij_old = preintegrated.deltaVij();    // before adding new measurement
 | ||||
|   const Vector3 deltaPij_old = preintegrated.deltaPij();    // before adding new measurement
 | ||||
| 
 | ||||
|   Matrix oldPreintCovariance = preintegrated.preintMeasCov(); | ||||
| 
 | ||||
|   Matrix Factual, Gactual; | ||||
|   preintegrated.integrateMeasurement(newMeasuredAcc, newMeasuredOmega, newDeltaT, | ||||
|       body_P_sensor, Factual, Gactual); | ||||
| 
 | ||||
|   bool use2ndOrderIntegration = false; | ||||
| 
 | ||||
|   //////////////////////////////////////////////////////////////////////////////////////////////
 | ||||
|   // COMPUTE NUMERICAL DERIVATIVES FOR F
 | ||||
|   //////////////////////////////////////////////////////////////////////////////////////////////
 | ||||
|   // Compute expected F wrt positions (15,3)
 | ||||
|   Matrix df_dpos = | ||||
|       numericalDerivative11<Vector, Vector3>(boost::bind(&updatePreintegratedMeasurementsTest, | ||||
|           _1, deltaVij_old, deltaRij_old, bias_old, | ||||
|           newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaPij_old); | ||||
|   // rotation part has to be done properly, on manifold
 | ||||
|   df_dpos.block<3,3>(6,0) = numericalDerivative11<Rot3, Vector3>(boost::bind(&updatePreintegratedMeasurementsRot, | ||||
|           _1, deltaVij_old, deltaRij_old, bias_old, | ||||
|            newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaPij_old); | ||||
| 
 | ||||
|   // Compute expected F wrt velocities (15,3)
 | ||||
|   Matrix df_dvel = | ||||
|       numericalDerivative11<Vector, Vector3>(boost::bind(&updatePreintegratedMeasurementsTest, | ||||
|           deltaPij_old, _1, deltaRij_old, bias_old, | ||||
|           newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaVij_old); | ||||
|   // rotation part has to be done properly, on manifold
 | ||||
|   df_dvel.block<3,3>(6,0) = numericalDerivative11<Rot3, Vector3>(boost::bind(&updatePreintegratedMeasurementsRot, | ||||
|             deltaPij_old, _1, deltaRij_old, bias_old, | ||||
|             newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaVij_old); | ||||
| 
 | ||||
|   // Compute expected F wrt angles (15,3)
 | ||||
|   Matrix df_dangle = numericalDerivative11<Vector, Rot3>(boost::bind(&updatePreintegratedMeasurementsTest, | ||||
|           deltaPij_old, deltaVij_old, _1, bias_old, | ||||
|           newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaRij_old); | ||||
|   // rotation part has to be done properly, on manifold
 | ||||
|   df_dangle.block<3,3>(6,0) = numericalDerivative11<Rot3, Rot3>(boost::bind(&updatePreintegratedMeasurementsRot, | ||||
|             deltaPij_old, deltaVij_old, _1, bias_old, | ||||
|             newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaRij_old); | ||||
| 
 | ||||
|   // Compute expected F wrt biases (15,6)
 | ||||
|   Matrix df_dbias = numericalDerivative11<Vector, imuBias::ConstantBias>(boost::bind(&updatePreintegratedMeasurementsTest, | ||||
|           deltaPij_old, deltaVij_old, deltaRij_old, _1, | ||||
|           newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), bias_old); | ||||
|   // rotation part has to be done properly, on manifold
 | ||||
|   df_dbias.block<3,6>(6,0) = numericalDerivative11<Rot3, imuBias::ConstantBias>(boost::bind(&updatePreintegratedMeasurementsRot, | ||||
|             deltaPij_old, deltaVij_old, deltaRij_old, _1, | ||||
|             newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), bias_old); | ||||
| 
 | ||||
|   Matrix Fexpected(15,15); | ||||
|   Fexpected << df_dpos, df_dvel, df_dangle, df_dbias; | ||||
|   EXPECT(assert_equal(Fexpected, Factual)); | ||||
| 
 | ||||
|   //////////////////////////////////////////////////////////////////////////////////////////////
 | ||||
|   // COMPUTE NUMERICAL DERIVATIVES FOR G
 | ||||
|   //////////////////////////////////////////////////////////////////////////////////////////////
 | ||||
|   // Compute expected G wrt integration noise
 | ||||
|   Matrix df_dintNoise(15,3); | ||||
|   df_dintNoise << I_3x3 * newDeltaT, Z_3x3, Z_3x3, Z_3x3, Z_3x3; | ||||
| 
 | ||||
|   // Compute expected G wrt acc noise (15,3)
 | ||||
|   Matrix df_daccNoise = numericalDerivative11<Vector, Vector3>(boost::bind(&updatePreintegratedMeasurementsTest, | ||||
|           deltaPij_old, deltaVij_old, deltaRij_old, bias_old, | ||||
|           _1, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), newMeasuredAcc); | ||||
|   // rotation part has to be done properly, on manifold
 | ||||
|   df_daccNoise.block<3,3>(6,0) =  numericalDerivative11<Rot3, Vector3>(boost::bind(&updatePreintegratedMeasurementsRot, | ||||
|             deltaPij_old, deltaVij_old, deltaRij_old, bias_old, | ||||
|             _1, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), newMeasuredAcc); | ||||
| 
 | ||||
|   // Compute expected G wrt gyro noise (15,3)
 | ||||
|   Matrix df_domegaNoise = numericalDerivative11<Vector, Vector3>(boost::bind(&updatePreintegratedMeasurementsTest, | ||||
|           deltaPij_old, deltaVij_old, deltaRij_old, bias_old, | ||||
|           newMeasuredAcc, _1, newDeltaT, use2ndOrderIntegration), newMeasuredOmega); | ||||
|   // rotation part has to be done properly, on manifold
 | ||||
|   df_domegaNoise.block<3,3>(6,0) = numericalDerivative11< Rot3, Vector3>(boost::bind(&updatePreintegratedMeasurementsRot, | ||||
|             deltaPij_old, deltaVij_old, deltaRij_old, bias_old, | ||||
|             newMeasuredAcc, _1, newDeltaT, use2ndOrderIntegration), newMeasuredOmega); | ||||
| 
 | ||||
|   // Compute expected G wrt bias random walk noise (15,6)
 | ||||
|   Matrix df_rwBias(15,6); // random walk on the bias does not appear in the first 9 entries
 | ||||
|   df_rwBias.setZero(); | ||||
|   df_rwBias.block<6,6>(9,0) = eye(6); | ||||
| 
 | ||||
|   // Compute expected G wrt gyro noise (15,6)
 | ||||
|   Matrix df_dinitBias = numericalDerivative11<Vector, imuBias::ConstantBias>(boost::bind(&updatePreintegratedMeasurementsTest, | ||||
|           deltaPij_old, deltaVij_old, deltaRij_old, _1, | ||||
|           newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), bias_old); | ||||
|   // rotation part has to be done properly, on manifold
 | ||||
|   df_dinitBias.block<3,6>(6,0) = numericalDerivative11<Rot3, imuBias::ConstantBias>(boost::bind(&updatePreintegratedMeasurementsRot, | ||||
|             deltaPij_old, deltaVij_old, deltaRij_old, _1, | ||||
|             newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), bias_old); | ||||
|   df_dinitBias.block<6,6>(9,0) = Matrix::Zero(6,6); // only has to influence first 9 rows
 | ||||
| 
 | ||||
|   Matrix Gexpected(15,21); | ||||
|   Gexpected << df_dintNoise, df_daccNoise, df_domegaNoise, df_rwBias, df_dinitBias; | ||||
| 
 | ||||
|   EXPECT(assert_equal(Gexpected, Gactual)); | ||||
| 
 | ||||
|   // Check covariance propagation
 | ||||
|   Matrix newPreintCovarianceExpected = Factual * oldPreintCovariance * Factual.transpose() + | ||||
|       (1/newDeltaT) * Gactual * Gactual.transpose(); | ||||
| 
 | ||||
|   Matrix newPreintCovarianceActual = preintegrated.preintMeasCov(); | ||||
|   EXPECT(assert_equal(newPreintCovarianceExpected, newPreintCovarianceActual)); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
|   int main() { TestResult tr; return TestRegistry::runAllTests(tr);} | ||||
| int main() { TestResult tr; return TestRegistry::runAllTests(tr);} | ||||
| /* ************************************************************************* */ | ||||
|  |  | |||
|  | @ -37,6 +37,8 @@ using symbol_shorthand::B; | |||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| namespace { | ||||
| // Auxiliary functions to test evaluate error in ImuFactor
 | ||||
| /* ************************************************************************* */ | ||||
| Vector callEvaluateError(const ImuFactor& factor, | ||||
|     const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j, | ||||
|     const imuBias::ConstantBias& bias){ | ||||
|  | @ -49,14 +51,48 @@ Rot3 evaluateRotationError(const ImuFactor& factor, | |||
|   return Rot3::Expmap(factor.evaluateError(pose_i, vel_i, pose_j, vel_j, bias).tail(3) ) ; | ||||
| } | ||||
| 
 | ||||
| // Auxiliary functions to test Jacobians F and G used for
 | ||||
| // covariance propagation during preintegration
 | ||||
| /* ************************************************************************* */ | ||||
| Vector updatePreintegratedPosVel( | ||||
|     const Vector3 deltaPij_old, const Vector3& deltaVij_old, const Rot3& deltaRij_old, | ||||
|     const Vector3& correctedAcc, const Vector3& correctedOmega, const double deltaT, | ||||
|     const bool use2ndOrderIntegration_) { | ||||
| 
 | ||||
|   Matrix3 dRij = deltaRij_old.matrix(); | ||||
|   Vector3 temp = dRij * correctedAcc * deltaT; | ||||
|   Vector3 deltaPij_new; | ||||
|   if(!use2ndOrderIntegration_){ | ||||
|     deltaPij_new = deltaPij_old + deltaVij_old * deltaT; | ||||
|   }else{ | ||||
|     deltaPij_new += deltaPij_old + deltaVij_old * deltaT + 0.5 * temp * deltaT; | ||||
|   } | ||||
|   Vector3 deltaVij_new = deltaVij_old + temp; | ||||
| 
 | ||||
|   Vector result(6);  result << deltaPij_new,  deltaVij_new; | ||||
|   return result; | ||||
| } | ||||
| 
 | ||||
| Rot3 updatePreintegratedRot(const Rot3& deltaRij_old, | ||||
|     const Vector3& correctedOmega, const double deltaT) { | ||||
|   Rot3 deltaRij_new = deltaRij_old * Rot3::Expmap(correctedOmega * deltaT); | ||||
|   return deltaRij_new; | ||||
| } | ||||
| 
 | ||||
| // Auxiliary functions to test preintegrated Jacobians
 | ||||
| // delPdelBiasAcc_ delPdelBiasOmega_ delVdelBiasAcc_ delVdelBiasOmega_ delRdelBiasOmega_
 | ||||
| /* ************************************************************************* */ | ||||
| double accNoiseVar = 0.01; | ||||
| double omegaNoiseVar = 0.03; | ||||
| double intNoiseVar = 0.0001; | ||||
| ImuFactor::PreintegratedMeasurements evaluatePreintegratedMeasurements( | ||||
|     const imuBias::ConstantBias& bias, | ||||
|     const list<Vector3>& measuredAccs, | ||||
|     const list<Vector3>& measuredOmegas, | ||||
|     const list<double>& deltaTs, | ||||
|     const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0)  ){ | ||||
|   ImuFactor::PreintegratedMeasurements result(bias, Matrix3::Identity(), | ||||
|       Matrix3::Identity(), Matrix3::Identity()); | ||||
|   ImuFactor::PreintegratedMeasurements result(bias, accNoiseVar * Matrix3::Identity(), | ||||
|       omegaNoiseVar *Matrix3::Identity(), intNoiseVar * Matrix3::Identity()); | ||||
| 
 | ||||
|   list<Vector3>::const_iterator itAcc = measuredAccs.begin(); | ||||
|   list<Vector3>::const_iterator itOmega = measuredOmegas.begin(); | ||||
|  | @ -152,7 +188,7 @@ TEST( ImuFactor, PreintegratedMeasurements ) | |||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| TEST( ImuFactor, Error ) | ||||
| TEST( ImuFactor, ErrorAndJacobians ) | ||||
| { | ||||
|   // Linearization point
 | ||||
|   imuBias::ConstantBias bias; // Bias
 | ||||
|  | @ -180,6 +216,77 @@ TEST( ImuFactor, Error ) | |||
|   Vector errorExpected(9); errorExpected << 0, 0, 0, 0, 0, 0, 0, 0, 0; | ||||
|   EXPECT(assert_equal(errorExpected, errorActual, 1e-6)); | ||||
| 
 | ||||
|   // Actual Jacobians
 | ||||
|   Matrix H1a, H2a, H3a, H4a, H5a; | ||||
|   (void) factor.evaluateError(x1, v1, x2, v2, bias, H1a, H2a, H3a, H4a, H5a); | ||||
| 
 | ||||
|   // Expected Jacobians
 | ||||
|   /////////////////// H1 ///////////////////////////
 | ||||
|   Matrix H1e = numericalDerivative11<Vector,Pose3>( | ||||
|       boost::bind(&callEvaluateError, factor, _1, v1, x2, v2, bias), x1); | ||||
|   // Jacobians are around zero, so the rotation part is the same as:
 | ||||
|   Matrix H1Rot3 = numericalDerivative11<Rot3,Pose3>( | ||||
|       boost::bind(&evaluateRotationError, factor, _1, v1, x2, v2, bias), x1); | ||||
|   EXPECT(assert_equal(H1Rot3, H1e.bottomRows(3))); | ||||
|   EXPECT(assert_equal(H1e, H1a)); | ||||
| 
 | ||||
|   /////////////////// H2 ///////////////////////////
 | ||||
|   Matrix H2e = numericalDerivative11<Vector,Vector3>( | ||||
|       boost::bind(&callEvaluateError, factor, x1, _1, x2, v2, bias), v1); | ||||
|   EXPECT(assert_equal(H2e, H2a)); | ||||
| 
 | ||||
|   /////////////////// H3 ///////////////////////////
 | ||||
|   Matrix H3e = numericalDerivative11<Vector,Pose3>( | ||||
|       boost::bind(&callEvaluateError, factor, x1, v1, _1, v2, bias), x2); | ||||
|   // Jacobians are around zero, so the rotation part is the same as:
 | ||||
|   Matrix H3Rot3 = numericalDerivative11<Rot3,Pose3>( | ||||
|       boost::bind(&evaluateRotationError, factor, x1, v1, _1, v2, bias), x2); | ||||
|   EXPECT(assert_equal(H3Rot3, H3e.bottomRows(3))); | ||||
|   EXPECT(assert_equal(H3e, H3a)); | ||||
| 
 | ||||
|   /////////////////// H4 ///////////////////////////
 | ||||
|   Matrix H4e = numericalDerivative11<Vector,Vector3>( | ||||
|       boost::bind(&callEvaluateError, factor, x1, v1, x2, _1, bias), v2); | ||||
|   EXPECT(assert_equal(H4e, H4a)); | ||||
| 
 | ||||
|   /////////////////// H5 ///////////////////////////
 | ||||
|   Matrix H5e = numericalDerivative11<Vector,imuBias::ConstantBias>( | ||||
|       boost::bind(&callEvaluateError, factor, x1, v1, x2, v2, _1), bias); | ||||
|   EXPECT(assert_equal(H5e, H5a)); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| TEST( ImuFactor, ErrorAndJacobianWithBiases ) | ||||
| { | ||||
|   imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0.1, 0, 0.3)); // Biases (acc, rot)
 | ||||
|   Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/10.0), Point3(5.0, 1.0, -50.0)); | ||||
|   Vector3 v1(Vector3(0.5, 0.0, 0.0)); | ||||
|   Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI/10.0 + M_PI/10.0)), Point3(5.5, 1.0, -50.0)); | ||||
|   Vector3 v2(Vector3(0.5, 0.0, 0.0)); | ||||
| 
 | ||||
|   // Measurements
 | ||||
|   Vector3 gravity; gravity << 0, 0, 9.81; | ||||
|   Vector3 omegaCoriolis; omegaCoriolis << 0, 0.1, 0.1; | ||||
|   Vector3 measuredOmega; measuredOmega << 0, 0, M_PI/10.0+0.3; | ||||
|   Vector3 measuredAcc = x1.rotation().unrotate(-Point3(gravity)).vector() + Vector3(0.2,0.0,0.0); | ||||
|   double deltaT = 1.0; | ||||
| 
 | ||||
|   ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), | ||||
|       Vector3(0.0, 0.0, 0.1)), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero()); | ||||
|   pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); | ||||
| 
 | ||||
|   // Create factor
 | ||||
|   ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis); | ||||
| 
 | ||||
|   SETDEBUG("ImuFactor evaluateError", false); | ||||
|   Vector errorActual = factor.evaluateError(x1, v1, x2, v2, bias); | ||||
|   SETDEBUG("ImuFactor evaluateError", false); | ||||
| 
 | ||||
|   // Expected error (should not be zero in this test, as we want to evaluate Jacobians
 | ||||
|   // at a nontrivial linearization point)
 | ||||
|   // Vector errorExpected(9); errorExpected << 0, 0, 0, 0, 0, 0, 0, 0, 0;
 | ||||
|   // EXPECT(assert_equal(errorExpected, errorActual, 1e-6));
 | ||||
| 
 | ||||
|   // Expected Jacobians
 | ||||
|   Matrix H1e = numericalDerivative11<Vector,Pose3>( | ||||
|       boost::bind(&callEvaluateError, factor, _1, v1, x2, v2, bias), x1); | ||||
|  | @ -197,45 +304,27 @@ TEST( ImuFactor, Error ) | |||
|       boost::bind(&evaluateRotationError, factor, _1, v1, x2, v2, bias), x1); | ||||
|   Matrix RH3e = numericalDerivative11<Rot3,Pose3>( | ||||
|       boost::bind(&evaluateRotationError, factor, x1, v1, _1, v2, bias), x2); | ||||
|   Matrix RH5e = numericalDerivative11<Rot3,imuBias::ConstantBias>( | ||||
|       boost::bind(&evaluateRotationError, factor, x1, v1, x2, v2, _1), bias); | ||||
| 
 | ||||
|   // Actual Jacobians
 | ||||
|   Matrix H1a, H2a, H3a, H4a, H5a; | ||||
|   (void) factor.evaluateError(x1, v1, x2, v2, bias, H1a, H2a, H3a, H4a, H5a); | ||||
| 
 | ||||
|   // positions and velocities
 | ||||
|   Matrix H1etop6 =  H1e.topRows(6); | ||||
|   Matrix H1atop6 =  H1a.topRows(6); | ||||
|   EXPECT(assert_equal(H1etop6, H1atop6)); | ||||
|   // rotations
 | ||||
|   EXPECT(assert_equal(RH1e, H1a.bottomRows(3), 1e-5));  // 1e-5 needs to be added only when using quaternions for rotations
 | ||||
| 
 | ||||
|   EXPECT(assert_equal(H1e, H1a)); | ||||
|   EXPECT(assert_equal(H2e, H2a)); | ||||
| 
 | ||||
|   // positions and velocities
 | ||||
|   Matrix H3etop6 =  H3e.topRows(6); | ||||
|   Matrix H3atop6 =  H3a.topRows(6); | ||||
|   EXPECT(assert_equal(H3etop6, H3atop6)); | ||||
|   // rotations
 | ||||
|   EXPECT(assert_equal(RH3e, H3a.bottomRows(3), 1e-5));  // 1e-5 needs to be added only when using quaternions for rotations
 | ||||
| 
 | ||||
|   EXPECT(assert_equal(H3e, H3a)); | ||||
|   EXPECT(assert_equal(H4e, H4a)); | ||||
|   // EXPECT(assert_equal(H5e, H5a));
 | ||||
|   EXPECT(assert_equal(H5e, H5a)); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| TEST( ImuFactor, ErrorWithBiases ) | ||||
| TEST( ImuFactor, ErrorAndJacobianWith2ndOrderCoriolis ) | ||||
| { | ||||
|   // Linearization point
 | ||||
| //  Vector bias(6); bias << 0.2, 0, 0, 0.1, 0, 0; // Biases (acc, rot)
 | ||||
| //  Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/4.0), Point3(5.0, 1.0, -50.0));
 | ||||
| //  Vector3 v1(Vector3(0.5, 0.0, 0.0));
 | ||||
| //  Pose3 x2(Rot3::RzRyRx(M_PI/12.0 + M_PI/10.0, M_PI/6.0, M_PI/4.0), Point3(5.5, 1.0, -50.0));
 | ||||
| //  Vector3 v2(Vector3(0.5, 0.0, 0.0));
 | ||||
| 
 | ||||
|   imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot)
 | ||||
|   Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI/4.0)), Point3(5.0, 1.0, -50.0)); | ||||
|   imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0.1, 0, 0.3)); // Biases (acc, rot)
 | ||||
|   Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/10.0), Point3(5.0, 1.0, -50.0)); | ||||
|   Vector3 v1(Vector3(0.5, 0.0, 0.0)); | ||||
|   Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI/4.0 + M_PI/10.0)), Point3(5.5, 1.0, -50.0)); | ||||
|   Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI/10.0 + M_PI/10.0)), Point3(5.5, 1.0, -50.0)); | ||||
|   Vector3 v2(Vector3(0.5, 0.0, 0.0)); | ||||
| 
 | ||||
|   // Measurements
 | ||||
|  | @ -245,56 +334,57 @@ TEST( ImuFactor, ErrorWithBiases ) | |||
|   Vector3 measuredAcc = x1.rotation().unrotate(-Point3(gravity)).vector() + Vector3(0.2,0.0,0.0); | ||||
|   double deltaT = 1.0; | ||||
| 
 | ||||
|   ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero()); | ||||
|     pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); | ||||
|   ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), | ||||
|       Vector3(0.0, 0.0, 0.1)), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero()); | ||||
|   pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); | ||||
| 
 | ||||
|   //  ImuFactor::PreintegratedMeasurements pre_int_data(bias.head(3), bias.tail(3));
 | ||||
|   //    pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
 | ||||
|   // Create factor
 | ||||
|   Pose3 bodyPsensor = Pose3(); | ||||
|   bool use2ndOrderCoriolis = true; | ||||
|   ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis, bodyPsensor, use2ndOrderCoriolis); | ||||
| 
 | ||||
|     // Create factor
 | ||||
|     ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis); | ||||
|   SETDEBUG("ImuFactor evaluateError", false); | ||||
|   Vector errorActual = factor.evaluateError(x1, v1, x2, v2, bias); | ||||
|   SETDEBUG("ImuFactor evaluateError", false); | ||||
| 
 | ||||
|     SETDEBUG("ImuFactor evaluateError", false); | ||||
|     Vector errorActual = factor.evaluateError(x1, v1, x2, v2, bias); | ||||
|     SETDEBUG("ImuFactor evaluateError", false); | ||||
|   // Expected error (should not be zero in this test, as we want to evaluate Jacobians
 | ||||
|   // at a nontrivial linearization point)
 | ||||
|   // Vector errorExpected(9); errorExpected << 0, 0, 0, 0, 0, 0, 0, 0, 0;
 | ||||
|   // EXPECT(assert_equal(errorExpected, errorActual, 1e-6));
 | ||||
| 
 | ||||
|     // Expected error
 | ||||
|     Vector errorExpected(9); errorExpected << 0, 0, 0, 0, 0, 0, 0, 0, 0; | ||||
|     // EXPECT(assert_equal(errorExpected, errorActual, 1e-6));
 | ||||
|   // Expected Jacobians
 | ||||
|   Matrix H1e = numericalDerivative11<Vector,Pose3>( | ||||
|       boost::bind(&callEvaluateError, factor, _1, v1, x2, v2, bias), x1); | ||||
|   Matrix H2e = numericalDerivative11<Vector,Vector3>( | ||||
|       boost::bind(&callEvaluateError, factor, x1, _1, x2, v2, bias), v1); | ||||
|   Matrix H3e = numericalDerivative11<Vector,Pose3>( | ||||
|       boost::bind(&callEvaluateError, factor, x1, v1, _1, v2, bias), x2); | ||||
|   Matrix H4e = numericalDerivative11<Vector,Vector3>( | ||||
|       boost::bind(&callEvaluateError, factor, x1, v1, x2, _1, bias), v2); | ||||
|   Matrix H5e = numericalDerivative11<Vector,imuBias::ConstantBias>( | ||||
|       boost::bind(&callEvaluateError, factor, x1, v1, x2, v2, _1), bias); | ||||
| 
 | ||||
|     // Expected Jacobians
 | ||||
|     Matrix H1e = numericalDerivative11<Vector,Pose3>( | ||||
|         boost::bind(&callEvaluateError, factor, _1, v1, x2, v2, bias), x1); | ||||
|     Matrix H2e = numericalDerivative11<Vector,Vector3>( | ||||
|         boost::bind(&callEvaluateError, factor, x1, _1, x2, v2, bias), v1); | ||||
|     Matrix H3e = numericalDerivative11<Vector,Pose3>( | ||||
|         boost::bind(&callEvaluateError, factor, x1, v1, _1, v2, bias), x2); | ||||
|     Matrix H4e = numericalDerivative11<Vector,Vector3>( | ||||
|         boost::bind(&callEvaluateError, factor, x1, v1, x2, _1, bias), v2); | ||||
|     Matrix H5e = numericalDerivative11<Vector,imuBias::ConstantBias>( | ||||
|         boost::bind(&callEvaluateError, factor, x1, v1, x2, v2, _1), bias); | ||||
|   // Check rotation Jacobians
 | ||||
|   Matrix RH1e = numericalDerivative11<Rot3,Pose3>( | ||||
|       boost::bind(&evaluateRotationError, factor, _1, v1, x2, v2, bias), x1); | ||||
|   Matrix RH3e = numericalDerivative11<Rot3,Pose3>( | ||||
|       boost::bind(&evaluateRotationError, factor, x1, v1, _1, v2, bias), x2); | ||||
|   Matrix RH5e = numericalDerivative11<Rot3,imuBias::ConstantBias>( | ||||
|       boost::bind(&evaluateRotationError, factor, x1, v1, x2, v2, _1), bias); | ||||
| 
 | ||||
|     // Check rotation Jacobians
 | ||||
|     Matrix RH1e = numericalDerivative11<Rot3,Pose3>( | ||||
|         boost::bind(&evaluateRotationError, factor, _1, v1, x2, v2, bias), x1); | ||||
|     Matrix RH3e = numericalDerivative11<Rot3,Pose3>( | ||||
|         boost::bind(&evaluateRotationError, factor, x1, v1, _1, v2, bias), x2); | ||||
|     Matrix RH5e = numericalDerivative11<Rot3,imuBias::ConstantBias>( | ||||
|         boost::bind(&evaluateRotationError, factor, x1, v1, x2, v2, _1), bias); | ||||
|   // Actual Jacobians
 | ||||
|   Matrix H1a, H2a, H3a, H4a, H5a; | ||||
|   (void) factor.evaluateError(x1, v1, x2, v2, bias, H1a, H2a, H3a, H4a, H5a); | ||||
| 
 | ||||
|     // Actual Jacobians
 | ||||
|     Matrix H1a, H2a, H3a, H4a, H5a; | ||||
|     (void) factor.evaluateError(x1, v1, x2, v2, bias, H1a, H2a, H3a, H4a, H5a); | ||||
| 
 | ||||
|     EXPECT(assert_equal(H1e, H1a)); | ||||
|     EXPECT(assert_equal(H2e, H2a)); | ||||
|     EXPECT(assert_equal(H3e, H3a)); | ||||
|     EXPECT(assert_equal(H4e, H4a)); | ||||
|     EXPECT(assert_equal(H5e, H5a)); | ||||
|   EXPECT(assert_equal(H1e, H1a)); | ||||
|   EXPECT(assert_equal(H2e, H2a)); | ||||
|   EXPECT(assert_equal(H3e, H3a)); | ||||
|   EXPECT(assert_equal(H4e, H4a)); | ||||
|   EXPECT(assert_equal(H5e, H5a)); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| TEST( ImuFactor, PartialDerivativeExpmap ) | ||||
| TEST( ImuFactor, PartialDerivative_wrt_Bias ) | ||||
| { | ||||
|   // Linearization point
 | ||||
|   Vector3 biasOmega; biasOmega << 0,0,0; ///< Current estimate of rotation rate bias
 | ||||
|  | @ -324,20 +414,14 @@ TEST( ImuFactor, PartialDerivativeLogmap ) | |||
|   // Measurements
 | ||||
|   Vector3 deltatheta; deltatheta << 0, 0, 0; | ||||
| 
 | ||||
| 
 | ||||
|   // Compute numerical derivatives
 | ||||
|   Matrix expectedDelFdeltheta = numericalDerivative11<Vector,Vector3>(boost::bind( | ||||
|       &evaluateLogRotation, thetahat, _1), Vector3(deltatheta)); | ||||
| 
 | ||||
|   const Vector3 x = thetahat; // parametrization of so(3)
 | ||||
|   const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^
 | ||||
|   double normx = norm_2(x); | ||||
|   const Matrix3  actualDelFdeltheta = Matrix3::Identity() + | ||||
|        0.5 * X + (1/(normx*normx) - (1+cos(normx))/(2*normx * sin(normx)) ) * X * X; | ||||
|   Matrix3 actualDelFdeltheta = Rot3::LogmapDerivative(thetahat); | ||||
| 
 | ||||
|   // Compare Jacobians
 | ||||
|   EXPECT(assert_equal(expectedDelFdeltheta, actualDelFdeltheta)); | ||||
| 
 | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
|  | @ -354,7 +438,6 @@ TEST( ImuFactor, fistOrderExponential ) | |||
|   double alpha = 0.0; | ||||
|   Vector3 deltabiasOmega; deltabiasOmega << alpha,alpha,alpha; | ||||
| 
 | ||||
| 
 | ||||
|   const Matrix3 Jr = Rot3::ExpmapDerivative((measuredOmega - biasOmega) * deltaT); | ||||
| 
 | ||||
|   Matrix3  delRdelBiasOmega = - Jr * deltaT; // the delta bias appears with the minus sign
 | ||||
|  | @ -366,7 +449,7 @@ TEST( ImuFactor, fistOrderExponential ) | |||
|       hatRot * Rot3::Expmap(delRdelBiasOmega * deltabiasOmega).matrix(); | ||||
|   //hatRot * (Matrix3::Identity() + skewSymmetric(delRdelBiasOmega * deltabiasOmega));
 | ||||
| 
 | ||||
|   // Compare Jacobians
 | ||||
|   // This is a first order expansion so the equality is only an approximation
 | ||||
|   EXPECT(assert_equal(expectedRot, actualRot)); | ||||
| } | ||||
| 
 | ||||
|  | @ -423,6 +506,128 @@ TEST( ImuFactor, FirstOrderPreIntegratedMeasurements ) | |||
|   EXPECT(assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega(), 1e-3)); // 1e-3 needs to be added only when using quaternions for rotations
 | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| TEST( ImuFactor, JacobianPreintegratedCovariancePropagation ) | ||||
| { | ||||
|   // Linearization point
 | ||||
|   imuBias::ConstantBias bias; ///< Current estimate of acceleration and rotation rate biases
 | ||||
|   Pose3 body_P_sensor = Pose3(); // (Rot3::Expmap(Vector3(0,0.1,0.1)), Point3(1, 0, 1));
 | ||||
| 
 | ||||
|   // Measurements
 | ||||
|   list<Vector3> measuredAccs, measuredOmegas; | ||||
|   list<double> deltaTs; | ||||
|   measuredAccs.push_back(Vector3(0.1, 0.0, 0.0)); | ||||
|   measuredOmegas.push_back(Vector3(M_PI/100.0, 0.0, 0.0)); | ||||
|   deltaTs.push_back(0.01); | ||||
|   measuredAccs.push_back(Vector3(0.1, 0.0, 0.0)); | ||||
|   measuredOmegas.push_back(Vector3(M_PI/100.0, 0.0, 0.0)); | ||||
|   deltaTs.push_back(0.01); | ||||
|   for(int i=1;i<100;i++) | ||||
|   { | ||||
|     measuredAccs.push_back(Vector3(0.05, 0.09, 0.01)); | ||||
|     measuredOmegas.push_back(Vector3(M_PI/100.0, M_PI/300.0, 2*M_PI/100.0)); | ||||
|     deltaTs.push_back(0.01); | ||||
|   } | ||||
|   // Actual preintegrated values
 | ||||
|   ImuFactor::PreintegratedMeasurements preintegrated = | ||||
|       evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)); | ||||
| 
 | ||||
|   // so far we only created a nontrivial linearization point for the preintegrated measurements
 | ||||
|   // Now we add a new measurement and ask for Jacobians
 | ||||
|   const Vector3 newMeasuredAcc = Vector3(0.1, 0.0, 0.0); | ||||
|   const Vector3 newMeasuredOmega = Vector3(M_PI/100.0, 0.0, 0.0); | ||||
|   const double newDeltaT = 0.01; | ||||
|   const Rot3    deltaRij_old = preintegrated.deltaRij();    // before adding new measurement
 | ||||
|   const Vector3 deltaVij_old = preintegrated.deltaVij();    // before adding new measurement
 | ||||
|   const Vector3 deltaPij_old = preintegrated.deltaPij();    // before adding new measurement
 | ||||
| 
 | ||||
|   Matrix oldPreintCovariance = preintegrated.preintMeasCov(); | ||||
| 
 | ||||
|   Matrix Factual, Gactual; | ||||
|   preintegrated.integrateMeasurement(newMeasuredAcc, newMeasuredOmega, newDeltaT, | ||||
|       body_P_sensor, Factual, Gactual); | ||||
| 
 | ||||
|   bool use2ndOrderIntegration = false; | ||||
| 
 | ||||
|   //////////////////////////////////////////////////////////////////////////////////////////////
 | ||||
|   // COMPUTE NUMERICAL DERIVATIVES FOR F
 | ||||
|   //////////////////////////////////////////////////////////////////////////////////////////////
 | ||||
|   // Compute expected f_pos_vel wrt positions
 | ||||
|   Matrix dfpv_dpos = | ||||
|       numericalDerivative11<Vector, Vector3>(boost::bind(&updatePreintegratedPosVel, | ||||
|           _1, deltaVij_old, deltaRij_old, | ||||
|           newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaPij_old); | ||||
| 
 | ||||
|   // Compute expected f_pos_vel wrt velocities
 | ||||
|   Matrix dfpv_dvel = | ||||
|       numericalDerivative11<Vector, Vector3>(boost::bind(&updatePreintegratedPosVel, | ||||
|           deltaPij_old, _1, deltaRij_old, | ||||
|           newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaVij_old); | ||||
| 
 | ||||
|   // Compute expected f_pos_vel wrt angles
 | ||||
|   Matrix dfpv_dangle = | ||||
|       numericalDerivative11<Vector, Rot3>(boost::bind(&updatePreintegratedPosVel, | ||||
|           deltaPij_old, deltaVij_old, _1, | ||||
|           newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaRij_old); | ||||
| 
 | ||||
|   Matrix FexpectedTop6(6,9);   FexpectedTop6 << dfpv_dpos, dfpv_dvel, dfpv_dangle; | ||||
| 
 | ||||
|   // Compute expected f_rot wrt angles
 | ||||
|   Matrix dfr_dangle = | ||||
|       numericalDerivative11<Rot3, Rot3>(boost::bind(&updatePreintegratedRot, | ||||
|           _1, newMeasuredOmega, newDeltaT), deltaRij_old); | ||||
| 
 | ||||
|   Matrix FexpectedBottom3(3,9); | ||||
|   FexpectedBottom3 << Z_3x3, Z_3x3, dfr_dangle; | ||||
|   Matrix Fexpected(9,9);  Fexpected << FexpectedTop6, FexpectedBottom3; | ||||
| 
 | ||||
|   EXPECT(assert_equal(Fexpected, Factual)); | ||||
| 
 | ||||
|   //////////////////////////////////////////////////////////////////////////////////////////////
 | ||||
|   // COMPUTE NUMERICAL DERIVATIVES FOR G
 | ||||
|   //////////////////////////////////////////////////////////////////////////////////////////////
 | ||||
|   // Compute jacobian wrt integration noise
 | ||||
|   Matrix dgpv_dintNoise(6,3); | ||||
|   dgpv_dintNoise << I_3x3 * newDeltaT, Z_3x3; | ||||
| 
 | ||||
|   // Compute jacobian wrt acc noise
 | ||||
|   Matrix dgpv_daccNoise = | ||||
|       numericalDerivative11<Vector, Vector3>(boost::bind(&updatePreintegratedPosVel, | ||||
|           deltaPij_old, deltaVij_old, deltaRij_old, | ||||
|           _1, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), newMeasuredAcc); | ||||
| 
 | ||||
|   // Compute expected F wrt gyro noise
 | ||||
|   Matrix dgpv_domegaNoise = | ||||
|       numericalDerivative11<Vector, Vector3>(boost::bind(&updatePreintegratedPosVel, | ||||
|           deltaPij_old, deltaVij_old, deltaRij_old, | ||||
|           newMeasuredAcc, _1, newDeltaT, use2ndOrderIntegration), newMeasuredOmega); | ||||
|   Matrix GexpectedTop6(6,9); | ||||
|   GexpectedTop6 << dgpv_dintNoise, dgpv_daccNoise, dgpv_domegaNoise; | ||||
| 
 | ||||
|   // Compute expected f_rot wrt gyro noise
 | ||||
|   Matrix dgr_dangle = | ||||
|       numericalDerivative11<Rot3, Vector3>(boost::bind(&updatePreintegratedRot, | ||||
|           deltaRij_old, _1, newDeltaT), newMeasuredOmega); | ||||
| 
 | ||||
|   Matrix GexpectedBottom3(3,9); | ||||
|   GexpectedBottom3 << Z_3x3, Z_3x3, dgr_dangle; | ||||
|   Matrix Gexpected(9,9);  Gexpected << GexpectedTop6, GexpectedBottom3; | ||||
| 
 | ||||
|   EXPECT(assert_equal(Gexpected, Gactual)); | ||||
| 
 | ||||
|   // Check covariance propagation
 | ||||
|   Matrix9 measurementCovariance; | ||||
|   measurementCovariance << intNoiseVar*I_3x3, Z_3x3,             Z_3x3, | ||||
|                            Z_3x3,             accNoiseVar*I_3x3, Z_3x3, | ||||
|                            Z_3x3,             Z_3x3,             omegaNoiseVar*I_3x3; | ||||
| 
 | ||||
|   Matrix newPreintCovarianceExpected = Factual * oldPreintCovariance * Factual.transpose() + | ||||
|       (1/newDeltaT) * Gactual * measurementCovariance * Gactual.transpose(); | ||||
| 
 | ||||
|   Matrix newPreintCovarianceActual = preintegrated.preintMeasCov(); | ||||
|   EXPECT(assert_equal(newPreintCovarianceExpected, newPreintCovarianceActual)); | ||||
| } | ||||
| 
 | ||||
| //#include <gtsam/linear/GaussianFactorGraph.h>
 | ||||
| ///* ************************************************************************* */
 | ||||
| //TEST( ImuFactor, LinearizeTiming)
 | ||||
|  | @ -561,13 +766,11 @@ TEST(ImuFactor, PredictPositionAndVelocity){ | |||
|     // Predict
 | ||||
|     Pose3 x1; | ||||
|     Vector3 v1(0, 0.0, 0.0); | ||||
|     PoseVelocity poseVelocity = factor.Predict(x1, v1, bias, pre_int_data, gravity, omegaCoriolis); | ||||
|     PoseVelocityBias poseVelocity = pre_int_data.predict(x1, v1, bias, gravity, omegaCoriolis); | ||||
|     Pose3 expectedPose(Rot3(), Point3(0, 0.5, 0)); | ||||
|     Vector3 expectedVelocity; expectedVelocity<<0,1,0; | ||||
|     EXPECT(assert_equal(expectedPose, poseVelocity.pose)); | ||||
|     EXPECT(assert_equal(Vector(expectedVelocity), Vector(poseVelocity.velocity))); | ||||
| 
 | ||||
| 
 | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
|  | @ -595,7 +798,7 @@ TEST(ImuFactor, PredictRotation) { | |||
|     // Predict
 | ||||
|     Pose3 x1; | ||||
|     Vector3 v1(0, 0.0, 0.0); | ||||
|     PoseVelocity poseVelocity = factor.Predict(x1, v1, bias, pre_int_data, gravity, omegaCoriolis); | ||||
|     PoseVelocityBias poseVelocity = pre_int_data.predict(x1, v1, bias, gravity, omegaCoriolis); | ||||
|     Pose3 expectedPose(Rot3().ypr(M_PI/10, 0, 0), Point3(0, 0, 0)); | ||||
|     Vector3 expectedVelocity; expectedVelocity<<0,0,0; | ||||
|     EXPECT(assert_equal(expectedPose, poseVelocity.pose)); | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue