diff --git a/README.md b/README.md index 679af5a2f..ccdc7f07e 100644 --- a/README.md +++ b/README.md @@ -31,6 +31,7 @@ Prerequisites: - [Boost](http://www.boost.org/users/download/) >= 1.43 (Ubuntu: `sudo apt-get install libboost-all-dev`) - [CMake](http://www.cmake.org/cmake/resources/software.html) >= 2.6 (Ubuntu: `sudo apt-get install cmake`) +- A modern compiler, i.e., at least gcc 4.7.3 on Linux. Optional prerequisites - used automatically if findable by CMake: @@ -46,6 +47,6 @@ See the [`INSTALL`](INSTALL) file for more detailed installation instructions. GTSAM is open source under the BSD license, see the [`LICENSE`](LICENSE) and [`LICENSE.BSD`](LICENSE.BSD) files. -Please see the [`examples/`](examples) directory and the [`USAGE`](USAGE.md) file for examples on how to use GTSAM. - +Please see the [`examples/`](examples) directory and the [`USAGE`](USAGE.md) file for examples on how to use GTSAM. + GTSAM was developed in the lab of [Frank Dellaert](http://www.cc.gatech.edu/~dellaert) at the [Georgia Institute of Technology](http://www.gatech.edu), with the help of many contributors over the years, see [THANKS](THANKS). \ No newline at end of file diff --git a/gtsam.h b/gtsam.h index 492e9de30..8640b1492 100644 --- a/gtsam.h +++ b/gtsam.h @@ -2474,18 +2474,18 @@ class ConstantBias { class PoseVelocityBias{ PoseVelocityBias(const gtsam::Pose3& pose, Vector velocity, const gtsam::imuBias::ConstantBias& bias); }; -class ImuFactorPreintegratedMeasurements { +class PreintegratedImuMeasurements { // Standard Constructor - ImuFactorPreintegratedMeasurements(const gtsam::imuBias::ConstantBias& bias, Matrix measuredAccCovariance,Matrix measuredOmegaCovariance, Matrix integrationErrorCovariance, bool use2ndOrderIntegration); - ImuFactorPreintegratedMeasurements(const gtsam::imuBias::ConstantBias& bias, Matrix measuredAccCovariance,Matrix measuredOmegaCovariance, Matrix integrationErrorCovariance); - // ImuFactorPreintegratedMeasurements(const gtsam::ImuFactorPreintegratedMeasurements& rhs); + PreintegratedImuMeasurements(const gtsam::imuBias::ConstantBias& bias, Matrix measuredAccCovariance,Matrix measuredOmegaCovariance, Matrix integrationErrorCovariance, bool use2ndOrderIntegration); + PreintegratedImuMeasurements(const gtsam::imuBias::ConstantBias& bias, Matrix measuredAccCovariance,Matrix measuredOmegaCovariance, Matrix integrationErrorCovariance); + // PreintegratedImuMeasurements(const gtsam::PreintegratedImuMeasurements& rhs); // Testable void print(string s) const; - bool equals(const gtsam::ImuFactorPreintegratedMeasurements& expected, double tol); + bool equals(const gtsam::PreintegratedImuMeasurements& expected, double tol); double deltaTij() const; - Matrix deltaRij() const; + gtsam::Rot3 deltaRij() const; Vector deltaPij() const; Vector deltaVij() const; Vector biasHatVector() const; @@ -2498,25 +2498,24 @@ class ImuFactorPreintegratedMeasurements { // Standard Interface void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT); - void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT, const gtsam::Pose3& body_P_sensor); gtsam::PoseVelocityBias predict(const gtsam::Pose3& pose_i, Vector vel_i, const gtsam::imuBias::ConstantBias& bias, Vector gravity, Vector omegaCoriolis) const; }; virtual class ImuFactor : gtsam::NonlinearFactor { ImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, size_t bias, - const gtsam::ImuFactorPreintegratedMeasurements& preintegratedMeasurements, Vector gravity, Vector omegaCoriolis); + const gtsam::PreintegratedImuMeasurements& preintegratedMeasurements, Vector gravity, Vector omegaCoriolis); ImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, size_t bias, - const gtsam::ImuFactorPreintegratedMeasurements& preintegratedMeasurements, Vector gravity, Vector omegaCoriolis, + const gtsam::PreintegratedImuMeasurements& preintegratedMeasurements, Vector gravity, Vector omegaCoriolis, const gtsam::Pose3& body_P_sensor); // Standard Interface - gtsam::ImuFactorPreintegratedMeasurements preintegratedMeasurements() const; + gtsam::PreintegratedImuMeasurements preintegratedMeasurements() const; }; #include -class CombinedImuFactorPreintegratedMeasurements { +class PreintegratedCombinedMeasurements { // Standard Constructor - CombinedImuFactorPreintegratedMeasurements( + PreintegratedCombinedMeasurements( const gtsam::imuBias::ConstantBias& bias, Matrix measuredAccCovariance, Matrix measuredOmegaCovariance, @@ -2524,7 +2523,7 @@ class CombinedImuFactorPreintegratedMeasurements { Matrix biasAccCovariance, Matrix biasOmegaCovariance, Matrix biasAccOmegaInit); - CombinedImuFactorPreintegratedMeasurements( + PreintegratedCombinedMeasurements( const gtsam::imuBias::ConstantBias& bias, Matrix measuredAccCovariance, Matrix measuredOmegaCovariance, @@ -2533,14 +2532,14 @@ class CombinedImuFactorPreintegratedMeasurements { Matrix biasOmegaCovariance, Matrix biasAccOmegaInit, bool use2ndOrderIntegration); - // CombinedImuFactorPreintegratedMeasurements(const gtsam::CombinedImuFactorPreintegratedMeasurements& rhs); + // PreintegratedCombinedMeasurements(const gtsam::PreintegratedCombinedMeasurements& rhs); // Testable void print(string s) const; - bool equals(const gtsam::CombinedImuFactorPreintegratedMeasurements& expected, double tol); + bool equals(const gtsam::PreintegratedCombinedMeasurements& expected, double tol); double deltaTij() const; - Matrix deltaRij() const; + gtsam::Rot3 deltaRij() const; Vector deltaPij() const; Vector deltaVij() const; Vector biasHatVector() const; @@ -2553,53 +2552,51 @@ class CombinedImuFactorPreintegratedMeasurements { // Standard Interface void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT); - void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT, const gtsam::Pose3& body_P_sensor); gtsam::PoseVelocityBias predict(const gtsam::Pose3& pose_i, Vector vel_i, const gtsam::imuBias::ConstantBias& bias, Vector gravity, Vector omegaCoriolis) const; }; virtual class CombinedImuFactor : gtsam::NonlinearFactor { CombinedImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, size_t bias_i, size_t bias_j, - const gtsam::CombinedImuFactorPreintegratedMeasurements& CombinedPreintegratedMeasurements, Vector gravity, Vector omegaCoriolis); + const gtsam::PreintegratedCombinedMeasurements& CombinedPreintegratedMeasurements, Vector gravity, Vector omegaCoriolis); // Standard Interface - gtsam::CombinedImuFactorPreintegratedMeasurements preintegratedMeasurements() const; + gtsam::PreintegratedCombinedMeasurements preintegratedMeasurements() const; }; #include -class AHRSFactorPreintegratedMeasurements { +class PreintegratedAhrsMeasurements { // Standard Constructor - AHRSFactorPreintegratedMeasurements(Vector bias, Matrix measuredOmegaCovariance); - AHRSFactorPreintegratedMeasurements(Vector bias, Matrix measuredOmegaCovariance); - AHRSFactorPreintegratedMeasurements(const gtsam::AHRSFactorPreintegratedMeasurements& rhs); + PreintegratedAhrsMeasurements(Vector bias, Matrix measuredOmegaCovariance); + PreintegratedAhrsMeasurements(Vector bias, Matrix measuredOmegaCovariance); + PreintegratedAhrsMeasurements(const gtsam::PreintegratedAhrsMeasurements& rhs); // Testable void print(string s) const; - bool equals(const gtsam::AHRSFactorPreintegratedMeasurements& expected, double tol); + bool equals(const gtsam::PreintegratedAhrsMeasurements& expected, double tol); // get Data - Matrix deltaRij() const; + gtsam::Rot3 deltaRij() const; double deltaTij() const; Vector biasHat() const; // Standard Interface void integrateMeasurement(Vector measuredOmega, double deltaT); - void integrateMeasurement(Vector measuredOmega, double deltaT, const gtsam::Pose3& body_P_sensor); void resetIntegration() ; }; virtual class AHRSFactor : gtsam::NonlinearFactor { AHRSFactor(size_t rot_i, size_t rot_j,size_t bias, - const gtsam::AHRSFactorPreintegratedMeasurements& preintegratedMeasurements, Vector omegaCoriolis); + const gtsam::PreintegratedAhrsMeasurements& preintegratedMeasurements, Vector omegaCoriolis); AHRSFactor(size_t rot_i, size_t rot_j, size_t bias, - const gtsam::AHRSFactorPreintegratedMeasurements& preintegratedMeasurements, Vector omegaCoriolis, + const gtsam::PreintegratedAhrsMeasurements& preintegratedMeasurements, Vector omegaCoriolis, const gtsam::Pose3& body_P_sensor); // Standard Interface - gtsam::AHRSFactorPreintegratedMeasurements preintegratedMeasurements() const; + gtsam::PreintegratedAhrsMeasurements preintegratedMeasurements() const; Vector evaluateError(const gtsam::Rot3& rot_i, const gtsam::Rot3& rot_j, Vector bias) const; gtsam::Rot3 predict(const gtsam::Rot3& rot_i, Vector bias, - const gtsam::AHRSFactorPreintegratedMeasurements& preintegratedMeasurements, + const gtsam::PreintegratedAhrsMeasurements& preintegratedMeasurements, Vector omegaCoriolis) const; }; diff --git a/gtsam/base/ProductLieGroup.h b/gtsam/base/ProductLieGroup.h index 90aeb54d1..463b5f5d9 100644 --- a/gtsam/base/ProductLieGroup.h +++ b/gtsam/base/ProductLieGroup.h @@ -55,7 +55,7 @@ public: traits::Compose(this->second,other.second)); } ProductLieGroup inverse() const { - return ProductLieGroup(this->first.inverse(), this->second.inverse()); + return ProductLieGroup(traits::Inverse(this->first), traits::Inverse(this->second)); } ProductLieGroup compose(const ProductLieGroup& g) const { return (*this) * g; @@ -74,17 +74,21 @@ public: typedef Eigen::Matrix TangentVector; typedef OptionalJacobian ChartJacobian; - static ProductLieGroup Retract(const TangentVector& v) { - return ProductLieGroup::ChartAtOrigin::Retract(v); + ProductLieGroup retract(const TangentVector& v, // + ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) const { + if (H1||H2) throw std::runtime_error("ProductLieGroup::retract derivatives not implemented yet"); + G g = traits::Retract(this->first, v.template head()); + H h = traits::Retract(this->second, v.template tail()); + return ProductLieGroup(g,h); } - static TangentVector LocalCoordinates(const ProductLieGroup& g) { - return ProductLieGroup::ChartAtOrigin::Local(g); - } - ProductLieGroup retract(const TangentVector& v) const { - return compose(ProductLieGroup::ChartAtOrigin::Retract(v)); - } - TangentVector localCoordinates(const ProductLieGroup& g) const { - return ProductLieGroup::ChartAtOrigin::Local(between(g)); + TangentVector localCoordinates(const ProductLieGroup& g, // + ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) const { + if (H1||H2) throw std::runtime_error("ProductLieGroup::localCoordinates derivatives not implemented yet"); + typename traits::TangentVector v1 = traits::Local(this->first, g.first); + typename traits::TangentVector v2 = traits::Local(this->second, g.second); + TangentVector v; + v << v1, v2; + return v; } /// @} @@ -147,51 +151,19 @@ public: v << v1, v2; return v; } - struct ChartAtOrigin { - static TangentVector Local(const ProductLieGroup& m, ChartJacobian Hm = boost::none) { - return Logmap(m, Hm); - } - static ProductLieGroup Retract(const TangentVector& v, ChartJacobian Hv = boost::none) { - return Expmap(v, Hv); - } - }; ProductLieGroup expmap(const TangentVector& v) const { return compose(ProductLieGroup::Expmap(v)); } TangentVector logmap(const ProductLieGroup& g) const { return ProductLieGroup::Logmap(between(g)); } - static ProductLieGroup Retract(const TangentVector& v, ChartJacobian H1) { - return ProductLieGroup::ChartAtOrigin::Retract(v,H1); - } - static TangentVector LocalCoordinates(const ProductLieGroup& g, ChartJacobian H1) { - return ProductLieGroup::ChartAtOrigin::Local(g,H1); - } - ProductLieGroup retract(const TangentVector& v, // - ChartJacobian H1, ChartJacobian H2 = boost::none) const { - Jacobian D_g_v; - ProductLieGroup g = ProductLieGroup::ChartAtOrigin::Retract(v,H2 ? &D_g_v : 0); - ProductLieGroup h = compose(g,H1,H2); - if (H2) *H2 = (*H2) * D_g_v; - return h; - } - TangentVector localCoordinates(const ProductLieGroup& g, // - ChartJacobian H1, ChartJacobian H2 = boost::none) const { - ProductLieGroup h = between(g,H1,H2); - Jacobian D_v_h; - TangentVector v = ProductLieGroup::ChartAtOrigin::Local(h, (H1 || H2) ? &D_v_h : 0); - if (H1) *H1 = D_v_h * (*H1); - if (H2) *H2 = D_v_h * (*H2); - return v; - } /// @} }; // Define any direct product group to be a model of the multiplicative Group concept template -struct traits > : internal::LieGroupTraits< - ProductLieGroup > { -}; +struct traits > : internal::LieGroupTraits > {}; + } // namespace gtsam diff --git a/gtsam/base/Testable.h b/gtsam/base/Testable.h index 4790530ab..92ccb9156 100644 --- a/gtsam/base/Testable.h +++ b/gtsam/base/Testable.h @@ -123,8 +123,8 @@ namespace gtsam { double tol_; equals_star(double tol = 1e-9) : tol_(tol) {} bool operator()(const boost::shared_ptr& expected, const boost::shared_ptr& actual) { - if (!actual || !expected) return false; - return (traits::Equals(*actual,*expected, tol_)); + if (!actual || !expected) return true; + return actual && expected && traits::Equals(*actual,*expected, tol_); } }; diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 3f46df40d..f373e5ad4 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -387,7 +387,7 @@ boost::optional align(const vector& pairs) { Matrix3 UVtranspose = U * V.transpose(); Matrix3 detWeighting = I_3x3; detWeighting(2, 2) = UVtranspose.determinant(); - Rot3 R(Matrix(V * detWeighting * U.transpose())); + Rot3 R(Matrix3(V * detWeighting * U.transpose())); Point3 t = Point3(cq) - R * Point3(cp); return Pose3(R, t); } diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 608f41954..e548f8eea 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -85,11 +85,33 @@ namespace gtsam { double R21, double R22, double R23, double R31, double R32, double R33); - /** constructor from a rotation matrix */ - Rot3(const Matrix3& R); + /** + * Constructor from a rotation matrix + * Version for generic matrices. Need casting to Matrix3 + * in quaternion mode, since Eigen's quaternion doesn't + * allow assignment/construction from a generic matrix. + * See: http://stackoverflow.com/questions/27094132/cannot-understand-if-this-is-circular-dependency-or-clang#tab-top + */ + template + inline Rot3(const Eigen::MatrixBase& R) { + #ifdef GTSAM_USE_QUATERNIONS + quaternion_=Matrix3(R); + #else + rot_ = R; + #endif + } - /** constructor from a rotation matrix */ - Rot3(const Matrix& R); + /** + * Constructor from a rotation matrix + * Overload version for Matrix3 to avoid casting in quaternion mode. + */ + inline Rot3(const Matrix3& R) { + #ifdef GTSAM_USE_QUATERNIONS + quaternion_=R; + #else + rot_ = R; + #endif + } /** Constructor from a quaternion. This can also be called using a plain * Vector, due to implicit conversion from Vector to Quaternion @@ -330,6 +352,17 @@ namespace gtsam { Point3 rotate(const Point3& p, OptionalJacobian<3,3> H1 = boost::none, OptionalJacobian<3,3> H2 = boost::none) const; + /// operator* for Vector3 + inline Vector3 operator*(const Vector3& v) const { + return rotate(Point3(v)).vector(); + } + + /// rotate for Vector3 + Vector3 rotate(const Vector3& v, OptionalJacobian<3, 3> H1 = boost::none, + OptionalJacobian<3, 3> H2 = boost::none) const { + return rotate(Point3(v), H1, H2).vector(); + } + /// rotate point from rotated coordinate frame to world = R*p Point3 operator*(const Point3& p) const; @@ -337,6 +370,12 @@ namespace gtsam { Point3 unrotate(const Point3& p, OptionalJacobian<3,3> H1 = boost::none, OptionalJacobian<3,3> H2=boost::none) const; + /// unrotate for Vector3 + Vector3 unrotate(const Vector3& v, OptionalJacobian<3, 3> H1 = boost::none, + OptionalJacobian<3, 3> H2 = boost::none) const { + return unrotate(Point3(v), H1, H2).vector(); + } + /// @} /// @name Group Action on Unit3 /// @{ diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index 18628aec3..a7b654070 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -50,18 +50,6 @@ Rot3::Rot3(double R11, double R12, double R13, R31, R32, R33; } -/* ************************************************************************* */ -Rot3::Rot3(const Matrix3& R) { - rot_ = R; -} - -/* ************************************************************************* */ -Rot3::Rot3(const Matrix& R) { - if (R.rows()!=3 || R.cols()!=3) - throw invalid_argument("Rot3 constructor expects 3*3 matrix"); - rot_ = R; -} - /* ************************************************************************* */ Rot3::Rot3(const Quaternion& q) : rot_(q.toRotationMatrix()) { } @@ -131,10 +119,8 @@ Matrix3 Rot3::transpose() const { /* ************************************************************************* */ Point3 Rot3::rotate(const Point3& p, OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const { - if (H1 || H2) { - if (H1) *H1 = rot_ * skewSymmetric(-p.x(), -p.y(), -p.z()); - if (H2) *H2 = rot_; - } + if (H1) *H1 = rot_ * skewSymmetric(-p.x(), -p.y(), -p.z()); + if (H2) *H2 = rot_; return Point3(rot_ * p.vector()); } diff --git a/gtsam/geometry/Rot3Q.cpp b/gtsam/geometry/Rot3Q.cpp index c97e76718..b255b082d 100644 --- a/gtsam/geometry/Rot3Q.cpp +++ b/gtsam/geometry/Rot3Q.cpp @@ -48,14 +48,6 @@ namespace gtsam { R21, R22, R23, R31, R32, R33).finished()) {} - /* ************************************************************************* */ - Rot3::Rot3(const Matrix3& R) : - quaternion_(R) {} - - /* ************************************************************************* */ - Rot3::Rot3(const Matrix& R) : - quaternion_(Matrix3(R)) {} - /* ************************************************************************* */ Rot3::Rot3(const Quaternion& q) : quaternion_(q) { diff --git a/gtsam/geometry/tests/testCalibratedCamera.cpp b/gtsam/geometry/tests/testCalibratedCamera.cpp index 199ae30ce..3b0906b44 100644 --- a/gtsam/geometry/tests/testCalibratedCamera.cpp +++ b/gtsam/geometry/tests/testCalibratedCamera.cpp @@ -30,7 +30,7 @@ using namespace gtsam; GTSAM_CONCEPT_MANIFOLD_INST(CalibratedCamera) // Camera situated at 0.5 meters high, looking down -static const Pose3 pose1((Matrix)(Matrix(3,3) << +static const Pose3 pose1((Matrix3() << 1., 0., 0., 0.,-1., 0., 0., 0.,-1. diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index f6f8b7b40..9007ce1bd 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -182,8 +182,8 @@ TEST(Pose3, expmaps_galore_full) xi = (Vector(6) << 0.2, 0.3, -0.8, 100.0, 120.0, -60.0).finished(); actual = Pose3::Expmap(xi); EXPECT(assert_equal(expm(xi,10), actual,1e-5)); - EXPECT(assert_equal(Agrawal06iros(xi), actual,1e-6)); - EXPECT(assert_equal(xi, Pose3::Logmap(actual),1e-6)); + EXPECT(assert_equal(Agrawal06iros(xi), actual,1e-9)); + EXPECT(assert_equal(xi, Pose3::Logmap(actual),1e-9)); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testSimpleCamera.cpp b/gtsam/geometry/tests/testSimpleCamera.cpp index 71979481c..515542298 100644 --- a/gtsam/geometry/tests/testSimpleCamera.cpp +++ b/gtsam/geometry/tests/testSimpleCamera.cpp @@ -28,7 +28,7 @@ using namespace gtsam; static const Cal3_S2 K(625, 625, 0, 0, 0); -static const Pose3 pose1((Matrix)(Matrix(3,3) << +static const Pose3 pose1((Matrix3() << 1., 0., 0., 0.,-1., 0., 0., 0.,-1. diff --git a/gtsam/linear/Sampler.cpp b/gtsam/linear/Sampler.cpp index 48972a953..bfbc222ba 100644 --- a/gtsam/linear/Sampler.cpp +++ b/gtsam/linear/Sampler.cpp @@ -14,9 +14,6 @@ * @author Alex Cunningham */ -#include -#include - #include namespace gtsam { @@ -51,7 +48,7 @@ Vector Sampler::sampleDiagonal(const Vector& sigmas) { } else { typedef boost::normal_distribution Normal; Normal dist(0.0, sigma); - boost::variate_generator norm(generator_, dist); + boost::variate_generator norm(generator_, dist); result(i) = norm(); } } diff --git a/gtsam/linear/Sampler.h b/gtsam/linear/Sampler.h index f2632308b..6c84bfda2 100644 --- a/gtsam/linear/Sampler.h +++ b/gtsam/linear/Sampler.h @@ -20,7 +20,7 @@ #include -#include +#include namespace gtsam { @@ -37,7 +37,7 @@ protected: noiseModel::Diagonal::shared_ptr model_; /** generator */ - boost::minstd_rand generator_; + boost::mt19937_64 generator_; public: typedef boost::shared_ptr shared_ptr; diff --git a/gtsam/navigation/AHRSFactor.cpp b/gtsam/navigation/AHRSFactor.cpp index 2f03d72a4..0d7e05515 100644 --- a/gtsam/navigation/AHRSFactor.cpp +++ b/gtsam/navigation/AHRSFactor.cpp @@ -27,83 +27,50 @@ namespace gtsam { //------------------------------------------------------------------------------ // Inner class PreintegratedMeasurements //------------------------------------------------------------------------------ -AHRSFactor::PreintegratedMeasurements::PreintegratedMeasurements( - const Vector3& bias, const Matrix3& measuredOmegaCovariance) : - PreintegratedRotation(measuredOmegaCovariance), biasHat_(bias) -{ - preintMeasCov_.setZero(); -} - -//------------------------------------------------------------------------------ -AHRSFactor::PreintegratedMeasurements::PreintegratedMeasurements() : - PreintegratedRotation(I_3x3), biasHat_(Vector3()) -{ - preintMeasCov_.setZero(); -} - -//------------------------------------------------------------------------------ -void AHRSFactor::PreintegratedMeasurements::print(const string& s) const { +void PreintegratedAhrsMeasurements::print(const string& s) const { PreintegratedRotation::print(s); cout << "biasHat [" << biasHat_.transpose() << "]" << endl; cout << " PreintMeasCov [ " << preintMeasCov_ << " ]" << endl; } //------------------------------------------------------------------------------ -bool AHRSFactor::PreintegratedMeasurements::equals( - const PreintegratedMeasurements& other, double tol) const { - return PreintegratedRotation::equals(other, tol) - && equal_with_abs_tol(biasHat_, other.biasHat_, tol); +bool PreintegratedAhrsMeasurements::equals( + const PreintegratedAhrsMeasurements& other, double tol) const { + return PreintegratedRotation::equals(other, tol) && + equal_with_abs_tol(biasHat_, other.biasHat_, tol); } //------------------------------------------------------------------------------ -void AHRSFactor::PreintegratedMeasurements::resetIntegration() { +void PreintegratedAhrsMeasurements::resetIntegration() { PreintegratedRotation::resetIntegration(); preintMeasCov_.setZero(); } //------------------------------------------------------------------------------ -void AHRSFactor::PreintegratedMeasurements::integrateMeasurement( - const Vector3& measuredOmega, double deltaT, - boost::optional body_P_sensor) { +void PreintegratedAhrsMeasurements::integrateMeasurement( + const Vector3& measuredOmega, double deltaT) { - // First we compensate the measurements for the bias - Vector3 correctedOmega = measuredOmega - biasHat_; - - // Then compensate for sensor-body displacement: we express the quantities - // (originally in the IMU frame) into the body frame - if (body_P_sensor) { - Matrix3 body_R_sensor = body_P_sensor->rotation().matrix(); - // rotation rate vector in the body frame - correctedOmega = body_R_sensor * correctedOmega; - } - - // rotation vector describing rotation increment computed from the - // current rotation rate measurement - const Vector3 theta_incr = correctedOmega * deltaT; - Matrix3 D_Rincr_integratedOmega; - const Rot3 incrR = Rot3::Expmap(theta_incr, D_Rincr_integratedOmega); // expensive !! - - // Update Jacobian - update_delRdelBiasOmega(D_Rincr_integratedOmega, incrR, deltaT); - - // Update rotation and deltaTij. - Matrix3 Fr; // Jacobian of the update - updateIntegratedRotationAndDeltaT(incrR, deltaT, Fr); + Matrix3 D_incrR_integratedOmega, Fr; + PreintegratedRotation::integrateMeasurement(measuredOmega, + biasHat_, deltaT, &D_incrR_integratedOmega, &Fr); // first order uncertainty propagation // the deltaT allows to pass from continuous time noise to discrete time noise - preintMeasCov_ = Fr * preintMeasCov_ * Fr.transpose() - + gyroscopeCovariance() * deltaT; + preintMeasCov_ = Fr * preintMeasCov_ * Fr.transpose() + p().gyroscopeCovariance * deltaT; } //------------------------------------------------------------------------------ -Vector3 AHRSFactor::PreintegratedMeasurements::predict(const Vector3& bias, - boost::optional H) const { +Vector3 PreintegratedAhrsMeasurements::predict(const Vector3& bias, + OptionalJacobian<3,3> H) const { const Vector3 biasOmegaIncr = bias - biasHat_; - return biascorrectedThetaRij(biasOmegaIncr, H); + const Rot3 biascorrected = biascorrectedDeltaRij(biasOmegaIncr, H); + Matrix3 D_omega_biascorrected; + const Vector3 omega = Rot3::Logmap(biascorrected, H ? &D_omega_biascorrected : 0); + if (H) (*H) = D_omega_biascorrected * (*H); + return omega; } //------------------------------------------------------------------------------ -Vector AHRSFactor::PreintegratedMeasurements::DeltaAngles( +Vector PreintegratedAhrsMeasurements::DeltaAngles( const Vector& msr_gyro_t, const double msr_dt, const Vector3& delta_angles) { @@ -121,22 +88,16 @@ Vector AHRSFactor::PreintegratedMeasurements::DeltaAngles( //------------------------------------------------------------------------------ // AHRSFactor methods //------------------------------------------------------------------------------ -AHRSFactor::AHRSFactor() : - _PIM_(Vector3(), Z_3x3) { -} +AHRSFactor::AHRSFactor( + Key rot_i, Key rot_j, Key bias, + const PreintegratedAhrsMeasurements& preintegratedMeasurements) + : Base(noiseModel::Gaussian::Covariance( + preintegratedMeasurements.preintMeasCov_), + rot_i, rot_j, bias), + _PIM_(preintegratedMeasurements) {} -AHRSFactor::AHRSFactor(Key rot_i, Key rot_j, Key bias, - const PreintegratedMeasurements& preintegratedMeasurements, - const Vector3& omegaCoriolis, boost::optional body_P_sensor) : - Base( - noiseModel::Gaussian::Covariance( - preintegratedMeasurements.preintMeasCov_), rot_i, rot_j, bias), _PIM_( - preintegratedMeasurements), omegaCoriolis_(omegaCoriolis), body_P_sensor_( - body_P_sensor) { -} - -//------------------------------------------------------------------------------ gtsam::NonlinearFactor::shared_ptr AHRSFactor::clone() const { +//------------------------------------------------------------------------------ return boost::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } @@ -147,20 +108,13 @@ void AHRSFactor::print(const string& s, cout << s << "AHRSFactor(" << keyFormatter(this->key1()) << "," << keyFormatter(this->key2()) << "," << keyFormatter(this->key3()) << ","; _PIM_.print(" preintegrated measurements:"); - cout << " omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]" << endl; noiseModel_->print(" noise model: "); - if (body_P_sensor_) - body_P_sensor_->print(" sensor pose in body frame: "); } //------------------------------------------------------------------------------ bool AHRSFactor::equals(const NonlinearFactor& other, double tol) const { const This *e = dynamic_cast(&other); - return e != NULL && Base::equals(*e, tol) && _PIM_.equals(e->_PIM_, tol) - && equal_with_abs_tol(omegaCoriolis_, e->omegaCoriolis_, tol) - && ((!body_P_sensor_ && !e->body_P_sensor_) - || (body_P_sensor_ && e->body_P_sensor_ - && body_P_sensor_->equals(*e->body_P_sensor_))); + return e != NULL && Base::equals(*e, tol) && _PIM_.equals(e->_PIM_, tol); } //------------------------------------------------------------------------------ @@ -172,8 +126,7 @@ Vector AHRSFactor::evaluateError(const Rot3& Ri, const Rot3& Rj, const Vector3 biascorrectedOmega = _PIM_.predict(bias, H3); // Coriolis term - const Vector3 coriolis = _PIM_.integrateCoriolis(Ri, omegaCoriolis_); - const Matrix3 coriolisHat = skewSymmetric(coriolis); + const Vector3 coriolis = _PIM_.integrateCoriolis(Ri); const Vector3 correctedOmega = biascorrectedOmega - coriolis; // Prediction @@ -191,7 +144,7 @@ Vector AHRSFactor::evaluateError(const Rot3& Ri, const Rot3& Rj, if (H1) { // dfR/dRi H1->resize(3, 3); - Matrix3 D_coriolis = -D_cDeltaRij_cOmega * coriolisHat; + Matrix3 D_coriolis = -D_cDeltaRij_cOmega * skewSymmetric(coriolis); (*H1) << D_fR_fRrot * (-actualRij.transpose() - fRrot.transpose() * D_coriolis); } @@ -199,7 +152,7 @@ Vector AHRSFactor::evaluateError(const Rot3& Ri, const Rot3& Rj, if (H2) { // dfR/dPosej H2->resize(3, 3); - (*H2) << D_fR_fRrot * Matrix3::Identity(); + (*H2) << D_fR_fRrot; } if (H3) { @@ -215,15 +168,13 @@ Vector AHRSFactor::evaluateError(const Rot3& Ri, const Rot3& Rj, } //------------------------------------------------------------------------------ -Rot3 AHRSFactor::predict(const Rot3& rot_i, const Vector3& bias, - const PreintegratedMeasurements preintegratedMeasurements, - const Vector3& omegaCoriolis, boost::optional body_P_sensor) { - +Rot3 AHRSFactor::Predict( + const Rot3& rot_i, const Vector3& bias, + const PreintegratedAhrsMeasurements preintegratedMeasurements) { const Vector3 biascorrectedOmega = preintegratedMeasurements.predict(bias); // Coriolis term - const Vector3 coriolis = // - preintegratedMeasurements.integrateCoriolis(rot_i, omegaCoriolis); + const Vector3 coriolis = preintegratedMeasurements.integrateCoriolis(rot_i); const Vector3 correctedOmega = biascorrectedOmega - coriolis; const Rot3 correctedDeltaRij = Rot3::Expmap(correctedOmega); @@ -231,4 +182,31 @@ Rot3 AHRSFactor::predict(const Rot3& rot_i, const Vector3& bias, return rot_i.compose(correctedDeltaRij); } -} //namespace gtsam +//------------------------------------------------------------------------------ +AHRSFactor::AHRSFactor(Key rot_i, Key rot_j, Key bias, + const PreintegratedMeasurements& pim, + const Vector3& omegaCoriolis, + const boost::optional& body_P_sensor) + : Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), rot_i, rot_j, bias), + _PIM_(pim) { + boost::shared_ptr p = + boost::make_shared(pim.p()); + p->body_P_sensor = body_P_sensor; + _PIM_.p_ = p; +} + +//------------------------------------------------------------------------------ +Rot3 AHRSFactor::predict(const Rot3& rot_i, const Vector3& bias, + const PreintegratedMeasurements pim, + const Vector3& omegaCoriolis, + const boost::optional& body_P_sensor) { + boost::shared_ptr p = + boost::make_shared(pim.p()); + p->omegaCoriolis = omegaCoriolis; + p->body_P_sensor = body_P_sensor; + PreintegratedMeasurements newPim = pim; + newPim.p_ = p; + return Predict(rot_i, bias, newPim); +} + +} // namespace gtsam diff --git a/gtsam/navigation/AHRSFactor.h b/gtsam/navigation/AHRSFactor.h index fbf7d51a1..c2a88bd44 100644 --- a/gtsam/navigation/AHRSFactor.h +++ b/gtsam/navigation/AHRSFactor.h @@ -26,91 +26,94 @@ namespace gtsam { -class GTSAM_EXPORT AHRSFactor: public NoiseModelFactor3 { -public: +/** + * PreintegratedAHRSMeasurements accumulates (integrates) the Gyroscope + * measurements (rotation rates) and the corresponding covariance matrix. + * Can be built incrementally so as to avoid costly integration at time of factor construction. + */ +class GTSAM_EXPORT PreintegratedAhrsMeasurements : public PreintegratedRotation { + + protected: + + Vector3 biasHat_; ///< Angular rate bias values used during preintegration. + Matrix3 preintMeasCov_; ///< Covariance matrix of the preintegrated measurements (first-order propagation from *measurementCovariance*) + + /// Default constructor, only for serialization + PreintegratedAhrsMeasurements() {} + + friend class AHRSFactor; + + public: /** - * CombinedPreintegratedMeasurements accumulates (integrates) the Gyroscope - * measurements (rotation rates) and the corresponding covariance matrix. - * The measurements are then used to build the Preintegrated AHRS factor. - * Can be built incrementally so as to avoid costly integration at time of - * factor construction. + * Default constructor, initialize with no measurements + * @param bias Current estimate of acceleration and rotation rate biases */ - class GTSAM_EXPORT PreintegratedMeasurements : public PreintegratedRotation { + PreintegratedAhrsMeasurements(const boost::shared_ptr& p, + const Vector3& biasHat) : + PreintegratedRotation(p), biasHat_(biasHat) { + resetIntegration(); + } - friend class AHRSFactor; + const Params& p() const { return *boost::static_pointer_cast(p_);} + const Vector3& biasHat() const { return biasHat_; } + const Matrix3& preintMeasCov() const { return preintMeasCov_; } - protected: - Vector3 biasHat_; ///< Acceleration and angular rate bias values used during preintegration. Note that we won't be using the accelerometer - Matrix3 preintMeasCov_; ///< Covariance matrix of the preintegrated measurements (first-order propagation from *measurementCovariance*) + /// print + void print(const std::string& s = "Preintegrated Measurements: ") const; - public: + /// equals + bool equals(const PreintegratedAhrsMeasurements&, double tol = 1e-9) const; - /// Default constructor - PreintegratedMeasurements(); + /// Reset inetgrated quantities to zero + void resetIntegration(); - /** - * Default constructor, initialize with no measurements - * @param bias Current estimate of acceleration and rotation rate biases - * @param measuredOmegaCovariance Covariance matrix of measured angular rate - */ - PreintegratedMeasurements(const Vector3& bias, - const Matrix3& measuredOmegaCovariance); + /** + * Add a single Gyroscope measurement to the preintegration. + * @param measureOmedga Measured angular velocity (in body frame) + * @param deltaT Time step + */ + void integrateMeasurement(const Vector3& measuredOmega, double deltaT); - Vector3 biasHat() const { - return biasHat_; - } - const Matrix3& preintMeasCov() const { - return preintMeasCov_; - } + /// Predict bias-corrected incremental rotation + /// TODO: The matrix Hbias is the derivative of predict? Unit-test? + Vector3 predict(const Vector3& bias, OptionalJacobian<3,3> H = boost::none) const; - /// print - void print(const std::string& s = "Preintegrated Measurements: ") const; + // This function is only used for test purposes + // (compare numerical derivatives wrt analytic ones) + static Vector DeltaAngles(const Vector& msr_gyro_t, const double msr_dt, + const Vector3& delta_angles); - /// equals - bool equals(const PreintegratedMeasurements&, double tol = 1e-9) const; - - /// TODO: Document - void resetIntegration(); - - /** - * Add a single Gyroscope measurement to the preintegration. - * @param measureOmedga Measured angular velocity (in body frame) - * @param deltaT Time step - * @param body_P_sensor Optional sensor frame - */ - void integrateMeasurement(const Vector3& measuredOmega, double deltaT, - boost::optional body_P_sensor = boost::none); - - /// Predict bias-corrected incremental rotation - /// TODO: The matrix Hbias is the derivative of predict? Unit-test? - Vector3 predict(const Vector3& bias, boost::optional H = - boost::none) const; - - // This function is only used for test purposes - // (compare numerical derivatives wrt analytic ones) - static Vector DeltaAngles(const Vector& msr_gyro_t, const double msr_dt, - const Vector3& delta_angles); - - private: - - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegratedRotation); - ar & BOOST_SERIALIZATION_NVP(biasHat_); - } - }; + /// @deprecated constructor + PreintegratedAhrsMeasurements(const Vector3& biasHat, + const Matrix3& measuredOmegaCovariance) + : PreintegratedRotation(boost::make_shared()), + biasHat_(biasHat) { + p_->gyroscopeCovariance = measuredOmegaCovariance; + resetIntegration(); + } private: + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegratedRotation); + ar & BOOST_SERIALIZATION_NVP(p_); + ar & BOOST_SERIALIZATION_NVP(biasHat_); + } +}; + +class GTSAM_EXPORT AHRSFactor: public NoiseModelFactor3 { + typedef AHRSFactor This; typedef NoiseModelFactor3 Base; - PreintegratedMeasurements _PIM_; - Vector3 gravity_; - Vector3 omegaCoriolis_; ///< Controls whether higher order terms are included when calculating the Coriolis Effect - boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame + PreintegratedAhrsMeasurements _PIM_; + + /** Default constructor - only use for serialization */ + AHRSFactor() {} public: @@ -121,22 +124,15 @@ public: typedef boost::shared_ptr shared_ptr; #endif - /** Default constructor - only use for serialization */ - AHRSFactor(); - /** * Constructor * @param rot_i previous rot key * @param rot_j current rot key * @param bias previous bias key * @param preintegratedMeasurements preintegrated measurements - * @param omegaCoriolis rotation rate of the inertial frame - * @param body_P_sensor Optional pose of the sensor frame in the body frame */ AHRSFactor(Key rot_i, Key rot_j, Key bias, - const PreintegratedMeasurements& preintegratedMeasurements, - const Vector3& omegaCoriolis, - boost::optional body_P_sensor = boost::none); + const PreintegratedAhrsMeasurements& preintegratedMeasurements); virtual ~AHRSFactor() { } @@ -152,14 +148,10 @@ public: virtual bool equals(const NonlinearFactor&, double tol = 1e-9) const; /// Access the preintegrated measurements. - const PreintegratedMeasurements& preintegratedMeasurements() const { + const PreintegratedAhrsMeasurements& preintegratedMeasurements() const { return _PIM_; } - const Vector3& omegaCoriolis() const { - return omegaCoriolis_; - } - /** implement functions needed to derive from Factor */ /// vector of errors @@ -169,10 +161,25 @@ public: boost::none) const; /// predicted states from IMU + /// TODO(frank): relationship with PIM predict ?? + static Rot3 Predict( + const Rot3& rot_i, const Vector3& bias, + const PreintegratedAhrsMeasurements preintegratedMeasurements); + + /// @deprecated name + typedef PreintegratedAhrsMeasurements PreintegratedMeasurements; + + /// @deprecated constructor + AHRSFactor(Key rot_i, Key rot_j, Key bias, + const PreintegratedMeasurements& preintegratedMeasurements, + const Vector3& omegaCoriolis, + const boost::optional& body_P_sensor = boost::none); + + /// @deprecated static function static Rot3 predict(const Rot3& rot_i, const Vector3& bias, const PreintegratedMeasurements preintegratedMeasurements, const Vector3& omegaCoriolis, - boost::optional body_P_sensor = boost::none); + const boost::optional& body_P_sensor = boost::none); private: @@ -184,13 +191,9 @@ private: & boost::serialization::make_nvp("NoiseModelFactor3", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(_PIM_); - ar & BOOST_SERIALIZATION_NVP(omegaCoriolis_); - ar & BOOST_SERIALIZATION_NVP(body_P_sensor_); } }; // AHRSFactor -typedef AHRSFactor::PreintegratedMeasurements AHRSFactorPreintegratedMeasurements; - } //namespace gtsam diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index 3547719ac..a697a5e4c 100644 --- a/gtsam/navigation/CombinedImuFactor.cpp +++ b/gtsam/navigation/CombinedImuFactor.cpp @@ -29,157 +29,125 @@ namespace gtsam { using namespace std; //------------------------------------------------------------------------------ -// Inner class CombinedPreintegratedMeasurements +// Inner class PreintegratedCombinedMeasurements //------------------------------------------------------------------------------ -CombinedImuFactor::CombinedPreintegratedMeasurements::CombinedPreintegratedMeasurements( - const imuBias::ConstantBias& bias, const Matrix3& measuredAccCovariance, - const Matrix3& measuredOmegaCovariance, - const Matrix3& integrationErrorCovariance, const Matrix3& biasAccCovariance, - const Matrix3& biasOmegaCovariance, const Matrix& biasAccOmegaInit, - const bool use2ndOrderIntegration) : - PreintegrationBase(bias, measuredAccCovariance, measuredOmegaCovariance, - integrationErrorCovariance, use2ndOrderIntegration), biasAccCovariance_( - biasAccCovariance), biasOmegaCovariance_(biasOmegaCovariance), biasAccOmegaInit_( - biasAccOmegaInit) { - preintMeasCov_.setZero(); -} - -//------------------------------------------------------------------------------ -void CombinedImuFactor::CombinedPreintegratedMeasurements::print( +void PreintegratedCombinedMeasurements::print( const string& s) const { PreintegrationBase::print(s); - cout << " biasAccCovariance [ " << biasAccCovariance_ << " ]" << endl; - cout << " biasOmegaCovariance [ " << biasOmegaCovariance_ << " ]" << endl; - cout << " biasAccOmegaInit [ " << biasAccOmegaInit_ << " ]" << endl; cout << " preintMeasCov [ " << preintMeasCov_ << " ]" << endl; } //------------------------------------------------------------------------------ -bool CombinedImuFactor::CombinedPreintegratedMeasurements::equals( - const CombinedPreintegratedMeasurements& expected, double tol) const { - return equal_with_abs_tol(biasAccCovariance_, expected.biasAccCovariance_, - tol) - && equal_with_abs_tol(biasOmegaCovariance_, expected.biasOmegaCovariance_, - tol) - && equal_with_abs_tol(biasAccOmegaInit_, expected.biasAccOmegaInit_, tol) - && equal_with_abs_tol(preintMeasCov_, expected.preintMeasCov_, tol) - && PreintegrationBase::equals(expected, tol); +bool PreintegratedCombinedMeasurements::equals( + const PreintegratedCombinedMeasurements& other, double tol) const { + return PreintegrationBase::equals(other, tol) && + equal_with_abs_tol(preintMeasCov_, other.preintMeasCov_, tol); } //------------------------------------------------------------------------------ -void CombinedImuFactor::CombinedPreintegratedMeasurements::resetIntegration() { +void PreintegratedCombinedMeasurements::resetIntegration() { PreintegrationBase::resetIntegration(); preintMeasCov_.setZero(); } //------------------------------------------------------------------------------ -void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement( - const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT, - boost::optional body_P_sensor, - boost::optional F_test, boost::optional G_test) { +// sugar for derivative blocks +#define D_R_R(H) (H)->block<3,3>(0,0) +#define D_R_t(H) (H)->block<3,3>(0,3) +#define D_R_v(H) (H)->block<3,3>(0,6) +#define D_t_R(H) (H)->block<3,3>(3,0) +#define D_t_t(H) (H)->block<3,3>(3,3) +#define D_t_v(H) (H)->block<3,3>(3,6) +#define D_v_R(H) (H)->block<3,3>(6,0) +#define D_v_t(H) (H)->block<3,3>(6,3) +#define D_v_v(H) (H)->block<3,3>(6,6) +#define D_a_a(H) (H)->block<3,3>(9,9) +#define D_g_g(H) (H)->block<3,3>(12,12) - // NOTE: order is important here because each update uses old values, e.g., velocity and position updates are based on previous rotation estimate. - // (i.e., we have to update jacobians and covariances before updating preintegrated measurements). +//------------------------------------------------------------------------------ +void PreintegratedCombinedMeasurements::integrateMeasurement( + const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT) { - Vector3 correctedAcc, correctedOmega; - correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega, - correctedAcc, correctedOmega, body_P_sensor); + const Matrix3 dRij = deltaXij_.R(); // expensive when quaternion - const Vector3 integratedOmega = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement - Matrix3 D_Rincr_integratedOmega; // Right jacobian computed at theta_incr - const Rot3 Rincr = Rot3::Expmap(integratedOmega, D_Rincr_integratedOmega); // rotation increment computed from the current rotation rate measurement - - // Update Jacobians - /* ----------------------------------------------------------------------------------------------------------------------- */ - updatePreintegratedJacobians(correctedAcc, D_Rincr_integratedOmega, Rincr, - deltaT); + // Update preintegrated measurements. + Matrix3 D_incrR_integratedOmega; // Right jacobian computed at theta_incr + Matrix9 F_9x9; // overall Jacobian wrt preintegrated measurements (df/dx) + Matrix93 G1,G2; + PreintegrationBase::update(measuredAcc, measuredOmega, deltaT, + &D_incrR_integratedOmega, &F_9x9, &G1, &G2); // Update preintegrated measurements covariance: as in [2] we consider a first order propagation that // can be seen as a prediction phase in an EKF framework. In this implementation, contrarily to [2] we // consider the uncertainty of the bias selection and we keep correlation between biases and preintegrated measurements /* ----------------------------------------------------------------------------------------------------------------------- */ - const Matrix3 R_i = deltaRij(); // store this - // Update preintegrated measurements. TODO Frank moved from end of this function !!! - Matrix9 F_9x9; - updatePreintegratedMeasurements(correctedAcc, Rincr, deltaT, F_9x9); // Single Jacobians to propagate covariance - Matrix3 H_vel_biasacc = -R_i * deltaT; - Matrix3 H_angles_biasomega = -D_Rincr_integratedOmega * deltaT; + Matrix3 H_vel_biasacc = -dRij * deltaT; + Matrix3 H_angles_biasomega = -D_incrR_integratedOmega * deltaT; // overall Jacobian wrt preintegrated measurements (df/dx) - Matrix F(15, 15); - // for documentation: - // F << I_3x3, I_3x3 * deltaT, Z_3x3, Z_3x3, Z_3x3, - // Z_3x3, I_3x3, H_vel_angles, H_vel_biasacc, Z_3x3, - // Z_3x3, Z_3x3, H_angles_angles, Z_3x3, H_angles_biasomega, - // Z_3x3, Z_3x3, Z_3x3, I_3x3, Z_3x3, - // Z_3x3, Z_3x3, Z_3x3, Z_3x3, I_3x3; + Eigen::Matrix F; F.setZero(); F.block<9, 9>(0, 0) = F_9x9; + F.block<3, 3>(0, 12) = H_angles_biasomega; + F.block<3, 3>(6, 9) = H_vel_biasacc; F.block<6, 6>(9, 9) = I_6x6; - F.block<3, 3>(3, 9) = H_vel_biasacc; - F.block<3, 3>(6, 12) = H_angles_biasomega; // first order uncertainty propagation // Optimized matrix multiplication (1/deltaT) * G * measurementCovariance * G.transpose() - Matrix G_measCov_Gt = Matrix::Zero(15, 15); + Eigen::Matrix G_measCov_Gt; + G_measCov_Gt.setZero(15, 15); -// BLOCK DIAGONAL TERMS - G_measCov_Gt.block<3, 3>(0, 0) = deltaT * integrationCovariance(); - G_measCov_Gt.block<3, 3>(3, 3) = (1 / deltaT) * (H_vel_biasacc) - * (accelerometerCovariance() + biasAccOmegaInit_.block<3, 3>(0, 0)) + // BLOCK DIAGONAL TERMS + D_t_t(&G_measCov_Gt) = deltaT * p().integrationCovariance; + D_v_v(&G_measCov_Gt) = (1 / deltaT) * (H_vel_biasacc) + * (p().accelerometerCovariance + p().biasAccOmegaInit.block<3, 3>(0, 0)) * (H_vel_biasacc.transpose()); - G_measCov_Gt.block<3, 3>(6, 6) = (1 / deltaT) * (H_angles_biasomega) - * (gyroscopeCovariance() + biasAccOmegaInit_.block<3, 3>(3, 3)) + D_R_R(&G_measCov_Gt) = (1 / deltaT) * (H_angles_biasomega) + * (p().gyroscopeCovariance + p().biasAccOmegaInit.block<3, 3>(3, 3)) * (H_angles_biasomega.transpose()); - G_measCov_Gt.block<3, 3>(9, 9) = (1 / deltaT) * biasAccCovariance_; - G_measCov_Gt.block<3, 3>(12, 12) = (1 / deltaT) * biasOmegaCovariance_; - // OFF BLOCK DIAGONAL TERMS - Matrix3 block23 = H_vel_biasacc * biasAccOmegaInit_.block<3, 3>(3, 0) - * H_angles_biasomega.transpose(); - G_measCov_Gt.block<3, 3>(3, 6) = block23; - G_measCov_Gt.block<3, 3>(6, 3) = block23.transpose(); - preintMeasCov_ = F * preintMeasCov_ * F.transpose() + G_measCov_Gt; + D_a_a(&G_measCov_Gt) = (1 / deltaT) * p().biasAccCovariance; + D_g_g(&G_measCov_Gt) = (1 / deltaT) * p().biasOmegaCovariance; - // F_test and G_test are used for testing purposes and are not needed by the factor - if (F_test) { - F_test->resize(15, 15); - (*F_test) << F; - } - if (G_test) { - G_test->resize(15, 21); - // This is for testing & documentation - ///< measurementCovariance_ : cov[integrationError measuredAcc measuredOmega biasAccRandomWalk biasOmegaRandomWalk biasAccInit biasOmegaInit] in R^(21 x 21) - (*G_test) << // - I_3x3 * deltaT, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, // - Z_3x3, -H_vel_biasacc, Z_3x3, Z_3x3, Z_3x3, H_vel_biasacc, Z_3x3, // - Z_3x3, Z_3x3, -H_angles_biasomega, Z_3x3, Z_3x3, Z_3x3, H_angles_biasomega, // - Z_3x3, Z_3x3, Z_3x3, I_3x3, Z_3x3, Z_3x3, Z_3x3, // - Z_3x3, Z_3x3, Z_3x3, Z_3x3, I_3x3, Z_3x3, Z_3x3; - } + // OFF BLOCK DIAGONAL TERMS + Matrix3 temp = H_vel_biasacc * p().biasAccOmegaInit.block<3, 3>(3, 0) + * H_angles_biasomega.transpose(); + D_v_R(&G_measCov_Gt) = temp; + D_R_v(&G_measCov_Gt) = temp.transpose(); + preintMeasCov_ = F * preintMeasCov_ * F.transpose() + G_measCov_Gt; } +//------------------------------------------------------------------------------ +PreintegratedCombinedMeasurements::PreintegratedCombinedMeasurements( + const imuBias::ConstantBias& biasHat, const Matrix3& measuredAccCovariance, + const Matrix3& measuredOmegaCovariance, + const Matrix3& integrationErrorCovariance, const Matrix3& biasAccCovariance, + const Matrix3& biasOmegaCovariance, const Matrix6& biasAccOmegaInit, + const bool use2ndOrderIntegration) { + if (!use2ndOrderIntegration) + throw("PreintegratedImuMeasurements no longer supports first-order integration: it incorrectly compensated for gravity"); + biasHat_ = biasHat; + boost::shared_ptr p = Params::MakeSharedD(); + p->gyroscopeCovariance = measuredOmegaCovariance; + p->accelerometerCovariance = measuredAccCovariance; + p->integrationCovariance = integrationErrorCovariance; + p->biasAccCovariance = biasAccCovariance; + p->biasOmegaCovariance = biasOmegaCovariance; + p->biasAccOmegaInit = biasAccOmegaInit; + p_ = p; + resetIntegration(); + preintMeasCov_.setZero(); +} //------------------------------------------------------------------------------ // CombinedImuFactor methods //------------------------------------------------------------------------------ -CombinedImuFactor::CombinedImuFactor() : - ImuFactorBase(), _PIM_(imuBias::ConstantBias(), Z_3x3, Z_3x3, Z_3x3, Z_3x3, - Z_3x3, Z_6x6) { -} - -//------------------------------------------------------------------------------ -CombinedImuFactor::CombinedImuFactor(Key pose_i, Key vel_i, Key pose_j, - Key vel_j, Key bias_i, Key bias_j, - const CombinedPreintegratedMeasurements& preintegratedMeasurements, - const Vector3& gravity, const Vector3& omegaCoriolis, - boost::optional body_P_sensor, const bool use2ndOrderCoriolis) : - Base( - noiseModel::Gaussian::Covariance( - preintegratedMeasurements.preintMeasCov_), pose_i, vel_i, pose_j, - vel_j, bias_i, bias_j), ImuFactorBase(gravity, omegaCoriolis, - body_P_sensor, use2ndOrderCoriolis), _PIM_(preintegratedMeasurements) { -} +CombinedImuFactor::CombinedImuFactor( + Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i, Key bias_j, + const PreintegratedCombinedMeasurements& pim) + : Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i, + pose_j, vel_j, bias_i, bias_j), + _PIM_(pim) {} //------------------------------------------------------------------------------ gtsam::NonlinearFactor::shared_ptr CombinedImuFactor::clone() const { @@ -194,17 +162,14 @@ void CombinedImuFactor::print(const string& s, << keyFormatter(this->key2()) << "," << keyFormatter(this->key3()) << "," << keyFormatter(this->key4()) << "," << keyFormatter(this->key5()) << "," << keyFormatter(this->key6()) << ")\n"; - ImuFactorBase::print(""); _PIM_.print(" preintegrated measurements:"); this->noiseModel_->print(" noise model: "); } //------------------------------------------------------------------------------ -bool CombinedImuFactor::equals(const NonlinearFactor& expected, - double tol) const { - const This *e = dynamic_cast(&expected); - return e != NULL && Base::equals(*e, tol) && _PIM_.equals(e->_PIM_, tol) - && ImuFactorBase::equals(*e, tol); +bool CombinedImuFactor::equals(const NonlinearFactor& other, double tol) const { + const This* e = dynamic_cast(&other); + return e != NULL && Base::equals(*e, tol) && _PIM_.equals(e->_PIM_, tol); } //------------------------------------------------------------------------------ @@ -224,8 +189,7 @@ Vector CombinedImuFactor::evaluateError(const Pose3& pose_i, Matrix93 D_r_vel_i, D_r_vel_j; // error wrt preintegrated measurements - Vector9 r_pvR = _PIM_.computeErrorAndJacobians(pose_i, vel_i, pose_j, vel_j, - bias_i, gravity_, omegaCoriolis_, use2ndOrderCoriolis_, + Vector9 r_Rpv = _PIM_.computeErrorAndJacobians(pose_i, vel_i, pose_j, vel_j, bias_i, H1 ? &D_r_pose_i : 0, H2 ? &D_r_vel_i : 0, H3 ? &D_r_pose_j : 0, H4 ? &D_r_vel_j : 0, H5 ? &D_r_bias_i : 0); @@ -234,25 +198,25 @@ Vector CombinedImuFactor::evaluateError(const Pose3& pose_i, H1->resize(15, 6); H1->block<9, 6>(0, 0) = D_r_pose_i; // adding: [dBiasAcc/dPi ; dBiasOmega/dPi] - H1->block<6, 6>(9, 0) = Z_6x6; + H1->block<6, 6>(9, 0).setZero(); } if (H2) { H2->resize(15, 3); H2->block<9, 3>(0, 0) = D_r_vel_i; // adding: [dBiasAcc/dVi ; dBiasOmega/dVi] - H2->block<6, 3>(9, 0) = Matrix::Zero(6, 3); + H2->block<6, 3>(9, 0).setZero(); } if (H3) { H3->resize(15, 6); H3->block<9, 6>(0, 0) = D_r_pose_j; // adding: [dBiasAcc/dPj ; dBiasOmega/dPj] - H3->block<6, 6>(9, 0) = Z_6x6; + H3->block<6, 6>(9, 0).setZero(); } if (H4) { H4->resize(15, 3); H4->block<9, 3>(0, 0) = D_r_vel_j; // adding: [dBiasAcc/dVi ; dBiasOmega/dVi] - H4->block<6, 3>(9, 0) = Matrix::Zero(6, 3); + H4->block<6, 3>(9, 0).setZero(); } if (H5) { H5->resize(15, 6); @@ -262,15 +226,47 @@ Vector CombinedImuFactor::evaluateError(const Pose3& pose_i, } if (H6) { H6->resize(15, 6); - H6->block<9, 6>(0, 0) = Matrix::Zero(9, 6); + H6->block<9, 6>(0, 0).setZero(); // adding: [dBiasAcc/dBias_j ; dBiasOmega/dBias_j] H6->block<6, 6>(9, 0) = Hbias_j; } // overall error Vector r(15); - r << r_pvR, fbias; // vector of size 15 + r << r_Rpv, fbias; // vector of size 15 return r; } +//------------------------------------------------------------------------------ +CombinedImuFactor::CombinedImuFactor( + Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i, Key bias_j, + const CombinedPreintegratedMeasurements& pim, const Vector3& n_gravity, + const Vector3& omegaCoriolis, const boost::optional& body_P_sensor, + const bool use2ndOrderCoriolis) + : Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i, + pose_j, vel_j, bias_i, bias_j), + _PIM_(pim) { + boost::shared_ptr p = + boost::make_shared(pim.p()); + p->n_gravity = n_gravity; + p->omegaCoriolis = omegaCoriolis; + p->body_P_sensor = body_P_sensor; + p->use2ndOrderCoriolis = use2ndOrderCoriolis; + _PIM_.p_ = p; +} +//------------------------------------------------------------------------------ +void CombinedImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i, + Pose3& pose_j, Vector3& vel_j, + const imuBias::ConstantBias& bias_i, + CombinedPreintegratedMeasurements& pim, + const Vector3& n_gravity, + const Vector3& omegaCoriolis, + const bool use2ndOrderCoriolis) { + // use deprecated predict + PoseVelocityBias pvb = pim.predict(pose_i, vel_i, bias_i, n_gravity, + omegaCoriolis, use2ndOrderCoriolis); + pose_j = pvb.pose; + vel_j = pvb.velocity; +} + } /// namespace gtsam diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index a65a4d3f7..6ed382966 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -24,21 +24,17 @@ /* GTSAM includes */ #include #include -#include -#include +#include namespace gtsam { -/** - * - * @addtogroup SLAM - * +/* * If you are using the factor, please cite: * L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating * conditionally independent sets in factor graphs: a unifying perspective based * on smart factors, Int. Conf. on Robotics and Automation (ICRA), 2014. * - ** REFERENCES: + * REFERENCES: * [1] G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", * Volume 2, 2008. * [2] T. Lupton and S.Sukkarieh, "Visual-Inertial-Aided Navigation for @@ -46,14 +42,137 @@ namespace gtsam { * TRO, 28(1):61-76, 2012. * [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor: * Computation of the Jacobian Matrices", Tech. Report, 2013. + * [4] C. Forster, L. Carlone, F. Dellaert, D. Scaramuzza, IMU Preintegration on + * Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation, + * Robotics: Science and Systems (RSS), 2015. */ +/** + * PreintegratedCombinedMeasurements integrates the IMU measurements + * (rotation rates and accelerations) and the corresponding covariance matrix. + * The measurements are then used to build the CombinedImuFactor. Integration + * is done incrementally (ideally, one integrates the measurement as soon as + * it is received from the IMU) so as to avoid costly integration at time of + * factor construction. + * + * @addtogroup SLAM + */ +class PreintegratedCombinedMeasurements : public PreintegrationBase { + + /// Parameters for pre-integration: + /// Usage: Create just a single Params and pass a shared pointer to the constructor + struct Params : PreintegrationBase::Params { + Matrix3 biasAccCovariance; ///< continuous-time "Covariance" describing accelerometer bias random walk + Matrix3 biasOmegaCovariance; ///< continuous-time "Covariance" describing gyroscope bias random walk + Matrix6 biasAccOmegaInit; ///< covariance of bias used for pre-integration + + /// See two named constructors below for good values of n_gravity in body frame + Params(const Vector3& n_gravity) : + PreintegrationBase::Params(n_gravity), biasAccCovariance(I_3x3), biasOmegaCovariance( + I_3x3), biasAccOmegaInit(I_6x6) { + } + + // Default Params for a Z-down navigation frame, such as NED: gravity points along positive Z-axis + static boost::shared_ptr MakeSharedD(double g = 9.81) { + return boost::make_shared(Vector3(0, 0, g)); + } + + // Default Params for a Z-up navigation frame, such as ENU: gravity points along negative Z-axis + static boost::shared_ptr MakeSharedU(double g = 9.81) { + return boost::make_shared(Vector3(0, 0, -g)); + } + + private: + /// Default constructor makes unitialized params struct + Params() {} + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegratedRotation::Params); + ar& BOOST_SERIALIZATION_NVP(biasAccCovariance); + ar& BOOST_SERIALIZATION_NVP(biasOmegaCovariance); + ar& BOOST_SERIALIZATION_NVP(biasAccOmegaInit); + } + }; + + protected: + /* Covariance matrix of the preintegrated measurements + * COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION BiasAcc BiasOmega] + * (first-order propagation from *measurementCovariance*). + * PreintegratedCombinedMeasurements also include the biases and keep the correlation + * between the preintegrated measurements and the biases + */ + Eigen::Matrix preintMeasCov_; + + PreintegratedCombinedMeasurements() {} + + friend class CombinedImuFactor; + + public: + /** + * Default constructor, initializes the class with no measurements + * @param bias Current estimate of acceleration and rotation rate biases + */ + PreintegratedCombinedMeasurements(const boost::shared_ptr& p, + const imuBias::ConstantBias& biasHat) + : PreintegrationBase(p, biasHat) { + preintMeasCov_.setZero(); + } + + Params& p() const { return *boost::static_pointer_cast(p_);} + + /// print + void print(const std::string& s = "Preintegrated Measurements:") const; + + /// equals + bool equals(const PreintegratedCombinedMeasurements& expected, + double tol = 1e-9) const; + + /// Re-initialize PreintegratedCombinedMeasurements + void resetIntegration(); + + /** + * Add a single IMU measurement to the preintegration. + * @param measuredAcc Measured acceleration (in body frame, as given by the + * sensor) + * @param measuredOmega Measured angular velocity (as given by the sensor) + * @param deltaT Time interval between two consecutive IMU measurements + * @param body_P_sensor Optional sensor frame (pose of the IMU in the body + * frame) + */ + void integrateMeasurement(const Vector3& measuredAcc, + const Vector3& measuredOmega, double deltaT); + + /// methods to access class variables + Matrix preintMeasCov() const { return preintMeasCov_; } + + /// deprecated constructor + /// NOTE(frank): assumes Z-Down convention, only second order integration supported + PreintegratedCombinedMeasurements(const imuBias::ConstantBias& biasHat, + const Matrix3& measuredAccCovariance, + const Matrix3& measuredOmegaCovariance, + const Matrix3& integrationErrorCovariance, + const Matrix3& biasAccCovariance, const Matrix3& biasOmegaCovariance, + const Matrix6& biasAccOmegaInit, const bool use2ndOrderIntegration = true); + + private: + /// Serialization function + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationBase); + ar& BOOST_SERIALIZATION_NVP(preintMeasCov_); + } +}; + /** * CombinedImuFactor is a 6-ways factor involving previous state (pose and * velocity of the vehicle, as well as bias at previous time step), and current * state (pose, velocity, bias at current time step). Following the pre- * integration scheme proposed in [2], the CombinedImuFactor includes many IMU - * measurements, which are "summarized" using the CombinedPreintegratedMeasurements + * measurements, which are "summarized" using the PreintegratedCombinedMeasurements * class. There are 3 main differences wrpt the ImuFactor class: * 1) The factor is 6-ways, meaning that it also involves both biases (previous * and current time step).Therefore, the factor internally imposes the biases @@ -61,105 +180,26 @@ namespace gtsam { * "biasOmegaCovariance" described the random walk that models bias evolution. * 2) The preintegration covariance takes into account the noise in the bias * estimate used for integration. - * 3) The covariance matrix of the CombinedPreintegratedMeasurements preserves + * 3) The covariance matrix of the PreintegratedCombinedMeasurements preserves * the correlation between the bias uncertainty and the preintegrated * measurements uncertainty. + * + * @addtogroup SLAM */ class CombinedImuFactor: public NoiseModelFactor6, public ImuFactorBase { + Vector3, imuBias::ConstantBias, imuBias::ConstantBias> { public: - /** - * CombinedPreintegratedMeasurements integrates the IMU measurements - * (rotation rates and accelerations) and the corresponding covariance matrix. - * The measurements are then used to build the CombinedImuFactor. Integration - * is done incrementally (ideally, one integrates the measurement as soon as - * it is received from the IMU) so as to avoid costly integration at time of - * factor construction. - */ - class CombinedPreintegratedMeasurements: public PreintegrationBase { - - friend class CombinedImuFactor; - - protected: - - Matrix3 biasAccCovariance_; ///< continuous-time "Covariance" describing accelerometer bias random walk - Matrix3 biasOmegaCovariance_; ///< continuous-time "Covariance" describing gyroscope bias random walk - Matrix6 biasAccOmegaInit_; ///< covariance of bias used for pre-integration - - Eigen::Matrix preintMeasCov_; ///< Covariance matrix of the preintegrated measurements - ///< COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION BiasAcc BiasOmega] - ///< (first-order propagation from *measurementCovariance*). CombinedPreintegratedMeasurements also include the biases and keep the correlation - ///< between the preintegrated measurements and the biases - - public: - - /** - * Default constructor, initializes the class with no measurements - * @param bias Current estimate of acceleration and rotation rate biases - * @param measuredAccCovariance Covariance matrix of measuredAcc - * @param measuredOmegaCovariance Covariance matrix of measured Angular Rate - * @param integrationErrorCovariance Covariance matrix of integration errors (velocity -> position) - * @param biasAccCovariance Covariance matrix of biasAcc (random walk describing BIAS evolution) - * @param biasOmegaCovariance Covariance matrix of biasOmega (random walk describing BIAS evolution) - * @param biasAccOmegaInit Covariance of biasAcc & biasOmega when preintegrating measurements - * @param use2ndOrderIntegration Controls the order of integration - * (if false: p(t+1) = p(t) + v(t) deltaT ; if true: p(t+1) = p(t) + v(t) deltaT + 0.5 * acc(t) deltaT^2) - */ - CombinedPreintegratedMeasurements(const imuBias::ConstantBias& bias, - const Matrix3& measuredAccCovariance, - const Matrix3& measuredOmegaCovariance, - const Matrix3& integrationErrorCovariance, - const Matrix3& biasAccCovariance, const Matrix3& biasOmegaCovariance, - const Matrix& biasAccOmegaInit, const bool use2ndOrderIntegration = - false); - - /// print - void print(const std::string& s = "Preintegrated Measurements:") const; - - /// equals - bool equals(const CombinedPreintegratedMeasurements& expected, double tol = - 1e-9) const; - - /// Re-initialize CombinedPreintegratedMeasurements - void resetIntegration(); - - /** - * Add a single IMU measurement to the preintegration. - * @param measuredAcc Measured acceleration (in body frame, as given by the sensor) - * @param measuredOmega Measured angular velocity (as given by the sensor) - * @param deltaT Time interval between two consecutive IMU measurements - * @param body_P_sensor Optional sensor frame (pose of the IMU in the body frame) - */ - void integrateMeasurement(const Vector3& measuredAcc, - const Vector3& measuredOmega, double deltaT, - boost::optional body_P_sensor = boost::none, - boost::optional F_test = boost::none, - boost::optional G_test = boost::none); - - /// methods to access class variables - Matrix preintMeasCov() const { - return preintMeasCov_; - } - - private: - - /// Serialization function - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationBase); - ar & BOOST_SERIALIZATION_NVP(preintMeasCov_); - } - }; - private: typedef CombinedImuFactor This; typedef NoiseModelFactor6 Base; - CombinedPreintegratedMeasurements _PIM_; + PreintegratedCombinedMeasurements _PIM_; + + /** Default constructor - only use for serialization */ + CombinedImuFactor() {} public: @@ -170,9 +210,6 @@ public: typedef boost::shared_ptr shared_ptr; #endif - /** Default constructor - only use for serialization */ - CombinedImuFactor(); - /** * Constructor * @param pose_i Previous pose key @@ -181,21 +218,13 @@ public: * @param vel_j Current velocity key * @param bias_i Previous bias key * @param bias_j Current bias key - * @param CombinedPreintegratedMeasurements CombinedPreintegratedMeasurements IMU measurements - * @param gravity Gravity vector expressed in the global frame - * @param omegaCoriolis Rotation rate of the global frame w.r.t. an inertial frame - * @param body_P_sensor Optional pose of the sensor frame in the body frame - * @param use2ndOrderCoriolis When true, the second-order term is used in the calculation of the Coriolis Effect + * @param PreintegratedCombinedMeasurements Combined IMU measurements */ - CombinedImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i, - Key bias_j, - const CombinedPreintegratedMeasurements& preintegratedMeasurements, - const Vector3& gravity, const Vector3& omegaCoriolis, - boost::optional body_P_sensor = boost::none, - const bool use2ndOrderCoriolis = false); + CombinedImuFactor( + Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i, Key bias_j, + const PreintegratedCombinedMeasurements& preintegratedMeasurements); - virtual ~CombinedImuFactor() { - } + virtual ~CombinedImuFactor() {} /// @return a deep copy of this factor virtual gtsam::NonlinearFactor::shared_ptr clone() const; @@ -211,7 +240,7 @@ public: /** Access the preintegrated measurements. */ - const CombinedPreintegratedMeasurements& preintegratedMeasurements() const { + const PreintegratedCombinedMeasurements& preintegratedMeasurements() const { return _PIM_; } @@ -226,20 +255,22 @@ public: boost::optional H4 = boost::none, boost::optional H5 = boost::none, boost::optional H6 = boost::none) const; - /** @deprecated The following function has been deprecated, use PreintegrationBase::predict with the same signature instead */ + /// @deprecated typename + typedef gtsam::PreintegratedCombinedMeasurements CombinedPreintegratedMeasurements; + + /// @deprecated constructor + CombinedImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i, + Key bias_j, const CombinedPreintegratedMeasurements& pim, + const Vector3& n_gravity, const Vector3& omegaCoriolis, + const boost::optional& body_P_sensor = boost::none, + const bool use2ndOrderCoriolis = false); + + // @deprecated use PreintegrationBase::predict static void Predict(const Pose3& pose_i, const Vector3& vel_i, Pose3& pose_j, - Vector3& vel_j, const imuBias::ConstantBias& bias_i, - const CombinedPreintegratedMeasurements& PIM, const Vector3& gravity, - const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false, - boost::optional deltaPij_biascorrected_out = boost::none, - boost::optional deltaVij_biascorrected_out = boost::none) { - PoseVelocityBias PVB( - PIM.predict(pose_i, vel_i, bias_i, gravity, omegaCoriolis, - use2ndOrderCoriolis, deltaPij_biascorrected_out, - deltaVij_biascorrected_out)); - pose_j = PVB.pose; - vel_j = PVB.velocity; - } + Vector3& vel_j, const imuBias::ConstantBias& bias_i, + CombinedPreintegratedMeasurements& pim, + const Vector3& n_gravity, const Vector3& omegaCoriolis, + const bool use2ndOrderCoriolis = false); private: @@ -250,13 +281,8 @@ private: ar & boost::serialization::make_nvp("NoiseModelFactor6", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(_PIM_); - ar & BOOST_SERIALIZATION_NVP(gravity_); - ar & BOOST_SERIALIZATION_NVP(omegaCoriolis_); - ar & BOOST_SERIALIZATION_NVP(body_P_sensor_); } }; // class CombinedImuFactor -typedef CombinedImuFactor::CombinedPreintegratedMeasurements CombinedImuFactorPreintegratedMeasurements; - } /// namespace gtsam diff --git a/gtsam/navigation/ImuBias.h b/gtsam/navigation/ImuBias.h index 571fb7249..047f24e8f 100644 --- a/gtsam/navigation/ImuBias.h +++ b/gtsam/navigation/ImuBias.h @@ -78,20 +78,18 @@ public: /** Correct an accelerometer measurement using this bias model, and optionally compute Jacobians */ Vector3 correctAccelerometer(const Vector3& measurement, - boost::optional H = boost::none) const { + OptionalJacobian<3, 6> H = boost::none) const { if (H) { - H->resize(3, 6); - (*H) << -Matrix3::Identity(), Matrix3::Zero(); + (*H) << -I_3x3, Z_3x3; } return measurement - biasAcc_; } /** Correct a gyroscope measurement using this bias model, and optionally compute Jacobians */ Vector3 correctGyroscope(const Vector3& measurement, - boost::optional H = boost::none) const { + OptionalJacobian<3, 6> H = boost::none) const { if (H) { - H->resize(3, 6); - (*H) << Matrix3::Zero(), -Matrix3::Identity(); + (*H) << Z_3x3, -I_3x3; } return measurement - biasGyro_; } diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp index 01de5a8f3..70527d91d 100644 --- a/gtsam/navigation/ImuFactor.cpp +++ b/gtsam/navigation/ImuFactor.cpp @@ -31,113 +31,91 @@ using namespace std; //------------------------------------------------------------------------------ // Inner class PreintegratedMeasurements //------------------------------------------------------------------------------ -ImuFactor::PreintegratedMeasurements::PreintegratedMeasurements( - const imuBias::ConstantBias& bias, const Matrix3& measuredAccCovariance, - const Matrix3& measuredOmegaCovariance, - const Matrix3& integrationErrorCovariance, - const bool use2ndOrderIntegration) : - PreintegrationBase(bias, measuredAccCovariance, measuredOmegaCovariance, - integrationErrorCovariance, use2ndOrderIntegration) { - preintMeasCov_.setZero(); -} - -//------------------------------------------------------------------------------ -void ImuFactor::PreintegratedMeasurements::print(const string& s) const { +void PreintegratedImuMeasurements::print(const string& s) const { PreintegrationBase::print(s); + cout << " preintMeasCov \n[" << preintMeasCov_ << "]" << endl; } //------------------------------------------------------------------------------ -bool ImuFactor::PreintegratedMeasurements::equals( - const PreintegratedMeasurements& expected, double tol) const { - return equal_with_abs_tol(preintMeasCov_, expected.preintMeasCov_, tol) - && PreintegrationBase::equals(expected, tol); +bool PreintegratedImuMeasurements::equals( + const PreintegratedImuMeasurements& other, double tol) const { + return PreintegrationBase::equals(other, tol) + && equal_with_abs_tol(preintMeasCov_, other.preintMeasCov_, tol); } //------------------------------------------------------------------------------ -void ImuFactor::PreintegratedMeasurements::resetIntegration() { +void PreintegratedImuMeasurements::resetIntegration() { PreintegrationBase::resetIntegration(); preintMeasCov_.setZero(); } //------------------------------------------------------------------------------ -void ImuFactor::PreintegratedMeasurements::integrateMeasurement( - const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT, - boost::optional body_P_sensor, OptionalJacobian<9, 9> F_test, - OptionalJacobian<9, 9> G_test) { +void PreintegratedImuMeasurements::integrateMeasurement( + const Vector3& measuredAcc, const Vector3& measuredOmega, double dt) { - Vector3 correctedAcc, correctedOmega; - correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega, - correctedAcc, correctedOmega, body_P_sensor); - - const Vector3 integratedOmega = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement - Matrix3 D_Rincr_integratedOmega; // Right jacobian computed at theta_incr - const Rot3 Rincr = Rot3::Expmap(integratedOmega, D_Rincr_integratedOmega); // rotation increment computed from the current rotation rate measurement - - // Update Jacobians - updatePreintegratedJacobians(correctedAcc, D_Rincr_integratedOmega, Rincr, deltaT); + static const Matrix93 Gi = (Matrix93() << Z_3x3, I_3x3, Z_3x3).finished(); // Update preintegrated measurements (also get Jacobian) - const Matrix3 R_i = deltaRij(); // store this, which is useful to compute G_test Matrix9 F; // overall Jacobian wrt preintegrated measurements (df/dx) - updatePreintegratedMeasurements(correctedAcc, Rincr, deltaT, F); + Matrix93 G1, G2; + Matrix3 D_incrR_integratedOmega; + PreintegrationBase::update(measuredAcc, measuredOmega, dt, + &D_incrR_integratedOmega, &F, &G1, &G2); // first order covariance propagation: - // as in [2] we consider a first order propagation that can be seen as a prediction phase in an EKF framework - /* ----------------------------------------------------------------------------------------------------------------------- */ - // preintMeasCov = F * preintMeasCov * F.transpose() + G * (1/deltaT) * measurementCovariance * G.transpose(); + // as in [2] we consider a first order propagation that can be seen as a prediction phase in EKF + /* --------------------------------------------------------------------------------------------*/ + // preintMeasCov = F * preintMeasCov * F.transpose() + G * (1/deltaT) * measurementCovariance * G' // NOTE 1: (1/deltaT) allows to pass from continuous time noise to discrete time noise // measurementCovariance_discrete = measurementCovariance_contTime * (1/deltaT) - // NOTE 2: the computation of G * (1/deltaT) * measurementCovariance * G.transpose() is done block-wise, - // as G and measurementCovariance are block-diagonal matrices - preintMeasCov_ = F * preintMeasCov_ * F.transpose(); - preintMeasCov_.block<3, 3>(0, 0) += integrationCovariance() * deltaT; - preintMeasCov_.block<3, 3>(3, 3) += R_i * accelerometerCovariance() - * R_i.transpose() * deltaT; - preintMeasCov_.block<3, 3>(6, 6) += D_Rincr_integratedOmega - * gyroscopeCovariance() * D_Rincr_integratedOmega.transpose() * deltaT; +#ifdef OLD_JACOBIAN_CALCULATION + Matrix9 G; + G << G1, Gi, G2; + Matrix9 Cov; + Cov << p().accelerometerCovariance / dt, Z_3x3, Z_3x3, + Z_3x3, p().integrationCovariance * dt, Z_3x3, + Z_3x3, Z_3x3, p().gyroscopeCovariance / dt; + preintMeasCov_ = F * preintMeasCov_ * F.transpose() + G * Cov * G.transpose(); +#else + preintMeasCov_ = F * preintMeasCov_ * F.transpose() + + Gi * (p().integrationCovariance * dt) * Gi.transpose() // NOTE(frank): (Gi*dt)*(C/dt)*(Gi'*dt) + + G1 * (p().accelerometerCovariance / dt) * G1.transpose() + + G2 * (p().gyroscopeCovariance / dt) * G2.transpose(); +#endif +} - Matrix3 F_pos_noiseacc; - if (use2ndOrderIntegration()) { - F_pos_noiseacc = 0.5 * R_i * deltaT * deltaT; - preintMeasCov_.block<3, 3>(0, 0) += (1 / deltaT) * F_pos_noiseacc - * accelerometerCovariance() * F_pos_noiseacc.transpose(); - Matrix3 temp = F_pos_noiseacc * accelerometerCovariance() * R_i.transpose(); // already includes 1/deltaT - preintMeasCov_.block<3, 3>(0, 3) += temp; - preintMeasCov_.block<3, 3>(3, 0) += temp.transpose(); - } +//------------------------------------------------------------------------------ +PreintegratedImuMeasurements::PreintegratedImuMeasurements( + const imuBias::ConstantBias& biasHat, const Matrix3& measuredAccCovariance, + const Matrix3& measuredOmegaCovariance, + const Matrix3& integrationErrorCovariance, bool use2ndOrderIntegration) { + if (!use2ndOrderIntegration) + throw("PreintegratedImuMeasurements no longer supports first-order integration: it incorrectly compensated for gravity"); + biasHat_ = biasHat; + boost::shared_ptr p = Params::MakeSharedD(); + p->gyroscopeCovariance = measuredOmegaCovariance; + p->accelerometerCovariance = measuredAccCovariance; + p->integrationCovariance = integrationErrorCovariance; + p_ = p; + resetIntegration(); +} - // F_test and G_test are given as output for testing purposes and are not needed by the factor - if (F_test) { // This in only for testing - (*F_test) << F; - } - if (G_test) { // This in only for testing & documentation, while the actual computation is done block-wise - if (!use2ndOrderIntegration()) - F_pos_noiseacc = Z_3x3; - - // intNoise accNoise omegaNoise - (*G_test) << I_3x3 * deltaT, F_pos_noiseacc, Z_3x3, // pos - Z_3x3, R_i * deltaT, Z_3x3, // vel - Z_3x3, Z_3x3, D_Rincr_integratedOmega * deltaT; // angle - } +//------------------------------------------------------------------------------ +void PreintegratedImuMeasurements::integrateMeasurement( + const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT, + boost::optional body_P_sensor) { + // modify parameters to accommodate deprecated method:-( + p_->body_P_sensor = body_P_sensor; + integrateMeasurement(measuredAcc, measuredOmega, deltaT); } //------------------------------------------------------------------------------ // ImuFactor methods -//------------------------------------------------------------------------------ -ImuFactor::ImuFactor() : - ImuFactorBase(), _PIM_(imuBias::ConstantBias(), Z_3x3, Z_3x3, Z_3x3) { -} - //------------------------------------------------------------------------------ ImuFactor::ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias, - const PreintegratedMeasurements& preintegratedMeasurements, - const Vector3& gravity, const Vector3& omegaCoriolis, - boost::optional body_P_sensor, const bool use2ndOrderCoriolis) : - Base( - noiseModel::Gaussian::Covariance( - preintegratedMeasurements.preintMeasCov_), pose_i, vel_i, pose_j, - vel_j, bias), ImuFactorBase(gravity, omegaCoriolis, body_P_sensor, - use2ndOrderCoriolis), _PIM_(preintegratedMeasurements) { + const PreintegratedImuMeasurements& pim) : + Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i, + pose_j, vel_j, bias), _PIM_(pim) { } //------------------------------------------------------------------------------ @@ -152,17 +130,18 @@ void ImuFactor::print(const string& s, const KeyFormatter& keyFormatter) const { << keyFormatter(this->key2()) << "," << keyFormatter(this->key3()) << "," << keyFormatter(this->key4()) << "," << keyFormatter(this->key5()) << ")\n"; - ImuFactorBase::print(""); + Base::print(""); _PIM_.print(" preintegrated measurements:"); // Print standard deviations on covariance only - cout << " noise model sigmas: " << this->noiseModel_->sigmas().transpose() << endl; + cout << " noise model sigmas: " << this->noiseModel_->sigmas().transpose() + << endl; } //------------------------------------------------------------------------------ -bool ImuFactor::equals(const NonlinearFactor& expected, double tol) const { - const This *e = dynamic_cast(&expected); +bool ImuFactor::equals(const NonlinearFactor& other, double tol) const { + const This *e = dynamic_cast(&other); return e != NULL && Base::equals(*e, tol) && _PIM_.equals(e->_PIM_, tol) - && ImuFactorBase::equals(*e, tol); + && Base::equals(*e, tol); } //------------------------------------------------------------------------------ @@ -171,9 +150,36 @@ Vector ImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_i, const imuBias::ConstantBias& bias_i, boost::optional H1, boost::optional H2, boost::optional H3, boost::optional H4, boost::optional H5) const { - return _PIM_.computeErrorAndJacobians(pose_i, vel_i, pose_j, vel_j, bias_i, - gravity_, omegaCoriolis_, use2ndOrderCoriolis_, H1, H2, H3, H4, H5); + H1, H2, H3, H4, H5); } -} /// namespace gtsam +//------------------------------------------------------------------------------ +ImuFactor::ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias, + const PreintegratedMeasurements& pim, const Vector3& n_gravity, + const Vector3& omegaCoriolis, const boost::optional& body_P_sensor, + const bool use2ndOrderCoriolis) : + Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i, + pose_j, vel_j, bias), _PIM_(pim) { + boost::shared_ptr p = boost::make_shared< + PreintegratedMeasurements::Params>(pim.p()); + p->n_gravity = n_gravity; + p->omegaCoriolis = omegaCoriolis; + p->body_P_sensor = body_P_sensor; + p->use2ndOrderCoriolis = use2ndOrderCoriolis; + _PIM_.p_ = p; +} + +//------------------------------------------------------------------------------ +void ImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i, + Pose3& pose_j, Vector3& vel_j, const imuBias::ConstantBias& bias_i, + PreintegratedMeasurements& pim, const Vector3& n_gravity, + const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis) { + // use deprecated predict + PoseVelocityBias pvb = pim.predict(pose_i, vel_i, bias_i, n_gravity, + omegaCoriolis, use2ndOrderCoriolis); + pose_j = pvb.pose; + vel_j = pvb.velocity; +} + +} // namespace gtsam diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index 50e6c835f..855c14365 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -24,21 +24,21 @@ /* GTSAM includes */ #include #include -#include #include namespace gtsam { -/** - * - * @addtogroup SLAM - * +/* * If you are using the factor, please cite: - * L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating conditionally - * independent sets in factor graphs: a unifying perspective based on smart factors, - * Int. Conf. on Robotics and Automation (ICRA), 2014. + * L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, "Eliminating + * conditionally independent sets in factor graphs: a unifying perspective based + * on smart factors", Int. Conf. on Robotics and Automation (ICRA), 2014. * - ** REFERENCES: + * C. Forster, L. Carlone, F. Dellaert, D. Scaramuzza, "IMU Preintegration on + * Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation", + * Robotics: Science and Systems (RSS), 2015. + * + * REFERENCES: * [1] G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", * Volume 2, 2008. * [2] T. Lupton and S.Sukkarieh, "Visual-Inertial-Aided Navigation for @@ -46,103 +46,116 @@ namespace gtsam { * TRO, 28(1):61-76, 2012. * [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor: * Computation of the Jacobian Matrices", Tech. Report, 2013. + * [4] C. Forster, L. Carlone, F. Dellaert, D. Scaramuzza, "IMU Preintegration on + * Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation", + * Robotics: Science and Systems (RSS), 2015. */ +/** + * PreintegratedIMUMeasurements accumulates (integrates) the IMU measurements + * (rotation rates and accelerations) and the corresponding covariance matrix. + * The measurements are then used to build the Preintegrated IMU factor. + * Integration is done incrementally (ideally, one integrates the measurement + * as soon as it is received from the IMU) so as to avoid costly integration + * at time of factor construction. + * + * @addtogroup SLAM + */ +class PreintegratedImuMeasurements: public PreintegrationBase { + + friend class ImuFactor; + +protected: + + Matrix9 preintMeasCov_; ///< COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION] + ///< (first-order propagation from *measurementCovariance*). + + /// Default constructor for serialization + PreintegratedImuMeasurements() {} + +public: + + /** + * Constructor, initializes the class with no measurements + * @param bias Current estimate of acceleration and rotation rate biases + * @param p Parameters, typically fixed in a single application + */ + PreintegratedImuMeasurements(const boost::shared_ptr& p, + const imuBias::ConstantBias& biasHat) : + PreintegrationBase(p, biasHat) { + preintMeasCov_.setZero(); + } + + /// print + void print(const std::string& s = "Preintegrated Measurements:") const; + + /// equals + bool equals(const PreintegratedImuMeasurements& expected, + double tol = 1e-9) const; + + /// Re-initialize PreintegratedIMUMeasurements + void resetIntegration(); + + /** + * Add a single IMU measurement to the preintegration. + * @param measuredAcc Measured acceleration (in body frame, as given by the sensor) + * @param measuredOmega Measured angular velocity (as given by the sensor) + * @param dt Time interval between this and the last IMU measurement + */ + void integrateMeasurement(const Vector3& measuredAcc, + const Vector3& measuredOmega, double dt); + + /// Return pre-integrated measurement covariance + Matrix preintMeasCov() const { return preintMeasCov_; } + + /// @deprecated constructor + /// NOTE(frank): assumes Z-Down convention, only second order integration supported + PreintegratedImuMeasurements(const imuBias::ConstantBias& biasHat, + const Matrix3& measuredAccCovariance, + const Matrix3& measuredOmegaCovariance, + const Matrix3& integrationErrorCovariance, + bool use2ndOrderIntegration = true); + + /// @deprecated version of integrateMeasurement with body_P_sensor + /// Use parameters instead + void integrateMeasurement(const Vector3& measuredAcc, + const Vector3& measuredOmega, double dt, + boost::optional body_P_sensor); + +private: + + /// Serialization function + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationBase); + ar & BOOST_SERIALIZATION_NVP(preintMeasCov_); + } +}; + /** * ImuFactor is a 5-ways factor involving previous state (pose and velocity of * the vehicle at previous time step), current state (pose and velocity at * current time step), and the bias estimate. Following the preintegration * scheme proposed in [2], the ImuFactor includes many IMU measurements, which - * are "summarized" using the PreintegratedMeasurements class. + * are "summarized" using the PreintegratedIMUMeasurements class. * Note that this factor does not model "temporal consistency" of the biases * (which are usually slowly varying quantities), which is up to the caller. * See also CombinedImuFactor for a class that does this for you. + * + * @addtogroup SLAM */ class ImuFactor: public NoiseModelFactor5, public ImuFactorBase { + imuBias::ConstantBias> { public: - /** - * PreintegratedMeasurements accumulates (integrates) the IMU measurements - * (rotation rates and accelerations) and the corresponding covariance matrix. - * The measurements are then used to build the Preintegrated IMU factor. - * Integration is done incrementally (ideally, one integrates the measurement - * as soon as it is received from the IMU) so as to avoid costly integration - * at time of factor construction. - */ - class PreintegratedMeasurements: public PreintegrationBase { - - friend class ImuFactor; - - protected: - - Eigen::Matrix preintMeasCov_; ///< COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION] - ///< (first-order propagation from *measurementCovariance*). - - public: - - /** - * Default constructor, initializes the class with no measurements - * @param bias Current estimate of acceleration and rotation rate biases - * @param measuredAccCovariance Covariance matrix of measuredAcc - * @param measuredOmegaCovariance Covariance matrix of measured Angular Rate - * @param integrationErrorCovariance Covariance matrix of integration errors (velocity -> position) - * @param use2ndOrderIntegration Controls the order of integration - * (if false: p(t+1) = p(t) + v(t) deltaT ; if true: p(t+1) = p(t) + v(t) deltaT + 0.5 * acc(t) deltaT^2) - */ - PreintegratedMeasurements(const imuBias::ConstantBias& bias, - const Matrix3& measuredAccCovariance, - const Matrix3& measuredOmegaCovariance, - const Matrix3& integrationErrorCovariance, - const bool use2ndOrderIntegration = false); - - /// print - void print(const std::string& s = "Preintegrated Measurements:") const; - - /// equals - bool equals(const PreintegratedMeasurements& expected, - double tol = 1e-9) const; - - /// Re-initialize PreintegratedMeasurements - void resetIntegration(); - - /** - * Add a single IMU measurement to the preintegration. - * @param measuredAcc Measured acceleration (in body frame, as given by the sensor) - * @param measuredOmega Measured angular velocity (as given by the sensor) - * @param deltaT Time interval between this and the last IMU measurement - * @param body_P_sensor Optional sensor frame (pose of the IMU in the body frame) - * @param Fout, Gout Jacobians used internally (only needed for testing) - */ - void integrateMeasurement(const Vector3& measuredAcc, - const Vector3& measuredOmega, double deltaT, - boost::optional body_P_sensor = boost::none, - OptionalJacobian<9, 9> Fout = boost::none, OptionalJacobian<9, 9> Gout = - boost::none); - - /// methods to access class variables - Matrix preintMeasCov() const { - return preintMeasCov_; - } - - private: - - /// Serialization function - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationBase); - ar & BOOST_SERIALIZATION_NVP(preintMeasCov_); - } - }; - private: typedef ImuFactor This; typedef NoiseModelFactor5 Base; - PreintegratedMeasurements _PIM_; + PreintegratedImuMeasurements _PIM_; public: @@ -163,17 +176,9 @@ public: * @param pose_j Current pose key * @param vel_j Current velocity key * @param bias Previous bias key - * @param preintegratedMeasurements Preintegrated IMU measurements - * @param gravity Gravity vector expressed in the global frame - * @param omegaCoriolis Rotation rate of the global frame w.r.t. an inertial frame - * @param body_P_sensor Optional pose of the sensor frame in the body frame - * @param use2ndOrderCoriolis When true, the second-order term is used in the calculation of the Coriolis Effect */ ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias, - const PreintegratedMeasurements& preintegratedMeasurements, - const Vector3& gravity, const Vector3& omegaCoriolis, - boost::optional body_P_sensor = boost::none, - const bool use2ndOrderCoriolis = false); + const PreintegratedImuMeasurements& preintegratedMeasurements); virtual ~ImuFactor() { } @@ -192,7 +197,7 @@ public: /** Access the preintegrated measurements. */ - const PreintegratedMeasurements& preintegratedMeasurements() const { + const PreintegratedImuMeasurements& preintegratedMeasurements() const { return _PIM_; } @@ -206,20 +211,22 @@ public: boost::optional H3 = boost::none, boost::optional H4 = boost::none, boost::optional H5 = boost::none) const; - /** @deprecated The following function has been deprecated, use PreintegrationBase::predict with the same signature instead */ + /// @deprecated typename + typedef PreintegratedImuMeasurements PreintegratedMeasurements; + + /// @deprecated constructor, in the new one gravity, coriolis settings are in Params + ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias, + const PreintegratedMeasurements& preintegratedMeasurements, + const Vector3& n_gravity, const Vector3& omegaCoriolis, + const boost::optional& body_P_sensor = boost::none, + const bool use2ndOrderCoriolis = false); + + /// @deprecated use PreintegrationBase::predict, + /// in the new one gravity, coriolis settings are in Params static void Predict(const Pose3& pose_i, const Vector3& vel_i, Pose3& pose_j, Vector3& vel_j, const imuBias::ConstantBias& bias_i, - const PreintegratedMeasurements& PIM, const Vector3& gravity, - const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false, - boost::optional deltaPij_biascorrected_out = boost::none, - boost::optional deltaVij_biascorrected_out = boost::none) { - PoseVelocityBias PVB( - PIM.predict(pose_i, vel_i, bias_i, gravity, omegaCoriolis, - use2ndOrderCoriolis, deltaPij_biascorrected_out, - deltaVij_biascorrected_out)); - pose_j = PVB.pose; - vel_j = PVB.velocity; - } + PreintegratedMeasurements& pim, const Vector3& n_gravity, + const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false); private: @@ -230,13 +237,8 @@ private: ar & boost::serialization::make_nvp("NoiseModelFactor5", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(_PIM_); - ar & BOOST_SERIALIZATION_NVP(gravity_); - ar & BOOST_SERIALIZATION_NVP(omegaCoriolis_); - ar & BOOST_SERIALIZATION_NVP(body_P_sensor_); } }; // class ImuFactor -typedef ImuFactor::PreintegratedMeasurements ImuFactorPreintegratedMeasurements; - } /// namespace gtsam diff --git a/gtsam/navigation/ImuFactorBase.h b/gtsam/navigation/ImuFactorBase.h deleted file mode 100644 index 1e4e51220..000000000 --- a/gtsam/navigation/ImuFactorBase.h +++ /dev/null @@ -1,94 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file PreintegrationBase.h - * @author Luca Carlone - * @author Stephen Williams - * @author Richard Roberts - * @author Vadim Indelman - * @author David Jensen - * @author Frank Dellaert - **/ - -#pragma once - -/* GTSAM includes */ -#include -#include - -namespace gtsam { - -class ImuFactorBase { - -protected: - - Vector3 gravity_; - Vector3 omegaCoriolis_; - boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame - bool use2ndOrderCoriolis_; ///< Controls whether higher order terms are included when calculating the Coriolis Effect - -public: - - /** Default constructor - only use for serialization */ - ImuFactorBase() : - gravity_(Vector3(0.0, 0.0, 9.81)), omegaCoriolis_(Vector3(0.0, 0.0, 0.0)), body_P_sensor_( - boost::none), use2ndOrderCoriolis_(false) { - } - - /** - * Default constructor, stores basic quantities required by the Imu factors - * @param gravity Gravity vector expressed in the global frame - * @param omegaCoriolis Rotation rate of the global frame w.r.t. an inertial frame - * @param body_P_sensor Optional pose of the sensor frame in the body frame - * @param use2ndOrderCoriolis When true, the second-order term is used in the calculation of the Coriolis Effect - */ - ImuFactorBase(const Vector3& gravity, const Vector3& omegaCoriolis, - boost::optional body_P_sensor = boost::none, - const bool use2ndOrderCoriolis = false) : - gravity_(gravity), omegaCoriolis_(omegaCoriolis), body_P_sensor_( - body_P_sensor), use2ndOrderCoriolis_(use2ndOrderCoriolis) { - } - - /// Methods to access class variables - const Vector3& gravity() const { - return gravity_; - } - const Vector3& omegaCoriolis() const { - return omegaCoriolis_; - } - - /// Needed for testable - //------------------------------------------------------------------------------ - void print(const std::string& /*s*/) const { - std::cout << " gravity: [ " << gravity_.transpose() << " ]" << std::endl; - std::cout << " omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]" - << std::endl; - std::cout << " use2ndOrderCoriolis: [ " << use2ndOrderCoriolis_ << " ]" - << std::endl; - if (this->body_P_sensor_) - this->body_P_sensor_->print(" sensor pose in body frame: "); - } - - /// Needed for testable - //------------------------------------------------------------------------------ - bool equals(const ImuFactorBase& expected, double tol) const { - return equal_with_abs_tol(gravity_, expected.gravity_, tol) - && equal_with_abs_tol(omegaCoriolis_, expected.omegaCoriolis_, tol) - && (use2ndOrderCoriolis_ == expected.use2ndOrderCoriolis_) - && ((!body_P_sensor_ && !expected.body_P_sensor_) - || (body_P_sensor_ && expected.body_P_sensor_ - && body_P_sensor_->equals(*expected.body_P_sensor_))); - } - -}; - -} /// namespace gtsam diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp new file mode 100644 index 000000000..6266c328f --- /dev/null +++ b/gtsam/navigation/NavState.cpp @@ -0,0 +1,358 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file NavState.h + * @brief Navigation state composing of attitude, position, and velocity + * @author Frank Dellaert + * @date July 2015 + **/ + +#include + +using namespace std; + +namespace gtsam { + +#define TIE(R,t,v,x) const Rot3& R = (x).R_;const Point3& t = (x).t_;const Velocity3& v = (x).v_; + +//------------------------------------------------------------------------------ +NavState NavState::FromPoseVelocity(const Pose3& pose, const Vector3& vel, + OptionalJacobian<9, 6> H1, OptionalJacobian<9, 3> H2) { + if (H1) + *H1 << I_3x3, Z_3x3, Z_3x3, I_3x3, Z_3x3, Z_3x3; + if (H2) + *H2 << Z_3x3, Z_3x3, pose.rotation().transpose(); + return NavState(pose, vel); +} + +//------------------------------------------------------------------------------ +const Rot3& NavState::attitude(OptionalJacobian<3, 9> H) const { + if (H) + *H << I_3x3, Z_3x3, Z_3x3; + return R_; +} + +//------------------------------------------------------------------------------ +const Point3& NavState::position(OptionalJacobian<3, 9> H) const { + if (H) + *H << Z_3x3, R(), Z_3x3; + return t_; +} + +//------------------------------------------------------------------------------ +const Vector3& NavState::velocity(OptionalJacobian<3, 9> H) const { + if (H) + *H << Z_3x3, Z_3x3, R(); + return v_; +} + +//------------------------------------------------------------------------------ +Vector3 NavState::bodyVelocity(OptionalJacobian<3, 9> H) const { + const Rot3& nRb = R_; + const Vector3& n_v = v_; + Matrix3 D_bv_nRb; + Vector3 b_v = nRb.unrotate(n_v, H ? &D_bv_nRb : 0); + if (H) + *H << D_bv_nRb, Z_3x3, I_3x3; + return b_v; +} + +//------------------------------------------------------------------------------ +Matrix7 NavState::matrix() const { + Matrix3 R = this->R(); + Matrix7 T; + T << R, Z_3x3, t(), Z_3x3, R, v(), Vector6::Zero().transpose(), 1.0; + return T; +} + +//------------------------------------------------------------------------------ +void NavState::print(const string& s) const { + attitude().print(s + ".R"); + position().print(s + ".p"); + gtsam::print((Vector) v_, s + ".v"); +} + +//------------------------------------------------------------------------------ +bool NavState::equals(const NavState& other, double tol) const { + return R_.equals(other.R_, tol) && t_.equals(other.t_, tol) + && equal_with_abs_tol(v_, other.v_, tol); +} + +//------------------------------------------------------------------------------ +NavState NavState::inverse() const { + TIE(nRb, n_t, n_v, *this); + const Rot3 bRn = nRb.inverse(); + return NavState(bRn, -(bRn * n_t), -(bRn * n_v)); +} + +//------------------------------------------------------------------------------ +NavState NavState::operator*(const NavState& bTc) const { + TIE(nRb, n_t, n_v, *this); + TIE(bRc, b_t, b_v, bTc); + return NavState(nRb * bRc, nRb * b_t + n_t, nRb * b_v + n_v); +} + +//------------------------------------------------------------------------------ +NavState::PositionAndVelocity // +NavState::operator*(const PositionAndVelocity& b_tv) const { + TIE(nRb, n_t, n_v, *this); + const Point3& b_t = b_tv.first; + const Velocity3& b_v = b_tv.second; + return PositionAndVelocity(nRb * b_t + n_t, nRb * b_v + n_v); +} + +//------------------------------------------------------------------------------ +Point3 NavState::operator*(const Point3& b_t) const { + return Point3(R_ * b_t + t_); +} + +//------------------------------------------------------------------------------ +Velocity3 NavState::operator*(const Velocity3& b_v) const { + return Velocity3(R_ * b_v + v_); +} + +//------------------------------------------------------------------------------ +NavState NavState::ChartAtOrigin::Retract(const Vector9& xi, + OptionalJacobian<9, 9> H) { + Matrix3 D_R_xi; + const Rot3 R = Rot3::Expmap(dR(xi), H ? &D_R_xi : 0); + const Point3 p = Point3(dP(xi)); + const Vector v = dV(xi); + const NavState result(R, p, v); + if (H) { + *H << D_R_xi, Z_3x3, Z_3x3, // + Z_3x3, R.transpose(), Z_3x3, // + Z_3x3, Z_3x3, R.transpose(); + } + return result; +} + +//------------------------------------------------------------------------------ +Vector9 NavState::ChartAtOrigin::Local(const NavState& x, + OptionalJacobian<9, 9> H) { + Vector9 xi; + Matrix3 D_xi_R; + xi << Rot3::Logmap(x.R_, H ? &D_xi_R : 0), x.t(), x.v(); + if (H) { + *H << D_xi_R, Z_3x3, Z_3x3, // + Z_3x3, x.R(), Z_3x3, // + Z_3x3, Z_3x3, x.R(); + } + return xi; +} + +//------------------------------------------------------------------------------ +NavState NavState::Expmap(const Vector9& xi, OptionalJacobian<9, 9> H) { + if (H) + throw runtime_error("NavState::Expmap derivative not implemented yet"); + + Eigen::Block n_omega_nb = dR(xi); + Eigen::Block v = dP(xi); + Eigen::Block a = dV(xi); + + // NOTE(frank): See Pose3::Expmap + Rot3 nRb = Rot3::Expmap(n_omega_nb); + double theta2 = n_omega_nb.dot(n_omega_nb); + if (theta2 > numeric_limits::epsilon()) { + // Expmap implements a "screw" motion in the direction of n_omega_nb + Vector3 n_t_parallel = n_omega_nb * n_omega_nb.dot(v); // component along rotation axis + Vector3 omega_cross_v = n_omega_nb.cross(v); // points towards axis + Point3 n_t = Point3(omega_cross_v - nRb * omega_cross_v + n_t_parallel) + / theta2; + Vector3 n_v_parallel = n_omega_nb * n_omega_nb.dot(a); // component along rotation axis + Vector3 omega_cross_a = n_omega_nb.cross(a); // points towards axis + Vector3 n_v = (omega_cross_a - nRb * omega_cross_a + n_v_parallel) / theta2; + return NavState(nRb, n_t, n_v); + } else { + return NavState(nRb, Point3(v), a); + } +} + +//------------------------------------------------------------------------------ +Vector9 NavState::Logmap(const NavState& nTb, OptionalJacobian<9, 9> H) { + if (H) + throw runtime_error("NavState::Logmap derivative not implemented yet"); + + TIE(nRb, n_p, n_v, nTb); + Vector3 n_t = n_p.vector(); + + // NOTE(frank): See Pose3::Logmap + Vector9 xi; + Vector3 n_omega_nb = Rot3::Logmap(nRb); + double theta = n_omega_nb.norm(); + if (theta * theta <= numeric_limits::epsilon()) { + xi << n_omega_nb, n_t, n_v; + } else { + Matrix3 W = skewSymmetric(n_omega_nb / theta); + // Formula from Agrawal06iros, equation (14) + // simplified with Mathematica, and multiplying in n_t to avoid matrix math + double factor = (1 - theta / (2. * tan(0.5 * theta))); + Vector3 n_x = W * n_t; + Vector3 v = n_t - (0.5 * theta) * n_x + factor * (W * n_x); + Vector3 n_y = W * n_v; + Vector3 a = n_v - (0.5 * theta) * n_y + factor * (W * n_y); + xi << n_omega_nb, v, a; + } + return xi; +} + +//------------------------------------------------------------------------------ +Matrix9 NavState::AdjointMap() const { + // NOTE(frank): See Pose3::AdjointMap + const Matrix3 nRb = R(); + Matrix3 pAr = skewSymmetric(t()) * nRb; + Matrix3 vAr = skewSymmetric(v()) * nRb; + Matrix9 adj; + // nR/bR nR/bP nR/bV nP/bR nP/bP nP/bV nV/bR nV/bP nV/bV + adj << nRb, Z_3x3, Z_3x3, pAr, nRb, Z_3x3, vAr, Z_3x3, nRb; + return adj; +} + +//------------------------------------------------------------------------------ +Matrix7 NavState::wedge(const Vector9& xi) { + const Matrix3 Omega = skewSymmetric(dR(xi)); + Matrix7 T; + T << Omega, Z_3x3, dP(xi), Z_3x3, Omega, dV(xi), Vector6::Zero().transpose(), 1.0; + return T; +} + +//------------------------------------------------------------------------------ +// sugar for derivative blocks +#define D_R_R(H) (H)->block<3,3>(0,0) +#define D_R_t(H) (H)->block<3,3>(0,3) +#define D_R_v(H) (H)->block<3,3>(0,6) +#define D_t_R(H) (H)->block<3,3>(3,0) +#define D_t_t(H) (H)->block<3,3>(3,3) +#define D_t_v(H) (H)->block<3,3>(3,6) +#define D_v_R(H) (H)->block<3,3>(6,0) +#define D_v_t(H) (H)->block<3,3>(6,3) +#define D_v_v(H) (H)->block<3,3>(6,6) + +//------------------------------------------------------------------------------ +NavState NavState::update(const Vector3& b_acceleration, const Vector3& b_omega, + const double dt, OptionalJacobian<9, 9> F, OptionalJacobian<9, 3> G1, + OptionalJacobian<9, 3> G2) const { + + Vector9 xi; + Matrix39 D_xiP_state; + Vector3 b_v = bodyVelocity(F ? &D_xiP_state : 0); + double dt22 = 0.5 * dt * dt; + + // Integrate on tangent space. TODO(frank): coriolis? + dR(xi) << dt * b_omega; + dP(xi) << dt * b_v + dt22 * b_acceleration; + dV(xi) << dt * b_acceleration; + + // Bring back to manifold + Matrix9 D_newState_xi; + NavState newState = retract(xi, F, G1 || G2 ? &D_newState_xi : 0); + + // Derivative wrt state is computed by retract directly + // However, as dP(xi) also depends on state, we need to add that contribution + if (F) { + F->middleRows<3>(3) += dt * D_t_t(F) * D_xiP_state; + } + // derivative wrt acceleration + if (G1) { + // D_newState_dPxi = D_newState_xi.middleCols<3>(3) + // D_dPxi_acc = dt22 * I_3x3 + // D_newState_dVxi = D_newState_xi.rightCols<3>() + // D_dVxi_acc = dt * I_3x3 + // *G2 = D_newState_acc = D_newState_dPxi * D_dPxi_acc + D_newState_dVxi * D_dVxi_acc + *G1 = D_newState_xi.middleCols<3>(3) * dt22 + + D_newState_xi.rightCols<3>() * dt; + } + // derivative wrt omega + if (G2) { + // D_newState_dRxi = D_newState_xi.leftCols<3>() + // D_dRxi_omega = dt * I_3x3 + // *G1 = D_newState_omega = D_newState_dRxi * D_dRxi_omega + *G2 = D_newState_xi.leftCols<3>() * dt; + } + return newState; +} + +//------------------------------------------------------------------------------ +Vector9 NavState::coriolis(double dt, const Vector3& omega, bool secondOrder, + OptionalJacobian<9, 9> H) const { + TIE(nRb, n_t, n_v, *this); + const double dt2 = dt * dt; + const Vector3 omega_cross_vel = omega.cross(n_v); + + Vector9 xi; + Matrix3 D_dP_R; + dR(xi) << nRb.unrotate((-dt) * omega, H ? &D_dP_R : 0); + dP(xi) << ((-dt2) * omega_cross_vel); // NOTE(luca): we got rid of the 2 wrt INS paper + dV(xi) << ((-2.0 * dt) * omega_cross_vel); + if (secondOrder) { + const Vector3 omega_cross2_t = omega.cross(omega.cross(n_t.vector())); + dP(xi) -= (0.5 * dt2) * omega_cross2_t; + dV(xi) -= dt * omega_cross2_t; + } + if (H) { + H->setZero(); + const Matrix3 Omega = skewSymmetric(omega); + const Matrix3 D_cross_state = Omega * R(); + H->setZero(); + D_R_R(H) << D_dP_R; + D_t_v(H) << (-dt2) * D_cross_state; + D_v_v(H) << (-2.0 * dt) * D_cross_state; + if (secondOrder) { + const Matrix3 D_cross2_state = Omega * D_cross_state; + D_t_t(H) -= (0.5 * dt2) * D_cross2_state; + D_v_t(H) -= dt * D_cross2_state; + } + } + return xi; +} + +//------------------------------------------------------------------------------ +Vector9 NavState::correctPIM(const Vector9& pim, double dt, + const Vector3& n_gravity, const boost::optional& omegaCoriolis, + bool use2ndOrderCoriolis, OptionalJacobian<9, 9> H1, + OptionalJacobian<9, 9> H2) const { + const Rot3& nRb = R_; + const Velocity3& n_v = v_; // derivative is Ri ! + const double dt22 = 0.5 * dt * dt; + + Vector9 xi; + Matrix3 D_dP_Ri1, D_dP_Ri2, D_dP_nv, D_dV_Ri; + dR(xi) = dR(pim); + dP(xi) = dP(pim) + + dt * nRb.unrotate(n_v, H1 ? &D_dP_Ri1 : 0, H2 ? &D_dP_nv : 0) + + dt22 * nRb.unrotate(n_gravity, H1 ? &D_dP_Ri2 : 0); + dV(xi) = dV(pim) + dt * nRb.unrotate(n_gravity, H1 ? &D_dV_Ri : 0); + + if (omegaCoriolis) { + xi += coriolis(dt, *omegaCoriolis, use2ndOrderCoriolis, H1); + } + + if (H1 || H2) { + Matrix3 Ri = nRb.matrix(); + + if (H1) { + if (!omegaCoriolis) + H1->setZero(); // if coriolis H1 is already initialized + D_t_R(H1) += dt * D_dP_Ri1 + dt22 * D_dP_Ri2; + D_t_v(H1) += dt * D_dP_nv * Ri; + D_v_R(H1) += dt * D_dV_Ri; + } + if (H2) { + H2->setIdentity(); + } + } + + return xi; +} +//------------------------------------------------------------------------------ + +}/// namespace gtsam diff --git a/gtsam/navigation/NavState.h b/gtsam/navigation/NavState.h new file mode 100644 index 000000000..9561aa77b --- /dev/null +++ b/gtsam/navigation/NavState.h @@ -0,0 +1,257 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file NavState.h + * @brief Navigation state composing of attitude, position, and velocity + * @author Frank Dellaert + * @date July 2015 + **/ + +#pragma once + +#include +#include +#include + +namespace gtsam { + +/// Velocity is currently typedef'd to Vector3 +typedef Vector3 Velocity3; + +/** + * Navigation state: Pose (rotation, translation) + velocity + * Implements semi-direct Lie group product of SO(3) and R^6, where R^6 is position/velocity + */ +class NavState: public LieGroup { +private: + + // TODO(frank): + // - should we rename t_ to p_? if not, we should rename dP do dT + Rot3 R_; ///< Rotation nRb, rotates points/velocities in body to points/velocities in nav + Point3 t_; ///< position n_t, in nav frame + Velocity3 v_; ///< velocity n_v in nav frame + +public: + + typedef std::pair PositionAndVelocity; + + /// @name Constructors + /// @{ + + /// Default constructor + NavState() : + v_(Vector3::Zero()) { + } + /// Construct from attitude, position, velocity + NavState(const Rot3& R, const Point3& t, const Velocity3& v) : + R_(R), t_(t), v_(v) { + } + /// Construct from pose and velocity + NavState(const Pose3& pose, const Velocity3& v) : + R_(pose.rotation()), t_(pose.translation()), v_(v) { + } + /// Construct from Matrix group representation (no checking) + NavState(const Matrix7& T) : + R_(T.block<3, 3>(0, 0)), t_(T.block<3, 1>(0, 6)), v_(T.block<3, 1>(3, 6)) { + } + /// Construct from SO(3) and R^6 + NavState(const Matrix3& R, const Vector9 tv) : + R_(R), t_(tv.head<3>()), v_(tv.tail<3>()) { + } + /// Named constructor with derivatives + static NavState FromPoseVelocity(const Pose3& pose, const Vector3& vel, + OptionalJacobian<9, 6> H1, OptionalJacobian<9, 3> H2); + + /// @} + /// @name Component Access + /// @{ + + inline const Rot3& attitude() const { + return R_; + } + inline const Point3& position() const { + return t_; + } + inline const Velocity3& velocity() const { + return v_; + } + const Rot3& attitude(OptionalJacobian<3, 9> H) const; + const Point3& position(OptionalJacobian<3, 9> H) const; + const Velocity3& velocity(OptionalJacobian<3, 9> H) const; + + const Pose3 pose() const { + return Pose3(attitude(), position()); + } + + /// @} + /// @name Derived quantities + /// @{ + + /// Return rotation matrix. Induces computation in quaternion mode + Matrix3 R() const { + return R_.matrix(); + } + /// Return quaternion. Induces computation in matrix mode + Quaternion quaternion() const { + return R_.toQuaternion(); + } + /// Return position as Vector3 + Vector3 t() const { + return t_.vector(); + } + /// Return velocity as Vector3. Computation-free. + const Vector3& v() const { + return v_; + } + // Return velocity in body frame + Velocity3 bodyVelocity(OptionalJacobian<3, 9> H = boost::none) const; + + /// Return matrix group representation, in MATLAB notation: + /// nTb = [nRb 0 n_t; 0 nRb n_v; 0 0 1] + /// With this embedding in GL(3), matrix product agrees with compose + Matrix7 matrix() const; + + /// @} + /// @name Testable + /// @{ + + /// print + void print(const std::string& s = "") const; + + /// equals + bool equals(const NavState& other, double tol = 1e-8) const; + + /// @} + /// @name Group + /// @{ + + /// identity for group operation + static NavState identity() { + return NavState(); + } + + /// inverse transformation with derivatives + NavState inverse() const; + + /// Group compose is the semi-direct product as specified below: + /// nTc = nTb * bTc = (nRb * bRc, nRb * b_t + n_t, nRb * b_v + n_v) + NavState operator*(const NavState& bTc) const; + + /// Native group action is on position/velocity pairs *in the body frame* as follows: + /// nTb * (b_t,b_v) = (nRb * b_t + n_t, nRb * b_v + n_v) + PositionAndVelocity operator*(const PositionAndVelocity& b_tv) const; + + /// Act on position alone, n_t = nRb * b_t + n_t + Point3 operator*(const Point3& b_t) const; + + /// Act on velocity alone, n_v = nRb * b_v + n_v + Velocity3 operator*(const Velocity3& b_v) const; + + /// @} + /// @name Manifold + /// @{ + + // Tangent space sugar. + // TODO(frank): move to private navstate namespace in cpp + static Eigen::Block dR(Vector9& v) { + return v.segment<3>(0); + } + static Eigen::Block dP(Vector9& v) { + return v.segment<3>(3); + } + static Eigen::Block dV(Vector9& v) { + return v.segment<3>(6); + } + static Eigen::Block dR(const Vector9& v) { + return v.segment<3>(0); + } + static Eigen::Block dP(const Vector9& v) { + return v.segment<3>(3); + } + static Eigen::Block dV(const Vector9& v) { + return v.segment<3>(6); + } + + // Chart at origin, constructs components separately (as opposed to Expmap) + struct ChartAtOrigin { + static NavState Retract(const Vector9& xi, // + OptionalJacobian<9, 9> H = boost::none); + static Vector9 Local(const NavState& x, // + OptionalJacobian<9, 9> H = boost::none); + }; + + /// @} + /// @name Lie Group + /// @{ + + /// Exponential map at identity - create a NavState from canonical coordinates + static NavState Expmap(const Vector9& xi, // + OptionalJacobian<9, 9> H = boost::none); + + /// Log map at identity - return the canonical coordinates for this NavState + static Vector9 Logmap(const NavState& p, // + OptionalJacobian<9, 9> H = boost::none); + + /// Calculate Adjoint map, a 9x9 matrix that takes a tangent vector in the body frame, and transforms + /// it to a tangent vector at identity, such that Exmap(AdjointMap()*xi) = (*this) * Exmpap(xi); + Matrix9 AdjointMap() const; + + /// wedge creates Lie algebra element from tangent vector + static Matrix7 wedge(const Vector9& xi); + + /// @} + /// @name Dynamics + /// @{ + + /// Integrate forward in time given angular velocity and acceleration in body frame + /// Uses second order integration for position, returns derivatives except dt. + NavState update(const Vector3& b_acceleration, const Vector3& b_omega, + const double dt, OptionalJacobian<9, 9> F, OptionalJacobian<9, 3> G1, + OptionalJacobian<9, 3> G2) const; + + /// Compute tangent space contribution due to Coriolis forces + Vector9 coriolis(double dt, const Vector3& omega, bool secondOrder = false, + OptionalJacobian<9, 9> H = boost::none) const; + + /// Correct preintegrated tangent vector with our velocity and rotated gravity, + /// taking into account Coriolis forces if omegaCoriolis is given. + Vector9 correctPIM(const Vector9& pim, double dt, const Vector3& n_gravity, + const boost::optional& omegaCoriolis, bool use2ndOrderCoriolis = + false, OptionalJacobian<9, 9> H1 = boost::none, + OptionalJacobian<9, 9> H2 = boost::none) const; + +private: + /// @{ + /// serialization + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + ar & BOOST_SERIALIZATION_NVP(R_); + ar & BOOST_SERIALIZATION_NVP(t_); + ar & BOOST_SERIALIZATION_NVP(v_); + } + /// @} +}; + +// Specialize NavState traits to use a Retract/Local that agrees with IMUFactors +template<> +struct traits : Testable, internal::LieGroupTraits { +}; + +// Partial specialization of wedge +// TODO: deprecate, make part of traits +template<> +inline Matrix wedge(const Vector& xi) { + return NavState::wedge(xi); +} + +} // namespace gtsam diff --git a/gtsam/navigation/PreintegratedRotation.cpp b/gtsam/navigation/PreintegratedRotation.cpp new file mode 100644 index 000000000..708d1a3f6 --- /dev/null +++ b/gtsam/navigation/PreintegratedRotation.cpp @@ -0,0 +1,111 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file PreintegratedRotation.cpp + * @author Luca Carlone + * @author Stephen Williams + * @author Richard Roberts + * @author Vadim Indelman + * @author David Jensen + * @author Frank Dellaert + **/ + +#include "PreintegratedRotation.h" + +using namespace std; + +namespace gtsam { + +void PreintegratedRotation::Params::print(const string& s) const { + cout << s << endl; + cout << "gyroscopeCovariance:\n[\n" << gyroscopeCovariance << "\n]" << endl; + if (omegaCoriolis) + cout << "omegaCoriolis = (" << omegaCoriolis->transpose() << ")" + << endl; + if (body_P_sensor) + body_P_sensor->print("body_P_sensor"); +} + +void PreintegratedRotation::resetIntegration() { + deltaTij_ = 0.0; + deltaRij_ = Rot3(); + delRdelBiasOmega_ = Z_3x3; +} + +void PreintegratedRotation::print(const string& s) const { + cout << s << endl; + cout << " deltaTij [" << deltaTij_ << "]" << endl; + cout << " deltaRij.ypr = (" << deltaRij_.ypr().transpose() << ")" << endl; +} + +bool PreintegratedRotation::equals(const PreintegratedRotation& other, + double tol) const { + return deltaRij_.equals(other.deltaRij_, tol) + && fabs(deltaTij_ - other.deltaTij_) < tol + && equal_with_abs_tol(delRdelBiasOmega_, other.delRdelBiasOmega_, tol); +} + +Rot3 PreintegratedRotation::incrementalRotation(const Vector3& measuredOmega, + const Vector3& biasHat, double deltaT, + OptionalJacobian<3, 3> D_incrR_integratedOmega) const { + + // First we compensate the measurements for the bias + Vector3 correctedOmega = measuredOmega - biasHat; + + // Then compensate for sensor-body displacement: we express the quantities + // (originally in the IMU frame) into the body frame + if (p_->body_P_sensor) { + Matrix3 body_R_sensor = p_->body_P_sensor->rotation().matrix(); + // rotation rate vector in the body frame + correctedOmega = body_R_sensor * correctedOmega; + } + + // rotation vector describing rotation increment computed from the + // current rotation rate measurement + const Vector3 integratedOmega = correctedOmega * deltaT; + return Rot3::Expmap(integratedOmega, D_incrR_integratedOmega); // expensive !! +} + +void PreintegratedRotation::integrateMeasurement(const Vector3& measuredOmega, + const Vector3& biasHat, double deltaT, Matrix3* D_incrR_integratedOmega, + Matrix3* F) { + + const Rot3 incrR = incrementalRotation(measuredOmega, biasHat, deltaT, + D_incrR_integratedOmega); + + // Update deltaTij and rotation + deltaTij_ += deltaT; + deltaRij_ = deltaRij_.compose(incrR, F); + + // Update Jacobian + const Matrix3 incrRt = incrR.transpose(); + delRdelBiasOmega_ = incrRt * delRdelBiasOmega_ + - *D_incrR_integratedOmega * deltaT; +} + +Rot3 PreintegratedRotation::biascorrectedDeltaRij(const Vector3& biasOmegaIncr, + OptionalJacobian<3, 3> H) const { + const Vector3 biasInducedOmega = delRdelBiasOmega_ * biasOmegaIncr; + const Rot3 deltaRij_biascorrected = deltaRij_.expmap(biasInducedOmega, + boost::none, H); + if (H) + (*H) *= delRdelBiasOmega_; + return deltaRij_biascorrected; +} + +Vector3 PreintegratedRotation::integrateCoriolis(const Rot3& rot_i) const { + if (!p_->omegaCoriolis) + return Vector3::Zero(); + return rot_i.transpose() * (*p_->omegaCoriolis) * deltaTij_; +} + +} // namespace gtsam diff --git a/gtsam/navigation/PreintegratedRotation.h b/gtsam/navigation/PreintegratedRotation.h index 2379f58af..002afea76 100644 --- a/gtsam/navigation/PreintegratedRotation.h +++ b/gtsam/navigation/PreintegratedRotation.h @@ -21,7 +21,8 @@ #pragma once -#include +#include +#include namespace gtsam { @@ -31,124 +32,104 @@ namespace gtsam { * It includes the definitions of the preintegrated rotation. */ class PreintegratedRotation { - - Rot3 deltaRij_; ///< Preintegrated relative orientation (in frame i) - double deltaTij_; ///< Time interval from i to j - - /// Jacobian of preintegrated rotation w.r.t. angular rate bias - Matrix3 delRdelBiasOmega_; - Matrix3 gyroscopeCovariance_; ///< continuous-time "Covariance" of gyroscope measurements - public: - /** - * Default constructor, initializes the variables in the base class - */ - PreintegratedRotation(const Matrix3& measuredOmegaCovariance) : - deltaRij_(Rot3()), deltaTij_(0.0), delRdelBiasOmega_(Z_3x3), gyroscopeCovariance_( - measuredOmegaCovariance) { + /// Parameters for pre-integration: + /// Usage: Create just a single Params and pass a shared pointer to the constructor + struct Params { + Matrix3 gyroscopeCovariance; ///< continuous-time "Covariance" of gyroscope measurements + boost::optional omegaCoriolis; ///< Coriolis constant + boost::optional body_P_sensor; ///< The pose of the sensor in the body frame + + Params() : + gyroscopeCovariance(I_3x3) { + } + + virtual void print(const std::string& s) const; + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + ar & BOOST_SERIALIZATION_NVP(gyroscopeCovariance); + ar & BOOST_SERIALIZATION_NVP(omegaCoriolis); + ar & BOOST_SERIALIZATION_NVP(body_P_sensor); + } + }; + +protected: + double deltaTij_; ///< Time interval from i to j + Rot3 deltaRij_; ///< Preintegrated relative orientation (in frame i) + Matrix3 delRdelBiasOmega_; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias + + /// Parameters + boost::shared_ptr p_; + + /// Default constructor for serialization + PreintegratedRotation() { } - /// methods to access class variables - Matrix3 deltaRij() const { - return deltaRij_.matrix(); - } // expensive - Vector3 thetaRij(OptionalJacobian<3, 3> H = boost::none) const { - return Rot3::Logmap(deltaRij_, H); - } // super-expensive +public: + /// Default constructor, resets integration to zero + explicit PreintegratedRotation(const boost::shared_ptr& p) : + p_(p) { + resetIntegration(); + } + + /// Re-initialize PreintegratedMeasurements + void resetIntegration(); + + /// @name Access instance variables + /// @{ const double& deltaTij() const { return deltaTij_; } + const Rot3& deltaRij() const { + return deltaRij_; + } const Matrix3& delRdelBiasOmega() const { return delRdelBiasOmega_; } - const Matrix3& gyroscopeCovariance() const { - return gyroscopeCovariance_; - } + /// @} - /// Needed for testable - void print(const std::string& s) const { - std::cout << s << std::endl; - std::cout << " deltaTij [" << deltaTij_ << "]" << std::endl; - std::cout << " deltaRij.ypr = (" << deltaRij_.ypr().transpose() << ")" << std::endl; - } + /// @name Testable + /// @{ - /// Needed for testable - bool equals(const PreintegratedRotation& expected, double tol) const { - return deltaRij_.equals(expected.deltaRij_, tol) - && fabs(deltaTij_ - expected.deltaTij_) < tol - && equal_with_abs_tol(delRdelBiasOmega_, expected.delRdelBiasOmega_, - tol) - && equal_with_abs_tol(gyroscopeCovariance_, - expected.gyroscopeCovariance_, tol); - } + void print(const std::string& s) const; + bool equals(const PreintegratedRotation& other, double tol) const; - /// Re-initialize PreintegratedMeasurements - void resetIntegration() { - deltaRij_ = Rot3(); - deltaTij_ = 0.0; - delRdelBiasOmega_ = Z_3x3; - } + /// @} - /// Update preintegrated measurements - void updateIntegratedRotationAndDeltaT(const Rot3& incrR, const double deltaT, - OptionalJacobian<3, 3> H = boost::none) { - deltaRij_ = deltaRij_.compose(incrR, H, boost::none); - deltaTij_ += deltaT; - } + /// Take the gyro measurement, correct it using the (constant) bias estimate + /// and possibly the sensor pose, and then integrate it forward in time to yield + /// an incremental rotation. + Rot3 incrementalRotation(const Vector3& measuredOmega, const Vector3& biasHat, + double deltaT, OptionalJacobian<3, 3> D_incrR_integratedOmega) const; - /** - * Update Jacobians to be used during preintegration - * TODO: explain arguments - */ - void update_delRdelBiasOmega(const Matrix3& D_Rincr_integratedOmega, - const Rot3& incrR, double deltaT) { - const Matrix3 incrRt = incrR.transpose(); - delRdelBiasOmega_ = incrRt * delRdelBiasOmega_ - - D_Rincr_integratedOmega * deltaT; - } + /// Calculate an incremental rotation given the gyro measurement and a time interval, + /// and update both deltaTij_ and deltaRij_. + void integrateMeasurement(const Vector3& measuredOmega, + const Vector3& biasHat, double deltaT, Matrix3* D_incrR_integratedOmega, + Matrix3* F); - /// Return a bias corrected version of the integrated rotation - expensive - Rot3 biascorrectedDeltaRij(const Vector3& biasOmegaIncr) const { - return deltaRij_ * Rot3::Expmap(delRdelBiasOmega_ * biasOmegaIncr); - } - - /// Get so<3> version of bias corrected rotation, with optional Jacobian - // Implements: log( deltaRij_ * expmap(delRdelBiasOmega_ * biasOmegaIncr) ) - Vector3 biascorrectedThetaRij(const Vector3& biasOmegaIncr, - OptionalJacobian<3, 3> H = boost::none) const { - // First, we correct deltaRij using the biasOmegaIncr, rotated - const Rot3 deltaRij_biascorrected = biascorrectedDeltaRij(biasOmegaIncr); - - if (H) { - Matrix3 Jrinv_theta_bc; - // This was done via an expmap, now we go *back* to so<3> - const Vector3 biascorrectedOmega = Rot3::Logmap(deltaRij_biascorrected, - Jrinv_theta_bc); - const Matrix3 Jr_JbiasOmegaIncr = // - Rot3::ExpmapDerivative(delRdelBiasOmega_ * biasOmegaIncr); - (*H) = Jrinv_theta_bc * Jr_JbiasOmegaIncr * delRdelBiasOmega_; - return biascorrectedOmega; - } - //else - return Rot3::Logmap(deltaRij_biascorrected); - } + /// Return a bias corrected version of the integrated rotation, with optional Jacobian + Rot3 biascorrectedDeltaRij(const Vector3& biasOmegaIncr, + OptionalJacobian<3, 3> H = boost::none) const; /// Integrate coriolis correction in body frame rot_i - Vector3 integrateCoriolis(const Rot3& rot_i, - const Vector3& omegaCoriolis) const { - return rot_i.transpose() * omegaCoriolis * deltaTij(); - } + Vector3 integrateCoriolis(const Rot3& rot_i) const; private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - ar & BOOST_SERIALIZATION_NVP(deltaRij_); + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { // NOLINT + ar & BOOST_SERIALIZATION_NVP(p_); ar & BOOST_SERIALIZATION_NVP(deltaTij_); + ar & BOOST_SERIALIZATION_NVP(deltaRij_); ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega_); } }; -} /// namespace gtsam +} // namespace gtsam diff --git a/gtsam/navigation/PreintegrationBase.cpp b/gtsam/navigation/PreintegrationBase.cpp index 496569599..bb01971e6 100644 --- a/gtsam/navigation/PreintegrationBase.cpp +++ b/gtsam/navigation/PreintegrationBase.cpp @@ -20,338 +20,293 @@ **/ #include "PreintegrationBase.h" +#include + +using namespace std; namespace gtsam { -PreintegrationBase::PreintegrationBase(const imuBias::ConstantBias& bias, - const Matrix3& measuredAccCovariance, - const Matrix3& measuredOmegaCovariance, - const Matrix3&integrationErrorCovariance, - const bool use2ndOrderIntegration) - : PreintegratedRotation(measuredOmegaCovariance), - biasHat_(bias), - use2ndOrderIntegration_(use2ndOrderIntegration), - deltaPij_(Vector3::Zero()), - deltaVij_(Vector3::Zero()), - delPdelBiasAcc_(Z_3x3), - delPdelBiasOmega_(Z_3x3), - delVdelBiasAcc_(Z_3x3), - delVdelBiasOmega_(Z_3x3), - accelerometerCovariance_(measuredAccCovariance), - integrationCovariance_(integrationErrorCovariance) { +//------------------------------------------------------------------------------ +void PreintegrationBase::Params::print(const string& s) const { + PreintegratedRotation::Params::print(s); + cout << "accelerometerCovariance:\n[\n" << accelerometerCovariance << "\n]" + << endl; + cout << "integrationCovariance:\n[\n" << accelerometerCovariance << "\n]" + << endl; + if (omegaCoriolis && use2ndOrderCoriolis) + cout << "Using 2nd-order Coriolis" << endl; + if (body_P_sensor) + body_P_sensor->print(" "); + cout << "n_gravity = (" << n_gravity.transpose() << ")" << endl; } -/// Needed for testable -void PreintegrationBase::print(const std::string& s) const { - PreintegratedRotation::print(s); - std::cout << " deltaPij [ " << deltaPij_.transpose() << " ]" << std::endl; - std::cout << " deltaVij [ " << deltaVij_.transpose() << " ]" << std::endl; - biasHat_.print(" biasHat"); -} - -/// Needed for testable -bool PreintegrationBase::equals(const PreintegrationBase& other, double tol) const { - return PreintegratedRotation::equals(other, tol) && biasHat_.equals(other.biasHat_, tol) - && equal_with_abs_tol(deltaPij_, other.deltaPij_, tol) - && equal_with_abs_tol(deltaVij_, other.deltaVij_, tol) - && equal_with_abs_tol(delPdelBiasAcc_, other.delPdelBiasAcc_, tol) - && equal_with_abs_tol(delPdelBiasOmega_, other.delPdelBiasOmega_, tol) - && equal_with_abs_tol(delVdelBiasAcc_, other.delVdelBiasAcc_, tol) - && equal_with_abs_tol(delVdelBiasOmega_, other.delVdelBiasOmega_, tol) - && equal_with_abs_tol(accelerometerCovariance_, other.accelerometerCovariance_, tol) - && equal_with_abs_tol(integrationCovariance_, other.integrationCovariance_, tol); -} - -/// Re-initialize PreintegratedMeasurements +//------------------------------------------------------------------------------ void PreintegrationBase::resetIntegration() { - PreintegratedRotation::resetIntegration(); - deltaPij_ = Vector3::Zero(); - deltaVij_ = Vector3::Zero(); + deltaTij_ = 0.0; + deltaXij_ = NavState(); + delRdelBiasOmega_ = Z_3x3; delPdelBiasAcc_ = Z_3x3; delPdelBiasOmega_ = Z_3x3; delVdelBiasAcc_ = Z_3x3; delVdelBiasOmega_ = Z_3x3; } -/// Update preintegrated measurements -void PreintegrationBase::updatePreintegratedMeasurements(const Vector3& correctedAcc, - const Rot3& incrR, const double deltaT, - OptionalJacobian<9, 9> F) { - - const Matrix3 dRij = deltaRij(); // expensive - const Vector3 temp = dRij * correctedAcc * deltaT; - if (!use2ndOrderIntegration_) { - deltaPij_ += deltaVij_ * deltaT; - } else { - deltaPij_ += deltaVij_ * deltaT + 0.5 * temp * deltaT; - } - deltaVij_ += temp; - - Matrix3 R_i, F_angles_angles; - if (F) - R_i = deltaRij(); // has to be executed before updateIntegratedRotationAndDeltaT as that updates deltaRij - updateIntegratedRotationAndDeltaT(incrR, deltaT, F ? &F_angles_angles : 0); - - if (F) { - const Matrix3 F_vel_angles = -R_i * skewSymmetric(correctedAcc) * deltaT; - Matrix3 F_pos_angles; - if (use2ndOrderIntegration_) - F_pos_angles = 0.5 * F_vel_angles * deltaT; - else - F_pos_angles = Z_3x3; - - // pos vel angle - *F << // - I_3x3, I_3x3 * deltaT, F_pos_angles, // pos - Z_3x3, I_3x3, F_vel_angles, // vel - Z_3x3, Z_3x3, F_angles_angles; // angle - } +//------------------------------------------------------------------------------ +void PreintegrationBase::print(const string& s) const { + cout << s << endl; + cout << " deltaTij [" << deltaTij_ << "]" << endl; + cout << " deltaRij.ypr = (" << deltaRij().ypr().transpose() << ")" << endl; + cout << " deltaPij [ " << deltaPij().transpose() << " ]" << endl; + cout << " deltaVij [ " << deltaVij().transpose() << " ]" << endl; + biasHat_.print(" biasHat"); } -/// Update Jacobians to be used during preintegration -void PreintegrationBase::updatePreintegratedJacobians(const Vector3& correctedAcc, - const Matrix3& D_Rincr_integratedOmega, - const Rot3& incrR, double deltaT) { - const Matrix3 dRij = deltaRij(); // expensive - const Matrix3 temp = -dRij * skewSymmetric(correctedAcc) * deltaT * delRdelBiasOmega(); - if (!use2ndOrderIntegration_) { - delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT; - delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT; - } else { - delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT - 0.5 * dRij * deltaT * deltaT; - delPdelBiasOmega_ += deltaT * (delVdelBiasOmega_ + temp * 0.5); - } - delVdelBiasAcc_ += -dRij * deltaT; - delVdelBiasOmega_ += temp; - update_delRdelBiasOmega(D_Rincr_integratedOmega, incrR, deltaT); +//------------------------------------------------------------------------------ +bool PreintegrationBase::equals(const PreintegrationBase& other, + double tol) const { + return fabs(deltaTij_ - other.deltaTij_) < tol + && deltaXij_.equals(other.deltaXij_, tol) + && biasHat_.equals(other.biasHat_, tol) + && equal_with_abs_tol(delRdelBiasOmega_, other.delRdelBiasOmega_, tol) + && equal_with_abs_tol(delPdelBiasAcc_, other.delPdelBiasAcc_, tol) + && equal_with_abs_tol(delPdelBiasOmega_, other.delPdelBiasOmega_, tol) + && equal_with_abs_tol(delVdelBiasAcc_, other.delVdelBiasAcc_, tol) + && equal_with_abs_tol(delVdelBiasOmega_, other.delVdelBiasOmega_, tol); } -void PreintegrationBase::correctMeasurementsByBiasAndSensorPose( - const Vector3& measuredAcc, const Vector3& measuredOmega, Vector3& correctedAcc, - Vector3& correctedOmega, boost::optional body_P_sensor) { - correctedAcc = biasHat_.correctAccelerometer(measuredAcc); - correctedOmega = biasHat_.correctGyroscope(measuredOmega); +//------------------------------------------------------------------------------ +pair PreintegrationBase::correctMeasurementsByBiasAndSensorPose( + const Vector3& j_measuredAcc, const Vector3& j_measuredOmega, + OptionalJacobian<3, 3> D_correctedAcc_measuredAcc, + OptionalJacobian<3, 3> D_correctedAcc_measuredOmega, + OptionalJacobian<3, 3> D_correctedOmega_measuredOmega) const { - // Then compensate for sensor-body displacement: we express the quantities + // Correct for bias in the sensor frame + Vector3 j_correctedAcc = biasHat_.correctAccelerometer(j_measuredAcc); + Vector3 j_correctedOmega = biasHat_.correctGyroscope(j_measuredOmega); + + // Compensate for sensor-body displacement if needed: we express the quantities // (originally in the IMU frame) into the body frame - if (body_P_sensor) { - Matrix3 body_R_sensor = body_P_sensor->rotation().matrix(); - correctedOmega = body_R_sensor * correctedOmega; // rotation rate vector in the body frame - Matrix3 body_omega_body__cross = skewSymmetric(correctedOmega); - correctedAcc = body_R_sensor * correctedAcc - - body_omega_body__cross * body_omega_body__cross * body_P_sensor->translation().vector(); - // linear acceleration vector in the body frame - } -} + // Equations below assume the "body" frame is the CG + if (p().body_P_sensor) { + // Correct omega to rotation rate vector in the body frame + const Matrix3 bRs = p().body_P_sensor->rotation().matrix(); + j_correctedOmega = bRs * j_correctedOmega; -/// Predict state at time j -//------------------------------------------------------------------------------ -PoseVelocityBias PreintegrationBase::predict( - const Pose3& pose_i, const Vector3& vel_i, const imuBias::ConstantBias& bias_i, - const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis, - boost::optional deltaPij_biascorrected_out, - boost::optional deltaVij_biascorrected_out) const { + // Correct acceleration + j_correctedAcc = bRs * j_correctedAcc; - const imuBias::ConstantBias biasIncr = bias_i - biasHat(); - const Vector3& biasAccIncr = biasIncr.accelerometer(); - const Vector3& biasOmegaIncr = biasIncr.gyroscope(); + // Jacobians + if (D_correctedAcc_measuredAcc) *D_correctedAcc_measuredAcc = bRs; + if (D_correctedAcc_measuredOmega) *D_correctedAcc_measuredOmega = Matrix3::Zero(); + if (D_correctedOmega_measuredOmega) *D_correctedOmega_measuredOmega = bRs; - const Rot3& Rot_i = pose_i.rotation(); - const Matrix3 Rot_i_matrix = Rot_i.matrix(); - const Vector3 pos_i = pose_i.translation().vector(); - - // Predict state at time j - /* ---------------------------------------------------------------------------------------------------- */ - const Vector3 deltaPij_biascorrected = deltaPij() + delPdelBiasAcc() * biasAccIncr - + delPdelBiasOmega() * biasOmegaIncr; - if (deltaPij_biascorrected_out) // if desired, store this - *deltaPij_biascorrected_out = deltaPij_biascorrected; - - const double dt = deltaTij(), dt2 = dt * dt; - Vector3 pos_j = pos_i + Rot_i_matrix * deltaPij_biascorrected + vel_i * dt - - omegaCoriolis.cross(vel_i) * dt2 // Coriolis term - we got rid of the 2 wrt ins paper - + 0.5 * gravity * dt2; - - const Vector3 deltaVij_biascorrected = deltaVij() + delVdelBiasAcc() * biasAccIncr - + delVdelBiasOmega() * biasOmegaIncr; - if (deltaVij_biascorrected_out) // if desired, store this - *deltaVij_biascorrected_out = deltaVij_biascorrected; - - Vector3 vel_j = Vector3( - vel_i + Rot_i_matrix * deltaVij_biascorrected - 2 * omegaCoriolis.cross(vel_i) * dt // Coriolis term - + gravity * dt); - - if (use2ndOrderCoriolis) { - pos_j += -0.5 * omegaCoriolis.cross(omegaCoriolis.cross(pos_i)) * dt2; // 2nd order coriolis term for position - vel_j += -omegaCoriolis.cross(omegaCoriolis.cross(pos_i)) * dt; // 2nd order term for velocity - } - - const Rot3 deltaRij_biascorrected = biascorrectedDeltaRij(biasOmegaIncr); - // deltaRij_biascorrected = deltaRij * expmap(delRdelBiasOmega * biasOmegaIncr) - - const Vector3 biascorrectedOmega = Rot3::Logmap(deltaRij_biascorrected); - const Vector3 correctedOmega = biascorrectedOmega - - Rot_i_matrix.transpose() * omegaCoriolis * dt; // Coriolis term - const Rot3 correctedDeltaRij = Rot3::Expmap(correctedOmega); - const Rot3 Rot_j = Rot_i.compose(correctedDeltaRij); - - const Pose3 pose_j = Pose3(Rot_j, Point3(pos_j)); - return PoseVelocityBias(pose_j, vel_j, bias_i); // bias is predicted as a constant -} - -/// Compute errors w.r.t. preintegrated measurements and Jacobians wrpt pose_i, vel_i, bias_i, pose_j, bias_j -//------------------------------------------------------------------------------ -Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i, const Vector3& vel_i, - const Pose3& pose_j, const Vector3& vel_j, - const imuBias::ConstantBias& bias_i, - const Vector3& gravity, - const Vector3& omegaCoriolis, - const bool use2ndOrderCoriolis, - OptionalJacobian<9, 6> H1, - OptionalJacobian<9, 3> H2, - OptionalJacobian<9, 6> H3, - OptionalJacobian<9, 3> H4, - OptionalJacobian<9, 6> H5) const { - - // We need the mismatch w.r.t. the biases used for preintegration - const Vector3 biasOmegaIncr = bias_i.gyroscope() - biasHat().gyroscope(); - - // we give some shorter name to rotations and translations - const Rot3& Ri = pose_i.rotation(); - const Matrix3 Ri_transpose_matrix = Ri.transpose(); - - const Rot3& Rj = pose_j.rotation(); - const Vector3 pos_j = pose_j.translation().vector(); - - // Evaluate residual error, according to [3] - /* ---------------------------------------------------------------------------------------------------- */ - Vector3 deltaPij_biascorrected, deltaVij_biascorrected; - PoseVelocityBias predictedState_j = predict(pose_i, vel_i, bias_i, gravity, omegaCoriolis, - use2ndOrderCoriolis, deltaPij_biascorrected, - deltaVij_biascorrected); - - // Ri.transpose() is important here to preserve a model with *additive* Gaussian noise of correct covariance - const Vector3 fp = Ri_transpose_matrix * (pos_j - predictedState_j.pose.translation().vector()); - - // Ri.transpose() is important here to preserve a model with *additive* Gaussian noise of correct covariance - const Vector3 fv = Ri_transpose_matrix * (vel_j - predictedState_j.velocity); - - // fR will be computed later. Note: it is the same as: fR = (predictedState_j.pose.translation()).between(Rot_j) - - /* ---------------------------------------------------------------------------------------------------- */ - // Get Get so<3> version of bias corrected rotation - // If H5 is asked for, we will need the Jacobian, which we store in H5 - // H5 will then be corrected below to take into account the Coriolis effect - Matrix3 D_cThetaRij_biasOmegaIncr; - const Vector3 biascorrectedOmega = biascorrectedThetaRij(biasOmegaIncr, - H5 ? &D_cThetaRij_biasOmegaIncr : 0); - - // Coriolis term, note inconsistent with AHRS, where coriolisHat is *after* integration - const Vector3 coriolis = integrateCoriolis(Ri, omegaCoriolis); - const Vector3 correctedOmega = biascorrectedOmega - coriolis; - - // Calculate Jacobians, matrices below needed only for some Jacobians... - Vector3 fR; - Matrix3 D_cDeltaRij_cOmega, D_coriolis, D_fR_fRrot, Ritranspose_omegaCoriolisHat; - - if (H1 || H2) - Ritranspose_omegaCoriolisHat = Ri_transpose_matrix * skewSymmetric(omegaCoriolis); - - // This is done to save computation: we ask for the jacobians only when they are needed - const double dt = deltaTij(), dt2 = dt*dt; - Rot3 fRrot; - const Rot3 RiBetweenRj = Rot3(Ri_transpose_matrix) * Rj; - if (H1 || H2 || H3 || H4 || H5) { - const Rot3 correctedDeltaRij = Rot3::Expmap(correctedOmega, D_cDeltaRij_cOmega); - // Residual rotation error - fRrot = correctedDeltaRij.between(RiBetweenRj); - fR = Rot3::Logmap(fRrot, D_fR_fRrot); - D_coriolis = -D_cDeltaRij_cOmega * skewSymmetric(coriolis); - } else { - const Rot3 correctedDeltaRij = Rot3::Expmap(correctedOmega); - // Residual rotation error - fRrot = correctedDeltaRij.between(RiBetweenRj); - fR = Rot3::Logmap(fRrot); - } - if (H1) { - Matrix3 dfPdPi = -I_3x3, dfVdPi = Z_3x3; - if (use2ndOrderCoriolis) { - // this is the same as: Ri.transpose() * omegaCoriolisHat * omegaCoriolisHat * Ri.matrix() - const Matrix3 temp = Ritranspose_omegaCoriolisHat - * (-Ritranspose_omegaCoriolisHat.transpose()); - dfPdPi += 0.5 * temp * dt2; - dfVdPi += temp * dt; + // Centrifugal acceleration + const Vector3 b_arm = p().body_P_sensor->translation().vector(); + if (!b_arm.isZero()) { + // Subtract out the the centripetal acceleration from the measured one + // to get linear acceleration vector in the body frame: + const Matrix3 body_Omega_body = skewSymmetric(j_correctedOmega); + const Vector3 b_velocity_bs = body_Omega_body * b_arm; // magnitude: omega * arm + j_correctedAcc -= body_Omega_body * b_velocity_bs; + // Update derivative: centrifugal causes the correlation between acc and omega!!! + if (D_correctedAcc_measuredOmega) { + double wdp = j_correctedOmega.dot(b_arm); + *D_correctedAcc_measuredOmega = -(diag(Vector3::Constant(wdp)) + + j_correctedOmega * b_arm.transpose()) * bRs.matrix() + + 2 * b_arm * j_measuredOmega.transpose(); + } } - (*H1) << - // dfP/dRi - skewSymmetric(fp + deltaPij_biascorrected), - // dfP/dPi - dfPdPi, - // dfV/dRi - skewSymmetric(fv + deltaVij_biascorrected), - // dfV/dPi - dfVdPi, - // dfR/dRi - D_fR_fRrot * (-Rj.between(Ri).matrix() - fRrot.inverse().matrix() * D_coriolis), - // dfR/dPi - Z_3x3; } - if (H2) { - (*H2) << - // dfP/dVi - -Ri_transpose_matrix * dt + Ritranspose_omegaCoriolisHat * dt2, // Coriolis term - we got rid of the 2 wrt ins paper - // dfV/dVi - -Ri_transpose_matrix + 2 * Ritranspose_omegaCoriolisHat * dt, // Coriolis term - // dfR/dVi - Z_3x3; - } - if (H3) { - (*H3) << - // dfP/dPosej - Z_3x3, Ri_transpose_matrix * Rj.matrix(), - // dfV/dPosej - Matrix::Zero(3, 6), - // dfR/dPosej - D_fR_fRrot, Z_3x3; - } - if (H4) { - (*H4) << - // dfP/dVj - Z_3x3, - // dfV/dVj - Ri_transpose_matrix, - // dfR/dVj - Z_3x3; - } - if (H5) { - // H5 by this point already contains 3*3 biascorrectedThetaRij derivative - const Matrix3 JbiasOmega = D_cDeltaRij_cOmega * D_cThetaRij_biasOmegaIncr; - (*H5) << - // dfP/dBias - -delPdelBiasAcc(), -delPdelBiasOmega(), - // dfV/dBias - -delVdelBiasAcc(), -delVdelBiasOmega(), - // dfR/dBias - Z_3x3, D_fR_fRrot * (-fRrot.inverse().matrix() * JbiasOmega); - } - Vector9 r; - r << fp, fv, fR; - return r; + + // Do update in one fell swoop + return make_pair(j_correctedAcc, j_correctedOmega); } -ImuBase::ImuBase() - : gravity_(Vector3(0.0, 0.0, 9.81)), - omegaCoriolis_(Vector3(0.0, 0.0, 0.0)), - body_P_sensor_(boost::none), - use2ndOrderCoriolis_(false) { +//------------------------------------------------------------------------------ +NavState PreintegrationBase::updatedDeltaXij(const Vector3& j_measuredAcc, + const Vector3& j_measuredOmega, const double dt, + OptionalJacobian<9, 9> D_updated_current, + OptionalJacobian<9, 3> D_updated_measuredAcc, + OptionalJacobian<9, 3> D_updated_measuredOmega) const { + + Vector3 j_correctedAcc, j_correctedOmega; + Matrix3 D_correctedAcc_measuredAcc, // + D_correctedAcc_measuredOmega, // + D_correctedOmega_measuredOmega; + bool needDerivs = D_updated_measuredAcc && D_updated_measuredOmega && p().body_P_sensor; + boost::tie(j_correctedAcc, j_correctedOmega) = + correctMeasurementsByBiasAndSensorPose(j_measuredAcc, j_measuredOmega, + (needDerivs ? &D_correctedAcc_measuredAcc : 0), + (needDerivs ? &D_correctedAcc_measuredOmega : 0), + (needDerivs ? &D_correctedOmega_measuredOmega : 0)); + // Do update in one fell swoop + Matrix93 D_updated_correctedAcc, D_updated_correctedOmega; + NavState updated = deltaXij_.update(j_correctedAcc, j_correctedOmega, dt, D_updated_current, + (needDerivs ? D_updated_correctedAcc : D_updated_measuredAcc), + (needDerivs ? D_updated_correctedOmega : D_updated_measuredOmega)); + if (needDerivs) { + *D_updated_measuredAcc = D_updated_correctedAcc * D_correctedAcc_measuredAcc; + *D_updated_measuredOmega = D_updated_correctedOmega * D_correctedOmega_measuredOmega; + if (!p().body_P_sensor->translation().vector().isZero()) { + *D_updated_measuredOmega += D_updated_correctedAcc * D_correctedAcc_measuredOmega; + } + } + return updated; } -ImuBase::ImuBase(const Vector3& gravity, const Vector3& omegaCoriolis, - boost::optional body_P_sensor, const bool use2ndOrderCoriolis) - : gravity_(gravity), - omegaCoriolis_(omegaCoriolis), - body_P_sensor_(body_P_sensor), - use2ndOrderCoriolis_(use2ndOrderCoriolis) { +//------------------------------------------------------------------------------ +void PreintegrationBase::update(const Vector3& j_measuredAcc, + const Vector3& j_measuredOmega, const double dt, + Matrix3* D_incrR_integratedOmega, Matrix9* D_updated_current, + Matrix93* D_updated_measuredAcc, Matrix93* D_updated_measuredOmega) { + + // Save current rotation for updating Jacobians + const Rot3 oldRij = deltaXij_.attitude(); + + // Do update + deltaTij_ += dt; + deltaXij_ = updatedDeltaXij(j_measuredAcc, j_measuredOmega, dt, + D_updated_current, D_updated_measuredAcc, D_updated_measuredOmega); // functional + + // Update Jacobians + // TODO(frank): we are repeating some computation here: accessible in F ? + Vector3 j_correctedAcc, j_correctedOmega; + boost::tie(j_correctedAcc, j_correctedOmega) = + correctMeasurementsByBiasAndSensorPose(j_measuredAcc, j_measuredOmega); + + Matrix3 D_acc_R; + oldRij.rotate(j_correctedAcc, D_acc_R); + const Matrix3 D_acc_biasOmega = D_acc_R * delRdelBiasOmega_; + + const Vector3 integratedOmega = j_correctedOmega * dt; + const Rot3 incrR = Rot3::Expmap(integratedOmega, D_incrR_integratedOmega); // expensive !! + const Matrix3 incrRt = incrR.transpose(); + delRdelBiasOmega_ = incrRt * delRdelBiasOmega_ + - *D_incrR_integratedOmega * dt; + + double dt22 = 0.5 * dt * dt; + const Matrix3 dRij = oldRij.matrix(); // expensive + delPdelBiasAcc_ += delVdelBiasAcc_ * dt - dt22 * dRij; + delPdelBiasOmega_ += dt * delVdelBiasOmega_ + dt22 * D_acc_biasOmega; + delVdelBiasAcc_ += -dRij * dt; + delVdelBiasOmega_ += D_acc_biasOmega * dt; } -} /// namespace gtsam +//------------------------------------------------------------------------------ +Vector9 PreintegrationBase::biasCorrectedDelta( + const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 6> H) const { + // Correct deltaRij, derivative is delRdelBiasOmega_ + const imuBias::ConstantBias biasIncr = bias_i - biasHat_; + Matrix3 D_correctedRij_bias; + const Vector3 biasInducedOmega = delRdelBiasOmega_ * biasIncr.gyroscope(); + const Rot3 correctedRij = deltaRij().expmap(biasInducedOmega, boost::none, + H ? &D_correctedRij_bias : 0); + if (H) + D_correctedRij_bias *= delRdelBiasOmega_; + + Vector9 xi; + Matrix3 D_dR_correctedRij; + // TODO(frank): could line below be simplified? It is equivalent to + // LogMap(deltaRij_.compose(Expmap(biasInducedOmega))) + NavState::dR(xi) = Rot3::Logmap(correctedRij, H ? &D_dR_correctedRij : 0); + NavState::dP(xi) = deltaPij() + delPdelBiasAcc_ * biasIncr.accelerometer() + + delPdelBiasOmega_ * biasIncr.gyroscope(); + NavState::dV(xi) = deltaVij() + delVdelBiasAcc_ * biasIncr.accelerometer() + + delVdelBiasOmega_ * biasIncr.gyroscope(); + + if (H) { + Matrix36 D_dR_bias, D_dP_bias, D_dV_bias; + D_dR_bias << Z_3x3, D_dR_correctedRij * D_correctedRij_bias; + D_dP_bias << delPdelBiasAcc_, delPdelBiasOmega_; + D_dV_bias << delVdelBiasAcc_, delVdelBiasOmega_; + (*H) << D_dR_bias, D_dP_bias, D_dV_bias; + } + return xi; +} + +//------------------------------------------------------------------------------ +NavState PreintegrationBase::predict(const NavState& state_i, + const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 9> H1, + OptionalJacobian<9, 6> H2) const { + // correct for bias + Matrix96 D_biasCorrected_bias; + Vector9 biasCorrected = biasCorrectedDelta(bias_i, + H2 ? &D_biasCorrected_bias : 0); + + // integrate on tangent space + Matrix9 D_delta_state, D_delta_biasCorrected; + Vector9 xi = state_i.correctPIM(biasCorrected, deltaTij_, p().n_gravity, + p().omegaCoriolis, p().use2ndOrderCoriolis, H1 ? &D_delta_state : 0, + H2 ? &D_delta_biasCorrected : 0); + + // Use retract to get back to NavState manifold + Matrix9 D_predict_state, D_predict_delta; + NavState state_j = state_i.retract(xi, D_predict_state, D_predict_delta); + if (H1) + *H1 = D_predict_state + D_predict_delta * D_delta_state; + if (H2) + *H2 = D_predict_delta * D_delta_biasCorrected * D_biasCorrected_bias; + return state_j; +} + +//------------------------------------------------------------------------------ +Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i, + const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j, + const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 6> H1, + OptionalJacobian<9, 3> H2, OptionalJacobian<9, 6> H3, + OptionalJacobian<9, 3> H4, OptionalJacobian<9, 6> H5) const { + + // Note that derivative of constructors below is not identity for velocity, but + // a 9*3 matrix == Z_3x3, Z_3x3, state.R().transpose() + NavState state_i(pose_i, vel_i); + NavState state_j(pose_j, vel_j); + + /// Predict state at time j + Matrix99 D_predict_state_i; + Matrix96 D_predict_bias_i; + NavState predictedState_j = predict(state_i, bias_i, + H1 || H2 ? &D_predict_state_i : 0, H5 ? &D_predict_bias_i : 0); + + Matrix9 D_error_state_j, D_error_predict; + Vector9 error = state_j.localCoordinates(predictedState_j, + H3 || H4 ? &D_error_state_j : 0, H1 || H2 || H5 ? &D_error_predict : 0); + + // Separate out derivatives in terms of 5 arguments + // Note that doing so requires special treatment of velocities, as when treated as + // separate variables the retract applied will not be the semi-direct product in NavState + // Instead, the velocities in nav are updated using a straight addition + // This is difference is accounted for by the R().transpose calls below + if (H1) + *H1 << D_error_predict * D_predict_state_i.leftCols<6>(); + if (H2) + *H2 + << D_error_predict * D_predict_state_i.rightCols<3>() + * state_i.R().transpose(); + if (H3) + *H3 << D_error_state_j.leftCols<6>(); + if (H4) + *H4 << D_error_state_j.rightCols<3>() * state_j.R().transpose(); + if (H5) + *H5 << D_error_predict * D_predict_bias_i; + + return error; +} + +//------------------------------------------------------------------------------ +PoseVelocityBias PreintegrationBase::predict(const Pose3& pose_i, + const Vector3& vel_i, const imuBias::ConstantBias& bias_i, + const Vector3& n_gravity, const Vector3& omegaCoriolis, + const bool use2ndOrderCoriolis) { + // NOTE(frank): parameters are supposed to be constant, below is only provided for compatibility + boost::shared_ptr q = boost::make_shared(p()); + q->n_gravity = n_gravity; + q->omegaCoriolis = omegaCoriolis; + q->use2ndOrderCoriolis = use2ndOrderCoriolis; + p_ = q; + return PoseVelocityBias(predict(NavState(pose_i, vel_i), bias_i), bias_i); +} + +//------------------------------------------------------------------------------ + +}/// namespace gtsam diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index f6b24e752..07225dd9a 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -22,25 +22,26 @@ #pragma once #include +#include #include -#include -#include -#include +#include namespace gtsam { -/** - * Struct to hold all state variables of returned by Predict function - */ +/// @deprecated struct PoseVelocityBias { Pose3 pose; Vector3 velocity; imuBias::ConstantBias bias; - - PoseVelocityBias(const Pose3& _pose, const Vector3& _velocity, const imuBias::ConstantBias _bias) - : pose(_pose), - velocity(_velocity), - bias(_bias) { + PoseVelocityBias(const Pose3& _pose, const Vector3& _velocity, + const imuBias::ConstantBias _bias) : + pose(_pose), velocity(_velocity), bias(_bias) { + } + PoseVelocityBias(const NavState& navState, const imuBias::ConstantBias _bias) : + pose(navState.pose()), velocity(navState.velocity()), bias(_bias) { + } + NavState navState() const { + return NavState(pose, velocity); } }; @@ -50,51 +51,126 @@ struct PoseVelocityBias { * It includes the definitions of the preintegrated variables and the methods * to access, print, and compare them. */ -class PreintegrationBase : public PreintegratedRotation { +class PreintegrationBase { - imuBias::ConstantBias biasHat_; ///< Acceleration and angular rate bias values used during preintegration - bool use2ndOrderIntegration_; ///< Controls the order of integration +public: - Vector3 deltaPij_; ///< Preintegrated relative position (does not take into account velocity at time i, see deltap+, in [2]) (in frame i) - Vector3 deltaVij_; ///< Preintegrated relative velocity (in global frame) + /// Parameters for pre-integration: + /// Usage: Create just a single Params and pass a shared pointer to the constructor + struct Params: PreintegratedRotation::Params { + Matrix3 accelerometerCovariance; ///< continuous-time "Covariance" of accelerometer + Matrix3 integrationCovariance; ///< continuous-time "Covariance" describing integration uncertainty + bool use2ndOrderCoriolis; ///< Whether to use second order Coriolis integration + Vector3 n_gravity; ///< Gravity vector in nav frame - Matrix3 delPdelBiasAcc_; ///< Jacobian of preintegrated position w.r.t. acceleration bias - Matrix3 delPdelBiasOmega_; ///< Jacobian of preintegrated position w.r.t. angular rate bias - Matrix3 delVdelBiasAcc_; ///< Jacobian of preintegrated velocity w.r.t. acceleration bias - Matrix3 delVdelBiasOmega_; ///< Jacobian of preintegrated velocity w.r.t. angular rate bias + /// The Params constructor insists on getting the navigation frame gravity vector + /// For convenience, two commonly used conventions are provided by named constructors below + Params(const Vector3& n_gravity) : + accelerometerCovariance(I_3x3), integrationCovariance(I_3x3), use2ndOrderCoriolis( + false), n_gravity(n_gravity) { + } - const Matrix3 accelerometerCovariance_; ///< continuous-time "Covariance" of accelerometer measurements - const Matrix3 integrationCovariance_; ///< continuous-time "Covariance" describing integration uncertainty - /// (to compensate errors in Euler integration) + // Default Params for a Z-down navigation frame, such as NED: gravity points along positive Z-axis + static boost::shared_ptr MakeSharedD(double g = 9.81) { + return boost::make_shared(Vector3(0, 0, g)); + } - public: + // Default Params for a Z-up navigation frame, such as ENU: gravity points along negative Z-axis + static boost::shared_ptr MakeSharedU(double g = 9.81) { + return boost::make_shared(Vector3(0, 0, -g)); + } + + void print(const std::string& s) const; + + protected: + /// Default constructor for serialization only: uninitialized! + Params(); + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegratedRotation::Params); + ar & BOOST_SERIALIZATION_NVP(accelerometerCovariance); + ar & BOOST_SERIALIZATION_NVP(integrationCovariance); + ar & BOOST_SERIALIZATION_NVP(use2ndOrderCoriolis); + ar & BOOST_SERIALIZATION_NVP(n_gravity); + } + }; + +protected: + + double deltaTij_; ///< Time interval from i to j /** - * Default constructor, initializes the variables in the base class - * @param bias Current estimate of acceleration and rotation rate biases - * @param use2ndOrderIntegration Controls the order of integration - * (if false: p(t+1) = p(t) + v(t) deltaT ; if true: p(t+1) = p(t) + v(t) deltaT + 0.5 * acc(t) deltaT^2) + * Preintegrated navigation state, from frame i to frame j + * Note: relative position does not take into account velocity at time i, see deltap+, in [2] + * Note: velocity is now also in frame i, as opposed to deltaVij in [2] */ - PreintegrationBase(const imuBias::ConstantBias& bias, const Matrix3& measuredAccCovariance, - const Matrix3& measuredOmegaCovariance, - const Matrix3&integrationErrorCovariance, const bool use2ndOrderIntegration); + NavState deltaXij_; - /// methods to access class variables - bool use2ndOrderIntegration() const { - return use2ndOrderIntegration_; + /// Parameters + boost::shared_ptr p_; + + /// Acceleration and gyro bias used for preintegration + imuBias::ConstantBias biasHat_; + + Matrix3 delRdelBiasOmega_; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias + Matrix3 delPdelBiasAcc_; ///< Jacobian of preintegrated position w.r.t. acceleration bias + Matrix3 delPdelBiasOmega_; ///< Jacobian of preintegrated position w.r.t. angular rate bias + Matrix3 delVdelBiasAcc_; ///< Jacobian of preintegrated velocity w.r.t. acceleration bias + Matrix3 delVdelBiasOmega_; ///< Jacobian of preintegrated velocity w.r.t. angular rate bias + + /// Default constructor for serialization + PreintegrationBase() { } - const Vector3& deltaPij() const { - return deltaPij_; + +public: + + /** + * Constructor, initializes the variables in the base class + * @param bias Current estimate of acceleration and rotation rate biases + * @param p Parameters, typically fixed in a single application + */ + PreintegrationBase(const boost::shared_ptr& p, + const imuBias::ConstantBias& biasHat) : + p_(p), biasHat_(biasHat) { + resetIntegration(); } - const Vector3& deltaVij() const { - return deltaVij_; + + /// Re-initialize PreintegratedMeasurements + void resetIntegration(); + + const Params& p() const { + return *boost::static_pointer_cast(p_); + } + + void set_body_P_sensor(const Pose3& body_P_sensor) { + p_->body_P_sensor = body_P_sensor; + } + + /// getters + const NavState& deltaXij() const { + return deltaXij_; + } + const double& deltaTij() const { + return deltaTij_; + } + const Rot3& deltaRij() const { + return deltaXij_.attitude(); + } + Vector3 deltaPij() const { + return deltaXij_.position().vector(); + } + Vector3 deltaVij() const { + return deltaXij_.velocity(); } const imuBias::ConstantBias& biasHat() const { return biasHat_; } - Vector6 biasHatVector() const { - return biasHat_.vector(); - } // expensive + const Matrix3& delRdelBiasOmega() const { + return delRdelBiasOmega_; + } const Matrix3& delPdelBiasAcc() const { return delPdelBiasAcc_; } @@ -108,11 +184,9 @@ class PreintegrationBase : public PreintegratedRotation { return delVdelBiasOmega_; } - const Matrix3& accelerometerCovariance() const { - return accelerometerCovariance_; - } - const Matrix3& integrationCovariance() const { - return integrationCovariance_; + // Exposed for MATLAB + Vector6 biasHatVector() const { + return biasHat_.vector(); } /// print @@ -121,50 +195,62 @@ class PreintegrationBase : public PreintegratedRotation { /// check equality bool equals(const PreintegrationBase& other, double tol) const; - /// Re-initialize PreintegratedMeasurements - void resetIntegration(); + /// Subtract estimate and correct for sensor pose + /// Compute the derivatives due to non-identity body_P_sensor (rotation and centrifugal acc) + /// Ignore D_correctedOmega_measuredAcc as it is trivially zero + std::pair correctMeasurementsByBiasAndSensorPose( + const Vector3& j_measuredAcc, const Vector3& j_measuredOmega, + OptionalJacobian<3, 3> D_correctedAcc_measuredAcc = boost::none, + OptionalJacobian<3, 3> D_correctedAcc_measuredOmega = boost::none, + OptionalJacobian<3, 3> D_correctedOmega_measuredOmega = boost::none) const; - /// Update preintegrated measurements - void updatePreintegratedMeasurements(const Vector3& correctedAcc, const Rot3& incrR, - const double deltaT, OptionalJacobian<9, 9> F); + /// Calculate the updated preintegrated measurement, does not modify + /// It takes measured quantities in the j frame + NavState updatedDeltaXij(const Vector3& j_measuredAcc, + const Vector3& j_measuredOmega, const double dt, + OptionalJacobian<9, 9> D_updated_current = boost::none, + OptionalJacobian<9, 3> D_updated_measuredAcc = boost::none, + OptionalJacobian<9, 3> D_updated_measuredOmega = boost::none) const; - /// Update Jacobians to be used during preintegration - void updatePreintegratedJacobians(const Vector3& correctedAcc, - const Matrix3& D_Rincr_integratedOmega, const Rot3& incrR, - double deltaT); + /// Update preintegrated measurements and get derivatives + /// It takes measured quantities in the j frame + void update(const Vector3& j_measuredAcc, const Vector3& j_measuredOmega, + const double deltaT, Matrix3* D_incrR_integratedOmega, Matrix9* D_updated_current, + Matrix93* D_udpated_measuredAcc, Matrix93* D_updated_measuredOmega); - void correctMeasurementsByBiasAndSensorPose(const Vector3& measuredAcc, - const Vector3& measuredOmega, Vector3& correctedAcc, - Vector3& correctedOmega, - boost::optional body_P_sensor); + /// Given the estimate of the bias, return a NavState tangent vector + /// summarizing the preintegrated IMU measurements so far + Vector9 biasCorrectedDelta(const imuBias::ConstantBias& bias_i, + OptionalJacobian<9, 6> H = boost::none) const; /// Predict state at time j - PoseVelocityBias predict( - const Pose3& pose_i, const Vector3& vel_i, const imuBias::ConstantBias& bias_i, - const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false, - boost::optional deltaPij_biascorrected_out = boost::none, - boost::optional deltaVij_biascorrected_out = boost::none) const; + NavState predict(const NavState& state_i, const imuBias::ConstantBias& bias_i, + OptionalJacobian<9, 9> H1 = boost::none, OptionalJacobian<9, 6> H2 = + boost::none) const; /// Compute errors w.r.t. preintegrated measurements and jacobians wrt pose_i, vel_i, bias_i, pose_j, bias_j - Vector9 computeErrorAndJacobians(const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, - const Vector3& vel_j, const imuBias::ConstantBias& bias_i, - const Vector3& gravity, const Vector3& omegaCoriolis, - const bool use2ndOrderCoriolis, OptionalJacobian<9, 6> H1 = - boost::none, - OptionalJacobian<9, 3> H2 = boost::none, - OptionalJacobian<9, 6> H3 = boost::none, - OptionalJacobian<9, 3> H4 = boost::none, - OptionalJacobian<9, 6> H5 = boost::none) const; + Vector9 computeErrorAndJacobians(const Pose3& pose_i, const Vector3& vel_i, + const Pose3& pose_j, const Vector3& vel_j, + const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 6> H1 = + boost::none, OptionalJacobian<9, 3> H2 = boost::none, + OptionalJacobian<9, 6> H3 = boost::none, OptionalJacobian<9, 3> H4 = + boost::none, OptionalJacobian<9, 6> H5 = boost::none) const; - private: + /// @deprecated predict + PoseVelocityBias predict(const Pose3& pose_i, const Vector3& vel_i, + const imuBias::ConstantBias& bias_i, const Vector3& n_gravity, + const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false); + +private: /** Serialization function */ friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegratedRotation); + ar & BOOST_SERIALIZATION_NVP(p_); + ar & BOOST_SERIALIZATION_NVP(deltaTij_); + ar & BOOST_SERIALIZATION_NVP(deltaXij_); ar & BOOST_SERIALIZATION_NVP(biasHat_); - ar & BOOST_SERIALIZATION_NVP(deltaPij_); - ar & BOOST_SERIALIZATION_NVP(deltaVij_); + ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega_); ar & BOOST_SERIALIZATION_NVP(delPdelBiasAcc_); ar & BOOST_SERIALIZATION_NVP(delPdelBiasOmega_); ar & BOOST_SERIALIZATION_NVP(delVdelBiasAcc_); @@ -172,32 +258,4 @@ class PreintegrationBase : public PreintegratedRotation { } }; -class ImuBase { - - protected: - - Vector3 gravity_; - Vector3 omegaCoriolis_; - boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame - bool use2ndOrderCoriolis_; ///< Controls whether higher order terms are included when calculating the Coriolis Effect - - public: - - /// Default constructor, with decent gravity and zero coriolis - ImuBase(); - - /// Fully fledge constructor - ImuBase(const Vector3& gravity, const Vector3& omegaCoriolis, - boost::optional body_P_sensor = boost::none, - const bool use2ndOrderCoriolis = false); - - const Vector3& gravity() const { - return gravity_; - } - const Vector3& omegaCoriolis() const { - return omegaCoriolis_; - } - -}; - -} /// namespace gtsam +} /// namespace gtsam diff --git a/gtsam/navigation/tests/testAHRSFactor.cpp b/gtsam/navigation/tests/testAHRSFactor.cpp index 928c0f74f..2121eda35 100644 --- a/gtsam/navigation/tests/testAHRSFactor.cpp +++ b/gtsam/navigation/tests/testAHRSFactor.cpp @@ -35,6 +35,12 @@ using symbol_shorthand::X; using symbol_shorthand::V; using symbol_shorthand::B; +Vector3 kZeroOmegaCoriolis(0,0,0); + +// Define covariance matrices +double accNoiseVar = 0.01; +const Matrix3 kMeasuredAccCovariance = accNoiseVar * I_3x3; + //****************************************************************************** namespace { Vector callEvaluateError(const AHRSFactor& factor, const Rot3 rot_i, @@ -50,8 +56,8 @@ Rot3 evaluateRotationError(const AHRSFactor& factor, const Rot3 rot_i, AHRSFactor::PreintegratedMeasurements evaluatePreintegratedMeasurements( const Vector3& bias, const list& measuredOmegas, const list& deltaTs, - const Vector3& initialRotationRate = Vector3()) { - AHRSFactor::PreintegratedMeasurements result(bias, Matrix3::Identity()); + const Vector3& initialRotationRate = Vector3::Zero()) { + AHRSFactor::PreintegratedMeasurements result(bias, I_3x3); list::const_iterator itOmega = measuredOmegas.begin(); list::const_iterator itDeltaT = deltaTs.begin(); @@ -95,7 +101,7 @@ TEST( AHRSFactor, PreintegratedMeasurements ) { double expectedDeltaT1(0.5); // Actual preintegrated values - AHRSFactor::PreintegratedMeasurements actual1(bias, Matrix3::Zero()); + AHRSFactor::PreintegratedMeasurements actual1(bias, Z_3x3); actual1.integrateMeasurement(measuredOmega, deltaT); EXPECT(assert_equal(expectedDeltaR1, Rot3(actual1.deltaRij()), 1e-6)); @@ -121,18 +127,14 @@ TEST(AHRSFactor, Error) { Rot3 x2(Rot3::RzRyRx(M_PI / 12.0 + M_PI / 100.0, M_PI / 6.0, M_PI / 4.0)); // Measurements - Vector3 gravity; - gravity << 0, 0, 9.81; - Vector3 omegaCoriolis; - omegaCoriolis << 0, 0, 0; Vector3 measuredOmega; measuredOmega << M_PI / 100, 0, 0; double deltaT = 1.0; - AHRSFactor::PreintegratedMeasurements pre_int_data(bias, Matrix3::Zero()); - pre_int_data.integrateMeasurement(measuredOmega, deltaT); + AHRSFactor::PreintegratedMeasurements pim(bias, Z_3x3); + pim.integrateMeasurement(measuredOmega, deltaT); // Create factor - AHRSFactor factor(X(1), X(2), B(1), pre_int_data, omegaCoriolis, boost::none); + AHRSFactor factor(X(1), X(2), B(1), pim, kZeroOmegaCoriolis, boost::none); Vector3 errorActual = factor.evaluateError(x1, x2, bias); @@ -182,18 +184,16 @@ TEST(AHRSFactor, ErrorWithBiases) { Rot3 x2(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0 + M_PI / 10.0))); // Measurements - Vector3 omegaCoriolis; - omegaCoriolis << 0, 0.0, 0.0; Vector3 measuredOmega; measuredOmega << 0, 0, M_PI / 10.0 + 0.3; double deltaT = 1.0; - AHRSFactor::PreintegratedMeasurements pre_int_data(Vector3(0,0,0), - Matrix3::Zero()); - pre_int_data.integrateMeasurement(measuredOmega, deltaT); + AHRSFactor::PreintegratedMeasurements pim(Vector3(0,0,0), + Z_3x3); + pim.integrateMeasurement(measuredOmega, deltaT); // Create factor - AHRSFactor factor(X(1), X(2), B(1), pre_int_data, omegaCoriolis); + AHRSFactor factor(X(1), X(2), B(1), pim, kZeroOmegaCoriolis); Vector errorActual = factor.evaluateError(x1, x2, bias); @@ -269,7 +269,7 @@ TEST( AHRSFactor, PartialDerivativeLogmap ) { const Vector3 x = thetahat; // parametrization of so(3) const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^ double normx = norm_2(x); - const Matrix3 actualDelFdeltheta = Matrix3::Identity() + 0.5 * X + const Matrix3 actualDelFdeltheta = I_3x3 + 0.5 * X + (1 / (normx * normx) - (1 + cos(normx)) / (2 * normx * sin(normx))) * X * X; @@ -359,8 +359,6 @@ TEST( AHRSFactor, ErrorWithBiasesAndSensorBodyDisplacement ) { Rot3 x2(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0 + M_PI / 10.0))); // Measurements - Vector3 gravity; - gravity << 0, 0, 9.81; Vector3 omegaCoriolis; omegaCoriolis << 0, 0.1, 0.1; Vector3 measuredOmega; @@ -370,13 +368,15 @@ TEST( AHRSFactor, ErrorWithBiasesAndSensorBodyDisplacement ) { const Pose3 body_P_sensor(Rot3::Expmap(Vector3(0, 0.10, 0.10)), Point3(1, 0, 0)); - AHRSFactor::PreintegratedMeasurements pre_int_data(Vector3::Zero(), - Matrix3::Zero()); + AHRSFactor::PreintegratedMeasurements pim(Vector3::Zero(), kMeasuredAccCovariance); - pre_int_data.integrateMeasurement(measuredOmega, deltaT); + pim.integrateMeasurement(measuredOmega, deltaT); + + // Check preintegrated covariance + EXPECT(assert_equal(kMeasuredAccCovariance, pim.preintMeasCov())); // Create factor - AHRSFactor factor(X(1), X(2), B(1), pre_int_data, omegaCoriolis); + AHRSFactor factor(X(1), X(2), B(1), pim, omegaCoriolis); // Expected Jacobians Matrix H1e = numericalDerivative11( @@ -407,34 +407,35 @@ TEST (AHRSFactor, predictTest) { Vector3 bias(0,0,0); // Measurements - Vector3 gravity; - gravity << 0, 0, 9.81; - Vector3 omegaCoriolis; - omegaCoriolis << 0, 0.0, 0.0; Vector3 measuredOmega; measuredOmega << 0, 0, M_PI / 10.0; - double deltaT = 0.001; - AHRSFactor::PreintegratedMeasurements pre_int_data(bias, Matrix3::Zero()); + double deltaT = 0.2; + AHRSFactor::PreintegratedMeasurements pim(bias, kMeasuredAccCovariance); for (int i = 0; i < 1000; ++i) { - pre_int_data.integrateMeasurement(measuredOmega, deltaT); + pim.integrateMeasurement(measuredOmega, deltaT); } - AHRSFactor factor(X(1), X(2), B(1), pre_int_data, omegaCoriolis); + // Check preintegrated covariance + Matrix expectedMeasCov(3,3); + expectedMeasCov = 200*kMeasuredAccCovariance; + EXPECT(assert_equal(expectedMeasCov, pim.preintMeasCov())); + + AHRSFactor factor(X(1), X(2), B(1), pim, kZeroOmegaCoriolis); // Predict Rot3 x; - Rot3 expectedRot = Rot3().ypr(M_PI / 10, 0, 0); - Rot3 actualRot = factor.predict(x, bias, pre_int_data, omegaCoriolis); + Rot3 expectedRot = Rot3().ypr(20*M_PI, 0, 0); + Rot3 actualRot = factor.predict(x, bias, pim, kZeroOmegaCoriolis); EXPECT(assert_equal(expectedRot, actualRot, 1e-6)); // AHRSFactor::PreintegratedMeasurements::predict Matrix expectedH = numericalDerivative11( boost::bind(&AHRSFactor::PreintegratedMeasurements::predict, - &pre_int_data, _1, boost::none), bias); + &pim, _1, boost::none), bias); // Actual Jacobians Matrix H; - (void) pre_int_data.predict(bias,H); - EXPECT(assert_equal(expectedH, H)); + (void) pim.predict(bias,H); + EXPECT(assert_equal(expectedH, H, 1e-8)); } //****************************************************************************** #include @@ -448,12 +449,7 @@ TEST (AHRSFactor, graphTest) { // PreIntegrator Vector3 biasHat(0, 0, 0); - Vector3 gravity; - gravity << 0, 0, 9.81; - Vector3 omegaCoriolis; - omegaCoriolis << 0, 0, 0; - AHRSFactor::PreintegratedMeasurements pre_int_data(biasHat, - Matrix3::Identity()); + AHRSFactor::PreintegratedMeasurements pim(biasHat, kMeasuredAccCovariance); // Pre-integrate measurements Vector3 measuredOmega(0, M_PI / 20, 0); @@ -461,15 +457,15 @@ TEST (AHRSFactor, graphTest) { // Create Factor noiseModel::Base::shared_ptr model = // - noiseModel::Gaussian::Covariance(pre_int_data.preintMeasCov()); + noiseModel::Gaussian::Covariance(pim.preintMeasCov()); NonlinearFactorGraph graph; Values values; for (size_t i = 0; i < 5; ++i) { - pre_int_data.integrateMeasurement(measuredOmega, deltaT); + pim.integrateMeasurement(measuredOmega, deltaT); } - // pre_int_data.print("Pre integrated measurementes"); - AHRSFactor factor(X(1), X(2), B(1), pre_int_data, omegaCoriolis); + // pim.print("Pre integrated measurementes"); + AHRSFactor factor(X(1), X(2), B(1), pim, kZeroOmegaCoriolis); values.insert(X(1), x1); values.insert(X(2), x2); values.insert(B(1), bias); diff --git a/gtsam/navigation/tests/testCombinedImuFactor.cpp b/gtsam/navigation/tests/testCombinedImuFactor.cpp index 9fb0f939b..d473207da 100644 --- a/gtsam/navigation/tests/testCombinedImuFactor.cpp +++ b/gtsam/navigation/tests/testCombinedImuFactor.cpp @@ -40,46 +40,6 @@ using symbol_shorthand::V; using symbol_shorthand::B; namespace { -/* ************************************************************************* */ -// Auxiliary functions to test Jacobians F and G used for -// covariance propagation during preintegration -/* ************************************************************************* */ -Vector updatePreintegratedMeasurementsTest(const Vector3 deltaPij_old, - const Vector3& deltaVij_old, const Rot3& deltaRij_old, - const imuBias::ConstantBias& bias_old, const Vector3& correctedAcc, - const Vector3& correctedOmega, const double deltaT, - const bool use2ndOrderIntegration) { - - Matrix3 dRij = deltaRij_old.matrix(); - Vector3 temp = dRij * (correctedAcc - bias_old.accelerometer()) * deltaT; - Vector3 deltaPij_new; - if (!use2ndOrderIntegration) { - deltaPij_new = deltaPij_old + deltaVij_old * deltaT; - } else { - deltaPij_new = deltaPij_old + deltaVij_old * deltaT + 0.5 * temp * deltaT; - } - Vector3 deltaVij_new = deltaVij_old + temp; - Rot3 deltaRij_new = deltaRij_old - * Rot3::Expmap((correctedOmega - bias_old.gyroscope()) * deltaT); - Vector3 logDeltaRij_new = Rot3::Logmap(deltaRij_new); // not important any more - imuBias::ConstantBias bias_new(bias_old); - Vector result(15); - result << deltaPij_new, deltaVij_new, logDeltaRij_new, bias_new.vector(); - return result; -} - -Rot3 updatePreintegratedMeasurementsRot(const Vector3 deltaPij_old, - const Vector3& deltaVij_old, const Rot3& deltaRij_old, - const imuBias::ConstantBias& bias_old, const Vector3& correctedAcc, - const Vector3& correctedOmega, const double deltaT, - const bool use2ndOrderIntegration) { - - Vector result = updatePreintegratedMeasurementsTest(deltaPij_old, - deltaVij_old, deltaRij_old, bias_old, correctedAcc, correctedOmega, - deltaT, use2ndOrderIntegration); - - return Rot3::Expmap(result.segment<3>(6)); -} // Auxiliary functions to test preintegrated Jacobians // delPdelBiasAcc_ delPdelBiasOmega_ delVdelBiasAcc_ delVdelBiasOmega_ delRdelBiasOmega_ @@ -87,9 +47,8 @@ Rot3 updatePreintegratedMeasurementsRot(const Vector3 deltaPij_old, CombinedImuFactor::CombinedPreintegratedMeasurements evaluatePreintegratedMeasurements( const imuBias::ConstantBias& bias, const list& measuredAccs, const list& measuredOmegas, const list& deltaTs) { - CombinedImuFactor::CombinedPreintegratedMeasurements result(bias, - Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), - Matrix3::Identity(), Matrix3::Identity(), Matrix::Identity(6, 6), false); + CombinedImuFactor::CombinedPreintegratedMeasurements result(bias, I_3x3, + I_3x3, I_3x3, I_3x3, I_3x3, I_6x6); list::const_iterator itAcc = measuredAccs.begin(); list::const_iterator itOmega = measuredOmegas.begin(); @@ -136,25 +95,17 @@ TEST( CombinedImuFactor, PreintegratedMeasurements ) { double tol = 1e-6; // Actual preintegrated values - ImuFactor::PreintegratedMeasurements expected1(bias, Matrix3::Zero(), - Matrix3::Zero(), Matrix3::Zero()); + ImuFactor::PreintegratedMeasurements expected1(bias, Z_3x3, Z_3x3, Z_3x3); expected1.integrateMeasurement(measuredAcc, measuredOmega, deltaT); - CombinedImuFactor::CombinedPreintegratedMeasurements actual1(bias, - Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), - Matrix3::Zero(), Matrix::Zero(6, 6)); + CombinedImuFactor::CombinedPreintegratedMeasurements actual1(bias, Z_3x3, + Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_6x6); actual1.integrateMeasurement(measuredAcc, measuredOmega, deltaT); - EXPECT( - assert_equal(Vector(expected1.deltaPij()), Vector(actual1.deltaPij()), - tol)); - EXPECT( - assert_equal(Vector(expected1.deltaVij()), Vector(actual1.deltaVij()), - tol)); - EXPECT( - assert_equal(Matrix(expected1.deltaRij()), Matrix(actual1.deltaRij()), - tol)); + EXPECT(assert_equal(Vector(expected1.deltaPij()), actual1.deltaPij(), tol)); + EXPECT(assert_equal(Vector(expected1.deltaVij()), actual1.deltaVij(), tol)); + EXPECT(assert_equal(expected1.deltaRij(), actual1.deltaRij(), tol)); DOUBLES_EQUAL(expected1.deltaTij(), actual1.deltaTij(), tol); } @@ -180,46 +131,39 @@ TEST( CombinedImuFactor, ErrorWithBiases ) { double deltaT = 1.0; double tol = 1e-6; - Matrix I6x6(6, 6); - I6x6 = Matrix::Identity(6, 6); - - ImuFactor::PreintegratedMeasurements pre_int_data( + ImuFactor::PreintegratedMeasurements pim( imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), - Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity()); + I_3x3, I_3x3, I_3x3); - pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); - CombinedImuFactor::CombinedPreintegratedMeasurements Combined_pre_int_data( + CombinedImuFactor::CombinedPreintegratedMeasurements combined_pim( 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); + I_3x3, I_3x3, I_3x3, I_3x3, 2 * I_3x3, I_6x6); - Combined_pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, - deltaT); + combined_pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); // Create factor - ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, + ImuFactor imuFactor(X(1), V(1), X(2), V(2), B(1), pim, gravity, omegaCoriolis); noiseModel::Gaussian::shared_ptr Combinedmodel = - noiseModel::Gaussian::Covariance(Combined_pre_int_data.preintMeasCov()); - CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), - Combined_pre_int_data, gravity, omegaCoriolis); + noiseModel::Gaussian::Covariance(combined_pim.preintMeasCov()); + CombinedImuFactor combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), + combined_pim, gravity, omegaCoriolis); - Vector errorExpected = factor.evaluateError(x1, v1, x2, v2, bias); - - Vector errorActual = Combinedfactor.evaluateError(x1, v1, x2, v2, bias, + Vector errorExpected = imuFactor.evaluateError(x1, v1, x2, v2, bias); + Vector errorActual = combinedfactor.evaluateError(x1, v1, x2, v2, bias, bias2); - EXPECT(assert_equal(errorExpected, errorActual.head(9), tol)); // Expected Jacobians Matrix H1e, H2e, H3e, H4e, H5e; - (void) factor.evaluateError(x1, v1, x2, v2, bias, H1e, H2e, H3e, H4e, H5e); + (void) imuFactor.evaluateError(x1, v1, x2, v2, bias, H1e, H2e, H3e, H4e, H5e); // Actual Jacobians Matrix H1a, H2a, H3a, H4a, H5a, H6a; - (void) Combinedfactor.evaluateError(x1, v1, x2, v2, bias, bias2, H1a, H2a, + (void) combinedfactor.evaluateError(x1, v1, x2, v2, bias, bias2, H1a, H2a, H3a, H4a, H5a, H6a); EXPECT(assert_equal(H1e, H1a.topRows(9))); @@ -253,7 +197,7 @@ TEST( CombinedImuFactor, FirstOrderPreIntegratedMeasurements ) { } // Actual preintegrated values - CombinedImuFactor::CombinedPreintegratedMeasurements preintegrated = + CombinedImuFactor::CombinedPreintegratedMeasurements pim = evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs); @@ -280,16 +224,12 @@ TEST( CombinedImuFactor, FirstOrderPreIntegratedMeasurements ) { Matrix expectedDelRdelBiasOmega = expectedDelRdelBias.rightCols(3); // Compare Jacobians - EXPECT(assert_equal(expectedDelPdelBiasAcc, preintegrated.delPdelBiasAcc())); - EXPECT( - assert_equal(expectedDelPdelBiasOmega, preintegrated.delPdelBiasOmega())); - EXPECT(assert_equal(expectedDelVdelBiasAcc, preintegrated.delVdelBiasAcc())); - EXPECT( - assert_equal(expectedDelVdelBiasOmega, preintegrated.delVdelBiasOmega())); + EXPECT(assert_equal(expectedDelPdelBiasAcc, pim.delPdelBiasAcc())); + EXPECT(assert_equal(expectedDelPdelBiasOmega, pim.delPdelBiasOmega())); + EXPECT(assert_equal(expectedDelVdelBiasAcc, pim.delVdelBiasAcc())); + EXPECT(assert_equal(expectedDelVdelBiasOmega, pim.delVdelBiasOmega())); EXPECT(assert_equal(expectedDelRdelBiasAcc, Matrix::Zero(3, 3))); - EXPECT( - assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega(), - 1e-3)); // 1e-3 needs to be added only when using quaternions for rotations + EXPECT(assert_equal(expectedDelRdelBiasOmega, pim.delRdelBiasOmega(), 1e-3)); // 1e-3 needs to be added only when using quaternions for rotations } /* ************************************************************************* */ @@ -307,28 +247,23 @@ TEST(CombinedImuFactor, PredictPositionAndVelocity) { measuredAcc << 0, 1.1, -9.81; double deltaT = 0.001; - Matrix I6x6(6, 6); - I6x6 = Matrix::Identity(6, 6); - - CombinedImuFactor::CombinedPreintegratedMeasurements Combined_pre_int_data( - bias, Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), - Matrix3::Identity(), 2 * Matrix3::Identity(), I6x6, true); + CombinedImuFactor::CombinedPreintegratedMeasurements pim(bias, I_3x3, I_3x3, + I_3x3, I_3x3, 2 * I_3x3, I_6x6); for (int i = 0; i < 1000; ++i) - Combined_pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, - deltaT); + pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); // Create factor - noiseModel::Gaussian::shared_ptr Combinedmodel = - noiseModel::Gaussian::Covariance(Combined_pre_int_data.preintMeasCov()); - CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), - Combined_pre_int_data, gravity, omegaCoriolis); + noiseModel::Gaussian::shared_ptr combinedmodel = + noiseModel::Gaussian::Covariance(pim.preintMeasCov()); + CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), pim, + gravity, omegaCoriolis); // Predict Pose3 x1; Vector3 v1(0, 0.0, 0.0); - PoseVelocityBias poseVelocityBias = Combined_pre_int_data.predict(x1, v1, - bias, gravity, omegaCoriolis); + PoseVelocityBias poseVelocityBias = pim.predict(x1, v1, bias, gravity, + omegaCoriolis); Pose3 expectedPose(Rot3(), Point3(0, 0.5, 0)); Vector3 expectedVelocity; expectedVelocity << 0, 1, 0; @@ -340,10 +275,8 @@ TEST(CombinedImuFactor, PredictPositionAndVelocity) { /* ************************************************************************* */ TEST(CombinedImuFactor, PredictRotation) { imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot) - Matrix I6x6(6, 6); - CombinedImuFactor::CombinedPreintegratedMeasurements Combined_pre_int_data( - bias, Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), - Matrix3::Identity(), 2 * Matrix3::Identity(), I6x6, true); + CombinedImuFactor::CombinedPreintegratedMeasurements pim(bias, I_3x3, I_3x3, + I_3x3, I_3x3, 2 * I_3x3, I_6x6); Vector3 measuredAcc; measuredAcc << 0, 0, -9.81; Vector3 gravity; @@ -355,175 +288,18 @@ TEST(CombinedImuFactor, PredictRotation) { double deltaT = 0.001; double tol = 1e-4; for (int i = 0; i < 1000; ++i) - Combined_pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, - deltaT); - CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), - Combined_pre_int_data, gravity, omegaCoriolis); + pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), pim, + gravity, omegaCoriolis); // Predict Pose3 x(Rot3().ypr(0, 0, 0), Point3(0, 0, 0)), x2; Vector3 v(0, 0, 0), v2; - CombinedImuFactor::Predict(x, v, x2, v2, bias, - Combinedfactor.preintegratedMeasurements(), gravity, omegaCoriolis); + CombinedImuFactor::Predict(x, v, x2, v2, bias, pim, gravity, omegaCoriolis); Pose3 expectedPose(Rot3().ypr(M_PI / 10, 0, 0), Point3(0, 0, 0)); EXPECT(assert_equal(expectedPose, x2, tol)); } -/* ************************************************************************* */ -TEST( CombinedImuFactor, JacobianPreintegratedCovariancePropagation ) { - // Linearization point - imuBias::ConstantBias bias_old = imuBias::ConstantBias(); ///< Current estimate of acceleration and rotation rate biases - Pose3 body_P_sensor = Pose3(); - - // Measurements - list measuredAccs, measuredOmegas; - list deltaTs; - measuredAccs.push_back(Vector3(0.1, 0.0, 0.0)); - measuredOmegas.push_back(Vector3(M_PI / 100.0, 0.0, 0.0)); - deltaTs.push_back(0.01); - measuredAccs.push_back(Vector3(0.1, 0.0, 0.0)); - measuredOmegas.push_back(Vector3(M_PI / 100.0, 0.0, 0.0)); - deltaTs.push_back(0.01); - for (int i = 1; i < 100; i++) { - measuredAccs.push_back(Vector3(0.05, 0.09, 0.01)); - measuredOmegas.push_back( - Vector3(M_PI / 100.0, M_PI / 300.0, 2 * M_PI / 100.0)); - deltaTs.push_back(0.01); - } - // Actual preintegrated values - CombinedImuFactor::CombinedPreintegratedMeasurements preintegrated = - evaluatePreintegratedMeasurements(bias_old, measuredAccs, measuredOmegas, - deltaTs); - - // so far we only created a nontrivial linearization point for the preintegrated measurements - // Now we add a new measurement and ask for Jacobians - const Vector3 newMeasuredAcc = Vector3(0.1, 0.0, 0.0); - const Vector3 newMeasuredOmega = Vector3(M_PI / 100.0, 0.0, 0.0); - const double newDeltaT = 0.01; - const Rot3 deltaRij_old = preintegrated.deltaRij(); // before adding new measurement - const Vector3 deltaVij_old = preintegrated.deltaVij(); // before adding new measurement - const Vector3 deltaPij_old = preintegrated.deltaPij(); // before adding new measurement - - Matrix oldPreintCovariance = preintegrated.preintMeasCov(); - - Matrix Factual, Gactual; - preintegrated.integrateMeasurement(newMeasuredAcc, newMeasuredOmega, - newDeltaT, body_P_sensor, Factual, Gactual); - - bool use2ndOrderIntegration = false; - - ////////////////////////////////////////////////////////////////////////////////////////////// - // COMPUTE NUMERICAL DERIVATIVES FOR F - ////////////////////////////////////////////////////////////////////////////////////////////// - // Compute expected F wrt positions (15,3) - Matrix df_dpos = numericalDerivative11( - boost::bind(&updatePreintegratedMeasurementsTest, _1, deltaVij_old, - deltaRij_old, bias_old, newMeasuredAcc, newMeasuredOmega, newDeltaT, - use2ndOrderIntegration), deltaPij_old); - // rotation part has to be done properly, on manifold - df_dpos.block<3, 3>(6, 0) = numericalDerivative11( - boost::bind(&updatePreintegratedMeasurementsRot, _1, deltaVij_old, - deltaRij_old, bias_old, newMeasuredAcc, newMeasuredOmega, newDeltaT, - use2ndOrderIntegration), deltaPij_old); - - // Compute expected F wrt velocities (15,3) - Matrix df_dvel = numericalDerivative11( - boost::bind(&updatePreintegratedMeasurementsTest, deltaPij_old, _1, - deltaRij_old, bias_old, newMeasuredAcc, newMeasuredOmega, newDeltaT, - use2ndOrderIntegration), deltaVij_old); - // rotation part has to be done properly, on manifold - df_dvel.block<3, 3>(6, 0) = numericalDerivative11( - boost::bind(&updatePreintegratedMeasurementsRot, deltaPij_old, _1, - deltaRij_old, bias_old, newMeasuredAcc, newMeasuredOmega, newDeltaT, - use2ndOrderIntegration), deltaVij_old); - - // Compute expected F wrt angles (15,3) - Matrix df_dangle = numericalDerivative11( - boost::bind(&updatePreintegratedMeasurementsTest, deltaPij_old, - deltaVij_old, _1, bias_old, newMeasuredAcc, newMeasuredOmega, - newDeltaT, use2ndOrderIntegration), deltaRij_old); - // rotation part has to be done properly, on manifold - df_dangle.block<3, 3>(6, 0) = numericalDerivative11( - boost::bind(&updatePreintegratedMeasurementsRot, deltaPij_old, - deltaVij_old, _1, bias_old, newMeasuredAcc, newMeasuredOmega, - newDeltaT, use2ndOrderIntegration), deltaRij_old); - - // Compute expected F wrt biases (15,6) - Matrix df_dbias = numericalDerivative11( - boost::bind(&updatePreintegratedMeasurementsTest, deltaPij_old, - deltaVij_old, deltaRij_old, _1, newMeasuredAcc, newMeasuredOmega, - newDeltaT, use2ndOrderIntegration), bias_old); - // rotation part has to be done properly, on manifold - df_dbias.block<3, 6>(6, 0) = - numericalDerivative11( - boost::bind(&updatePreintegratedMeasurementsRot, deltaPij_old, - deltaVij_old, deltaRij_old, _1, newMeasuredAcc, newMeasuredOmega, - newDeltaT, use2ndOrderIntegration), bias_old); - - Matrix Fexpected(15, 15); - Fexpected << df_dpos, df_dvel, df_dangle, df_dbias; - EXPECT(assert_equal(Fexpected, Factual)); - - ////////////////////////////////////////////////////////////////////////////////////////////// - // COMPUTE NUMERICAL DERIVATIVES FOR G - ////////////////////////////////////////////////////////////////////////////////////////////// - // Compute expected G wrt integration noise - Matrix df_dintNoise(15, 3); - df_dintNoise << I_3x3 * newDeltaT, Z_3x3, Z_3x3, Z_3x3, Z_3x3; - - // Compute expected G wrt acc noise (15,3) - Matrix df_daccNoise = numericalDerivative11( - boost::bind(&updatePreintegratedMeasurementsTest, deltaPij_old, - deltaVij_old, deltaRij_old, bias_old, _1, newMeasuredOmega, newDeltaT, - use2ndOrderIntegration), newMeasuredAcc); - // rotation part has to be done properly, on manifold - df_daccNoise.block<3, 3>(6, 0) = numericalDerivative11( - boost::bind(&updatePreintegratedMeasurementsRot, deltaPij_old, - deltaVij_old, deltaRij_old, bias_old, _1, newMeasuredOmega, newDeltaT, - use2ndOrderIntegration), newMeasuredAcc); - - // Compute expected G wrt gyro noise (15,3) - Matrix df_domegaNoise = numericalDerivative11( - boost::bind(&updatePreintegratedMeasurementsTest, deltaPij_old, - deltaVij_old, deltaRij_old, bias_old, newMeasuredAcc, _1, newDeltaT, - use2ndOrderIntegration), newMeasuredOmega); - // rotation part has to be done properly, on manifold - df_domegaNoise.block<3, 3>(6, 0) = numericalDerivative11( - boost::bind(&updatePreintegratedMeasurementsRot, deltaPij_old, - deltaVij_old, deltaRij_old, bias_old, newMeasuredAcc, _1, newDeltaT, - use2ndOrderIntegration), newMeasuredOmega); - - // Compute expected G wrt bias random walk noise (15,6) - Matrix df_rwBias(15, 6); // random walk on the bias does not appear in the first 9 entries - df_rwBias.setZero(); - df_rwBias.block<6, 6>(9, 0) = eye(6); - - // Compute expected G wrt gyro noise (15,6) - Matrix df_dinitBias = numericalDerivative11( - boost::bind(&updatePreintegratedMeasurementsTest, deltaPij_old, - deltaVij_old, deltaRij_old, _1, newMeasuredAcc, newMeasuredOmega, - newDeltaT, use2ndOrderIntegration), bias_old); - // rotation part has to be done properly, on manifold - df_dinitBias.block<3, 6>(6, 0) = numericalDerivative11( - boost::bind(&updatePreintegratedMeasurementsRot, deltaPij_old, - deltaVij_old, deltaRij_old, _1, newMeasuredAcc, newMeasuredOmega, - newDeltaT, use2ndOrderIntegration), bias_old); - df_dinitBias.block<6, 6>(9, 0) = Matrix::Zero(6, 6); // only has to influence first 9 rows - - Matrix Gexpected(15, 21); - Gexpected << df_dintNoise, df_daccNoise, df_domegaNoise, df_rwBias, df_dinitBias; - - EXPECT(assert_equal(Gexpected, Gactual)); - - // Check covariance propagation - Matrix newPreintCovarianceExpected = Factual * oldPreintCovariance - * Factual.transpose() + (1 / newDeltaT) * Gactual * Gactual.transpose(); - - Matrix newPreintCovarianceActual = preintegrated.preintMeasCov(); - EXPECT(assert_equal(newPreintCovarianceExpected, newPreintCovarianceActual)); -} - /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index c27921fc9..084213e20 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -12,21 +12,23 @@ /** * @file testImuFactor.cpp * @brief Unit test for ImuFactor - * @author Luca Carlone, Stephen Williams, Richard Roberts + * @author Luca Carlone, Stephen Williams, Richard Roberts, Frank Dellaert */ #include -#include -#include -#include #include #include +#include +#include +#include +#include #include #include -#include +#include #include #include +#include using namespace std; using namespace gtsam; @@ -36,50 +38,20 @@ using symbol_shorthand::X; using symbol_shorthand::V; using symbol_shorthand::B; +static const Vector3 kGravityAlongNavZDown(0, 0, 9.81); +static const Vector3 kZeroOmegaCoriolis(0, 0, 0); +static const Vector3 kNonZeroOmegaCoriolis(0, 0.1, 0.1); +static const imuBias::ConstantBias kZeroBiasHat, kZeroBias; + /* ************************************************************************* */ namespace { // Auxiliary functions to test evaluate error in ImuFactor /* ************************************************************************* */ -Vector callEvaluateError(const ImuFactor& factor, const Pose3& pose_i, - const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j, - const imuBias::ConstantBias& bias) { - return factor.evaluateError(pose_i, vel_i, pose_j, vel_j, bias); -} - Rot3 evaluateRotationError(const ImuFactor& factor, const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j, const imuBias::ConstantBias& bias) { return Rot3::Expmap( - factor.evaluateError(pose_i, vel_i, pose_j, vel_j, bias).tail(3)); -} - -// Auxiliary functions to test Jacobians F and G used for -// covariance propagation during preintegration -/* ************************************************************************* */ -Vector updatePreintegratedPosVel(const Vector3 deltaPij_old, - const Vector3& deltaVij_old, const Rot3& deltaRij_old, - const Vector3& correctedAcc, const Vector3& correctedOmega, - const double deltaT, const bool use2ndOrderIntegration_) { - - Matrix3 dRij = deltaRij_old.matrix(); - Vector3 temp = dRij * correctedAcc * deltaT; - Vector3 deltaPij_new; - if (!use2ndOrderIntegration_) { - deltaPij_new = deltaPij_old + deltaVij_old * deltaT; - } else { - deltaPij_new = deltaPij_old + deltaVij_old * deltaT + 0.5 * temp * deltaT; - } - Vector3 deltaVij_new = deltaVij_old + temp; - - Vector result(6); - result << deltaPij_new, deltaVij_new; - return result; -} - -Rot3 updatePreintegratedRot(const Rot3& deltaRij_old, - const Vector3& correctedOmega, const double deltaT) { - Rot3 deltaRij_new = deltaRij_old * Rot3::Expmap(correctedOmega * deltaT); - return deltaRij_new; + factor.evaluateError(pose_i, vel_i, pose_j, vel_j, bias).head(3)); } // Define covariance matrices @@ -87,22 +59,21 @@ Rot3 updatePreintegratedRot(const Rot3& deltaRij_old, double accNoiseVar = 0.01; double omegaNoiseVar = 0.03; double intNoiseVar = 0.0001; -const Matrix3 kMeasuredAccCovariance = accNoiseVar * Matrix3::Identity(); -const Matrix3 kMeasuredOmegaCovariance = omegaNoiseVar * Matrix3::Identity(); -const Matrix3 kIntegrationErrorCovariance = intNoiseVar * Matrix3::Identity(); +const Matrix3 kMeasuredAccCovariance = accNoiseVar * I_3x3; +const Matrix3 kMeasuredOmegaCovariance = omegaNoiseVar * I_3x3; +const Matrix3 kIntegrationErrorCovariance = intNoiseVar * I_3x3; +const Vector3 accNoiseVar2(0.01, 0.02, 0.03); +const Vector3 omegaNoiseVar2(0.03, 0.01, 0.02); +int32_t accSamplerSeed = 29284, omegaSamplerSeed = 10; // Auxiliary functions to test preintegrated Jacobians // delPdelBiasAcc_ delPdelBiasOmega_ delVdelBiasAcc_ delVdelBiasOmega_ delRdelBiasOmega_ /* ************************************************************************* */ -ImuFactor::PreintegratedMeasurements evaluatePreintegratedMeasurements( +PreintegratedImuMeasurements evaluatePreintegratedMeasurements( const imuBias::ConstantBias& bias, const list& measuredAccs, - const list& measuredOmegas, const list& deltaTs, - const bool use2ndOrderIntegration = false) { - ImuFactor::PreintegratedMeasurements result(bias, - kMeasuredAccCovariance, - kMeasuredOmegaCovariance, - kIntegrationErrorCovariance, - use2ndOrderIntegration); + const list& measuredOmegas, const list& deltaTs) { + PreintegratedImuMeasurements result(bias, kMeasuredAccCovariance, + kMeasuredOmegaCovariance, kIntegrationErrorCovariance); list::const_iterator itAcc = measuredAccs.begin(); list::const_iterator itOmega = measuredOmegas.begin(); @@ -144,13 +115,123 @@ Vector3 evaluateLogRotation(const Vector3 thetahat, const Vector3 deltatheta) { return Rot3::Logmap(Rot3::Expmap(thetahat).compose(Rot3::Expmap(deltatheta))); } +} // namespace + +/* ************************************************************************* */ +bool MonteCarlo(const PreintegratedImuMeasurements& pim, + const NavState& state, const imuBias::ConstantBias& bias, double dt, + boost::optional body_P_sensor, const Vector3& measuredAcc, + const Vector3& measuredOmega, const Vector3& accNoiseVar, + const Vector3& omegaNoiseVar, size_t N = 10000, + size_t M = 1) { + // Get mean prediction from "ground truth" measurements + PreintegratedImuMeasurements pim1 = pim; + for (size_t k = 0; k < M; k++) + pim1.integrateMeasurement(measuredAcc, measuredOmega, dt, body_P_sensor); + NavState prediction = pim1.predict(state, bias); + Matrix9 actual = pim1.preintMeasCov(); + + // Do a Monte Carlo analysis to determine empirical density on the predicted state + Sampler sampleAccelerationNoise((accNoiseVar/dt).cwiseSqrt(), accSamplerSeed); + Sampler sampleOmegaNoise((omegaNoiseVar/dt).cwiseSqrt(), omegaSamplerSeed); + Matrix samples(9, N); + Vector9 sum = Vector9::Zero(); + for (size_t i = 0; i < N; i++) { + PreintegratedImuMeasurements pim2 = pim; + for (size_t k = 0; k < M; k++) { + Vector3 perturbedAcc = measuredAcc + sampleAccelerationNoise.sample(); + Vector3 perturbedOmega = measuredOmega + sampleOmegaNoise.sample(); + pim2.integrateMeasurement(perturbedAcc, perturbedOmega, dt, + body_P_sensor); + } + NavState sampled = pim2.predict(state, bias); + Vector9 xi = sampled.localCoordinates(prediction); + samples.col(i) = xi; + sum += xi; + } + + Vector9 sampleMean = sum / N; + Matrix9 Q; + Q.setZero(); + for (size_t i = 0; i < N; i++) { + Vector9 xi = samples.col(i); + xi -= sampleMean; + Q += xi * (xi.transpose() / (N - 1)); + } + + // Compare MonteCarlo value with actual (computed) value + return assert_equal(Matrix(Q), actual, 1e-4); +} + +#if 1 + +/* ************************************************************************* */ +TEST(ImuFactor, StraightLine) { + // Set up IMU measurements + static const double g = 10; // make gravity 10 to make this easy to check + static const double v = 50.0, a = 0.2, dt = 3.0; + const double dt22 = dt * dt / 2; + + // Set up body pointing towards y axis, and start at 10,20,0 with velocity going in X + // The body itself has Z axis pointing down + static const Rot3 nRb(Point3(0, 1, 0), Point3(1, 0, 0), Point3(0, 0, -1)); + static const Point3 initial_position(10, 20, 0); + static const Vector3 initial_velocity(v, 0, 0); + static const NavState state1(nRb, initial_position, initial_velocity); + + // set up acceleration in X direction, no angular velocity. + // Since body Z-axis is pointing down, accelerometer measures table exerting force in negative Z + Vector3 measuredAcc(a, 0, -g), measuredOmega(0, 0, 0); + + // Create parameters assuming nav frame has Z up + boost::shared_ptr p = + PreintegratedImuMeasurements::Params::MakeSharedU(g); + p->accelerometerCovariance = kMeasuredAccCovariance; + p->gyroscopeCovariance = kMeasuredOmegaCovariance; + + // Check G1 and G2 derivatives of pim.update + + // Now, preintegrate for 3 seconds, in 10 steps + PreintegratedImuMeasurements pim(p, kZeroBiasHat); + for (size_t i = 0; i < 10; i++) + pim.integrateMeasurement(measuredAcc, measuredOmega, dt / 10); + + Matrix93 aG1,aG2; + boost::function f = + boost::bind(&PreintegrationBase::updatedDeltaXij, pim, _1, _2, dt/10, + boost::none, boost::none, boost::none); + pim.updatedDeltaXij(measuredAcc, measuredOmega, dt / 10, boost::none, aG1, aG2); + EXPECT(assert_equal(numericalDerivative21(f, measuredAcc, measuredOmega, 1e-7), aG1, 1e-7)); + EXPECT(assert_equal(numericalDerivative22(f, measuredAcc, measuredOmega, 1e-7), aG2, 1e-7)); + + // Do Monte-Carlo analysis + PreintegratedImuMeasurements pimMC(kZeroBiasHat, p->accelerometerCovariance, + p->gyroscopeCovariance, Z_3x3, true); // MonteCarlo does not sample integration noise + EXPECT( + MonteCarlo(pimMC, state1, kZeroBias, dt / 10, boost::none, measuredAcc, + measuredOmega, Vector3::Constant(accNoiseVar), Vector3::Constant(omegaNoiseVar), 100000)); + + // Check integrated quantities in body frame: gravity measured by IMU is integrated! + Vector3 b_deltaP(a * dt22, 0, -g * dt22); + Vector3 b_deltaV(a * dt, 0, -g * dt); + EXPECT(assert_equal(Rot3(), pim.deltaRij())); + EXPECT(assert_equal(b_deltaP, pim.deltaPij())); + EXPECT(assert_equal(b_deltaV, pim.deltaVij())); + + // Check bias-corrected delta: also in body frame + Vector9 expectedBC; + expectedBC << Vector3(0, 0, 0), b_deltaP, b_deltaV; + EXPECT(assert_equal(expectedBC, pim.biasCorrectedDelta(kZeroBias))); + + // Check prediction in nav, note we move along x in body, along y in nav + Point3 expected_position(10 + v * dt, 20 + a * dt22, 0); + Velocity3 expected_velocity(v, a * dt, 0); + NavState expected(nRb, expected_position, expected_velocity); + EXPECT(assert_equal(expected, pim.predict(state1, kZeroBias))); } /* ************************************************************************* */ -TEST( ImuFactor, PreintegratedMeasurements ) { - // Linearization point - imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); ///< Current estimate of acceleration and angular rate biases - +TEST(ImuFactor, PreintegratedMeasurements) { // Measurements Vector3 measuredAcc(0.1, 0.0, 0.0); Vector3 measuredOmega(M_PI / 100.0, 0.0, 0.0); @@ -163,21 +244,15 @@ TEST( ImuFactor, PreintegratedMeasurements ) { Rot3 expectedDeltaR1 = Rot3::RzRyRx(0.5 * M_PI / 100.0, 0.0, 0.0); double expectedDeltaT1(0.5); - bool use2ndOrderIntegration = true; // Actual preintegrated values - ImuFactor::PreintegratedMeasurements actual1(bias, - kMeasuredAccCovariance, - kMeasuredOmegaCovariance, - kIntegrationErrorCovariance, - use2ndOrderIntegration); + PreintegratedImuMeasurements actual1(kZeroBiasHat, kMeasuredAccCovariance, + kMeasuredOmegaCovariance, kIntegrationErrorCovariance); actual1.integrateMeasurement(measuredAcc, measuredOmega, deltaT); - EXPECT( - assert_equal(Vector(expectedDeltaP1), Vector(actual1.deltaPij()), 1e-6)); - EXPECT( - assert_equal(Vector(expectedDeltaV1), Vector(actual1.deltaVij()), 1e-6)); - EXPECT(assert_equal(expectedDeltaR1, Rot3(actual1.deltaRij()), 1e-6)); - DOUBLES_EQUAL(expectedDeltaT1, actual1.deltaTij(), 1e-6); + EXPECT(assert_equal(Vector(expectedDeltaP1), Vector(actual1.deltaPij()))); + EXPECT(assert_equal(Vector(expectedDeltaV1), Vector(actual1.deltaVij()))); + EXPECT(assert_equal(expectedDeltaR1, Rot3(actual1.deltaRij()))); + DOUBLES_EQUAL(expectedDeltaT1, actual1.deltaTij(), 1e-9); // Integrate again Vector3 expectedDeltaP2; @@ -188,280 +263,229 @@ TEST( ImuFactor, PreintegratedMeasurements ) { double expectedDeltaT2(1); // Actual preintegrated values - ImuFactor::PreintegratedMeasurements actual2 = actual1; + PreintegratedImuMeasurements actual2 = actual1; actual2.integrateMeasurement(measuredAcc, measuredOmega, deltaT); - EXPECT( - assert_equal(Vector(expectedDeltaP2), Vector(actual2.deltaPij()), 1e-6)); - EXPECT( - assert_equal(Vector(expectedDeltaV2), Vector(actual2.deltaVij()), 1e-6)); - EXPECT(assert_equal(expectedDeltaR2, Rot3(actual2.deltaRij()), 1e-6)); - DOUBLES_EQUAL(expectedDeltaT2, actual2.deltaTij(), 1e-6); + EXPECT(assert_equal(Vector(expectedDeltaP2), Vector(actual2.deltaPij()))); + EXPECT(assert_equal(Vector(expectedDeltaV2), Vector(actual2.deltaVij()))); + EXPECT(assert_equal(expectedDeltaR2, Rot3(actual2.deltaRij()))); + DOUBLES_EQUAL(expectedDeltaT2, actual2.deltaTij(), 1e-9); } /* ************************************************************************* */ -TEST( ImuFactor, ErrorAndJacobians ) { - // Linearization point - imuBias::ConstantBias bias; // Bias - Pose3 x1(Rot3::RzRyRx(M_PI / 12.0, M_PI / 6.0, M_PI / 4.0), - Point3(5.0, 1.0, -50.0)); - Vector3 v1(Vector3(0.5, 0.0, 0.0)); - Pose3 x2(Rot3::RzRyRx(M_PI / 12.0 + M_PI / 100.0, M_PI / 6.0, M_PI / 4.0), - Point3(5.5, 1.0, -50.0)); - Vector3 v2(Vector3(0.5, 0.0, 0.0)); +// Common linearization point and measurements for tests +namespace common { +static const Pose3 x1(Rot3::RzRyRx(M_PI / 12.0, M_PI / 6.0, M_PI / 4.0), + Point3(5.0, 1.0, 0)); +static const Vector3 v1(Vector3(0.5, 0.0, 0.0)); +static const NavState state1(x1, v1); - // Measurements - Vector3 gravity; - gravity << 0, 0, 9.81; - Vector3 omegaCoriolis; - omegaCoriolis << 0, 0, 0; - Vector3 measuredOmega; - measuredOmega << M_PI / 100, 0, 0; - Vector3 measuredAcc = x1.rotation().unrotate(-Point3(gravity)).vector(); - double deltaT = 1.0; - bool use2ndOrderIntegration = true; - ImuFactor::PreintegratedMeasurements pre_int_data(bias, - kMeasuredAccCovariance, - kMeasuredOmegaCovariance, - kIntegrationErrorCovariance, - use2ndOrderIntegration); - pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); +// Measurements +static const double w = M_PI / 100; +static const Vector3 measuredOmega(w, 0, 0); +static const Vector3 measuredAcc = x1.rotation().unrotate( + -kGravityAlongNavZDown); +static const double deltaT = 1.0; + +static const Pose3 x2(Rot3::RzRyRx(M_PI / 12.0 + w, M_PI / 6.0, M_PI / 4.0), + Point3(5.5, 1.0, 0)); +static const Vector3 v2(Vector3(0.5, 0.0, 0.0)); +static const NavState state2(x2, v2); +} // namespace common + +/* ************************************************************************* */ +TEST(ImuFactor, PreintegrationBaseMethods) { + using namespace common; + boost::shared_ptr p = + PreintegratedImuMeasurements::Params::MakeSharedD(); + p->gyroscopeCovariance = kMeasuredOmegaCovariance; + p->omegaCoriolis = Vector3(0.02, 0.03, 0.04); + p->accelerometerCovariance = kMeasuredAccCovariance; + p->integrationCovariance = kIntegrationErrorCovariance; + p->use2ndOrderCoriolis = true; + + PreintegratedImuMeasurements pim(p, kZeroBiasHat); + pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + + // biasCorrectedDelta + Matrix96 actualH; + pim.biasCorrectedDelta(kZeroBias, actualH); + Matrix expectedH = numericalDerivative11( + boost::bind(&PreintegrationBase::biasCorrectedDelta, pim, _1, + boost::none), kZeroBias); + EXPECT(assert_equal(expectedH, actualH)); + + Matrix9 aH1; + Matrix96 aH2; + NavState predictedState = pim.predict(state1, kZeroBias, aH1, aH2); + Matrix eH1 = numericalDerivative11( + boost::bind(&PreintegrationBase::predict, pim, _1, kZeroBias, boost::none, + boost::none), state1); + EXPECT(assert_equal(eH1, aH1)); + Matrix eH2 = numericalDerivative11( + boost::bind(&PreintegrationBase::predict, pim, state1, _1, boost::none, + boost::none), kZeroBias); + EXPECT(assert_equal(eH2, aH2)); + return; + +} + +/* ************************************************************************* */ +TEST(ImuFactor, ErrorAndJacobians) { + using namespace common; + PreintegratedImuMeasurements pim(kZeroBiasHat, kMeasuredAccCovariance, + kMeasuredOmegaCovariance, kIntegrationErrorCovariance); + + pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + EXPECT(assert_equal(state2, pim.predict(state1, kZeroBias))); // Create factor - ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, - omegaCoriolis); - - Vector errorActual = factor.evaluateError(x1, v1, x2, v2, bias); + ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, kGravityAlongNavZDown, + kZeroOmegaCoriolis); // Expected error - Vector errorExpected(9); - errorExpected << 0, 0, 0, 0, 0, 0, 0, 0, 0; - EXPECT(assert_equal(errorExpected, errorActual, 1e-6)); - - // Actual Jacobians - Matrix H1a, H2a, H3a, H4a, H5a; - (void) factor.evaluateError(x1, v1, x2, v2, bias, H1a, H2a, H3a, H4a, H5a); - - // Expected Jacobians - /////////////////// H1 /////////////////////////// - Matrix H1e = numericalDerivative11( - boost::bind(&callEvaluateError, factor, _1, v1, x2, v2, bias), x1); - // Jacobians are around zero, so the rotation part is the same as: - Matrix H1Rot3 = numericalDerivative11( - boost::bind(&evaluateRotationError, factor, _1, v1, x2, v2, bias), x1); - EXPECT(assert_equal(H1Rot3, H1e.bottomRows(3))); - EXPECT(assert_equal(H1e, H1a)); - - /////////////////// H2 /////////////////////////// - Matrix H2e = numericalDerivative11( - boost::bind(&callEvaluateError, factor, x1, _1, x2, v2, bias), v1); - EXPECT(assert_equal(H2e, H2a)); - - /////////////////// H3 /////////////////////////// - Matrix H3e = numericalDerivative11( - boost::bind(&callEvaluateError, factor, x1, v1, _1, v2, bias), x2); - // Jacobians are around zero, so the rotation part is the same as: - Matrix H3Rot3 = numericalDerivative11( - boost::bind(&evaluateRotationError, factor, x1, v1, _1, v2, bias), x2); - EXPECT(assert_equal(H3Rot3, H3e.bottomRows(3))); - EXPECT(assert_equal(H3e, H3a)); - - /////////////////// H4 /////////////////////////// - Matrix H4e = numericalDerivative11( - boost::bind(&callEvaluateError, factor, x1, v1, x2, _1, bias), v2); - EXPECT(assert_equal(H4e, H4a)); - - /////////////////// H5 /////////////////////////// - Matrix H5e = numericalDerivative11( - boost::bind(&callEvaluateError, factor, x1, v1, x2, v2, _1), bias); - EXPECT(assert_equal(H5e, H5a)); - - //////////////////////////////////////////////////////////////////////////// - // Evaluate error with wrong values - Vector3 v2_wrong = v2 + Vector3(0.1, 0.1, 0.1); - errorActual = factor.evaluateError(x1, v1, x2, v2_wrong, bias); - errorExpected << 0, 0, 0, 0.0724744871, 0.040715657, 0.151952901, 0, 0, 0; - EXPECT(assert_equal(errorExpected, errorActual, 1e-6)); + Vector expectedError(9); + expectedError << 0, 0, 0, 0, 0, 0, 0, 0, 0; + EXPECT( + assert_equal(expectedError, factor.evaluateError(x1, v1, x2, v2, kZeroBias))); Values values; values.insert(X(1), x1); values.insert(V(1), v1); values.insert(X(2), x2); - values.insert(V(2), v2_wrong); - values.insert(B(1), bias); - errorExpected << 0, 0, 0, 0.0724744871, 0.040715657, 0.151952901, 0, 0, 0; - EXPECT(assert_equal(factor.unwhitenedError(values), errorActual, 1e-6)); + values.insert(V(2), v2); + values.insert(B(1), kZeroBias); + EXPECT(assert_equal(expectedError, factor.unwhitenedError(values))); + + // Make sure linearization is correct + double diffDelta = 1e-7; + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, diffDelta, 1e-3); + + // Actual Jacobians + Matrix H1a, H2a, H3a, H4a, H5a; + (void) factor.evaluateError(x1, v1, x2, v2, kZeroBias, H1a, H2a, H3a, H4a, H5a); + + // Make sure rotation part is correct when error is interpreted as axis-angle + // Jacobians are around zero, so the rotation part is the same as: + Matrix H1Rot3 = numericalDerivative11( + boost::bind(&evaluateRotationError, factor, _1, v1, x2, v2, kZeroBias), x1); + EXPECT(assert_equal(H1Rot3, H1a.topRows(3))); + + Matrix H3Rot3 = numericalDerivative11( + boost::bind(&evaluateRotationError, factor, x1, v1, _1, v2, kZeroBias), x2); + EXPECT(assert_equal(H3Rot3, H3a.topRows(3))); + + // Evaluate error with wrong values + Vector3 v2_wrong = v2 + Vector3(0.1, 0.1, 0.1); + values.update(V(2), v2_wrong); + expectedError << 0, 0, 0, 0, 0, 0, -0.0724744871, -0.040715657, -0.151952901; + EXPECT( + assert_equal(expectedError, + factor.evaluateError(x1, v1, x2, v2_wrong, kZeroBias), 1e-2)); + EXPECT(assert_equal(expectedError, factor.unwhitenedError(values), 1e-2)); // Make sure the whitening is done correctly - Matrix cov = pre_int_data.preintMeasCov(); + Matrix cov = pim.preintMeasCov(); Matrix R = RtR(cov.inverse()); - Vector whitened = R * errorActual; - EXPECT(assert_equal(0.5 * whitened.squaredNorm(), factor.error(values), 1e-6)); + Vector whitened = R * expectedError; + EXPECT(assert_equal(0.5 * whitened.squaredNorm(), factor.error(values), 1e-5)); - /////////////////////////////////////////////////////////////////////////////// // Make sure linearization is correct - // Create expected value by numerical differentiation - JacobianFactor expected = linearizeNumerically(factor, values, 1e-8); - - // Create actual value by linearize - GaussianFactor::shared_ptr linearized = factor.linearize(values); - JacobianFactor* actual = dynamic_cast(linearized.get()); - - // Check cast result and then equality - EXPECT(assert_equal(expected, *actual, 1e-4)); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, diffDelta, 1e-3); } /* ************************************************************************* */ -TEST( ImuFactor, ErrorAndJacobianWithBiases ) { +TEST(ImuFactor, ErrorAndJacobianWithBiases) { + using common::x1; + using common::v1; + using common::v2; imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0.1, 0, 0.3)); // Biases (acc, rot) - Pose3 x1(Rot3::RzRyRx(M_PI / 12.0, M_PI / 6.0, M_PI / 10.0), - Point3(5.0, 1.0, -50.0)); - Vector3 v1(Vector3(0.5, 0.0, 0.0)); Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI / 10.0 + M_PI / 10.0)), Point3(5.5, 1.0, -50.0)); - Vector3 v2(Vector3(0.5, 0.0, 0.0)); // Measurements - Vector3 gravity; - gravity << 0, 0, 9.81; - Vector3 omegaCoriolis; - omegaCoriolis << 0, 0.1, 0.1; Vector3 measuredOmega; measuredOmega << 0, 0, M_PI / 10.0 + 0.3; - Vector3 measuredAcc = x1.rotation().unrotate(-Point3(gravity)).vector() + Vector3 measuredAcc = x1.rotation().unrotate(-kGravityAlongNavZDown) + Vector3(0.2, 0.0, 0.0); double deltaT = 1.0; - ImuFactor::PreintegratedMeasurements pre_int_data( - imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.1)), kMeasuredAccCovariance, + imuBias::ConstantBias biasHat(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.1)); + PreintegratedImuMeasurements pim(biasHat, kMeasuredAccCovariance, kMeasuredOmegaCovariance, kIntegrationErrorCovariance); - pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + + // Make sure of biasCorrectedDelta + Matrix96 actualH; + pim.biasCorrectedDelta(bias, actualH); + Matrix expectedH = numericalDerivative11( + boost::bind(&PreintegrationBase::biasCorrectedDelta, pim, _1, + boost::none), bias); + EXPECT(assert_equal(expectedH, actualH)); // Create factor - ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, - omegaCoriolis); + ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, kGravityAlongNavZDown, + kNonZeroOmegaCoriolis); - SETDEBUG("ImuFactor evaluateError", false); - Vector errorActual = factor.evaluateError(x1, v1, x2, v2, bias); - SETDEBUG("ImuFactor evaluateError", false); + Values values; + values.insert(X(1), x1); + values.insert(V(1), v1); + values.insert(X(2), x2); + values.insert(V(2), v2); + values.insert(B(1), bias); - // Expected error (should not be zero in this test, as we want to evaluate Jacobians - // at a nontrivial linearization point) - // Vector errorExpected(9); errorExpected << 0, 0, 0, 0, 0, 0, 0, 0, 0; - // EXPECT(assert_equal(errorExpected, errorActual, 1e-6)); - - // Expected Jacobians - Matrix H1e = numericalDerivative11( - boost::bind(&callEvaluateError, factor, _1, v1, x2, v2, bias), x1); - Matrix H2e = numericalDerivative11( - boost::bind(&callEvaluateError, factor, x1, _1, x2, v2, bias), v1); - Matrix H3e = numericalDerivative11( - boost::bind(&callEvaluateError, factor, x1, v1, _1, v2, bias), x2); - Matrix H4e = numericalDerivative11( - boost::bind(&callEvaluateError, factor, x1, v1, x2, _1, bias), v2); - Matrix H5e = numericalDerivative11( - boost::bind(&callEvaluateError, factor, x1, v1, x2, v2, _1), bias); - - // Check rotation Jacobians - Matrix RH1e = numericalDerivative11( - boost::bind(&evaluateRotationError, factor, _1, v1, x2, v2, bias), x1); - Matrix RH3e = numericalDerivative11( - boost::bind(&evaluateRotationError, factor, x1, v1, _1, v2, bias), x2); - Matrix RH5e = numericalDerivative11( - boost::bind(&evaluateRotationError, factor, x1, v1, x2, v2, _1), bias); - - // Actual Jacobians - Matrix H1a, H2a, H3a, H4a, H5a; - (void) factor.evaluateError(x1, v1, x2, v2, bias, H1a, H2a, H3a, H4a, H5a); - - EXPECT(assert_equal(H1e, H1a)); - EXPECT(assert_equal(H2e, H2a)); - EXPECT(assert_equal(H3e, H3a)); - EXPECT(assert_equal(H4e, H4a)); - EXPECT(assert_equal(H5e, H5a)); + // Make sure linearization is correct + double diffDelta = 1e-7; + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, diffDelta, 1e-3); } /* ************************************************************************* */ -TEST( ImuFactor, ErrorAndJacobianWith2ndOrderCoriolis ) { +TEST(ImuFactor, ErrorAndJacobianWith2ndOrderCoriolis) { + using common::x1; + using common::v1; + using common::v2; imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0.1, 0, 0.3)); // Biases (acc, rot) - Pose3 x1(Rot3::RzRyRx(M_PI / 12.0, M_PI / 6.0, M_PI / 10.0), - Point3(5.0, 1.0, -50.0)); - Vector3 v1(Vector3(0.5, 0.0, 0.0)); Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI / 10.0 + M_PI / 10.0)), Point3(5.5, 1.0, -50.0)); - Vector3 v2(Vector3(0.5, 0.0, 0.0)); // Measurements - Vector3 gravity; - gravity << 0, 0, 9.81; - Vector3 omegaCoriolis; - omegaCoriolis << 0, 0.1, 0.1; Vector3 measuredOmega; measuredOmega << 0, 0, M_PI / 10.0 + 0.3; - Vector3 measuredAcc = x1.rotation().unrotate(-Point3(gravity)).vector() + Vector3 measuredAcc = x1.rotation().unrotate(-kGravityAlongNavZDown) + Vector3(0.2, 0.0, 0.0); double deltaT = 1.0; - ImuFactor::PreintegratedMeasurements pre_int_data( - imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.1)), kMeasuredAccCovariance, - kMeasuredOmegaCovariance, kIntegrationErrorCovariance); - pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + PreintegratedImuMeasurements pim( + imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.1)), + kMeasuredAccCovariance, kMeasuredOmegaCovariance, + kIntegrationErrorCovariance); + pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); // Create factor - Pose3 bodyPsensor = Pose3(); bool use2ndOrderCoriolis = true; - ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, - omegaCoriolis, bodyPsensor, use2ndOrderCoriolis); + ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, kGravityAlongNavZDown, + kNonZeroOmegaCoriolis, boost::none, use2ndOrderCoriolis); - SETDEBUG("ImuFactor evaluateError", false); - Vector errorActual = factor.evaluateError(x1, v1, x2, v2, bias); - SETDEBUG("ImuFactor evaluateError", false); + Values values; + values.insert(X(1), x1); + values.insert(V(1), v1); + values.insert(X(2), x2); + values.insert(V(2), v2); + values.insert(B(1), bias); - // Expected error (should not be zero in this test, as we want to evaluate Jacobians - // at a nontrivial linearization point) - // Vector errorExpected(9); errorExpected << 0, 0, 0, 0, 0, 0, 0, 0, 0; - // EXPECT(assert_equal(errorExpected, errorActual, 1e-6)); - - // Expected Jacobians - Matrix H1e = numericalDerivative11( - boost::bind(&callEvaluateError, factor, _1, v1, x2, v2, bias), x1); - Matrix H2e = numericalDerivative11( - boost::bind(&callEvaluateError, factor, x1, _1, x2, v2, bias), v1); - Matrix H3e = numericalDerivative11( - boost::bind(&callEvaluateError, factor, x1, v1, _1, v2, bias), x2); - Matrix H4e = numericalDerivative11( - boost::bind(&callEvaluateError, factor, x1, v1, x2, _1, bias), v2); - Matrix H5e = numericalDerivative11( - boost::bind(&callEvaluateError, factor, x1, v1, x2, v2, _1), bias); - - // Check rotation Jacobians - Matrix RH1e = numericalDerivative11( - boost::bind(&evaluateRotationError, factor, _1, v1, x2, v2, bias), x1); - Matrix RH3e = numericalDerivative11( - boost::bind(&evaluateRotationError, factor, x1, v1, _1, v2, bias), x2); - Matrix RH5e = numericalDerivative11( - boost::bind(&evaluateRotationError, factor, x1, v1, x2, v2, _1), bias); - - // Actual Jacobians - Matrix H1a, H2a, H3a, H4a, H5a; - (void) factor.evaluateError(x1, v1, x2, v2, bias, H1a, H2a, H3a, H4a, H5a); - - EXPECT(assert_equal(H1e, H1a)); - EXPECT(assert_equal(H2e, H2a)); - EXPECT(assert_equal(H3e, H3a)); - EXPECT(assert_equal(H4e, H4a)); - EXPECT(assert_equal(H5e, H5a)); + // Make sure linearization is correct + double diffDelta = 1e-7; + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, diffDelta, 1e-3); } /* ************************************************************************* */ -TEST( ImuFactor, PartialDerivative_wrt_Bias ) { +TEST(ImuFactor, PartialDerivative_wrt_Bias) { // Linearization point - Vector3 biasOmega; - biasOmega << 0, 0, 0; ///< Current estimate of rotation rate bias + Vector3 biasOmega(0, 0, 0); // Current estimate of rotation rate bias // Measurements - Vector3 measuredOmega; - measuredOmega << 0.1, 0, 0; + Vector3 measuredOmega(0.1, 0, 0); double deltaT = 0.5; // Compute numerical derivatives @@ -475,18 +499,16 @@ TEST( ImuFactor, PartialDerivative_wrt_Bias ) { Matrix3 actualdelRdelBiasOmega = -Jr * deltaT; // the delta bias appears with the minus sign // Compare Jacobians - EXPECT(assert_equal(expectedDelRdelBiasOmega, actualdelRdelBiasOmega, 1e-3)); // 1e-3 needs to be added only when using quaternions for rotations + EXPECT(assert_equal(expectedDelRdelBiasOmega, actualdelRdelBiasOmega, 1e-9)); } /* ************************************************************************* */ -TEST( ImuFactor, PartialDerivativeLogmap ) { +TEST(ImuFactor, PartialDerivativeLogmap) { // Linearization point - Vector3 thetahat; - thetahat << 0.1, 0.1, 0; ///< Current estimate of rotation rate bias + Vector3 thetahat(0.1, 0.1, 0); // Current estimate of rotation rate bias // Measurements - Vector3 deltatheta; - deltatheta << 0, 0, 0; + Vector3 deltatheta(0, 0, 0); // Compute numerical derivatives Matrix expectedDelFdeltheta = numericalDerivative11( @@ -499,14 +521,12 @@ TEST( ImuFactor, PartialDerivativeLogmap ) { } /* ************************************************************************* */ -TEST( ImuFactor, fistOrderExponential ) { +TEST(ImuFactor, fistOrderExponential) { // Linearization point - Vector3 biasOmega; - biasOmega << 0, 0, 0; ///< Current estimate of rotation rate bias + Vector3 biasOmega(0, 0, 0); // Current estimate of rotation rate bias // Measurements - Vector3 measuredOmega; - measuredOmega << 0.1, 0, 0; + Vector3 measuredOmega(0.1, 0, 0); double deltaT = 1.0; // change w.r.t. linearization point @@ -526,17 +546,14 @@ TEST( ImuFactor, fistOrderExponential ) { Rot3::Expmap((measuredOmega - biasOmega) * deltaT).matrix(); const Matrix3 actualRot = hatRot * Rot3::Expmap(delRdelBiasOmega * deltabiasOmega).matrix(); - //hatRot * (Matrix3::Identity() + skewSymmetric(delRdelBiasOmega * deltabiasOmega)); + // hatRot * (I_3x3 + skewSymmetric(delRdelBiasOmega * deltabiasOmega)); // This is a first order expansion so the equality is only an approximation EXPECT(assert_equal(expectedRot, actualRot)); } /* ************************************************************************* */ -TEST( ImuFactor, FirstOrderPreIntegratedMeasurements ) { - // Linearization point - imuBias::ConstantBias bias; ///< Current estimate of acceleration and rotation rate biases - +TEST(ImuFactor, FirstOrderPreIntegratedMeasurements) { Pose3 body_P_sensor(Rot3::Expmap(Vector3(0, 0.1, 0.1)), Point3(1, 0, 1)); // Measurements @@ -556,29 +573,29 @@ TEST( ImuFactor, FirstOrderPreIntegratedMeasurements ) { } // Actual preintegrated values - ImuFactor::PreintegratedMeasurements preintegrated = - evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, + PreintegratedImuMeasurements preintegrated = + evaluatePreintegratedMeasurements(kZeroBias, measuredAccs, measuredOmegas, deltaTs); // Compute numerical derivatives Matrix expectedDelPdelBias = numericalDerivative11( boost::bind(&evaluatePreintegratedMeasurementsPosition, _1, measuredAccs, - measuredOmegas, deltaTs), bias); + measuredOmegas, deltaTs), kZeroBias); Matrix expectedDelPdelBiasAcc = expectedDelPdelBias.leftCols(3); Matrix expectedDelPdelBiasOmega = expectedDelPdelBias.rightCols(3); Matrix expectedDelVdelBias = numericalDerivative11( boost::bind(&evaluatePreintegratedMeasurementsVelocity, _1, measuredAccs, - measuredOmegas, deltaTs), bias); + measuredOmegas, deltaTs), kZeroBias); Matrix expectedDelVdelBiasAcc = expectedDelVdelBias.leftCols(3); Matrix expectedDelVdelBiasOmega = expectedDelVdelBias.rightCols(3); Matrix expectedDelRdelBias = numericalDerivative11( boost::bind(&evaluatePreintegratedMeasurementsRotation, _1, - measuredAccs, measuredOmegas, deltaTs), bias); + measuredAccs, measuredOmegas, deltaTs), kZeroBias); Matrix expectedDelRdelBiasAcc = expectedDelRdelBias.leftCols(3); Matrix expectedDelRdelBiasOmega = expectedDelRdelBias.rightCols(3); @@ -591,316 +608,16 @@ TEST( ImuFactor, FirstOrderPreIntegratedMeasurements ) { assert_equal(expectedDelVdelBiasOmega, preintegrated.delVdelBiasOmega())); EXPECT(assert_equal(expectedDelRdelBiasAcc, Matrix::Zero(3, 3))); EXPECT( - assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega(), - 1e-3)); // 1e-3 needs to be added only when using quaternions for rotations + assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega())); } +#endif /* ************************************************************************* */ -TEST( ImuFactor, JacobianPreintegratedCovariancePropagation ) { - // Linearization point - imuBias::ConstantBias bias; ///< Current estimate of acceleration and rotation rate biases - Pose3 body_P_sensor = Pose3(); // (Rot3::Expmap(Vector3(0,0.1,0.1)), Point3(1, 0, 1)); - - // Measurements - list measuredAccs, measuredOmegas; - list deltaTs; - measuredAccs.push_back(Vector3(0.1, 0.0, 0.0)); - measuredOmegas.push_back(Vector3(M_PI / 100.0, 0.0, 0.0)); - deltaTs.push_back(0.01); - measuredAccs.push_back(Vector3(0.1, 0.0, 0.0)); - measuredOmegas.push_back(Vector3(M_PI / 100.0, 0.0, 0.0)); - deltaTs.push_back(0.01); - for (int i = 1; i < 100; i++) { - measuredAccs.push_back(Vector3(0.05, 0.09, 0.01)); - measuredOmegas.push_back( - Vector3(M_PI / 100.0, M_PI / 300.0, 2 * M_PI / 100.0)); - deltaTs.push_back(0.01); - } - bool use2ndOrderIntegration = false; - // Actual preintegrated values - ImuFactor::PreintegratedMeasurements preintegrated = - evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, - deltaTs, use2ndOrderIntegration); - - // so far we only created a nontrivial linearization point for the preintegrated measurements - // Now we add a new measurement and ask for Jacobians - const Vector3 newMeasuredAcc = Vector3(0.1, 0.0, 0.0); - const Vector3 newMeasuredOmega = Vector3(M_PI / 100.0, 0.0, 0.0); - const double newDeltaT = 0.01; - const Rot3 deltaRij_old = preintegrated.deltaRij(); // before adding new measurement - const Vector3 deltaVij_old = preintegrated.deltaVij(); // before adding new measurement - const Vector3 deltaPij_old = preintegrated.deltaPij(); // before adding new measurement - - Matrix oldPreintCovariance = preintegrated.preintMeasCov(); - - Matrix Factual, Gactual; - preintegrated.integrateMeasurement(newMeasuredAcc, newMeasuredOmega, - newDeltaT, body_P_sensor, Factual, Gactual); - - ////////////////////////////////////////////////////////////////////////////////////////////// - // COMPUTE NUMERICAL DERIVATIVES FOR F - ////////////////////////////////////////////////////////////////////////////////////////////// - // Compute expected f_pos_vel wrt positions - Matrix dfpv_dpos = numericalDerivative11( - boost::bind(&updatePreintegratedPosVel, _1, deltaVij_old, deltaRij_old, - newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), - deltaPij_old); - - // Compute expected f_pos_vel wrt velocities - Matrix dfpv_dvel = numericalDerivative11( - boost::bind(&updatePreintegratedPosVel, deltaPij_old, _1, deltaRij_old, - newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), - deltaVij_old); - - // Compute expected f_pos_vel wrt angles - Matrix dfpv_dangle = numericalDerivative11( - boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old, _1, - newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), - deltaRij_old); - - Matrix FexpectedTop6(6, 9); - FexpectedTop6 << dfpv_dpos, dfpv_dvel, dfpv_dangle; - - // Compute expected f_rot wrt angles - Matrix dfr_dangle = numericalDerivative11( - boost::bind(&updatePreintegratedRot, _1, newMeasuredOmega, newDeltaT), - deltaRij_old); - - Matrix FexpectedBottom3(3, 9); - FexpectedBottom3 << Z_3x3, Z_3x3, dfr_dangle; - Matrix Fexpected(9, 9); - Fexpected << FexpectedTop6, FexpectedBottom3; - - EXPECT(assert_equal(Fexpected, Factual)); - - ////////////////////////////////////////////////////////////////////////////////////////////// - // COMPUTE NUMERICAL DERIVATIVES FOR G - ////////////////////////////////////////////////////////////////////////////////////////////// - // Compute jacobian wrt integration noise - Matrix dgpv_dintNoise(6, 3); - dgpv_dintNoise << I_3x3 * newDeltaT, Z_3x3; - - // Compute jacobian wrt acc noise - Matrix dgpv_daccNoise = numericalDerivative11( - boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old, - deltaRij_old, _1, newMeasuredOmega, newDeltaT, - use2ndOrderIntegration), newMeasuredAcc); - - // Compute expected F wrt gyro noise - Matrix dgpv_domegaNoise = numericalDerivative11( - boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old, - deltaRij_old, newMeasuredAcc, _1, newDeltaT, use2ndOrderIntegration), - newMeasuredOmega); - Matrix GexpectedTop6(6, 9); - GexpectedTop6 << dgpv_dintNoise, dgpv_daccNoise, dgpv_domegaNoise; - - // Compute expected f_rot wrt gyro noise - Matrix dgr_dangle = numericalDerivative11( - boost::bind(&updatePreintegratedRot, deltaRij_old, _1, newDeltaT), - newMeasuredOmega); - - Matrix GexpectedBottom3(3, 9); - GexpectedBottom3 << Z_3x3, Z_3x3, dgr_dangle; - Matrix Gexpected(9, 9); - Gexpected << GexpectedTop6, GexpectedBottom3; - - EXPECT(assert_equal(Gexpected, Gactual)); - - // Check covariance propagation - Matrix9 measurementCovariance; - measurementCovariance << intNoiseVar * I_3x3, Z_3x3, Z_3x3, Z_3x3, accNoiseVar - * I_3x3, Z_3x3, Z_3x3, Z_3x3, omegaNoiseVar * I_3x3; - - Matrix newPreintCovarianceExpected = Factual * oldPreintCovariance - * Factual.transpose() - + (1 / newDeltaT) * Gactual * measurementCovariance * Gactual.transpose(); - - Matrix newPreintCovarianceActual = preintegrated.preintMeasCov(); - EXPECT(assert_equal(newPreintCovarianceExpected, newPreintCovarianceActual)); +Vector3 correctedAcc(const PreintegratedImuMeasurements& pim, const Vector3& measuredAcc, const Vector3& measuredOmega) { + return pim.correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega).first; } -/* ************************************************************************* */ -TEST( ImuFactor, JacobianPreintegratedCovariancePropagation_2ndOrderInt ) { - // Linearization point - imuBias::ConstantBias bias; ///< Current estimate of acceleration and rotation rate biases - Pose3 body_P_sensor = Pose3(); // (Rot3::Expmap(Vector3(0,0.1,0.1)), Point3(1, 0, 1)); - - // Measurements - list measuredAccs, measuredOmegas; - list deltaTs; - measuredAccs.push_back(Vector3(0.1, 0.0, 0.0)); - measuredOmegas.push_back(Vector3(M_PI / 100.0, 0.0, 0.0)); - deltaTs.push_back(0.01); - measuredAccs.push_back(Vector3(0.1, 0.0, 0.0)); - measuredOmegas.push_back(Vector3(M_PI / 100.0, 0.0, 0.0)); - deltaTs.push_back(0.01); - for (int i = 1; i < 100; i++) { - measuredAccs.push_back(Vector3(0.05, 0.09, 0.01)); - measuredOmegas.push_back( - Vector3(M_PI / 100.0, M_PI / 300.0, 2 * M_PI / 100.0)); - deltaTs.push_back(0.01); - } - bool use2ndOrderIntegration = true; - // Actual preintegrated values - ImuFactor::PreintegratedMeasurements preintegrated = - evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, - deltaTs, use2ndOrderIntegration); - - // so far we only created a nontrivial linearization point for the preintegrated measurements - // Now we add a new measurement and ask for Jacobians - const Vector3 newMeasuredAcc = Vector3(0.1, 0.0, 0.0); - const Vector3 newMeasuredOmega = Vector3(M_PI / 100.0, 0.0, 0.0); - const double newDeltaT = 0.01; - const Rot3 deltaRij_old = preintegrated.deltaRij(); // before adding new measurement - const Vector3 deltaVij_old = preintegrated.deltaVij(); // before adding new measurement - const Vector3 deltaPij_old = preintegrated.deltaPij(); // before adding new measurement - - Matrix oldPreintCovariance = preintegrated.preintMeasCov(); - - Matrix Factual, Gactual; - preintegrated.integrateMeasurement(newMeasuredAcc, newMeasuredOmega, - newDeltaT, body_P_sensor, Factual, Gactual); - - ////////////////////////////////////////////////////////////////////////////////////////////// - // COMPUTE NUMERICAL DERIVATIVES FOR F - ////////////////////////////////////////////////////////////////////////////////////////////// - // Compute expected f_pos_vel wrt positions - Matrix dfpv_dpos = numericalDerivative11( - boost::bind(&updatePreintegratedPosVel, _1, deltaVij_old, deltaRij_old, - newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), - deltaPij_old); - - // Compute expected f_pos_vel wrt velocities - Matrix dfpv_dvel = numericalDerivative11( - boost::bind(&updatePreintegratedPosVel, deltaPij_old, _1, deltaRij_old, - newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), - deltaVij_old); - - // Compute expected f_pos_vel wrt angles - Matrix dfpv_dangle = numericalDerivative11( - boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old, _1, - newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), - deltaRij_old); - - Matrix FexpectedTop6(6, 9); - FexpectedTop6 << dfpv_dpos, dfpv_dvel, dfpv_dangle; - - // Compute expected f_rot wrt angles - Matrix dfr_dangle = numericalDerivative11( - boost::bind(&updatePreintegratedRot, _1, newMeasuredOmega, newDeltaT), - deltaRij_old); - - Matrix FexpectedBottom3(3, 9); - FexpectedBottom3 << Z_3x3, Z_3x3, dfr_dangle; - Matrix Fexpected(9, 9); - Fexpected << FexpectedTop6, FexpectedBottom3; - - EXPECT(assert_equal(Fexpected, Factual)); - - ////////////////////////////////////////////////////////////////////////////////////////////// - // COMPUTE NUMERICAL DERIVATIVES FOR G - ////////////////////////////////////////////////////////////////////////////////////////////// - // Compute jacobian wrt integration noise - Matrix dgpv_dintNoise(6, 3); - dgpv_dintNoise << I_3x3 * newDeltaT, Z_3x3; - - // Compute jacobian wrt acc noise - Matrix dgpv_daccNoise = numericalDerivative11( - boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old, - deltaRij_old, _1, newMeasuredOmega, newDeltaT, - use2ndOrderIntegration), newMeasuredAcc); - - // Compute expected F wrt gyro noise - Matrix dgpv_domegaNoise = numericalDerivative11( - boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old, - deltaRij_old, newMeasuredAcc, _1, newDeltaT, use2ndOrderIntegration), - newMeasuredOmega); - Matrix GexpectedTop6(6, 9); - GexpectedTop6 << dgpv_dintNoise, dgpv_daccNoise, dgpv_domegaNoise; - - // Compute expected f_rot wrt gyro noise - Matrix dgr_dangle = numericalDerivative11( - boost::bind(&updatePreintegratedRot, deltaRij_old, _1, newDeltaT), - newMeasuredOmega); - - Matrix GexpectedBottom3(3, 9); - GexpectedBottom3 << Z_3x3, Z_3x3, dgr_dangle; - Matrix Gexpected(9, 9); - Gexpected << GexpectedTop6, GexpectedBottom3; - - EXPECT(assert_equal(Gexpected, Gactual)); - - // Check covariance propagation - Matrix9 measurementCovariance; - measurementCovariance << intNoiseVar * I_3x3, Z_3x3, Z_3x3, Z_3x3, accNoiseVar - * I_3x3, Z_3x3, Z_3x3, Z_3x3, omegaNoiseVar * I_3x3; - - Matrix newPreintCovarianceExpected = Factual * oldPreintCovariance - * Factual.transpose() - + (1 / newDeltaT) * Gactual * measurementCovariance * Gactual.transpose(); - - Matrix newPreintCovarianceActual = preintegrated.preintMeasCov(); - EXPECT(assert_equal(newPreintCovarianceExpected, newPreintCovarianceActual)); -} - -//#include -///* ************************************************************************* */ -//TEST( ImuFactor, LinearizeTiming) -//{ -// // Linearization point -// Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/4.0), Point3(5.0, 1.0, -50.0)); -// Vector3 v1(Vector3(0.5, 0.0, 0.0)); -// Pose3 x2(Rot3::RzRyRx(M_PI/12.0 + M_PI/100.0, M_PI/6.0, M_PI/4.0), Point3(5.5, 1.0, -50.0)); -// Vector3 v2(Vector3(0.5, 0.0, 0.0)); -// imuBias::ConstantBias bias(Vector3(0.001, 0.002, 0.008), Vector3(0.002, 0.004, 0.012)); -// -// // Pre-integrator -// imuBias::ConstantBias biasHat(Vector3(0, 0, 0.10), Vector3(0, 0, 0.10)); -// Vector3 gravity; gravity << 0, 0, 9.81; -// Vector3 omegaCoriolis; omegaCoriolis << 0.0001, 0, 0.01; -// ImuFactor::PreintegratedMeasurements pre_int_data(biasHat, Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity()); -// -// // Pre-integrate Measurements -// Vector3 measuredAcc(0.1, 0.0, 0.0); -// Vector3 measuredOmega(M_PI/100.0, 0.0, 0.0); -// double deltaT = 0.5; -// for(size_t i = 0; i < 50; ++i) { -// pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); -// } -// -// // Create factor -// noiseModel::Base::shared_ptr model = noiseModel::Gaussian::Covariance(pre_int_data.MeasurementCovariance()); -// ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis); -// -// Values values; -// values.insert(X(1), x1); -// values.insert(X(2), x2); -// values.insert(V(1), v1); -// values.insert(V(2), v2); -// values.insert(B(1), bias); -// -// Ordering ordering; -// ordering.push_back(X(1)); -// ordering.push_back(V(1)); -// ordering.push_back(X(2)); -// ordering.push_back(V(2)); -// ordering.push_back(B(1)); -// -// GaussianFactorGraph graph; -// gttic_(LinearizeTiming); -// for(size_t i = 0; i < 100000; ++i) { -// GaussianFactor::shared_ptr g = factor.linearize(values, ordering); -// graph.push_back(g); -// } -// gttoc_(LinearizeTiming); -// tictoc_finishedIteration_(); -// std::cout << "Linear Error: " << graph.error(values.zeroVectors(ordering)) << std::endl; -// tictoc_print_(); -//} - -/* ************************************************************************* */ -TEST( ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement ) { - +TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) { imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot) Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0)), Point3(5.0, 1.0, -50.0)); Vector3 v1(Vector3(0.5, 0.0, 0.0)); @@ -909,58 +626,130 @@ TEST( ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement ) { Vector3 v2(Vector3(0.5, 0.0, 0.0)); // Measurements - Vector3 gravity; - gravity << 0, 0, 9.81; - Vector3 omegaCoriolis; - omegaCoriolis << 0, 0.1, 0.1; Vector3 measuredOmega; measuredOmega << 0, 0, M_PI / 10.0 + 0.3; - Vector3 measuredAcc = x1.rotation().unrotate(-Point3(gravity)).vector() + Vector3 measuredAcc = x1.rotation().unrotate(-kGravityAlongNavZDown) + Vector3(0.2, 0.0, 0.0); - double deltaT = 1.0; + double dt = 0.1; - const Pose3 body_P_sensor(Rot3::Expmap(Vector3(0, 0.10, 0.10)), - Point3(1, 0, 0)); + Pose3 body_P_sensor(Rot3::Expmap(Vector3(0, M_PI/2, 0)), Point3(0.1, 0.05, 0.01)); + imuBias::ConstantBias biasHat(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)); - ImuFactor::PreintegratedMeasurements pre_int_data( - imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), kMeasuredAccCovariance, - kMeasuredOmegaCovariance, kIntegrationErrorCovariance); + // Get mean prediction from "ground truth" measurements + PreintegratedImuMeasurements pim(biasHat, accNoiseVar2.asDiagonal(), + omegaNoiseVar2.asDiagonal(), Z_3x3, true); // MonteCarlo does not sample integration noise + pim.set_body_P_sensor(body_P_sensor); - pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + // Check updatedDeltaXij derivatives + Matrix3 D_correctedAcc_measuredOmega = Matrix3::Zero(); + pim.correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega, boost::none, D_correctedAcc_measuredOmega, boost::none); + Matrix3 expectedD = numericalDerivative11(boost::bind(correctedAcc, pim, measuredAcc, _1), measuredOmega, 1e-6); + EXPECT(assert_equal(expectedD, D_correctedAcc_measuredOmega, 1e-5)); + + Matrix93 G1, G2; + NavState preint = pim.updatedDeltaXij(measuredAcc, measuredOmega, dt, boost::none, G1, G2); +// Matrix9 preintCov = G1*((accNoiseVar2/dt).asDiagonal())*G1.transpose() + G2*((omegaNoiseVar2/dt).asDiagonal())*G2.transpose(); + + Matrix93 expectedG1 = numericalDerivative21( + boost::bind(&PreintegratedImuMeasurements::updatedDeltaXij, pim, _1, _2, + dt, boost::none, boost::none, boost::none), measuredAcc, measuredOmega, + 1e-6); + EXPECT(assert_equal(expectedG1, G1, 1e-5)); + + Matrix93 expectedG2 = numericalDerivative22( + boost::bind(&PreintegratedImuMeasurements::updatedDeltaXij, pim, _1, _2, + dt, boost::none, boost::none, boost::none), measuredAcc, measuredOmega, + 1e-6); + EXPECT(assert_equal(expectedG2, G2, 1e-5)); + +#if 0 + /* + * This code is to verify the quality of the generated samples + * by checking if the covariance of the generated noises matches + * with the input covariance, and visualizing the nonlinearity of + * the sample set using the following matlab script: + * + noises = dlmread('noises.txt'); + cov(noises) + + samples = dlmread('noises.txt'); + figure(1); + for i=1:9 + subplot(3,3,i) + hist(samples(:,i), 500) + end + */ + size_t N = 10000; + Matrix samples(9,N); + Sampler sampleAccelerationNoise((accNoiseVar2/dt).cwiseSqrt(), 29284); + Sampler sampleOmegaNoise((omegaNoiseVar2/dt).cwiseSqrt(), 10); + ofstream samplesOut("preintSamples.txt"); + ofstream noiseOut("noises.txt"); + for (size_t i = 0; i( - boost::bind(&callEvaluateError, factor, _1, v1, x2, v2, bias), x1); - Matrix H2e = numericalDerivative11( - boost::bind(&callEvaluateError, factor, x1, _1, x2, v2, bias), v1); - Matrix H3e = numericalDerivative11( - boost::bind(&callEvaluateError, factor, x1, v1, _1, v2, bias), x2); - Matrix H4e = numericalDerivative11( - boost::bind(&callEvaluateError, factor, x1, v1, x2, _1, bias), v2); - Matrix H5e = numericalDerivative11( - boost::bind(&callEvaluateError, factor, x1, v1, x2, v2, _1), bias); + // Predict + Pose3 actual_x2; + Vector3 actual_v2; + ImuFactor::Predict(x1, v1, actual_x2, actual_v2, bias, pim, + kGravityAlongNavZDown, kZeroOmegaCoriolis); + // Regression test with +// Rot3 expectedR( // +// 0.456795409, -0.888128414, 0.0506544554, // +// 0.889548908, 0.45563417, -0.0331699173, // +// 0.00637924528, 0.0602114814, 0.998165258); +// Point3 expectedT(5.30373101, 0.768972495, -49.9942188); +// Vector3 expected_v2(0.107462014, -0.46205501, 0.0115624037); +// Pose3 expected_x2(expectedR, expectedT); +// EXPECT(assert_equal(expected_x2, actual_x2, 1e-7)); +// EXPECT(assert_equal(Vector(expected_v2), actual_v2, 1e-7)); - // Check rotation Jacobians - Matrix RH1e = numericalDerivative11( - boost::bind(&evaluateRotationError, factor, _1, v1, x2, v2, bias), x1); - Matrix RH3e = numericalDerivative11( - boost::bind(&evaluateRotationError, factor, x1, v1, _1, v2, bias), x2); - Matrix RH5e = numericalDerivative11( - boost::bind(&evaluateRotationError, factor, x1, v1, x2, v2, _1), bias); + Values values; + values.insert(X(1), x1); + values.insert(V(1), v1); + values.insert(X(2), x2); + values.insert(V(2), v2); + values.insert(B(1), bias); - // Actual Jacobians - Matrix H1a, H2a, H3a, H4a, H5a; - (void) factor.evaluateError(x1, v1, x2, v2, bias, H1a, H2a, H3a, H4a, H5a); +// factor.get_noiseModel()->print("noise: "); // Make sure the noise is valid - EXPECT(assert_equal(H1e, H1a)); - EXPECT(assert_equal(H2e, H2a)); - EXPECT(assert_equal(H3e, H3a)); - EXPECT(assert_equal(H4e, H4a)); - EXPECT(assert_equal(H5e, H5a)); + // Make sure linearization is correct + double diffDelta = 1e-8; + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, diffDelta, 1e-3); } /* ************************************************************************* */ @@ -968,35 +757,29 @@ TEST(ImuFactor, PredictPositionAndVelocity) { imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot) // Measurements - Vector3 gravity; - gravity << 0, 0, 9.81; - Vector3 omegaCoriolis; - omegaCoriolis << 0, 0, 0; Vector3 measuredOmega; - measuredOmega << 0, 0, 0; //M_PI/10.0+0.3; + measuredOmega << 0, 0, 0; // M_PI/10.0+0.3; Vector3 measuredAcc; measuredAcc << 0, 1, -9.81; double deltaT = 0.001; - Matrix I6x6(6, 6); - I6x6 = Matrix::Identity(6, 6); - - ImuFactor::PreintegratedMeasurements pre_int_data( - imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), kMeasuredAccCovariance, - kMeasuredOmegaCovariance, kIntegrationErrorCovariance, true); + PreintegratedImuMeasurements pim( + imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), + kMeasuredAccCovariance, kMeasuredOmegaCovariance, + kIntegrationErrorCovariance, true); for (int i = 0; i < 1000; ++i) - pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); // Create factor - ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, - omegaCoriolis); + ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, kGravityAlongNavZDown, + kZeroOmegaCoriolis); // Predict Pose3 x1; Vector3 v1(0, 0.0, 0.0); - PoseVelocityBias poseVelocity = pre_int_data.predict(x1, v1, bias, gravity, - omegaCoriolis); + PoseVelocityBias poseVelocity = pim.predict(x1, v1, bias, + kGravityAlongNavZDown, kZeroOmegaCoriolis); Pose3 expectedPose(Rot3(), Point3(0, 0.5, 0)); Vector3 expectedVelocity; expectedVelocity << 0, 1, 0; @@ -1009,36 +792,30 @@ TEST(ImuFactor, PredictRotation) { imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot) // Measurements - Vector3 gravity; - gravity << 0, 0, 9.81; - Vector3 omegaCoriolis; - omegaCoriolis << 0, 0, 0; Vector3 measuredOmega; - measuredOmega << 0, 0, M_PI / 10; //M_PI/10.0+0.3; + measuredOmega << 0, 0, M_PI / 10; // M_PI/10.0+0.3; Vector3 measuredAcc; measuredAcc << 0, 0, -9.81; double deltaT = 0.001; - Matrix I6x6(6, 6); - I6x6 = Matrix::Identity(6, 6); - - ImuFactor::PreintegratedMeasurements pre_int_data( - imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), kMeasuredAccCovariance, - kMeasuredOmegaCovariance, kIntegrationErrorCovariance, true); + PreintegratedImuMeasurements pim( + imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), + kMeasuredAccCovariance, kMeasuredOmegaCovariance, + kIntegrationErrorCovariance, true); for (int i = 0; i < 1000; ++i) - pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); // Create factor - ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, - omegaCoriolis); + ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, kGravityAlongNavZDown, + kZeroOmegaCoriolis); // Predict Pose3 x1, x2; Vector3 v1 = Vector3(0, 0.0, 0.0); Vector3 v2; - ImuFactor::Predict(x1, v1, x2, v2, bias, factor.preintegratedMeasurements(), - gravity, omegaCoriolis); + ImuFactor::Predict(x1, v1, x2, v2, bias, pim, kGravityAlongNavZDown, + kZeroOmegaCoriolis); Pose3 expectedPose(Rot3().ypr(M_PI / 10, 0, 0), Point3(0, 0, 0)); Vector3 expectedVelocity; expectedVelocity << 0, 0, 0; @@ -1046,6 +823,196 @@ TEST(ImuFactor, PredictRotation) { EXPECT(assert_equal(Vector(expectedVelocity), Vector(v2))); } +/* ************************************************************************* */ +TEST(ImuFactor, PredictArbitrary) { + imuBias::ConstantBias biasHat(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)); + + // Measurements + Vector3 measuredOmega(M_PI / 10, M_PI / 10, M_PI / 10); + Vector3 measuredAcc(0.1, 0.2, -9.81); + double dt = 0.001; + + ImuFactor::PreintegratedMeasurements pim(biasHat, kMeasuredAccCovariance, + kMeasuredOmegaCovariance, Z_3x3, true); // MonteCarlo does not sample integration noise + Pose3 x1; + Vector3 v1 = Vector3(0, 0, 0); + imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); + EXPECT(MonteCarlo(pim, NavState(x1, v1), bias, 0.1, boost::none, + measuredAcc, measuredOmega, Vector3::Constant(accNoiseVar), Vector3::Constant(omegaNoiseVar))); + + for (int i = 0; i < 1000; ++i) + pim.integrateMeasurement(measuredAcc, measuredOmega, dt); + +// Matrix expected(9,9); +// expected << // +// 0.0299999995, 2.46739898e-10, 2.46739896e-10, -0.0144839494, 0.044978128, 0.0100471195, -0.0409843415, 0.134423822, 0.0383280513, // +// 2.46739898e-10, 0.0299999995, 2.46739902e-10, -0.0454268484, -0.0149428917, 0.00609093775, -0.13554868, -0.0471183681, 0.0247643646, // +// 2.46739896e-10, 2.46739902e-10, 0.0299999995, 0.00489577218, 0.00839301168, 0.000448720395, 0.00879031682, 0.0162199769, 0.00112485862, // +// -0.0144839494, -0.0454268484, 0.00489577218, 0.142448905, 0.00345595825, -0.0225794125, 0.34774305, 0.0119449979, -0.075667905, // +// 0.044978128, -0.0149428917, 0.00839301168, 0.00345595825, 0.143318431, 0.0200549262, 0.0112877167, 0.351503176, 0.0629164907, // +// 0.0100471195, 0.00609093775, 0.000448720395, -0.0225794125, 0.0200549262, 0.0104041905, -0.0580647212, 0.051116506, 0.0285371399, // +// -0.0409843415, -0.13554868, 0.00879031682, 0.34774305, 0.0112877167, -0.0580647212, 0.911721561, 0.0412249672, -0.205920425, // +// 0.134423822, -0.0471183681, 0.0162199769, 0.0119449979, 0.351503176, 0.051116506, 0.0412249672, 0.928013807, 0.169935105, // +// 0.0383280513, 0.0247643646, 0.00112485862, -0.075667905, 0.0629164907, 0.0285371399, -0.205920425, 0.169935105, 0.09407764; +// EXPECT(assert_equal(expected, pim.preintMeasCov(), 1e-7)); + + // Create factor + ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, kGravityAlongNavZDown, + kZeroOmegaCoriolis); + + // Predict + Pose3 x2; + Vector3 v2; + ImuFactor::Predict(x1, v1, x2, v2, bias, pim, kGravityAlongNavZDown, + kZeroOmegaCoriolis); + + // Regression test for Imu Refactor + Rot3 expectedR( // + +0.903715275, -0.250741668, 0.347026393, // + +0.347026393, 0.903715275, -0.250741668, // + -0.250741668, 0.347026393, 0.903715275); + Point3 expectedT(-0.505517319, 0.569413747, 0.0861035711); + Vector3 expectedV(-1.59121524, 1.55353139, 0.3376838540); + Pose3 expectedPose(expectedR, expectedT); + EXPECT(assert_equal(expectedPose, x2, 1e-7)); + EXPECT(assert_equal(Vector(expectedV), v2, 1e-7)); +} + +/* ************************************************************************* */ +TEST(ImuFactor, bodyPSensorNoBias) { + imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0.1, 0)); // Biases (acc, rot) + + // Measurements + Vector3 n_gravity(0, 0, -9.81); // z-up nav frame + Vector3 omegaCoriolis(0, 0, 0); + // Sensor frame is z-down + // Gyroscope measurement is the angular velocity of sensor w.r.t nav frame in sensor frame + Vector3 s_omegaMeas_ns(0, 0.1, M_PI / 10); + // Acc measurement is acceleration of sensor in the sensor frame, when stationary, + // table exerts an equal and opposite force w.r.t gravity + Vector3 s_accMeas(0, 0, -9.81); + double dt = 0.001; + + // Rotate sensor (z-down) to body (same as navigation) i.e. z-up + Pose3 body_P_sensor(Rot3::ypr(0, 0, M_PI), Point3(0, 0, 0)); + + ImuFactor::PreintegratedMeasurements pim(bias, Z_3x3, Z_3x3, Z_3x3, true); + + for (int i = 0; i < 1000; ++i) + pim.integrateMeasurement(s_accMeas, s_omegaMeas_ns, dt, body_P_sensor); + + // Create factor + ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, n_gravity, omegaCoriolis); + + // Predict + Pose3 x1; + Vector3 v1(0, 0, 0); + PoseVelocityBias poseVelocity = pim.predict(x1, v1, bias, n_gravity, + omegaCoriolis); + + Pose3 expectedPose(Rot3().ypr(-M_PI / 10, 0, 0), Point3(0, 0, 0)); + EXPECT(assert_equal(expectedPose, poseVelocity.pose)); + + Vector3 expectedVelocity(0, 0, 0); + EXPECT(assert_equal(Vector(expectedVelocity), Vector(poseVelocity.velocity))); +} + +/* ************************************************************************* */ +#include +#include +#include +#include +#include + +TEST(ImuFactor, bodyPSensorWithBias) { + using noiseModel::Diagonal; + typedef imuBias::ConstantBias Bias; + + int numFactors = 80; + Vector6 noiseBetweenBiasSigma; + noiseBetweenBiasSigma << Vector3(2.0e-5, 2.0e-5, 2.0e-5), Vector3(3.0e-6, + 3.0e-6, 3.0e-6); + SharedDiagonal biasNoiseModel = Diagonal::Sigmas(noiseBetweenBiasSigma); + + // Measurements + Vector3 n_gravity(0, 0, -9.81); + Vector3 omegaCoriolis(0, 0, 0); + + // Sensor frame is z-down + // Gyroscope measurement is the angular velocity of sensor w.r.t nav frame in sensor frame + Vector3 measuredOmega(0, 0.01, 0); + // Acc measurement is acceleration of sensor in the sensor frame, when stationary, + // table exerts an equal and opposite force w.r.t gravity + Vector3 measuredAcc(0, 0, -9.81); + + Pose3 body_P_sensor(Rot3::ypr(0, 0, M_PI), Point3()); + + Matrix3 accCov = 1e-7 * I_3x3; + Matrix3 gyroCov = 1e-8 * I_3x3; + Matrix3 integrationCov = 1e-9 * I_3x3; + double deltaT = 0.005; + + // Specify noise values on priors + Vector6 priorNoisePoseSigmas( + (Vector(6) << 0.001, 0.001, 0.001, 0.01, 0.01, 0.01).finished()); + Vector3 priorNoiseVelSigmas((Vector(3) << 0.1, 0.1, 0.1).finished()); + Vector6 priorNoiseBiasSigmas( + (Vector(6) << 0.1, 0.1, 0.1, 0.5e-1, 0.5e-1, 0.5e-1).finished()); + SharedDiagonal priorNoisePose = Diagonal::Sigmas(priorNoisePoseSigmas); + SharedDiagonal priorNoiseVel = Diagonal::Sigmas(priorNoiseVelSigmas); + SharedDiagonal priorNoiseBias = Diagonal::Sigmas(priorNoiseBiasSigmas); + Vector3 zeroVel(0, 0, 0); + + // Create a factor graph with priors on initial pose, vlocity and bias + NonlinearFactorGraph graph; + Values values; + + PriorFactor priorPose(X(0), Pose3(), priorNoisePose); + graph.add(priorPose); + values.insert(X(0), Pose3()); + + PriorFactor priorVel(V(0), zeroVel, priorNoiseVel); + graph.add(priorVel); + values.insert(V(0), zeroVel); + + // The key to this test is that we specify the bias, in the sensor frame, as known a priori + // We also create factors below that encode our assumption that this bias is constant over time + // In theory, after optimization, we should recover that same bias estimate + Bias priorBias(Vector3(0, 0, 0), Vector3(0, 0.01, 0)); // Biases (acc, rot) + PriorFactor priorBiasFactor(B(0), priorBias, priorNoiseBias); + graph.add(priorBiasFactor); + values.insert(B(0), priorBias); + + // Now add IMU factors and bias noise models + Bias zeroBias(Vector3(0, 0, 0), Vector3(0, 0, 0)); + for (int i = 1; i < numFactors; i++) { + ImuFactor::PreintegratedMeasurements pim = + ImuFactor::PreintegratedMeasurements(priorBias, accCov, gyroCov, + integrationCov, true); + for (int j = 0; j < 200; ++j) + pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT, + body_P_sensor); + + // Create factors + graph.add( + ImuFactor(X(i - 1), V(i - 1), X(i), V(i), B(i - 1), pim, n_gravity, + omegaCoriolis)); + graph.add(BetweenFactor(B(i - 1), B(i), zeroBias, biasNoiseModel)); + + values.insert(X(i), Pose3()); + values.insert(V(i), zeroVel); + values.insert(B(i), priorBias); + } + + // Finally, optimize, and get bias at last time step + Values results = LevenbergMarquardtOptimizer(graph, values).optimize(); + Bias biasActual = results.at(B(numFactors - 1)); + + // And compare it with expected value (our prior) + Bias biasExpected(Vector3(0, 0, 0), Vector3(0, 0.01, 0)); + EXPECT(assert_equal(biasExpected, biasActual, 1e-3)); +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/navigation/tests/testNavState.cpp b/gtsam/navigation/tests/testNavState.cpp new file mode 100644 index 000000000..a62ca06a8 --- /dev/null +++ b/gtsam/navigation/tests/testNavState.cpp @@ -0,0 +1,272 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testNavState.cpp + * @brief Unit tests for NavState + * @author Frank Dellaert + * @date July 2015 + */ + +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +static const Rot3 kAttitude = Rot3::RzRyRx(0.1, 0.2, 0.3); +static const Point3 kPosition(1.0, 2.0, 3.0); +static const Pose3 kPose(kAttitude, kPosition); +static const Velocity3 kVelocity(0.4, 0.5, 0.6); +static const NavState kIdentity; +static const NavState kState1(kAttitude, kPosition, kVelocity); +static const Vector3 kOmegaCoriolis(0.02, 0.03, 0.04); +static const Vector3 kGravity(0, 0, 9.81); +static const Vector9 kZeroXi = Vector9::Zero(); + +/* ************************************************************************* */ +TEST(NavState, Constructor) { + boost::function construct = + boost::bind(&NavState::FromPoseVelocity, _1, _2, boost::none, + boost::none); + Matrix aH1, aH2; + EXPECT( + assert_equal(kState1, + NavState::FromPoseVelocity(kPose, kVelocity, aH1, aH2))); + EXPECT(assert_equal(numericalDerivative21(construct, kPose, kVelocity), aH1)); + EXPECT(assert_equal(numericalDerivative22(construct, kPose, kVelocity), aH2)); +} + +/* ************************************************************************* */ +TEST( NavState, Attitude) { + Matrix39 aH, eH; + Rot3 actual = kState1.attitude(aH); + EXPECT(assert_equal(actual, kAttitude)); + eH = numericalDerivative11( + boost::bind(&NavState::attitude, _1, boost::none), kState1); + EXPECT(assert_equal((Matrix )eH, aH)); +} + +/* ************************************************************************* */ +TEST( NavState, Position) { + Matrix39 aH, eH; + Point3 actual = kState1.position(aH); + EXPECT(assert_equal(actual, kPosition)); + eH = numericalDerivative11( + boost::bind(&NavState::position, _1, boost::none), kState1); + EXPECT(assert_equal((Matrix )eH, aH)); +} + +/* ************************************************************************* */ +TEST( NavState, Velocity) { + Matrix39 aH, eH; + Velocity3 actual = kState1.velocity(aH); + EXPECT(assert_equal(actual, kVelocity)); + eH = numericalDerivative11( + boost::bind(&NavState::velocity, _1, boost::none), kState1); + EXPECT(assert_equal((Matrix )eH, aH)); +} + +/* ************************************************************************* */ +TEST( NavState, BodyVelocity) { + Matrix39 aH, eH; + Velocity3 actual = kState1.bodyVelocity(aH); + EXPECT(assert_equal(actual, kAttitude.unrotate(kVelocity))); + eH = numericalDerivative11( + boost::bind(&NavState::bodyVelocity, _1, boost::none), kState1); + EXPECT(assert_equal((Matrix )eH, aH)); +} + +/* ************************************************************************* */ +TEST( NavState, MatrixGroup ) { + // check roundtrip conversion to 7*7 matrix representation + Matrix7 T = kState1.matrix(); + EXPECT(assert_equal(kState1, NavState(T))); + + // check group product agrees with matrix product + NavState state2 = kState1 * kState1; + Matrix T2 = T * T; + EXPECT(assert_equal(state2, NavState(T2))); + EXPECT(assert_equal(T2, state2.matrix())); +} + +/* ************************************************************************* */ +TEST( NavState, Manifold ) { + // Check zero xi + EXPECT(assert_equal(kIdentity, kIdentity.retract(kZeroXi))); + EXPECT(assert_equal(kZeroXi, kIdentity.localCoordinates(kIdentity))); + EXPECT(assert_equal(kState1, kState1.retract(kZeroXi))); + EXPECT(assert_equal(kZeroXi, kState1.localCoordinates(kState1))); + + // Check definition of retract as operating on components separately + Vector9 xi; + xi << 0.1, 0.1, 0.1, 0.2, 0.3, 0.4, -0.1, -0.2, -0.3; + Rot3 drot = Rot3::Expmap(xi.head<3>()); + Point3 dt = Point3(xi.segment<3>(3)); + Velocity3 dvel = Velocity3(-0.1, -0.2, -0.3); + NavState state2 = kState1 * NavState(drot, dt, dvel); + EXPECT(assert_equal(state2, kState1.retract(xi))); + EXPECT(assert_equal(xi, kState1.localCoordinates(state2))); + + // roundtrip from state2 to state3 and back + NavState state3 = state2.retract(xi); + EXPECT(assert_equal(xi, state2.localCoordinates(state3))); + + // Check derivatives for ChartAtOrigin::Retract + Matrix9 aH; + // For zero xi + boost::function Retract = boost::bind( + NavState::ChartAtOrigin::Retract, _1, boost::none); + NavState::ChartAtOrigin::Retract(kZeroXi, aH); + EXPECT(assert_equal(numericalDerivative11(Retract, kZeroXi), aH)); + // For non-zero xi + NavState::ChartAtOrigin::Retract(xi, aH); + EXPECT(assert_equal(numericalDerivative11(Retract, xi), aH)); + + // Check derivatives for ChartAtOrigin::Local + // For zero xi + boost::function Local = boost::bind( + NavState::ChartAtOrigin::Local, _1, boost::none); + NavState::ChartAtOrigin::Local(kIdentity, aH); + EXPECT(assert_equal(numericalDerivative11(Local, kIdentity), aH)); + // For non-zero xi + NavState::ChartAtOrigin::Local(kState1, aH); + EXPECT(assert_equal(numericalDerivative11(Local, kState1), aH)); + + // Check retract derivatives + Matrix9 aH1, aH2; + kState1.retract(xi, aH1, aH2); + boost::function retract = + boost::bind(&NavState::retract, _1, _2, boost::none, boost::none); + EXPECT(assert_equal(numericalDerivative21(retract, kState1, xi), aH1)); + EXPECT(assert_equal(numericalDerivative22(retract, kState1, xi), aH2)); + + // Check localCoordinates derivatives + boost::function local = + boost::bind(&NavState::localCoordinates, _1, _2, boost::none, + boost::none); + // from state1 to state2 + kState1.localCoordinates(state2, aH1, aH2); + EXPECT(assert_equal(numericalDerivative21(local, kState1, state2), aH1)); + EXPECT(assert_equal(numericalDerivative22(local, kState1, state2), aH2)); + // from identity to state2 + kIdentity.localCoordinates(state2, aH1, aH2); + EXPECT(assert_equal(numericalDerivative21(local, kIdentity, state2), aH1)); + EXPECT(assert_equal(numericalDerivative22(local, kIdentity, state2), aH2)); + // from state2 to identity + state2.localCoordinates(kIdentity, aH1, aH2); + EXPECT(assert_equal(numericalDerivative21(local, state2, kIdentity), aH1)); + EXPECT(assert_equal(numericalDerivative22(local, state2, kIdentity), aH2)); +} + +/* ************************************************************************* */ +TEST( NavState, Lie ) { + // Check zero xi + EXPECT(assert_equal(kIdentity, kIdentity.expmap(kZeroXi))); + EXPECT(assert_equal(kZeroXi, kIdentity.logmap(kIdentity))); + EXPECT(assert_equal(kState1, kState1.expmap(kZeroXi))); + EXPECT(assert_equal(kZeroXi, kState1.logmap(kState1))); + + // Expmap/Logmap roundtrip + Vector xi(9); + xi << 0.1, 0.1, 0.1, 0.2, 0.3, 0.4, -0.1, -0.2, -0.3; + NavState state2 = NavState::Expmap(xi); + EXPECT(assert_equal(xi, NavState::Logmap(state2))); + + // roundtrip from state2 to state3 and back + NavState state3 = state2.expmap(xi); + EXPECT(assert_equal(xi, state2.logmap(state3))); + + // For the expmap/logmap (not necessarily expmap/local) -xi goes other way + EXPECT(assert_equal(state2, state3.expmap(-xi))); + EXPECT(assert_equal(xi, -state3.logmap(state2))); +} + +/* ************************************************************************* */ +TEST(NavState, Update) { + Vector3 omega(M_PI / 100.0, 0.0, 0.0); + Vector3 acc(0.1, 0.0, 0.0); + double dt = 10; + Matrix9 aF; + Matrix93 aG1, aG2; + boost::function update = + boost::bind(&NavState::update, _1, _2, _3, dt, boost::none, + boost::none, boost::none); + Vector3 b_acc = kAttitude * acc; + NavState expected(kAttitude.expmap(dt * omega), + kPosition + Point3((kVelocity + b_acc * dt / 2) * dt), + kVelocity + b_acc * dt); + NavState actual = kState1.update(acc, omega, dt, aF, aG1, aG2); + EXPECT(assert_equal(expected, actual)); + EXPECT(assert_equal(numericalDerivative31(update, kState1, acc, omega, 1e-7), aF, 1e-7)); + EXPECT(assert_equal(numericalDerivative32(update, kState1, acc, omega, 1e-7), aG1, 1e-7)); + EXPECT(assert_equal(numericalDerivative33(update, kState1, acc, omega, 1e-7), aG2, 1e-7)); + + // Try different values + omega = Vector3(0.1, 0.2, 0.3); + acc = Vector3(0.4, 0.5, 0.6); + kState1.update(acc, omega, dt, aF, aG1, aG2); + EXPECT(assert_equal(numericalDerivative31(update, kState1, acc, omega, 1e-7), aF, 1e-7)); + EXPECT(assert_equal(numericalDerivative32(update, kState1, acc, omega, 1e-7), aG1, 1e-7)); + EXPECT(assert_equal(numericalDerivative33(update, kState1, acc, omega, 1e-7), aG2, 1e-7)); +} + +/* ************************************************************************* */ +static const double dt = 2.0; +boost::function coriolis = boost::bind( + &NavState::coriolis, _1, dt, kOmegaCoriolis, _2, boost::none); + +TEST(NavState, Coriolis) { + Matrix9 aH; + + // first-order + kState1.coriolis(dt, kOmegaCoriolis, false, aH); + EXPECT(assert_equal(numericalDerivative21(coriolis, kState1, false), aH)); + // second-order + kState1.coriolis(dt, kOmegaCoriolis, true, aH); + EXPECT(assert_equal(numericalDerivative21(coriolis, kState1, true), aH)); +} + +TEST(NavState, Coriolis2) { + Matrix9 aH; + NavState state2(Rot3::RzRyRx(M_PI / 12.0, M_PI / 6.0, M_PI / 4.0), + Point3(5.0, 1.0, -50.0), Vector3(0.5, 0.0, 0.0)); + + // first-order + state2.coriolis(dt, kOmegaCoriolis, false, aH); + EXPECT(assert_equal(numericalDerivative21(coriolis, state2, false), aH)); + // second-order + state2.coriolis(dt, kOmegaCoriolis, true, aH); + EXPECT(assert_equal(numericalDerivative21(coriolis, state2, true), aH)); +} + +/* ************************************************************************* */ +TEST(NavState, CorrectPIM) { + Vector9 xi; + xi << 0.1, 0.1, 0.1, 0.2, 0.3, 0.4, -0.1, -0.2, -0.3; + double dt = 0.5; + Matrix9 aH1, aH2; + boost::function correctPIM = + boost::bind(&NavState::correctPIM, _1, _2, dt, kGravity, kOmegaCoriolis, + false, boost::none, boost::none); + kState1.correctPIM(xi, dt, kGravity, kOmegaCoriolis, false, aH1, aH2); + EXPECT(assert_equal(numericalDerivative21(correctPIM, kState1, xi), aH1)); + EXPECT(assert_equal(numericalDerivative22(correctPIM, kState1, xi), aH2)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ diff --git a/gtsam/navigation/tests/testPoseVelocityBias.cpp b/gtsam/navigation/tests/testPoseVelocityBias.cpp new file mode 100644 index 000000000..ce419fdd0 --- /dev/null +++ b/gtsam/navigation/tests/testPoseVelocityBias.cpp @@ -0,0 +1,64 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testPoseVelocityBias.cpp + * @brief Unit test for PoseVelocityBias + * @author Frank Dellaert + */ + +#include +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +// Should be seen as between(pvb1,pvb2), i.e., written as pvb2 \omin pvb1 +Vector9 error(const PoseVelocityBias& pvb1, const PoseVelocityBias& pvb2) { + Matrix3 R1 = pvb1.pose.rotation().matrix(); + // Ri.transpose() translate the error from the global frame into pose1's frame + const Vector3 fp = R1.transpose() * (pvb2.pose.translation() - pvb1.pose.translation()).vector(); + const Vector3 fv = R1.transpose() * (pvb2.velocity - pvb1.velocity); + const Rot3 R1BetweenR2 = pvb1.pose.rotation().between(pvb2.pose.rotation()); + const Vector3 fR = Rot3::Logmap(R1BetweenR2); + Vector9 r; + r << fp, fv, fR; + return r; +} + +/* ************************************************************************************************/ +TEST(PoseVelocityBias, error) { + Point3 i1(0, 1, 0), j1(-1, 0, 0), k1(0, 0, 1); + Pose3 x1(Rot3(i1, j1, k1), Point3(5.0, 1.0, 0.0)); + Vector3 v1(Vector3(0.5, 0.0, 0.0)); + imuBias::ConstantBias bias1(Vector3(0.2, 0, 0), Vector3(0.1, 0, 0.3)); + + Pose3 x2(Rot3(i1, j1, k1).expmap(Vector3(0.1, 0.2, 0.3)), Point3(5.5, 1.0, 6.0)); + Vector3 v2(Vector3(0.5, 4.0, 3.0)); + imuBias::ConstantBias bias2(Vector3(0.1, 0.2, -0.3), Vector3(0.2, 0.3, 0.1)); + + PoseVelocityBias pvb1(x1, v1, bias1), pvb2(x2, v2, bias2); + + Vector9 expected, actual = error(pvb1, pvb2); + expected << 0.0, -0.5, 6.0, 4.0, 0.0, 3.0, 0.1, 0.2, 0.3; + EXPECT(assert_equal(expected, actual, 1e-9)); +} + +/* ************************************************************************************************/ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************************************/ diff --git a/gtsam/nonlinear/factorTesting.h b/gtsam/nonlinear/factorTesting.h index 52f103630..2a9d70cb4 100644 --- a/gtsam/nonlinear/factorTesting.h +++ b/gtsam/nonlinear/factorTesting.h @@ -85,9 +85,23 @@ bool testFactorJacobians(TestResult& result_, const std::string& name_, // Create actual value by linearize boost::shared_ptr actual = // boost::dynamic_pointer_cast(factor.linearize(values)); + if (!actual) return false; // Check cast result and then equality - return actual && assert_equal(expected, *actual, tolerance); + bool equal = assert_equal(expected, *actual, tolerance); + + // if not equal, test individual jacobians: + if (!equal) { + for (size_t i = 0; i < actual->size(); i++) { + bool i_good = assert_equal((Matrix) (expected.getA(expected.begin() + i)), + (Matrix) (actual->getA(actual->begin() + i)), tolerance); + if (!i_good) { + std::cout << "Mismatch in Jacobian " << i+1 << " (base 1), as shown above" << std::endl; + } + } + } + + return equal; } } diff --git a/gtsam_unstable/dynamics/PoseRTV.cpp b/gtsam_unstable/dynamics/PoseRTV.cpp index 942e1ab55..51737285a 100644 --- a/gtsam_unstable/dynamics/PoseRTV.cpp +++ b/gtsam_unstable/dynamics/PoseRTV.cpp @@ -38,13 +38,14 @@ Vector PoseRTV::vector() const { Vector rtv(9); rtv.head(3) = rotation().xyz(); rtv.segment(3,3) = translation().vector(); - rtv.tail(3) = velocity().vector(); + rtv.tail(3) = velocity(); return rtv; } /* ************************************************************************* */ bool PoseRTV::equals(const PoseRTV& other, double tol) const { - return pose().equals(other.pose(), tol) && velocity().equals(other.velocity(), tol); + return pose().equals(other.pose(), tol) + && equal_with_abs_tol(velocity(), other.velocity(), tol); } /* ************************************************************************* */ @@ -52,7 +53,7 @@ void PoseRTV::print(const string& s) const { cout << s << ":" << endl; gtsam::print((Vector)R().xyz(), " R:rpy"); t().print(" T"); - velocity().print(" V"); + gtsam::print((Vector)velocity(), " V"); } /* ************************************************************************* */ @@ -137,7 +138,7 @@ Vector6 PoseRTV::imuPrediction(const PoseRTV& x2, double dt) const { Vector6 imu; // acceleration - Vector3 accel = (v2-v1).vector() / dt; + Vector3 accel = (v2-v1) / dt; imu.head<3>() = r2.transpose() * (accel - kGravity); // rotation rates @@ -160,7 +161,7 @@ Vector6 PoseRTV::imuPrediction(const PoseRTV& x2, double dt) const { Point3 PoseRTV::translationIntegration(const Rot3& r2, const Velocity3& v2, double dt) const { // predict point for constraint // NOTE: uses simple Euler approach for prediction - Point3 pred_t2 = t() + v2 * dt; + Point3 pred_t2 = t().retract(v2 * dt); return pred_t2; } @@ -187,7 +188,7 @@ PoseRTV PoseRTV::transformed_from(const Pose3& trans, ChartJacobian Dglobal, // Note that we rotate the velocity Matrix3 D_newvel_R, D_newvel_v; - Velocity3 newvel = trans.rotation().rotate(velocity(), D_newvel_R, D_newvel_v); + Velocity3 newvel = trans.rotation().rotate(Point3(velocity()), D_newvel_R, D_newvel_v).vector(); if (Dglobal) { Dglobal->setZero(); @@ -204,10 +205,10 @@ PoseRTV PoseRTV::transformed_from(const Pose3& trans, ChartJacobian Dglobal, } /* ************************************************************************* */ -Matrix PoseRTV::RRTMbn(const Vector& euler) { +Matrix PoseRTV::RRTMbn(const Vector3& euler) { assert(euler.size() == 3); - const double s1 = sin(euler(1-1)), c1 = cos(euler(1-1)); - const double t2 = tan(euler(2-1)), c2 = cos(euler(2-1)); + const double s1 = sin(euler.x()), c1 = cos(euler.x()); + const double t2 = tan(euler.y()), c2 = cos(euler.y()); Matrix Ebn(3,3); Ebn << 1.0, s1 * t2, c1 * t2, 0.0, c1, -s1, @@ -221,11 +222,10 @@ Matrix PoseRTV::RRTMbn(const Rot3& att) { } /* ************************************************************************* */ -Matrix PoseRTV::RRTMnb(const Vector& euler) { - assert(euler.size() == 3); +Matrix PoseRTV::RRTMnb(const Vector3& euler) { Matrix Enb(3,3); - const double s1 = sin(euler(1-1)), c1 = cos(euler(1-1)); - const double s2 = sin(euler(2-1)), c2 = cos(euler(2-1)); + const double s1 = sin(euler.x()), c1 = cos(euler.x()); + const double s2 = sin(euler.y()), c2 = cos(euler.y()); Enb << 1.0, 0.0, -s2, 0.0, c1, s1*c2, 0.0, -s1, c1*c2; diff --git a/gtsam_unstable/dynamics/PoseRTV.h b/gtsam_unstable/dynamics/PoseRTV.h index a6bc6006a..b59ea4614 100644 --- a/gtsam_unstable/dynamics/PoseRTV.h +++ b/gtsam_unstable/dynamics/PoseRTV.h @@ -13,11 +13,12 @@ namespace gtsam { /// Syntactic sugar to clarify components -typedef Point3 Velocity3; +typedef Vector3 Velocity3; /** * Robot state for use with IMU measurements * - contains translation, translational velocity and rotation + * TODO(frank): Alex should deprecate/move to project */ class GTSAM_UNSTABLE_EXPORT PoseRTV : public ProductLieGroup { protected: @@ -34,11 +35,11 @@ public: PoseRTV(const Rot3& rot, const Point3& t, const Velocity3& vel) : Base(Pose3(rot, t), vel) {} explicit PoseRTV(const Point3& t) - : Base(Pose3(Rot3(), t),Velocity3()) {} + : Base(Pose3(Rot3(), t),Vector3::Zero()) {} PoseRTV(const Pose3& pose, const Velocity3& vel) : Base(pose, vel) {} explicit PoseRTV(const Pose3& pose) - : Base(pose,Velocity3()) {} + : Base(pose,Vector3::Zero()) {} // Construct from Base PoseRTV(const Base& base) @@ -66,7 +67,7 @@ public: // and avoidance of Point3 Vector vector() const; Vector translationVec() const { return pose().translation().vector(); } - Vector velocityVec() const { return velocity().vector(); } + const Velocity3& velocityVec() const { return velocity(); } // testable bool equals(const PoseRTV& other, double tol=1e-6) const; @@ -145,14 +146,12 @@ public: /// RRTMbn - Function computes the rotation rate transformation matrix from /// body axis rates to euler angle (global) rates - static Matrix RRTMbn(const Vector& euler); - + static Matrix RRTMbn(const Vector3& euler); static Matrix RRTMbn(const Rot3& att); /// RRTMnb - Function computes the rotation rate transformation matrix from /// euler angle rates to body axis rates - static Matrix RRTMnb(const Vector& euler); - + static Matrix RRTMnb(const Vector3& euler); static Matrix RRTMnb(const Rot3& att); /// @} @@ -173,14 +172,7 @@ struct traits : public internal::LieGroup {}; // Define Range functor specializations that are used in RangeFactor template struct Range; -template <> -struct Range { - typedef double result_type; - double operator()(const PoseRTV& pose1, const PoseRTV& pose2, - OptionalJacobian<1, 9> H1 = boost::none, - OptionalJacobian<1, 9> H2 = boost::none) { - return pose1.range(pose2, H1, H2); - } -}; +template<> +struct Range : HasRange {}; } // \namespace gtsam diff --git a/gtsam_unstable/dynamics/VelocityConstraint.h b/gtsam_unstable/dynamics/VelocityConstraint.h index c9db449f9..c41ea220c 100644 --- a/gtsam_unstable/dynamics/VelocityConstraint.h +++ b/gtsam_unstable/dynamics/VelocityConstraint.h @@ -106,12 +106,13 @@ private: static gtsam::Vector evaluateError_(const PoseRTV& x1, const PoseRTV& x2, double dt, const dynamics::IntegrationMode& mode) { - const Velocity3& v1 = x1.v(), v2 = x2.v(), p1 = x1.t(), p2 = x2.t(); - Velocity3 hx; + const Velocity3& v1 = x1.v(), v2 = x2.v(); + const Point3& p1 = x1.t(), p2 = x2.t(); + Point3 hx; switch(mode) { - case dynamics::TRAPEZOIDAL: hx = p1 + (v1 + v2) * dt *0.5; break; - case dynamics::EULER_START: hx = p1 + v1 * dt; break; - case dynamics::EULER_END : hx = p1 + v2 * dt; break; + case dynamics::TRAPEZOIDAL: hx = p1.retract((v1 + v2) * dt *0.5); break; + case dynamics::EULER_START: hx = p1.retract(v1 * dt); break; + case dynamics::EULER_END : hx = p1.retract(v2 * dt); break; default: assert(false); break; } return (p2 - hx).vector(); diff --git a/gtsam_unstable/dynamics/tests/testIMUSystem.cpp b/gtsam_unstable/dynamics/tests/testIMUSystem.cpp index 51d027b3c..3fff06de1 100644 --- a/gtsam_unstable/dynamics/tests/testIMUSystem.cpp +++ b/gtsam_unstable/dynamics/tests/testIMUSystem.cpp @@ -56,9 +56,9 @@ TEST( testIMUSystem, optimize_chain ) { // create a simple chain of poses to generate IMU measurements const double dt = 1.0; PoseRTV pose1, - pose2(Point3(1.0, 1.0, 0.0), Rot3::ypr(0.1, 0.0, 0.0), Point3(2.0, 2.0, 0.0)), - pose3(Point3(2.0, 2.0, 0.0), Rot3::ypr(0.2, 0.0, 0.0), Point3(0.0, 0.0, 0.0)), - pose4(Point3(3.0, 3.0, 0.0), Rot3::ypr(0.3, 0.0, 0.0), Point3(2.0, 2.0, 0.0)); + pose2(Point3(1.0, 1.0, 0.0), Rot3::ypr(0.1, 0.0, 0.0), Velocity3(2.0, 2.0, 0.0)), + pose3(Point3(2.0, 2.0, 0.0), Rot3::ypr(0.2, 0.0, 0.0), Velocity3(0.0, 0.0, 0.0)), + pose4(Point3(3.0, 3.0, 0.0), Rot3::ypr(0.3, 0.0, 0.0), Velocity3(2.0, 2.0, 0.0)); // create measurements SharedDiagonal model = noiseModel::Unit::Create(6); @@ -102,9 +102,9 @@ TEST( testIMUSystem, optimize_chain_fullfactor ) { // create a simple chain of poses to generate IMU measurements const double dt = 1.0; PoseRTV pose1, - pose2(Point3(1.0, 0.0, 0.0), Rot3::ypr(0.0, 0.0, 0.0), Point3(1.0, 0.0, 0.0)), - pose3(Point3(2.0, 0.0, 0.0), Rot3::ypr(0.0, 0.0, 0.0), Point3(1.0, 0.0, 0.0)), - pose4(Point3(3.0, 0.0, 0.0), Rot3::ypr(0.0, 0.0, 0.0), Point3(1.0, 0.0, 0.0)); + pose2(Point3(1.0, 0.0, 0.0), Rot3::ypr(0.0, 0.0, 0.0), Velocity3(1.0, 0.0, 0.0)), + pose3(Point3(2.0, 0.0, 0.0), Rot3::ypr(0.0, 0.0, 0.0), Velocity3(1.0, 0.0, 0.0)), + pose4(Point3(3.0, 0.0, 0.0), Rot3::ypr(0.0, 0.0, 0.0), Velocity3(1.0, 0.0, 0.0)); // create measurements SharedDiagonal model = noiseModel::Isotropic::Sigma(9, 1.0); diff --git a/gtsam_unstable/dynamics/tests/testPoseRTV.cpp b/gtsam_unstable/dynamics/tests/testPoseRTV.cpp index 5b052eb02..0386d8bcd 100644 --- a/gtsam_unstable/dynamics/tests/testPoseRTV.cpp +++ b/gtsam_unstable/dynamics/tests/testPoseRTV.cpp @@ -15,44 +15,43 @@ using namespace gtsam; GTSAM_CONCEPT_TESTABLE_INST(PoseRTV) GTSAM_CONCEPT_LIE_INST(PoseRTV) -const double tol=1e-5; - -Rot3 rot = Rot3::RzRyRx(0.1, 0.2, 0.3); -Point3 pt(1.0, 2.0, 3.0); -Velocity3 vel(0.4, 0.5, 0.6); +static const Rot3 rot = Rot3::RzRyRx(0.1, 0.2, 0.3); +static const Point3 pt(1.0, 2.0, 3.0); +static const Velocity3 vel(0.4, 0.5, 0.6); +static const Vector3 kZero3 = Vector3::Zero(); /* ************************************************************************* */ TEST( testPoseRTV, constructors ) { PoseRTV state1(pt, rot, vel); - EXPECT(assert_equal(pt, state1.t(), tol)); - EXPECT(assert_equal(rot, state1.R(), tol)); - EXPECT(assert_equal(vel, state1.v(), tol)); - EXPECT(assert_equal(Pose3(rot, pt), state1.pose(), tol)); + EXPECT(assert_equal(pt, state1.t())); + EXPECT(assert_equal(rot, state1.R())); + EXPECT(assert_equal(vel, state1.v())); + EXPECT(assert_equal(Pose3(rot, pt), state1.pose())); PoseRTV state2; - EXPECT(assert_equal(Point3(), state2.t(), tol)); - EXPECT(assert_equal(Rot3(), state2.R(), tol)); - EXPECT(assert_equal(Velocity3(), state2.v(), tol)); - EXPECT(assert_equal(Pose3(), state2.pose(), tol)); + EXPECT(assert_equal(Point3(), state2.t())); + EXPECT(assert_equal(Rot3(), state2.R())); + EXPECT(assert_equal(kZero3, state2.v())); + EXPECT(assert_equal(Pose3(), state2.pose())); PoseRTV state3(Pose3(rot, pt), vel); - EXPECT(assert_equal(pt, state3.t(), tol)); - EXPECT(assert_equal(rot, state3.R(), tol)); - EXPECT(assert_equal(vel, state3.v(), tol)); - EXPECT(assert_equal(Pose3(rot, pt), state3.pose(), tol)); + EXPECT(assert_equal(pt, state3.t())); + EXPECT(assert_equal(rot, state3.R())); + EXPECT(assert_equal(vel, state3.v())); + EXPECT(assert_equal(Pose3(rot, pt), state3.pose())); PoseRTV state4(Pose3(rot, pt)); - EXPECT(assert_equal(pt, state4.t(), tol)); - EXPECT(assert_equal(rot, state4.R(), tol)); - EXPECT(assert_equal(Velocity3(), state4.v(), tol)); - EXPECT(assert_equal(Pose3(rot, pt), state4.pose(), tol)); + EXPECT(assert_equal(pt, state4.t())); + EXPECT(assert_equal(rot, state4.R())); + EXPECT(assert_equal(kZero3, state4.v())); + EXPECT(assert_equal(Pose3(rot, pt), state4.pose())); Vector vec_init = (Vector(9) << 0.1, 0.2, 0.3, 1.0, 2.0, 3.0, 0.4, 0.5, 0.6).finished(); PoseRTV state5(vec_init); - EXPECT(assert_equal(pt, state5.t(), tol)); - EXPECT(assert_equal(rot, state5.R(), tol)); - EXPECT(assert_equal(vel, state5.v(), tol)); - EXPECT(assert_equal(vec_init, state5.vector(), tol)); + EXPECT(assert_equal(pt, state5.t())); + EXPECT(assert_equal(rot, state5.R())); + EXPECT(assert_equal(vel, state5.v())); + EXPECT(assert_equal(vec_init, state5.vector())); } /* ************************************************************************* */ @@ -65,33 +64,44 @@ TEST( testPoseRTV, dim ) { /* ************************************************************************* */ TEST( testPoseRTV, equals ) { PoseRTV state1, state2(pt, rot, vel), state3(state2), state4(Pose3(rot, pt)); - EXPECT(assert_equal(state1, state1, tol)); - EXPECT(assert_equal(state2, state3, tol)); - EXPECT(assert_equal(state3, state2, tol)); - EXPECT(assert_inequal(state1, state2, tol)); - EXPECT(assert_inequal(state2, state1, tol)); - EXPECT(assert_inequal(state2, state4, tol)); + EXPECT(assert_equal(state1, state1)); + EXPECT(assert_equal(state2, state3)); + EXPECT(assert_equal(state3, state2)); + EXPECT(assert_inequal(state1, state2)); + EXPECT(assert_inequal(state2, state1)); + EXPECT(assert_inequal(state2, state4)); } /* ************************************************************************* */ TEST( testPoseRTV, Lie ) { // origin and zero deltas PoseRTV identity; - EXPECT(assert_equal(identity, (PoseRTV)identity.retract(zero(9)), tol)); - EXPECT(assert_equal(zero(9), identity.localCoordinates(identity), tol)); + EXPECT(assert_equal(identity, (PoseRTV)identity.retract(zero(9)))); + EXPECT(assert_equal(zero(9), identity.localCoordinates(identity))); PoseRTV state1(pt, rot, vel); - EXPECT(assert_equal(state1, (PoseRTV)state1.retract(zero(9)), tol)); - EXPECT(assert_equal(zero(9), state1.localCoordinates(state1), tol)); + EXPECT(assert_equal(state1, (PoseRTV)state1.retract(zero(9)))); + EXPECT(assert_equal(zero(9), state1.localCoordinates(state1))); - Vector delta = (Vector(9) << 0.1, 0.1, 0.1, 0.2, 0.3, 0.4,-0.1,-0.2,-0.3).finished(); - Rot3 rot2 = rot.retract(repeat(3, 0.1)); - Point3 pt2 = pt + rot * Point3(0.2, 0.3, 0.4); - Velocity3 vel2 = vel + rot * Velocity3(-0.1,-0.2,-0.3); - PoseRTV state2(pt2, rot2, vel2); - EXPECT(assert_equal(state2, (PoseRTV)state1.retract(delta), 1e-1)); - EXPECT(assert_equal(delta, state1.localCoordinates(state2), 1e-1)); - EXPECT(assert_equal(delta, -state2.localCoordinates(state1), 1e-1)); + Vector delta(9); + delta << 0.1, 0.1, 0.1, 0.2, 0.3, 0.4,-0.1,-0.2,-0.3; + Pose3 pose2 = Pose3(rot, pt).retract(delta.head<6>()); + Velocity3 vel2 = vel + Velocity3(-0.1, -0.2, -0.3); + PoseRTV state2(pose2.translation(), pose2.rotation(), vel2); + EXPECT(assert_equal(state2, (PoseRTV)state1.retract(delta))); + EXPECT(assert_equal(delta, state1.localCoordinates(state2))); + + // roundtrip from state2 to state3 and back + PoseRTV state3 = state2.retract(delta); + EXPECT(assert_equal(delta, state2.localCoordinates(state3))); + + // roundtrip from state3 to state4 and back, with expmap. + PoseRTV state4 = state3.expmap(delta); + EXPECT(assert_equal(delta, state3.logmap(state4))); + + // For the expmap/logmap (not necessarily retract/local) -delta goes other way + EXPECT(assert_equal(state3, (PoseRTV)state4.expmap(-delta))); + EXPECT(assert_equal(delta, -state4.logmap(state3))); } /* ************************************************************************* */ @@ -108,15 +118,15 @@ TEST( testPoseRTV, dynamics_identities ) { x3 = x2.generalDynamics(accel, gyro, dt); x4 = x3.generalDynamics(accel, gyro, dt); -// EXPECT(assert_equal(imu01, x0.imuPrediction(x1, dt).first, tol)); -// EXPECT(assert_equal(imu12, x1.imuPrediction(x2, dt).first, tol)); -// EXPECT(assert_equal(imu23, x2.imuPrediction(x3, dt).first, tol)); -// EXPECT(assert_equal(imu34, x3.imuPrediction(x4, dt).first, tol)); +// EXPECT(assert_equal(imu01, x0.imuPrediction(x1, dt).first)); +// EXPECT(assert_equal(imu12, x1.imuPrediction(x2, dt).first)); +// EXPECT(assert_equal(imu23, x2.imuPrediction(x3, dt).first)); +// EXPECT(assert_equal(imu34, x3.imuPrediction(x4, dt).first)); // -// EXPECT(assert_equal(x1.translation(), x0.imuPrediction(x1, dt).second, tol)); -// EXPECT(assert_equal(x2.translation(), x1.imuPrediction(x2, dt).second, tol)); -// EXPECT(assert_equal(x3.translation(), x2.imuPrediction(x3, dt).second, tol)); -// EXPECT(assert_equal(x4.translation(), x3.imuPrediction(x4, dt).second, tol)); +// EXPECT(assert_equal(x1.translation(), x0.imuPrediction(x1, dt).second)); +// EXPECT(assert_equal(x2.translation(), x1.imuPrediction(x2, dt).second)); +// EXPECT(assert_equal(x3.translation(), x2.imuPrediction(x3, dt).second)); +// EXPECT(assert_equal(x4.translation(), x3.imuPrediction(x4, dt).second)); } @@ -129,8 +139,8 @@ TEST( testPoseRTV, compose ) { state1.compose(state2, actH1, actH2); Matrix numericH1 = numericalDerivative21(compose_proxy, state1, state2); Matrix numericH2 = numericalDerivative22(compose_proxy, state1, state2); - EXPECT(assert_equal(numericH1, actH1, tol)); - EXPECT(assert_equal(numericH2, actH2, tol)); + EXPECT(assert_equal(numericH1, actH1)); + EXPECT(assert_equal(numericH2, actH2)); } /* ************************************************************************* */ @@ -142,8 +152,8 @@ TEST( testPoseRTV, between ) { state1.between(state2, actH1, actH2); Matrix numericH1 = numericalDerivative21(between_proxy, state1, state2); Matrix numericH2 = numericalDerivative22(between_proxy, state1, state2); - EXPECT(assert_equal(numericH1, actH1, tol)); - EXPECT(assert_equal(numericH2, actH2, tol)); + EXPECT(assert_equal(numericH1, actH1)); + EXPECT(assert_equal(numericH2, actH2)); } /* ************************************************************************* */ @@ -154,7 +164,7 @@ TEST( testPoseRTV, inverse ) { Matrix actH1; state1.inverse(actH1); Matrix numericH1 = numericalDerivative11(inverse_proxy, state1); - EXPECT(assert_equal(numericH1, actH1, tol)); + EXPECT(assert_equal(numericH1, actH1)); } /* ************************************************************************* */ @@ -162,16 +172,16 @@ double range_proxy(const PoseRTV& A, const PoseRTV& B) { return A.range(B); } TEST( testPoseRTV, range ) { Point3 tA(1.0, 2.0, 3.0), tB(3.0, 2.0, 3.0); PoseRTV rtvA(tA), rtvB(tB); - EXPECT_DOUBLES_EQUAL(0.0, rtvA.range(rtvA), tol); - EXPECT_DOUBLES_EQUAL(2.0, rtvA.range(rtvB), tol); - EXPECT_DOUBLES_EQUAL(2.0, rtvB.range(rtvA), tol); + EXPECT_DOUBLES_EQUAL(0.0, rtvA.range(rtvA), 1e-9); + EXPECT_DOUBLES_EQUAL(2.0, rtvA.range(rtvB), 1e-9); + EXPECT_DOUBLES_EQUAL(2.0, rtvB.range(rtvA), 1e-9); Matrix actH1, actH2; rtvA.range(rtvB, actH1, actH2); Matrix numericH1 = numericalDerivative21(range_proxy, rtvA, rtvB); Matrix numericH2 = numericalDerivative22(range_proxy, rtvA, rtvB); - EXPECT(assert_equal(numericH1, actH1, tol)); - EXPECT(assert_equal(numericH2, actH2, tol)); + EXPECT(assert_equal(numericH1, actH1)); + EXPECT(assert_equal(numericH2, actH2)); } /* ************************************************************************* */ @@ -187,13 +197,13 @@ TEST( testPoseRTV, transformed_from_1 ) { Matrix actDTrans, actDGlobal; PoseRTV actual = start.transformed_from(transform, actDGlobal, actDTrans); - PoseRTV expected(transform.compose(start.pose()), transform.rotation().rotate(V)); - EXPECT(assert_equal(expected, actual, tol)); + PoseRTV expected(transform.compose(start.pose()), transform.rotation().matrix() * V); + EXPECT(assert_equal(expected, actual)); Matrix numDGlobal = numericalDerivative21(transformed_from_proxy, start, transform, 1e-5); // At 1e-8, fails Matrix numDTrans = numericalDerivative22(transformed_from_proxy, start, transform, 1e-8); // Sensitive to step size - EXPECT(assert_equal(numDGlobal, actDGlobal, tol)); - EXPECT(assert_equal(numDTrans, actDTrans, tol)); // FIXME: still needs analytic derivative + EXPECT(assert_equal(numDGlobal, actDGlobal)); + EXPECT(assert_equal(numDTrans, actDTrans, 1e-5)); // FIXME: still needs analytic derivative } /* ************************************************************************* */ @@ -206,27 +216,27 @@ TEST( testPoseRTV, transformed_from_2 ) { Matrix actDTrans, actDGlobal; PoseRTV actual = start.transformed_from(transform, actDGlobal, actDTrans); - PoseRTV expected(transform.compose(start.pose()), transform.rotation().rotate(V)); - EXPECT(assert_equal(expected, actual, tol)); + PoseRTV expected(transform.compose(start.pose()), transform.rotation().matrix() * V); + EXPECT(assert_equal(expected, actual)); Matrix numDGlobal = numericalDerivative21(transformed_from_proxy, start, transform, 1e-5); // At 1e-8, fails Matrix numDTrans = numericalDerivative22(transformed_from_proxy, start, transform, 1e-8); // Sensitive to step size - EXPECT(assert_equal(numDGlobal, actDGlobal, tol)); - EXPECT(assert_equal(numDTrans, actDTrans, tol)); // FIXME: still needs analytic derivative + EXPECT(assert_equal(numDGlobal, actDGlobal)); + EXPECT(assert_equal(numDTrans, actDTrans, 1e-5)); // FIXME: still needs analytic derivative } /* ************************************************************************* */ TEST(testPoseRTV, RRTMbn) { - EXPECT(assert_equal(Matrix::Identity(3,3), PoseRTV::RRTMbn(zero(3)), tol)); - EXPECT(assert_equal(Matrix::Identity(3,3), PoseRTV::RRTMbn(Rot3()), tol)); - EXPECT(assert_equal(PoseRTV::RRTMbn(Vector3(0.3, 0.2, 0.1)), PoseRTV::RRTMbn(Rot3::ypr(0.1, 0.2, 0.3)), tol)); + EXPECT(assert_equal(Matrix::Identity(3,3), PoseRTV::RRTMbn(kZero3))); + EXPECT(assert_equal(Matrix::Identity(3,3), PoseRTV::RRTMbn(Rot3()))); + EXPECT(assert_equal(PoseRTV::RRTMbn(Vector3(0.3, 0.2, 0.1)), PoseRTV::RRTMbn(Rot3::ypr(0.1, 0.2, 0.3)))); } /* ************************************************************************* */ TEST(testPoseRTV, RRTMnb) { - EXPECT(assert_equal(Matrix::Identity(3,3), PoseRTV::RRTMnb(zero(3)), tol)); - EXPECT(assert_equal(Matrix::Identity(3,3), PoseRTV::RRTMnb(Rot3()), tol)); - EXPECT(assert_equal(PoseRTV::RRTMnb(Vector3(0.3, 0.2, 0.1)), PoseRTV::RRTMnb(Rot3::ypr(0.1, 0.2, 0.3)), tol)); + EXPECT(assert_equal(Matrix::Identity(3,3), PoseRTV::RRTMnb(kZero3))); + EXPECT(assert_equal(Matrix::Identity(3,3), PoseRTV::RRTMnb(Rot3()))); + EXPECT(assert_equal(PoseRTV::RRTMnb(Vector3(0.3, 0.2, 0.1)), PoseRTV::RRTMnb(Rot3::ypr(0.1, 0.2, 0.3)))); } /* ************************************************************************* */ diff --git a/gtsam_unstable/dynamics/tests/testVelocityConstraint.cpp b/gtsam_unstable/dynamics/tests/testVelocityConstraint.cpp index 56d38714a..f253be371 100644 --- a/gtsam_unstable/dynamics/tests/testVelocityConstraint.cpp +++ b/gtsam_unstable/dynamics/tests/testVelocityConstraint.cpp @@ -15,9 +15,9 @@ const Key x1 = 1, x2 = 2; const double dt = 1.0; PoseRTV origin, - pose1(Point3(0.5, 0.0, 0.0), Rot3::identity(), Point3(1.0, 0.0, 0.0)), + pose1(Point3(0.5, 0.0, 0.0), Rot3::identity(), Velocity3(1.0, 0.0, 0.0)), pose1a(Point3(0.5, 0.0, 0.0)), - pose2(Point3(1.5, 0.0, 0.0), Rot3::identity(), Point3(1.0, 0.0, 0.0)); + pose2(Point3(1.5, 0.0, 0.0), Rot3::identity(), Velocity3(1.0, 0.0, 0.0)); /* ************************************************************************* */ TEST( testVelocityConstraint, trapezoidal ) { diff --git a/gtsam_unstable/geometry/Event.cpp b/gtsam_unstable/geometry/Event.cpp index 5248241e1..dda2c32e6 100644 --- a/gtsam_unstable/geometry/Event.cpp +++ b/gtsam_unstable/geometry/Event.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) diff --git a/gtsam_unstable/gtsam_unstable.h b/gtsam_unstable/gtsam_unstable.h index bbfcaa418..99b33182f 100644 --- a/gtsam_unstable/gtsam_unstable.h +++ b/gtsam_unstable/gtsam_unstable.h @@ -53,9 +53,9 @@ class Dummy { class PoseRTV { PoseRTV(); PoseRTV(Vector rtv); - PoseRTV(const gtsam::Point3& pt, const gtsam::Rot3& rot, const gtsam::Point3& vel); - PoseRTV(const gtsam::Rot3& rot, const gtsam::Point3& pt, const gtsam::Point3& vel); - PoseRTV(const gtsam::Pose3& pose, const gtsam::Point3& vel); + PoseRTV(const gtsam::Point3& pt, const gtsam::Rot3& rot, const Vector& vel); + PoseRTV(const gtsam::Rot3& rot, const gtsam::Point3& pt, const Vector& vel); + PoseRTV(const gtsam::Pose3& pose, const Vector& vel); PoseRTV(const gtsam::Pose3& pose); PoseRTV(double roll, double pitch, double yaw, double x, double y, double z, double vx, double vy, double vz); @@ -66,7 +66,7 @@ class PoseRTV { // access gtsam::Point3 translation() const; gtsam::Rot3 rotation() const; - gtsam::Point3 velocity() const; + Vector velocity() const; gtsam::Pose3 pose() const; // Vector interfaces diff --git a/gtsam_unstable/slam/Mechanization_bRn2.cpp b/gtsam_unstable/slam/Mechanization_bRn2.cpp index d2dd02dd3..f6f1c4a5c 100644 --- a/gtsam_unstable/slam/Mechanization_bRn2.cpp +++ b/gtsam_unstable/slam/Mechanization_bRn2.cpp @@ -101,7 +101,7 @@ Mechanization_bRn2 Mechanization_bRn2::integrate(const Vector3& u, // convert to navigation frame Rot3 nRb = bRn_.inverse(); - Vector3 n_omega_bn = (nRb*b_omega_bn).vector(); + Vector3 n_omega_bn = nRb * b_omega_bn; // integrate bRn using exponential map, assuming constant over dt Rot3 delta_nRn = Rot3::Rodrigues(n_omega_bn*dt); diff --git a/gtsam_unstable/slam/Mechanization_bRn2.h b/gtsam_unstable/slam/Mechanization_bRn2.h index fa33ce5b9..4c85614d4 100644 --- a/gtsam_unstable/slam/Mechanization_bRn2.h +++ b/gtsam_unstable/slam/Mechanization_bRn2.h @@ -42,7 +42,7 @@ public: /// gravity in the body frame Vector3 b_g(double g_e) const { Vector3 n_g(0, 0, g_e); - return (bRn_ * n_g).vector(); + return bRn_ * n_g; } const Rot3& bRn() const {return bRn_; } diff --git a/tests/testLie.cpp b/tests/testLie.cpp index 7be629053..c153adf5f 100644 --- a/tests/testLie.cpp +++ b/tests/testLie.cpp @@ -54,9 +54,9 @@ TEST(Lie, ProductLieGroup) { Vector5 d; d << 1, 2, 0.1, 0.2, 0.3; Product expected(Point2(1, 2), Pose2::Expmap(Vector3(0.1, 0.2, 0.3))); - Product pair2 = pair1.retract(d); + Product pair2 = pair1.expmap(d); EXPECT(assert_equal(expected, pair2, 1e-9)); - EXPECT(assert_equal(d, pair1.localCoordinates(pair2), 1e-9)); + EXPECT(assert_equal(d, pair1.logmap(pair2), 1e-9)); } /* ************************************************************************* */