From 6f8b05c0d0716c6e8241dd3cc76d0d3e6de47054 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 30 Dec 2015 00:14:51 -0800 Subject: [PATCH 1/7] ignore some files --- doc/.gitignore | 2 ++ 1 file changed, 2 insertions(+) diff --git a/doc/.gitignore b/doc/.gitignore index ac7af2e80..8a3139177 100644 --- a/doc/.gitignore +++ b/doc/.gitignore @@ -1 +1,3 @@ /html/ +*.lyx~ +*.bib~ From b3ffc6d8245da4fb2c06d44173cbab6c1e06e5d0 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 30 Dec 2015 00:15:02 -0800 Subject: [PATCH 2/7] Added missing Jacobians --- gtsam/navigation/ImuBias.h | 16 ++-- gtsam/navigation/tests/testImuBias.cpp | 102 +++++++++++++++---------- 2 files changed, 70 insertions(+), 48 deletions(-) diff --git a/gtsam/navigation/ImuBias.h b/gtsam/navigation/ImuBias.h index 047f24e8f..4acccb578 100644 --- a/gtsam/navigation/ImuBias.h +++ b/gtsam/navigation/ImuBias.h @@ -78,19 +78,19 @@ public: /** Correct an accelerometer measurement using this bias model, and optionally compute Jacobians */ Vector3 correctAccelerometer(const Vector3& measurement, - OptionalJacobian<3, 6> H = boost::none) const { - if (H) { - (*H) << -I_3x3, Z_3x3; - } + OptionalJacobian<3, 6> H1 = boost::none, + OptionalJacobian<3, 3> H2 = boost::none) const { + if (H1) (*H1) << -I_3x3, Z_3x3; + if (H2) (*H2) << I_3x3; return measurement - biasAcc_; } /** Correct a gyroscope measurement using this bias model, and optionally compute Jacobians */ Vector3 correctGyroscope(const Vector3& measurement, - OptionalJacobian<3, 6> H = boost::none) const { - if (H) { - (*H) << Z_3x3, -I_3x3; - } + OptionalJacobian<3, 6> H1 = boost::none, + OptionalJacobian<3, 3> H2 = boost::none) const { + if (H1) (*H1) << Z_3x3, -I_3x3; + if (H2) (*H2) << I_3x3; return measurement - biasGyro_; } diff --git a/gtsam/navigation/tests/testImuBias.cpp b/gtsam/navigation/tests/testImuBias.cpp index 4d5df3f05..596b76f6a 100644 --- a/gtsam/navigation/tests/testImuBias.cpp +++ b/gtsam/navigation/tests/testImuBias.cpp @@ -16,113 +16,135 @@ */ #include +#include + #include +#include using namespace std; using namespace gtsam; +typedef imuBias::ConstantBias Bias; + Vector biasAcc1(Vector3(0.2, -0.1, 0)); Vector biasGyro1(Vector3(0.1, -0.3, -0.2)); -imuBias::ConstantBias bias1(biasAcc1, biasGyro1); +Bias bias1(biasAcc1, biasGyro1); Vector biasAcc2(Vector3(0.1, 0.2, 0.04)); Vector biasGyro2(Vector3(-0.002, 0.005, 0.03)); -imuBias::ConstantBias bias2(biasAcc2, biasGyro2); +Bias bias2(biasAcc2, biasGyro2); /* ************************************************************************* */ -TEST( ImuBias, Constructor) { +TEST(ImuBias, Constructor) { // Default Constructor - imuBias::ConstantBias bias1; + Bias bias1; // Acc + Gyro Constructor - imuBias::ConstantBias bias2(biasAcc2, biasGyro2); + Bias bias2(biasAcc2, biasGyro2); // Copy Constructor - imuBias::ConstantBias bias3(bias2); + Bias bias3(bias2); } /* ************************************************************************* */ -TEST( ImuBias, inverse) { - imuBias::ConstantBias biasActual = bias1.inverse(); - imuBias::ConstantBias biasExpected = imuBias::ConstantBias(-biasAcc1, - -biasGyro1); +TEST(ImuBias, inverse) { + Bias biasActual = bias1.inverse(); + Bias biasExpected = Bias(-biasAcc1, -biasGyro1); EXPECT(assert_equal(biasExpected, biasActual)); } /* ************************************************************************* */ -TEST( ImuBias, compose) { - imuBias::ConstantBias biasActual = bias2.compose(bias1); - imuBias::ConstantBias biasExpected = imuBias::ConstantBias( - biasAcc1 + biasAcc2, biasGyro1 + biasGyro2); +TEST(ImuBias, compose) { + Bias biasActual = bias2.compose(bias1); + Bias biasExpected = Bias(biasAcc1 + biasAcc2, biasGyro1 + biasGyro2); EXPECT(assert_equal(biasExpected, biasActual)); } /* ************************************************************************* */ -TEST( ImuBias, between) { +TEST(ImuBias, between) { // p.between(q) == q - p - imuBias::ConstantBias biasActual = bias2.between(bias1); - imuBias::ConstantBias biasExpected = imuBias::ConstantBias( - biasAcc1 - biasAcc2, biasGyro1 - biasGyro2); + Bias biasActual = bias2.between(bias1); + Bias biasExpected = Bias(biasAcc1 - biasAcc2, biasGyro1 - biasGyro2); EXPECT(assert_equal(biasExpected, biasActual)); } /* ************************************************************************* */ -TEST( ImuBias, localCoordinates) { +TEST(ImuBias, localCoordinates) { Vector deltaActual = Vector(bias2.localCoordinates(bias1)); - Vector deltaExpected = (imuBias::ConstantBias(biasAcc1 - biasAcc2, - biasGyro1 - biasGyro2)).vector(); + Vector deltaExpected = + (Bias(biasAcc1 - biasAcc2, biasGyro1 - biasGyro2)).vector(); EXPECT(assert_equal(deltaExpected, deltaActual)); } /* ************************************************************************* */ -TEST( ImuBias, retract) { +TEST(ImuBias, retract) { Vector6 delta; delta << 0.1, 0.2, -0.3, 0.1, -0.1, 0.2; - imuBias::ConstantBias biasActual = bias2.retract(delta); - imuBias::ConstantBias biasExpected = imuBias::ConstantBias( - biasAcc2 + delta.block<3, 1>(0, 0), biasGyro2 + delta.block<3, 1>(3, 0)); + Bias biasActual = bias2.retract(delta); + Bias biasExpected = Bias(biasAcc2 + delta.block<3, 1>(0, 0), + biasGyro2 + delta.block<3, 1>(3, 0)); EXPECT(assert_equal(biasExpected, biasActual)); } /* ************************************************************************* */ -TEST( ImuBias, Logmap) { +TEST(ImuBias, Logmap) { Vector deltaActual = bias2.Logmap(bias1); Vector deltaExpected = bias1.vector(); EXPECT(assert_equal(deltaExpected, deltaActual)); } /* ************************************************************************* */ -TEST( ImuBias, Expmap) { +TEST(ImuBias, Expmap) { Vector6 delta; delta << 0.1, 0.2, -0.3, 0.1, -0.1, 0.2; - imuBias::ConstantBias biasActual = bias2.Expmap(delta); - imuBias::ConstantBias biasExpected = imuBias::ConstantBias(delta); + Bias biasActual = bias2.Expmap(delta); + Bias biasExpected = Bias(delta); EXPECT(assert_equal(biasExpected, biasActual)); } /* ************************************************************************* */ -TEST( ImuBias, operatorSub) { - imuBias::ConstantBias biasActual = -bias1; - imuBias::ConstantBias biasExpected(-biasAcc1, -biasGyro1); +TEST(ImuBias, operatorSub) { + Bias biasActual = -bias1; + Bias biasExpected(-biasAcc1, -biasGyro1); EXPECT(assert_equal(biasExpected, biasActual)); } /* ************************************************************************* */ -TEST( ImuBias, operatorAdd) { - imuBias::ConstantBias biasActual = bias2 + bias1; - imuBias::ConstantBias biasExpected(biasAcc2 + biasAcc1, - biasGyro2 + biasGyro1); +TEST(ImuBias, operatorAdd) { + Bias biasActual = bias2 + bias1; + Bias biasExpected(biasAcc2 + biasAcc1, biasGyro2 + biasGyro1); EXPECT(assert_equal(biasExpected, biasActual)); } /* ************************************************************************* */ -TEST( ImuBias, operatorSubB) { - imuBias::ConstantBias biasActual = bias2 - bias1; - imuBias::ConstantBias biasExpected(biasAcc2 - biasAcc1, - biasGyro2 - biasGyro1); +TEST(ImuBias, operatorSubB) { + Bias biasActual = bias2 - bias1; + Bias biasExpected(biasAcc2 - biasAcc1, biasGyro2 - biasGyro1); EXPECT(assert_equal(biasExpected, biasActual)); } +/* ************************************************************************* */ +TEST(ImuBias, Correct1) { + Matrix aH1, aH2; + const Vector3 measurement(1, 2, 3); + boost::function f = boost::bind( + &Bias::correctAccelerometer, _1, _2, boost::none, boost::none); + bias1.correctAccelerometer(measurement, aH1, aH2); + EXPECT(assert_equal(numericalDerivative21(f, bias1, measurement), aH1)); + EXPECT(assert_equal(numericalDerivative22(f, bias1, measurement), aH2)); +} + +/* ************************************************************************* */ +TEST(ImuBias, Correct2) { + Matrix aH1, aH2; + const Vector3 measurement(1, 2, 3); + boost::function f = + boost::bind(&Bias::correctGyroscope, _1, _2, boost::none, boost::none); + bias1.correctGyroscope(measurement, aH1, aH2); + EXPECT(assert_equal(numericalDerivative21(f, bias1, measurement), aH1)); + EXPECT(assert_equal(numericalDerivative22(f, bias1, measurement), aH2)); +} + /* ************************************************************************* */ int main() { TestResult tr; From 6ab909a92cfbfd382dab872b55244fae2b3f39c3 Mon Sep 17 00:00:00 2001 From: Jing Dong Date: Wed, 13 Jan 2016 12:22:58 -0500 Subject: [PATCH 3/7] fixed smart factors serialization, add unit tests] --- gtsam/geometry/triangulation.h | 9 +++++ gtsam/slam/SmartFactorBase.h | 3 ++ gtsam/slam/SmartProjectionFactor.h | 7 +++- gtsam/slam/SmartProjectionPoseFactor.h | 5 +++ gtsam/slam/tests/testSmartFactorBase.cpp | 37 +++++++++++++++++++ .../tests/testSmartProjectionCameraFactor.cpp | 21 +++++++++++ .../tests/testSmartProjectionPoseFactor.cpp | 22 +++++++++++ 7 files changed, 103 insertions(+), 1 deletion(-) diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index c3ab56200..bec901830 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -376,6 +376,15 @@ class TriangulationResult: public boost::optional { status_(s) { } public: + + /** + * Default constructor, only for serialization + */ + TriangulationResult() {} + + /** + * Constructor + */ TriangulationResult(const Point3& p) : status_(VALID) { reset(p); diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index e903d9651..01a8fcf8d 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -93,6 +93,9 @@ public: /// We use the new CameraSte data structure to refer to a set of cameras typedef CameraSet Cameras; + /// Default Constructor, for serialization + SmartFactorBase() {} + /// Constructor SmartFactorBase(const SharedNoiseModel& sharedNoiseModel, boost::optional body_P_sensor = boost::none) : diff --git a/gtsam/slam/SmartProjectionFactor.h b/gtsam/slam/SmartProjectionFactor.h index 13a4dd38f..09fe84caa 100644 --- a/gtsam/slam/SmartProjectionFactor.h +++ b/gtsam/slam/SmartProjectionFactor.h @@ -137,7 +137,7 @@ protected: /// @name Parameters /// @{ - const SmartProjectionParams params_; + SmartProjectionParams params_; /// @} /// @name Caching triangulation @@ -154,6 +154,11 @@ public: /// shorthand for a set of cameras typedef CameraSet Cameras; + /** + * Default constructor, only for serialization + */ + SmartProjectionFactor() {} + /** * Constructor * @param body_P_sensor pose of the camera in the body frame diff --git a/gtsam/slam/SmartProjectionPoseFactor.h b/gtsam/slam/SmartProjectionPoseFactor.h index c091ff79d..455e0de87 100644 --- a/gtsam/slam/SmartProjectionPoseFactor.h +++ b/gtsam/slam/SmartProjectionPoseFactor.h @@ -59,6 +59,11 @@ public: /// shorthand for a smart pointer to a factor typedef boost::shared_ptr shared_ptr; + /** + * Default constructor, only for serialization + */ + SmartProjectionPoseFactor() {} + /** * Constructor * @param Isotropic measurement noise diff --git a/gtsam/slam/tests/testSmartFactorBase.cpp b/gtsam/slam/tests/testSmartFactorBase.cpp index bf5969d4d..96052bd0f 100644 --- a/gtsam/slam/tests/testSmartFactorBase.cpp +++ b/gtsam/slam/tests/testSmartFactorBase.cpp @@ -18,6 +18,7 @@ #include #include +#include #include using namespace std; @@ -29,9 +30,13 @@ static SharedNoiseModel unit3(noiseModel::Unit::Create(3)); /* ************************************************************************* */ #include #include + +namespace gtsam { + class PinholeFactor: public SmartFactorBase > { public: typedef SmartFactorBase > Base; + PinholeFactor() {} PinholeFactor(const SharedNoiseModel& sharedNoiseModel): Base(sharedNoiseModel) { } virtual double error(const Values& values) const { @@ -43,6 +48,11 @@ public: } }; +/// traits +template<> +struct traits : public Testable {}; +} + TEST(SmartFactorBase, Pinhole) { PinholeFactor f= PinholeFactor(unit2); f.add(Point2(), 1); @@ -52,9 +62,13 @@ TEST(SmartFactorBase, Pinhole) { /* ************************************************************************* */ #include + +namespace gtsam { + class StereoFactor: public SmartFactorBase { public: typedef SmartFactorBase Base; + StereoFactor() {} StereoFactor(const SharedNoiseModel& sharedNoiseModel): Base(sharedNoiseModel) { } virtual double error(const Values& values) const { @@ -66,6 +80,11 @@ public: } }; +/// traits +template<> +struct traits : public Testable {}; +} + TEST(SmartFactorBase, Stereo) { StereoFactor f(unit3); f.add(StereoPoint2(), 1); @@ -73,6 +92,24 @@ TEST(SmartFactorBase, Stereo) { EXPECT_LONGS_EQUAL(2 * 3, f.dim()); } +/* ************************************************************************* */ +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic"); +BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel"); +BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal"); + +TEST(SmartFactorBase, serialize) { + using namespace gtsam::serializationTestHelpers; + PinholeFactor factor(unit2); + + EXPECT(equalsObj(factor)); + EXPECT(equalsXML(factor)); + EXPECT(equalsBinary(factor)); +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp b/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp index d4f60b085..54bbd6c22 100644 --- a/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp @@ -22,6 +22,7 @@ #include "smartFactorScenarios.h" #include #include +#include #include #include #include @@ -843,6 +844,26 @@ TEST( SmartProjectionCameraFactor, implicitJacobianFactor ) { EXPECT(assert_equal(yActual, yExpected, 1e-7)); } + +/* ************************************************************************* */ +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic"); +BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel"); +BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal"); + +TEST( SmartProjectionCameraFactor, serialize) { + using namespace vanilla; + using namespace gtsam::serializationTestHelpers; + SmartFactor factor(unit2); + + EXPECT(equalsObj(factor)); + EXPECT(equalsXML(factor)); + EXPECT(equalsBinary(factor)); +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp index 8796dfe65..0e2429840 100644 --- a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp @@ -24,6 +24,7 @@ #include #include #include +#include #include #include #include @@ -1387,6 +1388,27 @@ TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) { values.at(x3))); } +/* ************************************************************************* */ +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic"); +BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel"); +BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal"); + +TEST(SmartProjectionPoseFactor, serialize) { + using namespace vanillaPose; + using namespace gtsam::serializationTestHelpers; + SmartProjectionParams params; + params.setRankTolerance(rankTol); + SmartFactor factor(model, sharedK, boost::none, params); + + EXPECT(equalsObj(factor)); + EXPECT(equalsXML(factor)); + EXPECT(equalsBinary(factor)); +} + /* ************************************************************************* */ int main() { TestResult tr; From c3edee1e2dd728bf83dd82ccd464b28e6f9507d1 Mon Sep 17 00:00:00 2001 From: Jing Dong Date: Wed, 13 Jan 2016 21:33:41 -0500 Subject: [PATCH 4/7] fixed imu factor serialization, add unit test --- gtsam/navigation/ImuBias.h | 1 - gtsam/navigation/ImuFactor.h | 8 +++-- gtsam/navigation/PreintegratedRotation.h | 3 +- gtsam/navigation/PreintegrationBase.h | 21 ++++++----- gtsam/navigation/tests/testImuFactor.cpp | 46 ++++++++++++++++++++++++ 5 files changed, 66 insertions(+), 13 deletions(-) diff --git a/gtsam/navigation/ImuBias.h b/gtsam/navigation/ImuBias.h index 4acccb578..7664c7fd5 100644 --- a/gtsam/navigation/ImuBias.h +++ b/gtsam/navigation/ImuBias.h @@ -201,7 +201,6 @@ private: friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - ar & boost::serialization::make_nvp("imuBias::ConstantBias", *this); ar & BOOST_SERIALIZATION_NVP(biasAcc_); ar & BOOST_SERIALIZATION_NVP(biasGyro_); } diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index 855c14365..46306f8c7 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -128,8 +128,9 @@ private: friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + namespace bs = ::boost::serialization; ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationBase); - ar & BOOST_SERIALIZATION_NVP(preintMeasCov_); + ar & bs::make_nvp("preintMeasCov_", bs::make_array(preintMeasCov_.data(), preintMeasCov_.size())); } }; @@ -167,7 +168,7 @@ public: #endif /** Default constructor - only use for serialization */ - ImuFactor(); + ImuFactor() {} /** * Constructor @@ -241,4 +242,7 @@ private: }; // class ImuFactor +/// traits +template<> struct traits : public Testable {}; + } /// namespace gtsam diff --git a/gtsam/navigation/PreintegratedRotation.h b/gtsam/navigation/PreintegratedRotation.h index 002afea76..eb25b35e2 100644 --- a/gtsam/navigation/PreintegratedRotation.h +++ b/gtsam/navigation/PreintegratedRotation.h @@ -52,7 +52,8 @@ public: friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - ar & BOOST_SERIALIZATION_NVP(gyroscopeCovariance); + namespace bs = ::boost::serialization; + ar & bs::make_nvp("gyroscopeCovariance", bs::make_array(gyroscopeCovariance.data(), gyroscopeCovariance.size())); ar & BOOST_SERIALIZATION_NVP(omegaCoriolis); ar & BOOST_SERIALIZATION_NVP(body_P_sensor); } diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index 07225dd9a..c53a8c878 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -84,15 +84,17 @@ public: protected: /// Default constructor for serialization only: uninitialized! - Params(); + 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); + namespace bs = ::boost::serialization; + ar & boost::serialization::make_nvp("PreintegratedRotation_Params", + boost::serialization::base_object(*this)); + ar & bs::make_nvp("accelerometerCovariance", bs::make_array(accelerometerCovariance.data(), accelerometerCovariance.size())); + ar & bs::make_nvp("integrationCovariance", bs::make_array(integrationCovariance.data(), integrationCovariance.size())); ar & BOOST_SERIALIZATION_NVP(use2ndOrderCoriolis); ar & BOOST_SERIALIZATION_NVP(n_gravity); } @@ -246,15 +248,16 @@ private: friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + namespace bs = ::boost::serialization; ar & BOOST_SERIALIZATION_NVP(p_); ar & BOOST_SERIALIZATION_NVP(deltaTij_); ar & BOOST_SERIALIZATION_NVP(deltaXij_); ar & BOOST_SERIALIZATION_NVP(biasHat_); - ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega_); - ar & BOOST_SERIALIZATION_NVP(delPdelBiasAcc_); - ar & BOOST_SERIALIZATION_NVP(delPdelBiasOmega_); - ar & BOOST_SERIALIZATION_NVP(delVdelBiasAcc_); - ar & BOOST_SERIALIZATION_NVP(delVdelBiasOmega_); + ar & bs::make_nvp("delRdelBiasOmega_", bs::make_array(delRdelBiasOmega_.data(), delRdelBiasOmega_.size())); + ar & bs::make_nvp("delPdelBiasAcc_", bs::make_array(delPdelBiasAcc_.data(), delPdelBiasAcc_.size())); + ar & bs::make_nvp("delPdelBiasOmega_", bs::make_array(delPdelBiasOmega_.data(), delPdelBiasOmega_.size())); + ar & bs::make_nvp("delVdelBiasAcc_", bs::make_array(delVdelBiasAcc_.data(), delVdelBiasAcc_.size())); + ar & bs::make_nvp("delVdelBiasOmega_", bs::make_array(delVdelBiasOmega_.data(), delVdelBiasOmega_.size())); } }; diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index 92cb92e70..25a6e732c 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -1010,6 +1010,52 @@ TEST(ImuFactor, bodyPSensorWithBias) { EXPECT(assert_equal(biasExpected, biasActual, 1e-3)); } +/* ************************************************************************** */ +#include + +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic"); +BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel"); +BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal"); + +TEST(ImuFactor, serialization) { + using namespace gtsam::serializationTestHelpers; + + Vector3 n_gravity(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; + imuBias::ConstantBias priorBias(Vector3(0, 0, 0), Vector3(0, 0.01, 0)); // Biases (acc, rot) + + ImuFactor::PreintegratedMeasurements pim = + ImuFactor::PreintegratedMeasurements(priorBias, accCov, gyroCov, + integrationCov, true); + + // measurements are needed for non-inf noise model, otherwise will throw err when deserialize + + // 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); + + for (int j = 0; j < 200; ++j) + pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT, + body_P_sensor); + + ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim); + + EXPECT(equalsObj(factor)); + EXPECT(equalsXML(factor)); + EXPECT(equalsBinary(factor)); +} + /* ************************************************************************* */ int main() { TestResult tr; From 3c1ddd7a3fea0027e752eb87495bb3ea25e9c366 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 17 Jan 2016 19:22:14 -0800 Subject: [PATCH 5/7] Inlined skewSymmetric --- gtsam/base/Matrix.cpp | 9 --------- gtsam/base/Matrix.h | 12 +++++++++--- 2 files changed, 9 insertions(+), 12 deletions(-) diff --git a/gtsam/base/Matrix.cpp b/gtsam/base/Matrix.cpp index 3cafdd0ba..c6af89486 100644 --- a/gtsam/base/Matrix.cpp +++ b/gtsam/base/Matrix.cpp @@ -580,15 +580,6 @@ Matrix vector_scale(const Matrix& A, const Vector& v, bool inf_mask) { return M; } -/* ************************************************************************* */ -Matrix3 skewSymmetric(double wx, double wy, double wz) -{ - return (Matrix3() << - 0.0, -wz, +wy, - +wz, 0.0, -wx, - -wy, +wx, 0.0).finished(); -} - /* ************************************************************************* */ Matrix LLt(const Matrix& A) { diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index 41ffa27b5..e2b40b02b 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -477,9 +477,15 @@ GTSAM_EXPORT Matrix vector_scale(const Matrix& A, const Vector& v, bool inf_mask * @param wz * @return a 3*3 skew symmetric matrix */ -GTSAM_EXPORT Matrix3 skewSymmetric(double wx, double wy, double wz); -template -inline Matrix3 skewSymmetric(const Eigen::MatrixBase& w) { return skewSymmetric(w(0),w(1),w(2));} + +inline Matrix3 skewSymmetric(double wx, double wy, double wz) { + return (Matrix3() << 0.0, -wz, +wy, +wz, 0.0, -wx, -wy, +wx, 0.0).finished(); +} + +template +inline Matrix3 skewSymmetric(const Eigen::MatrixBase& w) { + return skewSymmetric(w(0), w(1), w(2)); +} /** Use Cholesky to calculate inverse square root of a matrix */ GTSAM_EXPORT Matrix inverse_square_root(const Matrix& A); From c1b2e9d72654b8b7800dd8ad0c905772f5008104 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 17 Jan 2016 19:22:49 -0800 Subject: [PATCH 6/7] Optmized ExpmapDerivative --- gtsam/geometry/SO3.cpp | 16 +++++++++++----- 1 file changed, 11 insertions(+), 5 deletions(-) diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp index af5803cb7..a417164e4 100644 --- a/gtsam/geometry/SO3.cpp +++ b/gtsam/geometry/SO3.cpp @@ -133,9 +133,9 @@ Matrix3 SO3::ExpmapDerivative(const Vector3& omega) { using std::cos; using std::sin; - double theta2 = omega.dot(omega); + const double theta2 = omega.dot(omega); if (theta2 <= std::numeric_limits::epsilon()) return I_3x3; - double theta = std::sqrt(theta2); // rotation angle + const double theta = std::sqrt(theta2); // rotation angle #ifdef DUY_VERSION /// Follow Iserles05an, B10, pg 147, with a sign change in the second term (left version) Matrix3 X = skewSymmetric(omega); @@ -154,9 +154,15 @@ Matrix3 SO3::ExpmapDerivative(const Vector3& omega) { * a perturbation on the manifold (expmap(Jr * omega)) */ // element of Lie algebra so(3): X = omega^, normalized by normx - const Matrix3 Y = skewSymmetric(omega) / theta; - return I_3x3 - ((1 - cos(theta)) / (theta)) * Y - + (1 - sin(theta) / theta) * Y * Y; // right Jacobian + const double wx = omega.x(), wy = omega.y(), wz = omega.z(); + Matrix3 Y; + Y << 0.0, -wz, +wy, + +wz, 0.0, -wx, + -wy, +wx, 0.0; + const double s2 = sin(theta / 2.0); + const double a = (2.0 * s2 * s2 / theta2); + const double b = (1.0 - sin(theta) / theta) / theta2; + return I_3x3 - a * Y + b * Y * Y; #endif } From a0f32f6d1421f854d044ece939181243e9c346af Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 17 Jan 2016 19:23:18 -0800 Subject: [PATCH 7/7] Got rid of dynamic Matrix in rotate --- gtsam/geometry/Rot3Q.cpp | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/gtsam/geometry/Rot3Q.cpp b/gtsam/geometry/Rot3Q.cpp index b255b082d..2497f6806 100644 --- a/gtsam/geometry/Rot3Q.cpp +++ b/gtsam/geometry/Rot3Q.cpp @@ -27,14 +27,12 @@ using namespace std; namespace gtsam { - static const Matrix I3 = eye(3); - /* ************************************************************************* */ Rot3::Rot3() : quaternion_(Quaternion::Identity()) {} /* ************************************************************************* */ Rot3::Rot3(const Point3& col1, const Point3& col2, const Point3& col3) : - quaternion_((Eigen::Matrix3d() << + quaternion_((Matrix3() << col1.x(), col2.x(), col3.x(), col1.y(), col2.y(), col3.y(), col1.z(), col2.z(), col3.z()).finished()) {} @@ -43,7 +41,7 @@ namespace gtsam { Rot3::Rot3(double R11, double R12, double R13, double R21, double R22, double R23, double R31, double R32, double R33) : - quaternion_((Eigen::Matrix3d() << + quaternion_((Matrix3() << R11, R12, R13, R21, R22, R23, R31, R32, R33).finished()) {} @@ -91,10 +89,10 @@ namespace gtsam { /* ************************************************************************* */ Point3 Rot3::rotate(const Point3& p, OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const { - Matrix R = matrix(); + const Matrix3 R = matrix(); if (H1) *H1 = R * skewSymmetric(-p.x(), -p.y(), -p.z()); if (H2) *H2 = R; - Eigen::Vector3d r = R * p.vector(); + const Vector3 r = R * p.vector(); return Point3(r.x(), r.y(), r.z()); }