Taking ImuFactor noise model automatically from preintegrated measurement covariance
							parent
							
								
									0cb6e97803
								
							
						
					
					
						commit
						1eae17561d
					
				|  | @ -646,11 +646,10 @@ class ImuFactorPreintegratedMeasurements { | |||
| 
 | ||||
| virtual class ImuFactor : gtsam::NonlinearFactor { | ||||
|   ImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, size_t bias, | ||||
|       const gtsam::ImuFactorPreintegratedMeasurements& preintegratedMeasurements, Vector gravity, Vector omegaCoriolis, | ||||
|       const gtsam::noiseModel::Base* model); | ||||
|       const gtsam::ImuFactorPreintegratedMeasurements& preintegratedMeasurements, Vector gravity, Vector omegaCoriolis); | ||||
|   ImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, size_t bias, | ||||
|       const gtsam::ImuFactorPreintegratedMeasurements& preintegratedMeasurements, Vector gravity, Vector omegaCoriolis, | ||||
|       const gtsam::noiseModel::Base* model, const gtsam::Pose3& body_P_sensor); | ||||
|       const gtsam::Pose3& body_P_sensor); | ||||
| 
 | ||||
|   // Standard Interface
 | ||||
|   gtsam::ImuFactorPreintegratedMeasurements preintegratedMeasurements() const; | ||||
|  |  | |||
|  | @ -312,8 +312,8 @@ namespace gtsam { | |||
|     /** Constructor */ | ||||
|     ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias, | ||||
|         const PreintegratedMeasurements& preintegratedMeasurements, const Vector3& gravity, const Vector3& omegaCoriolis, | ||||
|         const SharedNoiseModel& model, boost::optional<const Pose3&> body_P_sensor = boost::none) : | ||||
|       Base(model, pose_i, vel_i, pose_j, vel_j, bias), | ||||
|         boost::optional<const Pose3&> body_P_sensor = boost::none) : | ||||
|       Base(noiseModel::Gaussian::Covariance(preintegratedMeasurements.PreintMeasCov), pose_i, vel_i, pose_j, vel_j, bias), | ||||
|       preintegratedMeasurements_(preintegratedMeasurements), | ||||
|       gravity_(gravity), | ||||
|       omegaCoriolis_(omegaCoriolis), | ||||
|  |  | |||
|  | @ -206,8 +206,7 @@ TEST( CombinedImuFactor, ErrorWithBiases ) | |||
| 
 | ||||
| 
 | ||||
|     // Create factor
 | ||||
|     noiseModel::Gaussian::shared_ptr model = noiseModel::Gaussian::Covariance(pre_int_data.PreintMeasCov); | ||||
|     ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis, model); | ||||
|     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, Combinedmodel); | ||||
|  |  | |||
|  | @ -182,8 +182,7 @@ TEST( ImuFactor, Error ) | |||
|   pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); | ||||
| 
 | ||||
|   // Create factor
 | ||||
|   noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector_(9, 0.15, 0.15, 0.15, 1.5, 1.5, 1.5, 0.5, 0.5, 0.5)); | ||||
|   ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis, model); | ||||
|   ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis); | ||||
| 
 | ||||
|   Vector errorActual = factor.evaluateError(x1, v1, x2, v2, bias); | ||||
| 
 | ||||
|  | @ -265,8 +264,7 @@ TEST( ImuFactor, ErrorWithBiases ) | |||
| //    pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
 | ||||
| 
 | ||||
|     // Create factor
 | ||||
|     noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector_(9, 0.15, 0.15, 0.15, 1.5, 1.5, 1.5, 0.5, 0.5, 0.5)); | ||||
|     ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis, model); | ||||
|     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); | ||||
|  | @ -320,7 +318,7 @@ TEST( ImuFactor, PartialDerivativeExpmap ) | |||
| 
 | ||||
|   // Compute numerical derivatives
 | ||||
|   Matrix expectedDelRdelBiasOmega = numericalDerivative11<Rot3, LieVector>(boost::bind( | ||||
|       &evaluateRotation, measuredOmega, _1, deltaT), biasOmega); | ||||
|       &evaluateRotation, measuredOmega, _1, deltaT), LieVector(biasOmega)); | ||||
| 
 | ||||
|   const Matrix3 Jr = ImuFactor::rightJacobianExpMapSO3((measuredOmega - biasOmega) * deltaT); | ||||
| 
 | ||||
|  | @ -343,7 +341,7 @@ TEST( ImuFactor, PartialDerivativeLogmap ) | |||
| 
 | ||||
|   // Compute numerical derivatives
 | ||||
|   Matrix expectedDelFdeltheta = numericalDerivative11<LieVector>(boost::bind( | ||||
|       &evaluateLogRotation, thetahat, _1), deltatheta); | ||||
|       &evaluateLogRotation, thetahat, _1), LieVector(deltatheta)); | ||||
| 
 | ||||
|   const Vector3 x = thetahat; // parametrization of so(3)
 | ||||
|   const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^
 | ||||
|  | @ -529,8 +527,7 @@ TEST( ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement ) | |||
|   pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); | ||||
| 
 | ||||
|     // Create factor
 | ||||
|     noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector_(9, 0.15, 0.15, 0.15, 1.5, 1.5, 1.5, 0.5, 0.5, 0.5)); | ||||
|     ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis, model); | ||||
|     ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis); | ||||
| 
 | ||||
|     // Expected Jacobians
 | ||||
|     Matrix H1e = numericalDerivative11<Pose3>( | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue