diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index 1c22bab9a..075d16022 100644 --- a/gtsam/navigation/CombinedImuFactor.cpp +++ b/gtsam/navigation/CombinedImuFactor.cpp @@ -33,19 +33,20 @@ using namespace std; //------------------------------------------------------------------------------ CombinedImuFactor::CombinedPreintegratedMeasurements::CombinedPreintegratedMeasurements( const imuBias::ConstantBias& bias, const Matrix3& measuredAccCovariance, - const Matrix3& measuredOmegaCovariance, const Matrix3& integrationErrorCovariance, - const Matrix3& biasAccCovariance, const Matrix3& biasOmegaCovariance, - const Matrix& biasAccOmegaInit, const bool use2ndOrderIntegration) : - PreintegrationBase(bias, measuredAccCovariance, measuredOmegaCovariance, - integrationErrorCovariance, use2ndOrderIntegration), - biasAccCovariance_(biasAccCovariance), biasOmegaCovariance_(biasOmegaCovariance), - biasAccOmegaInit_(biasAccOmegaInit) -{ + const Matrix3& measuredOmegaCovariance, + const Matrix3& integrationErrorCovariance, const Matrix3& biasAccCovariance, + const Matrix3& biasOmegaCovariance, const Matrix& biasAccOmegaInit, + const bool use2ndOrderIntegration) : + PreintegrationBase(bias, measuredAccCovariance, measuredOmegaCovariance, + integrationErrorCovariance, use2ndOrderIntegration), biasAccCovariance_( + biasAccCovariance), biasOmegaCovariance_(biasOmegaCovariance), biasAccOmegaInit_( + biasAccOmegaInit) { preintMeasCov_.setZero(); } //------------------------------------------------------------------------------ -void CombinedImuFactor::CombinedPreintegratedMeasurements::print(const string& s) const{ +void CombinedImuFactor::CombinedPreintegratedMeasurements::print( + const string& s) const { PreintegrationBase::print(s); cout << " biasAccCovariance [ " << biasAccCovariance_ << " ]" << endl; cout << " biasOmegaCovariance [ " << biasOmegaCovariance_ << " ]" << endl; @@ -54,31 +55,35 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::print(const string& s } //------------------------------------------------------------------------------ -bool CombinedImuFactor::CombinedPreintegratedMeasurements::equals(const CombinedPreintegratedMeasurements& expected, double tol) const{ - 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) +bool CombinedImuFactor::CombinedPreintegratedMeasurements::equals( + const CombinedPreintegratedMeasurements& expected, double tol) const { + 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(){ +void CombinedImuFactor::CombinedPreintegratedMeasurements::resetIntegration() { PreintegrationBase::resetIntegration(); preintMeasCov_.setZero(); } //------------------------------------------------------------------------------ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement( - const Vector3& measuredAcc, const Vector3& measuredOmega, - double deltaT, boost::optional body_P_sensor, + const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT, + boost::optional body_P_sensor, boost::optional F_test, boost::optional 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). Vector3 correctedAcc, correctedOmega; - correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega, correctedAcc, correctedOmega, body_P_sensor); + correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega, + correctedAcc, correctedOmega, body_P_sensor); 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 @@ -86,7 +91,8 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement( // Update Jacobians /* ----------------------------------------------------------------------------------------------------------------------- */ - updatePreintegratedJacobians(correctedAcc, D_Rincr_integratedOmega, Rincr, 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 @@ -98,11 +104,11 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement( updatePreintegratedMeasurements(correctedAcc, Rincr, deltaT, F_9x9); // Single Jacobians to propagate covariance - Matrix3 H_vel_biasacc = - R_i * deltaT; - Matrix3 H_angles_biasomega =- D_Rincr_integratedOmega * deltaT; + 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); + Matrix F(15, 15); // 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, @@ -110,45 +116,42 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement( // 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; + 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); + Matrix G_measCov_Gt = Matrix::Zero(15, 15); // 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) * - (accelerometerCovariance() + biasAccOmegaInit_.block<3,3>(0,0) ) * - (H_vel_biasacc.transpose()); - G_measCov_Gt.block<3,3>(6,6) = (1/deltaT) * (H_angles_biasomega) * - (gyroscopeCovariance() + biasAccOmegaInit_.block<3,3>(3,3) ) * - (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_; + G_measCov_Gt.block<3, 3>(0, 0) = deltaT * integrationCovariance(); + G_measCov_Gt.block<3, 3>(3, 3) = (1 / deltaT) * (H_vel_biasacc) + * (accelerometerCovariance() + biasAccOmegaInit_.block<3, 3>(0, 0)) + * (H_vel_biasacc.transpose()); + G_measCov_Gt.block<3, 3>(6, 6) = (1 / deltaT) * (H_angles_biasomega) + * (gyroscopeCovariance() + biasAccOmegaInit_.block<3, 3>(3, 3)) + * (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(); + 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; // 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); + if (F_test) { + F_test->resize(15, 15); (*F_test) << F; } - if(G_test){ - G_test->resize(15,21); + 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; + (*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; } } @@ -156,16 +159,22 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement( // CombinedImuFactor methods //------------------------------------------------------------------------------ CombinedImuFactor::CombinedImuFactor() : - ImuFactorBase(), _PIM_(imuBias::ConstantBias(), Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_6x6) {} + 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, +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 body_P_sensor, const bool 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) {} + 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 { @@ -174,13 +183,11 @@ gtsam::NonlinearFactor::shared_ptr CombinedImuFactor::clone() const { } //------------------------------------------------------------------------------ -void CombinedImuFactor::print(const string& s, const KeyFormatter& keyFormatter) const { - cout << s << "CombinedImuFactor(" - << keyFormatter(this->key1()) << "," - << keyFormatter(this->key2()) << "," - << keyFormatter(this->key3()) << "," - << keyFormatter(this->key4()) << "," - << keyFormatter(this->key5()) << "," +void CombinedImuFactor::print(const string& s, + const KeyFormatter& keyFormatter) const { + cout << s << "CombinedImuFactor(" << keyFormatter(this->key1()) << "," + << keyFormatter(this->key2()) << "," << keyFormatter(this->key3()) << "," + << keyFormatter(this->key4()) << "," << keyFormatter(this->key5()) << "," << keyFormatter(this->key6()) << ")\n"; ImuFactorBase::print(""); _PIM_.print(" preintegrated measurements:"); @@ -188,11 +195,11 @@ void CombinedImuFactor::print(const string& s, const KeyFormatter& keyFormatter) } //------------------------------------------------------------------------------ -bool CombinedImuFactor::equals(const NonlinearFactor& expected, double tol) const { - const This *e = dynamic_cast (&expected); - return e != NULL && Base::equals(*e, tol) - && _PIM_.equals(e->_PIM_, tol) - && ImuFactorBase::equals(*e, tol); +bool CombinedImuFactor::equals(const NonlinearFactor& expected, + double tol) const { + const This *e = dynamic_cast(&expected); + return e != NULL && Base::equals(*e, tol) && _PIM_.equals(e->_PIM_, tol) + && ImuFactorBase::equals(*e, tol); } //------------------------------------------------------------------------------ @@ -220,39 +227,39 @@ Vector CombinedImuFactor::evaluateError(const Pose3& pose_i, // if we need the jacobians if (H1) { H1->resize(15, 6); - H1->block < 9, 6 > (0, 0) = D_r_pose_i; + H1->block<9, 6>(0, 0) = D_r_pose_i; // adding: [dBiasAcc/dPi ; dBiasOmega/dPi] - H1->block < 6, 6 > (9, 0) = Z_6x6; + H1->block<6, 6>(9, 0) = Z_6x6; } if (H2) { H2->resize(15, 3); - H2->block < 9, 3 > (0, 0) = D_r_vel_i; + H2->block<9, 3>(0, 0) = D_r_vel_i; // adding: [dBiasAcc/dVi ; dBiasOmega/dVi] - H2->block < 6, 3 > (9, 0) = Matrix::Zero(6, 3); + H2->block<6, 3>(9, 0) = Matrix::Zero(6, 3); } if (H3) { H3->resize(15, 6); - H3->block < 9, 6 > (0, 0) = D_r_pose_j; + H3->block<9, 6>(0, 0) = D_r_pose_j; // adding: [dBiasAcc/dPj ; dBiasOmega/dPj] - H3->block < 6, 6 > (9, 0) = Z_6x6; + H3->block<6, 6>(9, 0) = Z_6x6; } if (H4) { H4->resize(15, 3); - H4->block < 9, 3 > (0, 0) = D_r_vel_j; + H4->block<9, 3>(0, 0) = D_r_vel_j; // adding: [dBiasAcc/dVi ; dBiasOmega/dVi] - H4->block < 6, 3 > (9, 0) = Matrix::Zero(6, 3); + H4->block<6, 3>(9, 0) = Matrix::Zero(6, 3); } if (H5) { H5->resize(15, 6); - H5->block < 9, 6 > (0, 0) = D_r_bias_i; + H5->block<9, 6>(0, 0) = D_r_bias_i; // adding: [dBiasAcc/dBias_i ; dBiasOmega/dBias_i] - H5->block < 6, 6 > (9, 0) = Hbias_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); + 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; + H6->block<6, 6>(9, 0) = Hbias_j; } // overall error diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index d2ad32122..50b7e7c01 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -65,7 +65,8 @@ namespace gtsam { * the correlation between the bias uncertainty and the preintegrated * measurements uncertainty. */ -class CombinedImuFactor: public NoiseModelFactor6, public ImuFactorBase{ +class CombinedImuFactor: public NoiseModelFactor6, public ImuFactorBase { public: /** @@ -82,11 +83,11 @@ public: protected: - Matrix3 biasAccCovariance_; ///< continuous-time "Covariance" describing accelerometer bias random walk + 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 + Matrix6 biasAccOmegaInit_; ///< covariance of bias used for pre-integration - Eigen::Matrix preintMeasCov_; ///< Covariance matrix of the preintegrated measurements + Eigen::Matrix 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 @@ -105,16 +106,20 @@ public: * @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) */ - CombinedPreintegratedMeasurements(const imuBias::ConstantBias& bias, const Matrix3& measuredAccCovariance, - const Matrix3& measuredOmegaCovariance, const Matrix3& integrationErrorCovariance, + CombinedPreintegratedMeasurements(const imuBias::ConstantBias& bias, + const Matrix3& measuredAccCovariance, + const Matrix3& measuredOmegaCovariance, + const Matrix3& integrationErrorCovariance, const Matrix3& biasAccCovariance, const Matrix3& biasOmegaCovariance, - const Matrix& biasAccOmegaInit, const bool use2ndOrderIntegration = false); + const Matrix& biasAccOmegaInit, const bool use2ndOrderIntegration = + false); /// print void print(const std::string& s = "Preintegrated Measurements:") const; /// equals - bool equals(const CombinedPreintegratedMeasurements& expected, double tol=1e-9) const; + bool equals(const CombinedPreintegratedMeasurements& expected, double tol = + 1e-9) const; /// Re-initialize CombinedPreintegratedMeasurements void resetIntegration(); @@ -126,12 +131,16 @@ public: * @param deltaT Time interval between two consecutive IMU measurements * @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, + void integrateMeasurement(const Vector3& measuredAcc, + const Vector3& measuredOmega, double deltaT, boost::optional body_P_sensor = boost::none, - boost::optional F_test = boost::none, boost::optional G_test = boost::none); + boost::optional F_test = boost::none, + boost::optional G_test = boost::none); /// methods to access class variables - Matrix preintMeasCov() const { return preintMeasCov_;} + Matrix preintMeasCov() const { + return preintMeasCov_; + } private: @@ -147,7 +156,8 @@ public: private: typedef CombinedImuFactor This; - typedef NoiseModelFactor6 Base; + typedef NoiseModelFactor6 Base; CombinedPreintegratedMeasurements _PIM_; @@ -177,12 +187,15 @@ public: * @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 */ - CombinedImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i, Key bias_j, + 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 body_P_sensor = boost::none, const bool use2ndOrderCoriolis = false); + boost::optional body_P_sensor = boost::none, + const bool use2ndOrderCoriolis = false); - virtual ~CombinedImuFactor() {} + virtual ~CombinedImuFactor() { + } /// @return a deep copy of this factor virtual gtsam::NonlinearFactor::shared_ptr clone() const; @@ -190,54 +203,60 @@ public: /** implement functions needed for Testable */ /// print - virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + virtual void print(const std::string& s, const KeyFormatter& keyFormatter = + DefaultKeyFormatter) const; /// equals - virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const; + virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const; /** Access the preintegrated measurements. */ const CombinedPreintegratedMeasurements& preintegratedMeasurements() const { - return _PIM_; } + 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, + Vector evaluateError(const Pose3& pose_i, const Vector3& vel_i, + const Pose3& pose_j, const Vector3& vel_j, const imuBias::ConstantBias& bias_i, const imuBias::ConstantBias& bias_j, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none, - boost::optional H3 = boost::none, - boost::optional H4 = boost::none, - boost::optional H5 = boost::none, - boost::optional H6 = boost::none) const; + boost::optional H1 = boost::none, boost::optional H2 = + boost::none, boost::optional H3 = boost::none, + boost::optional H4 = boost::none, boost::optional H5 = + boost::none, boost::optional H6 = boost::none) const; /** @deprecated The following function has been deprecated, use PreintegrationBase::predict with the same signature instead */ - static void Predict(const Pose3& pose_i, const Vector3& vel_i, Pose3& pose_j, Vector3& vel_j, - const imuBias::ConstantBias& bias_i, const CombinedPreintegratedMeasurements& PIM, const Vector3& gravity, + static void Predict(const Pose3& pose_i, const Vector3& vel_i, Pose3& pose_j, + Vector3& vel_j, const imuBias::ConstantBias& bias_i, + const CombinedPreintegratedMeasurements& PIM, const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false, boost::optional deltaPij_biascorrected_out = boost::none, boost::optional deltaVij_biascorrected_out = boost::none) { - PoseVelocityBias PVB(PIM.predict(pose_i, vel_i, bias_i, gravity, omegaCoriolis, use2ndOrderCoriolis, deltaPij_biascorrected_out, deltaVij_biascorrected_out)); - pose_j = PVB.pose; - vel_j = PVB.velocity; + PoseVelocityBias PVB( + PIM.predict(pose_i, vel_i, bias_i, gravity, omegaCoriolis, + use2ndOrderCoriolis, deltaPij_biascorrected_out, + deltaVij_biascorrected_out)); + pose_j = PVB.pose; + vel_j = PVB.velocity; } - private: /** Serialization function */ friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("NoiseModelFactor6", - boost::serialization::base_object(*this)); + ar + & boost::serialization::make_nvp("NoiseModelFactor6", + boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(_PIM_); ar & BOOST_SERIALIZATION_NVP(gravity_); ar & BOOST_SERIALIZATION_NVP(omegaCoriolis_); ar & BOOST_SERIALIZATION_NVP(body_P_sensor_); } -}; // class CombinedImuFactor +}; +// class CombinedImuFactor typedef CombinedImuFactor::CombinedPreintegratedMeasurements CombinedImuFactorPreintegratedMeasurements; diff --git a/gtsam/navigation/ImuBias.h b/gtsam/navigation/ImuBias.h index 9573c27aa..e65ad8da4 100644 --- a/gtsam/navigation/ImuBias.h +++ b/gtsam/navigation/ImuBias.h @@ -36,57 +36,63 @@ namespace gtsam { /// All bias models live in the imuBias namespace namespace imuBias { - class ConstantBias { - private: - Vector3 biasAcc_; - Vector3 biasGyro_; +class ConstantBias { +private: + Vector3 biasAcc_; + Vector3 biasGyro_; - public: - /// dimension of the variable - used to autodetect sizes - static const size_t dimension = 6; +public: + /// dimension of the variable - used to autodetect sizes + static const size_t dimension = 6; - ConstantBias(): - biasAcc_(0.0, 0.0, 0.0), biasGyro_(0.0, 0.0, 0.0) { - } + ConstantBias() : + biasAcc_(0.0, 0.0, 0.0), biasGyro_(0.0, 0.0, 0.0) { + } - ConstantBias(const Vector3& biasAcc, const Vector3& biasGyro): + ConstantBias(const Vector3& biasAcc, const Vector3& biasGyro) : biasAcc_(biasAcc), biasGyro_(biasGyro) { - } + } - ConstantBias(const Vector6& v): + ConstantBias(const Vector6& v) : biasAcc_(v.head<3>()), biasGyro_(v.tail<3>()) { + } + + /** return the accelerometer and gyro biases in a single vector */ + Vector6 vector() const { + Vector6 v; + v << biasAcc_, biasGyro_; + return v; + } + + /** get accelerometer bias */ + const Vector3& accelerometer() const { + return biasAcc_; + } + + /** get gyroscope bias */ + const Vector3& gyroscope() const { + return biasGyro_; + } + + /** Correct an accelerometer measurement using this bias model, and optionally compute Jacobians */ + Vector3 correctAccelerometer(const Vector3& measurement, + boost::optional H = boost::none) const { + if (H) { + H->resize(3, 6); + (*H) << -Matrix3::Identity(), Matrix3::Zero(); } + return measurement - biasAcc_; + } - /** return the accelerometer and gyro biases in a single vector */ - Vector6 vector() const { - Vector6 v; - v << biasAcc_, biasGyro_; - return v; - } - - /** get accelerometer bias */ - const Vector3& accelerometer() const { return biasAcc_; } - - /** get gyroscope bias */ - const Vector3& gyroscope() const { return biasGyro_; } - - /** Correct an accelerometer measurement using this bias model, and optionally compute Jacobians */ - Vector3 correctAccelerometer(const Vector3& measurement, boost::optional H=boost::none) const { - if (H) { - H->resize(3, 6); - (*H) << -Matrix3::Identity(), Matrix3::Zero(); - } - return measurement - biasAcc_; - } - - /** Correct a gyroscope measurement using this bias model, and optionally compute Jacobians */ - Vector3 correctGyroscope(const Vector3& measurement, boost::optional H=boost::none) const { - if (H) { - H->resize(3, 6); - (*H) << Matrix3::Zero(), -Matrix3::Identity(); - } - return measurement - biasGyro_; + /** Correct a gyroscope measurement using this bias model, and optionally compute Jacobians */ + Vector3 correctGyroscope(const Vector3& measurement, + boost::optional H = boost::none) const { + if (H) { + H->resize(3, 6); + (*H) << Matrix3::Zero(), -Matrix3::Identity(); } + return measurement - biasGyro_; + } // // H1: Jacobian w.r.t. IMUBias // // H2: Jacobian w.r.t. pose @@ -118,77 +124,93 @@ namespace imuBias { //// return measurement - bias_gyro_temp - R_G_to_I * w_earth_rate_G; // } - /// @} - /// @name Testable - /// @{ +/// @} +/// @name Testable +/// @{ - /// print with optional string - void print(const std::string& s = "") const { - // explicit printing for now. - std::cout << s + ".biasAcc [" << biasAcc_.transpose() << "]" << std::endl; - std::cout << s + ".biasGyro [" << biasGyro_.transpose() << "]" << std::endl; - } +/// print with optional string + void print(const std::string& s = "") const { + // explicit printing for now. + std::cout << s + ".biasAcc [" << biasAcc_.transpose() << "]" << std::endl; + std::cout << s + ".biasGyro [" << biasGyro_.transpose() << "]" << std::endl; + } - /** equality up to tolerance */ - inline bool equals(const ConstantBias& expected, double tol=1e-5) const { - return equal_with_abs_tol(biasAcc_, expected.biasAcc_, tol) - && equal_with_abs_tol(biasGyro_, expected.biasGyro_, tol); - } + /** equality up to tolerance */ + inline bool equals(const ConstantBias& expected, double tol = 1e-5) const { + return equal_with_abs_tol(biasAcc_, expected.biasAcc_, tol) + && equal_with_abs_tol(biasGyro_, expected.biasGyro_, tol); + } - /// @} - /// @name Group - /// @{ + /// @} + /// @name Group + /// @{ - /** identity for group operation */ - static ConstantBias identity() { return ConstantBias(); } + /** identity for group operation */ + static ConstantBias identity() { + return ConstantBias(); + } - /** inverse */ - inline ConstantBias operator-() const { - return ConstantBias(-biasAcc_, -biasGyro_); - } + /** inverse */ + inline ConstantBias operator-() const { + return ConstantBias(-biasAcc_, -biasGyro_); + } - /** addition */ - ConstantBias operator+(const ConstantBias& b) const { - return ConstantBias(biasAcc_ + b.biasAcc_, biasGyro_ + b.biasGyro_); - } + /** addition */ + ConstantBias operator+(const ConstantBias& b) const { + return ConstantBias(biasAcc_ + b.biasAcc_, biasGyro_ + b.biasGyro_); + } - /** subtraction */ - ConstantBias operator-(const ConstantBias& b) const { - return ConstantBias(biasAcc_ - b.biasAcc_, biasGyro_ - b.biasGyro_); - } + /** subtraction */ + ConstantBias operator-(const ConstantBias& b) const { + return ConstantBias(biasAcc_ - b.biasAcc_, biasGyro_ - b.biasGyro_); + } - /// @} + /// @} - /// @name Deprecated - /// @{ - ConstantBias inverse() { return -(*this);} - ConstantBias compose(const ConstantBias& q) { return (*this)+q;} - ConstantBias between(const ConstantBias& q) { return q-(*this);} - Vector6 localCoordinates(const ConstantBias& q) { return between(q).vector();} - ConstantBias retract(const Vector6& v) {return compose(ConstantBias(v));} - static Vector6 Logmap(const ConstantBias& p) {return p.vector();} - static ConstantBias Expmap(const Vector6& v) { return ConstantBias(v);} - /// @} + /// @name Deprecated + /// @{ + ConstantBias inverse() { + return -(*this); + } + ConstantBias compose(const ConstantBias& q) { + return (*this) + q; + } + ConstantBias between(const ConstantBias& q) { + return q - (*this); + } + Vector6 localCoordinates(const ConstantBias& q) { + return between(q).vector(); + } + ConstantBias retract(const Vector6& v) { + return compose(ConstantBias(v)); + } + static Vector6 Logmap(const ConstantBias& p) { + return p.vector(); + } + static ConstantBias Expmap(const Vector6& v) { + return ConstantBias(v); + } + /// @} - private: +private: - /// @name Advanced Interface - /// @{ + /// @name Advanced Interface + /// @{ - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) - { - ar & boost::serialization::make_nvp("imuBias::ConstantBias",*this); - ar & BOOST_SERIALIZATION_NVP(biasAcc_); - ar & BOOST_SERIALIZATION_NVP(biasGyro_); - } + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("imuBias::ConstantBias", *this); + ar & BOOST_SERIALIZATION_NVP(biasAcc_); + ar & BOOST_SERIALIZATION_NVP(biasGyro_); + } - /// @} + /// @} - }; // ConstantBias class -} // namespace imuBias +}; +// ConstantBias class +}// namespace imuBias template<> struct traits : public internal::VectorSpace< @@ -197,4 +219,3 @@ struct traits : public internal::VectorSpace< } // namespace gtsam - diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp index 81120b7c6..728251636 100644 --- a/gtsam/navigation/ImuFactor.cpp +++ b/gtsam/navigation/ImuFactor.cpp @@ -33,12 +33,11 @@ using namespace std; //------------------------------------------------------------------------------ ImuFactor::PreintegratedMeasurements::PreintegratedMeasurements( const imuBias::ConstantBias& bias, const Matrix3& measuredAccCovariance, - const Matrix3& measuredOmegaCovariance, const Matrix3& integrationErrorCovariance, + const Matrix3& measuredOmegaCovariance, + const Matrix3& integrationErrorCovariance, const bool use2ndOrderIntegration) : - PreintegrationBase(bias, - measuredAccCovariance, measuredOmegaCovariance, - integrationErrorCovariance, use2ndOrderIntegration) -{ + PreintegrationBase(bias, measuredAccCovariance, measuredOmegaCovariance, + integrationErrorCovariance, use2ndOrderIntegration) { preintMeasCov_.setZero(); } @@ -49,13 +48,14 @@ void ImuFactor::PreintegratedMeasurements::print(const string& s) const { } //------------------------------------------------------------------------------ -bool ImuFactor::PreintegratedMeasurements::equals(const PreintegratedMeasurements& expected, double tol) const { +bool ImuFactor::PreintegratedMeasurements::equals( + const PreintegratedMeasurements& expected, double tol) const { return equal_with_abs_tol(preintMeasCov_, expected.preintMeasCov_, tol) - && PreintegrationBase::equals(expected, tol); + && PreintegrationBase::equals(expected, tol); } //------------------------------------------------------------------------------ -void ImuFactor::PreintegratedMeasurements::resetIntegration(){ +void ImuFactor::PreintegratedMeasurements::resetIntegration() { PreintegrationBase::resetIntegration(); preintMeasCov_.setZero(); } @@ -63,18 +63,20 @@ void ImuFactor::PreintegratedMeasurements::resetIntegration(){ //------------------------------------------------------------------------------ void ImuFactor::PreintegratedMeasurements::integrateMeasurement( const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT, - boost::optional body_P_sensor, - OptionalJacobian<9, 9> F_test, OptionalJacobian<9, 9> G_test) { + boost::optional body_P_sensor, OptionalJacobian<9, 9> F_test, + OptionalJacobian<9, 9> G_test) { Vector3 correctedAcc, correctedOmega; - correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega, correctedAcc, correctedOmega, body_P_sensor); + correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega, + correctedAcc, correctedOmega, body_P_sensor); 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 - updatePreintegratedJacobians(correctedAcc, D_Rincr_integratedOmega, Rincr, deltaT); + updatePreintegratedJacobians(correctedAcc, D_Rincr_integratedOmega, Rincr, + deltaT); // Update preintegrated measurements (also get Jacobian) const Matrix3 R_i = deltaRij(); // store this, which is useful to compute G_test @@ -90,31 +92,34 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement( // 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; + 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; Matrix3 F_pos_noiseacc; - if(use2ndOrderIntegration()){ + if (use2ndOrderIntegration()) { F_pos_noiseacc = 0.5 * R_i * deltaT * deltaT; - preintMeasCov_.block<3,3>(0,0) += (1/deltaT) * F_pos_noiseacc * accelerometerCovariance() * F_pos_noiseacc.transpose(); + preintMeasCov_.block<3, 3>(0, 0) += (1 / deltaT) * F_pos_noiseacc + * accelerometerCovariance() * F_pos_noiseacc.transpose(); Matrix3 temp = F_pos_noiseacc * accelerometerCovariance() * R_i.transpose(); // already includes 1/deltaT - preintMeasCov_.block<3,3>(0,3) += temp; - preintMeasCov_.block<3,3>(3,0) += temp.transpose(); + preintMeasCov_.block<3, 3>(0, 3) += temp; + preintMeasCov_.block<3, 3>(3, 0) += temp.transpose(); } // 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 + 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 - if(!use2ndOrderIntegration()) + if (G_test) { // This in only for testing & documentation, while the actual computation is done block-wise + if (!use2ndOrderIntegration()) F_pos_noiseacc = Z_3x3; // intNoise accNoise omegaNoise - (*G_test) << I_3x3 * deltaT, F_pos_noiseacc, Z_3x3, // pos - Z_3x3, R_i * deltaT, Z_3x3, // vel - Z_3x3, Z_3x3, D_Rincr_integratedOmega * deltaT; // angle + (*G_test) << I_3x3 * deltaT, F_pos_noiseacc, Z_3x3, // pos + Z_3x3, R_i * deltaT, Z_3x3, // vel + Z_3x3, Z_3x3, D_Rincr_integratedOmega * deltaT; // angle } } @@ -122,19 +127,20 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement( // ImuFactor methods //------------------------------------------------------------------------------ ImuFactor::ImuFactor() : - ImuFactorBase(), _PIM_(imuBias::ConstantBias(), Z_3x3, Z_3x3, Z_3x3) {} + ImuFactorBase(), _PIM_(imuBias::ConstantBias(), Z_3x3, Z_3x3, Z_3x3) { +} //------------------------------------------------------------------------------ -ImuFactor::ImuFactor( - Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias, +ImuFactor::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 body_P_sensor, - const bool 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) {} + boost::optional body_P_sensor, const bool 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 { @@ -144,12 +150,10 @@ gtsam::NonlinearFactor::shared_ptr ImuFactor::clone() const { //------------------------------------------------------------------------------ void ImuFactor::print(const string& s, const KeyFormatter& keyFormatter) const { - cout << s << "ImuFactor(" - << keyFormatter(this->key1()) << "," - << keyFormatter(this->key2()) << "," - << keyFormatter(this->key3()) << "," - << keyFormatter(this->key4()) << "," - << keyFormatter(this->key5()) << ")\n"; + cout << s << "ImuFactor(" << keyFormatter(this->key1()) << "," + << keyFormatter(this->key2()) << "," << keyFormatter(this->key3()) << "," + << keyFormatter(this->key4()) << "," << keyFormatter(this->key5()) + << ")\n"; ImuFactorBase::print(""); _PIM_.print(" preintegrated measurements:"); this->noiseModel_->print(" noise model: "); @@ -157,10 +161,9 @@ void ImuFactor::print(const string& s, const KeyFormatter& keyFormatter) const { //------------------------------------------------------------------------------ bool ImuFactor::equals(const NonlinearFactor& expected, double tol) const { - const This *e = dynamic_cast (&expected); - return e != NULL && Base::equals(*e, tol) - && _PIM_.equals(e->_PIM_, tol) - && ImuFactorBase::equals(*e, tol); + const This *e = dynamic_cast(&expected); + return e != NULL && Base::equals(*e, tol) && _PIM_.equals(e->_PIM_, tol) + && ImuFactorBase::equals(*e, tol); } //------------------------------------------------------------------------------ diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index 984d62d08..02e648bbe 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -58,7 +58,8 @@ namespace gtsam { * (which are usually slowly varying quantities), which is up to the caller. * See also CombinedImuFactor for a class that does this for you. */ -class ImuFactor: public NoiseModelFactor5, public ImuFactorBase { +class ImuFactor: public NoiseModelFactor5, public ImuFactorBase { public: /** @@ -75,7 +76,7 @@ public: protected: - Eigen::Matrix preintMeasCov_; ///< COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION] + Eigen::Matrix preintMeasCov_; ///< COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION] ///< (first-order propagation from *measurementCovariance*). public: @@ -90,14 +91,17 @@ public: * (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) */ PreintegratedMeasurements(const imuBias::ConstantBias& bias, - const Matrix3& measuredAccCovariance, const Matrix3& measuredOmegaCovariance, - const Matrix3& integrationErrorCovariance, const bool use2ndOrderIntegration = false); + const Matrix3& measuredAccCovariance, + const Matrix3& measuredOmegaCovariance, + const Matrix3& integrationErrorCovariance, + const bool use2ndOrderIntegration = false); /// print void print(const std::string& s = "Preintegrated Measurements:") const; /// equals - bool equals(const PreintegratedMeasurements& expected, double tol=1e-9) const; + bool equals(const PreintegratedMeasurements& expected, + double tol = 1e-9) const; /// Re-initialize PreintegratedMeasurements void resetIntegration(); @@ -110,12 +114,16 @@ public: * @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, + void integrateMeasurement(const Vector3& measuredAcc, + const Vector3& measuredOmega, double deltaT, boost::optional body_P_sensor = boost::none, - OptionalJacobian<9, 9> Fout = boost::none, OptionalJacobian<9, 9> Gout = boost::none); + OptionalJacobian<9, 9> Fout = boost::none, OptionalJacobian<9, 9> Gout = + boost::none); /// methods to access class variables - Matrix preintMeasCov() const { return preintMeasCov_;} + Matrix preintMeasCov() const { + return preintMeasCov_; + } private: @@ -128,14 +136,15 @@ public: } }; - private: +private: typedef ImuFactor This; - typedef NoiseModelFactor5 Base; + typedef NoiseModelFactor5 Base; PreintegratedMeasurements _PIM_; - public: +public: /** Shorthand for a smart pointer to a factor */ #if !defined(_MSC_VER) && __GNUC__ == 4 && __GNUC_MINOR__ > 5 @@ -163,9 +172,11 @@ public: 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 body_P_sensor = boost::none, const bool use2ndOrderCoriolis = false); + boost::optional body_P_sensor = boost::none, + const bool use2ndOrderCoriolis = false); - virtual ~ImuFactor() {} + virtual ~ImuFactor() { + } /// @return a deep copy of this factor virtual gtsam::NonlinearFactor::shared_ptr clone() const; @@ -173,52 +184,59 @@ public: /** implement functions needed for Testable */ /// print - virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + virtual void print(const std::string& s, const KeyFormatter& keyFormatter = + DefaultKeyFormatter) const; /// equals - virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const; + virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const; /** Access the preintegrated measurements. */ const PreintegratedMeasurements& preintegratedMeasurements() const { - return _PIM_; } + 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 H1 = boost::none, - boost::optional H2 = boost::none, - boost::optional H3 = boost::none, - boost::optional H4 = boost::none, - boost::optional H5 = boost::none) const; + Vector evaluateError(const Pose3& pose_i, const Vector3& vel_i, + const Pose3& pose_j, const Vector3& vel_j, + const imuBias::ConstantBias& bias, boost::optional H1 = + boost::none, boost::optional H2 = boost::none, + boost::optional H3 = boost::none, boost::optional H4 = + boost::none, boost::optional H5 = boost::none) const; /** @deprecated The following function has been deprecated, use PreintegrationBase::predict with the same signature instead */ - static void Predict(const Pose3& pose_i, const Vector3& vel_i, Pose3& pose_j, Vector3& vel_j, - const imuBias::ConstantBias& bias_i, const PreintegratedMeasurements& PIM, const Vector3& gravity, + static void Predict(const Pose3& pose_i, const Vector3& vel_i, Pose3& pose_j, + Vector3& vel_j, const imuBias::ConstantBias& bias_i, + const PreintegratedMeasurements& PIM, const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false, boost::optional deltaPij_biascorrected_out = boost::none, boost::optional deltaVij_biascorrected_out = boost::none) { - PoseVelocityBias PVB(PIM.predict(pose_i, vel_i, bias_i, gravity, omegaCoriolis, use2ndOrderCoriolis, deltaPij_biascorrected_out, deltaVij_biascorrected_out)); - pose_j = PVB.pose; - vel_j = PVB.velocity; + PoseVelocityBias PVB( + PIM.predict(pose_i, vel_i, bias_i, gravity, omegaCoriolis, + use2ndOrderCoriolis, deltaPij_biascorrected_out, + deltaVij_biascorrected_out)); + pose_j = PVB.pose; + vel_j = PVB.velocity; } - private: +private: /** Serialization function */ friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("NoiseModelFactor5", - boost::serialization::base_object(*this)); + ar + & boost::serialization::make_nvp("NoiseModelFactor5", + boost::serialization::base_object(*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 +}; +// class ImuFactor typedef ImuFactor::PreintegratedMeasurements ImuFactorPreintegratedMeasurements; diff --git a/gtsam/navigation/ImuFactorBase.h b/gtsam/navigation/ImuFactorBase.h index 1b7919a82..f5b910ff1 100644 --- a/gtsam/navigation/ImuFactorBase.h +++ b/gtsam/navigation/ImuFactorBase.h @@ -33,15 +33,16 @@ protected: Vector3 gravity_; Vector3 omegaCoriolis_; - boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame + boost::optional 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) {} + 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 @@ -51,21 +52,29 @@ public: * @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 body_P_sensor = boost::none, const bool use2ndOrderCoriolis = false) : - gravity_(gravity), omegaCoriolis_(omegaCoriolis), - body_P_sensor_(body_P_sensor), use2ndOrderCoriolis_(use2ndOrderCoriolis) {} + boost::optional 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_; } + 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_) + 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: "); } @@ -73,10 +82,11 @@ public: //------------------------------------------------------------------------------ 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_))); + && 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_))); } }; diff --git a/gtsam/navigation/PreintegratedRotation.h b/gtsam/navigation/PreintegratedRotation.h index 67deb2d99..64ea9a1a1 100644 --- a/gtsam/navigation/PreintegratedRotation.h +++ b/gtsam/navigation/PreintegratedRotation.h @@ -32,12 +32,12 @@ namespace gtsam { */ class PreintegratedRotation { - Rot3 deltaRij_; ///< Preintegrated relative orientation (in frame i) - double deltaTij_; ///< Time interval from i to j + 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 + Matrix3 gyroscopeCovariance_; ///< continuous-time "Covariance" of gyroscope measurements public: @@ -45,35 +45,49 @@ public: * Default constructor, initializes the variables in the base class */ PreintegratedRotation(const Matrix3& measuredOmegaCovariance) : - deltaRij_(Rot3()), deltaTij_(0.0), - delRdelBiasOmega_(Z_3x3), gyroscopeCovariance_(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(OptionalJacobian<3,3> 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_;} + Matrix3 deltaRij() const { + return deltaRij_.matrix(); + } // expensive + Vector3 thetaRij(OptionalJacobian<3, 3> 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; + 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); + && 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(){ + void resetIntegration() { deltaRij_ = Rot3(); deltaTij_ = 0.0; delRdelBiasOmega_ = Z_3x3; @@ -81,7 +95,7 @@ public: /// Update preintegrated measurements void updateIntegratedRotationAndDeltaT(const Rot3& incrR, const double deltaT, - OptionalJacobian<3, 3> H = boost::none){ + OptionalJacobian<3, 3> H = boost::none) { deltaRij_ = deltaRij_.compose(incrR, H, boost::none); deltaTij_ += deltaT; } @@ -90,15 +104,16 @@ public: * Update Jacobians to be used during preintegration * TODO: explain arguments */ - void update_delRdelBiasOmega(const Matrix3& D_Rincr_integratedOmega, const Rot3& incrR, - double deltaT) { + 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; + 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); + return deltaRij_ * Rot3::Expmap(delRdelBiasOmega_ * biasOmegaIncr); } /// Get so<3> version of bias corrected rotation, with optional Jacobian @@ -111,7 +126,8 @@ public: 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 Vector3 biascorrectedOmega = Rot3::Logmap(deltaRij_biascorrected, + Jrinv_theta_bc); const Matrix3 Jr_JbiasOmegaIncr = // Rot3::ExpmapDerivative(delRdelBiasOmega_ * biasOmegaIncr); (*H) = Jrinv_theta_bc * Jr_JbiasOmegaIncr * delRdelBiasOmega_; diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index 26bbf1ca2..b8cd08a6d 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -36,7 +36,7 @@ struct PoseVelocityBias { PoseVelocityBias(const Pose3& _pose, const Vector3& _velocity, const imuBias::ConstantBias _bias) : - pose(_pose), velocity(_velocity), bias(_bias) { + pose(_pose), velocity(_velocity), bias(_bias) { } }; @@ -46,7 +46,7 @@ struct PoseVelocityBias { * It includes the definitions of the preintegrated variables and the methods * to access, print, and compare them. */ -class PreintegrationBase : public PreintegratedRotation { +class PreintegrationBase: public PreintegratedRotation { imuBias::ConstantBias biasHat_; ///< Acceleration and angular rate bias values used during preintegration bool use2ndOrderIntegration_; ///< Controls the order of integration @@ -54,13 +54,13 @@ class PreintegrationBase : public PreintegratedRotation { 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 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 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 + Matrix3 integrationCovariance_; ///< continuous-time "Covariance" describing integration uncertainty /// (to compensate errors in Euler integration) public: @@ -72,35 +72,61 @@ public: * (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) {} + 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 - bool use2ndOrderIntegration() const {return use2ndOrderIntegration_;} - 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_;} + bool use2ndOrderIntegration() const { + return use2ndOrderIntegration_; + } + 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_;} + 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 << " 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"); @@ -109,19 +135,23 @@ public: /// 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); + && 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(){ + void resetIntegration() { PreintegratedRotation::resetIntegration(); deltaPij_ = Vector3::Zero(); deltaVij_ = Vector3::Zero(); @@ -133,51 +163,55 @@ public: /// Update preintegrated measurements void updatePreintegratedMeasurements(const Vector3& correctedAcc, - const Rot3& incrR, const double deltaT, OptionalJacobian<9, 9> F) { + const Rot3& incrR, const double deltaT, OptionalJacobian<9, 9> F) { Matrix3 dRij = deltaRij(); // expensive Vector3 temp = dRij * correctedAcc * deltaT; - if(!use2ndOrderIntegration_){ + if (!use2ndOrderIntegration_) { deltaPij_ += deltaVij_ * deltaT; - }else{ + } 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) + 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; + if (F) { + Matrix3 F_vel_angles = -R_i * skewSymmetric(correctedAcc) * deltaT; Matrix3 F_pos_angles; - if(use2ndOrderIntegration_) + if (use2ndOrderIntegration_) F_pos_angles = 0.5 * F_vel_angles * deltaT; else F_pos_angles = Z_3x3; // pos vel angle - *F << I_3x3, I_3x3 * deltaT, F_pos_angles, // pos - Z_3x3, I_3x3, F_vel_angles, // vel - Z_3x3, Z_3x3, F_angles_angles;// angle + *F << I_3x3, I_3x3 * deltaT, F_pos_angles, // 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){ + const Matrix3& D_Rincr_integratedOmega, const Rot3& incrR, + double deltaT) { Matrix3 dRij = deltaRij(); // expensive - Matrix3 temp = -dRij * skewSymmetric(correctedAcc) * deltaT * delRdelBiasOmega(); + 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); + 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); + update_delRdelBiasOmega(D_Rincr_integratedOmega, incrR, deltaT); } void correctMeasurementsByBiasAndSensorPose(const Vector3& measuredAcc, @@ -188,11 +222,13 @@ public: // Then compensate for sensor-body displacement: we express the quantities // (originally in the IMU frame) into the body frame - if(body_P_sensor){ + 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(); + 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 } } @@ -214,39 +250,42 @@ public: // Predict state at time j /* ---------------------------------------------------------------------------------------------------- */ - Vector3 deltaPij_biascorrected = deltaPij() + delPdelBiasAcc() * biasAccIncr + delPdelBiasOmega() * biasOmegaIncr; - if(deltaPij_biascorrected_out)// if desired, store this + 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 + 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(); + - 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 + 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 + 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 + 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 ); + 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) ); + Pose3 pose_j = Pose3(Rot_j, Point3(pos_j)); return PoseVelocityBias(pose_j, vel_j, bias_i); // bias is predicted as a constant } @@ -256,11 +295,10 @@ public: 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 { + 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 biasOmegaIncr = bias_i.gyroscope() - biasHat().gyroscope(); @@ -274,13 +312,15 @@ public: /* ---------------------------------------------------------------------------------------------------- */ Vector3 deltaPij_biascorrected, deltaVij_biascorrected; PoseVelocityBias predictedState_j = predict(pose_i, vel_i, bias_i, gravity, - omegaCoriolis, use2ndOrderCoriolis, deltaPij_biascorrected, deltaVij_biascorrected); + 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() ); + 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 ); + 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) @@ -289,101 +329,107 @@ public: // 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); + Vector3 biascorrectedOmega = biascorrectedThetaRij(biasOmegaIncr, + H5 ? &D_cThetaRij_biasOmegaIncr : 0); // Coriolis term, note inconsistent with AHRS, where coriolisHat is *after* integration const Vector3 coriolis = integrateCoriolis(Ri, omegaCoriolis); - Vector3 correctedOmega = biascorrectedOmega - coriolis; + Vector3 correctedOmega = biascorrectedOmega - coriolis; // Calculate Jacobians, matrices below needed only for some Jacobians... Vector3 fR; Rot3 correctedDeltaRij, fRrot; - Matrix3 D_cDeltaRij_cOmega, D_coriolis, D_fR_fRrot, Ritranspose_omegaCoriolisHat; + Matrix3 D_cDeltaRij_cOmega, D_coriolis, D_fR_fRrot, + Ritranspose_omegaCoriolisHat; - if (H1 || H2) Ritranspose_omegaCoriolisHat = Ri.transpose() * skewSymmetric(omegaCoriolis); + if (H1 || H2) + Ritranspose_omegaCoriolisHat = Ri.transpose() + * skewSymmetric(omegaCoriolis); // 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); + 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); + 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); + if (H1) { + H1->resize(9, 6); Matrix3 dfPdPi = -I_3x3; Matrix3 dfVdPi = Z_3x3; - if(use2ndOrderCoriolis){ + 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(); + Matrix3 temp = Ritranspose_omegaCoriolisHat + * (-Ritranspose_omegaCoriolisHat.transpose()); + dfPdPi += 0.5 * temp * deltaTij() * deltaTij(); dfVdPi += temp * deltaTij(); } (*H1) << - // dfP/dRi + // 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; + // 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); + 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 + // 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; + Z_3x3; } - if(H3) { - H3->resize(9,6); + if (H3) { + H3->resize(9, 6); (*H3) << - // dfP/dPosej + // dfP/dPosej Z_3x3, Ri.transpose() * Rj.matrix(), - // dfV/dPosej - Matrix::Zero(3,6), - // dfR/dPosej - D_fR_fRrot, Z_3x3; + // dfV/dPosej + Matrix::Zero(3, 6), + // dfR/dPosej + D_fR_fRrot, Z_3x3; } - if(H4) { - H4->resize(9,3); + if (H4) { + H4->resize(9, 3); (*H4) << - // dfP/dVj + // dfP/dVj Z_3x3, - // dfV/dVj - Ri.transpose(), - // dfR/dVj - Z_3x3; + // dfV/dVj + Ri.transpose(), + // dfR/dVj + Z_3x3; } - if(H5) { + 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->resize(9, 6); (*H5) << - // dfP/dBias - - delPdelBiasAcc(), - delPdelBiasOmega(), - // dfV/dBias - - delVdelBiasAcc(), - delVdelBiasOmega(), - // dfR/dBias - Z_3x3, D_fR_fRrot * ( - fRrot.inverse().matrix() * JbiasOmega); + // 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; + Vector9 r; + r << fp, fv, fR; return r; } @@ -409,22 +455,29 @@ protected: Vector3 gravity_; Vector3 omegaCoriolis_; - boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame + boost::optional 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) {} + 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 body_P_sensor = boost::none, const bool use2ndOrderCoriolis = false) : - gravity_(gravity), omegaCoriolis_(omegaCoriolis), - body_P_sensor_(body_P_sensor), use2ndOrderCoriolis_(use2ndOrderCoriolis) {} + boost::optional 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_; } + const Vector3& gravity() const { + return gravity_; + } + const Vector3& omegaCoriolis() const { + return omegaCoriolis_; + } }; diff --git a/gtsam/navigation/tests/testCombinedImuFactor.cpp b/gtsam/navigation/tests/testCombinedImuFactor.cpp index 00033978f..9fb0f939b 100644 --- a/gtsam/navigation/tests/testCombinedImuFactor.cpp +++ b/gtsam/navigation/tests/testCombinedImuFactor.cpp @@ -44,36 +44,39 @@ 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, +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) { Matrix3 dRij = deltaRij_old.matrix(); Vector3 temp = dRij * (correctedAcc - bias_old.accelerometer()) * deltaT; Vector3 deltaPij_new; - if(!use2ndOrderIntegration){ + if (!use2ndOrderIntegration) { deltaPij_new = deltaPij_old + deltaVij_old * deltaT; - }else{ + } 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); + 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(); + 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){ +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); + Vector result = updatePreintegratedMeasurementsTest(deltaPij_old, + deltaVij_old, deltaRij_old, bias_old, correctedAcc, correctedOmega, + deltaT, use2ndOrderIntegration); return Rot3::Expmap(result.segment<3>(6)); } @@ -82,60 +85,53 @@ Rot3 updatePreintegratedMeasurementsRot( // delPdelBiasAcc_ delPdelBiasOmega_ delVdelBiasAcc_ delVdelBiasOmega_ delRdelBiasOmega_ /* ************************************************************************* */ CombinedImuFactor::CombinedPreintegratedMeasurements evaluatePreintegratedMeasurements( - const imuBias::ConstantBias& bias, - const list& measuredAccs, - const list& measuredOmegas, - const list& deltaTs){ - CombinedImuFactor::CombinedPreintegratedMeasurements result(bias, Matrix3::Identity(), - Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), Matrix::Identity(6,6), false); + const imuBias::ConstantBias& bias, const list& measuredAccs, + const list& measuredOmegas, const list& deltaTs) { + CombinedImuFactor::CombinedPreintegratedMeasurements result(bias, + Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), + Matrix3::Identity(), Matrix3::Identity(), Matrix::Identity(6, 6), false); list::const_iterator itAcc = measuredAccs.begin(); list::const_iterator itOmega = measuredOmegas.begin(); list::const_iterator itDeltaT = deltaTs.begin(); - for( ; itAcc != measuredAccs.end(); ++itAcc, ++itOmega, ++itDeltaT) { + for (; itAcc != measuredAccs.end(); ++itAcc, ++itOmega, ++itDeltaT) { result.integrateMeasurement(*itAcc, *itOmega, *itDeltaT); } return result; } Vector3 evaluatePreintegratedMeasurementsPosition( - const imuBias::ConstantBias& bias, - const list& measuredAccs, - const list& measuredOmegas, - const list& deltaTs){ - return evaluatePreintegratedMeasurements(bias, - measuredAccs, measuredOmegas, deltaTs).deltaPij(); + const imuBias::ConstantBias& bias, const list& measuredAccs, + const list& measuredOmegas, const list& deltaTs) { + return evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, + deltaTs).deltaPij(); } Vector3 evaluatePreintegratedMeasurementsVelocity( - const imuBias::ConstantBias& bias, - const list& measuredAccs, - const list& measuredOmegas, - const list& deltaTs){ - return evaluatePreintegratedMeasurements(bias, - measuredAccs, measuredOmegas, deltaTs).deltaVij(); + const imuBias::ConstantBias& bias, const list& measuredAccs, + const list& measuredOmegas, const list& deltaTs) { + return evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, + deltaTs).deltaVij(); } Rot3 evaluatePreintegratedMeasurementsRotation( - const imuBias::ConstantBias& bias, - const list& measuredAccs, - const list& measuredOmegas, - const list& deltaTs){ - return Rot3(evaluatePreintegratedMeasurements(bias, - measuredAccs, measuredOmegas, deltaTs).deltaRij()); + const imuBias::ConstantBias& bias, const list& measuredAccs, + const list& measuredOmegas, const list& deltaTs) { + return Rot3( + evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, + deltaTs).deltaRij()); } } /* ************************************************************************* */ -TEST( CombinedImuFactor, PreintegratedMeasurements ) -{ +TEST( CombinedImuFactor, PreintegratedMeasurements ) { // Linearization point - imuBias::ConstantBias bias(Vector3(0,0,0), Vector3(0,0,0)); ///< Current estimate of acceleration and angular rate biases + imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); ///< Current estimate of acceleration and angular rate biases // Measurements Vector3 measuredAcc(0.1, 0.0, 0.0); - Vector3 measuredOmega(M_PI/100.0, 0.0, 0.0); + Vector3 measuredOmega(M_PI / 100.0, 0.0, 0.0); double deltaT = 0.5; double tol = 1e-6; @@ -145,58 +141,75 @@ TEST( CombinedImuFactor, PreintegratedMeasurements ) expected1.integrateMeasurement(measuredAcc, measuredOmega, deltaT); CombinedImuFactor::CombinedPreintegratedMeasurements actual1(bias, - Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), - Matrix3::Zero(), Matrix3::Zero(), Matrix::Zero(6,6)); + Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), + Matrix3::Zero(), Matrix::Zero(6, 6)); 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(Matrix(expected1.deltaRij()), Matrix(actual1.deltaRij()), tol)); + EXPECT( + assert_equal(Vector(expected1.deltaPij()), Vector(actual1.deltaPij()), + 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 ) -{ +TEST( CombinedImuFactor, ErrorWithBiases ) { 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)); + Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0)), Point3(5.0, 1.0, -50.0)); Vector3 v1(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 / 4.0 + M_PI / 10.0)), + Point3(5.5, 1.0, -50.0)); Vector3 v2(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); + 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; double tol = 1e-6; - Matrix I6x6(6,6); - I6x6 = Matrix::Identity(6,6); + 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)), + 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); 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 ); + 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); + 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 errorActual = Combinedfactor.evaluateError(x1, v1, x2, v2, bias, + bias2); EXPECT(assert_equal(errorExpected, errorActual.head(9), tol)); @@ -206,7 +219,8 @@ TEST( CombinedImuFactor, ErrorWithBiases ) // Actual Jacobians Matrix H1a, H2a, H3a, H4a, H5a, H6a; - (void) Combinedfactor.evaluateError(x1, v1, x2, v2, bias, bias2, H1a, H2a, H3a, H4a, H5a, H6a); + (void) Combinedfactor.evaluateError(x1, v1, x2, v2, bias, bias2, H1a, H2a, + H3a, H4a, H5a, H6a); EXPECT(assert_equal(H1e, H1a.topRows(9))); EXPECT(assert_equal(H2e, H2a.topRows(9))); @@ -216,101 +230,128 @@ TEST( CombinedImuFactor, ErrorWithBiases ) } /* ************************************************************************* */ -TEST( CombinedImuFactor, FirstOrderPreIntegratedMeasurements ) -{ +TEST( CombinedImuFactor, FirstOrderPreIntegratedMeasurements ) { // Linearization point imuBias::ConstantBias bias; ///< Current estimate of acceleration and rotation rate biases - Pose3 body_P_sensor(Rot3::Expmap(Vector3(0,0.1,0.1)), Point3(1, 0, 1)); + Pose3 body_P_sensor(Rot3::Expmap(Vector3(0, 0.1, 0.1)), Point3(1, 0, 1)); // Measurements list measuredAccs, measuredOmegas; list deltaTs; measuredAccs.push_back(Vector3(0.1, 0.0, 0.0)); - measuredOmegas.push_back(Vector3(M_PI/100.0, 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)); + measuredOmegas.push_back(Vector3(M_PI / 100.0, 0.0, 0.0)); deltaTs.push_back(0.01); - for(int i=1;i<100;i++) - { + 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)); + 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, measuredAccs, measuredOmegas, deltaTs); + evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, + deltaTs); // Compute numerical derivatives - Matrix expectedDelPdelBias = numericalDerivative11( - boost::bind(&evaluatePreintegratedMeasurementsPosition, _1, measuredAccs, measuredOmegas, deltaTs), bias); - Matrix expectedDelPdelBiasAcc = expectedDelPdelBias.leftCols(3); + Matrix expectedDelPdelBias = numericalDerivative11( + boost::bind(&evaluatePreintegratedMeasurementsPosition, _1, measuredAccs, + measuredOmegas, deltaTs), bias); + Matrix expectedDelPdelBiasAcc = expectedDelPdelBias.leftCols(3); Matrix expectedDelPdelBiasOmega = expectedDelPdelBias.rightCols(3); - Matrix expectedDelVdelBias = numericalDerivative11( - boost::bind(&evaluatePreintegratedMeasurementsVelocity, _1, measuredAccs, measuredOmegas, deltaTs), bias); - Matrix expectedDelVdelBiasAcc = expectedDelVdelBias.leftCols(3); + Matrix expectedDelVdelBias = numericalDerivative11( + boost::bind(&evaluatePreintegratedMeasurementsVelocity, _1, measuredAccs, + measuredOmegas, deltaTs), bias); + Matrix expectedDelVdelBiasAcc = expectedDelVdelBias.leftCols(3); Matrix expectedDelVdelBiasOmega = expectedDelVdelBias.rightCols(3); - Matrix expectedDelRdelBias = numericalDerivative11( - boost::bind(&evaluatePreintegratedMeasurementsRotation, _1, measuredAccs, measuredOmegas, deltaTs), bias); - Matrix expectedDelRdelBiasAcc = expectedDelRdelBias.leftCols(3); + Matrix expectedDelRdelBias = + numericalDerivative11( + boost::bind(&evaluatePreintegratedMeasurementsRotation, _1, + measuredAccs, measuredOmegas, deltaTs), bias); + Matrix expectedDelRdelBiasAcc = expectedDelRdelBias.leftCols(3); Matrix expectedDelRdelBiasOmega = expectedDelRdelBias.rightCols(3); // Compare Jacobians EXPECT(assert_equal(expectedDelPdelBiasAcc, preintegrated.delPdelBiasAcc())); - EXPECT(assert_equal(expectedDelPdelBiasOmega, preintegrated.delPdelBiasOmega())); + EXPECT( + assert_equal(expectedDelPdelBiasOmega, preintegrated.delPdelBiasOmega())); EXPECT(assert_equal(expectedDelVdelBiasAcc, preintegrated.delVdelBiasAcc())); - EXPECT(assert_equal(expectedDelVdelBiasOmega, preintegrated.delVdelBiasOmega())); - EXPECT(assert_equal(expectedDelRdelBiasAcc, Matrix::Zero(3,3))); - EXPECT(assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega(), 1e-3)); // 1e-3 needs to be added only when using quaternions for rotations + EXPECT( + assert_equal(expectedDelVdelBiasOmega, preintegrated.delVdelBiasOmega())); + EXPECT(assert_equal(expectedDelRdelBiasAcc, Matrix::Zero(3, 3))); + EXPECT( + assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega(), + 1e-3)); // 1e-3 needs to be added only when using quaternions for rotations } /* ************************************************************************* */ -TEST(CombinedImuFactor, PredictPositionAndVelocity){ +TEST(CombinedImuFactor, PredictPositionAndVelocity) { imuBias::ConstantBias bias(Vector3(0, 0.1, 0), Vector3(0, 0.1, 0)); // Biases (acc, rot) // Measurements - Vector3 gravity; gravity << 0, 0, 9.81; - Vector3 omegaCoriolis; omegaCoriolis << 0, 0, 0; - Vector3 measuredOmega; measuredOmega << 0, 0.1, 0;//M_PI/10.0+0.3; - Vector3 measuredAcc; measuredAcc << 0,1.1,-9.81; + Vector3 gravity; + gravity << 0, 0, 9.81; + Vector3 omegaCoriolis; + omegaCoriolis << 0, 0, 0; + Vector3 measuredOmega; + measuredOmega << 0, 0.1, 0; //M_PI/10.0+0.3; + Vector3 measuredAcc; + measuredAcc << 0, 1.1, -9.81; double deltaT = 0.001; - Matrix I6x6(6,6); - I6x6 = Matrix::Identity(6,6); + Matrix I6x6(6, 6); + I6x6 = Matrix::Identity(6, 6); CombinedImuFactor::CombinedPreintegratedMeasurements Combined_pre_int_data( - bias, Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), 2 * Matrix3::Identity(), I6x6, true ); + bias, Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), + Matrix3::Identity(), 2 * Matrix3::Identity(), I6x6, true); - for (int i = 0; i<1000; ++i) Combined_pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + 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); + 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); + 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; + Vector3 expectedVelocity; + expectedVelocity << 0, 1, 0; EXPECT(assert_equal(expectedPose, poseVelocityBias.pose)); - EXPECT(assert_equal(Vector(expectedVelocity), Vector(poseVelocityBias.velocity))); + 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); + Matrix I6x6(6, 6); CombinedImuFactor::CombinedPreintegratedMeasurements Combined_pre_int_data( - bias, Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), 2 * Matrix3::Identity(), I6x6, true ); - Vector3 measuredAcc; measuredAcc << 0, 0, -9.81; - Vector3 gravity; gravity<<0,0,9.81; - Vector3 omegaCoriolis; omegaCoriolis << 0,0,0; - Vector3 measuredOmega; measuredOmega << 0, 0, M_PI / 10.0; + bias, Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), + Matrix3::Identity(), 2 * Matrix3::Identity(), I6x6, true); + Vector3 measuredAcc; + measuredAcc << 0, 0, -9.81; + Vector3 gravity; + gravity << 0, 0, 9.81; + Vector3 omegaCoriolis; + omegaCoriolis << 0, 0, 0; + Vector3 measuredOmega; + measuredOmega << 0, 0, M_PI / 10.0; double deltaT = 0.001; double tol = 1e-4; for (int i = 0; i < 1000; ++i) @@ -320,16 +361,16 @@ TEST(CombinedImuFactor, PredictRotation) { Combined_pre_int_data, gravity, omegaCoriolis); // Predict - Pose3 x(Rot3().ypr(0,0, 0), Point3(0,0,0)), x2; - Vector3 v(0,0,0), v2; - CombinedImuFactor::Predict(x, v, x2, v2, bias, Combinedfactor.preintegratedMeasurements(), gravity, omegaCoriolis); - Pose3 expectedPose(Rot3().ypr(M_PI/10, 0,0), Point3(0,0,0)); + Pose3 x(Rot3().ypr(0, 0, 0), Point3(0, 0, 0)), x2; + Vector3 v(0, 0, 0), v2; + CombinedImuFactor::Predict(x, v, x2, v2, bias, + Combinedfactor.preintegratedMeasurements(), gravity, omegaCoriolis); + Pose3 expectedPose(Rot3().ypr(M_PI / 10, 0, 0), Point3(0, 0, 0)); EXPECT(assert_equal(expectedPose, x2, tol)); } /* ************************************************************************* */ -TEST( CombinedImuFactor, JacobianPreintegratedCovariancePropagation ) -{ +TEST( CombinedImuFactor, JacobianPreintegratedCovariancePropagation ) { // Linearization point imuBias::ConstantBias bias_old = imuBias::ConstantBias(); ///< Current estimate of acceleration and rotation rate biases Pose3 body_P_sensor = Pose3(); @@ -338,35 +379,36 @@ TEST( CombinedImuFactor, JacobianPreintegratedCovariancePropagation ) list measuredAccs, measuredOmegas; list deltaTs; measuredAccs.push_back(Vector3(0.1, 0.0, 0.0)); - measuredOmegas.push_back(Vector3(M_PI/100.0, 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)); + measuredOmegas.push_back(Vector3(M_PI / 100.0, 0.0, 0.0)); deltaTs.push_back(0.01); - for(int i=1;i<100;i++) - { + 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)); + 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); + 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 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 + 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); + preintegrated.integrateMeasurement(newMeasuredAcc, newMeasuredOmega, + newDeltaT, body_P_sensor, Factual, Gactual); bool use2ndOrderIntegration = false; @@ -374,44 +416,51 @@ TEST( CombinedImuFactor, JacobianPreintegratedCovariancePropagation ) // COMPUTE NUMERICAL DERIVATIVES FOR F ////////////////////////////////////////////////////////////////////////////////////////////// // Compute expected F wrt positions (15,3) - Matrix df_dpos = - numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsTest, - _1, deltaVij_old, deltaRij_old, bias_old, - newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaPij_old); + Matrix df_dpos = numericalDerivative11( + 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(boost::bind(&updatePreintegratedMeasurementsRot, - _1, deltaVij_old, deltaRij_old, bias_old, - newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaPij_old); + df_dpos.block<3, 3>(6, 0) = numericalDerivative11( + 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(boost::bind(&updatePreintegratedMeasurementsTest, - deltaPij_old, _1, deltaRij_old, bias_old, - newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaVij_old); + Matrix df_dvel = numericalDerivative11( + 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(boost::bind(&updatePreintegratedMeasurementsRot, - deltaPij_old, _1, deltaRij_old, bias_old, - newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaVij_old); + df_dvel.block<3, 3>(6, 0) = numericalDerivative11( + 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(boost::bind(&updatePreintegratedMeasurementsTest, - deltaPij_old, deltaVij_old, _1, bias_old, - newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaRij_old); + Matrix df_dangle = numericalDerivative11( + 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(boost::bind(&updatePreintegratedMeasurementsRot, - deltaPij_old, deltaVij_old, _1, bias_old, - newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaRij_old); + df_dangle.block<3, 3>(6, 0) = numericalDerivative11( + 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(boost::bind(&updatePreintegratedMeasurementsTest, - deltaPij_old, deltaVij_old, deltaRij_old, _1, - newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), bias_old); + Matrix df_dbias = numericalDerivative11( + 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(boost::bind(&updatePreintegratedMeasurementsRot, - deltaPij_old, deltaVij_old, deltaRij_old, _1, - newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), bias_old); + df_dbias.block<3, 6>(6, 0) = + numericalDerivative11( + boost::bind(&updatePreintegratedMeasurementsRot, deltaPij_old, + deltaVij_old, deltaRij_old, _1, newMeasuredAcc, newMeasuredOmega, + newDeltaT, use2ndOrderIntegration), bias_old); - Matrix Fexpected(15,15); + Matrix Fexpected(15, 15); Fexpected << df_dpos, df_dvel, df_dangle, df_dbias; EXPECT(assert_equal(Fexpected, Factual)); @@ -419,55 +468,65 @@ TEST( CombinedImuFactor, JacobianPreintegratedCovariancePropagation ) // COMPUTE NUMERICAL DERIVATIVES FOR G ////////////////////////////////////////////////////////////////////////////////////////////// // Compute expected G wrt integration noise - Matrix df_dintNoise(15,3); + 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(boost::bind(&updatePreintegratedMeasurementsTest, - deltaPij_old, deltaVij_old, deltaRij_old, bias_old, - _1, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), newMeasuredAcc); + Matrix df_daccNoise = numericalDerivative11( + 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(boost::bind(&updatePreintegratedMeasurementsRot, - deltaPij_old, deltaVij_old, deltaRij_old, bias_old, - _1, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), newMeasuredAcc); + df_daccNoise.block<3, 3>(6, 0) = numericalDerivative11( + 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(boost::bind(&updatePreintegratedMeasurementsTest, - deltaPij_old, deltaVij_old, deltaRij_old, bias_old, - newMeasuredAcc, _1, newDeltaT, use2ndOrderIntegration), newMeasuredOmega); + Matrix df_domegaNoise = numericalDerivative11( + 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); + df_domegaNoise.block<3, 3>(6, 0) = numericalDerivative11( + 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 + 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); + df_rwBias.block<6, 6>(9, 0) = eye(6); // Compute expected G wrt gyro noise (15,6) - Matrix df_dinitBias = numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsTest, - deltaPij_old, deltaVij_old, deltaRij_old, _1, - newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), bias_old); + Matrix df_dinitBias = numericalDerivative11( + 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(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 + df_dinitBias.block<3, 6>(6, 0) = numericalDerivative11( + 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); + 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 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); +} /* ************************************************************************* */ diff --git a/gtsam/navigation/tests/testImuBias.cpp b/gtsam/navigation/tests/testImuBias.cpp index fb857f901..4d5df3f05 100644 --- a/gtsam/navigation/tests/testImuBias.cpp +++ b/gtsam/navigation/tests/testImuBias.cpp @@ -25,13 +25,12 @@ Vector biasAcc1(Vector3(0.2, -0.1, 0)); Vector biasGyro1(Vector3(0.1, -0.3, -0.2)); imuBias::ConstantBias bias1(biasAcc1, biasGyro1); -Vector biasAcc2(Vector3(0.1,0.2,0.04)); +Vector biasAcc2(Vector3(0.1, 0.2, 0.04)); Vector biasGyro2(Vector3(-0.002, 0.005, 0.03)); imuBias::ConstantBias bias2(biasAcc2, biasGyro2); /* ************************************************************************* */ -TEST( ImuBias, Constructor) -{ +TEST( ImuBias, Constructor) { // Default Constructor imuBias::ConstantBias bias1; @@ -43,87 +42,90 @@ TEST( ImuBias, Constructor) } /* ************************************************************************* */ -TEST( ImuBias, inverse) -{ +TEST( ImuBias, inverse) { imuBias::ConstantBias biasActual = bias1.inverse(); - imuBias::ConstantBias biasExpected = imuBias::ConstantBias(-biasAcc1, -biasGyro1); + imuBias::ConstantBias biasExpected = imuBias::ConstantBias(-biasAcc1, + -biasGyro1); EXPECT(assert_equal(biasExpected, biasActual)); } /* ************************************************************************* */ -TEST( ImuBias, compose) -{ - imuBias::ConstantBias biasActual = bias2.compose(bias1); - imuBias::ConstantBias biasExpected = imuBias::ConstantBias(biasAcc1+biasAcc2, biasGyro1+biasGyro2); - EXPECT(assert_equal(biasExpected, biasActual)); +TEST( ImuBias, compose) { + imuBias::ConstantBias biasActual = bias2.compose(bias1); + imuBias::ConstantBias biasExpected = imuBias::ConstantBias( + biasAcc1 + biasAcc2, biasGyro1 + biasGyro2); + EXPECT(assert_equal(biasExpected, biasActual)); } /* ************************************************************************* */ -TEST( ImuBias, between) -{ - // p.between(q) == q - p - imuBias::ConstantBias biasActual = bias2.between(bias1); - imuBias::ConstantBias biasExpected = imuBias::ConstantBias(biasAcc1-biasAcc2, biasGyro1-biasGyro2); - EXPECT(assert_equal(biasExpected, biasActual)); +TEST( ImuBias, between) { + // p.between(q) == q - p + imuBias::ConstantBias biasActual = bias2.between(bias1); + imuBias::ConstantBias biasExpected = imuBias::ConstantBias( + biasAcc1 - biasAcc2, biasGyro1 - biasGyro2); + EXPECT(assert_equal(biasExpected, biasActual)); } /* ************************************************************************* */ -TEST( ImuBias, localCoordinates) -{ - Vector deltaActual = Vector(bias2.localCoordinates(bias1)); - Vector deltaExpected = (imuBias::ConstantBias(biasAcc1-biasAcc2, biasGyro1-biasGyro2)).vector(); - EXPECT(assert_equal(deltaExpected, deltaActual)); +TEST( ImuBias, localCoordinates) { + Vector deltaActual = Vector(bias2.localCoordinates(bias1)); + Vector deltaExpected = (imuBias::ConstantBias(biasAcc1 - biasAcc2, + biasGyro1 - biasGyro2)).vector(); + EXPECT(assert_equal(deltaExpected, deltaActual)); } /* ************************************************************************* */ -TEST( ImuBias, retract) -{ - Vector6 delta; delta << 0.1, 0.2, -0.3, 0.1, -0.1, 0.2; - imuBias::ConstantBias biasActual = bias2.retract(delta); - imuBias::ConstantBias biasExpected = imuBias::ConstantBias(biasAcc2+delta.block<3,1>(0,0), biasGyro2+delta.block<3,1>(3,0)); - EXPECT(assert_equal(biasExpected, biasActual)); +TEST( ImuBias, retract) { + Vector6 delta; + delta << 0.1, 0.2, -0.3, 0.1, -0.1, 0.2; + imuBias::ConstantBias biasActual = bias2.retract(delta); + imuBias::ConstantBias biasExpected = imuBias::ConstantBias( + biasAcc2 + delta.block<3, 1>(0, 0), biasGyro2 + delta.block<3, 1>(3, 0)); + EXPECT(assert_equal(biasExpected, biasActual)); } /* ************************************************************************* */ -TEST( ImuBias, Logmap) -{ - Vector deltaActual = bias2.Logmap(bias1); - Vector deltaExpected = bias1.vector(); - EXPECT(assert_equal(deltaExpected, deltaActual)); +TEST( ImuBias, Logmap) { + Vector deltaActual = bias2.Logmap(bias1); + Vector deltaExpected = bias1.vector(); + EXPECT(assert_equal(deltaExpected, deltaActual)); } /* ************************************************************************* */ -TEST( ImuBias, Expmap) -{ - Vector6 delta; delta << 0.1, 0.2, -0.3, 0.1, -0.1, 0.2; - imuBias::ConstantBias biasActual = bias2.Expmap(delta); - imuBias::ConstantBias biasExpected = imuBias::ConstantBias(delta); - EXPECT(assert_equal(biasExpected, biasActual)); +TEST( ImuBias, Expmap) { + Vector6 delta; + delta << 0.1, 0.2, -0.3, 0.1, -0.1, 0.2; + imuBias::ConstantBias biasActual = bias2.Expmap(delta); + imuBias::ConstantBias biasExpected = imuBias::ConstantBias(delta); + EXPECT(assert_equal(biasExpected, biasActual)); } /* ************************************************************************* */ -TEST( ImuBias, operatorSub) -{ - imuBias::ConstantBias biasActual = -bias1; - imuBias::ConstantBias biasExpected(-biasAcc1, -biasGyro1); - EXPECT(assert_equal(biasExpected, biasActual)); +TEST( ImuBias, operatorSub) { + imuBias::ConstantBias biasActual = -bias1; + imuBias::ConstantBias biasExpected(-biasAcc1, -biasGyro1); + EXPECT(assert_equal(biasExpected, biasActual)); } /* ************************************************************************* */ -TEST( ImuBias, operatorAdd) -{ - imuBias::ConstantBias biasActual = bias2 + bias1; - imuBias::ConstantBias biasExpected(biasAcc2 + biasAcc1, biasGyro2 + biasGyro1); - EXPECT(assert_equal(biasExpected, biasActual)); +TEST( ImuBias, operatorAdd) { + imuBias::ConstantBias biasActual = bias2 + bias1; + imuBias::ConstantBias biasExpected(biasAcc2 + biasAcc1, + biasGyro2 + biasGyro1); + EXPECT(assert_equal(biasExpected, biasActual)); } /* ************************************************************************* */ -TEST( ImuBias, operatorSubB) -{ - imuBias::ConstantBias biasActual = bias2 - bias1; - imuBias::ConstantBias biasExpected(biasAcc2 - biasAcc1, biasGyro2 - biasGyro1); - EXPECT(assert_equal(biasExpected, biasActual));} +TEST( ImuBias, operatorSubB) { + imuBias::ConstantBias biasActual = bias2 - bias1; + imuBias::ConstantBias biasExpected(biasAcc2 - biasAcc1, + biasGyro2 - biasGyro1); + EXPECT(assert_equal(biasExpected, biasActual)); +} /* ************************************************************************* */ - int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} /* ************************************************************************* */ diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index 62f496894..b8fc8062c 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -39,37 +39,39 @@ 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){ +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) { return factor.evaluateError(pose_i, vel_i, pose_j, vel_j, bias); } -Rot3 evaluateRotationError(const ImuFactor& factor, - const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j, - const imuBias::ConstantBias& bias){ - return Rot3::Expmap(factor.evaluateError(pose_i, vel_i, pose_j, vel_j, bias).tail(3) ) ; +Rot3 evaluateRotationError(const ImuFactor& factor, const Pose3& pose_i, + const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j, + const imuBias::ConstantBias& bias) { + 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_) { +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_){ + if (!use2ndOrderIntegration_) { deltaPij_new = deltaPij_old + deltaVij_old * deltaT; - }else{ + } 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; + Vector result(6); + result << deltaPij_new, deltaVij_new; return result; } @@ -86,131 +88,139 @@ double accNoiseVar = 0.01; double omegaNoiseVar = 0.03; double intNoiseVar = 0.0001; ImuFactor::PreintegratedMeasurements evaluatePreintegratedMeasurements( - const imuBias::ConstantBias& bias, - const list& measuredAccs, - const list& measuredOmegas, - const list& deltaTs, - const bool use2ndOrderIntegration = false){ - ImuFactor::PreintegratedMeasurements result(bias, accNoiseVar * Matrix3::Identity(), - omegaNoiseVar *Matrix3::Identity(), intNoiseVar * Matrix3::Identity(),use2ndOrderIntegration); + const imuBias::ConstantBias& bias, const list& measuredAccs, + const list& measuredOmegas, const list& deltaTs, + const bool use2ndOrderIntegration = false) { + ImuFactor::PreintegratedMeasurements result(bias, + accNoiseVar * Matrix3::Identity(), omegaNoiseVar * Matrix3::Identity(), + intNoiseVar * Matrix3::Identity(), use2ndOrderIntegration); list::const_iterator itAcc = measuredAccs.begin(); list::const_iterator itOmega = measuredOmegas.begin(); list::const_iterator itDeltaT = deltaTs.begin(); - for( ; itAcc != measuredAccs.end(); ++itAcc, ++itOmega, ++itDeltaT) { + for (; itAcc != measuredAccs.end(); ++itAcc, ++itOmega, ++itDeltaT) { result.integrateMeasurement(*itAcc, *itOmega, *itDeltaT); } return result; } Vector3 evaluatePreintegratedMeasurementsPosition( - const imuBias::ConstantBias& bias, - const list& measuredAccs, - const list& measuredOmegas, - const list& deltaTs){ - return evaluatePreintegratedMeasurements(bias, - measuredAccs, measuredOmegas, deltaTs).deltaPij(); + const imuBias::ConstantBias& bias, const list& measuredAccs, + const list& measuredOmegas, const list& deltaTs) { + return evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, + deltaTs).deltaPij(); } Vector3 evaluatePreintegratedMeasurementsVelocity( - const imuBias::ConstantBias& bias, - const list& measuredAccs, - const list& measuredOmegas, - const list& deltaTs) -{ - return evaluatePreintegratedMeasurements(bias, - measuredAccs, measuredOmegas, deltaTs).deltaVij(); + const imuBias::ConstantBias& bias, const list& measuredAccs, + const list& measuredOmegas, const list& deltaTs) { + return evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, + deltaTs).deltaVij(); } Rot3 evaluatePreintegratedMeasurementsRotation( - const imuBias::ConstantBias& bias, - const list& measuredAccs, - const list& measuredOmegas, - const list& deltaTs){ - return Rot3(evaluatePreintegratedMeasurements(bias, - measuredAccs, measuredOmegas, deltaTs).deltaRij()); + const imuBias::ConstantBias& bias, const list& measuredAccs, + const list& measuredOmegas, const list& deltaTs) { + return Rot3( + evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, + deltaTs).deltaRij()); } -Rot3 evaluateRotation(const Vector3 measuredOmega, const Vector3 biasOmega, const double deltaT){ +Rot3 evaluateRotation(const Vector3 measuredOmega, const Vector3 biasOmega, + const double deltaT) { return Rot3::Expmap((measuredOmega - biasOmega) * deltaT); } -Vector3 evaluateLogRotation(const Vector3 thetahat, const Vector3 deltatheta){ - return Rot3::Logmap( Rot3::Expmap(thetahat).compose( Rot3::Expmap(deltatheta) ) ); +Vector3 evaluateLogRotation(const Vector3 thetahat, const Vector3 deltatheta) { + return Rot3::Logmap(Rot3::Expmap(thetahat).compose(Rot3::Expmap(deltatheta))); } } /* ************************************************************************* */ -TEST( ImuFactor, PreintegratedMeasurements ) -{ +TEST( ImuFactor, PreintegratedMeasurements ) { // Linearization point - imuBias::ConstantBias bias(Vector3(0,0,0), Vector3(0,0,0)); ///< Current estimate of acceleration and angular rate biases + imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); ///< Current estimate of acceleration and angular rate biases // Measurements Vector3 measuredAcc(0.1, 0.0, 0.0); - Vector3 measuredOmega(M_PI/100.0, 0.0, 0.0); + Vector3 measuredOmega(M_PI / 100.0, 0.0, 0.0); double deltaT = 0.5; // Expected preintegrated values - Vector3 expectedDeltaP1; expectedDeltaP1 << 0.5*0.1*0.5*0.5, 0, 0; + Vector3 expectedDeltaP1; + expectedDeltaP1 << 0.5 * 0.1 * 0.5 * 0.5, 0, 0; Vector3 expectedDeltaV1(0.05, 0.0, 0.0); - Rot3 expectedDeltaR1 = Rot3::RzRyRx(0.5 * M_PI/100.0, 0.0, 0.0); + Rot3 expectedDeltaR1 = Rot3::RzRyRx(0.5 * M_PI / 100.0, 0.0, 0.0); double expectedDeltaT1(0.5); bool use2ndOrderIntegration = true; // Actual preintegrated values - ImuFactor::PreintegratedMeasurements actual1(bias, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), use2ndOrderIntegration); + ImuFactor::PreintegratedMeasurements actual1(bias, Matrix3::Zero(), + Matrix3::Zero(), Matrix3::Zero(), use2ndOrderIntegration); actual1.integrateMeasurement(measuredAcc, measuredOmega, deltaT); - EXPECT(assert_equal(Vector(expectedDeltaP1), Vector(actual1.deltaPij()), 1e-6)); - EXPECT(assert_equal(Vector(expectedDeltaV1), Vector(actual1.deltaVij()), 1e-6)); + EXPECT( + assert_equal(Vector(expectedDeltaP1), Vector(actual1.deltaPij()), 1e-6)); + EXPECT( + assert_equal(Vector(expectedDeltaV1), Vector(actual1.deltaVij()), 1e-6)); EXPECT(assert_equal(expectedDeltaR1, Rot3(actual1.deltaRij()), 1e-6)); DOUBLES_EQUAL(expectedDeltaT1, actual1.deltaTij(), 1e-6); // Integrate again - Vector3 expectedDeltaP2; expectedDeltaP2 << 0.025 + expectedDeltaP1(0) + 0.5*0.1*0.5*0.5, 0, 0; - Vector3 expectedDeltaV2 = Vector3(0.05, 0.0, 0.0) + expectedDeltaR1.matrix() * measuredAcc * 0.5; - Rot3 expectedDeltaR2 = Rot3::RzRyRx(2.0 * 0.5 * M_PI/100.0, 0.0, 0.0); + Vector3 expectedDeltaP2; + expectedDeltaP2 << 0.025 + expectedDeltaP1(0) + 0.5 * 0.1 * 0.5 * 0.5, 0, 0; + Vector3 expectedDeltaV2 = Vector3(0.05, 0.0, 0.0) + + expectedDeltaR1.matrix() * measuredAcc * 0.5; + Rot3 expectedDeltaR2 = Rot3::RzRyRx(2.0 * 0.5 * M_PI / 100.0, 0.0, 0.0); double expectedDeltaT2(1); // Actual preintegrated values ImuFactor::PreintegratedMeasurements actual2 = actual1; actual2.integrateMeasurement(measuredAcc, measuredOmega, deltaT); - EXPECT(assert_equal(Vector(expectedDeltaP2), Vector(actual2.deltaPij()), 1e-6)); - EXPECT(assert_equal(Vector(expectedDeltaV2), Vector(actual2.deltaVij()), 1e-6)); + EXPECT( + assert_equal(Vector(expectedDeltaP2), Vector(actual2.deltaPij()), 1e-6)); + EXPECT( + assert_equal(Vector(expectedDeltaV2), Vector(actual2.deltaVij()), 1e-6)); EXPECT(assert_equal(expectedDeltaR2, Rot3(actual2.deltaRij()), 1e-6)); DOUBLES_EQUAL(expectedDeltaT2, actual2.deltaTij(), 1e-6); } /* ************************************************************************* */ -TEST( ImuFactor, ErrorAndJacobians ) -{ +TEST( ImuFactor, ErrorAndJacobians ) { // Linearization point imuBias::ConstantBias bias; // Bias - Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/4.0), Point3(5.0, 1.0, -50.0)); + 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/100.0, M_PI/6.0, M_PI/4.0), Point3(5.5, 1.0, -50.0)); + Pose3 x2(Rot3::RzRyRx(M_PI / 12.0 + M_PI / 100.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)); // Measurements - Vector3 gravity; gravity << 0, 0, 9.81; - Vector3 omegaCoriolis; omegaCoriolis << 0, 0, 0; - Vector3 measuredOmega; measuredOmega << M_PI/100, 0, 0; + Vector3 gravity; + gravity << 0, 0, 9.81; + Vector3 omegaCoriolis; + omegaCoriolis << 0, 0, 0; + Vector3 measuredOmega; + measuredOmega << M_PI / 100, 0, 0; Vector3 measuredAcc = x1.rotation().unrotate(-Point3(gravity)).vector(); double deltaT = 1.0; bool use2ndOrderIntegration = true; - ImuFactor::PreintegratedMeasurements pre_int_data(bias, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), use2ndOrderIntegration); + ImuFactor::PreintegratedMeasurements pre_int_data(bias, Matrix3::Zero(), + Matrix3::Zero(), Matrix3::Zero(), use2ndOrderIntegration); 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); + 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); // Expected error - Vector errorExpected(9); errorExpected << 0, 0, 0, 0, 0, 0, 0, 0, 0; + Vector errorExpected(9); + errorExpected << 0, 0, 0, 0, 0, 0, 0, 0, 0; EXPECT(assert_equal(errorExpected, errorActual, 1e-6)); // Actual Jacobians @@ -219,61 +229,68 @@ TEST( ImuFactor, ErrorAndJacobians ) // Expected Jacobians /////////////////// H1 /////////////////////////// - Matrix H1e = numericalDerivative11( + Matrix H1e = numericalDerivative11( 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( + Matrix H1Rot3 = numericalDerivative11( 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( + Matrix H2e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, _1, x2, v2, bias), v1); EXPECT(assert_equal(H2e, H2a)); /////////////////// H3 /////////////////////////// - Matrix H3e = numericalDerivative11( + Matrix H3e = numericalDerivative11( 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( + Matrix H3Rot3 = numericalDerivative11( 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( + Matrix H4e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, v1, x2, _1, bias), v2); EXPECT(assert_equal(H4e, H4a)); /////////////////// H5 /////////////////////////// - Matrix H5e = numericalDerivative11( + Matrix H5e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, v1, x2, v2, _1), bias); EXPECT(assert_equal(H5e, H5a)); } /* ************************************************************************* */ -TEST( ImuFactor, ErrorAndJacobianWithBiases ) -{ +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)); + 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)); + 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); + 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()); + 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); + 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); @@ -285,23 +302,23 @@ TEST( ImuFactor, ErrorAndJacobianWithBiases ) // EXPECT(assert_equal(errorExpected, errorActual, 1e-6)); // Expected Jacobians - Matrix H1e = numericalDerivative11( + Matrix H1e = numericalDerivative11( boost::bind(&callEvaluateError, factor, _1, v1, x2, v2, bias), x1); - Matrix H2e = numericalDerivative11( + Matrix H2e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, _1, x2, v2, bias), v1); - Matrix H3e = numericalDerivative11( + Matrix H3e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, v1, _1, v2, bias), x2); - Matrix H4e = numericalDerivative11( + Matrix H4e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, v1, x2, _1, bias), v2); - Matrix H5e = numericalDerivative11( + Matrix H5e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, v1, x2, v2, _1), bias); // Check rotation Jacobians - Matrix RH1e = numericalDerivative11( + Matrix RH1e = numericalDerivative11( boost::bind(&evaluateRotationError, factor, _1, v1, x2, v2, bias), x1); - Matrix RH3e = numericalDerivative11( + Matrix RH3e = numericalDerivative11( boost::bind(&evaluateRotationError, factor, x1, v1, _1, v2, bias), x2); - Matrix RH5e = numericalDerivative11( + Matrix RH5e = numericalDerivative11( boost::bind(&evaluateRotationError, factor, x1, v1, x2, v2, _1), bias); // Actual Jacobians @@ -316,29 +333,36 @@ TEST( ImuFactor, ErrorAndJacobianWithBiases ) } /* ************************************************************************* */ -TEST( ImuFactor, ErrorAndJacobianWith2ndOrderCoriolis ) -{ +TEST( ImuFactor, ErrorAndJacobianWith2ndOrderCoriolis ) { 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)); + 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)); + 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); + 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()); + 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 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); + ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, + omegaCoriolis, bodyPsensor, use2ndOrderCoriolis); SETDEBUG("ImuFactor evaluateError", false); Vector errorActual = factor.evaluateError(x1, v1, x2, v2, bias); @@ -350,23 +374,23 @@ TEST( ImuFactor, ErrorAndJacobianWith2ndOrderCoriolis ) // EXPECT(assert_equal(errorExpected, errorActual, 1e-6)); // Expected Jacobians - Matrix H1e = numericalDerivative11( + Matrix H1e = numericalDerivative11( boost::bind(&callEvaluateError, factor, _1, v1, x2, v2, bias), x1); - Matrix H2e = numericalDerivative11( + Matrix H2e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, _1, x2, v2, bias), v1); - Matrix H3e = numericalDerivative11( + Matrix H3e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, v1, _1, v2, bias), x2); - Matrix H4e = numericalDerivative11( + Matrix H4e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, v1, x2, _1, bias), v2); - Matrix H5e = numericalDerivative11( + Matrix H5e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, v1, x2, v2, _1), bias); // Check rotation Jacobians - Matrix RH1e = numericalDerivative11( + Matrix RH1e = numericalDerivative11( boost::bind(&evaluateRotationError, factor, _1, v1, x2, v2, bias), x1); - Matrix RH3e = numericalDerivative11( + Matrix RH3e = numericalDerivative11( boost::bind(&evaluateRotationError, factor, x1, v1, _1, v2, bias), x2); - Matrix RH5e = numericalDerivative11( + Matrix RH5e = numericalDerivative11( boost::bind(&evaluateRotationError, factor, x1, v1, x2, v2, _1), bias); // Actual Jacobians @@ -381,39 +405,43 @@ TEST( ImuFactor, ErrorAndJacobianWith2ndOrderCoriolis ) } /* ************************************************************************* */ -TEST( ImuFactor, PartialDerivative_wrt_Bias ) -{ +TEST( ImuFactor, PartialDerivative_wrt_Bias ) { // Linearization point - Vector3 biasOmega; biasOmega << 0,0,0; ///< Current estimate of rotation rate bias + Vector3 biasOmega; + biasOmega << 0, 0, 0; ///< Current estimate of rotation rate bias // Measurements - Vector3 measuredOmega; measuredOmega << 0.1, 0, 0; + Vector3 measuredOmega; + measuredOmega << 0.1, 0, 0; double deltaT = 0.5; // Compute numerical derivatives - Matrix expectedDelRdelBiasOmega = numericalDerivative11(boost::bind( - &evaluateRotation, measuredOmega, _1, deltaT), Vector3(biasOmega)); + Matrix expectedDelRdelBiasOmega = numericalDerivative11( + boost::bind(&evaluateRotation, measuredOmega, _1, deltaT), + Vector3(biasOmega)); - const Matrix3 Jr = Rot3::ExpmapDerivative((measuredOmega - biasOmega) * deltaT); + const Matrix3 Jr = Rot3::ExpmapDerivative( + (measuredOmega - biasOmega) * deltaT); - Matrix3 actualdelRdelBiasOmega = - Jr * deltaT; // the delta bias appears with the minus sign + Matrix3 actualdelRdelBiasOmega = -Jr * deltaT; // the delta bias appears with the minus sign // Compare Jacobians - EXPECT(assert_equal(expectedDelRdelBiasOmega, actualdelRdelBiasOmega, 1e-3)); // 1e-3 needs to be added only when using quaternions for rotations + EXPECT(assert_equal(expectedDelRdelBiasOmega, actualdelRdelBiasOmega, 1e-3)); // 1e-3 needs to be added only when using quaternions for rotations } /* ************************************************************************* */ -TEST( ImuFactor, PartialDerivativeLogmap ) -{ +TEST( ImuFactor, PartialDerivativeLogmap ) { // Linearization point - Vector3 thetahat; thetahat << 0.1,0.1,0; ///< Current estimate of rotation rate bias + Vector3 thetahat; + thetahat << 0.1, 0.1, 0; ///< Current estimate of rotation rate bias // Measurements - Vector3 deltatheta; deltatheta << 0, 0, 0; + Vector3 deltatheta; + deltatheta << 0, 0, 0; // Compute numerical derivatives - Matrix expectedDelFdeltheta = numericalDerivative11(boost::bind( - &evaluateLogRotation, thetahat, _1), Vector3(deltatheta)); + Matrix expectedDelFdeltheta = numericalDerivative11( + boost::bind(&evaluateLogRotation, thetahat, _1), Vector3(deltatheta)); Matrix3 actualDelFdeltheta = Rot3::LogmapDerivative(thetahat); @@ -422,28 +450,33 @@ TEST( ImuFactor, PartialDerivativeLogmap ) } /* ************************************************************************* */ -TEST( ImuFactor, fistOrderExponential ) -{ +TEST( ImuFactor, fistOrderExponential ) { // Linearization point - Vector3 biasOmega; biasOmega << 0,0,0; ///< Current estimate of rotation rate bias + Vector3 biasOmega; + biasOmega << 0, 0, 0; ///< Current estimate of rotation rate bias // Measurements - Vector3 measuredOmega; measuredOmega << 0.1, 0, 0; + Vector3 measuredOmega; + measuredOmega << 0.1, 0, 0; double deltaT = 1.0; // change w.r.t. linearization point double alpha = 0.0; - Vector3 deltabiasOmega; deltabiasOmega << alpha,alpha,alpha; + Vector3 deltabiasOmega; + deltabiasOmega << alpha, alpha, alpha; - const Matrix3 Jr = Rot3::ExpmapDerivative((measuredOmega - biasOmega) * deltaT); + const Matrix3 Jr = Rot3::ExpmapDerivative( + (measuredOmega - biasOmega) * deltaT); - Matrix3 delRdelBiasOmega = - Jr * deltaT; // the delta bias appears with the minus sign + Matrix3 delRdelBiasOmega = -Jr * deltaT; // the delta bias appears with the minus sign - const Matrix expectedRot = Rot3::Expmap((measuredOmega - biasOmega - deltabiasOmega) * deltaT).matrix(); + const Matrix expectedRot = Rot3::Expmap( + (measuredOmega - biasOmega - deltabiasOmega) * deltaT).matrix(); - const Matrix3 hatRot = Rot3::Expmap((measuredOmega - biasOmega) * deltaT).matrix(); - const Matrix3 actualRot = - hatRot * Rot3::Expmap(delRdelBiasOmega * deltabiasOmega).matrix(); + const Matrix3 hatRot = + Rot3::Expmap((measuredOmega - biasOmega) * deltaT).matrix(); + const Matrix3 actualRot = hatRot + * Rot3::Expmap(delRdelBiasOmega * deltabiasOmega).matrix(); //hatRot * (Matrix3::Identity() + skewSymmetric(delRdelBiasOmega * deltabiasOmega)); // This is a first order expansion so the equality is only an approximation @@ -451,61 +484,70 @@ TEST( ImuFactor, fistOrderExponential ) } /* ************************************************************************* */ -TEST( ImuFactor, FirstOrderPreIntegratedMeasurements ) -{ +TEST( ImuFactor, FirstOrderPreIntegratedMeasurements ) { // Linearization point imuBias::ConstantBias bias; ///< Current estimate of acceleration and rotation rate biases - Pose3 body_P_sensor(Rot3::Expmap(Vector3(0,0.1,0.1)), Point3(1, 0, 1)); + Pose3 body_P_sensor(Rot3::Expmap(Vector3(0, 0.1, 0.1)), Point3(1, 0, 1)); // Measurements list measuredAccs, measuredOmegas; list deltaTs; measuredAccs.push_back(Vector3(0.1, 0.0, 0.0)); - measuredOmegas.push_back(Vector3(M_PI/100.0, 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)); + measuredOmegas.push_back(Vector3(M_PI / 100.0, 0.0, 0.0)); deltaTs.push_back(0.01); - for(int i=1;i<100;i++) - { + 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)); + 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); + evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, + deltaTs); // Compute numerical derivatives - Matrix expectedDelPdelBias = numericalDerivative11( - boost::bind(&evaluatePreintegratedMeasurementsPosition, _1, measuredAccs, measuredOmegas, deltaTs), bias); - Matrix expectedDelPdelBiasAcc = expectedDelPdelBias.leftCols(3); + Matrix expectedDelPdelBias = numericalDerivative11( + boost::bind(&evaluatePreintegratedMeasurementsPosition, _1, measuredAccs, + measuredOmegas, deltaTs), bias); + Matrix expectedDelPdelBiasAcc = expectedDelPdelBias.leftCols(3); Matrix expectedDelPdelBiasOmega = expectedDelPdelBias.rightCols(3); - Matrix expectedDelVdelBias = numericalDerivative11( - boost::bind(&evaluatePreintegratedMeasurementsVelocity, _1, measuredAccs, measuredOmegas, deltaTs), bias); - Matrix expectedDelVdelBiasAcc = expectedDelVdelBias.leftCols(3); + Matrix expectedDelVdelBias = numericalDerivative11( + boost::bind(&evaluatePreintegratedMeasurementsVelocity, _1, measuredAccs, + measuredOmegas, deltaTs), bias); + Matrix expectedDelVdelBiasAcc = expectedDelVdelBias.leftCols(3); Matrix expectedDelVdelBiasOmega = expectedDelVdelBias.rightCols(3); - Matrix expectedDelRdelBias = numericalDerivative11( - boost::bind(&evaluatePreintegratedMeasurementsRotation, _1, measuredAccs, measuredOmegas, deltaTs), bias); - Matrix expectedDelRdelBiasAcc = expectedDelRdelBias.leftCols(3); + Matrix expectedDelRdelBias = + numericalDerivative11( + boost::bind(&evaluatePreintegratedMeasurementsRotation, _1, + measuredAccs, measuredOmegas, deltaTs), bias); + Matrix expectedDelRdelBiasAcc = expectedDelRdelBias.leftCols(3); Matrix expectedDelRdelBiasOmega = expectedDelRdelBias.rightCols(3); // Compare Jacobians EXPECT(assert_equal(expectedDelPdelBiasAcc, preintegrated.delPdelBiasAcc())); - EXPECT(assert_equal(expectedDelPdelBiasOmega, preintegrated.delPdelBiasOmega())); + EXPECT( + assert_equal(expectedDelPdelBiasOmega, preintegrated.delPdelBiasOmega())); EXPECT(assert_equal(expectedDelVdelBiasAcc, preintegrated.delVdelBiasAcc())); - EXPECT(assert_equal(expectedDelVdelBiasOmega, preintegrated.delVdelBiasOmega())); - EXPECT(assert_equal(expectedDelRdelBiasAcc, Matrix::Zero(3,3))); - EXPECT(assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega(), 1e-3)); // 1e-3 needs to be added only when using quaternions for rotations + EXPECT( + assert_equal(expectedDelVdelBiasOmega, preintegrated.delVdelBiasOmega())); + EXPECT(assert_equal(expectedDelRdelBiasAcc, Matrix::Zero(3, 3))); + EXPECT( + assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega(), + 1e-3)); // 1e-3 needs to be added only when using quaternions for rotations } /* ************************************************************************* */ -TEST( ImuFactor, JacobianPreintegratedCovariancePropagation ) -{ +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)); @@ -514,68 +556,71 @@ TEST( ImuFactor, JacobianPreintegratedCovariancePropagation ) list measuredAccs, measuredOmegas; list deltaTs; measuredAccs.push_back(Vector3(0.1, 0.0, 0.0)); - measuredOmegas.push_back(Vector3(M_PI/100.0, 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)); + measuredOmegas.push_back(Vector3(M_PI / 100.0, 0.0, 0.0)); deltaTs.push_back(0.01); - for(int i=1;i<100;i++) - { + 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)); + measuredOmegas.push_back( + Vector3(M_PI / 100.0, M_PI / 300.0, 2 * M_PI / 100.0)); deltaTs.push_back(0.01); } bool use2ndOrderIntegration = false; // Actual preintegrated values ImuFactor::PreintegratedMeasurements preintegrated = - evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs, use2ndOrderIntegration); + evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, + deltaTs, use2ndOrderIntegration); // 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 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 + 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); + preintegrated.integrateMeasurement(newMeasuredAcc, newMeasuredOmega, + newDeltaT, body_P_sensor, Factual, Gactual); ////////////////////////////////////////////////////////////////////////////////////////////// // COMPUTE NUMERICAL DERIVATIVES FOR F ////////////////////////////////////////////////////////////////////////////////////////////// // Compute expected f_pos_vel wrt positions - Matrix dfpv_dpos = - numericalDerivative11(boost::bind(&updatePreintegratedPosVel, - _1, deltaVij_old, deltaRij_old, - newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaPij_old); + Matrix dfpv_dpos = numericalDerivative11( + 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(boost::bind(&updatePreintegratedPosVel, - deltaPij_old, _1, deltaRij_old, - newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaVij_old); + Matrix dfpv_dvel = numericalDerivative11( + 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(boost::bind(&updatePreintegratedPosVel, - deltaPij_old, deltaVij_old, _1, - newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaRij_old); + Matrix dfpv_dangle = numericalDerivative11( + 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; + Matrix FexpectedTop6(6, 9); + FexpectedTop6 << dfpv_dpos, dfpv_dvel, dfpv_dangle; // Compute expected f_rot wrt angles - Matrix dfr_dangle = - numericalDerivative11(boost::bind(&updatePreintegratedRot, - _1, newMeasuredOmega, newDeltaT), deltaRij_old); + Matrix dfr_dangle = numericalDerivative11( + boost::bind(&updatePreintegratedRot, _1, newMeasuredOmega, newDeltaT), + deltaRij_old); - Matrix FexpectedBottom3(3,9); + Matrix FexpectedBottom3(3, 9); FexpectedBottom3 << Z_3x3, Z_3x3, dfr_dangle; - Matrix Fexpected(9,9); Fexpected << FexpectedTop6, FexpectedBottom3; + Matrix Fexpected(9, 9); + Fexpected << FexpectedTop6, FexpectedBottom3; EXPECT(assert_equal(Fexpected, Factual)); @@ -583,50 +628,50 @@ TEST( ImuFactor, JacobianPreintegratedCovariancePropagation ) // COMPUTE NUMERICAL DERIVATIVES FOR G ////////////////////////////////////////////////////////////////////////////////////////////// // Compute jacobian wrt integration noise - Matrix dgpv_dintNoise(6,3); + Matrix dgpv_dintNoise(6, 3); dgpv_dintNoise << I_3x3 * newDeltaT, Z_3x3; // Compute jacobian wrt acc noise - Matrix dgpv_daccNoise = - numericalDerivative11(boost::bind(&updatePreintegratedPosVel, - deltaPij_old, deltaVij_old, deltaRij_old, - _1, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), newMeasuredAcc); + Matrix dgpv_daccNoise = numericalDerivative11( + boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old, + deltaRij_old, _1, newMeasuredOmega, newDeltaT, + use2ndOrderIntegration), newMeasuredAcc); // Compute expected F wrt gyro noise - Matrix dgpv_domegaNoise = - numericalDerivative11(boost::bind(&updatePreintegratedPosVel, - deltaPij_old, deltaVij_old, deltaRij_old, - newMeasuredAcc, _1, newDeltaT, use2ndOrderIntegration), newMeasuredOmega); - Matrix GexpectedTop6(6,9); + Matrix dgpv_domegaNoise = numericalDerivative11( + 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(boost::bind(&updatePreintegratedRot, - deltaRij_old, _1, newDeltaT), newMeasuredOmega); + Matrix dgr_dangle = numericalDerivative11( + boost::bind(&updatePreintegratedRot, deltaRij_old, _1, newDeltaT), + newMeasuredOmega); - Matrix GexpectedBottom3(3,9); + Matrix GexpectedBottom3(3, 9); GexpectedBottom3 << Z_3x3, Z_3x3, dgr_dangle; - Matrix Gexpected(9,9); Gexpected << GexpectedTop6, GexpectedBottom3; + 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; + 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 newPreintCovarianceExpected = Factual * oldPreintCovariance + * Factual.transpose() + + (1 / newDeltaT) * Gactual * measurementCovariance * Gactual.transpose(); Matrix newPreintCovarianceActual = preintegrated.preintMeasCov(); EXPECT(assert_equal(newPreintCovarianceExpected, newPreintCovarianceActual)); } /* ************************************************************************* */ -TEST( ImuFactor, JacobianPreintegratedCovariancePropagation_2ndOrderInt ) -{ +TEST( ImuFactor, JacobianPreintegratedCovariancePropagation_2ndOrderInt ) { // 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)); @@ -635,68 +680,71 @@ TEST( ImuFactor, JacobianPreintegratedCovariancePropagation_2ndOrderInt ) list measuredAccs, measuredOmegas; list deltaTs; measuredAccs.push_back(Vector3(0.1, 0.0, 0.0)); - measuredOmegas.push_back(Vector3(M_PI/100.0, 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)); + measuredOmegas.push_back(Vector3(M_PI / 100.0, 0.0, 0.0)); deltaTs.push_back(0.01); - for(int i=1;i<100;i++) - { + 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)); + measuredOmegas.push_back( + Vector3(M_PI / 100.0, M_PI / 300.0, 2 * M_PI / 100.0)); deltaTs.push_back(0.01); } bool use2ndOrderIntegration = true; // Actual preintegrated values ImuFactor::PreintegratedMeasurements preintegrated = - evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs, use2ndOrderIntegration); + evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, + deltaTs, use2ndOrderIntegration); // 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 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 + 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); + preintegrated.integrateMeasurement(newMeasuredAcc, newMeasuredOmega, + newDeltaT, body_P_sensor, Factual, Gactual); ////////////////////////////////////////////////////////////////////////////////////////////// // COMPUTE NUMERICAL DERIVATIVES FOR F ////////////////////////////////////////////////////////////////////////////////////////////// // Compute expected f_pos_vel wrt positions - Matrix dfpv_dpos = - numericalDerivative11(boost::bind(&updatePreintegratedPosVel, - _1, deltaVij_old, deltaRij_old, - newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaPij_old); + Matrix dfpv_dpos = numericalDerivative11( + 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(boost::bind(&updatePreintegratedPosVel, - deltaPij_old, _1, deltaRij_old, - newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaVij_old); + Matrix dfpv_dvel = numericalDerivative11( + 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(boost::bind(&updatePreintegratedPosVel, - deltaPij_old, deltaVij_old, _1, - newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaRij_old); + Matrix dfpv_dangle = numericalDerivative11( + 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; + Matrix FexpectedTop6(6, 9); + FexpectedTop6 << dfpv_dpos, dfpv_dvel, dfpv_dangle; // Compute expected f_rot wrt angles - Matrix dfr_dangle = - numericalDerivative11(boost::bind(&updatePreintegratedRot, - _1, newMeasuredOmega, newDeltaT), deltaRij_old); + Matrix dfr_dangle = numericalDerivative11( + boost::bind(&updatePreintegratedRot, _1, newMeasuredOmega, newDeltaT), + deltaRij_old); - Matrix FexpectedBottom3(3,9); + Matrix FexpectedBottom3(3, 9); FexpectedBottom3 << Z_3x3, Z_3x3, dfr_dangle; - Matrix Fexpected(9,9); Fexpected << FexpectedTop6, FexpectedBottom3; + Matrix Fexpected(9, 9); + Fexpected << FexpectedTop6, FexpectedBottom3; EXPECT(assert_equal(Fexpected, Factual)); @@ -704,42 +752,43 @@ TEST( ImuFactor, JacobianPreintegratedCovariancePropagation_2ndOrderInt ) // COMPUTE NUMERICAL DERIVATIVES FOR G ////////////////////////////////////////////////////////////////////////////////////////////// // Compute jacobian wrt integration noise - Matrix dgpv_dintNoise(6,3); + Matrix dgpv_dintNoise(6, 3); dgpv_dintNoise << I_3x3 * newDeltaT, Z_3x3; // Compute jacobian wrt acc noise - Matrix dgpv_daccNoise = - numericalDerivative11(boost::bind(&updatePreintegratedPosVel, - deltaPij_old, deltaVij_old, deltaRij_old, - _1, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), newMeasuredAcc); + Matrix dgpv_daccNoise = numericalDerivative11( + boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old, + deltaRij_old, _1, newMeasuredOmega, newDeltaT, + use2ndOrderIntegration), newMeasuredAcc); // Compute expected F wrt gyro noise - Matrix dgpv_domegaNoise = - numericalDerivative11(boost::bind(&updatePreintegratedPosVel, - deltaPij_old, deltaVij_old, deltaRij_old, - newMeasuredAcc, _1, newDeltaT, use2ndOrderIntegration), newMeasuredOmega); - Matrix GexpectedTop6(6,9); + Matrix dgpv_domegaNoise = numericalDerivative11( + 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(boost::bind(&updatePreintegratedRot, - deltaRij_old, _1, newDeltaT), newMeasuredOmega); + Matrix dgr_dangle = numericalDerivative11( + boost::bind(&updatePreintegratedRot, deltaRij_old, _1, newDeltaT), + newMeasuredOmega); - Matrix GexpectedBottom3(3,9); + Matrix GexpectedBottom3(3, 9); GexpectedBottom3 << Z_3x3, Z_3x3, dgr_dangle; - Matrix Gexpected(9,9); Gexpected << GexpectedTop6, GexpectedBottom3; + 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; + 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 newPreintCovarianceExpected = Factual * oldPreintCovariance + * Factual.transpose() + + (1 / newDeltaT) * Gactual * measurementCovariance * Gactual.transpose(); Matrix newPreintCovarianceActual = preintegrated.preintMeasCov(); EXPECT(assert_equal(newPreintCovarianceExpected, newPreintCovarianceActual)); @@ -801,93 +850,109 @@ TEST( ImuFactor, JacobianPreintegratedCovariancePropagation_2ndOrderInt ) //} /* ************************************************************************* */ -TEST( ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement ) -{ +TEST( ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement ) { 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)); + Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI / 4.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 / 4.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); + 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; - const Pose3 body_P_sensor(Rot3::Expmap(Vector3(0,0.10,0.10)), Point3(1,0,0)); + const Pose3 body_P_sensor(Rot3::Expmap(Vector3(0, 0.10, 0.10)), + Point3(1, 0, 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()); + 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); - // 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); - // Expected Jacobians - Matrix H1e = numericalDerivative11( - boost::bind(&callEvaluateError, factor, _1, v1, x2, v2, bias), x1); - Matrix H2e = numericalDerivative11( - boost::bind(&callEvaluateError, factor, x1, _1, x2, v2, bias), v1); - Matrix H3e = numericalDerivative11( - boost::bind(&callEvaluateError, factor, x1, v1, _1, v2, bias), x2); - Matrix H4e = numericalDerivative11( - boost::bind(&callEvaluateError, factor, x1, v1, x2, _1, bias), v2); - Matrix H5e = numericalDerivative11( - boost::bind(&callEvaluateError, factor, x1, v1, x2, v2, _1), bias); + // Expected Jacobians + Matrix H1e = numericalDerivative11( + boost::bind(&callEvaluateError, factor, _1, v1, x2, v2, bias), x1); + Matrix H2e = numericalDerivative11( + boost::bind(&callEvaluateError, factor, x1, _1, x2, v2, bias), v1); + Matrix H3e = numericalDerivative11( + boost::bind(&callEvaluateError, factor, x1, v1, _1, v2, bias), x2); + Matrix H4e = numericalDerivative11( + boost::bind(&callEvaluateError, factor, x1, v1, x2, _1, bias), v2); + Matrix H5e = numericalDerivative11( + boost::bind(&callEvaluateError, factor, x1, v1, x2, v2, _1), bias); - // Check rotation Jacobians - Matrix RH1e = numericalDerivative11( - boost::bind(&evaluateRotationError, factor, _1, v1, x2, v2, bias), x1); - Matrix RH3e = numericalDerivative11( - boost::bind(&evaluateRotationError, factor, x1, v1, _1, v2, bias), x2); - Matrix RH5e = numericalDerivative11( - boost::bind(&evaluateRotationError, factor, x1, v1, x2, v2, _1), bias); + // Check rotation Jacobians + Matrix RH1e = numericalDerivative11( + boost::bind(&evaluateRotationError, factor, _1, v1, x2, v2, bias), x1); + Matrix RH3e = numericalDerivative11( + boost::bind(&evaluateRotationError, factor, x1, v1, _1, v2, bias), x2); + Matrix RH5e = numericalDerivative11( + 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, PredictPositionAndVelocity){ +TEST(ImuFactor, PredictPositionAndVelocity) { imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot) // Measurements - Vector3 gravity; gravity << 0, 0, 9.81; - Vector3 omegaCoriolis; omegaCoriolis << 0, 0, 0; - Vector3 measuredOmega; measuredOmega << 0, 0, 0;//M_PI/10.0+0.3; - Vector3 measuredAcc; measuredAcc << 0,1,-9.81; + Vector3 gravity; + gravity << 0, 0, 9.81; + Vector3 omegaCoriolis; + omegaCoriolis << 0, 0, 0; + Vector3 measuredOmega; + measuredOmega << 0, 0, 0; //M_PI/10.0+0.3; + Vector3 measuredAcc; + measuredAcc << 0, 1, -9.81; double deltaT = 0.001; - Matrix I6x6(6,6); - I6x6 = Matrix::Identity(6,6); + 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::Zero(), Matrix3::Zero(), Matrix3::Zero(), true); + 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(), true); - for (int i = 0; i<1000; ++i) pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + for (int i = 0; i < 1000; ++i) + 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); - // Predict - Pose3 x1; - Vector3 v1(0, 0.0, 0.0); - 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))); + // Predict + Pose3 x1; + Vector3 v1(0, 0.0, 0.0); + 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))); } /* ************************************************************************* */ @@ -895,34 +960,46 @@ TEST(ImuFactor, PredictRotation) { imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot) // Measurements - Vector3 gravity; gravity << 0, 0, 9.81; - Vector3 omegaCoriolis; omegaCoriolis << 0, 0, 0; - Vector3 measuredOmega; measuredOmega << 0, 0, M_PI/10;//M_PI/10.0+0.3; - Vector3 measuredAcc; measuredAcc << 0,0,-9.81; + Vector3 gravity; + gravity << 0, 0, 9.81; + Vector3 omegaCoriolis; + omegaCoriolis << 0, 0, 0; + Vector3 measuredOmega; + measuredOmega << 0, 0, M_PI / 10; //M_PI/10.0+0.3; + Vector3 measuredAcc; + measuredAcc << 0, 0, -9.81; double deltaT = 0.001; - Matrix I6x6(6,6); - I6x6 = Matrix::Identity(6,6); + 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::Zero(), Matrix3::Zero(), Matrix3::Zero(), true); + 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(), true); - for (int i = 0; i<1000; ++i) pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + for (int i = 0; i < 1000; ++i) + 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); - // Predict - Pose3 x1, x2; - Vector3 v1 = Vector3(0, 0.0, 0.0); - Vector3 v2; - ImuFactor::Predict(x1, v1, x2, v2, bias, factor.preintegratedMeasurements(), 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, x2)); - EXPECT(assert_equal(Vector(expectedVelocity), Vector(v2))); + // Predict + Pose3 x1, x2; + Vector3 v1 = Vector3(0, 0.0, 0.0); + Vector3 v2; + ImuFactor::Predict(x1, v1, x2, v2, bias, factor.preintegratedMeasurements(), + 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, x2)); + EXPECT(assert_equal(Vector(expectedVelocity), Vector(v2))); } /* ************************************************************************* */ - int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} /* ************************************************************************* */