diff --git a/gtsam.h b/gtsam.h index 66091623b..81094ac89 100644 --- a/gtsam.h +++ b/gtsam.h @@ -907,7 +907,7 @@ class SymbolicBayesTree { //Constructors SymbolicBayesTree(); - SymbolicBayesTree(const gtsam::SymbolicBayesTree& other); + SymbolicBayesTree(const gtsam::SymbolicBayesTree& other); // Testable void print(string s); @@ -920,10 +920,10 @@ class SymbolicBayesTree { void clear(); void deleteCachedShortcuts(); size_t numCachedSeparatorMarginals() const; - - gtsam::SymbolicConditional* marginalFactor(size_t key) const; - gtsam::SymbolicFactorGraph* joint(size_t key1, size_t key2) const; - gtsam::SymbolicBayesNet* jointBayesNet(size_t key1, size_t key2) const; + + gtsam::SymbolicConditional* marginalFactor(size_t key) const; + gtsam::SymbolicFactorGraph* joint(size_t key1, size_t key2) const; + gtsam::SymbolicBayesNet* jointBayesNet(size_t key1, size_t key2) const; }; // class SymbolicBayesTreeClique { diff --git a/gtsam/base/LieMatrix.h b/gtsam/base/LieMatrix.h index 1eebd0514..498866589 100644 --- a/gtsam/base/LieMatrix.h +++ b/gtsam/base/LieMatrix.h @@ -154,7 +154,7 @@ struct LieMatrix : public Matrix, public DerivedValue { /** Logmap around identity - just returns with default cast back */ static inline Vector Logmap(const LieMatrix& p) { return Eigen::Map(&p(0,0), p.dim()); } - + /// @} private: diff --git a/gtsam/navigation/ImuBias.h b/gtsam/navigation/ImuBias.h index 12e702b7c..b535f5179 100644 --- a/gtsam/navigation/ImuBias.h +++ b/gtsam/navigation/ImuBias.h @@ -26,12 +26,12 @@ /* * NOTES: * - Earth-rate correction: - * + Currently the user should supply R_ECEF_to_G, which is the rotation from ECEF to Local-Level system (NED or ENU as defined by the user). - * + R_ECEF_to_G can be calculated by approximated values of latitude and longitude of the system. - * + A relatively small distance is traveled w.r.t. to initial pose is assumed, since R_ECEF_to_G is constant. - * Otherwise, R_ECEF_to_G should be updated each time using the current lat-lon. + * + Currently the user should supply R_ECEF_to_G, which is the rotation from ECEF to Local-Level system (NED or ENU as defined by the user). + * + R_ECEF_to_G can be calculated by approximated values of latitude and longitude of the system. + * + A relatively small distance is traveled w.r.t. to initial pose is assumed, since R_ECEF_to_G is constant. + * Otherwise, R_ECEF_to_G should be updated each time using the current lat-lon. * - * - Currently, an empty constructed is not enabled so that the user is forced to specify R_ECEF_to_G. + * - Currently, an empty constructed is not enabled so that the user is forced to specify R_ECEF_to_G. */ namespace gtsam { @@ -40,11 +40,11 @@ namespace gtsam { namespace imuBias { class ConstantBias : public DerivedValue { - private: + private: Vector3 biasAcc_; Vector3 biasGyro_; - public: + public: /// dimension of the variable - used to autodetect sizes static const size_t dimension = 6; @@ -144,17 +144,17 @@ namespace imuBias { /// return dimensionality of tangent space inline size_t dim() const { return dimension; } - /** Update the LieVector with a tangent space update */ - inline ConstantBias retract(const Vector& v) const { return ConstantBias(biasAcc_ + v.head(3), biasGyro_ + v.tail(3)); } + /** Update the LieVector with a tangent space update */ + inline ConstantBias retract(const Vector& v) const { return ConstantBias(biasAcc_ + v.head(3), biasGyro_ + v.tail(3)); } - /** @return the local coordinates of another object */ - inline Vector localCoordinates(const ConstantBias& b) const { return b.vector() - vector(); } + /** @return the local coordinates of another object */ + inline Vector localCoordinates(const ConstantBias& b) const { return b.vector() - vector(); } /// @} /// @name Group /// @{ - /** identity for group operation */ + /** identity for group operation */ static ConstantBias identity() { return ConstantBias(); } /** invert the object and yield a new one */ @@ -213,7 +213,7 @@ namespace imuBias { /// @} - }; // ConstantBias class + }; // ConstantBias class } // namespace ImuBias diff --git a/gtsam/navigation/tests/testCombinedImuFactor.cpp b/gtsam/navigation/tests/testCombinedImuFactor.cpp index fba083892..25a2765eb 100644 --- a/gtsam/navigation/tests/testCombinedImuFactor.cpp +++ b/gtsam/navigation/tests/testCombinedImuFactor.cpp @@ -136,12 +136,12 @@ TEST( CombinedImuFactor, PreintegratedMeasurements ) // Actual preintegrated values ImuFactor::PreintegratedMeasurements expected1(bias, Matrix3::Zero(), - Matrix3::Zero(), Matrix3::Zero()); + Matrix3::Zero(), Matrix3::Zero()); 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)); // const imuBias::ConstantBias& bias, ///< Current estimate of acceleration and rotation rate biases // const Matrix3& measuredAccCovariance, ///< Covariance matrix of measuredAcc @@ -193,13 +193,13 @@ TEST( CombinedImuFactor, ErrorWithBiases ) 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()); + 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); @@ -224,14 +224,14 @@ 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); + Matrix 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))); - EXPECT(assert_equal(H3e, H3a.topRows(9))); - EXPECT(assert_equal(H4e, H4a.topRows(9))); - EXPECT(assert_equal(H5e, H5a.topRows(9))); + EXPECT(assert_equal(H1e, H1a.topRows(9))); + EXPECT(assert_equal(H2e, H2a.topRows(9))); + EXPECT(assert_equal(H3e, H3a.topRows(9))); + EXPECT(assert_equal(H4e, H4a.topRows(9))); + EXPECT(assert_equal(H5e, H5a.topRows(9))); } /* ************************************************************************* */ diff --git a/gtsam/navigation/tests/testImuBias.cpp b/gtsam/navigation/tests/testImuBias.cpp index 2fdd2fb4e..c95383b64 100644 --- a/gtsam/navigation/tests/testImuBias.cpp +++ b/gtsam/navigation/tests/testImuBias.cpp @@ -39,5 +39,5 @@ TEST( ImuBias, Constructor) } /* ************************************************************************* */ - int main() { TestResult tr; return TestRegistry::runAllTests(tr);} + int main() { TestResult tr; return TestRegistry::runAllTests(tr);} /* ************************************************************************* */ diff --git a/gtsam/slam/tests/testBetweenFactor.cpp b/gtsam/slam/tests/testBetweenFactor.cpp index 44d401afd..fac2164d8 100644 --- a/gtsam/slam/tests/testBetweenFactor.cpp +++ b/gtsam/slam/tests/testBetweenFactor.cpp @@ -1,8 +1,8 @@ /** - * @file testBetweenFactor.cpp + * @file testBetweenFactor.cpp * @brief * @author Duy-Nguyen Ta - * @date Aug 2, 2013 + * @date Aug 2, 2013 */ #include diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp index 3dde8e1f6..68a0e1a45 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp @@ -289,7 +289,7 @@ ConcurrentBatchSmoother::Result ConcurrentBatchSmoother::optimize() { } gttoc(damp); if (lmVerbosity >= LevenbergMarquardtParams::DAMPED) - dampedFactorGraph.print("damped"); + dampedFactorGraph.print("damped"); result.lambdas++; gttic(solve); @@ -302,7 +302,7 @@ ConcurrentBatchSmoother::Result ConcurrentBatchSmoother::optimize() { if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) std::cout << "linear delta norm = " << newDelta.norm() << std::endl; if (lmVerbosity >= LevenbergMarquardtParams::TRYDELTA) - newDelta.print("delta"); + newDelta.print("delta"); // Evaluate the new error gttic(compute_error); @@ -310,7 +310,7 @@ ConcurrentBatchSmoother::Result ConcurrentBatchSmoother::optimize() { gttoc(compute_error); if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) - std::cout << "next error = " << error << std::endl; + std::cout << "next error = " << error << std::endl; if(error < result.error) { // Keep this change diff --git a/gtsam_unstable/slam/AHRS.cpp b/gtsam_unstable/slam/AHRS.cpp index 002c9e3e6..32ead0c15 100644 --- a/gtsam_unstable/slam/AHRS.cpp +++ b/gtsam_unstable/slam/AHRS.cpp @@ -25,7 +25,7 @@ Matrix Z3 = zeros(3, 3); /* ************************************************************************* */ AHRS::AHRS(const Matrix& stationaryU, const Matrix& stationaryF, double g_e, - bool flat) : + bool flat) : KF_(9) { // Initial state @@ -182,8 +182,8 @@ std::pair AHRS::aid( // F(:,k) = mech.x_a + dx_a - bRn*n_g; // F(:,k) = mech.x_a + dx_a - bRn*(I+P)*n_g; // F(:,k) = mech.x_a + dx_a - b_g - bRn*(rho x n_g); // P = [rho]_x - // Hence, the measurement z = b_g - (mech.x_a - F(:,k)) is predicted - // from the filter state (dx_a, rho) as dx_a + bRn*(n_g x rho) + // Hence, the measurement z = b_g - (mech.x_a - F(:,k)) is predicted + // from the filter state (dx_a, rho) as dx_a + bRn*(n_g x rho) // z = b_g - (mech.x_a - F(:,k)) = dx_a + bRn*(n_g x rho) z = bRn * n_g_ - measured_b_g; // Now the Jacobian H diff --git a/gtsam_unstable/slam/CombinedImuFactor.h b/gtsam_unstable/slam/CombinedImuFactor.h index b954b82f9..bf86b6933 100644 --- a/gtsam_unstable/slam/CombinedImuFactor.h +++ b/gtsam_unstable/slam/CombinedImuFactor.h @@ -118,14 +118,14 @@ namespace gtsam { delVdelBiasAcc(Matrix3::Zero()), delVdelBiasOmega(Matrix3::Zero()), delRdelBiasOmega(Matrix3::Zero()), PreintMeasCov(Matrix::Zero(15,15)) { - // COVARIANCE OF: [Integration AccMeasurement OmegaMeasurement BiasAccRandomWalk BiasOmegaRandomWalk (BiasAccInit BiasOmegaInit)] SIZE (21x21) - measurementCovariance << integrationErrorCovariance , Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), - Matrix3::Zero(), measuredAccCovariance, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), + // COVARIANCE OF: [Integration AccMeasurement OmegaMeasurement BiasAccRandomWalk BiasOmegaRandomWalk (BiasAccInit BiasOmegaInit)] SIZE (21x21) + measurementCovariance << integrationErrorCovariance , Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), + Matrix3::Zero(), measuredAccCovariance, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), measuredOmegaCovariance, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), - Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), biasAccCovariance, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), - Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), biasOmegaCovariance, Matrix3::Zero(), Matrix3::Zero(), - Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), biasAccOmegaInit.block(0,0,3,3), biasAccOmegaInit.block(0,3,3,3), - Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), biasAccOmegaInit.block(3,0,3,3), biasAccOmegaInit.block(3,3,3,3); + Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), biasAccCovariance, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), + Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), biasOmegaCovariance, Matrix3::Zero(), Matrix3::Zero(), + Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), biasAccOmegaInit.block(0,0,3,3), biasAccOmegaInit.block(0,3,3,3), + Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), biasAccOmegaInit.block(3,0,3,3), biasAccOmegaInit.block(3,3,3,3); } CombinedPreintegratedMeasurements() : @@ -231,10 +231,10 @@ namespace gtsam { // overall Jacobian wrt preintegrated measurements (df/dx) Matrix F(15,15); F << H_pos_pos, H_pos_vel, H_pos_angles, Z_3x3, Z_3x3, - H_vel_pos, H_vel_vel, H_vel_angles, H_vel_biasacc, Z_3x3, - H_angles_pos, H_angles_vel, H_angles_angles, Z_3x3, H_angles_biasomega, - Z_3x3, Z_3x3, Z_3x3, I_3x3, Z_3x3, - Z_3x3, Z_3x3, Z_3x3, Z_3x3, I_3x3; + H_vel_pos, H_vel_vel, H_vel_angles, H_vel_biasacc, Z_3x3, + H_angles_pos, H_angles_vel, H_angles_angles, Z_3x3, H_angles_biasomega, + Z_3x3, Z_3x3, Z_3x3, I_3x3, Z_3x3, + Z_3x3, Z_3x3, Z_3x3, Z_3x3, I_3x3; // first order uncertainty propagation @@ -567,18 +567,18 @@ namespace gtsam { if(H6) { - H6->resize(15,6); - (*H6) << - // dfP/dBias_j - Matrix3::Zero(), Matrix3::Zero(), - // dfV/dBias_j - Matrix3::Zero(), Matrix3::Zero(), - // dfR/dBias_j - Matrix3::Zero(), Matrix3::Zero(), - //dBiasAcc/dBias_j - Matrix3::Identity(), Matrix3::Zero(), - //dBiasOmega/dBias_j - Matrix3::Zero(), Matrix3::Identity(); + H6->resize(15,6); + (*H6) << + // dfP/dBias_j + Matrix3::Zero(), Matrix3::Zero(), + // dfV/dBias_j + Matrix3::Zero(), Matrix3::Zero(), + // dfR/dBias_j + Matrix3::Zero(), Matrix3::Zero(), + //dBiasAcc/dBias_j + Matrix3::Identity(), Matrix3::Zero(), + //dBiasOmega/dBias_j + Matrix3::Zero(), Matrix3::Identity(); } diff --git a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h index 650e11ff0..121295d50 100644 --- a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h +++ b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h @@ -39,8 +39,8 @@ namespace gtsam { * ===== * Concept: Based on [Lupton12tro] * - Pre-integrate IMU measurements using the static function PreIntegrateIMUObservations. - * Pre-integrated quantities are expressed in the body system of t0 - the first time instant (in which pre-integration began). - * All sensor-to-body transformations are performed here. + * Pre-integrated quantities are expressed in the body system of t0 - the first time instant (in which pre-integration began). + * All sensor-to-body transformations are performed here. * - If required, calculate inertial solution by calling the static functions: predictPose_inertial, predictVelocity_inertial. * - When the time is right, incorporate pre-integrated IMU data by creating an EquivInertialNavFactor_GlobalVel factor, which will * relate between navigation variables at the two time instances (t0 and current time). @@ -54,11 +54,11 @@ namespace gtsam { * matrices and the process\modeling covariance matrix. The IneritalNavFactor converts this into a * discrete form using the supplied delta_t between sub-sequential measurements. * - Earth-rate correction: - * + Currently the user should supply R_ECEF_to_G, which is the rotation from ECEF to the global - * frame (Local-Level system: ENU or NED, see above). - * + R_ECEF_to_G can be calculated by approximated values of latitude and longitude of the system. - * + Currently it is assumed that a relatively small distance is traveled w.r.t. to initial pose, since R_ECEF_to_G is constant. - * Otherwise, R_ECEF_to_G should be updated each time using the current lat-lon. + * + Currently the user should supply R_ECEF_to_G, which is the rotation from ECEF to the global + * frame (Local-Level system: ENU or NED, see above). + * + R_ECEF_to_G can be calculated by approximated values of latitude and longitude of the system. + * + Currently it is assumed that a relatively small distance is traveled w.r.t. to initial pose, since R_ECEF_to_G is constant. + * Otherwise, R_ECEF_to_G should be updated each time using the current lat-lon. * * - Frame Notation: * Quantities are written as {Frame of Representation/Destination Frame}_{Quantity Type}_{Quatity Description/Origination Frame} @@ -92,260 +92,260 @@ class EquivInertialNavFactor_GlobalVel : public NoiseModelFactor5 This; - typedef NoiseModelFactor5 Base; + typedef EquivInertialNavFactor_GlobalVel This; + typedef NoiseModelFactor5 Base; - Vector delta_pos_in_t0_; - Vector delta_vel_in_t0_; - Vector3 delta_angles_; - double dt12_; + Vector delta_pos_in_t0_; + Vector delta_vel_in_t0_; + Vector3 delta_angles_; + double dt12_; - Vector world_g_; - Vector world_rho_; - Vector world_omega_earth_; + Vector world_g_; + Vector world_rho_; + Vector world_omega_earth_; - Matrix Jacobian_wrt_t0_Overall_; + Matrix Jacobian_wrt_t0_Overall_; - boost::optional Bias_initial_; // Bias used when pre-integrating IMU measurements - boost::optional body_P_sensor_; // The pose of the sensor in the body frame + boost::optional Bias_initial_; // Bias used when pre-integrating IMU measurements + boost::optional body_P_sensor_; // The pose of the sensor in the body frame public: - // shorthand for a smart pointer to a factor - typedef typename boost::shared_ptr shared_ptr; + // shorthand for a smart pointer to a factor + typedef typename boost::shared_ptr shared_ptr; - /** default constructor - only use for serialization */ - EquivInertialNavFactor_GlobalVel() {} + /** default constructor - only use for serialization */ + EquivInertialNavFactor_GlobalVel() {} - /** Constructor */ - EquivInertialNavFactor_GlobalVel(const Key& Pose1, const Key& Vel1, const Key& IMUBias1, const Key& Pose2, const Key& Vel2, - const Vector& delta_pos_in_t0, const Vector& delta_vel_in_t0, const Vector3& delta_angles, - double dt12, const Vector world_g, const Vector world_rho, - const Vector& world_omega_earth, const noiseModel::Gaussian::shared_ptr& model_equivalent, - const Matrix& Jacobian_wrt_t0_Overall, - boost::optional Bias_initial = boost::none, boost::optional body_P_sensor = boost::none) : - Base(model_equivalent, Pose1, Vel1, IMUBias1, Pose2, Vel2), - delta_pos_in_t0_(delta_pos_in_t0), delta_vel_in_t0_(delta_vel_in_t0), delta_angles_(delta_angles), - dt12_(dt12), world_g_(world_g), world_rho_(world_rho), world_omega_earth_(world_omega_earth), Jacobian_wrt_t0_Overall_(Jacobian_wrt_t0_Overall), - Bias_initial_(Bias_initial), body_P_sensor_(body_P_sensor) { } + /** Constructor */ + EquivInertialNavFactor_GlobalVel(const Key& Pose1, const Key& Vel1, const Key& IMUBias1, const Key& Pose2, const Key& Vel2, + const Vector& delta_pos_in_t0, const Vector& delta_vel_in_t0, const Vector3& delta_angles, + double dt12, const Vector world_g, const Vector world_rho, + const Vector& world_omega_earth, const noiseModel::Gaussian::shared_ptr& model_equivalent, + const Matrix& Jacobian_wrt_t0_Overall, + boost::optional Bias_initial = boost::none, boost::optional body_P_sensor = boost::none) : + Base(model_equivalent, Pose1, Vel1, IMUBias1, Pose2, Vel2), + delta_pos_in_t0_(delta_pos_in_t0), delta_vel_in_t0_(delta_vel_in_t0), delta_angles_(delta_angles), + dt12_(dt12), world_g_(world_g), world_rho_(world_rho), world_omega_earth_(world_omega_earth), Jacobian_wrt_t0_Overall_(Jacobian_wrt_t0_Overall), + Bias_initial_(Bias_initial), body_P_sensor_(body_P_sensor) { } - virtual ~EquivInertialNavFactor_GlobalVel() {} + virtual ~EquivInertialNavFactor_GlobalVel() {} - /** implement functions needed for Testable */ + /** implement functions needed for Testable */ - /** print */ - virtual void print(const std::string& s = "EquivInertialNavFactor_GlobalVel", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { - std::cout << s << "(" - << keyFormatter(this->key1()) << "," - << keyFormatter(this->key2()) << "," - << keyFormatter(this->key3()) << "," - << keyFormatter(this->key4()) << "," - << keyFormatter(this->key5()) << "\n"; - std::cout << "delta_pos_in_t0: " << this->delta_pos_in_t0_.transpose() << std::endl; - std::cout << "delta_vel_in_t0: " << this->delta_vel_in_t0_.transpose() << std::endl; - std::cout << "delta_angles: " << this->delta_angles_ << std::endl; - std::cout << "dt12: " << this->dt12_ << std::endl; - std::cout << "gravity (in world frame): " << this->world_g_.transpose() << std::endl; - std::cout << "craft rate (in world frame): " << this->world_rho_.transpose() << std::endl; - std::cout << "earth's rotation (in world frame): " << this->world_omega_earth_.transpose() << std::endl; - if(this->body_P_sensor_) - this->body_P_sensor_->print(" sensor pose in body frame: "); - this->noiseModel_->print(" noise model"); - } + /** print */ + virtual void print(const std::string& s = "EquivInertialNavFactor_GlobalVel", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + std::cout << s << "(" + << keyFormatter(this->key1()) << "," + << keyFormatter(this->key2()) << "," + << keyFormatter(this->key3()) << "," + << keyFormatter(this->key4()) << "," + << keyFormatter(this->key5()) << "\n"; + std::cout << "delta_pos_in_t0: " << this->delta_pos_in_t0_.transpose() << std::endl; + std::cout << "delta_vel_in_t0: " << this->delta_vel_in_t0_.transpose() << std::endl; + std::cout << "delta_angles: " << this->delta_angles_ << std::endl; + std::cout << "dt12: " << this->dt12_ << std::endl; + std::cout << "gravity (in world frame): " << this->world_g_.transpose() << std::endl; + std::cout << "craft rate (in world frame): " << this->world_rho_.transpose() << std::endl; + std::cout << "earth's rotation (in world frame): " << this->world_omega_earth_.transpose() << std::endl; + if(this->body_P_sensor_) + this->body_P_sensor_->print(" sensor pose in body frame: "); + this->noiseModel_->print(" noise model"); + } - /** equals */ - virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { - const This *e = dynamic_cast (&expected); - return e != NULL && Base::equals(*e, tol) - && (delta_pos_in_t0_ - e->delta_pos_in_t0_).norm() < tol - && (delta_vel_in_t0_ - e->delta_vel_in_t0_).norm() < tol - && (delta_angles_ - e->delta_angles_).norm() < tol - && (dt12_ - e->dt12_) < tol - && (world_g_ - e->world_g_).norm() < tol - && (world_rho_ - e->world_rho_).norm() < tol - && (world_omega_earth_ - e->world_omega_earth_).norm() < tol - && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_))); - } + /** equals */ + virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { + const This *e = dynamic_cast (&expected); + return e != NULL && Base::equals(*e, tol) + && (delta_pos_in_t0_ - e->delta_pos_in_t0_).norm() < tol + && (delta_vel_in_t0_ - e->delta_vel_in_t0_).norm() < tol + && (delta_angles_ - e->delta_angles_).norm() < tol + && (dt12_ - e->dt12_) < tol + && (world_g_ - e->world_g_).norm() < tol + && (world_rho_ - e->world_rho_).norm() < tol + && (world_omega_earth_ - e->world_omega_earth_).norm() < tol + && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_))); + } - POSE predictPose(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1) const { + POSE predictPose(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1) const { - // Correct delta_pos_in_t0_ using (Bias1 - Bias_t0) - Vector delta_BiasAcc = Bias1.accelerometer(); - Vector delta_BiasGyro = Bias1.gyroscope(); - if (Bias_initial_){ - delta_BiasAcc -= Bias_initial_->accelerometer(); - delta_BiasGyro -= Bias_initial_->gyroscope(); - } + // Correct delta_pos_in_t0_ using (Bias1 - Bias_t0) + Vector delta_BiasAcc = Bias1.accelerometer(); + Vector delta_BiasGyro = Bias1.gyroscope(); + if (Bias_initial_){ + delta_BiasAcc -= Bias_initial_->accelerometer(); + delta_BiasGyro -= Bias_initial_->gyroscope(); + } - Matrix J_Pos_wrt_BiasAcc = Jacobian_wrt_t0_Overall_.block(4,9,3,3); - Matrix J_Pos_wrt_BiasGyro = Jacobian_wrt_t0_Overall_.block(4,12,3,3); - Matrix J_angles_wrt_BiasGyro = Jacobian_wrt_t0_Overall_.block(0,12,3,3); + Matrix J_Pos_wrt_BiasAcc = Jacobian_wrt_t0_Overall_.block(4,9,3,3); + Matrix J_Pos_wrt_BiasGyro = Jacobian_wrt_t0_Overall_.block(4,12,3,3); + Matrix J_angles_wrt_BiasGyro = Jacobian_wrt_t0_Overall_.block(0,12,3,3); - /* Position term */ - Vector delta_pos_in_t0_corrected = delta_pos_in_t0_ + J_Pos_wrt_BiasAcc*delta_BiasAcc + J_Pos_wrt_BiasGyro*delta_BiasGyro; + /* Position term */ + Vector delta_pos_in_t0_corrected = delta_pos_in_t0_ + J_Pos_wrt_BiasAcc*delta_BiasAcc + J_Pos_wrt_BiasGyro*delta_BiasGyro; - /* Rotation term */ - Vector delta_angles_corrected = delta_angles_ + J_angles_wrt_BiasGyro*delta_BiasGyro; - // Another alternative: - // Vector delta_angles_corrected = Rot3::Logmap( Rot3::Expmap(delta_angles_)*Rot3::Expmap(J_angles_wrt_BiasGyro*delta_BiasGyro) ); + /* Rotation term */ + Vector delta_angles_corrected = delta_angles_ + J_angles_wrt_BiasGyro*delta_BiasGyro; + // Another alternative: + // Vector delta_angles_corrected = Rot3::Logmap( Rot3::Expmap(delta_angles_)*Rot3::Expmap(J_angles_wrt_BiasGyro*delta_BiasGyro) ); - return predictPose_inertial(Pose1, Vel1, - delta_pos_in_t0_corrected, delta_angles_corrected, - dt12_, world_g_, world_rho_, world_omega_earth_); - } + return predictPose_inertial(Pose1, Vel1, + delta_pos_in_t0_corrected, delta_angles_corrected, + dt12_, world_g_, world_rho_, world_omega_earth_); + } - static inline POSE predictPose_inertial(const POSE& Pose1, const VELOCITY& Vel1, - const Vector& delta_pos_in_t0, const Vector3& delta_angles, - const double dt12, const Vector& world_g, const Vector& world_rho, const Vector& world_omega_earth){ + static inline POSE predictPose_inertial(const POSE& Pose1, const VELOCITY& Vel1, + const Vector& delta_pos_in_t0, const Vector3& delta_angles, + const double dt12, const Vector& world_g, const Vector& world_rho, const Vector& world_omega_earth){ - const POSE& world_P1_body = Pose1; - const VELOCITY& world_V1_body = Vel1; + const POSE& world_P1_body = Pose1; + const VELOCITY& world_V1_body = Vel1; - /* Position term */ - Vector body_deltaPos_body = delta_pos_in_t0; + /* Position term */ + Vector body_deltaPos_body = delta_pos_in_t0; - Vector world_deltaPos_pls_body = world_P1_body.rotation().matrix() * body_deltaPos_body; - Vector world_deltaPos_body = world_V1_body * dt12 + 0.5*world_g*dt12*dt12 + world_deltaPos_pls_body; + Vector world_deltaPos_pls_body = world_P1_body.rotation().matrix() * body_deltaPos_body; + Vector world_deltaPos_body = world_V1_body * dt12 + 0.5*world_g*dt12*dt12 + world_deltaPos_pls_body; - // Incorporate earth-related terms. Note - these are assumed to be constant between t1 and t2. - world_deltaPos_body -= 2*skewSymmetric(world_rho + world_omega_earth)*world_V1_body * dt12*dt12; + // Incorporate earth-related terms. Note - these are assumed to be constant between t1 and t2. + world_deltaPos_body -= 2*skewSymmetric(world_rho + world_omega_earth)*world_V1_body * dt12*dt12; - /* TODO: the term dt12*dt12 in 0.5*world_g*dt12*dt12 is not entirely correct: - * the gravity should be canceled from the accelerometer measurements, bust since position - * is added with a delta velocity from a previous term, the actual delta time is more complicated. - * Need to figure out this in the future - currently because of this issue we'll get some more error - * in Z axis. - */ + /* TODO: the term dt12*dt12 in 0.5*world_g*dt12*dt12 is not entirely correct: + * the gravity should be canceled from the accelerometer measurements, bust since position + * is added with a delta velocity from a previous term, the actual delta time is more complicated. + * Need to figure out this in the future - currently because of this issue we'll get some more error + * in Z axis. + */ - /* Rotation term */ - Vector body_deltaAngles_body = delta_angles; + /* Rotation term */ + Vector body_deltaAngles_body = delta_angles; - // Convert earth-related terms into the body frame - Matrix body_R_world(world_P1_body.rotation().inverse().matrix()); - Vector body_rho = body_R_world * world_rho; - Vector body_omega_earth = body_R_world * world_omega_earth; + // Convert earth-related terms into the body frame + Matrix body_R_world(world_P1_body.rotation().inverse().matrix()); + Vector body_rho = body_R_world * world_rho; + Vector body_omega_earth = body_R_world * world_omega_earth; - // Incorporate earth-related terms. Note - these are assumed to be constant between t1 and t2. - body_deltaAngles_body -= (body_rho + body_omega_earth)*dt12; + // Incorporate earth-related terms. Note - these are assumed to be constant between t1 and t2. + body_deltaAngles_body -= (body_rho + body_omega_earth)*dt12; - return POSE(Pose1.rotation() * POSE::Rotation::Expmap(body_deltaAngles_body), Pose1.translation() + typename POSE::Translation(world_deltaPos_body)); + return POSE(Pose1.rotation() * POSE::Rotation::Expmap(body_deltaAngles_body), Pose1.translation() + typename POSE::Translation(world_deltaPos_body)); - } + } - VELOCITY predictVelocity(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1) const { + VELOCITY predictVelocity(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1) const { - // Correct delta_vel_in_t0_ using (Bias1 - Bias_t0) - Vector delta_BiasAcc = Bias1.accelerometer(); - Vector delta_BiasGyro = Bias1.gyroscope(); - if (Bias_initial_){ - delta_BiasAcc -= Bias_initial_->accelerometer(); - delta_BiasGyro -= Bias_initial_->gyroscope(); - } + // Correct delta_vel_in_t0_ using (Bias1 - Bias_t0) + Vector delta_BiasAcc = Bias1.accelerometer(); + Vector delta_BiasGyro = Bias1.gyroscope(); + if (Bias_initial_){ + delta_BiasAcc -= Bias_initial_->accelerometer(); + delta_BiasGyro -= Bias_initial_->gyroscope(); + } - Matrix J_Vel_wrt_BiasAcc = Jacobian_wrt_t0_Overall_.block(6,9,3,3); - Matrix J_Vel_wrt_BiasGyro = Jacobian_wrt_t0_Overall_.block(6,12,3,3); + Matrix J_Vel_wrt_BiasAcc = Jacobian_wrt_t0_Overall_.block(6,9,3,3); + Matrix J_Vel_wrt_BiasGyro = Jacobian_wrt_t0_Overall_.block(6,12,3,3); - Vector delta_vel_in_t0_corrected = delta_vel_in_t0_ + J_Vel_wrt_BiasAcc*delta_BiasAcc + J_Vel_wrt_BiasGyro*delta_BiasGyro; + Vector delta_vel_in_t0_corrected = delta_vel_in_t0_ + J_Vel_wrt_BiasAcc*delta_BiasAcc + J_Vel_wrt_BiasGyro*delta_BiasGyro; - return predictVelocity_inertial(Pose1, Vel1, - delta_vel_in_t0_corrected, - dt12_, world_g_, world_rho_, world_omega_earth_); - } + return predictVelocity_inertial(Pose1, Vel1, + delta_vel_in_t0_corrected, + dt12_, world_g_, world_rho_, world_omega_earth_); + } - static inline VELOCITY predictVelocity_inertial(const POSE& Pose1, const VELOCITY& Vel1, - const Vector& delta_vel_in_t0, - const double dt12, const Vector& world_g, const Vector& world_rho, const Vector& world_omega_earth) { + static inline VELOCITY predictVelocity_inertial(const POSE& Pose1, const VELOCITY& Vel1, + const Vector& delta_vel_in_t0, + const double dt12, const Vector& world_g, const Vector& world_rho, const Vector& world_omega_earth) { - const POSE& world_P1_body = Pose1; - const VELOCITY& world_V1_body = Vel1; + const POSE& world_P1_body = Pose1; + const VELOCITY& world_V1_body = Vel1; - Vector body_deltaVel_body = delta_vel_in_t0; - Vector world_deltaVel_body = world_P1_body.rotation().matrix() * body_deltaVel_body; + Vector body_deltaVel_body = delta_vel_in_t0; + Vector world_deltaVel_body = world_P1_body.rotation().matrix() * body_deltaVel_body; - VELOCITY VelDelta( world_deltaVel_body + world_g * dt12 ); + VELOCITY VelDelta( world_deltaVel_body + world_g * dt12 ); - // Incorporate earth-related terms. Note - these are assumed to be constant between t1 and t2. - VelDelta -= 2*skewSymmetric(world_rho + world_omega_earth)*world_V1_body * dt12; + // Incorporate earth-related terms. Note - these are assumed to be constant between t1 and t2. + VelDelta -= 2*skewSymmetric(world_rho + world_omega_earth)*world_V1_body * dt12; - // Predict - return Vel1.compose( VelDelta ); + // Predict + return Vel1.compose( VelDelta ); - } + } - void predict(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1, POSE& Pose2, VELOCITY& Vel2) const { - Pose2 = predictPose(Pose1, Vel1, Bias1); - Vel2 = predictVelocity(Pose1, Vel1, Bias1); - } + void predict(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1, POSE& Pose2, VELOCITY& Vel2) const { + Pose2 = predictPose(Pose1, Vel1, Bias1); + Vel2 = predictVelocity(Pose1, Vel1, Bias1); + } - POSE evaluatePoseError(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1, const POSE& Pose2, const VELOCITY& Vel2) const { - // Predict - POSE Pose2Pred = predictPose(Pose1, Vel1, Bias1); + POSE evaluatePoseError(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1, const POSE& Pose2, const VELOCITY& Vel2) const { + // Predict + POSE Pose2Pred = predictPose(Pose1, Vel1, Bias1); - // Luca: difference between Pose2 and Pose2Pred - POSE DiffPose( Pose2.rotation().between(Pose2Pred.rotation()), Pose2Pred.translation() - Pose2.translation() ); -// DiffPose = Pose2.between(Pose2Pred); - return DiffPose; - // Calculate error - //return Pose2.between(Pose2Pred); - } + // Luca: difference between Pose2 and Pose2Pred + POSE DiffPose( Pose2.rotation().between(Pose2Pred.rotation()), Pose2Pred.translation() - Pose2.translation() ); +// DiffPose = Pose2.between(Pose2Pred); + return DiffPose; + // Calculate error + //return Pose2.between(Pose2Pred); + } - VELOCITY evaluateVelocityError(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1, const POSE& Pose2, const VELOCITY& Vel2) const { - // Predict - VELOCITY Vel2Pred = predictVelocity(Pose1, Vel1, Bias1); + VELOCITY evaluateVelocityError(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1, const POSE& Pose2, const VELOCITY& Vel2) const { + // Predict + VELOCITY Vel2Pred = predictVelocity(Pose1, Vel1, Bias1); - // Calculate error - return Vel2.between(Vel2Pred); - } + // Calculate error + return Vel2.between(Vel2Pred); + } - Vector evaluateError(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1, const POSE& Pose2, const VELOCITY& Vel2, - 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 POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1, const POSE& Pose2, const VELOCITY& Vel2, + 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 { - // TODO: Write analytical derivative calculations - // Jacobian w.r.t. Pose1 - if (H1){ - Matrix H1_Pose = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, this, _1, Vel1, Bias1, Pose2, Vel2), Pose1); - Matrix H1_Vel = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, this, _1, Vel1, Bias1, Pose2, Vel2), Pose1); - *H1 = stack(2, &H1_Pose, &H1_Vel); - } + // TODO: Write analytical derivative calculations + // Jacobian w.r.t. Pose1 + if (H1){ + Matrix H1_Pose = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, this, _1, Vel1, Bias1, Pose2, Vel2), Pose1); + Matrix H1_Vel = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, this, _1, Vel1, Bias1, Pose2, Vel2), Pose1); + *H1 = stack(2, &H1_Pose, &H1_Vel); + } - // Jacobian w.r.t. Vel1 - if (H2){ - Matrix H2_Pose = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, this, Pose1, _1, Bias1, Pose2, Vel2), Vel1); - Matrix H2_Vel = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, this, Pose1, _1, Bias1, Pose2, Vel2), Vel1); - *H2 = stack(2, &H2_Pose, &H2_Vel); - } + // Jacobian w.r.t. Vel1 + if (H2){ + Matrix H2_Pose = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, this, Pose1, _1, Bias1, Pose2, Vel2), Vel1); + Matrix H2_Vel = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, this, Pose1, _1, Bias1, Pose2, Vel2), Vel1); + *H2 = stack(2, &H2_Pose, &H2_Vel); + } - // Jacobian w.r.t. IMUBias1 - if (H3){ - Matrix H3_Pose = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, this, Pose1, Vel1, _1, Pose2, Vel2), Bias1); - Matrix H3_Vel = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, this, Pose1, Vel1, _1, Pose2, Vel2), Bias1); - *H3 = stack(2, &H3_Pose, &H3_Vel); - } + // Jacobian w.r.t. IMUBias1 + if (H3){ + Matrix H3_Pose = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, this, Pose1, Vel1, _1, Pose2, Vel2), Bias1); + Matrix H3_Vel = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, this, Pose1, Vel1, _1, Pose2, Vel2), Bias1); + *H3 = stack(2, &H3_Pose, &H3_Vel); + } - // Jacobian w.r.t. Pose2 - if (H4){ - Matrix H4_Pose = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, this, Pose1, Vel1, Bias1, _1, Vel2), Pose2); - Matrix H4_Vel = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, this, Pose1, Vel1, Bias1, _1, Vel2), Pose2); - *H4 = stack(2, &H4_Pose, &H4_Vel); - } + // Jacobian w.r.t. Pose2 + if (H4){ + Matrix H4_Pose = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, this, Pose1, Vel1, Bias1, _1, Vel2), Pose2); + Matrix H4_Vel = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, this, Pose1, Vel1, Bias1, _1, Vel2), Pose2); + *H4 = stack(2, &H4_Pose, &H4_Vel); + } - // Jacobian w.r.t. Vel2 - if (H5){ - Matrix H5_Pose = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, this, Pose1, Vel1, Bias1, Pose2, _1), Vel2); - Matrix H5_Vel = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, this, Pose1, Vel1, Bias1, Pose2, _1), Vel2); - *H5 = stack(2, &H5_Pose, &H5_Vel); - } + // Jacobian w.r.t. Vel2 + if (H5){ + Matrix H5_Pose = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, this, Pose1, Vel1, Bias1, Pose2, _1), Vel2); + Matrix H5_Vel = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, this, Pose1, Vel1, Bias1, Pose2, _1), Vel2); + *H5 = stack(2, &H5_Pose, &H5_Vel); + } - Vector ErrPoseVector(POSE::Logmap(evaluatePoseError(Pose1, Vel1, Bias1, Pose2, Vel2))); - Vector ErrVelVector(VELOCITY::Logmap(evaluateVelocityError(Pose1, Vel1, Bias1, Pose2, Vel2))); + Vector ErrPoseVector(POSE::Logmap(evaluatePoseError(Pose1, Vel1, Bias1, Pose2, Vel2))); + Vector ErrVelVector(VELOCITY::Logmap(evaluateVelocityError(Pose1, Vel1, Bias1, Pose2, Vel2))); - return concatVectors(2, &ErrPoseVector, &ErrVelVector); - } + return concatVectors(2, &ErrPoseVector, &ErrVelVector); + } @@ -413,137 +413,137 @@ public: static inline void PreIntegrateIMUObservations(const Vector& msr_acc_t, const Vector& msr_gyro_t, const double msr_dt, Vector& delta_pos_in_t0, Vector3& delta_angles, Vector& delta_vel_in_t0, double& delta_t, - const noiseModel::Gaussian::shared_ptr& model_continuous_overall, - Matrix& EquivCov_Overall, Matrix& Jacobian_wrt_t0_Overall, const IMUBIAS Bias_t0 = IMUBIAS(), - boost::optional p_body_P_sensor = boost::none){ - // Note: all delta terms refer to an IMU\sensor system at t0 - // Note: Earth-related terms are not accounted here but are incorporated in predict functions. + const noiseModel::Gaussian::shared_ptr& model_continuous_overall, + Matrix& EquivCov_Overall, Matrix& Jacobian_wrt_t0_Overall, const IMUBIAS Bias_t0 = IMUBIAS(), + boost::optional p_body_P_sensor = boost::none){ + // Note: all delta terms refer to an IMU\sensor system at t0 + // Note: Earth-related terms are not accounted here but are incorporated in predict functions. - POSE body_P_sensor = POSE(); - bool flag_use_body_P_sensor = false; - if (p_body_P_sensor){ - body_P_sensor = *p_body_P_sensor; - flag_use_body_P_sensor = true; - } + POSE body_P_sensor = POSE(); + bool flag_use_body_P_sensor = false; + if (p_body_P_sensor){ + body_P_sensor = *p_body_P_sensor; + flag_use_body_P_sensor = true; + } - delta_pos_in_t0 = PreIntegrateIMUObservations_delta_pos(msr_dt, delta_pos_in_t0, delta_vel_in_t0); - delta_vel_in_t0 = PreIntegrateIMUObservations_delta_vel(msr_gyro_t, msr_acc_t, msr_dt, delta_angles, delta_vel_in_t0, flag_use_body_P_sensor, body_P_sensor, Bias_t0); - delta_angles = PreIntegrateIMUObservations_delta_angles(msr_gyro_t, msr_dt, delta_angles, flag_use_body_P_sensor, body_P_sensor, Bias_t0); + delta_pos_in_t0 = PreIntegrateIMUObservations_delta_pos(msr_dt, delta_pos_in_t0, delta_vel_in_t0); + delta_vel_in_t0 = PreIntegrateIMUObservations_delta_vel(msr_gyro_t, msr_acc_t, msr_dt, delta_angles, delta_vel_in_t0, flag_use_body_P_sensor, body_P_sensor, Bias_t0); + delta_angles = PreIntegrateIMUObservations_delta_angles(msr_gyro_t, msr_dt, delta_angles, flag_use_body_P_sensor, body_P_sensor, Bias_t0); - delta_t += msr_dt; + delta_t += msr_dt; - // Update EquivCov_Overall - Matrix Z_3x3 = zeros(3,3); - Matrix I_3x3 = eye(3,3); + // Update EquivCov_Overall + Matrix Z_3x3 = zeros(3,3); + Matrix I_3x3 = eye(3,3); - Matrix H_pos_pos = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, _1, delta_vel_in_t0), delta_pos_in_t0); - Matrix H_pos_vel = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, delta_pos_in_t0, _1), delta_vel_in_t0); - Matrix H_pos_angles = Z_3x3; - Matrix H_pos_bias = collect(2, &Z_3x3, &Z_3x3); + Matrix H_pos_pos = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, _1, delta_vel_in_t0), delta_pos_in_t0); + Matrix H_pos_vel = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, delta_pos_in_t0, _1), delta_vel_in_t0); + Matrix H_pos_angles = Z_3x3; + Matrix H_pos_bias = collect(2, &Z_3x3, &Z_3x3); - Matrix H_vel_vel = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, delta_angles, _1, flag_use_body_P_sensor, body_P_sensor, Bias_t0), delta_vel_in_t0); - Matrix H_vel_angles = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, _1, delta_vel_in_t0, flag_use_body_P_sensor, body_P_sensor, Bias_t0), delta_angles); - Matrix H_vel_bias = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, delta_angles, delta_vel_in_t0, flag_use_body_P_sensor, body_P_sensor, _1), Bias_t0); - Matrix H_vel_pos = Z_3x3; + Matrix H_vel_vel = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, delta_angles, _1, flag_use_body_P_sensor, body_P_sensor, Bias_t0), delta_vel_in_t0); + Matrix H_vel_angles = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, _1, delta_vel_in_t0, flag_use_body_P_sensor, body_P_sensor, Bias_t0), delta_angles); + Matrix H_vel_bias = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, delta_angles, delta_vel_in_t0, flag_use_body_P_sensor, body_P_sensor, _1), Bias_t0); + Matrix H_vel_pos = Z_3x3; - Matrix H_angles_angles = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_angles, msr_gyro_t, msr_dt, _1, flag_use_body_P_sensor, body_P_sensor, Bias_t0), delta_angles); - Matrix H_angles_bias = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_angles, msr_gyro_t, msr_dt, delta_angles, flag_use_body_P_sensor, body_P_sensor, _1), Bias_t0); - Matrix H_angles_pos = Z_3x3; - Matrix H_angles_vel = Z_3x3; + Matrix H_angles_angles = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_angles, msr_gyro_t, msr_dt, _1, flag_use_body_P_sensor, body_P_sensor, Bias_t0), delta_angles); + Matrix H_angles_bias = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_angles, msr_gyro_t, msr_dt, delta_angles, flag_use_body_P_sensor, body_P_sensor, _1), Bias_t0); + Matrix H_angles_pos = Z_3x3; + Matrix H_angles_vel = Z_3x3; - Matrix F_angles = collect(4, &H_angles_angles, &H_angles_pos, &H_angles_vel, &H_angles_bias); - Matrix F_pos = collect(4, &H_pos_angles, &H_pos_pos, &H_pos_vel, &H_pos_bias); - Matrix F_vel = collect(4, &H_vel_angles, &H_vel_pos, &H_vel_vel, &H_vel_bias); - Matrix F_bias_a = collect(5, &Z_3x3, &Z_3x3, &Z_3x3, &I_3x3, &Z_3x3); - Matrix F_bias_g = collect(5, &Z_3x3, &Z_3x3, &Z_3x3, &Z_3x3, &I_3x3); - Matrix F = stack(5, &F_angles, &F_pos, &F_vel, &F_bias_a, &F_bias_g); + Matrix F_angles = collect(4, &H_angles_angles, &H_angles_pos, &H_angles_vel, &H_angles_bias); + Matrix F_pos = collect(4, &H_pos_angles, &H_pos_pos, &H_pos_vel, &H_pos_bias); + Matrix F_vel = collect(4, &H_vel_angles, &H_vel_pos, &H_vel_vel, &H_vel_bias); + Matrix F_bias_a = collect(5, &Z_3x3, &Z_3x3, &Z_3x3, &I_3x3, &Z_3x3); + Matrix F_bias_g = collect(5, &Z_3x3, &Z_3x3, &Z_3x3, &Z_3x3, &I_3x3); + Matrix F = stack(5, &F_angles, &F_pos, &F_vel, &F_bias_a, &F_bias_g); - noiseModel::Gaussian::shared_ptr model_discrete_curr = calc_descrete_noise_model(model_continuous_overall, msr_dt ); - Matrix Q_d = inverse(model_discrete_curr->R().transpose() * model_discrete_curr->R() ); + noiseModel::Gaussian::shared_ptr model_discrete_curr = calc_descrete_noise_model(model_continuous_overall, msr_dt ); + Matrix Q_d = inverse(model_discrete_curr->R().transpose() * model_discrete_curr->R() ); - EquivCov_Overall = F * EquivCov_Overall * F.transpose() + Q_d; - // Luca: force identity covariance matrix (for testing purposes) - // EquivCov_Overall = Matrix::Identity(15,15); + EquivCov_Overall = F * EquivCov_Overall * F.transpose() + Q_d; + // Luca: force identity covariance matrix (for testing purposes) + // EquivCov_Overall = Matrix::Identity(15,15); - // Update Jacobian_wrt_t0_Overall - Jacobian_wrt_t0_Overall = F * Jacobian_wrt_t0_Overall; - } + // Update Jacobian_wrt_t0_Overall + Jacobian_wrt_t0_Overall = F * Jacobian_wrt_t0_Overall; + } - static inline Vector PreIntegrateIMUObservations_delta_pos(const double msr_dt, - const Vector& delta_pos_in_t0, const Vector& delta_vel_in_t0){ + static inline Vector PreIntegrateIMUObservations_delta_pos(const double msr_dt, + const Vector& delta_pos_in_t0, const Vector& delta_vel_in_t0){ - // Note: all delta terms refer to an IMU\sensor system at t0 - // Note: delta_vel_in_t0 is already in body frame, so no need to use the body_P_sensor transformation here. + // Note: all delta terms refer to an IMU\sensor system at t0 + // Note: delta_vel_in_t0 is already in body frame, so no need to use the body_P_sensor transformation here. - return delta_pos_in_t0 + delta_vel_in_t0 * msr_dt; - } + return delta_pos_in_t0 + delta_vel_in_t0 * msr_dt; + } - static inline Vector PreIntegrateIMUObservations_delta_vel(const Vector& msr_gyro_t, const Vector& msr_acc_t, const double msr_dt, - const Vector3& delta_angles, const Vector& delta_vel_in_t0, const bool flag_use_body_P_sensor, const POSE& body_P_sensor, - IMUBIAS Bias_t0 = IMUBIAS()){ + static inline Vector PreIntegrateIMUObservations_delta_vel(const Vector& msr_gyro_t, const Vector& msr_acc_t, const double msr_dt, + const Vector3& delta_angles, const Vector& delta_vel_in_t0, const bool flag_use_body_P_sensor, const POSE& body_P_sensor, + IMUBIAS Bias_t0 = IMUBIAS()){ - // Note: all delta terms refer to an IMU\sensor system at t0 + // Note: all delta terms refer to an IMU\sensor system at t0 - // Calculate the corrected measurements using the Bias object - Vector AccCorrected = Bias_t0.correctAccelerometer(msr_acc_t); - Vector body_t_a_body; - if (flag_use_body_P_sensor){ - Matrix body_R_sensor = body_P_sensor.rotation().matrix(); + // Calculate the corrected measurements using the Bias object + Vector AccCorrected = Bias_t0.correctAccelerometer(msr_acc_t); + Vector body_t_a_body; + if (flag_use_body_P_sensor){ + Matrix body_R_sensor = body_P_sensor.rotation().matrix(); - Vector GyroCorrected(Bias_t0.correctGyroscope(msr_gyro_t)); + Vector GyroCorrected(Bias_t0.correctGyroscope(msr_gyro_t)); - Vector body_omega_body = body_R_sensor * GyroCorrected; - Matrix body_omega_body__cross = skewSymmetric(body_omega_body); + Vector body_omega_body = body_R_sensor * GyroCorrected; + Matrix body_omega_body__cross = skewSymmetric(body_omega_body); - body_t_a_body = body_R_sensor * AccCorrected - body_omega_body__cross * body_omega_body__cross * body_P_sensor.translation().vector(); - } else{ - body_t_a_body = AccCorrected; - } + body_t_a_body = body_R_sensor * AccCorrected - body_omega_body__cross * body_omega_body__cross * body_P_sensor.translation().vector(); + } else{ + body_t_a_body = AccCorrected; + } - Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles); + Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles); - return delta_vel_in_t0 + R_t_to_t0.matrix() * body_t_a_body * msr_dt; - } + return delta_vel_in_t0 + R_t_to_t0.matrix() * body_t_a_body * msr_dt; + } - static inline Vector PreIntegrateIMUObservations_delta_angles(const Vector& msr_gyro_t, const double msr_dt, - const Vector3& delta_angles, const bool flag_use_body_P_sensor, const POSE& body_P_sensor, - IMUBIAS Bias_t0 = IMUBIAS()){ + static inline Vector PreIntegrateIMUObservations_delta_angles(const Vector& msr_gyro_t, const double msr_dt, + const Vector3& delta_angles, const bool flag_use_body_P_sensor, const POSE& body_P_sensor, + IMUBIAS Bias_t0 = IMUBIAS()){ - // Note: all delta terms refer to an IMU\sensor system at t0 + // Note: all delta terms refer to an IMU\sensor system at t0 - // Calculate the corrected measurements using the Bias object - Vector GyroCorrected = Bias_t0.correctGyroscope(msr_gyro_t); + // Calculate the corrected measurements using the Bias object + Vector GyroCorrected = Bias_t0.correctGyroscope(msr_gyro_t); - Vector body_t_omega_body; - if (flag_use_body_P_sensor){ - body_t_omega_body = body_P_sensor.rotation().matrix() * GyroCorrected; - } else { - body_t_omega_body = GyroCorrected; - } + Vector body_t_omega_body; + if (flag_use_body_P_sensor){ + body_t_omega_body = body_P_sensor.rotation().matrix() * GyroCorrected; + } else { + body_t_omega_body = GyroCorrected; + } - Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles); + Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles); - R_t_to_t0 = R_t_to_t0 * Rot3::Expmap( body_t_omega_body*msr_dt ); - return Rot3::Logmap(R_t_to_t0); - } + R_t_to_t0 = R_t_to_t0 * Rot3::Expmap( body_t_omega_body*msr_dt ); + return Rot3::Logmap(R_t_to_t0); + } - static inline noiseModel::Gaussian::shared_ptr CalcEquivalentNoiseCov(const noiseModel::Gaussian::shared_ptr& gaussian_acc, const noiseModel::Gaussian::shared_ptr& gaussian_gyro, - const noiseModel::Gaussian::shared_ptr& gaussian_process){ + static inline noiseModel::Gaussian::shared_ptr CalcEquivalentNoiseCov(const noiseModel::Gaussian::shared_ptr& gaussian_acc, const noiseModel::Gaussian::shared_ptr& gaussian_gyro, + const noiseModel::Gaussian::shared_ptr& gaussian_process){ - Matrix cov_acc = inverse( gaussian_acc->R().transpose() * gaussian_acc->R() ); - Matrix cov_gyro = inverse( gaussian_gyro->R().transpose() * gaussian_gyro->R() ); - Matrix cov_process = inverse( gaussian_process->R().transpose() * gaussian_process->R() ); + Matrix cov_acc = inverse( gaussian_acc->R().transpose() * gaussian_acc->R() ); + Matrix cov_gyro = inverse( gaussian_gyro->R().transpose() * gaussian_gyro->R() ); + Matrix cov_process = inverse( gaussian_process->R().transpose() * gaussian_process->R() ); - cov_process.block(0,0, 3,3) += cov_gyro; - cov_process.block(6,6, 3,3) += cov_acc; + cov_process.block(0,0, 3,3) += cov_gyro; + cov_process.block(6,6, 3,3) += cov_acc; - return noiseModel::Gaussian::Covariance(cov_process); - } + return noiseModel::Gaussian::Covariance(cov_process); + } static inline void CalcEquivalentNoiseCov_DifferentParts(const noiseModel::Gaussian::shared_ptr& gaussian_acc, const noiseModel::Gaussian::shared_ptr& gaussian_gyro, const noiseModel::Gaussian::shared_ptr& gaussian_process, @@ -554,107 +554,107 @@ public: cov_process_without_acc_gyro = inverse( gaussian_process->R().transpose() * gaussian_process->R() ); } - static inline void Calc_g_rho_omega_earth_NED(const Vector& Pos_NED, const Vector& Vel_NED, const Vector& LatLonHeight_IC, const Vector& Pos_NED_Initial, - Vector& g_NED, Vector& rho_NED, Vector& omega_earth_NED) { + static inline void Calc_g_rho_omega_earth_NED(const Vector& Pos_NED, const Vector& Vel_NED, const Vector& LatLonHeight_IC, const Vector& Pos_NED_Initial, + Vector& g_NED, Vector& rho_NED, Vector& omega_earth_NED) { - Matrix ENU_to_NED = Matrix_(3, 3, - 0.0, 1.0, 0.0, - 1.0, 0.0, 0.0, - 0.0, 0.0, -1.0); + Matrix ENU_to_NED = Matrix_(3, 3, + 0.0, 1.0, 0.0, + 1.0, 0.0, 0.0, + 0.0, 0.0, -1.0); - Matrix NED_to_ENU = Matrix_(3, 3, - 0.0, 1.0, 0.0, - 1.0, 0.0, 0.0, - 0.0, 0.0, -1.0); + Matrix NED_to_ENU = Matrix_(3, 3, + 0.0, 1.0, 0.0, + 1.0, 0.0, 0.0, + 0.0, 0.0, -1.0); - // Convert incoming parameters to ENU - Vector Pos_ENU = NED_to_ENU * Pos_NED; - Vector Vel_ENU = NED_to_ENU * Vel_NED; - Vector Pos_ENU_Initial = NED_to_ENU * Pos_NED_Initial; + // Convert incoming parameters to ENU + Vector Pos_ENU = NED_to_ENU * Pos_NED; + Vector Vel_ENU = NED_to_ENU * Vel_NED; + Vector Pos_ENU_Initial = NED_to_ENU * Pos_NED_Initial; - // Call ENU version - Vector g_ENU; - Vector rho_ENU; - Vector omega_earth_ENU; - Calc_g_rho_omega_earth_ENU(Pos_ENU, Vel_ENU, LatLonHeight_IC, Pos_ENU_Initial, g_ENU, rho_ENU, omega_earth_ENU); + // Call ENU version + Vector g_ENU; + Vector rho_ENU; + Vector omega_earth_ENU; + Calc_g_rho_omega_earth_ENU(Pos_ENU, Vel_ENU, LatLonHeight_IC, Pos_ENU_Initial, g_ENU, rho_ENU, omega_earth_ENU); - // Convert output to NED - g_NED = ENU_to_NED * g_ENU; - rho_NED = ENU_to_NED * rho_ENU; - omega_earth_NED = ENU_to_NED * omega_earth_ENU; - } + // Convert output to NED + g_NED = ENU_to_NED * g_ENU; + rho_NED = ENU_to_NED * rho_ENU; + omega_earth_NED = ENU_to_NED * omega_earth_ENU; + } - static inline void Calc_g_rho_omega_earth_ENU(const Vector& Pos_ENU, const Vector& Vel_ENU, const Vector& LatLonHeight_IC, const Vector& Pos_ENU_Initial, - Vector& g_ENU, Vector& rho_ENU, Vector& omega_earth_ENU){ - double R0 = 6.378388e6; - double e = 1/297; - double Re( R0*( 1-e*(sin( LatLonHeight_IC(0) ))*(sin( LatLonHeight_IC(0) )) ) ); + static inline void Calc_g_rho_omega_earth_ENU(const Vector& Pos_ENU, const Vector& Vel_ENU, const Vector& LatLonHeight_IC, const Vector& Pos_ENU_Initial, + Vector& g_ENU, Vector& rho_ENU, Vector& omega_earth_ENU){ + double R0 = 6.378388e6; + double e = 1/297; + double Re( R0*( 1-e*(sin( LatLonHeight_IC(0) ))*(sin( LatLonHeight_IC(0) )) ) ); - // Calculate current lat, lon - Vector delta_Pos_ENU(Pos_ENU - Pos_ENU_Initial); - double delta_lat(delta_Pos_ENU(1)/Re); - double delta_lon(delta_Pos_ENU(0)/(Re*cos(LatLonHeight_IC(0)))); - double lat_new(LatLonHeight_IC(0) + delta_lat); - double lon_new(LatLonHeight_IC(1) + delta_lon); + // Calculate current lat, lon + Vector delta_Pos_ENU(Pos_ENU - Pos_ENU_Initial); + double delta_lat(delta_Pos_ENU(1)/Re); + double delta_lon(delta_Pos_ENU(0)/(Re*cos(LatLonHeight_IC(0)))); + double lat_new(LatLonHeight_IC(0) + delta_lat); + double lon_new(LatLonHeight_IC(1) + delta_lon); - // Rotation of lon about z axis - Rot3 C1(cos(lon_new), sin(lon_new), 0.0, - -sin(lon_new), cos(lon_new), 0.0, - 0.0, 0.0, 1.0); + // Rotation of lon about z axis + Rot3 C1(cos(lon_new), sin(lon_new), 0.0, + -sin(lon_new), cos(lon_new), 0.0, + 0.0, 0.0, 1.0); - // Rotation of lat about y axis - Rot3 C2(cos(lat_new), 0.0, sin(lat_new), - 0.0, 1.0, 0.0, - -sin(lat_new), 0.0, cos(lat_new)); + // Rotation of lat about y axis + Rot3 C2(cos(lat_new), 0.0, sin(lat_new), + 0.0, 1.0, 0.0, + -sin(lat_new), 0.0, cos(lat_new)); - Rot3 UEN_to_ENU(0, 1, 0, - 0, 0, 1, - 1, 0, 0); + Rot3 UEN_to_ENU(0, 1, 0, + 0, 0, 1, + 1, 0, 0); - Rot3 R_ECEF_to_ENU( UEN_to_ENU * C2 * C1 ); + Rot3 R_ECEF_to_ENU( UEN_to_ENU * C2 * C1 ); - Vector omega_earth_ECEF(Vector_(3, 0.0, 0.0, 7.292115e-5)); - omega_earth_ENU = R_ECEF_to_ENU.matrix() * omega_earth_ECEF; + Vector omega_earth_ECEF(Vector_(3, 0.0, 0.0, 7.292115e-5)); + omega_earth_ENU = R_ECEF_to_ENU.matrix() * omega_earth_ECEF; - // Calculating g - double height(LatLonHeight_IC(2)); - double EQUA_RADIUS = 6378137.0; // equatorial radius of the earth; WGS-84 - double ECCENTRICITY = 0.0818191908426; // eccentricity of the earth ellipsoid - double e2( pow(ECCENTRICITY,2) ); - double den( 1-e2*pow(sin(lat_new),2) ); - double Rm( (EQUA_RADIUS*(1-e2))/( pow(den,(3/2)) ) ); - double Rp( EQUA_RADIUS/( sqrt(den) ) ); - double Ro( sqrt(Rp*Rm) ); // mean earth radius of curvature - double g0( 9.780318*( 1 + 5.3024e-3 * pow(sin(lat_new),2) - 5.9e-6 * pow(sin(2*lat_new),2) ) ); - double g_calc( g0/( pow(1 + height/Ro, 2) ) ); - g_ENU = Vector_(3, 0.0, 0.0, -g_calc); + // Calculating g + double height(LatLonHeight_IC(2)); + double EQUA_RADIUS = 6378137.0; // equatorial radius of the earth; WGS-84 + double ECCENTRICITY = 0.0818191908426; // eccentricity of the earth ellipsoid + double e2( pow(ECCENTRICITY,2) ); + double den( 1-e2*pow(sin(lat_new),2) ); + double Rm( (EQUA_RADIUS*(1-e2))/( pow(den,(3/2)) ) ); + double Rp( EQUA_RADIUS/( sqrt(den) ) ); + double Ro( sqrt(Rp*Rm) ); // mean earth radius of curvature + double g0( 9.780318*( 1 + 5.3024e-3 * pow(sin(lat_new),2) - 5.9e-6 * pow(sin(2*lat_new),2) ) ); + double g_calc( g0/( pow(1 + height/Ro, 2) ) ); + g_ENU = Vector_(3, 0.0, 0.0, -g_calc); - // Calculate rho - double Ve( Vel_ENU(0) ); - double Vn( Vel_ENU(1) ); - double rho_E = -Vn/(Rm + height); - double rho_N = Ve/(Rp + height); - double rho_U = Ve*tan(lat_new)/(Rp + height); - rho_ENU = Vector_(3, rho_E, rho_N, rho_U); - } + // Calculate rho + double Ve( Vel_ENU(0) ); + double Vn( Vel_ENU(1) ); + double rho_E = -Vn/(Rm + height); + double rho_N = Ve/(Rp + height); + double rho_U = Ve*tan(lat_new)/(Rp + height); + rho_ENU = Vector_(3, rho_E, rho_N, rho_U); + } - static inline noiseModel::Gaussian::shared_ptr calc_descrete_noise_model(const noiseModel::Gaussian::shared_ptr& model, double delta_t){ - /* Q_d (approx)= Q * delta_t */ - /* In practice, square root of the information matrix is represented, so that: - * R_d (approx)= R / sqrt(delta_t) - * */ - return noiseModel::Gaussian::SqrtInformation(model->R()/sqrt(delta_t)); - } + static inline noiseModel::Gaussian::shared_ptr calc_descrete_noise_model(const noiseModel::Gaussian::shared_ptr& model, double delta_t){ + /* Q_d (approx)= Q * delta_t */ + /* In practice, square root of the information matrix is represented, so that: + * R_d (approx)= R / sqrt(delta_t) + * */ + return noiseModel::Gaussian::SqrtInformation(model->R()/sqrt(delta_t)); + } private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("NonlinearFactor2", - boost::serialization::base_object(*this)); - } + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("NonlinearFactor2", + boost::serialization::base_object(*this)); + } diff --git a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h index 2858b091e..0b7848325 100644 --- a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h +++ b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h @@ -39,8 +39,8 @@ namespace gtsam { * ===== * Concept: Based on [Lupton12tro] * - Pre-integrate IMU measurements using the static function PreIntegrateIMUObservations. - * Pre-integrated quantities are expressed in the body system of t0 - the first time instant (in which pre-integration began). - * All sensor-to-body transformations are performed here. + * Pre-integrated quantities are expressed in the body system of t0 - the first time instant (in which pre-integration began). + * All sensor-to-body transformations are performed here. * - If required, calculate inertial solution by calling the static functions: predictPose_inertial, predictVelocity_inertial. * - When the time is right, incorporate pre-integrated IMU data by creating an EquivInertialNavFactor_GlobalVel_NoBias factor, which will * relate between navigation variables at the two time instances (t0 and current time). @@ -54,11 +54,11 @@ namespace gtsam { * matrices and the process\modeling covariance matrix. The IneritalNavFactor converts this into a * discrete form using the supplied delta_t between sub-sequential measurements. * - Earth-rate correction: - * + Currently the user should supply R_ECEF_to_G, which is the rotation from ECEF to the global - * frame (Local-Level system: ENU or NED, see above). - * + R_ECEF_to_G can be calculated by approximated values of latitude and longitude of the system. - * + Currently it is assumed that a relatively small distance is traveled w.r.t. to initial pose, since R_ECEF_to_G is constant. - * Otherwise, R_ECEF_to_G should be updated each time using the current lat-lon. + * + Currently the user should supply R_ECEF_to_G, which is the rotation from ECEF to the global + * frame (Local-Level system: ENU or NED, see above). + * + R_ECEF_to_G can be calculated by approximated values of latitude and longitude of the system. + * + Currently it is assumed that a relatively small distance is traveled w.r.t. to initial pose, since R_ECEF_to_G is constant. + * Otherwise, R_ECEF_to_G should be updated each time using the current lat-lon. * * - Frame Notation: * Quantities are written as {Frame of Representation/Destination Frame}_{Quantity Type}_{Quatity Description/Origination Frame} @@ -92,222 +92,222 @@ class EquivInertialNavFactor_GlobalVel_NoBias : public NoiseModelFactor4 This; - typedef NoiseModelFactor4 Base; + typedef EquivInertialNavFactor_GlobalVel_NoBias This; + typedef NoiseModelFactor4 Base; - Vector delta_pos_in_t0_; - Vector delta_vel_in_t0_; - Vector3 delta_angles_; - double dt12_; + Vector delta_pos_in_t0_; + Vector delta_vel_in_t0_; + Vector3 delta_angles_; + double dt12_; - Vector world_g_; - Vector world_rho_; - Vector world_omega_earth_; + Vector world_g_; + Vector world_rho_; + Vector world_omega_earth_; - Matrix Jacobian_wrt_t0_Overall_; + Matrix Jacobian_wrt_t0_Overall_; - 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 public: - // shorthand for a smart pointer to a factor - typedef typename boost::shared_ptr shared_ptr; + // shorthand for a smart pointer to a factor + typedef typename boost::shared_ptr shared_ptr; - /** default constructor - only use for serialization */ - EquivInertialNavFactor_GlobalVel_NoBias() {} + /** default constructor - only use for serialization */ + EquivInertialNavFactor_GlobalVel_NoBias() {} - /** Constructor */ - EquivInertialNavFactor_GlobalVel_NoBias(const Key& Pose1, const Key& Vel1, const Key& Pose2, const Key& Vel2, - const Vector& delta_pos_in_t0, const Vector& delta_vel_in_t0, const Vector3& delta_angles, - double dt12, const Vector world_g, const Vector world_rho, - const Vector& world_omega_earth, const noiseModel::Gaussian::shared_ptr& model_equivalent, - const Matrix& Jacobian_wrt_t0_Overall, - boost::optional body_P_sensor = boost::none) : - Base(model_equivalent, Pose1, Vel1, Pose2, Vel2), - delta_pos_in_t0_(delta_pos_in_t0), delta_vel_in_t0_(delta_vel_in_t0), delta_angles_(delta_angles), - dt12_(dt12), world_g_(world_g), world_rho_(world_rho), world_omega_earth_(world_omega_earth), Jacobian_wrt_t0_Overall_(Jacobian_wrt_t0_Overall), - body_P_sensor_(body_P_sensor) { } + /** Constructor */ + EquivInertialNavFactor_GlobalVel_NoBias(const Key& Pose1, const Key& Vel1, const Key& Pose2, const Key& Vel2, + const Vector& delta_pos_in_t0, const Vector& delta_vel_in_t0, const Vector3& delta_angles, + double dt12, const Vector world_g, const Vector world_rho, + const Vector& world_omega_earth, const noiseModel::Gaussian::shared_ptr& model_equivalent, + const Matrix& Jacobian_wrt_t0_Overall, + boost::optional body_P_sensor = boost::none) : + Base(model_equivalent, Pose1, Vel1, Pose2, Vel2), + delta_pos_in_t0_(delta_pos_in_t0), delta_vel_in_t0_(delta_vel_in_t0), delta_angles_(delta_angles), + dt12_(dt12), world_g_(world_g), world_rho_(world_rho), world_omega_earth_(world_omega_earth), Jacobian_wrt_t0_Overall_(Jacobian_wrt_t0_Overall), + body_P_sensor_(body_P_sensor) { } - virtual ~EquivInertialNavFactor_GlobalVel_NoBias() {} + virtual ~EquivInertialNavFactor_GlobalVel_NoBias() {} - /** implement functions needed for Testable */ + /** implement functions needed for Testable */ - /** print */ - virtual void print(const std::string& s = "EquivInertialNavFactor_GlobalVel_NoBias", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { - std::cout << s << "(" - << keyFormatter(this->key1()) << "," - << keyFormatter(this->key2()) << "," - << keyFormatter(this->key3()) << "," - << keyFormatter(this->key4()) << "\n"; - std::cout << "delta_pos_in_t0: " << this->delta_pos_in_t0_.transpose() << std::endl; - std::cout << "delta_vel_in_t0: " << this->delta_vel_in_t0_.transpose() << std::endl; - std::cout << "delta_angles: " << this->delta_angles_ << std::endl; - std::cout << "dt12: " << this->dt12_ << std::endl; - std::cout << "gravity (in world frame): " << this->world_g_.transpose() << std::endl; - std::cout << "craft rate (in world frame): " << this->world_rho_.transpose() << std::endl; - std::cout << "earth's rotation (in world frame): " << this->world_omega_earth_.transpose() << std::endl; - if(this->body_P_sensor_) - this->body_P_sensor_->print(" sensor pose in body frame: "); - this->noiseModel_->print(" noise model"); - } + /** print */ + virtual void print(const std::string& s = "EquivInertialNavFactor_GlobalVel_NoBias", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + std::cout << s << "(" + << keyFormatter(this->key1()) << "," + << keyFormatter(this->key2()) << "," + << keyFormatter(this->key3()) << "," + << keyFormatter(this->key4()) << "\n"; + std::cout << "delta_pos_in_t0: " << this->delta_pos_in_t0_.transpose() << std::endl; + std::cout << "delta_vel_in_t0: " << this->delta_vel_in_t0_.transpose() << std::endl; + std::cout << "delta_angles: " << this->delta_angles_ << std::endl; + std::cout << "dt12: " << this->dt12_ << std::endl; + std::cout << "gravity (in world frame): " << this->world_g_.transpose() << std::endl; + std::cout << "craft rate (in world frame): " << this->world_rho_.transpose() << std::endl; + std::cout << "earth's rotation (in world frame): " << this->world_omega_earth_.transpose() << std::endl; + if(this->body_P_sensor_) + this->body_P_sensor_->print(" sensor pose in body frame: "); + this->noiseModel_->print(" noise model"); + } - /** equals */ - virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { - const This *e = dynamic_cast (&expected); - return e != NULL && Base::equals(*e, tol) - && (delta_pos_in_t0_ - e->delta_pos_in_t0_).norm() < tol - && (delta_vel_in_t0_ - e->delta_vel_in_t0_).norm() < tol - && (delta_angles_ - e->delta_angles_).norm() < tol - && (dt12_ - e->dt12_) < tol - && (world_g_ - e->world_g_).norm() < tol - && (world_rho_ - e->world_rho_).norm() < tol - && (world_omega_earth_ - e->world_omega_earth_).norm() < tol - && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_))); - } + /** equals */ + virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { + const This *e = dynamic_cast (&expected); + return e != NULL && Base::equals(*e, tol) + && (delta_pos_in_t0_ - e->delta_pos_in_t0_).norm() < tol + && (delta_vel_in_t0_ - e->delta_vel_in_t0_).norm() < tol + && (delta_angles_ - e->delta_angles_).norm() < tol + && (dt12_ - e->dt12_) < tol + && (world_g_ - e->world_g_).norm() < tol + && (world_rho_ - e->world_rho_).norm() < tol + && (world_omega_earth_ - e->world_omega_earth_).norm() < tol + && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_))); + } - POSE predictPose(const POSE& Pose1, const VELOCITY& Vel1) const { + POSE predictPose(const POSE& Pose1, const VELOCITY& Vel1) const { - /* Position term */ - Vector delta_pos_in_t0_corrected = delta_pos_in_t0_; + /* Position term */ + Vector delta_pos_in_t0_corrected = delta_pos_in_t0_; - /* Rotation term */ - Vector delta_angles_corrected = delta_angles_; + /* Rotation term */ + Vector delta_angles_corrected = delta_angles_; - return predictPose_inertial(Pose1, Vel1, - delta_pos_in_t0_corrected, delta_angles_corrected, - dt12_, world_g_, world_rho_, world_omega_earth_); - } + return predictPose_inertial(Pose1, Vel1, + delta_pos_in_t0_corrected, delta_angles_corrected, + dt12_, world_g_, world_rho_, world_omega_earth_); + } - static inline POSE predictPose_inertial(const POSE& Pose1, const VELOCITY& Vel1, - const Vector& delta_pos_in_t0, const Vector3& delta_angles, - const double dt12, const Vector& world_g, const Vector& world_rho, const Vector& world_omega_earth){ + static inline POSE predictPose_inertial(const POSE& Pose1, const VELOCITY& Vel1, + const Vector& delta_pos_in_t0, const Vector3& delta_angles, + const double dt12, const Vector& world_g, const Vector& world_rho, const Vector& world_omega_earth){ - const POSE& world_P1_body = Pose1; - const VELOCITY& world_V1_body = Vel1; + const POSE& world_P1_body = Pose1; + const VELOCITY& world_V1_body = Vel1; - /* Position term */ - Vector body_deltaPos_body = delta_pos_in_t0; + /* Position term */ + Vector body_deltaPos_body = delta_pos_in_t0; - Vector world_deltaPos_pls_body = world_P1_body.rotation().matrix() * body_deltaPos_body; - Vector world_deltaPos_body = world_V1_body * dt12 + 0.5*world_g*dt12*dt12 + world_deltaPos_pls_body; + Vector world_deltaPos_pls_body = world_P1_body.rotation().matrix() * body_deltaPos_body; + Vector world_deltaPos_body = world_V1_body * dt12 + 0.5*world_g*dt12*dt12 + world_deltaPos_pls_body; - // Incorporate earth-related terms. Note - these are assumed to be constant between t1 and t2. - world_deltaPos_body -= 2*skewSymmetric(world_rho + world_omega_earth)*world_V1_body * dt12*dt12; + // Incorporate earth-related terms. Note - these are assumed to be constant between t1 and t2. + world_deltaPos_body -= 2*skewSymmetric(world_rho + world_omega_earth)*world_V1_body * dt12*dt12; - /* TODO: the term dt12*dt12 in 0.5*world_g*dt12*dt12 is not entirely correct: - * the gravity should be canceled from the accelerometer measurements, bust since position - * is added with a delta velocity from a previous term, the actual delta time is more complicated. - * Need to figure out this in the future - currently because of this issue we'll get some more error - * in Z axis. - */ + /* TODO: the term dt12*dt12 in 0.5*world_g*dt12*dt12 is not entirely correct: + * the gravity should be canceled from the accelerometer measurements, bust since position + * is added with a delta velocity from a previous term, the actual delta time is more complicated. + * Need to figure out this in the future - currently because of this issue we'll get some more error + * in Z axis. + */ - /* Rotation term */ - Vector body_deltaAngles_body = delta_angles; + /* Rotation term */ + Vector body_deltaAngles_body = delta_angles; - // Convert earth-related terms into the body frame - Matrix body_R_world(world_P1_body.rotation().inverse().matrix()); - Vector body_rho = body_R_world * world_rho; - Vector body_omega_earth = body_R_world * world_omega_earth; + // Convert earth-related terms into the body frame + Matrix body_R_world(world_P1_body.rotation().inverse().matrix()); + Vector body_rho = body_R_world * world_rho; + Vector body_omega_earth = body_R_world * world_omega_earth; - // Incorporate earth-related terms. Note - these are assumed to be constant between t1 and t2. - body_deltaAngles_body -= (body_rho + body_omega_earth)*dt12; + // Incorporate earth-related terms. Note - these are assumed to be constant between t1 and t2. + body_deltaAngles_body -= (body_rho + body_omega_earth)*dt12; - return POSE(Pose1.rotation() * POSE::Rotation::Expmap(body_deltaAngles_body), Pose1.translation() + typename POSE::Translation(world_deltaPos_body)); + return POSE(Pose1.rotation() * POSE::Rotation::Expmap(body_deltaAngles_body), Pose1.translation() + typename POSE::Translation(world_deltaPos_body)); - } + } - VELOCITY predictVelocity(const POSE& Pose1, const VELOCITY& Vel1) const { + VELOCITY predictVelocity(const POSE& Pose1, const VELOCITY& Vel1) const { - Vector delta_vel_in_t0_corrected = delta_vel_in_t0_; + Vector delta_vel_in_t0_corrected = delta_vel_in_t0_; - return predictVelocity_inertial(Pose1, Vel1, - delta_vel_in_t0_corrected, - dt12_, world_g_, world_rho_, world_omega_earth_); - } + return predictVelocity_inertial(Pose1, Vel1, + delta_vel_in_t0_corrected, + dt12_, world_g_, world_rho_, world_omega_earth_); + } - static inline VELOCITY predictVelocity_inertial(const POSE& Pose1, const VELOCITY& Vel1, - const Vector& delta_vel_in_t0, - const double dt12, const Vector& world_g, const Vector& world_rho, const Vector& world_omega_earth) { + static inline VELOCITY predictVelocity_inertial(const POSE& Pose1, const VELOCITY& Vel1, + const Vector& delta_vel_in_t0, + const double dt12, const Vector& world_g, const Vector& world_rho, const Vector& world_omega_earth) { - const POSE& world_P1_body = Pose1; - const VELOCITY& world_V1_body = Vel1; + const POSE& world_P1_body = Pose1; + const VELOCITY& world_V1_body = Vel1; - Vector body_deltaVel_body = delta_vel_in_t0; - Vector world_deltaVel_body = world_P1_body.rotation().matrix() * body_deltaVel_body; + Vector body_deltaVel_body = delta_vel_in_t0; + Vector world_deltaVel_body = world_P1_body.rotation().matrix() * body_deltaVel_body; - VELOCITY VelDelta( world_deltaVel_body + world_g * dt12 ); + VELOCITY VelDelta( world_deltaVel_body + world_g * dt12 ); - // Incorporate earth-related terms. Note - these are assumed to be constant between t1 and t2. - VelDelta -= 2*skewSymmetric(world_rho + world_omega_earth)*world_V1_body * dt12; + // Incorporate earth-related terms. Note - these are assumed to be constant between t1 and t2. + VelDelta -= 2*skewSymmetric(world_rho + world_omega_earth)*world_V1_body * dt12; - // Predict - return Vel1.compose( VelDelta ); + // Predict + return Vel1.compose( VelDelta ); - } + } - void predict(const POSE& Pose1, const VELOCITY& Vel1, POSE& Pose2, VELOCITY& Vel2) const { - Pose2 = predictPose(Pose1, Vel1); - Vel2 = predictVelocity(Pose1, Vel1); - } + void predict(const POSE& Pose1, const VELOCITY& Vel1, POSE& Pose2, VELOCITY& Vel2) const { + Pose2 = predictPose(Pose1, Vel1); + Vel2 = predictVelocity(Pose1, Vel1); + } - POSE evaluatePoseError(const POSE& Pose1, const VELOCITY& Vel1, const POSE& Pose2, const VELOCITY& Vel2) const { - // Predict - POSE Pose2Pred = predictPose(Pose1, Vel1); + POSE evaluatePoseError(const POSE& Pose1, const VELOCITY& Vel1, const POSE& Pose2, const VELOCITY& Vel2) const { + // Predict + POSE Pose2Pred = predictPose(Pose1, Vel1); - // Calculate error - return Pose2.between(Pose2Pred); - } + // Calculate error + return Pose2.between(Pose2Pred); + } - VELOCITY evaluateVelocityError(const POSE& Pose1, const VELOCITY& Vel1, const POSE& Pose2, const VELOCITY& Vel2) const { - // Predict - VELOCITY Vel2Pred = predictVelocity(Pose1, Vel1); + VELOCITY evaluateVelocityError(const POSE& Pose1, const VELOCITY& Vel1, const POSE& Pose2, const VELOCITY& Vel2) const { + // Predict + VELOCITY Vel2Pred = predictVelocity(Pose1, Vel1); - // Calculate error - return Vel2.between(Vel2Pred); - } + // Calculate error + return Vel2.between(Vel2Pred); + } - Vector evaluateError(const POSE& Pose1, const VELOCITY& Vel1, const POSE& Pose2, const VELOCITY& Vel2, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none, - boost::optional H3 = boost::none, - boost::optional H4 = boost::none) const { + Vector evaluateError(const POSE& Pose1, const VELOCITY& Vel1, const POSE& Pose2, const VELOCITY& Vel2, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none, + boost::optional H3 = boost::none, + boost::optional H4 = boost::none) const { - // TODO: Write analytical derivative calculations - // Jacobian w.r.t. Pose1 - if (H1){ - Matrix H1_Pose = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluatePoseError, this, _1, Vel1, Pose2, Vel2), Pose1); - Matrix H1_Vel = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluateVelocityError, this, _1, Vel1, Pose2, Vel2), Pose1); - *H1 = stack(2, &H1_Pose, &H1_Vel); - } + // TODO: Write analytical derivative calculations + // Jacobian w.r.t. Pose1 + if (H1){ + Matrix H1_Pose = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluatePoseError, this, _1, Vel1, Pose2, Vel2), Pose1); + Matrix H1_Vel = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluateVelocityError, this, _1, Vel1, Pose2, Vel2), Pose1); + *H1 = stack(2, &H1_Pose, &H1_Vel); + } - // Jacobian w.r.t. Vel1 - if (H2){ - Matrix H2_Pose = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluatePoseError, this, Pose1, _1, Pose2, Vel2), Vel1); - Matrix H2_Vel = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluateVelocityError, this, Pose1, _1, Pose2, Vel2), Vel1); - *H2 = stack(2, &H2_Pose, &H2_Vel); - } + // Jacobian w.r.t. Vel1 + if (H2){ + Matrix H2_Pose = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluatePoseError, this, Pose1, _1, Pose2, Vel2), Vel1); + Matrix H2_Vel = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluateVelocityError, this, Pose1, _1, Pose2, Vel2), Vel1); + *H2 = stack(2, &H2_Pose, &H2_Vel); + } - // Jacobian w.r.t. Pose2 - if (H3){ - Matrix H3_Pose = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluatePoseError, this, Pose1, Vel1, _1, Vel2), Pose2); - Matrix H3_Vel = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluateVelocityError, this, Pose1, Vel1, _1, Vel2), Pose2); - *H3 = stack(2, &H3_Pose, &H3_Vel); - } + // Jacobian w.r.t. Pose2 + if (H3){ + Matrix H3_Pose = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluatePoseError, this, Pose1, Vel1, _1, Vel2), Pose2); + Matrix H3_Vel = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluateVelocityError, this, Pose1, Vel1, _1, Vel2), Pose2); + *H3 = stack(2, &H3_Pose, &H3_Vel); + } - // Jacobian w.r.t. Vel2 - if (H4){ - Matrix H4_Pose = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluatePoseError, this, Pose1, Vel1, Pose2, _1), Vel2); - Matrix H4_Vel = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluateVelocityError, this, Pose1, Vel1, Pose2, _1), Vel2); - *H4 = stack(2, &H4_Pose, &H4_Vel); - } + // Jacobian w.r.t. Vel2 + if (H4){ + Matrix H4_Pose = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluatePoseError, this, Pose1, Vel1, Pose2, _1), Vel2); + Matrix H4_Vel = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluateVelocityError, this, Pose1, Vel1, Pose2, _1), Vel2); + *H4 = stack(2, &H4_Pose, &H4_Vel); + } - Vector ErrPoseVector(POSE::Logmap(evaluatePoseError(Pose1, Vel1, Pose2, Vel2))); - Vector ErrVelVector(VELOCITY::Logmap(evaluateVelocityError(Pose1, Vel1, Pose2, Vel2))); + Vector ErrPoseVector(POSE::Logmap(evaluatePoseError(Pose1, Vel1, Pose2, Vel2))); + Vector ErrVelVector(VELOCITY::Logmap(evaluateVelocityError(Pose1, Vel1, Pose2, Vel2))); - return concatVectors(2, &ErrPoseVector, &ErrVelVector); - } + return concatVectors(2, &ErrPoseVector, &ErrVelVector); + } @@ -348,126 +348,126 @@ public: static inline void PreIntegrateIMUObservations(const Vector& msr_acc_t, const Vector& msr_gyro_t, const double msr_dt, Vector& delta_pos_in_t0, Vector3& delta_angles, Vector& delta_vel_in_t0, double& delta_t, - const noiseModel::Gaussian::shared_ptr& model_continuous_overall, - Matrix& EquivCov_Overall, Matrix& Jacobian_wrt_t0_Overall, - boost::optional p_body_P_sensor = boost::none){ - // Note: all delta terms refer to an IMU\sensor system at t0 - // Note: Earth-related terms are not accounted here but are incorporated in predict functions. + const noiseModel::Gaussian::shared_ptr& model_continuous_overall, + Matrix& EquivCov_Overall, Matrix& Jacobian_wrt_t0_Overall, + boost::optional p_body_P_sensor = boost::none){ + // Note: all delta terms refer to an IMU\sensor system at t0 + // Note: Earth-related terms are not accounted here but are incorporated in predict functions. - POSE body_P_sensor = POSE(); - bool flag_use_body_P_sensor = false; - if (p_body_P_sensor){ - body_P_sensor = *p_body_P_sensor; - flag_use_body_P_sensor = true; - } + POSE body_P_sensor = POSE(); + bool flag_use_body_P_sensor = false; + if (p_body_P_sensor){ + body_P_sensor = *p_body_P_sensor; + flag_use_body_P_sensor = true; + } - delta_pos_in_t0 = PreIntegrateIMUObservations_delta_pos(msr_dt, delta_pos_in_t0, delta_vel_in_t0); - delta_vel_in_t0 = PreIntegrateIMUObservations_delta_vel(msr_gyro_t, msr_acc_t, msr_dt, delta_angles, delta_vel_in_t0, flag_use_body_P_sensor, body_P_sensor); - delta_angles = PreIntegrateIMUObservations_delta_angles(msr_gyro_t, msr_dt, delta_angles, flag_use_body_P_sensor, body_P_sensor); + delta_pos_in_t0 = PreIntegrateIMUObservations_delta_pos(msr_dt, delta_pos_in_t0, delta_vel_in_t0); + delta_vel_in_t0 = PreIntegrateIMUObservations_delta_vel(msr_gyro_t, msr_acc_t, msr_dt, delta_angles, delta_vel_in_t0, flag_use_body_P_sensor, body_P_sensor); + delta_angles = PreIntegrateIMUObservations_delta_angles(msr_gyro_t, msr_dt, delta_angles, flag_use_body_P_sensor, body_P_sensor); - delta_t += msr_dt; + delta_t += msr_dt; - // Update EquivCov_Overall - Matrix Z_3x3 = zeros(3,3); - Matrix I_3x3 = eye(3,3); + // Update EquivCov_Overall + Matrix Z_3x3 = zeros(3,3); + Matrix I_3x3 = eye(3,3); - Matrix H_pos_pos = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, _1, delta_vel_in_t0), delta_pos_in_t0); - Matrix H_pos_vel = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, delta_pos_in_t0, _1), delta_vel_in_t0); - Matrix H_pos_angles = Z_3x3; + Matrix H_pos_pos = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, _1, delta_vel_in_t0), delta_pos_in_t0); + Matrix H_pos_vel = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, delta_pos_in_t0, _1), delta_vel_in_t0); + Matrix H_pos_angles = Z_3x3; - Matrix H_vel_vel = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, delta_angles, _1, flag_use_body_P_sensor, body_P_sensor), delta_vel_in_t0); - Matrix H_vel_angles = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, _1, delta_vel_in_t0, flag_use_body_P_sensor, body_P_sensor), delta_angles); - Matrix H_vel_pos = Z_3x3; + Matrix H_vel_vel = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, delta_angles, _1, flag_use_body_P_sensor, body_P_sensor), delta_vel_in_t0); + Matrix H_vel_angles = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, _1, delta_vel_in_t0, flag_use_body_P_sensor, body_P_sensor), delta_angles); + Matrix H_vel_pos = Z_3x3; - Matrix H_angles_angles = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_angles, msr_gyro_t, msr_dt, _1, flag_use_body_P_sensor, body_P_sensor), delta_angles); - Matrix H_angles_pos = Z_3x3; - Matrix H_angles_vel = Z_3x3; + Matrix H_angles_angles = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_angles, msr_gyro_t, msr_dt, _1, flag_use_body_P_sensor, body_P_sensor), delta_angles); + Matrix H_angles_pos = Z_3x3; + Matrix H_angles_vel = Z_3x3; - Matrix F_angles = collect(3, &H_angles_angles, &H_angles_pos, &H_angles_vel); - Matrix F_pos = collect(3, &H_pos_angles, &H_pos_pos, &H_pos_vel); - Matrix F_vel = collect(3, &H_vel_angles, &H_vel_pos, &H_vel_vel); - Matrix F = stack(3, &F_angles, &F_pos, &F_vel); + Matrix F_angles = collect(3, &H_angles_angles, &H_angles_pos, &H_angles_vel); + Matrix F_pos = collect(3, &H_pos_angles, &H_pos_pos, &H_pos_vel); + Matrix F_vel = collect(3, &H_vel_angles, &H_vel_pos, &H_vel_vel); + Matrix F = stack(3, &F_angles, &F_pos, &F_vel); - noiseModel::Gaussian::shared_ptr model_discrete_curr = calc_descrete_noise_model(model_continuous_overall, msr_dt ); - Matrix Q_d = inverse(model_discrete_curr->R().transpose() * model_discrete_curr->R() ); + noiseModel::Gaussian::shared_ptr model_discrete_curr = calc_descrete_noise_model(model_continuous_overall, msr_dt ); + Matrix Q_d = inverse(model_discrete_curr->R().transpose() * model_discrete_curr->R() ); - EquivCov_Overall = F * EquivCov_Overall * F.transpose() + Q_d; + EquivCov_Overall = F * EquivCov_Overall * F.transpose() + Q_d; - // Update Jacobian_wrt_t0_Overall - Jacobian_wrt_t0_Overall = F * Jacobian_wrt_t0_Overall; - } + // Update Jacobian_wrt_t0_Overall + Jacobian_wrt_t0_Overall = F * Jacobian_wrt_t0_Overall; + } - static inline Vector PreIntegrateIMUObservations_delta_pos(const double msr_dt, - const Vector& delta_pos_in_t0, const Vector& delta_vel_in_t0){ + static inline Vector PreIntegrateIMUObservations_delta_pos(const double msr_dt, + const Vector& delta_pos_in_t0, const Vector& delta_vel_in_t0){ - // Note: all delta terms refer to an IMU\sensor system at t0 - // Note: delta_vel_in_t0 is already in body frame, so no need to use the body_P_sensor transformation here. + // Note: all delta terms refer to an IMU\sensor system at t0 + // Note: delta_vel_in_t0 is already in body frame, so no need to use the body_P_sensor transformation here. - return delta_pos_in_t0 + delta_vel_in_t0 * msr_dt; - } + return delta_pos_in_t0 + delta_vel_in_t0 * msr_dt; + } - static inline Vector PreIntegrateIMUObservations_delta_vel(const Vector& msr_gyro_t, const Vector& msr_acc_t, const double msr_dt, - const Vector3& delta_angles, const Vector& delta_vel_in_t0, const bool flag_use_body_P_sensor, const POSE& body_P_sensor){ + static inline Vector PreIntegrateIMUObservations_delta_vel(const Vector& msr_gyro_t, const Vector& msr_acc_t, const double msr_dt, + const Vector3& delta_angles, const Vector& delta_vel_in_t0, const bool flag_use_body_P_sensor, const POSE& body_P_sensor){ - // Note: all delta terms refer to an IMU\sensor system at t0 + // Note: all delta terms refer to an IMU\sensor system at t0 - // Calculate the corrected measurements using the Bias object - Vector AccCorrected = msr_acc_t; - Vector body_t_a_body; - if (flag_use_body_P_sensor){ - Matrix body_R_sensor = body_P_sensor.rotation().matrix(); + // Calculate the corrected measurements using the Bias object + Vector AccCorrected = msr_acc_t; + Vector body_t_a_body; + if (flag_use_body_P_sensor){ + Matrix body_R_sensor = body_P_sensor.rotation().matrix(); - Vector GyroCorrected(msr_gyro_t); + Vector GyroCorrected(msr_gyro_t); - Vector body_omega_body = body_R_sensor * GyroCorrected; - Matrix body_omega_body__cross = skewSymmetric(body_omega_body); + Vector body_omega_body = body_R_sensor * GyroCorrected; + Matrix body_omega_body__cross = skewSymmetric(body_omega_body); - body_t_a_body = body_R_sensor * AccCorrected - body_omega_body__cross * body_omega_body__cross * body_P_sensor.translation().vector(); - } else{ - body_t_a_body = AccCorrected; - } + body_t_a_body = body_R_sensor * AccCorrected - body_omega_body__cross * body_omega_body__cross * body_P_sensor.translation().vector(); + } else{ + body_t_a_body = AccCorrected; + } - Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles); + Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles); - return delta_vel_in_t0 + R_t_to_t0.matrix() * body_t_a_body * msr_dt; - } + return delta_vel_in_t0 + R_t_to_t0.matrix() * body_t_a_body * msr_dt; + } - static inline Vector PreIntegrateIMUObservations_delta_angles(const Vector& msr_gyro_t, const double msr_dt, - const Vector3& delta_angles, const bool flag_use_body_P_sensor, const POSE& body_P_sensor){ + static inline Vector PreIntegrateIMUObservations_delta_angles(const Vector& msr_gyro_t, const double msr_dt, + const Vector3& delta_angles, const bool flag_use_body_P_sensor, const POSE& body_P_sensor){ - // Note: all delta terms refer to an IMU\sensor system at t0 + // Note: all delta terms refer to an IMU\sensor system at t0 - // Calculate the corrected measurements using the Bias object - Vector GyroCorrected = msr_gyro_t; + // Calculate the corrected measurements using the Bias object + Vector GyroCorrected = msr_gyro_t; - Vector body_t_omega_body; - if (flag_use_body_P_sensor){ - body_t_omega_body = body_P_sensor.rotation().matrix() * GyroCorrected; - } else { - body_t_omega_body = GyroCorrected; - } + Vector body_t_omega_body; + if (flag_use_body_P_sensor){ + body_t_omega_body = body_P_sensor.rotation().matrix() * GyroCorrected; + } else { + body_t_omega_body = GyroCorrected; + } - Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles); + Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles); - R_t_to_t0 = R_t_to_t0 * Rot3::Expmap( body_t_omega_body*msr_dt ); - return Rot3::Logmap(R_t_to_t0); - } + R_t_to_t0 = R_t_to_t0 * Rot3::Expmap( body_t_omega_body*msr_dt ); + return Rot3::Logmap(R_t_to_t0); + } - static inline noiseModel::Gaussian::shared_ptr CalcEquivalentNoiseCov(const noiseModel::Gaussian::shared_ptr& gaussian_acc, const noiseModel::Gaussian::shared_ptr& gaussian_gyro, - const noiseModel::Gaussian::shared_ptr& gaussian_process){ + static inline noiseModel::Gaussian::shared_ptr CalcEquivalentNoiseCov(const noiseModel::Gaussian::shared_ptr& gaussian_acc, const noiseModel::Gaussian::shared_ptr& gaussian_gyro, + const noiseModel::Gaussian::shared_ptr& gaussian_process){ - Matrix cov_acc = inverse( gaussian_acc->R().transpose() * gaussian_acc->R() ); - Matrix cov_gyro = inverse( gaussian_gyro->R().transpose() * gaussian_gyro->R() ); - Matrix cov_process = inverse( gaussian_process->R().transpose() * gaussian_process->R() ); + Matrix cov_acc = inverse( gaussian_acc->R().transpose() * gaussian_acc->R() ); + Matrix cov_gyro = inverse( gaussian_gyro->R().transpose() * gaussian_gyro->R() ); + Matrix cov_process = inverse( gaussian_process->R().transpose() * gaussian_process->R() ); - cov_process.block(0,0, 3,3) += cov_gyro; - cov_process.block(6,6, 3,3) += cov_acc; + cov_process.block(0,0, 3,3) += cov_gyro; + cov_process.block(6,6, 3,3) += cov_acc; - return noiseModel::Gaussian::Covariance(cov_process); - } + return noiseModel::Gaussian::Covariance(cov_process); + } static inline void CalcEquivalentNoiseCov_DifferentParts(const noiseModel::Gaussian::shared_ptr& gaussian_acc, const noiseModel::Gaussian::shared_ptr& gaussian_gyro, const noiseModel::Gaussian::shared_ptr& gaussian_process, @@ -478,107 +478,107 @@ public: cov_process_without_acc_gyro = inverse( gaussian_process->R().transpose() * gaussian_process->R() ); } - static inline void Calc_g_rho_omega_earth_NED(const Vector& Pos_NED, const Vector& Vel_NED, const Vector& LatLonHeight_IC, const Vector& Pos_NED_Initial, - Vector& g_NED, Vector& rho_NED, Vector& omega_earth_NED) { + static inline void Calc_g_rho_omega_earth_NED(const Vector& Pos_NED, const Vector& Vel_NED, const Vector& LatLonHeight_IC, const Vector& Pos_NED_Initial, + Vector& g_NED, Vector& rho_NED, Vector& omega_earth_NED) { - Matrix ENU_to_NED = Matrix_(3, 3, - 0.0, 1.0, 0.0, - 1.0, 0.0, 0.0, - 0.0, 0.0, -1.0); + Matrix ENU_to_NED = Matrix_(3, 3, + 0.0, 1.0, 0.0, + 1.0, 0.0, 0.0, + 0.0, 0.0, -1.0); - Matrix NED_to_ENU = Matrix_(3, 3, - 0.0, 1.0, 0.0, - 1.0, 0.0, 0.0, - 0.0, 0.0, -1.0); + Matrix NED_to_ENU = Matrix_(3, 3, + 0.0, 1.0, 0.0, + 1.0, 0.0, 0.0, + 0.0, 0.0, -1.0); - // Convert incoming parameters to ENU - Vector Pos_ENU = NED_to_ENU * Pos_NED; - Vector Vel_ENU = NED_to_ENU * Vel_NED; - Vector Pos_ENU_Initial = NED_to_ENU * Pos_NED_Initial; + // Convert incoming parameters to ENU + Vector Pos_ENU = NED_to_ENU * Pos_NED; + Vector Vel_ENU = NED_to_ENU * Vel_NED; + Vector Pos_ENU_Initial = NED_to_ENU * Pos_NED_Initial; - // Call ENU version - Vector g_ENU; - Vector rho_ENU; - Vector omega_earth_ENU; - Calc_g_rho_omega_earth_ENU(Pos_ENU, Vel_ENU, LatLonHeight_IC, Pos_ENU_Initial, g_ENU, rho_ENU, omega_earth_ENU); + // Call ENU version + Vector g_ENU; + Vector rho_ENU; + Vector omega_earth_ENU; + Calc_g_rho_omega_earth_ENU(Pos_ENU, Vel_ENU, LatLonHeight_IC, Pos_ENU_Initial, g_ENU, rho_ENU, omega_earth_ENU); - // Convert output to NED - g_NED = ENU_to_NED * g_ENU; - rho_NED = ENU_to_NED * rho_ENU; - omega_earth_NED = ENU_to_NED * omega_earth_ENU; - } + // Convert output to NED + g_NED = ENU_to_NED * g_ENU; + rho_NED = ENU_to_NED * rho_ENU; + omega_earth_NED = ENU_to_NED * omega_earth_ENU; + } - static inline void Calc_g_rho_omega_earth_ENU(const Vector& Pos_ENU, const Vector& Vel_ENU, const Vector& LatLonHeight_IC, const Vector& Pos_ENU_Initial, - Vector& g_ENU, Vector& rho_ENU, Vector& omega_earth_ENU){ - double R0 = 6.378388e6; - double e = 1/297; - double Re( R0*( 1-e*(sin( LatLonHeight_IC(0) ))*(sin( LatLonHeight_IC(0) )) ) ); + static inline void Calc_g_rho_omega_earth_ENU(const Vector& Pos_ENU, const Vector& Vel_ENU, const Vector& LatLonHeight_IC, const Vector& Pos_ENU_Initial, + Vector& g_ENU, Vector& rho_ENU, Vector& omega_earth_ENU){ + double R0 = 6.378388e6; + double e = 1/297; + double Re( R0*( 1-e*(sin( LatLonHeight_IC(0) ))*(sin( LatLonHeight_IC(0) )) ) ); - // Calculate current lat, lon - Vector delta_Pos_ENU(Pos_ENU - Pos_ENU_Initial); - double delta_lat(delta_Pos_ENU(1)/Re); - double delta_lon(delta_Pos_ENU(0)/(Re*cos(LatLonHeight_IC(0)))); - double lat_new(LatLonHeight_IC(0) + delta_lat); - double lon_new(LatLonHeight_IC(1) + delta_lon); + // Calculate current lat, lon + Vector delta_Pos_ENU(Pos_ENU - Pos_ENU_Initial); + double delta_lat(delta_Pos_ENU(1)/Re); + double delta_lon(delta_Pos_ENU(0)/(Re*cos(LatLonHeight_IC(0)))); + double lat_new(LatLonHeight_IC(0) + delta_lat); + double lon_new(LatLonHeight_IC(1) + delta_lon); - // Rotation of lon about z axis - Rot3 C1(cos(lon_new), sin(lon_new), 0.0, - -sin(lon_new), cos(lon_new), 0.0, - 0.0, 0.0, 1.0); + // Rotation of lon about z axis + Rot3 C1(cos(lon_new), sin(lon_new), 0.0, + -sin(lon_new), cos(lon_new), 0.0, + 0.0, 0.0, 1.0); - // Rotation of lat about y axis - Rot3 C2(cos(lat_new), 0.0, sin(lat_new), - 0.0, 1.0, 0.0, - -sin(lat_new), 0.0, cos(lat_new)); + // Rotation of lat about y axis + Rot3 C2(cos(lat_new), 0.0, sin(lat_new), + 0.0, 1.0, 0.0, + -sin(lat_new), 0.0, cos(lat_new)); - Rot3 UEN_to_ENU(0, 1, 0, - 0, 0, 1, - 1, 0, 0); + Rot3 UEN_to_ENU(0, 1, 0, + 0, 0, 1, + 1, 0, 0); - Rot3 R_ECEF_to_ENU( UEN_to_ENU * C2 * C1 ); + Rot3 R_ECEF_to_ENU( UEN_to_ENU * C2 * C1 ); - Vector omega_earth_ECEF(Vector_(3, 0.0, 0.0, 7.292115e-5)); - omega_earth_ENU = R_ECEF_to_ENU.matrix() * omega_earth_ECEF; + Vector omega_earth_ECEF(Vector_(3, 0.0, 0.0, 7.292115e-5)); + omega_earth_ENU = R_ECEF_to_ENU.matrix() * omega_earth_ECEF; - // Calculating g - double height(LatLonHeight_IC(2)); - double EQUA_RADIUS = 6378137.0; // equatorial radius of the earth; WGS-84 - double ECCENTRICITY = 0.0818191908426; // eccentricity of the earth ellipsoid - double e2( pow(ECCENTRICITY,2) ); - double den( 1-e2*pow(sin(lat_new),2) ); - double Rm( (EQUA_RADIUS*(1-e2))/( pow(den,(3/2)) ) ); - double Rp( EQUA_RADIUS/( sqrt(den) ) ); - double Ro( sqrt(Rp*Rm) ); // mean earth radius of curvature - double g0( 9.780318*( 1 + 5.3024e-3 * pow(sin(lat_new),2) - 5.9e-6 * pow(sin(2*lat_new),2) ) ); - double g_calc( g0/( pow(1 + height/Ro, 2) ) ); - g_ENU = Vector_(3, 0.0, 0.0, -g_calc); + // Calculating g + double height(LatLonHeight_IC(2)); + double EQUA_RADIUS = 6378137.0; // equatorial radius of the earth; WGS-84 + double ECCENTRICITY = 0.0818191908426; // eccentricity of the earth ellipsoid + double e2( pow(ECCENTRICITY,2) ); + double den( 1-e2*pow(sin(lat_new),2) ); + double Rm( (EQUA_RADIUS*(1-e2))/( pow(den,(3/2)) ) ); + double Rp( EQUA_RADIUS/( sqrt(den) ) ); + double Ro( sqrt(Rp*Rm) ); // mean earth radius of curvature + double g0( 9.780318*( 1 + 5.3024e-3 * pow(sin(lat_new),2) - 5.9e-6 * pow(sin(2*lat_new),2) ) ); + double g_calc( g0/( pow(1 + height/Ro, 2) ) ); + g_ENU = Vector_(3, 0.0, 0.0, -g_calc); - // Calculate rho - double Ve( Vel_ENU(0) ); - double Vn( Vel_ENU(1) ); - double rho_E = -Vn/(Rm + height); - double rho_N = Ve/(Rp + height); - double rho_U = Ve*tan(lat_new)/(Rp + height); - rho_ENU = Vector_(3, rho_E, rho_N, rho_U); - } + // Calculate rho + double Ve( Vel_ENU(0) ); + double Vn( Vel_ENU(1) ); + double rho_E = -Vn/(Rm + height); + double rho_N = Ve/(Rp + height); + double rho_U = Ve*tan(lat_new)/(Rp + height); + rho_ENU = Vector_(3, rho_E, rho_N, rho_U); + } - static inline noiseModel::Gaussian::shared_ptr calc_descrete_noise_model(const noiseModel::Gaussian::shared_ptr& model, double delta_t){ - /* Q_d (approx)= Q * delta_t */ - /* In practice, square root of the information matrix is represented, so that: - * R_d (approx)= R / sqrt(delta_t) - * */ - return noiseModel::Gaussian::SqrtInformation(model->R()/sqrt(delta_t)); - } + static inline noiseModel::Gaussian::shared_ptr calc_descrete_noise_model(const noiseModel::Gaussian::shared_ptr& model, double delta_t){ + /* Q_d (approx)= Q * delta_t */ + /* In practice, square root of the information matrix is represented, so that: + * R_d (approx)= R / sqrt(delta_t) + * */ + return noiseModel::Gaussian::SqrtInformation(model->R()/sqrt(delta_t)); + } private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("NonlinearFactor2", - boost::serialization::base_object(*this)); - } + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("NonlinearFactor2", + boost::serialization::base_object(*this)); + } diff --git a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h index 33eb8777d..f67c47e2a 100644 --- a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h +++ b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h @@ -44,11 +44,11 @@ namespace gtsam { * matrices and the process\modeling covariance matrix. The IneritalNavFactor converts this into a * discrete form using the supplied delta_t between sub-sequential measurements. * - Earth-rate correction: - * + Currently the user should supply R_ECEF_to_G, which is the rotation from ECEF to the global - * frame (Local-Level system: ENU or NED, see above). - * + R_ECEF_to_G can be calculated by approximated values of latitude and longitude of the system. - * + Currently it is assumed that a relatively small distance is traveled w.r.t. to initial pose, since R_ECEF_to_G is constant. - * Otherwise, R_ECEF_to_G should be updated each time using the current lat-lon. + * + Currently the user should supply R_ECEF_to_G, which is the rotation from ECEF to the global + * frame (Local-Level system: ENU or NED, see above). + * + R_ECEF_to_G can be calculated by approximated values of latitude and longitude of the system. + * + Currently it is assumed that a relatively small distance is traveled w.r.t. to initial pose, since R_ECEF_to_G is constant. + * Otherwise, R_ECEF_to_G should be updated each time using the current lat-lon. * * - Frame Notation: * Quantities are written as {Frame of Representation/Destination Frame}_{Quantity Type}_{Quatity Description/Origination Frame} @@ -81,70 +81,70 @@ class InertialNavFactor_GlobalVelocity : public NoiseModelFactor5 This; - typedef NoiseModelFactor5 Base; + typedef InertialNavFactor_GlobalVelocity This; + typedef NoiseModelFactor5 Base; - Vector measurement_acc_; - Vector measurement_gyro_; - double dt_; + Vector measurement_acc_; + Vector measurement_gyro_; + double dt_; - Vector world_g_; - Vector world_rho_; + Vector world_g_; + Vector world_rho_; Vector world_omega_earth_; boost::optional body_P_sensor_; // The pose of the sensor in the body frame public: - // shorthand for a smart pointer to a factor - typedef typename boost::shared_ptr shared_ptr; + // shorthand for a smart pointer to a factor + typedef typename boost::shared_ptr shared_ptr; - /** default constructor - only use for serialization */ - InertialNavFactor_GlobalVelocity() {} + /** default constructor - only use for serialization */ + InertialNavFactor_GlobalVelocity() {} - /** Constructor */ - InertialNavFactor_GlobalVelocity(const Key& Pose1, const Key& Vel1, const Key& IMUBias1, const Key& Pose2, const Key& Vel2, + /** Constructor */ + InertialNavFactor_GlobalVelocity(const Key& Pose1, const Key& Vel1, const Key& IMUBias1, const Key& Pose2, const Key& Vel2, const Vector& measurement_acc, const Vector& measurement_gyro, const double measurement_dt, const Vector world_g, const Vector world_rho, const Vector& world_omega_earth, const noiseModel::Gaussian::shared_ptr& model_continuous, boost::optional body_P_sensor = boost::none) : Base(calc_descrete_noise_model(model_continuous, measurement_dt ), Pose1, Vel1, IMUBias1, Pose2, Vel2), measurement_acc_(measurement_acc), measurement_gyro_(measurement_gyro), - dt_(measurement_dt), world_g_(world_g), world_rho_(world_rho), world_omega_earth_(world_omega_earth), body_P_sensor_(body_P_sensor) { } + dt_(measurement_dt), world_g_(world_g), world_rho_(world_rho), world_omega_earth_(world_omega_earth), body_P_sensor_(body_P_sensor) { } - virtual ~InertialNavFactor_GlobalVelocity() {} + virtual ~InertialNavFactor_GlobalVelocity() {} - /** implement functions needed for Testable */ + /** implement functions needed for Testable */ - /** print */ - virtual void print(const std::string& s = "InertialNavFactor_GlobalVelocity", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { - std::cout << s << "(" - << keyFormatter(this->key1()) << "," - << keyFormatter(this->key2()) << "," - << keyFormatter(this->key3()) << "," - << keyFormatter(this->key4()) << "," - << keyFormatter(this->key5()) << "\n"; - std::cout << "acc measurement: " << this->measurement_acc_.transpose() << std::endl; - std::cout << "gyro measurement: " << this->measurement_gyro_.transpose() << std::endl; - std::cout << "dt: " << this->dt_ << std::endl; - std::cout << "gravity (in world frame): " << this->world_g_.transpose() << std::endl; - std::cout << "craft rate (in world frame): " << this->world_rho_.transpose() << std::endl; - std::cout << "earth's rotation (in world frame): " << this->world_omega_earth_.transpose() << std::endl; - if(this->body_P_sensor_) - this->body_P_sensor_->print(" sensor pose in body frame: "); + /** print */ + virtual void print(const std::string& s = "InertialNavFactor_GlobalVelocity", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + std::cout << s << "(" + << keyFormatter(this->key1()) << "," + << keyFormatter(this->key2()) << "," + << keyFormatter(this->key3()) << "," + << keyFormatter(this->key4()) << "," + << keyFormatter(this->key5()) << "\n"; + std::cout << "acc measurement: " << this->measurement_acc_.transpose() << std::endl; + std::cout << "gyro measurement: " << this->measurement_gyro_.transpose() << std::endl; + std::cout << "dt: " << this->dt_ << std::endl; + std::cout << "gravity (in world frame): " << this->world_g_.transpose() << std::endl; + std::cout << "craft rate (in world frame): " << this->world_rho_.transpose() << std::endl; + std::cout << "earth's rotation (in world frame): " << this->world_omega_earth_.transpose() << std::endl; + if(this->body_P_sensor_) + this->body_P_sensor_->print(" sensor pose in body frame: "); this->noiseModel_->print(" noise model"); - } + } - /** equals */ - virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { - const This *e = dynamic_cast (&expected); - return e != NULL && Base::equals(*e, tol) - && (measurement_acc_ - e->measurement_acc_).norm() < tol - && (measurement_gyro_ - e->measurement_gyro_).norm() < tol - && (dt_ - e->dt_) < tol - && (world_g_ - e->world_g_).norm() < tol - && (world_rho_ - e->world_rho_).norm() < tol - && (world_omega_earth_ - e->world_omega_earth_).norm() < tol - && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_))); - } + /** equals */ + virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { + const This *e = dynamic_cast (&expected); + return e != NULL && Base::equals(*e, tol) + && (measurement_acc_ - e->measurement_acc_).norm() < tol + && (measurement_gyro_ - e->measurement_gyro_).norm() < tol + && (dt_ - e->dt_) < tol + && (world_g_ - e->world_g_).norm() < tol + && (world_rho_ - e->world_rho_).norm() < tol + && (world_omega_earth_ - e->world_omega_earth_).norm() < tol + && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_))); + } POSE predictPose(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1) const { // Calculate the corrected measurements using the Bias object @@ -225,12 +225,12 @@ public: } /** implement functions needed to derive from Factor */ - Vector evaluateError(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1, const POSE& Pose2, const VELOCITY& Vel2, - 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 POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1, const POSE& Pose2, const VELOCITY& Vel2, + 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 { // TODO: Write analytical derivative calculations // Jacobian w.r.t. Pose1 @@ -268,24 +268,24 @@ public: *H5 = stack(2, &H5_Pose, &H5_Vel); } - Vector ErrPoseVector(POSE::Logmap(evaluatePoseError(Pose1, Vel1, Bias1, Pose2, Vel2))); - Vector ErrVelVector(VELOCITY::Logmap(evaluateVelocityError(Pose1, Vel1, Bias1, Pose2, Vel2))); + Vector ErrPoseVector(POSE::Logmap(evaluatePoseError(Pose1, Vel1, Bias1, Pose2, Vel2))); + Vector ErrVelVector(VELOCITY::Logmap(evaluateVelocityError(Pose1, Vel1, Bias1, Pose2, Vel2))); - return concatVectors(2, &ErrPoseVector, &ErrVelVector); - } + return concatVectors(2, &ErrPoseVector, &ErrVelVector); + } - static inline noiseModel::Gaussian::shared_ptr CalcEquivalentNoiseCov(const noiseModel::Gaussian::shared_ptr& gaussian_acc, const noiseModel::Gaussian::shared_ptr& gaussian_gyro, - const noiseModel::Gaussian::shared_ptr& gaussian_process){ + static inline noiseModel::Gaussian::shared_ptr CalcEquivalentNoiseCov(const noiseModel::Gaussian::shared_ptr& gaussian_acc, const noiseModel::Gaussian::shared_ptr& gaussian_gyro, + const noiseModel::Gaussian::shared_ptr& gaussian_process){ - Matrix cov_acc = inverse( gaussian_acc->R().transpose() * gaussian_acc->R() ); - Matrix cov_gyro = inverse( gaussian_gyro->R().transpose() * gaussian_gyro->R() ); - Matrix cov_process = inverse( gaussian_process->R().transpose() * gaussian_process->R() ); + Matrix cov_acc = inverse( gaussian_acc->R().transpose() * gaussian_acc->R() ); + Matrix cov_gyro = inverse( gaussian_gyro->R().transpose() * gaussian_gyro->R() ); + Matrix cov_process = inverse( gaussian_process->R().transpose() * gaussian_process->R() ); - cov_process.block(0,0, 3,3) += cov_gyro; - cov_process.block(6,6, 3,3) += cov_acc; + cov_process.block(0,0, 3,3) += cov_gyro; + cov_process.block(6,6, 3,3) += cov_acc; - return noiseModel::Gaussian::Covariance(cov_process); - } + return noiseModel::Gaussian::Covariance(cov_process); + } static inline void Calc_g_rho_omega_earth_NED(const Vector& Pos_NED, const Vector& Vel_NED, const Vector& LatLonHeight_IC, const Vector& Pos_NED_Initial, Vector& g_NED, Vector& rho_NED, Vector& omega_earth_NED) { @@ -317,78 +317,78 @@ public: omega_earth_NED = ENU_to_NED * omega_earth_ENU; } - static inline void Calc_g_rho_omega_earth_ENU(const Vector& Pos_ENU, const Vector& Vel_ENU, const Vector& LatLonHeight_IC, const Vector& Pos_ENU_Initial, - Vector& g_ENU, Vector& rho_ENU, Vector& omega_earth_ENU){ - double R0 = 6.378388e6; - double e = 1/297; - double Re( R0*( 1-e*(sin( LatLonHeight_IC(0) ))*(sin( LatLonHeight_IC(0) )) ) ); + static inline void Calc_g_rho_omega_earth_ENU(const Vector& Pos_ENU, const Vector& Vel_ENU, const Vector& LatLonHeight_IC, const Vector& Pos_ENU_Initial, + Vector& g_ENU, Vector& rho_ENU, Vector& omega_earth_ENU){ + double R0 = 6.378388e6; + double e = 1/297; + double Re( R0*( 1-e*(sin( LatLonHeight_IC(0) ))*(sin( LatLonHeight_IC(0) )) ) ); - // Calculate current lat, lon - Vector delta_Pos_ENU(Pos_ENU - Pos_ENU_Initial); - double delta_lat(delta_Pos_ENU(1)/Re); - double delta_lon(delta_Pos_ENU(0)/(Re*cos(LatLonHeight_IC(0)))); - double lat_new(LatLonHeight_IC(0) + delta_lat); - double lon_new(LatLonHeight_IC(1) + delta_lon); + // Calculate current lat, lon + Vector delta_Pos_ENU(Pos_ENU - Pos_ENU_Initial); + double delta_lat(delta_Pos_ENU(1)/Re); + double delta_lon(delta_Pos_ENU(0)/(Re*cos(LatLonHeight_IC(0)))); + double lat_new(LatLonHeight_IC(0) + delta_lat); + double lon_new(LatLonHeight_IC(1) + delta_lon); - // Rotation of lon about z axis - Rot3 C1(cos(lon_new), sin(lon_new), 0.0, - -sin(lon_new), cos(lon_new), 0.0, - 0.0, 0.0, 1.0); + // Rotation of lon about z axis + Rot3 C1(cos(lon_new), sin(lon_new), 0.0, + -sin(lon_new), cos(lon_new), 0.0, + 0.0, 0.0, 1.0); - // Rotation of lat about y axis - Rot3 C2(cos(lat_new), 0.0, sin(lat_new), - 0.0, 1.0, 0.0, - -sin(lat_new), 0.0, cos(lat_new)); + // Rotation of lat about y axis + Rot3 C2(cos(lat_new), 0.0, sin(lat_new), + 0.0, 1.0, 0.0, + -sin(lat_new), 0.0, cos(lat_new)); - Rot3 UEN_to_ENU(0, 1, 0, - 0, 0, 1, - 1, 0, 0); + Rot3 UEN_to_ENU(0, 1, 0, + 0, 0, 1, + 1, 0, 0); - Rot3 R_ECEF_to_ENU( UEN_to_ENU * C2 * C1 ); + Rot3 R_ECEF_to_ENU( UEN_to_ENU * C2 * C1 ); - Vector omega_earth_ECEF(Vector_(3, 0.0, 0.0, 7.292115e-5)); - omega_earth_ENU = R_ECEF_to_ENU.matrix() * omega_earth_ECEF; + Vector omega_earth_ECEF(Vector_(3, 0.0, 0.0, 7.292115e-5)); + omega_earth_ENU = R_ECEF_to_ENU.matrix() * omega_earth_ECEF; - // Calculating g - double height(LatLonHeight_IC(2)); - double EQUA_RADIUS = 6378137.0; // equatorial radius of the earth; WGS-84 - double ECCENTRICITY = 0.0818191908426; // eccentricity of the earth ellipsoid - double e2( pow(ECCENTRICITY,2) ); - double den( 1-e2*pow(sin(lat_new),2) ); - double Rm( (EQUA_RADIUS*(1-e2))/( pow(den,(3/2)) ) ); - double Rp( EQUA_RADIUS/( sqrt(den) ) ); - double Ro( sqrt(Rp*Rm) ); // mean earth radius of curvature - double g0( 9.780318*( 1 + 5.3024e-3 * pow(sin(lat_new),2) - 5.9e-6 * pow(sin(2*lat_new),2) ) ); - double g_calc( g0/( pow(1 + height/Ro, 2) ) ); - g_ENU = Vector_(3, 0.0, 0.0, -g_calc); + // Calculating g + double height(LatLonHeight_IC(2)); + double EQUA_RADIUS = 6378137.0; // equatorial radius of the earth; WGS-84 + double ECCENTRICITY = 0.0818191908426; // eccentricity of the earth ellipsoid + double e2( pow(ECCENTRICITY,2) ); + double den( 1-e2*pow(sin(lat_new),2) ); + double Rm( (EQUA_RADIUS*(1-e2))/( pow(den,(3/2)) ) ); + double Rp( EQUA_RADIUS/( sqrt(den) ) ); + double Ro( sqrt(Rp*Rm) ); // mean earth radius of curvature + double g0( 9.780318*( 1 + 5.3024e-3 * pow(sin(lat_new),2) - 5.9e-6 * pow(sin(2*lat_new),2) ) ); + double g_calc( g0/( pow(1 + height/Ro, 2) ) ); + g_ENU = Vector_(3, 0.0, 0.0, -g_calc); - // Calculate rho - double Ve( Vel_ENU(0) ); - double Vn( Vel_ENU(1) ); - double rho_E = -Vn/(Rm + height); - double rho_N = Ve/(Rp + height); - double rho_U = Ve*tan(lat_new)/(Rp + height); - rho_ENU = Vector_(3, rho_E, rho_N, rho_U); - } + // Calculate rho + double Ve( Vel_ENU(0) ); + double Vn( Vel_ENU(1) ); + double rho_E = -Vn/(Rm + height); + double rho_N = Ve/(Rp + height); + double rho_U = Ve*tan(lat_new)/(Rp + height); + rho_ENU = Vector_(3, rho_E, rho_N, rho_U); + } - static inline noiseModel::Gaussian::shared_ptr calc_descrete_noise_model(const noiseModel::Gaussian::shared_ptr& model, double delta_t){ - /* Q_d (approx)= Q * delta_t */ - /* In practice, square root of the information matrix is represented, so that: - * R_d (approx)= R / sqrt(delta_t) - * */ - return noiseModel::Gaussian::SqrtInformation(model->R()/std::sqrt(delta_t)); - } + static inline noiseModel::Gaussian::shared_ptr calc_descrete_noise_model(const noiseModel::Gaussian::shared_ptr& model, double delta_t){ + /* Q_d (approx)= Q * delta_t */ + /* In practice, square root of the information matrix is represented, so that: + * R_d (approx)= R / sqrt(delta_t) + * */ + return noiseModel::Gaussian::SqrtInformation(model->R()/std::sqrt(delta_t)); + } private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("NonlinearFactor2", - boost::serialization::base_object(*this)); - } + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("NonlinearFactor2", + boost::serialization::base_object(*this)); + } }; // \class GaussMarkov1stOrderFactor diff --git a/gtsam_unstable/slam/tests/testAHRS.cpp b/gtsam_unstable/slam/tests/testAHRS.cpp index d59098402..c0c4bf636 100644 --- a/gtsam_unstable/slam/tests/testAHRS.cpp +++ b/gtsam_unstable/slam/tests/testAHRS.cpp @@ -92,8 +92,8 @@ TEST (AHRS, init) { */ /* ************************************************************************* */ int main() { - TestResult tr; - return TestRegistry::runAllTests(tr); + TestResult tr; + return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ diff --git a/gtsam_unstable/slam/tests/testEquivInertialNavFactor_GlobalVel.cpp b/gtsam_unstable/slam/tests/testEquivInertialNavFactor_GlobalVel.cpp index 68c9f01bf..fe537990a 100644 --- a/gtsam_unstable/slam/tests/testEquivInertialNavFactor_GlobalVel.cpp +++ b/gtsam_unstable/slam/tests/testEquivInertialNavFactor_GlobalVel.cpp @@ -31,11 +31,11 @@ using namespace gtsam; /* ************************************************************************* */ TEST( EquivInertialNavFactor_GlobalVel, Constructor) { - Key poseKey1(11); - Key poseKey2(12); - Key velKey1(21); - Key velKey2(22); - Key biasKey1(31); + Key poseKey1(11); + Key poseKey2(12); + Key velKey1(21); + Key velKey2(22); + Key biasKey1(31); // IMU accumulation variables Vector delta_pos_in_t0 = Vector_(3, 0.0, 0.0, 0.0); @@ -46,16 +46,16 @@ TEST( EquivInertialNavFactor_GlobalVel, Constructor) Matrix Jacobian_wrt_t0_Overall = eye(15); imuBias::ConstantBias bias1 = imuBias::ConstantBias(); - // Earth Terms (gravity, etc) + // Earth Terms (gravity, etc) Vector3 g(0.0, 0.0, -9.80); Vector3 rho(0.0, 0.0, 0.0); Vector3 omega_earth(0.0, 0.0, 0.0); - // IMU Noise Model - SharedGaussian imu_model = noiseModel::Gaussian::Covariance(EquivCov_Overall.block(0,0,9,9)); + // IMU Noise Model + SharedGaussian imu_model = noiseModel::Gaussian::Covariance(EquivCov_Overall.block(0,0,9,9)); - // Constructor - EquivInertialNavFactor_GlobalVel factor( + // Constructor + EquivInertialNavFactor_GlobalVel factor( poseKey1, velKey1, biasKey1, poseKey2, velKey2, delta_pos_in_t0, delta_vel_in_t0, delta_angles, delta_t, g, rho, omega_earth, imu_model, Jacobian_wrt_t0_Overall, bias1); @@ -63,5 +63,5 @@ TEST( EquivInertialNavFactor_GlobalVel, Constructor) } /* ************************************************************************* */ - int main() { TestResult tr; return TestRegistry::runAllTests(tr);} + int main() { TestResult tr; return TestRegistry::runAllTests(tr);} /* ************************************************************************* */ diff --git a/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp b/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp index 951e6931c..7a4dc8c94 100644 --- a/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp +++ b/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp @@ -30,9 +30,9 @@ using namespace std; using namespace gtsam; gtsam::Rot3 world_R_ECEF( - 0.31686, 0.51505, 0.79645, - 0.85173, -0.52399, 0, - 0.41733, 0.67835, -0.60471); + 0.31686, 0.51505, 0.79645, + 0.85173, -0.52399, 0, + 0.41733, 0.67835, -0.60471); gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5)); gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); @@ -49,24 +49,24 @@ gtsam::LieVector predictionErrorVel(const Pose3& p1, const LieVector& v1, const /* ************************************************************************* */ TEST( InertialNavFactor_GlobalVelocity, Constructor) { - gtsam::Key Pose1(11); - gtsam::Key Pose2(12); - gtsam::Key Vel1(21); - gtsam::Key Vel2(22); - gtsam::Key Bias1(31); + gtsam::Key Pose1(11); + gtsam::Key Pose2(12); + gtsam::Key Vel1(21); + gtsam::Key Vel2(22); + gtsam::Key Bias1(31); - Vector measurement_acc(Vector_(3,0.1,0.2,0.4)); - Vector measurement_gyro(Vector_(3, -0.2, 0.5, 0.03)); + Vector measurement_acc(Vector_(3,0.1,0.2,0.4)); + Vector measurement_gyro(Vector_(3, -0.2, 0.5, 0.03)); - double measurement_dt(0.1); - Vector world_g(Vector_(3, 0.0, 0.0, 9.81)); - Vector world_rho(Vector_(3, 0.0, -1.5724e-05, 0.0)); // NED system - gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5)); - gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); + double measurement_dt(0.1); + Vector world_g(Vector_(3, 0.0, 0.0, 9.81)); + Vector world_rho(Vector_(3, 0.0, -1.5724e-05, 0.0)); // NED system + gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5)); + gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); - SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); + SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); - InertialNavFactor_GlobalVelocity f(Pose1, Vel1, Bias1, Pose2, Vel2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model); + InertialNavFactor_GlobalVelocity f(Pose1, Vel1, Bias1, Pose2, Vel2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model); } /* ************************************************************************* */ @@ -78,20 +78,20 @@ TEST( InertialNavFactor_GlobalVelocity, Equals) gtsam::Key Vel2(22); gtsam::Key Bias1(31); - Vector measurement_acc(Vector_(3,0.1,0.2,0.4)); - Vector measurement_gyro(Vector_(3, -0.2, 0.5, 0.03)); + Vector measurement_acc(Vector_(3,0.1,0.2,0.4)); + Vector measurement_gyro(Vector_(3, -0.2, 0.5, 0.03)); - double measurement_dt(0.1); + double measurement_dt(0.1); Vector world_g(Vector_(3, 0.0, 0.0, 9.81)); Vector world_rho(Vector_(3, 0.0, -1.5724e-05, 0.0)); // NED system gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5)); gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); - SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); + SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); - InertialNavFactor_GlobalVelocity f(Pose1, Vel1, Bias1, Pose2, Vel2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model); - InertialNavFactor_GlobalVelocity g(Pose1, Vel1, Bias1, Pose2, Vel2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model); - CHECK(assert_equal(f, g, 1e-5)); + InertialNavFactor_GlobalVelocity f(Pose1, Vel1, Bias1, Pose2, Vel2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model); + InertialNavFactor_GlobalVelocity g(Pose1, Vel1, Bias1, Pose2, Vel2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model); + CHECK(assert_equal(f, g, 1e-5)); } /* ************************************************************************* */ @@ -140,31 +140,31 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorPosVel) gtsam::Key VelKey2(22); gtsam::Key BiasKey1(31); - double measurement_dt(0.1); + double measurement_dt(0.1); Vector world_g(Vector_(3, 0.0, 0.0, 9.81)); Vector world_rho(Vector_(3, 0.0, -1.5724e-05, 0.0)); // NED system gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5)); gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); - SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); + SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); - // First test: zero angular motion, some acceleration - Vector measurement_acc(Vector_(3,0.1,0.2,0.3-9.81)); - Vector measurement_gyro(Vector_(3, 0.0, 0.0, 0.0)); + // First test: zero angular motion, some acceleration + Vector measurement_acc(Vector_(3,0.1,0.2,0.3-9.81)); + Vector measurement_gyro(Vector_(3, 0.0, 0.0, 0.0)); - InertialNavFactor_GlobalVelocity f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model); + InertialNavFactor_GlobalVelocity f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model); - Pose3 Pose1(Rot3(), Point3(2.00, 1.00, 3.00)); - Pose3 Pose2(Rot3(), Point3(2.05, 0.95, 3.04)); - LieVector Vel1(3, 0.50, -0.50, 0.40); - LieVector Vel2(3, 0.51, -0.48, 0.43); - imuBias::ConstantBias Bias1; + Pose3 Pose1(Rot3(), Point3(2.00, 1.00, 3.00)); + Pose3 Pose2(Rot3(), Point3(2.05, 0.95, 3.04)); + LieVector Vel1(3, 0.50, -0.50, 0.40); + LieVector Vel2(3, 0.51, -0.48, 0.43); + imuBias::ConstantBias Bias1; - Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2)); - Vector ExpectedErr(zero(9)); + Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2)); + Vector ExpectedErr(zero(9)); - CHECK(assert_equal(ExpectedErr, ActualErr, 1e-5)); + CHECK(assert_equal(ExpectedErr, ActualErr, 1e-5)); } /* ************************************************************************* */ @@ -176,30 +176,30 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorRot) gtsam::Key VelKey2(22); gtsam::Key BiasKey1(31); - double measurement_dt(0.1); + double measurement_dt(0.1); Vector world_g(Vector_(3, 0.0, 0.0, 9.81)); Vector world_rho(Vector_(3, 0.0, -1.5724e-05, 0.0)); // NED system gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5)); gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); - SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); + SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); - // Second test: zero angular motion, some acceleration - Vector measurement_acc(Vector_(3,0.0,0.0,0.0-9.81)); - Vector measurement_gyro(Vector_(3, 0.1, 0.2, 0.3)); + // Second test: zero angular motion, some acceleration + Vector measurement_acc(Vector_(3,0.0,0.0,0.0-9.81)); + Vector measurement_gyro(Vector_(3, 0.1, 0.2, 0.3)); - InertialNavFactor_GlobalVelocity f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model); + InertialNavFactor_GlobalVelocity f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model); - Pose3 Pose1(Rot3(), Point3(2.0,1.0,3.0)); - Pose3 Pose2(Rot3::Expmap(measurement_gyro*measurement_dt), Point3(2.0,1.0,3.0)); - LieVector Vel1(3,0.0,0.0,0.0); - LieVector Vel2(3,0.0,0.0,0.0); - imuBias::ConstantBias Bias1; + Pose3 Pose1(Rot3(), Point3(2.0,1.0,3.0)); + Pose3 Pose2(Rot3::Expmap(measurement_gyro*measurement_dt), Point3(2.0,1.0,3.0)); + LieVector Vel1(3,0.0,0.0,0.0); + LieVector Vel2(3,0.0,0.0,0.0); + imuBias::ConstantBias Bias1; - Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2)); - Vector ExpectedErr(zero(9)); + Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2)); + Vector ExpectedErr(zero(9)); - CHECK(assert_equal(ExpectedErr, ActualErr, 1e-5)); + CHECK(assert_equal(ExpectedErr, ActualErr, 1e-5)); } /* ************************************************************************* */ @@ -211,67 +211,67 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorRotPosVel) gtsam::Key VelKey2(22); gtsam::Key BiasKey1(31); - double measurement_dt(0.1); + double measurement_dt(0.1); Vector world_g(Vector_(3, 0.0, 0.0, 9.81)); Vector world_rho(Vector_(3, 0.0, -1.5724e-05, 0.0)); // NED system gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5)); gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); - SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); + SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); - // Second test: zero angular motion, some acceleration - generated in matlab - Vector measurement_acc(Vector_(3, 6.501390843381716, -6.763926150509185, -2.300389940090343)); - Vector measurement_gyro(Vector_(3, 0.1, 0.2, 0.3)); + // Second test: zero angular motion, some acceleration - generated in matlab + Vector measurement_acc(Vector_(3, 6.501390843381716, -6.763926150509185, -2.300389940090343)); + Vector measurement_gyro(Vector_(3, 0.1, 0.2, 0.3)); - InertialNavFactor_GlobalVelocity f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model); + InertialNavFactor_GlobalVelocity f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model); - Rot3 R1(0.487316618, 0.125253866, 0.86419557, - 0.580273724, 0.693095498, -0.427669306, - -0.652537293, 0.709880342, 0.265075427); - Point3 t1(2.0,1.0,3.0); - Pose3 Pose1(R1, t1); - LieVector Vel1(3,0.5,-0.5,0.4); - Rot3 R2(0.473618898, 0.119523052, 0.872582019, - 0.609241153, 0.67099888, -0.422594037, - -0.636011287, 0.731761397, 0.244979388); - Point3 t2 = t1.compose( Point3(Vel1*measurement_dt) ); - Pose3 Pose2(R2, t2); - Vector dv = measurement_dt * (R1.matrix() * measurement_acc + world_g); - LieVector Vel2 = Vel1.compose( dv ); - imuBias::ConstantBias Bias1; + Rot3 R1(0.487316618, 0.125253866, 0.86419557, + 0.580273724, 0.693095498, -0.427669306, + -0.652537293, 0.709880342, 0.265075427); + Point3 t1(2.0,1.0,3.0); + Pose3 Pose1(R1, t1); + LieVector Vel1(3,0.5,-0.5,0.4); + Rot3 R2(0.473618898, 0.119523052, 0.872582019, + 0.609241153, 0.67099888, -0.422594037, + -0.636011287, 0.731761397, 0.244979388); + Point3 t2 = t1.compose( Point3(Vel1*measurement_dt) ); + Pose3 Pose2(R2, t2); + Vector dv = measurement_dt * (R1.matrix() * measurement_acc + world_g); + LieVector Vel2 = Vel1.compose( dv ); + imuBias::ConstantBias Bias1; - Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2)); - Vector ExpectedErr(zero(9)); + Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2)); + Vector ExpectedErr(zero(9)); - // TODO: Expected values need to be updated for global velocity version - CHECK(assert_equal(ExpectedErr, ActualErr, 1e-5)); + // TODO: Expected values need to be updated for global velocity version + CHECK(assert_equal(ExpectedErr, ActualErr, 1e-5)); } ///* VADIM - START ************************************************************************* */ //LieVector predictionRq(const LieVector angles, const LieVector q) { -// return (Rot3().RzRyRx(angles) * q).vector(); +// return (Rot3().RzRyRx(angles) * q).vector(); //} // //TEST (InertialNavFactor_GlobalVelocity, Rotation_Deriv ) { -// LieVector angles(Vector_(3, 3.001, -1.0004, 2.0005)); -// Rot3 R1(Rot3().RzRyRx(angles)); -// LieVector q(Vector_(3, 5.8, -2.2, 4.105)); -// Rot3 qx(0.0, -q[2], q[1], -// q[2], 0.0, -q[0], -// -q[1], q[0],0.0); -// Matrix J_hyp( -(R1*qx).matrix() ); +// LieVector angles(Vector_(3, 3.001, -1.0004, 2.0005)); +// Rot3 R1(Rot3().RzRyRx(angles)); +// LieVector q(Vector_(3, 5.8, -2.2, 4.105)); +// Rot3 qx(0.0, -q[2], q[1], +// q[2], 0.0, -q[0], +// -q[1], q[0],0.0); +// Matrix J_hyp( -(R1*qx).matrix() ); // -// gtsam::Matrix J_expected; +// gtsam::Matrix J_expected; // -// LieVector v(predictionRq(angles, q)); +// LieVector v(predictionRq(angles, q)); // -// J_expected = gtsam::numericalDerivative11(boost::bind(&predictionRq, _1, q), angles); +// J_expected = gtsam::numericalDerivative11(boost::bind(&predictionRq, _1, q), angles); // -// cout<<"J_hyp"< factor(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model); + InertialNavFactor_GlobalVelocity factor(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model); - Rot3 R1(0.487316618, 0.125253866, 0.86419557, - 0.580273724, 0.693095498, -0.427669306, - -0.652537293, 0.709880342, 0.265075427); - Point3 t1(2.0,1.0,3.0); - Pose3 Pose1(R1, t1); - LieVector Vel1(3,0.5,-0.5,0.4); - Rot3 R2(0.473618898, 0.119523052, 0.872582019, - 0.609241153, 0.67099888, -0.422594037, - -0.636011287, 0.731761397, 0.244979388); - Point3 t2(2.052670960415706, 0.977252139079380, 2.942482135362800); - Pose3 Pose2(R2, t2); - LieVector Vel2(3,0.510000000000000, -0.480000000000000, 0.430000000000000); - imuBias::ConstantBias Bias1; + Rot3 R1(0.487316618, 0.125253866, 0.86419557, + 0.580273724, 0.693095498, -0.427669306, + -0.652537293, 0.709880342, 0.265075427); + Point3 t1(2.0,1.0,3.0); + Pose3 Pose1(R1, t1); + LieVector Vel1(3,0.5,-0.5,0.4); + Rot3 R2(0.473618898, 0.119523052, 0.872582019, + 0.609241153, 0.67099888, -0.422594037, + -0.636011287, 0.731761397, 0.244979388); + Point3 t2(2.052670960415706, 0.977252139079380, 2.942482135362800); + Pose3 Pose2(R2, t2); + LieVector Vel2(3,0.510000000000000, -0.480000000000000, 0.430000000000000); + imuBias::ConstantBias Bias1; - Matrix H1_actual, H2_actual, H3_actual, H4_actual, H5_actual; + Matrix H1_actual, H2_actual, H3_actual, H4_actual, H5_actual; - Vector ActualErr(factor.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2, H1_actual, H2_actual, H3_actual, H4_actual, H5_actual)); + Vector ActualErr(factor.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2, H1_actual, H2_actual, H3_actual, H4_actual, H5_actual)); - // Checking for Pose part in the jacobians - // ****** - Matrix H1_actualPose(H1_actual.block(0,0,6,H1_actual.cols())); - Matrix H2_actualPose(H2_actual.block(0,0,6,H2_actual.cols())); - Matrix H3_actualPose(H3_actual.block(0,0,6,H3_actual.cols())); - Matrix H4_actualPose(H4_actual.block(0,0,6,H4_actual.cols())); - Matrix H5_actualPose(H5_actual.block(0,0,6,H5_actual.cols())); + // Checking for Pose part in the jacobians + // ****** + Matrix H1_actualPose(H1_actual.block(0,0,6,H1_actual.cols())); + Matrix H2_actualPose(H2_actual.block(0,0,6,H2_actual.cols())); + Matrix H3_actualPose(H3_actual.block(0,0,6,H3_actual.cols())); + Matrix H4_actualPose(H4_actual.block(0,0,6,H4_actual.cols())); + Matrix H5_actualPose(H5_actual.block(0,0,6,H5_actual.cols())); - // Calculate the Jacobian matrices H1 until H5 using the numerical derivative function - gtsam::Matrix H1_expectedPose, H2_expectedPose, H3_expectedPose, H4_expectedPose, H5_expectedPose; - H1_expectedPose = gtsam::numericalDerivative11(boost::bind(&predictionErrorPose, _1, Vel1, Bias1, Pose2, Vel2, factor), Pose1); - H2_expectedPose = gtsam::numericalDerivative11(boost::bind(&predictionErrorPose, Pose1, _1, Bias1, Pose2, Vel2, factor), Vel1); - H3_expectedPose = gtsam::numericalDerivative11(boost::bind(&predictionErrorPose, Pose1, Vel1, _1, Pose2, Vel2, factor), Bias1); - H4_expectedPose = gtsam::numericalDerivative11(boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, _1, Vel2, factor), Pose2); - H5_expectedPose = gtsam::numericalDerivative11(boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, Pose2, _1, factor), Vel2); + // Calculate the Jacobian matrices H1 until H5 using the numerical derivative function + gtsam::Matrix H1_expectedPose, H2_expectedPose, H3_expectedPose, H4_expectedPose, H5_expectedPose; + H1_expectedPose = gtsam::numericalDerivative11(boost::bind(&predictionErrorPose, _1, Vel1, Bias1, Pose2, Vel2, factor), Pose1); + H2_expectedPose = gtsam::numericalDerivative11(boost::bind(&predictionErrorPose, Pose1, _1, Bias1, Pose2, Vel2, factor), Vel1); + H3_expectedPose = gtsam::numericalDerivative11(boost::bind(&predictionErrorPose, Pose1, Vel1, _1, Pose2, Vel2, factor), Bias1); + H4_expectedPose = gtsam::numericalDerivative11(boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, _1, Vel2, factor), Pose2); + H5_expectedPose = gtsam::numericalDerivative11(boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, Pose2, _1, factor), Vel2); - // Verify they are equal for this choice of state - CHECK( gtsam::assert_equal(H1_expectedPose, H1_actualPose, 1e-5)); - CHECK( gtsam::assert_equal(H2_expectedPose, H2_actualPose, 1e-5)); - CHECK( gtsam::assert_equal(H3_expectedPose, H3_actualPose, 2e-3)); - CHECK( gtsam::assert_equal(H4_expectedPose, H4_actualPose, 1e-5)); - CHECK( gtsam::assert_equal(H5_expectedPose, H5_actualPose, 1e-5)); + // Verify they are equal for this choice of state + CHECK( gtsam::assert_equal(H1_expectedPose, H1_actualPose, 1e-5)); + CHECK( gtsam::assert_equal(H2_expectedPose, H2_actualPose, 1e-5)); + CHECK( gtsam::assert_equal(H3_expectedPose, H3_actualPose, 2e-3)); + CHECK( gtsam::assert_equal(H4_expectedPose, H4_actualPose, 1e-5)); + CHECK( gtsam::assert_equal(H5_expectedPose, H5_actualPose, 1e-5)); - // Checking for Vel part in the jacobians - // ****** - Matrix H1_actualVel(H1_actual.block(6,0,3,H1_actual.cols())); - Matrix H2_actualVel(H2_actual.block(6,0,3,H2_actual.cols())); - Matrix H3_actualVel(H3_actual.block(6,0,3,H3_actual.cols())); - Matrix H4_actualVel(H4_actual.block(6,0,3,H4_actual.cols())); - Matrix H5_actualVel(H5_actual.block(6,0,3,H5_actual.cols())); + // Checking for Vel part in the jacobians + // ****** + Matrix H1_actualVel(H1_actual.block(6,0,3,H1_actual.cols())); + Matrix H2_actualVel(H2_actual.block(6,0,3,H2_actual.cols())); + Matrix H3_actualVel(H3_actual.block(6,0,3,H3_actual.cols())); + Matrix H4_actualVel(H4_actual.block(6,0,3,H4_actual.cols())); + Matrix H5_actualVel(H5_actual.block(6,0,3,H5_actual.cols())); - // Calculate the Jacobian matrices H1 until H5 using the numerical derivative function - gtsam::Matrix H1_expectedVel, H2_expectedVel, H3_expectedVel, H4_expectedVel, H5_expectedVel; - H1_expectedVel = gtsam::numericalDerivative11(boost::bind(&predictionErrorVel, _1, Vel1, Bias1, Pose2, Vel2, factor), Pose1); - H2_expectedVel = gtsam::numericalDerivative11(boost::bind(&predictionErrorVel, Pose1, _1, Bias1, Pose2, Vel2, factor), Vel1); - H3_expectedVel = gtsam::numericalDerivative11(boost::bind(&predictionErrorVel, Pose1, Vel1, _1, Pose2, Vel2, factor), Bias1); - H4_expectedVel = gtsam::numericalDerivative11(boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, _1, Vel2, factor), Pose2); - H5_expectedVel = gtsam::numericalDerivative11(boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, Pose2, _1, factor), Vel2); + // Calculate the Jacobian matrices H1 until H5 using the numerical derivative function + gtsam::Matrix H1_expectedVel, H2_expectedVel, H3_expectedVel, H4_expectedVel, H5_expectedVel; + H1_expectedVel = gtsam::numericalDerivative11(boost::bind(&predictionErrorVel, _1, Vel1, Bias1, Pose2, Vel2, factor), Pose1); + H2_expectedVel = gtsam::numericalDerivative11(boost::bind(&predictionErrorVel, Pose1, _1, Bias1, Pose2, Vel2, factor), Vel1); + H3_expectedVel = gtsam::numericalDerivative11(boost::bind(&predictionErrorVel, Pose1, Vel1, _1, Pose2, Vel2, factor), Bias1); + H4_expectedVel = gtsam::numericalDerivative11(boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, _1, Vel2, factor), Pose2); + H5_expectedVel = gtsam::numericalDerivative11(boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, Pose2, _1, factor), Vel2); - // Verify they are equal for this choice of state - CHECK( gtsam::assert_equal(H1_expectedVel, H1_actualVel, 1e-5)); - CHECK( gtsam::assert_equal(H2_expectedVel, H2_actualVel, 1e-5)); - CHECK( gtsam::assert_equal(H3_expectedVel, H3_actualVel, 1e-5)); - CHECK( gtsam::assert_equal(H4_expectedVel, H4_actualVel, 1e-5)); - CHECK( gtsam::assert_equal(H5_expectedVel, H5_actualVel, 1e-5)); + // Verify they are equal for this choice of state + CHECK( gtsam::assert_equal(H1_expectedVel, H1_actualVel, 1e-5)); + CHECK( gtsam::assert_equal(H2_expectedVel, H2_actualVel, 1e-5)); + CHECK( gtsam::assert_equal(H3_expectedVel, H3_actualVel, 1e-5)); + CHECK( gtsam::assert_equal(H4_expectedVel, H4_actualVel, 1e-5)); + CHECK( gtsam::assert_equal(H5_expectedVel, H5_actualVel, 1e-5)); } @@ -679,5 +679,5 @@ TEST (InertialNavFactor_GlobalVelocity, JacobianWithTransform ) { } /* ************************************************************************* */ - int main() { TestResult tr; return TestRegistry::runAllTests(tr);} + int main() { TestResult tr; return TestRegistry::runAllTests(tr);} /* ************************************************************************* */ diff --git a/gtsam_unstable/slam/tests/timeInertialNavFactor_GlobalVelocity.cpp b/gtsam_unstable/slam/tests/timeInertialNavFactor_GlobalVelocity.cpp index f036eadeb..3fcdceefe 100644 --- a/gtsam_unstable/slam/tests/timeInertialNavFactor_GlobalVelocity.cpp +++ b/gtsam_unstable/slam/tests/timeInertialNavFactor_GlobalVelocity.cpp @@ -28,9 +28,9 @@ using namespace std; using namespace gtsam; gtsam::Rot3 world_R_ECEF( - 0.31686, 0.51505, 0.79645, - 0.85173, -0.52399, 0, - 0.41733, 0.67835, -0.60471); + 0.31686, 0.51505, 0.79645, + 0.85173, -0.52399, 0, + 0.41733, 0.67835, -0.60471); gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5)); gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);