diff --git a/.cproject b/.cproject index 9726fec60..7c8190e6a 100644 --- a/.cproject +++ b/.cproject @@ -532,14 +532,6 @@ true true - - make - -j4 - testSimilarity3.run - true - true - true - make -j2 @@ -755,6 +747,14 @@ true true + + make + -j4 + testSimilarity3.run + true + true + true + make -j5 @@ -1301,7 +1301,6 @@ make - testSimulated2DOriented.run true false @@ -1341,7 +1340,6 @@ make - testSimulated2D.run true false @@ -1349,7 +1347,6 @@ make - testSimulated3D.run true false @@ -1453,6 +1450,7 @@ make + testErrors.run true false @@ -1763,6 +1761,7 @@ cpack + -G DEB true false @@ -1770,6 +1769,7 @@ cpack + -G RPM true false @@ -1777,6 +1777,7 @@ cpack + -G TGZ true false @@ -1784,6 +1785,7 @@ cpack + --config CPackSourceConfig.cmake true false @@ -1975,7 +1977,6 @@ make - tests/testGaussianISAM2 true false @@ -2037,22 +2038,6 @@ true true - - make - -j5 - testExpressionMeta.run - true - true - true - - - make - -j4 - testCustomChartExpression.run - true - true - true - make -j5 @@ -2127,6 +2112,7 @@ make + tests/testBayesTree.run true false @@ -2134,6 +2120,7 @@ make + testBinaryBayesNet.run true false @@ -2181,6 +2168,7 @@ make + testSymbolicBayesNet.run true false @@ -2188,6 +2176,7 @@ make + tests/testSymbolicFactor.run true false @@ -2195,6 +2184,7 @@ make + testSymbolicFactorGraph.run true false @@ -2210,6 +2200,7 @@ make + tests/testBayesTree true false @@ -2783,6 +2774,14 @@ true true + + make + -j4 + testExecutionTrace.run + true + true + true + make -j5 @@ -3329,6 +3328,7 @@ make + testGraph.run true false @@ -3336,6 +3336,7 @@ make + testJunctionTree.run true false @@ -3343,6 +3344,7 @@ make + testSymbolicBayesNetB.run true false diff --git a/cmake/GtsamBuildTypes.cmake b/cmake/GtsamBuildTypes.cmake index 81c4adaeb..c2cd7b449 100644 --- a/cmake/GtsamBuildTypes.cmake +++ b/cmake/GtsamBuildTypes.cmake @@ -98,7 +98,8 @@ if( NOT cmake_build_type_tolower STREQUAL "" AND NOT cmake_build_type_tolower STREQUAL "release" AND NOT cmake_build_type_tolower STREQUAL "timing" AND NOT cmake_build_type_tolower STREQUAL "profiling" - AND NOT cmake_build_type_tolower STREQUAL "relwithdebinfo") + AND NOT cmake_build_type_tolower STREQUAL "relwithdebinfo" + AND NOT cmake_build_type_tolower STREQUAL "minsizerel") message(FATAL_ERROR "Unknown build type \"${CMAKE_BUILD_TYPE}\". Allowed values are None, Debug, Release, Timing, Profiling, RelWithDebInfo (case-insensitive).") endif() diff --git a/examples/SolverComparer.cpp b/examples/SolverComparer.cpp index 923b0b9de..0393affe1 100644 --- a/examples/SolverComparer.cpp +++ b/examples/SolverComparer.cpp @@ -31,28 +31,29 @@ * */ -#include -#include -#include -#include -#include #include #include -#include -#include -#include +#include +#include +#include #include #include #include +#include +#include +#include +#include +#include -#include -#include -#include #include -#include +#include #include #include #include +#include + +#include +#include #ifdef GTSAM_USE_TBB #include @@ -72,23 +73,6 @@ typedef NoiseModelFactor1 NM1; typedef NoiseModelFactor2 NM2; typedef BearingRangeFactor BR; -//GTSAM_VALUE_EXPORT(Value); -//GTSAM_VALUE_EXPORT(Pose); -//GTSAM_VALUE_EXPORT(Rot2); -//GTSAM_VALUE_EXPORT(Point2); -//GTSAM_VALUE_EXPORT(NonlinearFactor); -//GTSAM_VALUE_EXPORT(NoiseModelFactor); -//GTSAM_VALUE_EXPORT(NM1); -//GTSAM_VALUE_EXPORT(NM2); -//GTSAM_VALUE_EXPORT(BetweenFactor); -//GTSAM_VALUE_EXPORT(PriorFactor); -//GTSAM_VALUE_EXPORT(BR); -//GTSAM_VALUE_EXPORT(noiseModel::Base); -//GTSAM_VALUE_EXPORT(noiseModel::Isotropic); -//GTSAM_VALUE_EXPORT(noiseModel::Gaussian); -//GTSAM_VALUE_EXPORT(noiseModel::Diagonal); -//GTSAM_VALUE_EXPORT(noiseModel::Unit); - double chi2_red(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& config) { // Compute degrees of freedom (observations - variables) // In ocaml, +1 was added to the observations to account for the prior, but @@ -269,12 +253,12 @@ void runIncremental() boost::dynamic_pointer_cast >(datasetMeasurements[nextMeasurement])) { Key key1 = measurement->key1(), key2 = measurement->key2(); - if((key1 >= firstStep && key1 < key2) || (key2 >= firstStep && key2 < key1)) { + if(((int)key1 >= firstStep && key1 < key2) || ((int)key2 >= firstStep && key2 < key1)) { // We found an odometry starting at firstStep firstPose = std::min(key1, key2); break; } - if((key2 >= firstStep && key1 < key2) || (key1 >= firstStep && key2 < key1)) { + if(((int)key2 >= firstStep && key1 < key2) || ((int)key1 >= firstStep && key2 < key1)) { // We found an odometry joining firstStep with a previous pose havePreviousPose = true; firstPose = std::max(key1, key2); @@ -303,7 +287,9 @@ void runIncremental() cout << "Playing forward time steps..." << endl; - for(size_t step = firstPose; nextMeasurement < datasetMeasurements.size() && (lastStep == -1 || step <= lastStep); ++step) + for (size_t step = firstPose; + nextMeasurement < datasetMeasurements.size() && (lastStep == -1 || (int)step <= lastStep); + ++step) { Values newVariables; NonlinearFactorGraph newFactors; diff --git a/gtsam/3rdparty/ceres/eigen.h b/gtsam/3rdparty/ceres/eigen.h index 18a602cf4..a25fde97f 100644 --- a/gtsam/3rdparty/ceres/eigen.h +++ b/gtsam/3rdparty/ceres/eigen.h @@ -31,7 +31,7 @@ #ifndef CERES_INTERNAL_EIGEN_H_ #define CERES_INTERNAL_EIGEN_H_ -#include +#include namespace ceres { diff --git a/gtsam/3rdparty/ceres/fixed_array.h b/gtsam/3rdparty/ceres/fixed_array.h index db1591636..455fce383 100644 --- a/gtsam/3rdparty/ceres/fixed_array.h +++ b/gtsam/3rdparty/ceres/fixed_array.h @@ -33,7 +33,7 @@ #define CERES_PUBLIC_INTERNAL_FIXED_ARRAY_H_ #include -#include +#include #include #include diff --git a/gtsam/3rdparty/ceres/jet.h b/gtsam/3rdparty/ceres/jet.h index 12d4e8bc9..4a7683f50 100644 --- a/gtsam/3rdparty/ceres/jet.h +++ b/gtsam/3rdparty/ceres/jet.h @@ -162,7 +162,7 @@ #include #include -#include +#include #include namespace ceres { diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt index 0ebd6c07d..e26d85ff8 100644 --- a/gtsam/CMakeLists.txt +++ b/gtsam/CMakeLists.txt @@ -131,12 +131,6 @@ else() set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE) endif() -# Set dataset paths -set_property(SOURCE "${CMAKE_CURRENT_SOURCE_DIR}/slam/dataset.cpp" - APPEND PROPERTY COMPILE_DEFINITIONS - "SOURCE_TREE_DATASET_DIR=\"${PROJECT_SOURCE_DIR}/examples/Data\"" - "INSTALLED_DATASET_DIR=\"${GTSAM_TOOLBOX_INSTALL_PATH}/gtsam_examples/Data\"") - # Special cases if(MSVC) set_property(SOURCE diff --git a/gtsam/base/OptionalJacobian.h b/gtsam/base/OptionalJacobian.h index a83333caa..7055a287a 100644 --- a/gtsam/base/OptionalJacobian.h +++ b/gtsam/base/OptionalJacobian.h @@ -168,5 +168,20 @@ public: Jacobian* operator->(){ return pointer_; } }; +// forward declare +template struct traits; + +/** + * @brief: meta-function to generate JacobianTA optional reference + * Used mainly by Expressions + * @param T return type + * @param A argument type + */ +template +struct MakeOptionalJacobian { + typedef OptionalJacobian::dimension, + traits::dimension> type; +}; + } // namespace gtsam diff --git a/gtsam/base/Vector.cpp b/gtsam/base/Vector.cpp index 0a708427a..ed6373f5b 100644 --- a/gtsam/base/Vector.cpp +++ b/gtsam/base/Vector.cpp @@ -222,11 +222,7 @@ double norm_2(const Vector& v) { /* ************************************************************************* */ Vector reciprocal(const Vector &a) { - size_t n = a.size(); - Vector b(n); - for( size_t i = 0; i < n; i++ ) - b(i) = 1.0/a(i); - return b; + return a.array().inverse(); } /* ************************************************************************* */ diff --git a/gtsam/base/VectorSpace.h b/gtsam/base/VectorSpace.h index e1f20ea0c..558fe52c9 100644 --- a/gtsam/base/VectorSpace.h +++ b/gtsam/base/VectorSpace.h @@ -24,13 +24,6 @@ namespace internal { template struct VectorSpaceImpl { - /// @name Group - /// @{ - static Class Compose(const Class& v1, const Class& v2) { return v1+v2;} - static Class Between(const Class& v1, const Class& v2) { return v2-v1;} - static Class Inverse(const Class& m) { return -m;} - /// @} - /// @name Manifold /// @{ typedef Eigen::Matrix TangentVector; @@ -68,21 +61,21 @@ struct VectorSpaceImpl { return Class(v); } - static Class Compose(const Class& v1, const Class& v2, ChartJacobian H1, - ChartJacobian H2) { + static Class Compose(const Class& v1, const Class& v2, ChartJacobian H1 = boost::none, + ChartJacobian H2 = boost::none) { if (H1) *H1 = Jacobian::Identity(); if (H2) *H2 = Jacobian::Identity(); return v1 + v2; } - static Class Between(const Class& v1, const Class& v2, ChartJacobian H1, - ChartJacobian H2) { + static Class Between(const Class& v1, const Class& v2, ChartJacobian H1 = boost::none, + ChartJacobian H2 = boost::none) { if (H1) *H1 = - Jacobian::Identity(); if (H2) *H2 = Jacobian::Identity(); return v2 - v1; } - static Class Inverse(const Class& v, ChartJacobian H) { + static Class Inverse(const Class& v, ChartJacobian H = boost::none) { if (H) *H = - Jacobian::Identity(); return -v; } diff --git a/gtsam/base/tests/testTreeTraversal.cpp b/gtsam/base/tests/testTreeTraversal.cpp index c9083f781..8fe5e53ef 100644 --- a/gtsam/base/tests/testTreeTraversal.cpp +++ b/gtsam/base/tests/testTreeTraversal.cpp @@ -45,11 +45,11 @@ struct TestForest { }; TestForest makeTestForest() { - // 0 1 - // / \ - // 2 3 - // | - // 4 + // 0 1 + // / | + // 2 3 + // | + // 4 TestForest forest; forest.roots_.push_back(boost::make_shared(0)); forest.roots_.push_back(boost::make_shared(1)); diff --git a/gtsam/config.h.in b/gtsam/config.h.in index 8d406e21f..20073152e 100644 --- a/gtsam/config.h.in +++ b/gtsam/config.h.in @@ -25,7 +25,7 @@ #define GTSAM_VERSION_STRING "@GTSAM_VERSION_STRING@" // Paths to example datasets distributed with GTSAM -#define GTSAM_SOURCE_TREE_DATASET_DIR "@CMAKE_SOURCE_DIR@/examples/Data" +#define GTSAM_SOURCE_TREE_DATASET_DIR "@PROJECT_SOURCE_DIR@/examples/Data" #define GTSAM_INSTALLED_DATASET_DIR "@GTSAM_TOOLBOX_INSTALL_PATH@/gtsam_examples/Data" // Whether GTSAM is compiled to use quaternions for Rot3 (otherwise uses rotation matrices) diff --git a/gtsam/discrete/DecisionTree-inl.h b/gtsam/discrete/DecisionTree-inl.h index cd56780e5..c1648888e 100644 --- a/gtsam/discrete/DecisionTree-inl.h +++ b/gtsam/discrete/DecisionTree-inl.h @@ -467,7 +467,7 @@ namespace gtsam { // find highest label among branches boost::optional highestLabel; - boost::optional nrChoices; + size_t nrChoices = 0; for (Iterator it = begin; it != end; it++) { if (it->root_->isLeaf()) continue; @@ -475,22 +475,21 @@ namespace gtsam { boost::dynamic_pointer_cast(it->root_); if (!highestLabel || c->label() > *highestLabel) { highestLabel.reset(c->label()); - nrChoices.reset(c->nrChoices()); + nrChoices = c->nrChoices(); } } // if label is already in correct order, just put together a choice on label - if (!highestLabel || !nrChoices || label > *highestLabel) { + if (!nrChoices || !highestLabel || label > *highestLabel) { boost::shared_ptr choiceOnLabel(new Choice(label, end - begin)); for (Iterator it = begin; it != end; it++) choiceOnLabel->push_back(it->root_); return Choice::Unique(choiceOnLabel); } else { // Set up a new choice on the highest label - boost::shared_ptr choiceOnHighestLabel( - new Choice(*highestLabel, *nrChoices)); + boost::shared_ptr choiceOnHighestLabel(new Choice(*highestLabel, nrChoices)); // now, for all possible values of highestLabel - for (size_t index = 0; index < *nrChoices; index++) { + for (size_t index = 0; index < nrChoices; index++) { // make a new set of functions for composing by iterating over the given // functions, and selecting the appropriate branch. std::vector functions; diff --git a/gtsam/geometry/Cal3_S2.h b/gtsam/geometry/Cal3_S2.h index 3ab6cfd36..3d5342c92 100644 --- a/gtsam/geometry/Cal3_S2.h +++ b/gtsam/geometry/Cal3_S2.h @@ -21,7 +21,6 @@ #pragma once -#include #include namespace gtsam { diff --git a/gtsam/geometry/OrientedPlane3.cpp b/gtsam/geometry/OrientedPlane3.cpp index 088a84243..e96708942 100644 --- a/gtsam/geometry/OrientedPlane3.cpp +++ b/gtsam/geometry/OrientedPlane3.cpp @@ -73,7 +73,7 @@ Vector3 OrientedPlane3::error(const gtsam::OrientedPlane3& plane) const { } /* ************************************************************************* */ -OrientedPlane3 OrientedPlane3::retract(const Vector& v) const { +OrientedPlane3 OrientedPlane3::retract(const Vector3& v) const { // Retract the Unit3 Vector2 n_v(v(0), v(1)); Unit3 n_retracted = n_.retract(n_v); @@ -83,7 +83,7 @@ OrientedPlane3 OrientedPlane3::retract(const Vector& v) const { /* ************************************************************************* */ Vector3 OrientedPlane3::localCoordinates(const OrientedPlane3& y) const { - Vector n_local = n_.localCoordinates(y.n_); + Vector2 n_local = n_.localCoordinates(y.n_); double d_local = d_ - y.d_; Vector3 e; e << n_local(0), n_local(1), -d_local; diff --git a/gtsam/geometry/OrientedPlane3.h b/gtsam/geometry/OrientedPlane3.h index dad283760..d987ad7b3 100644 --- a/gtsam/geometry/OrientedPlane3.h +++ b/gtsam/geometry/OrientedPlane3.h @@ -51,7 +51,7 @@ public: n_(s), d_(d) { } - OrientedPlane3(Vector vec) : + OrientedPlane3(const Vector4& vec) : n_(vec(0), vec(1), vec(2)), d_(vec(3)) { } @@ -89,7 +89,7 @@ public: } /// The retract function - OrientedPlane3 retract(const Vector& v) const; + OrientedPlane3 retract(const Vector3& v) const; /// The local coordinates function Vector3 localCoordinates(const OrientedPlane3& s) const; diff --git a/gtsam/geometry/PinholePose.h b/gtsam/geometry/PinholePose.h index 0f4685770..a35887384 100644 --- a/gtsam/geometry/PinholePose.h +++ b/gtsam/geometry/PinholePose.h @@ -270,7 +270,10 @@ public: /// print void print(const std::string& s = "PinholePose") const { Base::print(s); - K_->print(s + ".calibration"); + if (!K_) + std::cout << "s No calibration given" << std::endl; + else + K_->print(s + ".calibration"); } /// @} diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index be8e1bfed..7b7c861fd 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -112,7 +112,9 @@ bool Pose3::equals(const Pose3& pose, double tol) const { /* ************************************************************************* */ /** Modified from Murray94book version (which assumes w and v normalized?) */ Pose3 Pose3::Expmap(const Vector& xi, OptionalJacobian<6, 6> H) { - if (H) *H = ExpmapDerivative(xi); + if (H) { + *H = ExpmapDerivative(xi); + } // get angular velocity omega and translational velocity v from twist xi Point3 w(xi(0), xi(1), xi(2)), v(xi(3), xi(4), xi(5)); @@ -254,6 +256,14 @@ Matrix6 Pose3::LogmapDerivative(const Pose3& pose) { return J; } +/* ************************************************************************* */ +const Point3& Pose3::translation(OptionalJacobian<3, 6> H) const { + if (H) { + *H << Z_3x3, rotation().matrix(); + } + return t_; +} + /* ************************************************************************* */ Matrix4 Pose3::matrix() const { const Matrix3 R = R_.matrix(); @@ -280,8 +290,9 @@ Point3 Pose3::transform_from(const Point3& p, OptionalJacobian<3,6> Dpose, Matrix3 DR = R * skewSymmetric(-p.x(), -p.y(), -p.z()); (*Dpose) << DR, R; } - if (Dpoint) + if (Dpoint) { *Dpoint = R_.matrix(); + } return R_ * p + t_; } @@ -299,17 +310,18 @@ Point3 Pose3::transform_to(const Point3& p, OptionalJacobian<3,6> Dpose, +wz, 0.0, -wx, 0.0,-1.0, 0.0, -wy, +wx, 0.0, 0.0, 0.0,-1.0; } - if (Dpoint) + if (Dpoint) { *Dpoint = Rt; + } return q; } /* ************************************************************************* */ double Pose3::range(const Point3& point, OptionalJacobian<1, 6> H1, OptionalJacobian<1, 3> H2) const { - if (!H1 && !H2) + if (!H1 && !H2) { return transform_to(point).norm(); - else { + } else { Matrix36 D1; Matrix3 D2; Point3 d = transform_to(point, H1 ? &D1 : 0, H2 ? &D2 : 0); diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 1ea8e8d5c..b11ae2587 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -223,9 +223,7 @@ public: } /// get translation - const Point3& translation() const { - return t_; - } + const Point3& translation(OptionalJacobian<3, 6> H = boost::none) const; /// get x double x() const { diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp index be48aecc9..cc3865b8e 100644 --- a/gtsam/geometry/Unit3.cpp +++ b/gtsam/geometry/Unit3.cpp @@ -119,7 +119,7 @@ Matrix3 Unit3::skew() const { } /* ************************************************************************* */ -Vector Unit3::error(const Unit3& q, OptionalJacobian<2,2> H) const { +Vector2 Unit3::error(const Unit3& q, OptionalJacobian<2,2> H) const { // 2D error is equal to B'*q, as B is 3x2 matrix and q is 3x1 Matrix23 Bt = basis().transpose(); Vector2 xi = Bt * q.p_.vector(); diff --git a/gtsam/geometry/Unit3.h b/gtsam/geometry/Unit3.h index 8e10b839b..12bfac5ce 100644 --- a/gtsam/geometry/Unit3.h +++ b/gtsam/geometry/Unit3.h @@ -108,7 +108,7 @@ public: } /// Return unit-norm Vector - Vector unitVector(boost::optional H = boost::none) const { + Vector3 unitVector(boost::optional H = boost::none) const { if (H) *H = basis(); return (p_.vector()); @@ -120,7 +120,7 @@ public: } /// Signed, vector-valued error between two directions - Vector error(const Unit3& q, OptionalJacobian<2, 2> H = boost::none) const; + Vector2 error(const Unit3& q, OptionalJacobian<2, 2> H = boost::none) const; /// Distance between two directions double distance(const Unit3& q, OptionalJacobian<1, 2> H = boost::none) const; diff --git a/gtsam/geometry/tests/testCalibratedCamera.cpp b/gtsam/geometry/tests/testCalibratedCamera.cpp index b1e265266..c02a11928 100644 --- a/gtsam/geometry/tests/testCalibratedCamera.cpp +++ b/gtsam/geometry/tests/testCalibratedCamera.cpp @@ -90,11 +90,11 @@ TEST( CalibratedCamera, project) /* ************************************************************************* */ TEST( CalibratedCamera, Dproject_to_camera1) { Point3 pp(155,233,131); - Matrix test1; - CalibratedCamera::project_to_camera(pp, test1); - Matrix test2 = numericalDerivative11( + Matrix actual; + CalibratedCamera::project_to_camera(pp, actual); + Matrix expected_numerical = numericalDerivative11( boost::bind(CalibratedCamera::project_to_camera, _1, boost::none), pp); - CHECK(assert_equal(test1, test2)); + CHECK(assert_equal(expected_numerical, actual)); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index a716406a4..d749ba6af 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -186,6 +186,17 @@ TEST(Pose3, expmaps_galore_full) EXPECT(assert_equal(xi, Pose3::Logmap(actual),1e-6)); } +/* ************************************************************************* */ +// Check translation and its pushforward +TEST(Pose3, translation) { + Matrix actualH; + EXPECT(assert_equal(Point3(3.5, -8.2, 4.2), T.translation(actualH), 1e-8)); + + Matrix numericalH = numericalDerivative11( + boost::bind(&Pose3::translation, _1, boost::none), T); + EXPECT(assert_equal(numericalH, actualH, 1e-6)); +} + /* ************************************************************************* */ TEST(Pose3, Adjoint_compose_full) { diff --git a/gtsam/geometry/triangulation.cpp b/gtsam/geometry/triangulation.cpp index 474689525..c0f69217c 100644 --- a/gtsam/geometry/triangulation.cpp +++ b/gtsam/geometry/triangulation.cpp @@ -23,14 +23,8 @@ namespace gtsam { -/** - * DLT triangulation: See Hartley and Zisserman, 2nd Ed., page 312 - * @param projection_matrices Projection matrices (K*P^-1) - * @param measurements 2D measurements - * @param rank_tol SVD rank tolerance - * @return Triangulated Point3 - */ -Point3 triangulateDLT(const std::vector& projection_matrices, +Vector4 triangulateHomogeneousDLT( + const std::vector& projection_matrices, const std::vector& measurements, double rank_tol) { // number of cameras @@ -57,7 +51,16 @@ Point3 triangulateDLT(const std::vector& projection_matrices, if (rank < 3) throw(TriangulationUnderconstrainedException()); - // Create 3D point from eigenvector + return v; +} + +Point3 triangulateDLT(const std::vector& projection_matrices, + const std::vector& measurements, double rank_tol) { + + Vector4 v = triangulateHomogeneousDLT(projection_matrices, measurements, + rank_tol); + + // Create 3D point from homogeneous coordinates return Point3(sub((v / v(3)), 0, 3)); } @@ -89,6 +92,5 @@ Point3 optimize(const NonlinearFactorGraph& graph, const Values& values, return result.at(landmarkKey); } - } // \namespace gtsam diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index ce83f780b..0eb59f016 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -18,7 +18,6 @@ #pragma once - #include #include #include @@ -43,6 +42,17 @@ public: } }; +/** + * DLT triangulation: See Hartley and Zisserman, 2nd Ed., page 312 + * @param projection_matrices Projection matrices (K*P^-1) + * @param measurements 2D measurements + * @param rank_tol SVD rank tolerance + * @return Triangulated point, in homogeneous coordinates + */ +GTSAM_EXPORT Vector4 triangulateHomogeneousDLT( + const std::vector& projection_matrices, + const std::vector& measurements, double rank_tol = 1e-9); + /** * DLT triangulation: See Hartley and Zisserman, 2nd Ed., page 312 * @param projection_matrices Projection matrices (K*P^-1) @@ -52,7 +62,7 @@ public: */ GTSAM_EXPORT Point3 triangulateDLT( const std::vector& projection_matrices, - const std::vector& measurements, double rank_tol); + const std::vector& measurements, double rank_tol = 1e-9); /// /** @@ -164,6 +174,25 @@ Point3 triangulateNonlinear( return optimize(graph, values, Symbol('p', 0)); } +/** + * Create a 3*4 camera projection matrix from calibration and pose. + * Functor for partial application on calibration + * @param pose The camera pose + * @param cal The calibration + * @return Returns a Matrix34 + */ +template +struct CameraProjectionMatrix { + CameraProjectionMatrix(const CALIBRATION& calibration) : + K_(calibration.K()) { + } + Matrix34 operator()(const Pose3& pose) const { + return K_ * (pose.inverse().matrix()).block<3, 4>(0, 0); + } +private: + const Matrix3 K_; +}; + /** * Function to triangulate 3D landmark point from an arbitrary number * of poses (at least 2) using the DLT. The function checks that the @@ -188,10 +217,10 @@ Point3 triangulatePoint3(const std::vector& poses, // construct projection matrices from poses & calibration std::vector projection_matrices; - BOOST_FOREACH(const Pose3& pose, poses) { - projection_matrices.push_back( - sharedCal->K() * (pose.inverse().matrix()).block<3,4>(0,0)); - } + CameraProjectionMatrix createP(*sharedCal); // partially apply + BOOST_FOREACH(const Pose3& pose, poses) + projection_matrices.push_back(createP(pose)); + // Triangulate linearly Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol); @@ -204,7 +233,7 @@ Point3 triangulatePoint3(const std::vector& poses, BOOST_FOREACH(const Pose3& pose, poses) { const Point3& p_local = pose.transform_to(point); if (p_local.z() <= 0) - throw(TriangulationCheiralityException()); + throw(TriangulationCheiralityException()); } #endif @@ -230,7 +259,7 @@ Point3 triangulatePoint3( bool optimize = false) { size_t m = cameras.size(); - assert(measurements.size()==m); + assert(measurements.size() == m); if (m < 2) throw(TriangulationUnderconstrainedException()); @@ -238,10 +267,10 @@ Point3 triangulatePoint3( // construct projection matrices from poses & calibration typedef PinholeCamera Camera; std::vector projection_matrices; - BOOST_FOREACH(const Camera& camera, cameras) { - Matrix P_ = (camera.pose().inverse().matrix()); - projection_matrices.push_back(camera.calibration().K()* (P_.block<3,4>(0,0)) ); - } + BOOST_FOREACH(const Camera& camera, cameras) + projection_matrices.push_back( + CameraProjectionMatrix(camera.calibration())( + camera.pose())); Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol); // The n refine using non-linear optimization @@ -253,7 +282,7 @@ Point3 triangulatePoint3( BOOST_FOREACH(const Camera& camera, cameras) { const Point3& p_local = camera.pose().transform_to(point); if (p_local.z() <= 0) - throw(TriangulationCheiralityException()); + throw(TriangulationCheiralityException()); } #endif diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index eba06f99a..11025fc0f 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -365,32 +365,50 @@ void JacobianFactor::print(const string& s, /* ************************************************************************* */ // Check if two linear factors are equal bool JacobianFactor::equals(const GaussianFactor& f_, double tol) const { - if (!dynamic_cast(&f_)) + static const bool verbose = false; + if (!dynamic_cast(&f_)) { + if (verbose) + cout << "JacobianFactor::equals: Incorrect type" << endl; return false; - else { + } else { const JacobianFactor& f(static_cast(f_)); // Check keys - if (keys() != f.keys()) + if (keys() != f.keys()) { + if (verbose) + cout << "JacobianFactor::equals: keys do not match" << endl; return false; + } // Check noise model - if ((model_ && !f.model_) || (!model_ && f.model_)) + if ((model_ && !f.model_) || (!model_ && f.model_)) { + if (verbose) + cout << "JacobianFactor::equals: noise model mismatch" << endl; return false; - if (model_ && f.model_ && !model_->equals(*f.model_, tol)) + } + if (model_ && f.model_ && !model_->equals(*f.model_, tol)) { + if (verbose) + cout << "JacobianFactor::equals: noise modesl are not equal" << endl; return false; + } // Check matrix sizes - if (!(Ab_.rows() == f.Ab_.rows() && Ab_.cols() == f.Ab_.cols())) + if (!(Ab_.rows() == f.Ab_.rows() && Ab_.cols() == f.Ab_.cols())) { + if (verbose) + cout << "JacobianFactor::equals: augmented size mismatch" << endl; return false; + } // Check matrix contents constABlock Ab1(Ab_.range(0, Ab_.nBlocks())); constABlock Ab2(f.Ab_.range(0, f.Ab_.nBlocks())); for (size_t row = 0; row < (size_t) Ab1.rows(); ++row) if (!equal_with_abs_tol(Ab1.row(row), Ab2.row(row), tol) - && !equal_with_abs_tol(-Ab1.row(row), Ab2.row(row), tol)) + && !equal_with_abs_tol(-Ab1.row(row), Ab2.row(row), tol)) { + if (verbose) + cout << "JacobianFactor::equals: matrix mismatch at row " << row << endl; return false; + } return true; } diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index 2031a4b73..a8b177b43 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -28,6 +28,7 @@ #include #include #include +#include using namespace std; @@ -70,6 +71,11 @@ boost::optional checkIfDiagonal(const Matrix M) { } } +/* ************************************************************************* */ +Vector Base::sigmas() const { + throw("Base::sigmas: sigmas() not implemented for this noise model"); +} + /* ************************************************************************* */ Gaussian::shared_ptr Gaussian::SqrtInformation(const Matrix& R, bool smart) { size_t m = R.rows(), n = R.cols(); @@ -79,24 +85,25 @@ Gaussian::shared_ptr Gaussian::SqrtInformation(const Matrix& R, bool smart) { if (smart) diagonal = checkIfDiagonal(R); if (diagonal) - return Diagonal::Sigmas(reciprocal(*diagonal), true); + return Diagonal::Sigmas(diagonal->array().inverse(), true); else return shared_ptr(new Gaussian(R.rows(), R)); } /* ************************************************************************* */ -Gaussian::shared_ptr Gaussian::Information(const Matrix& M, bool smart) { - size_t m = M.rows(), n = M.cols(); +Gaussian::shared_ptr Gaussian::Information(const Matrix& information, bool smart) { + size_t m = information.rows(), n = information.cols(); if (m != n) throw invalid_argument("Gaussian::Information: R not square"); boost::optional diagonal = boost::none; if (smart) - diagonal = checkIfDiagonal(M); + diagonal = checkIfDiagonal(information); if (diagonal) return Diagonal::Precisions(*diagonal, true); else { - Matrix R = RtR(M); - return shared_ptr(new Gaussian(R.rows(), R)); + Eigen::LLT llt(information); + Matrix R = llt.matrixU(); + return shared_ptr(new Gaussian(n, R)); } } @@ -111,13 +118,15 @@ Gaussian::shared_ptr Gaussian::Covariance(const Matrix& covariance, variances = checkIfDiagonal(covariance); if (variances) return Diagonal::Variances(*variances, true); - else - return shared_ptr(new Gaussian(n, inverse_square_root(covariance))); + else { + // TODO: can we do this more efficiently and still get an upper triangular nmatrix?? + return Information(covariance.inverse(), false); + } } /* ************************************************************************* */ void Gaussian::print(const string& name) const { - gtsam::print(thisR(), "Gaussian"); + gtsam::print(thisR(), name + "Gaussian"); } /* ************************************************************************* */ @@ -129,6 +138,12 @@ bool Gaussian::equals(const Base& expected, double tol) const { return equal_with_abs_tol(R(), p->R(), sqrt(tol)); } +/* ************************************************************************* */ +Vector Gaussian::sigmas() const { + // TODO(frank): can this be done faster? + return Vector((thisR().transpose() * thisR()).inverse().diagonal()).cwiseSqrt(); +} + /* ************************************************************************* */ Vector Gaussian::whiten(const Vector& v) const { return thisR() * v; @@ -221,9 +236,11 @@ Diagonal::Diagonal() : } /* ************************************************************************* */ -Diagonal::Diagonal(const Vector& sigmas) : - Gaussian(sigmas.size()), sigmas_(sigmas), invsigmas_(reciprocal(sigmas)), precisions_( - emul(invsigmas_, invsigmas_)) { +Diagonal::Diagonal(const Vector& sigmas) + : Gaussian(sigmas.size()), + sigmas_(sigmas), + invsigmas_(sigmas.array().inverse()), + precisions_(invsigmas_.array().square()) { } /* ************************************************************************* */ @@ -262,12 +279,12 @@ void Diagonal::print(const string& name) const { /* ************************************************************************* */ Vector Diagonal::whiten(const Vector& v) const { - return emul(v, invsigmas()); + return v.cwiseProduct(invsigmas_); } /* ************************************************************************* */ Vector Diagonal::unwhiten(const Vector& v) const { - return emul(v, sigmas_); + return v.cwiseProduct(sigmas_); } /* ************************************************************************* */ @@ -293,7 +310,7 @@ namespace internal { // switch precisions and invsigmas to finite value // TODO: why?? And, why not just ask s==0.0 below ? static void fix(const Vector& sigmas, Vector& precisions, Vector& invsigmas) { - for (size_t i = 0; i < sigmas.size(); ++i) + for (Vector::Index i = 0; i < sigmas.size(); ++i) if (!std::isfinite(1. / sigmas[i])) { precisions[i] = 0.0; invsigmas[i] = 0.0; @@ -342,7 +359,7 @@ Vector Constrained::whiten(const Vector& v) const { assert (b.size()==a.size()); Vector c(n); for( size_t i = 0; i < n; i++ ) { - const double& ai = a(i), &bi = b(i); + const double& ai = a(i), bi = b(i); c(i) = (bi==0.0) ? ai : ai/bi; // NOTE: not ediv_() } return c; @@ -404,8 +421,8 @@ SharedDiagonal Constrained::QR(Matrix& Ab) const { list Rd; Vector pseudo(m); // allocate storage for pseudo-inverse - Vector invsigmas = reciprocal(sigmas_); - Vector weights = emul(invsigmas,invsigmas); // calculate weights once + Vector invsigmas = sigmas_.array().inverse(); + Vector weights = invsigmas.array().square(); // calculate weights once // We loop over all columns, because the columns that can be eliminated // are not necessarily contiguous. For each one, estimate the corresponding @@ -542,16 +559,6 @@ Vector Base::weight(const Vector &error) const { return w; } -/** square root version of the weight function */ -Vector Base::sqrtWeight(const Vector &error) const { - const size_t n = error.rows(); - Vector w(n); - for ( size_t i = 0 ; i < n ; ++i ) - w(i) = sqrtWeight(error(i)); - return w; -} - - /** The following three functions reweight block matrices and a vector * according to their weight implementation */ @@ -560,8 +567,7 @@ void Base::reweight(Vector& error) const { const double w = sqrtWeight(error.norm()); error *= w; } else { - const Vector w = sqrtWeight(error); - error.array() *= w.array(); + error.array() *= weight(error).cwiseSqrt().array(); } } @@ -579,7 +585,7 @@ void Base::reweight(vector &A, Vector &error) const { BOOST_FOREACH(Matrix& Aj, A) { vector_scale_inplace(W,Aj); } - error = emul(W, error); + error = W.cwiseProduct(error); } } @@ -593,7 +599,7 @@ void Base::reweight(Matrix &A, Vector &error) const { else { const Vector W = sqrtWeight(error); vector_scale_inplace(W,A); - error = emul(W, error); + error = W.cwiseProduct(error); } } @@ -609,7 +615,7 @@ void Base::reweight(Matrix &A1, Matrix &A2, Vector &error) const { const Vector W = sqrtWeight(error); vector_scale_inplace(W,A1); vector_scale_inplace(W,A2); - error = emul(W, error); + error = W.cwiseProduct(error); } } @@ -627,7 +633,7 @@ void Base::reweight(Matrix &A1, Matrix &A2, Matrix &A3, Vector &error) const { vector_scale_inplace(W,A1); vector_scale_inplace(W,A2); vector_scale_inplace(W,A3); - error = emul(W, error); + error = W.cwiseProduct(error); } } @@ -641,7 +647,7 @@ void Null::print(const std::string &s="") const Null::shared_ptr Null::Create() { return shared_ptr(new Null()); } -Fair::Fair(const double c, const ReweightScheme reweight) +Fair::Fair(double c, const ReweightScheme reweight) : Base(reweight), c_(c) { if ( c_ <= 0 ) { cout << "mEstimator Fair takes only positive double in constructor. forced to 1.0" << endl; @@ -653,26 +659,26 @@ Fair::Fair(const double c, const ReweightScheme reweight) // Fair /* ************************************************************************* */ -double Fair::weight(const double &error) const +double Fair::weight(double error) const { return 1.0 / (1.0 + fabs(error)/c_); } void Fair::print(const std::string &s="") const { cout << s << "fair (" << c_ << ")" << endl; } -bool Fair::equals(const Base &expected, const double tol) const { +bool Fair::equals(const Base &expected, double tol) const { const Fair* p = dynamic_cast (&expected); if (p == NULL) return false; return fabs(c_ - p->c_ ) < tol; } -Fair::shared_ptr Fair::Create(const double c, const ReweightScheme reweight) +Fair::shared_ptr Fair::Create(double c, const ReweightScheme reweight) { return shared_ptr(new Fair(c, reweight)); } /* ************************************************************************* */ // Huber /* ************************************************************************* */ -Huber::Huber(const double k, const ReweightScheme reweight) +Huber::Huber(double k, const ReweightScheme reweight) : Base(reweight), k_(k) { if ( k_ <= 0 ) { cout << "mEstimator Huber takes only positive double in constructor. forced to 1.0" << endl; @@ -680,7 +686,7 @@ Huber::Huber(const double k, const ReweightScheme reweight) } } -double Huber::weight(const double &error) const { +double Huber::weight(double error) const { return (error < k_) ? (1.0) : (k_ / fabs(error)); } @@ -688,13 +694,13 @@ void Huber::print(const std::string &s="") const { cout << s << "huber (" << k_ << ")" << endl; } -bool Huber::equals(const Base &expected, const double tol) const { +bool Huber::equals(const Base &expected, double tol) const { const Huber* p = dynamic_cast(&expected); if (p == NULL) return false; return fabs(k_ - p->k_) < tol; } -Huber::shared_ptr Huber::Create(const double c, const ReweightScheme reweight) { +Huber::shared_ptr Huber::Create(double c, const ReweightScheme reweight) { return shared_ptr(new Huber(c, reweight)); } @@ -702,7 +708,7 @@ Huber::shared_ptr Huber::Create(const double c, const ReweightScheme reweight) { // Cauchy /* ************************************************************************* */ -Cauchy::Cauchy(const double k, const ReweightScheme reweight) +Cauchy::Cauchy(double k, const ReweightScheme reweight) : Base(reweight), k_(k) { if ( k_ <= 0 ) { cout << "mEstimator Cauchy takes only positive double in constructor. forced to 1.0" << endl; @@ -710,7 +716,7 @@ Cauchy::Cauchy(const double k, const ReweightScheme reweight) } } -double Cauchy::weight(const double &error) const { +double Cauchy::weight(double error) const { return k_*k_ / (k_*k_ + error*error); } @@ -718,24 +724,24 @@ void Cauchy::print(const std::string &s="") const { cout << s << "cauchy (" << k_ << ")" << endl; } -bool Cauchy::equals(const Base &expected, const double tol) const { +bool Cauchy::equals(const Base &expected, double tol) const { const Cauchy* p = dynamic_cast(&expected); if (p == NULL) return false; return fabs(k_ - p->k_) < tol; } -Cauchy::shared_ptr Cauchy::Create(const double c, const ReweightScheme reweight) { +Cauchy::shared_ptr Cauchy::Create(double c, const ReweightScheme reweight) { return shared_ptr(new Cauchy(c, reweight)); } /* ************************************************************************* */ // Tukey /* ************************************************************************* */ -Tukey::Tukey(const double c, const ReweightScheme reweight) +Tukey::Tukey(double c, const ReweightScheme reweight) : Base(reweight), c_(c) { } -double Tukey::weight(const double &error) const { +double Tukey::weight(double error) const { if (fabs(error) <= c_) { double xc2 = (error/c_)*(error/c_); double one_xc22 = (1.0-xc2)*(1.0-xc2); @@ -748,24 +754,24 @@ void Tukey::print(const std::string &s="") const { std::cout << s << ": Tukey (" << c_ << ")" << std::endl; } -bool Tukey::equals(const Base &expected, const double tol) const { +bool Tukey::equals(const Base &expected, double tol) const { const Tukey* p = dynamic_cast(&expected); if (p == NULL) return false; return fabs(c_ - p->c_) < tol; } -Tukey::shared_ptr Tukey::Create(const double c, const ReweightScheme reweight) { +Tukey::shared_ptr Tukey::Create(double c, const ReweightScheme reweight) { return shared_ptr(new Tukey(c, reweight)); } /* ************************************************************************* */ // Welsh /* ************************************************************************* */ -Welsh::Welsh(const double c, const ReweightScheme reweight) +Welsh::Welsh(double c, const ReweightScheme reweight) : Base(reweight), c_(c) { } -double Welsh::weight(const double &error) const { +double Welsh::weight(double error) const { double xc2 = (error/c_)*(error/c_); return std::exp(-xc2); } @@ -774,13 +780,13 @@ void Welsh::print(const std::string &s="") const { std::cout << s << ": Welsh (" << c_ << ")" << std::endl; } -bool Welsh::equals(const Base &expected, const double tol) const { +bool Welsh::equals(const Base &expected, double tol) const { const Welsh* p = dynamic_cast(&expected); if (p == NULL) return false; return fabs(c_ - p->c_) < tol; } -Welsh::shared_ptr Welsh::Create(const double c, const ReweightScheme reweight) { +Welsh::shared_ptr Welsh::Create(double c, const ReweightScheme reweight) { return shared_ptr(new Welsh(c, reweight)); } diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index 2532ac27e..923e7c7e9 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -26,7 +26,6 @@ #include #include #include -#include namespace gtsam { @@ -59,7 +58,7 @@ namespace gtsam { public: - /** primary constructor @param dim is the dimension of the model */ + /// primary constructor @param dim is the dimension of the model Base(size_t dim = 1):dim_(dim) {} virtual ~Base() {} @@ -75,14 +74,13 @@ namespace gtsam { virtual bool equals(const Base& expected, double tol=1e-9) const = 0; - /** - * Whiten an error vector. - */ + /// Calculate standard deviations + virtual Vector sigmas() const; + + /// Whiten an error vector. virtual Vector whiten(const Vector& v) const = 0; - /** - * Unwhiten an error vector. - */ + /// Unwhiten an error vector. virtual Vector unwhiten(const Vector& v) const = 0; virtual double distance(const Vector& v) const = 0; @@ -189,6 +187,7 @@ namespace gtsam { virtual void print(const std::string& name) const; virtual bool equals(const Base& expected, double tol=1e-9) const; + virtual Vector sigmas() const; virtual Vector whiten(const Vector& v) const; virtual Vector unwhiten(const Vector& v) const; @@ -220,9 +219,9 @@ namespace gtsam { /** * Whiten a system, in place as well */ - virtual void WhitenSystem(std::vector& A, Vector& b) const ; - virtual void WhitenSystem(Matrix& A, Vector& b) const ; - virtual void WhitenSystem(Matrix& A1, Matrix& A2, Vector& b) const ; + virtual void WhitenSystem(std::vector& A, Vector& b) const; + virtual void WhitenSystem(Matrix& A, Vector& b) const; + virtual void WhitenSystem(Matrix& A1, Matrix& A2, Vector& b) const; virtual void WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const; /** @@ -234,11 +233,15 @@ namespace gtsam { */ virtual boost::shared_ptr QR(Matrix& Ab) const; - /** - * Return R itself, but note that Whiten(H) is cheaper than R*H - */ + /// Return R itself, but note that Whiten(H) is cheaper than R*H virtual Matrix R() const { return thisR();} + /// Compute information matrix + virtual Matrix information() const { return thisR().transpose() * thisR(); } + + /// Compute covariance matrix + virtual Matrix covariance() const { return information().inverse(); } + private: /** Serialization function */ friend class boost::serialization::access; @@ -303,6 +306,7 @@ namespace gtsam { } virtual void print(const std::string& name) const; + virtual Vector sigmas() const { return sigmas_; } virtual Vector whiten(const Vector& v) const; virtual Vector unwhiten(const Vector& v) const; virtual Matrix Whiten(const Matrix& H) const; @@ -312,7 +316,6 @@ namespace gtsam { /** * Return standard deviations (sqrt of diagonal) */ - inline const Vector& sigmas() const { return sigmas_; } inline double sigma(size_t i) const { return sigmas_(i); } /** @@ -629,20 +632,23 @@ namespace gtsam { virtual ~Base() {} /// robust error function to implement - virtual double weight(const double &error) const = 0; + virtual double weight(double error) const = 0; virtual void print(const std::string &s) const = 0; - virtual bool equals(const Base& expected, const double tol=1e-8) const = 0; + virtual bool equals(const Base& expected, double tol=1e-8) const = 0; - inline double sqrtWeight(const double &error) const - { return std::sqrt(weight(error)); } + double sqrtWeight(double error) const { + return std::sqrt(weight(error)); + } /** produce a weight vector according to an error vector and the implemented * robust function */ Vector weight(const Vector &error) const; /** square root version of the weight function */ - Vector sqrtWeight(const Vector &error) const; + Vector sqrtWeight(const Vector &error) const { + return weight(error).cwiseSqrt(); + } /** reweight block matrices and a vector according to their weight implementation */ void reweight(Vector &error) const; @@ -667,9 +673,9 @@ namespace gtsam { Null(const ReweightScheme reweight = Block) : Base(reweight) {} virtual ~Null() {} - virtual double weight(const double& /*error*/) const { return 1.0; } + virtual double weight(double /*error*/) const { return 1.0; } virtual void print(const std::string &s) const; - virtual bool equals(const Base& /*expected*/, const double /*tol*/) const { return true; } + virtual bool equals(const Base& /*expected*/, double /*tol*/) const { return true; } static shared_ptr Create() ; private: @@ -686,12 +692,12 @@ namespace gtsam { public: typedef boost::shared_ptr shared_ptr; - Fair(const double c = 1.3998, const ReweightScheme reweight = Block); + Fair(double c = 1.3998, const ReweightScheme reweight = Block); virtual ~Fair() {} - virtual double weight(const double &error) const ; - virtual void print(const std::string &s) const ; - virtual bool equals(const Base& expected, const double tol=1e-8) const ; - static shared_ptr Create(const double c, const ReweightScheme reweight = Block) ; + virtual double weight(double error) const; + virtual void print(const std::string &s) const; + virtual bool equals(const Base& expected, double tol=1e-8) const; + static shared_ptr Create(double c, const ReweightScheme reweight = Block) ; protected: double c_; @@ -712,11 +718,11 @@ namespace gtsam { typedef boost::shared_ptr shared_ptr; virtual ~Huber() {} - Huber(const double k = 1.345, const ReweightScheme reweight = Block); - virtual double weight(const double &error) const ; - virtual void print(const std::string &s) const ; - virtual bool equals(const Base& expected, const double tol=1e-8) const ; - static shared_ptr Create(const double k, const ReweightScheme reweight = Block) ; + Huber(double k = 1.345, const ReweightScheme reweight = Block); + virtual double weight(double error) const; + virtual void print(const std::string &s) const; + virtual bool equals(const Base& expected, double tol=1e-8) const; + static shared_ptr Create(double k, const ReweightScheme reweight = Block) ; protected: double k_; @@ -741,11 +747,11 @@ namespace gtsam { typedef boost::shared_ptr shared_ptr; virtual ~Cauchy() {} - Cauchy(const double k = 0.1, const ReweightScheme reweight = Block); - virtual double weight(const double &error) const ; - virtual void print(const std::string &s) const ; - virtual bool equals(const Base& expected, const double tol=1e-8) const ; - static shared_ptr Create(const double k, const ReweightScheme reweight = Block) ; + Cauchy(double k = 0.1, const ReweightScheme reweight = Block); + virtual double weight(double error) const; + virtual void print(const std::string &s) const; + virtual bool equals(const Base& expected, double tol=1e-8) const; + static shared_ptr Create(double k, const ReweightScheme reweight = Block) ; protected: double k_; @@ -765,12 +771,12 @@ namespace gtsam { public: typedef boost::shared_ptr shared_ptr; - Tukey(const double c = 4.6851, const ReweightScheme reweight = Block); + Tukey(double c = 4.6851, const ReweightScheme reweight = Block); virtual ~Tukey() {} - virtual double weight(const double &error) const ; - virtual void print(const std::string &s) const ; - virtual bool equals(const Base& expected, const double tol=1e-8) const ; - static shared_ptr Create(const double k, const ReweightScheme reweight = Block) ; + virtual double weight(double error) const; + virtual void print(const std::string &s) const; + virtual bool equals(const Base& expected, double tol=1e-8) const; + static shared_ptr Create(double k, const ReweightScheme reweight = Block) ; protected: double c_; @@ -790,12 +796,12 @@ namespace gtsam { public: typedef boost::shared_ptr shared_ptr; - Welsh(const double c = 2.9846, const ReweightScheme reweight = Block); + Welsh(double c = 2.9846, const ReweightScheme reweight = Block); virtual ~Welsh() {} - virtual double weight(const double &error) const ; - virtual void print(const std::string &s) const ; - virtual bool equals(const Base& expected, const double tol=1e-8) const ; - static shared_ptr Create(const double k, const ReweightScheme reweight = Block) ; + virtual double weight(double error) const; + virtual void print(const std::string &s) const; + virtual bool equals(const Base& expected, double tol=1e-8) const; + static shared_ptr Create(double k, const ReweightScheme reweight = Block) ; protected: double c_; diff --git a/gtsam/linear/SubgraphPreconditioner.h b/gtsam/linear/SubgraphPreconditioner.h index d7dbbd124..350963bcf 100644 --- a/gtsam/linear/SubgraphPreconditioner.h +++ b/gtsam/linear/SubgraphPreconditioner.h @@ -70,7 +70,7 @@ namespace gtsam { Subgraph(const std::vector &indices) ; inline const Edges& edges() const { return edges_; } - inline const size_t size() const { return edges_.size(); } + inline size_t size() const { return edges_.size(); } EdgeIndices edgeIndices() const; iterator begin() { return edges_.begin(); } diff --git a/gtsam/linear/tests/testNoiseModel.cpp b/gtsam/linear/tests/testNoiseModel.cpp index 5c6b2729d..b2afb1709 100644 --- a/gtsam/linear/tests/testNoiseModel.cpp +++ b/gtsam/linear/tests/testNoiseModel.cpp @@ -33,15 +33,16 @@ using namespace gtsam; using namespace noiseModel; using namespace boost::assign; -static double sigma = 2, s_1=1.0/sigma, var = sigma*sigma, prc = 1.0/var; -static Matrix R = (Matrix(3, 3) << +static const double kSigma = 2, s_1=1.0/kSigma, kVariance = kSigma*kSigma, prc = 1.0/kVariance; +static const Matrix R = (Matrix(3, 3) << s_1, 0.0, 0.0, 0.0, s_1, 0.0, 0.0, 0.0, s_1).finished(); -static Matrix Sigma = (Matrix(3, 3) << - var, 0.0, 0.0, - 0.0, var, 0.0, - 0.0, 0.0, var).finished(); +static const Matrix kCovariance = (Matrix(3, 3) << + kVariance, 0.0, 0.0, + 0.0, kVariance, 0.0, + 0.0, 0.0, kVariance).finished(); +static const Vector3 kSigmas(kSigma, kSigma, kSigma); //static double inf = numeric_limits::infinity(); @@ -53,15 +54,19 @@ TEST(NoiseModel, constructors) // Construct noise models vector m; - m.push_back(Gaussian::SqrtInformation(R)); - m.push_back(Gaussian::Covariance(Sigma)); - //m.push_back(Gaussian::Information(Q)); - m.push_back(Diagonal::Sigmas((Vector(3) << sigma, sigma, sigma).finished())); - m.push_back(Diagonal::Variances((Vector(3) << var, var, var).finished())); - m.push_back(Diagonal::Precisions((Vector(3) << prc, prc, prc).finished())); - m.push_back(Isotropic::Sigma(3, sigma)); - m.push_back(Isotropic::Variance(3, var)); - m.push_back(Isotropic::Precision(3, prc)); + m.push_back(Gaussian::SqrtInformation(R,false)); + m.push_back(Gaussian::Covariance(kCovariance,false)); + m.push_back(Gaussian::Information(kCovariance.inverse(),false)); + m.push_back(Diagonal::Sigmas(kSigmas,false)); + m.push_back(Diagonal::Variances((Vector(3) << kVariance, kVariance, kVariance).finished(),false)); + m.push_back(Diagonal::Precisions((Vector(3) << prc, prc, prc).finished(),false)); + m.push_back(Isotropic::Sigma(3, kSigma,false)); + m.push_back(Isotropic::Variance(3, kVariance,false)); + m.push_back(Isotropic::Precision(3, prc,false)); + + // test kSigmas + BOOST_FOREACH(Gaussian::shared_ptr mi, m) + EXPECT(assert_equal(kSigmas,mi->sigmas())); // test whiten BOOST_FOREACH(Gaussian::shared_ptr mi, m) @@ -117,9 +122,9 @@ TEST(NoiseModel, equals) { Gaussian::shared_ptr g1 = Gaussian::SqrtInformation(R), g2 = Gaussian::SqrtInformation(eye(3,3)); - Diagonal::shared_ptr d1 = Diagonal::Sigmas((Vector(3) << sigma, sigma, sigma).finished()), + Diagonal::shared_ptr d1 = Diagonal::Sigmas((Vector(3) << kSigma, kSigma, kSigma).finished()), d2 = Diagonal::Sigmas(Vector3(0.1, 0.2, 0.3)); - Isotropic::shared_ptr i1 = Isotropic::Sigma(3, sigma), + Isotropic::shared_ptr i1 = Isotropic::Sigma(3, kSigma), i2 = Isotropic::Sigma(3, 0.7); EXPECT(assert_equal(*g1,*g1)); @@ -155,7 +160,7 @@ TEST(NoiseModel, ConstrainedConstructors ) Constrained::shared_ptr actual; size_t d = 3; double m = 100.0; - Vector sigmas = (Vector(3) << sigma, 0.0, 0.0).finished(); + Vector sigmas = (Vector(3) << kSigma, 0.0, 0.0).finished(); Vector mu = Vector3(200.0, 300.0, 400.0); actual = Constrained::All(d); // TODO: why should this be a thousand ??? Dummy variable? @@ -182,7 +187,7 @@ TEST(NoiseModel, ConstrainedMixed ) { Vector feasible = Vector3(1.0, 0.0, 1.0), infeasible = Vector3(1.0, 1.0, 1.0); - Diagonal::shared_ptr d = Constrained::MixedSigmas((Vector(3) << sigma, 0.0, sigma).finished()); + Diagonal::shared_ptr d = Constrained::MixedSigmas((Vector(3) << kSigma, 0.0, kSigma).finished()); // NOTE: we catch constrained variables elsewhere, so whitening does nothing EXPECT(assert_equal(Vector3(0.5, 1.0, 0.5),d->whiten(infeasible))); EXPECT(assert_equal(Vector3(0.5, 0.0, 0.5),d->whiten(feasible))); @@ -357,6 +362,44 @@ TEST(NoiseModel, robustNoise) DOUBLES_EQUAL(sqrt(k/100.0)*1000.0, A(1,1), 1e-8); } +/* ************************************************************************* */ +#define TEST_GAUSSIAN(gaussian)\ + EQUALITY(info, gaussian->information());\ + EQUALITY(cov, gaussian->covariance());\ + EXPECT(assert_equal(white, gaussian->whiten(e)));\ + EXPECT(assert_equal(e, gaussian->unwhiten(white)));\ + EXPECT_DOUBLES_EQUAL(251, gaussian->distance(e), 1e-9);\ + Matrix A = R.inverse(); Vector b = e;\ + gaussian->WhitenSystem(A, b);\ + EXPECT(assert_equal(I, A));\ + EXPECT(assert_equal(white, b)); + +TEST(NoiseModel, NonDiagonalGaussian) +{ + Matrix3 R; + R << 6, 5, 4, 0, 3, 2, 0, 0, 1; + const Matrix3 info = R.transpose() * R; + const Matrix3 cov = info.inverse(); + const Vector3 e(1, 1, 1), white = R * e; + Matrix I = Matrix3::Identity(); + + + { + SharedGaussian gaussian = Gaussian::SqrtInformation(R); + TEST_GAUSSIAN(gaussian); + } + + { + SharedGaussian gaussian = Gaussian::Information(info); + TEST_GAUSSIAN(gaussian); + } + + { + SharedGaussian gaussian = Gaussian::Covariance(cov); + TEST_GAUSSIAN(gaussian); + } +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ diff --git a/gtsam/navigation/ImuBias.h b/gtsam/navigation/ImuBias.h index 6281efb27..571fb7249 100644 --- a/gtsam/navigation/ImuBias.h +++ b/gtsam/navigation/ImuBias.h @@ -17,8 +17,10 @@ #pragma once +#include +#include +#include #include -#include /* * NOTES: @@ -131,8 +133,8 @@ public: /// print with optional string void print(const std::string& s = "") const { // explicit printing for now. - std::cout << s + ".biasAcc [" << biasAcc_.transpose() << "]" << std::endl; - std::cout << s + ".biasGyro [" << biasGyro_.transpose() << "]" << std::endl; + std::cout << s + ".Acc [" << biasAcc_.transpose() << "]" << std::endl; + std::cout << s + ".Gyro [" << biasGyro_.transpose() << "]" << std::endl; } /** equality up to tolerance */ diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp index 728251636..01de5a8f3 100644 --- a/gtsam/navigation/ImuFactor.cpp +++ b/gtsam/navigation/ImuFactor.cpp @@ -44,7 +44,6 @@ ImuFactor::PreintegratedMeasurements::PreintegratedMeasurements( //------------------------------------------------------------------------------ void ImuFactor::PreintegratedMeasurements::print(const string& s) const { PreintegrationBase::print(s); - cout << " preintMeasCov = \n [ " << preintMeasCov_ << " ]" << endl; } //------------------------------------------------------------------------------ @@ -75,8 +74,7 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement( const Rot3 Rincr = Rot3::Expmap(integratedOmega, D_Rincr_integratedOmega); // rotation increment computed from the current rotation rate measurement // Update Jacobians - updatePreintegratedJacobians(correctedAcc, D_Rincr_integratedOmega, Rincr, - deltaT); + updatePreintegratedJacobians(correctedAcc, D_Rincr_integratedOmega, Rincr, deltaT); // Update preintegrated measurements (also get Jacobian) const Matrix3 R_i = deltaRij(); // store this, which is useful to compute G_test @@ -89,8 +87,8 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement( // preintMeasCov = F * preintMeasCov * F.transpose() + G * (1/deltaT) * measurementCovariance * G.transpose(); // 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 blockwise, - // as G and measurementCovariance are blockdiagonal matrices + // 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() @@ -156,7 +154,8 @@ void ImuFactor::print(const string& s, const KeyFormatter& keyFormatter) const { << ")\n"; ImuFactorBase::print(""); _PIM_.print(" preintegrated measurements:"); - this->noiseModel_->print(" noise model: "); + // Print standard deviations on covariance only + cout << " noise model sigmas: " << this->noiseModel_->sigmas().transpose() << endl; } //------------------------------------------------------------------------------ diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index 6d870bee0..50e6c835f 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -110,7 +110,7 @@ public: * 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 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) */ diff --git a/gtsam/navigation/PreintegratedRotation.h b/gtsam/navigation/PreintegratedRotation.h index 3cce1b86e..2379f58af 100644 --- a/gtsam/navigation/PreintegratedRotation.h +++ b/gtsam/navigation/PreintegratedRotation.h @@ -69,11 +69,8 @@ public: /// Needed for testable void print(const std::string& s) const { std::cout << s << std::endl; - std::cout << "deltaTij [" << deltaTij_ << "]" << std::endl; - deltaRij_.print(" deltaRij "); - std::cout << "delRdelBiasOmega [" << delRdelBiasOmega_ << "]" << std::endl; - std::cout << "gyroscopeCovariance [" << gyroscopeCovariance_ << "]" - << std::endl; + std::cout << " deltaTij [" << deltaTij_ << "]" << std::endl; + std::cout << " deltaRij.ypr = (" << deltaRij_.ypr().transpose() << ")" << std::endl; } /// Needed for testable diff --git a/gtsam/navigation/PreintegrationBase.cpp b/gtsam/navigation/PreintegrationBase.cpp new file mode 100644 index 000000000..496569599 --- /dev/null +++ b/gtsam/navigation/PreintegrationBase.cpp @@ -0,0 +1,357 @@ +/* ---------------------------------------------------------------------------- + + * 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 + **/ + +#include "PreintegrationBase.h" + +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) { +} + +/// 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(); + 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 + } +} + +/// 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); +} + +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); + + // 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(); + 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 + } +} + +/// 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 { + + const imuBias::ConstantBias biasIncr = bias_i - biasHat(); + const Vector3& biasAccIncr = biasIncr.accelerometer(); + const Vector3& biasOmegaIncr = biasIncr.gyroscope(); + + 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; + } + (*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; +} + +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) { +} + +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) { +} + +} /// namespace gtsam diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index 522b564a8..f6b24e752 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -23,6 +23,9 @@ #include #include +#include +#include +#include namespace gtsam { @@ -34,9 +37,10 @@ struct PoseVelocityBias { 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) { } }; @@ -46,24 +50,24 @@ struct PoseVelocityBias { * It includes the definitions of the preintegrated variables and the methods * to access, print, and compare them. */ -class PreintegrationBase: public PreintegratedRotation { +class PreintegrationBase : public PreintegratedRotation { - imuBias::ConstantBias biasHat_; ///< Acceleration and angular rate bias values used during preintegration - bool use2ndOrderIntegration_; ///< Controls the order of integration + imuBias::ConstantBias biasHat_; ///< Acceleration and angular rate bias values used during preintegration + bool use2ndOrderIntegration_; ///< Controls the order of integration - 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) + Vector3 deltaPij_; ///< Preintegrated relative position (does not take into account velocity at time i, see deltap+, in [2]) (in frame i) + Vector3 deltaVij_; ///< Preintegrated relative velocity (in global frame) - Matrix3 delPdelBiasAcc_; ///< Jacobian of preintegrated position w.r.t. acceleration bias - Matrix3 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 + 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 - Matrix3 accelerometerCovariance_; ///< continuous-time "Covariance" of accelerometer measurements - Matrix3 integrationCovariance_; ///< continuous-time "Covariance" describing integration uncertainty + const Matrix3 accelerometerCovariance_; ///< continuous-time "Covariance" of accelerometer measurements + const Matrix3 integrationCovariance_; ///< continuous-time "Covariance" describing integration uncertainty /// (to compensate errors in Euler integration) -public: + public: /** * Default constructor, initializes the variables in the base class @@ -71,18 +75,9 @@ public: * @param use2ndOrderIntegration Controls the order of integration * (if false: p(t+1) = p(t) + v(t) deltaT ; if true: p(t+1) = p(t) + v(t) deltaT + 0.5 * acc(t) deltaT^2) */ - 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) { - } + PreintegrationBase(const imuBias::ConstantBias& bias, const Matrix3& measuredAccCovariance, + const Matrix3& measuredOmegaCovariance, + const Matrix3&integrationErrorCovariance, const bool use2ndOrderIntegration); /// methods to access class variables bool use2ndOrderIntegration() const { @@ -99,7 +94,7 @@ public: } Vector6 biasHatVector() const { return biasHat_.vector(); - } // expensive + } // expensive const Matrix3& delPdelBiasAcc() const { return delPdelBiasAcc_; } @@ -120,319 +115,48 @@ public: return integrationCovariance_; } - /// Needed for testable - void print(const std::string& s) const { - PreintegratedRotation::print(s); - std::cout << " accelerometerCovariance [ " << accelerometerCovariance_ - << " ]" << std::endl; - std::cout << " integrationCovariance [ " << integrationCovariance_ << " ]" - << std::endl; - std::cout << " deltaPij [ " << deltaPij_.transpose() << " ]" << std::endl; - std::cout << " deltaVij [ " << deltaVij_.transpose() << " ]" << std::endl; - biasHat_.print(" biasHat"); - } + /// print + void print(const std::string& s) const; - /// Needed for testable - bool 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); - } + /// check equality + bool equals(const PreintegrationBase& other, double tol) const; /// Re-initialize PreintegratedMeasurements - void resetIntegration() { - PreintegratedRotation::resetIntegration(); - deltaPij_ = Vector3::Zero(); - deltaVij_ = Vector3::Zero(); - delPdelBiasAcc_ = Z_3x3; - delPdelBiasOmega_ = Z_3x3; - delVdelBiasAcc_ = Z_3x3; - delVdelBiasOmega_ = Z_3x3; - } + void resetIntegration(); /// Update preintegrated measurements - void updatePreintegratedMeasurements(const Vector3& correctedAcc, - const Rot3& incrR, const double deltaT, OptionalJacobian<9, 9> F) { - - Matrix3 dRij = deltaRij(); // expensive - 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) { - 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 updatePreintegratedMeasurements(const Vector3& correctedAcc, const Rot3& incrR, + const double deltaT, OptionalJacobian<9, 9> F); /// Update Jacobians to be used during preintegration void updatePreintegratedJacobians(const Vector3& correctedAcc, - const Matrix3& D_Rincr_integratedOmega, const Rot3& incrR, - double deltaT) { - Matrix3 dRij = deltaRij(); // expensive - 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); - } + const Matrix3& D_Rincr_integratedOmega, const Rot3& incrR, + double deltaT); void correctMeasurementsByBiasAndSensorPose(const Vector3& measuredAcc, - const Vector3& measuredOmega, Vector3& correctedAcc, - Vector3& correctedOmega, boost::optional body_P_sensor) { - correctedAcc = biasHat_.correctAccelerometer(measuredAcc); - correctedOmega = biasHat_.correctGyroscope(measuredOmega); - - // 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(); - 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 - } - } + const Vector3& measuredOmega, Vector3& correctedAcc, + Vector3& correctedOmega, + boost::optional body_P_sensor); /// 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, + 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 { - - const imuBias::ConstantBias biasIncr = bias_i - biasHat(); - const Vector3& biasAccIncr = biasIncr.accelerometer(); - const Vector3& biasOmegaIncr = biasIncr.gyroscope(); - - const Rot3& Rot_i = pose_i.rotation(); - const Vector3& pos_i = pose_i.translation().vector(); - - // Predict state at time j - /* ---------------------------------------------------------------------------------------------------- */ - Vector3 deltaPij_biascorrected = deltaPij() + delPdelBiasAcc() * biasAccIncr - + delPdelBiasOmega() * biasOmegaIncr; - if (deltaPij_biascorrected_out) // if desired, store this - *deltaPij_biascorrected_out = deltaPij_biascorrected; - - Vector3 pos_j = pos_i + Rot_i.matrix() * deltaPij_biascorrected - + vel_i * deltaTij() - - omegaCoriolis.cross(vel_i) * deltaTij() * deltaTij() // Coriolis term - we got rid of the 2 wrt ins paper - + 0.5 * gravity * deltaTij() * deltaTij(); - - Vector3 deltaVij_biascorrected = deltaVij() + delVdelBiasAcc() * biasAccIncr - + delVdelBiasOmega() * biasOmegaIncr; - if (deltaVij_biascorrected_out) // if desired, store this - *deltaVij_biascorrected_out = deltaVij_biascorrected; - - Vector3 vel_j = Vector3( - vel_i + Rot_i.matrix() * deltaVij_biascorrected - - 2 * omegaCoriolis.cross(vel_i) * deltaTij() // Coriolis term - + gravity * deltaTij()); - - if (use2ndOrderCoriolis) { - pos_j += -0.5 * omegaCoriolis.cross(omegaCoriolis.cross(pos_i)) - * deltaTij() * deltaTij(); // 2nd order coriolis term for position - vel_j += -omegaCoriolis.cross(omegaCoriolis.cross(pos_i)) * deltaTij(); // 2nd order term for velocity - } - - const Rot3 deltaRij_biascorrected = biascorrectedDeltaRij(biasOmegaIncr); - // deltaRij_biascorrected = deltaRij * expmap(delRdelBiasOmega * biasOmegaIncr) - - Vector3 biascorrectedOmega = Rot3::Logmap(deltaRij_biascorrected); - Vector3 correctedOmega = biascorrectedOmega - - Rot_i.inverse().matrix() * omegaCoriolis * deltaTij(); // Coriolis term - const Rot3 correctedDeltaRij = Rot3::Expmap(correctedOmega); - const Rot3 Rot_j = Rot_i.compose(correctedDeltaRij); - - Pose3 pose_j = Pose3(Rot_j, Point3(pos_j)); - return PoseVelocityBias(pose_j, vel_j, bias_i); // bias is predicted as a constant - } + boost::optional deltaVij_biascorrected_out = 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, + 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; - // 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 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() - * (pos_j - predictedState_j.pose.translation().vector()); - - // Ri.transpose() is important here to preserve a model with *additive* Gaussian noise of correct covariance - const Vector3 fv = Ri.transpose() * (vel_j - predictedState_j.velocity); - - // 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; - Vector3 biascorrectedOmega = biascorrectedThetaRij(biasOmegaIncr, - H5 ? &D_cThetaRij_biasOmegaIncr : 0); - - // Coriolis term, note inconsistent with AHRS, where coriolisHat is *after* integration - const Vector3 coriolis = integrateCoriolis(Ri, omegaCoriolis); - Vector3 correctedOmega = biascorrectedOmega - coriolis; - - // Calculate Jacobians, matrices below needed only for some Jacobians... - Vector3 fR; - Rot3 correctedDeltaRij, fRrot; - Matrix3 D_cDeltaRij_cOmega, D_coriolis, D_fR_fRrot, - Ritranspose_omegaCoriolisHat; - - if (H1 || H2) - Ritranspose_omegaCoriolisHat = Ri.transpose() - * skewSymmetric(omegaCoriolis); - - // This is done to save computation: we ask for the jacobians only when they are needed - if (H1 || H2 || H3 || H4 || H5) { - correctedDeltaRij = Rot3::Expmap(correctedOmega, D_cDeltaRij_cOmega); - // Residual rotation error - fRrot = correctedDeltaRij.between(Ri.between(Rj)); - fR = Rot3::Logmap(fRrot, D_fR_fRrot); - D_coriolis = -D_cDeltaRij_cOmega * skewSymmetric(coriolis); - } else { - correctedDeltaRij = Rot3::Expmap(correctedOmega); - // Residual rotation error - fRrot = correctedDeltaRij.between(Ri.between(Rj)); - fR = Rot3::Logmap(fRrot); - } - if (H1) { - H1->resize(9, 6); - Matrix3 dfPdPi = -I_3x3; - Matrix3 dfVdPi = Z_3x3; - if (use2ndOrderCoriolis) { - // this is the same as: Ri.transpose() * omegaCoriolisHat * omegaCoriolisHat * Ri.matrix() - Matrix3 temp = Ritranspose_omegaCoriolisHat - * (-Ritranspose_omegaCoriolisHat.transpose()); - dfPdPi += 0.5 * temp * deltaTij() * deltaTij(); - dfVdPi += temp * deltaTij(); - } - (*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->resize(9, 3); - (*H2) << - // dfP/dVi - -Ri.transpose() * deltaTij() - + Ritranspose_omegaCoriolisHat * deltaTij() * deltaTij(), // Coriolis term - we got rid of the 2 wrt ins paper - // dfV/dVi - -Ri.transpose() + 2 * Ritranspose_omegaCoriolisHat * deltaTij(), // Coriolis term - // dfR/dVi - Z_3x3; - } - if (H3) { - H3->resize(9, 6); - (*H3) << - // dfP/dPosej - Z_3x3, Ri.transpose() * Rj.matrix(), - // dfV/dPosej - Matrix::Zero(3, 6), - // dfR/dPosej - D_fR_fRrot, Z_3x3; - } - if (H4) { - H4->resize(9, 3); - (*H4) << - // dfP/dVj - Z_3x3, - // dfV/dVj - Ri.transpose(), - // 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->resize(9, 6); - (*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; - } - -private: + private: /** Serialization function */ friend class boost::serialization::access; template @@ -450,26 +174,22 @@ private: class ImuBase { -protected: + 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 + 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: + public: - ImuBase() : - gravity_(Vector3(0.0, 0.0, 9.81)), omegaCoriolis_(Vector3(0.0, 0.0, 0.0)), body_P_sensor_( - boost::none), use2ndOrderCoriolis_(false) { - } + /// 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) : - gravity_(gravity), omegaCoriolis_(omegaCoriolis), body_P_sensor_( - body_P_sensor), use2ndOrderCoriolis_(use2ndOrderCoriolis) { - } + boost::optional body_P_sensor = boost::none, + const bool use2ndOrderCoriolis = false); const Vector3& gravity() const { return gravity_; @@ -480,4 +200,4 @@ public: }; -} /// namespace gtsam +} /// namespace gtsam diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index b8fc8062c..c27921fc9 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -17,6 +17,7 @@ #include #include +#include #include #include #include @@ -81,19 +82,27 @@ Rot3 updatePreintegratedRot(const Rot3& deltaRij_old, return deltaRij_new; } -// Auxiliary functions to test preintegrated Jacobians -// delPdelBiasAcc_ delPdelBiasOmega_ delVdelBiasAcc_ delVdelBiasOmega_ delRdelBiasOmega_ +// Define covariance matrices /* ************************************************************************* */ 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(); + +// Auxiliary functions to test preintegrated Jacobians +// delPdelBiasAcc_ delPdelBiasOmega_ delVdelBiasAcc_ delVdelBiasOmega_ delRdelBiasOmega_ +/* ************************************************************************* */ ImuFactor::PreintegratedMeasurements evaluatePreintegratedMeasurements( const imuBias::ConstantBias& bias, const list& measuredAccs, const list& measuredOmegas, const list& deltaTs, const bool use2ndOrderIntegration = false) { ImuFactor::PreintegratedMeasurements result(bias, - accNoiseVar * Matrix3::Identity(), omegaNoiseVar * Matrix3::Identity(), - intNoiseVar * Matrix3::Identity(), use2ndOrderIntegration); + kMeasuredAccCovariance, + kMeasuredOmegaCovariance, + kIntegrationErrorCovariance, + use2ndOrderIntegration); list::const_iterator itAcc = measuredAccs.begin(); list::const_iterator itOmega = measuredOmegas.begin(); @@ -156,8 +165,11 @@ TEST( ImuFactor, PreintegratedMeasurements ) { bool use2ndOrderIntegration = true; // Actual preintegrated values - ImuFactor::PreintegratedMeasurements actual1(bias, Matrix3::Zero(), - Matrix3::Zero(), Matrix3::Zero(), use2ndOrderIntegration); + ImuFactor::PreintegratedMeasurements actual1(bias, + kMeasuredAccCovariance, + kMeasuredOmegaCovariance, + kIntegrationErrorCovariance, + use2ndOrderIntegration); actual1.integrateMeasurement(measuredAcc, measuredOmega, deltaT); EXPECT( @@ -208,8 +220,11 @@ TEST( ImuFactor, ErrorAndJacobians ) { Vector3 measuredAcc = x1.rotation().unrotate(-Point3(gravity)).vector(); double deltaT = 1.0; bool use2ndOrderIntegration = true; - ImuFactor::PreintegratedMeasurements pre_int_data(bias, Matrix3::Zero(), - Matrix3::Zero(), Matrix3::Zero(), use2ndOrderIntegration); + ImuFactor::PreintegratedMeasurements pre_int_data(bias, + kMeasuredAccCovariance, + kMeasuredOmegaCovariance, + kIntegrationErrorCovariance, + use2ndOrderIntegration); pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); // Create factor @@ -260,6 +275,40 @@ TEST( ImuFactor, ErrorAndJacobians ) { 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)); + + 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)); + + // Make sure the whitening is done correctly + Matrix cov = pre_int_data.preintMeasCov(); + Matrix R = RtR(cov.inverse()); + Vector whitened = R * errorActual; + EXPECT(assert_equal(0.5 * whitened.squaredNorm(), factor.error(values), 1e-6)); + + /////////////////////////////////////////////////////////////////////////////// + // 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)); } /* ************************************************************************* */ @@ -284,8 +333,8 @@ TEST( ImuFactor, ErrorAndJacobianWithBiases ) { double deltaT = 1.0; ImuFactor::PreintegratedMeasurements pre_int_data( - imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.1)), - Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero()); + 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); // Create factor @@ -354,8 +403,8 @@ TEST( ImuFactor, ErrorAndJacobianWith2ndOrderCoriolis ) { double deltaT = 1.0; ImuFactor::PreintegratedMeasurements pre_int_data( - imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.1)), - Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero()); + 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); // Create factor @@ -874,8 +923,8 @@ TEST( ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement ) { Point3(1, 0, 0)); ImuFactor::PreintegratedMeasurements pre_int_data( - imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), - Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero()); + imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), kMeasuredAccCovariance, + kMeasuredOmegaCovariance, kIntegrationErrorCovariance); pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); @@ -933,8 +982,8 @@ TEST(ImuFactor, PredictPositionAndVelocity) { I6x6 = Matrix::Identity(6, 6); ImuFactor::PreintegratedMeasurements pre_int_data( - imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), - Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), true); + 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); @@ -974,8 +1023,8 @@ TEST(ImuFactor, PredictRotation) { I6x6 = Matrix::Identity(6, 6); ImuFactor::PreintegratedMeasurements pre_int_data( - imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), - Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), true); + 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); diff --git a/gtsam/nonlinear/Expression-inl.h b/gtsam/nonlinear/Expression-inl.h index a86e7312a..6c6c155c7 100644 --- a/gtsam/nonlinear/Expression-inl.h +++ b/gtsam/nonlinear/Expression-inl.h @@ -19,773 +19,244 @@ #pragma once -#include -#include -#include +#include -#include #include -#include -#include - -// template meta-programming headers -#include -namespace MPL = boost::mpl::placeholders; - -#include // operator typeid -#include - -class ExpressionFactorBinaryTest; -// Forward declare for testing +#include +#include namespace gtsam { -const unsigned TraceAlignment = 16; - template -T & upAlign(T & value, unsigned requiredAlignment = TraceAlignment) { - // right now only word sized types are supported. - // Easy to extend if needed, - // by somehow inferring the unsigned integer of same size - BOOST_STATIC_ASSERT(sizeof(T) == sizeof(size_t)); - size_t & uiValue = reinterpret_cast(value); - size_t misAlignment = uiValue % requiredAlignment; - if (misAlignment) { - uiValue += requiredAlignment - misAlignment; - } - return value; -} -template -T upAligned(T value, unsigned requiredAlignment = TraceAlignment) { - return upAlign(value, requiredAlignment); +void Expression::print(const std::string& s) const { + std::cout << s << *root_ << std::endl; } template -class Expression; - -/** - * Expressions are designed to write their derivatives into an already allocated - * Jacobian of the correct size, of type VerticalBlockMatrix. - * The JacobianMap provides a mapping from keys to the underlying blocks. - */ -class JacobianMap { - const FastVector& keys_; - VerticalBlockMatrix& Ab_; -public: - JacobianMap(const FastVector& keys, VerticalBlockMatrix& Ab) : - keys_(keys), Ab_(Ab) { - } - /// Access via key - VerticalBlockMatrix::Block operator()(Key key) { - FastVector::const_iterator it = std::find(keys_.begin(), keys_.end(), - key); - DenseIndex block = it - keys_.begin(); - return Ab_(block); - } -}; - -//----------------------------------------------------------------------------- - -namespace internal { - -template -struct UseBlockIf { - static void addToJacobian(const Eigen::MatrixBase& dTdA, - JacobianMap& jacobians, Key key) { - // block makes HUGE difference - jacobians(key).block( - 0, 0) += dTdA; - } - ; -}; -/// Handle Leaf Case for Dynamic Matrix type (slower) -template -struct UseBlockIf { - static void addToJacobian(const Eigen::MatrixBase& dTdA, - JacobianMap& jacobians, Key key) { - jacobians(key) += dTdA; - } -}; +Expression::Expression(const T& value) : + root_(new internal::ConstantExpression(value)) { } -/// Handle Leaf Case: reverse AD ends here, by writing a matrix into Jacobians -template -void handleLeafCase(const Eigen::MatrixBase& dTdA, - JacobianMap& jacobians, Key key) { - internal::UseBlockIf< - Derived::RowsAtCompileTime != Eigen::Dynamic - && Derived::ColsAtCompileTime != Eigen::Dynamic, Derived>::addToJacobian( - dTdA, jacobians, key); +template +Expression::Expression(const Key& key) : + root_(new internal::LeafExpression(key)) { } -//----------------------------------------------------------------------------- -/** - * The ExecutionTrace class records a tree-structured expression's execution. - * - * The class looks a bit complicated but it is so for performance. - * It is a tagged union that obviates the need to create - * a ExecutionTrace subclass for Constants and Leaf Expressions. Instead - * the key for the leaf is stored in the space normally used to store a - * CallRecord*. Nothing is stored for a Constant. - * - * A full execution trace of a Binary(Unary(Binary(Leaf,Constant)),Leaf) would be: - * Trace(Function) -> - * BinaryRecord with two traces in it - * trace1(Function) -> - * UnaryRecord with one trace in it - * trace1(Function) -> - * BinaryRecord with two traces in it - * trace1(Leaf) - * trace2(Constant) - * trace2(Leaf) - * Hence, there are three Record structs, written to memory by traceExecution - */ -template -class ExecutionTrace { +template +Expression::Expression(const Symbol& symbol) : + root_(new internal::LeafExpression(symbol)) { +} + +template +Expression::Expression(unsigned char c, size_t j) : + root_(new internal::LeafExpression(Symbol(c, j))) { +} + +/// Construct a unary function expression +template +template +Expression::Expression(typename UnaryFunction::type function, + const Expression& expression) : + root_(new internal::UnaryExpression(function, expression)) { +} + +/// Construct a binary function expression +template +template +Expression::Expression(typename BinaryFunction::type function, + const Expression& expression1, const Expression& expression2) : + root_( + new internal::BinaryExpression(function, expression1, + expression2)) { +} + +/// Construct a ternary function expression +template +template +Expression::Expression(typename TernaryFunction::type function, + const Expression& expression1, const Expression& expression2, + const Expression& expression3) : + root_( + new internal::TernaryExpression(function, expression1, + expression2, expression3)) { +} + +/// Construct a nullary method expression +template +template +Expression::Expression(const Expression& expression, + T (A::*method)(typename MakeOptionalJacobian::type) const) : + root_( + new internal::UnaryExpression(boost::bind(method, _1, _2), + expression)) { +} + +/// Construct a unary method expression +template +template +Expression::Expression(const Expression& expression1, + T (A1::*method)(const A2&, typename MakeOptionalJacobian::type, + typename MakeOptionalJacobian::type) const, + const Expression& expression2) : + root_( + new internal::BinaryExpression( + boost::bind(method, _1, _2, _3, _4), expression1, expression2)) { +} + +/// Construct a binary method expression +template +template +Expression::Expression(const Expression& expression1, + T (A1::*method)(const A2&, const A3&, + typename MakeOptionalJacobian::type, + typename MakeOptionalJacobian::type, + typename MakeOptionalJacobian::type) const, + const Expression& expression2, const Expression& expression3) : + root_( + new internal::TernaryExpression( + boost::bind(method, _1, _2, _3, _4, _5, _6), expression1, + expression2, expression3)) { +} + +template +std::set Expression::keys() const { + return root_->keys(); +} + +template +void Expression::dims(std::map& map) const { + root_->dims(map); +} + +template +T Expression::value(const Values& values, + boost::optional&> H) const { + + if (H) { + // Call private version that returns derivatives in H + KeysAndDims pair = keysAndDims(); + return valueAndDerivatives(values, pair.first, pair.second, *H); + } else + // no derivatives needed, just return value + return root_->value(values); +} + +template +const boost::shared_ptr >& Expression::root() const { + return root_; +} + +template +size_t Expression::traceSize() const { + return root_->traceSize(); +} + +// Private methods: + +template +T Expression::valueAndDerivatives(const Values& values, + const FastVector& keys, const FastVector& dims, + std::vector& H) const { + + // H should be pre-allocated + assert(H.size()==keys.size()); + + // Pre-allocate and zero VerticalBlockMatrix static const int Dim = traits::dimension; - enum { - Constant, Leaf, Function - } kind; - union { - Key key; - CallRecord* ptr; - } content; -public: - /// Pointer always starts out as a Constant - ExecutionTrace() : - kind(Constant) { - } - /// Change pointer to a Leaf Record - void setLeaf(Key key) { - kind = Leaf; - content.key = key; - } - /// Take ownership of pointer to a Function Record - void setFunction(CallRecord* record) { - kind = Function; - content.ptr = record; - } - /// Print - void print(const std::string& indent = "") const { - if (kind == Constant) - std::cout << indent << "Constant" << std::endl; - else if (kind == Leaf) - std::cout << indent << "Leaf, key = " << content.key << std::endl; - else if (kind == Function) { - std::cout << indent << "Function" << std::endl; - content.ptr->print(indent + " "); - } - } - /// Return record pointer, quite unsafe, used only for testing - template - boost::optional record() { - if (kind != Function) - return boost::none; - else { - Record* p = dynamic_cast(content.ptr); - return p ? boost::optional(p) : boost::none; - } - } - /** - * *** This is the main entry point for reverse AD, called from Expression *** - * Called only once, either inserts I into Jacobians (Leaf) or starts AD (Function) - */ - typedef Eigen::Matrix JacobianTT; - void startReverseAD1(JacobianMap& jacobians) const { - if (kind == Leaf) { - // This branch will only be called on trivial Leaf expressions, i.e. Priors - static const JacobianTT I = JacobianTT::Identity(); - handleLeafCase(I, jacobians, content.key); - } else if (kind == Function) - // This is the more typical entry point, starting the AD pipeline - // Inside startReverseAD2 the correctly dimensioned pipeline is chosen. - content.ptr->startReverseAD2(jacobians); - } - // Either add to Jacobians (Leaf) or propagate (Function) - template - void reverseAD1(const Eigen::MatrixBase & dTdA, - JacobianMap& jacobians) const { - if (kind == Leaf) - handleLeafCase(dTdA, jacobians, content.key); - else if (kind == Function) - content.ptr->reverseAD2(dTdA, jacobians); - } + VerticalBlockMatrix Ab(dims, Dim); + Ab.matrix().setZero(); + internal::JacobianMap jacobianMap(keys, Ab); - /// Define type so we can apply it as a meta-function - typedef ExecutionTrace type; -}; + // Call unsafe version + T result = valueAndJacobianMap(values, jacobianMap); -/// Storage type for the execution trace. -/// It enforces the proper alignment in a portable way. -/// Provide a traceSize() sized array of this type to traceExecution as traceStorage. -typedef boost::aligned_storage<1, TraceAlignment>::type ExecutionTraceStorage; + // Copy blocks into the vector of jacobians passed in + for (DenseIndex i = 0; i < static_cast(keys.size()); i++) + H[i] = Ab(i); -//----------------------------------------------------------------------------- -/** - * Expression node. The superclass for objects that do the heavy lifting - * An Expression has a pointer to an ExpressionNode underneath - * allowing Expressions to have polymorphic behaviour even though they - * are passed by value. This is the same way boost::function works. - * http://loki-lib.sourceforge.net/html/a00652.html - */ -template -class ExpressionNode { + return result; +} -protected: - - size_t traceSize_; - - /// Constructor, traceSize is size of the execution trace of expression rooted here - ExpressionNode(size_t traceSize = 0) : - traceSize_(traceSize) { - } - -public: - - /// Destructor - virtual ~ExpressionNode() { - } - - /// Streaming - GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, - const ExpressionNode& node) { - os << "Expression of type " << typeid(T).name(); - if (node.traceSize_>0) os << ", trace size = " << node.traceSize_; - os << "\n"; - return os; - } - - /// Return keys that play in this expression as a set - virtual std::set keys() const { - std::set keys; - return keys; - } - - /// Return dimensions for each argument, as a map - virtual void dims(std::map& map) const { - } - - // Return size needed for memory buffer in traceExecution - size_t traceSize() const { - return traceSize_; - } - - /// Return value - virtual T value(const Values& values) const = 0; - - /// Construct an execution trace for reverse AD - virtual T traceExecution(const Values& values, ExecutionTrace& trace, - ExecutionTraceStorage* traceStorage) const = 0; -}; - -//----------------------------------------------------------------------------- -/// Constant Expression -template -class ConstantExpression: public ExpressionNode { - - /// The constant value - T constant_; - - /// Constructor with a value, yielding a constant - ConstantExpression(const T& value) : - constant_(value) { - } - - friend class Expression ; - -public: - - /// Return value - virtual T value(const Values& values) const { - return constant_; - } - - /// Construct an execution trace for reverse AD - virtual T traceExecution(const Values& values, ExecutionTrace& trace, - ExecutionTraceStorage* traceStorage) const { - return constant_; - } -}; - - -//----------------------------------------------------------------------------- -/// Leaf Expression, if no chart is given, assume default chart and value_type is just the plain value template -class LeafExpression : public ExpressionNode { - typedef T value_type; +T Expression::traceExecution(const Values& values, + internal::ExecutionTrace& trace, void* traceStorage) const { + return root_->traceExecution(values, trace, + static_cast(traceStorage)); +} - /// The key into values - Key key_; +template +T Expression::valueAndJacobianMap(const Values& values, + internal::JacobianMap& jacobians) const { + // The following piece of code is absolutely crucial for performance. + // We allocate a block of memory on the stack, which can be done at runtime + // with modern C++ compilers. The traceExecution then fills this memory + // with an execution trace, made up entirely of "Record" structs, see + // the FunctionalNode class in expression-inl.h + size_t size = traceSize(); - /// Constructor with a single key - LeafExpression(Key key) : - key_(key) { - } - // todo: do we need a virtual destructor here too? - - friend class Expression ; - -public: - - /// Return keys that play in this expression - virtual std::set keys() const { - std::set keys; - keys.insert(key_); - return keys; - } - - /// Return dimensions for each argument - virtual void dims(std::map& map) const { - map[key_] = traits::dimension; - } - - /// Return value - virtual T value(const Values& values) const { - return values.at(key_); - } - - /// Construct an execution trace for reverse AD - virtual T traceExecution(const Values& values, ExecutionTrace& trace, - ExecutionTraceStorage* traceStorage) const { - trace.setLeaf(key_); - return values.at(key_); - } - -}; - -//----------------------------------------------------------------------------- -// Below we use the "Class Composition" technique described in the book -// C++ Template Metaprogramming: Concepts, Tools, and Techniques from Boost -// and Beyond. Abrahams, David; Gurtovoy, Aleksey. Pearson Education. -// to recursively generate a class, that will be the base for function nodes. -// -// The class generated, for three arguments A1, A2, and A3 will be -// -// struct Base1 : Argument, FunctionalBase { -// ... storage related to A1 ... -// ... methods that work on A1 ... -// }; -// -// struct Base2 : Argument, Base1 { -// ... storage related to A2 ... -// ... methods that work on A2 and (recursively) on A1 ... -// }; -// -// struct Base3 : Argument, Base2 { -// ... storage related to A3 ... -// ... methods that work on A3 and (recursively) on A2 and A1 ... -// }; -// -// struct FunctionalNode : Base3 { -// Provides convenience access to storage in hierarchy by using -// static_cast &>(*this) -// } -// -// All this magic happens when we generate the Base3 base class of FunctionalNode -// by invoking boost::mpl::fold over the meta-function GenerateFunctionalNode -// -// Similarly, the inner Record struct will be -// -// struct Record1 : JacobianTrace, CallRecord::value> { -// ... storage related to A1 ... -// ... methods that work on A1 ... -// }; -// -// struct Record2 : JacobianTrace, Record1 { -// ... storage related to A2 ... -// ... methods that work on A2 and (recursively) on A1 ... -// }; -// -// struct Record3 : JacobianTrace, Record2 { -// ... storage related to A3 ... -// ... methods that work on A3 and (recursively) on A2 and A1 ... -// }; -// -// struct Record : Record3 { -// Provides convenience access to storage in hierarchy by using -// static_cast &>(*this) -// } -// - -//----------------------------------------------------------------------------- - -/// meta-function to generate fixed-size JacobianTA type -template -struct Jacobian { - typedef Eigen::Matrix::dimension, - traits::dimension> type; -}; - -/// meta-function to generate JacobianTA optional reference -template -struct MakeOptionalJacobian { - typedef OptionalJacobian::dimension, - traits::dimension> type; -}; - -/** - * Base case for recursive FunctionalNode class - */ -template -struct FunctionalBase: ExpressionNode { - static size_t const N = 0; // number of arguments - - struct Record { - void print(const std::string& indent) const { - } - void startReverseAD4(JacobianMap& jacobians) const { - } - template - void reverseAD4(const SomeMatrix & dFdT, JacobianMap& jacobians) const { - } - }; - /// Construct an execution trace for reverse AD - void trace(const Values& values, Record* record, - ExecutionTraceStorage*& traceStorage) const { - // base case: does not do anything - } -}; - -/** - * Building block for recursive FunctionalNode class - * The integer argument N is to guarantee a unique type signature, - * so we are guaranteed to be able to extract their values by static cast. - */ -template -struct Argument { - /// Expression that will generate value/derivatives for argument - boost::shared_ptr > expression; -}; - -/** - * Building block for Recursive Record Class - * Records the evaluation of a single argument in a functional expression - */ -template -struct JacobianTrace { - A value; - ExecutionTrace trace; - typename Jacobian::type dTdA; -}; - -// Recursive Definition of Functional ExpressionNode -// The reason we inherit from Argument is because we can then -// case to this unique signature to retrieve the expression at any level -template -struct GenerateFunctionalNode: Argument, Base { - - static size_t const N = Base::N + 1; ///< Number of arguments in hierarchy - typedef Argument This; ///< The storage we have direct access to - - /// Return keys that play in this expression - virtual std::set keys() const { - std::set keys = Base::keys(); - std::set myKeys = This::expression->keys(); - keys.insert(myKeys.begin(), myKeys.end()); - return keys; - } - - /// Return dimensions for each argument - virtual void dims(std::map& map) const { - Base::dims(map); - This::expression->dims(map); - } - - // Recursive Record Class for Functional Expressions - // The reason we inherit from JacobianTrace is because we can then - // case to this unique signature to retrieve the value/trace at any level - struct Record: JacobianTrace, Base::Record { - - typedef T return_type; - typedef JacobianTrace This; - - /// Print to std::cout - void print(const std::string& indent) const { - Base::Record::print(indent); - static const Eigen::IOFormat matlab(0, 1, " ", "; ", "", "", "[", "]"); - std::cout << This::dTdA.format(matlab) << std::endl; - This::trace.print(indent); - } - - /// Start the reverse AD process - void startReverseAD4(JacobianMap& jacobians) const { - Base::Record::startReverseAD4(jacobians); - // This is the crucial point where the size of the AD pipeline is selected. - // One pipeline is started for each argument, but the number of rows in each - // pipeline is the same, namely the dimension of the output argument T. - // For example, if the entire expression is rooted by a binary function - // yielding a 2D result, then the matrix dTdA will have 2 rows. - // ExecutionTrace::reverseAD1 just passes this on to CallRecord::reverseAD2 - // which calls the correctly sized CallRecord::reverseAD3, which in turn - // calls reverseAD4 below. - This::trace.reverseAD1(This::dTdA, jacobians); - } - - /// Given df/dT, multiply in dT/dA and continue reverse AD process - // Cols is always known at compile time - template - void reverseAD4(const SomeMatrix & dFdT, - JacobianMap& jacobians) const { - Base::Record::reverseAD4(dFdT, jacobians); - This::trace.reverseAD1(dFdT * This::dTdA, jacobians); - } - }; - - /// Construct an execution trace for reverse AD - void trace(const Values& values, Record* record, - ExecutionTraceStorage*& traceStorage) const { - Base::trace(values, record, traceStorage); // recurse - // Write an Expression execution trace in record->trace - // Iff Constant or Leaf, this will not write to traceStorage, only to trace. - // Iff the expression is functional, write all Records in traceStorage buffer - // Return value of type T is recorded in record->value - record->Record::This::value = This::expression->traceExecution(values, - record->Record::This::trace, traceStorage); - // traceStorage is never modified by traceExecution, but if traceExecution has - // written in the buffer, the next caller expects we advance the pointer - traceStorage += This::expression->traceSize(); - } -}; - -/** - * Recursive GenerateFunctionalNode class Generator - */ -template -struct FunctionalNode { - - /// The following typedef generates the recursively defined Base class - typedef typename boost::mpl::fold, - GenerateFunctionalNode >::type Base; - - /** - * The type generated by this meta-function derives from Base - * and adds access functions as well as the crucial [trace] function - */ - struct type: public Base { - - // Argument types and derived, note these are base 0 ! - // These are currently not used - useful for Phoenix in future -#ifdef EXPRESSIONS_PHOENIX - typedef TYPES Arguments; - typedef typename boost::mpl::transform >::type Jacobians; - typedef typename boost::mpl::transform >::type Optionals; + // Windows does not support variable length arrays, so memory must be dynamically + // allocated on Visual Studio. For more information see the issue below + // https://bitbucket.org/gtborg/gtsam/issue/178/vlas-unsupported-in-visual-studio +#ifdef _MSC_VER + internal::ExecutionTraceStorage* traceStorage = new internal::ExecutionTraceStorage[size]; +#else + internal::ExecutionTraceStorage traceStorage[size]; #endif - /// Reset expression shared pointer - template - void reset(const boost::shared_ptr >& ptr) { - static_cast &>(*this).expression = ptr; - } + internal::ExecutionTrace trace; + T value(this->traceExecution(values, trace, traceStorage)); + trace.startReverseAD1(jacobians); - /// Access Expression shared pointer - template - boost::shared_ptr > expression() const { - return static_cast const &>(*this).expression; - } +#ifdef _MSC_VER + delete[] traceStorage; +#endif - /// Provide convenience access to Record storage and implement - /// the virtual function based interface of CallRecord using the CallRecordImplementor - struct Record: public internal::CallRecordImplementor::dimension>, public Base::Record { - using Base::Record::print; - using Base::Record::startReverseAD4; - using Base::Record::reverseAD4; - - virtual ~Record() { - } - - /// Access Value - template - const A& value() const { - return static_cast const &>(*this).value; - } - - /// Access Jacobian - template - typename Jacobian::type& jacobian() { - return static_cast&>(*this).dTdA; - } - }; - - /// Construct an execution trace for reverse AD - Record* trace(const Values& values, - ExecutionTraceStorage* traceStorage) const { - assert(reinterpret_cast(traceStorage) % TraceAlignment == 0); - - // Create the record and advance the pointer - Record* record = new (traceStorage) Record(); - traceStorage += upAligned(sizeof(Record)); - - // Record the traces for all arguments - // After this, the traceStorage pointer is set to after what was written - Base::trace(values, record, traceStorage); - - // Return the record for this function evaluation - return record; - } - }; -}; -//----------------------------------------------------------------------------- - -/// Unary Function Expression -template -class UnaryExpression: public FunctionalNode >::type { - - typedef typename MakeOptionalJacobian::type OJ1; - -public: - - typedef boost::function Function; - typedef typename FunctionalNode >::type Base; - typedef typename Base::Record Record; - -private: - - Function function_; - - /// Constructor with a unary function f, and input argument e - UnaryExpression(Function f, const Expression& e1) : - function_(f) { - this->template reset(e1.root()); - ExpressionNode::traceSize_ = upAligned(sizeof(Record)) + e1.traceSize(); - } - - friend class Expression ; - -public: - - /// Return value - virtual T value(const Values& values) const { - return function_(this->template expression()->value(values), boost::none); - } - - /// Construct an execution trace for reverse AD - virtual T traceExecution(const Values& values, ExecutionTrace& trace, - ExecutionTraceStorage* traceStorage) const { - - Record* record = Base::trace(values, traceStorage); - trace.setFunction(record); - - return function_(record->template value(), - record->template jacobian()); - } -}; - -//----------------------------------------------------------------------------- -/// Binary Expression - -template -class BinaryExpression: - public FunctionalNode >::type { - - typedef typename MakeOptionalJacobian::type OJ1; - typedef typename MakeOptionalJacobian::type OJ2; - -public: - - typedef boost::function Function; - typedef typename FunctionalNode >::type Base; - typedef typename Base::Record Record; - -private: - - Function function_; - - /// Constructor with a ternary function f, and three input arguments - BinaryExpression(Function f, const Expression& e1, - const Expression& e2) : - function_(f) { - this->template reset(e1.root()); - this->template reset(e2.root()); - ExpressionNode::traceSize_ = // - upAligned(sizeof(Record)) + e1.traceSize() + e2.traceSize(); - } - - friend class Expression ; - friend class ::ExpressionFactorBinaryTest; - -public: - - /// Return value - virtual T value(const Values& values) const { - using boost::none; - return function_(this->template expression()->value(values), - this->template expression()->value(values), - none, none); - } - - /// Construct an execution trace for reverse AD - virtual T traceExecution(const Values& values, ExecutionTrace& trace, - ExecutionTraceStorage* traceStorage) const { - - Record* record = Base::trace(values, traceStorage); - trace.setFunction(record); - - return function_(record->template value(), - record->template value(), record->template jacobian(), - record->template jacobian()); - } -}; - -//----------------------------------------------------------------------------- -/// Ternary Expression - -template -class TernaryExpression: - public FunctionalNode >::type { - - typedef typename MakeOptionalJacobian::type OJ1; - typedef typename MakeOptionalJacobian::type OJ2; - typedef typename MakeOptionalJacobian::type OJ3; - -public: - - typedef boost::function Function; - typedef typename FunctionalNode >::type Base; - typedef typename Base::Record Record; - -private: - - Function function_; - - /// Constructor with a ternary function f, and three input arguments - TernaryExpression(Function f, const Expression& e1, - const Expression& e2, const Expression& e3) : - function_(f) { - this->template reset(e1.root()); - this->template reset(e2.root()); - this->template reset(e3.root()); - ExpressionNode::traceSize_ = // - upAligned(sizeof(Record)) + e1.traceSize() + e2.traceSize() - + e3.traceSize(); - } - - friend class Expression ; - -public: - - /// Return value - virtual T value(const Values& values) const { - using boost::none; - return function_(this->template expression()->value(values), - this->template expression()->value(values), - this->template expression()->value(values), - none, none, none); - } - - /// Construct an execution trace for reverse AD - virtual T traceExecution(const Values& values, ExecutionTrace& trace, - ExecutionTraceStorage* traceStorage) const { - - Record* record = Base::trace(values, traceStorage); - trace.setFunction(record); - - return function_( - record->template value(), record->template value(), - record->template value(), record->template jacobian(), - record->template jacobian(), record->template jacobian()); - } - -}; -//----------------------------------------------------------------------------- + return value; } + +template +typename Expression::KeysAndDims Expression::keysAndDims() const { + std::map map; + dims(map); + size_t n = map.size(); + KeysAndDims pair = std::make_pair(FastVector(n), FastVector(n)); + boost::copy(map | boost::adaptors::map_keys, pair.first.begin()); + boost::copy(map | boost::adaptors::map_values, pair.second.begin()); + return pair; +} + +namespace internal { +// http://stackoverflow.com/questions/16260445/boost-bind-to-operator +template +struct apply_compose { + typedef T result_type; + static const int Dim = traits::dimension; + T operator()(const T& x, const T& y, OptionalJacobian H1 = + boost::none, OptionalJacobian H2 = boost::none) const { + return x.compose(y, H1, H2); + } +}; +} + +// Global methods: + +/// Construct a product expression, assumes T::compose(T) -> T +template +Expression operator*(const Expression& expression1, + const Expression& expression2) { + return Expression( + boost::bind(internal::apply_compose(), _1, _2, _3, _4), expression1, + expression2); +} + +/// Construct an array of leaves +template +std::vector > createUnknowns(size_t n, char c, size_t start) { + std::vector > unknowns; + unknowns.reserve(n); + for (size_t i = start; i < start + n; i++) + unknowns.push_back(Expression(c, i)); + return unknowns; +} + +} // namespace gtsam diff --git a/gtsam/nonlinear/Expression.h b/gtsam/nonlinear/Expression.h index a39b1557c..d24a568f5 100644 --- a/gtsam/nonlinear/Expression.h +++ b/gtsam/nonlinear/Expression.h @@ -19,21 +19,28 @@ #pragma once -#include +#include #include -#include +#include #include -#include -#include +#include +#include +// Forward declare tests class ExpressionFactorShallowTest; namespace gtsam { -// Forward declare +// Forward declares +class Values; template class ExpressionFactor; +namespace internal { +template class ExecutionTrace; +template class ExpressionNode; +} + /** * Expression class that supports automatic differentiation */ @@ -48,110 +55,97 @@ public: private: // Paul's trick shared pointer, polymorphic root of entire expression tree - boost::shared_ptr > root_; + boost::shared_ptr > root_; public: + // Expressions wrap trees of functions that can evaluate their own derivatives. + // The meta-functions below are useful to specify the type of those functions. + // Example, a function taking a camera and a 3D point and yielding a 2D point: + // Expression::BinaryFunction::type + template + struct UnaryFunction { + typedef boost::function< + T(const A1&, typename MakeOptionalJacobian::type)> type; + }; + + template + struct BinaryFunction { + typedef boost::function< + T(const A1&, const A2&, typename MakeOptionalJacobian::type, + typename MakeOptionalJacobian::type)> type; + }; + + template + struct TernaryFunction { + typedef boost::function< + T(const A1&, const A2&, const A3&, + typename MakeOptionalJacobian::type, + typename MakeOptionalJacobian::type, + typename MakeOptionalJacobian::type)> type; + }; + /// Print - void print(const std::string& s) const { - std::cout << s << *root_ << std::endl; - } + void print(const std::string& s) const; - // Construct a constant expression - Expression(const T& value) : - root_(new ConstantExpression(value)) { - } + /// Construct a constant expression + Expression(const T& value); - // Construct a leaf expression, with Key - Expression(const Key& key) : - root_(new LeafExpression(key)) { - } + /// Construct a leaf expression, with Key + Expression(const Key& key); - // Construct a leaf expression, with Symbol - Expression(const Symbol& symbol) : - root_(new LeafExpression(symbol)) { - } + /// Construct a leaf expression, with Symbol + Expression(const Symbol& symbol); - // Construct a leaf expression, creating Symbol - Expression(unsigned char c, size_t j) : - root_(new LeafExpression(Symbol(c, j))) { - } + /// Construct a leaf expression, creating Symbol + Expression(unsigned char c, size_t j); + + /// Construct a unary function expression + template + Expression(typename UnaryFunction::type function, + const Expression& expression); + + /// Construct a binary function expression + template + Expression(typename BinaryFunction::type function, + const Expression& expression1, const Expression& expression2); + + /// Construct a ternary function expression + template + Expression(typename TernaryFunction::type function, + const Expression& expression1, const Expression& expression2, + const Expression& expression3); /// Construct a nullary method expression template Expression(const Expression& expression, - T (A::*method)(typename MakeOptionalJacobian::type) const) : - root_(new UnaryExpression(boost::bind(method, _1, _2), expression)) { - } - - /// Construct a unary function expression - template - Expression(typename UnaryExpression::Function function, - const Expression& expression) : - root_(new UnaryExpression(function, expression)) { - } + T (A::*method)(typename MakeOptionalJacobian::type) const); /// Construct a unary method expression template Expression(const Expression& expression1, T (A1::*method)(const A2&, typename MakeOptionalJacobian::type, typename MakeOptionalJacobian::type) const, - const Expression& expression2) : - root_( - new BinaryExpression(boost::bind(method, _1, _2, _3, _4), - expression1, expression2)) { - } - - /// Construct a binary function expression - template - Expression(typename BinaryExpression::Function function, - const Expression& expression1, const Expression& expression2) : - root_(new BinaryExpression(function, expression1, expression2)) { - } + const Expression& expression2); /// Construct a binary method expression template Expression(const Expression& expression1, T (A1::*method)(const A2&, const A3&, - typename TernaryExpression::OJ1, - typename TernaryExpression::OJ2, - typename TernaryExpression::OJ3) const, - const Expression& expression2, const Expression& expression3) : - root_( - new TernaryExpression( - boost::bind(method, _1, _2, _3, _4, _5, _6), expression1, - expression2, expression3)) { - } + typename MakeOptionalJacobian::type, + typename MakeOptionalJacobian::type, + typename MakeOptionalJacobian::type) const, + const Expression& expression2, const Expression& expression3); - /// Construct a ternary function expression - template - Expression(typename TernaryExpression::Function function, - const Expression& expression1, const Expression& expression2, - const Expression& expression3) : - root_( - new TernaryExpression(function, expression1, - expression2, expression3)) { - } - - /// Return root - const boost::shared_ptr >& root() const { - return root_; - } - - // Return size needed for memory buffer in traceExecution - size_t traceSize() const { - return root_->traceSize(); + /// Destructor + virtual ~Expression() { } /// Return keys that play in this expression - std::set keys() const { - return root_->keys(); - } + std::set keys() const; /// Return dimensions for each argument, as a map - void dims(std::map& map) const { - root_->dims(map); - } + void dims(std::map& map) const; /** * @brief Return value and optional derivatives, reverse AD version @@ -159,16 +153,7 @@ public: * The order of the Jacobians is same as keys in either keys() or dims() */ T value(const Values& values, boost::optional&> H = - boost::none) const { - - if (H) { - // Call private version that returns derivatives in H - KeysAndDims pair = keysAndDims(); - return value(values, pair.first, pair.second, *H); - } else - // no derivatives needed, just return value - return root_->value(values); - } + boost::none) const; /** * @return a "deep" copy of this Expression @@ -179,116 +164,55 @@ public: return boost::make_shared(*this); } + /// Return root + const boost::shared_ptr >& root() const; + + /// Return size needed for memory buffer in traceExecution + size_t traceSize() const; + private: - /// Vaguely unsafe keys and dimensions in same order + /// Keys and dimensions in same order typedef std::pair, FastVector > KeysAndDims; - KeysAndDims keysAndDims() const { - std::map map; - dims(map); - size_t n = map.size(); - KeysAndDims pair = std::make_pair(FastVector(n), FastVector(n)); - boost::copy(map | boost::adaptors::map_keys, pair.first.begin()); - boost::copy(map | boost::adaptors::map_values, pair.second.begin()); - return pair; - } + KeysAndDims keysAndDims() const; /// private version that takes keys and dimensions, returns derivatives - T value(const Values& values, const FastVector& keys, - const FastVector& dims, std::vector& H) const { - - // H should be pre-allocated - assert(H.size()==keys.size()); - - // Pre-allocate and zero VerticalBlockMatrix - static const int Dim = traits::dimension; - VerticalBlockMatrix Ab(dims, Dim); - Ab.matrix().setZero(); - JacobianMap jacobianMap(keys, Ab); - - // Call unsafe version - T result = value(values, jacobianMap); - - // Copy blocks into the vector of jacobians passed in - for (DenseIndex i = 0; i < static_cast(keys.size()); i++) - H[i] = Ab(i); - - return result; - } + T valueAndDerivatives(const Values& values, const FastVector& keys, + const FastVector& dims, std::vector& H) const; /// trace execution, very unsafe - T traceExecution(const Values& values, ExecutionTrace& trace, - ExecutionTraceStorage* traceStorage) const { - return root_->traceExecution(values, trace, traceStorage); - } + T traceExecution(const Values& values, internal::ExecutionTrace& trace, + void* traceStorage) const; - /** - * @brief Return value and derivatives, reverse AD version - * This very unsafe method needs a JacobianMap with correctly allocated - * and initialized VerticalBlockMatrix, hence is declared private. - */ - T value(const Values& values, JacobianMap& jacobians) const { - // The following piece of code is absolutely crucial for performance. - // We allocate a block of memory on the stack, which can be done at runtime - // with modern C++ compilers. The traceExecution then fills this memory - // with an execution trace, made up entirely of "Record" structs, see - // the FunctionalNode class in expression-inl.h - size_t size = traceSize(); - - // Windows does not support variable length arrays, so memory must be dynamically - // allocated on Visual Studio. For more information see the issue below - // https://bitbucket.org/gtborg/gtsam/issue/178/vlas-unsupported-in-visual-studio -#ifdef _MSC_VER - ExecutionTraceStorage* traceStorage = new ExecutionTraceStorage[size]; -#else - ExecutionTraceStorage traceStorage[size]; -#endif - - ExecutionTrace trace; - T value(traceExecution(values, trace, traceStorage)); - trace.startReverseAD1(jacobians); - -#ifdef _MSC_VER - delete[] traceStorage; -#endif - - return value; - } + /// brief Return value and derivatives, reverse AD version + T valueAndJacobianMap(const Values& values, + internal::JacobianMap& jacobians) const; // be very selective on who can access these private methods: friend class ExpressionFactor ; + friend class internal::ExpressionNode; + + // and add tests friend class ::ExpressionFactorShallowTest; - }; -// http://stackoverflow.com/questions/16260445/boost-bind-to-operator -template -struct apply_compose { - typedef T result_type; - static const int Dim = traits::dimension; - T operator()(const T& x, const T& y, OptionalJacobian H1 = - boost::none, OptionalJacobian H2 = boost::none) const { - return x.compose(y, H1, H2); - } -}; - -/// Construct a product expression, assumes T::compose(T) -> T +/** + * Construct a product expression, assumes T::compose(T) -> T + * Example: + * Expression a(0), b(1), c = a*b; + */ template -Expression operator*(const Expression& expression1, - const Expression& expression2) { - return Expression(boost::bind(apply_compose(), _1, _2, _3, _4), - expression1, expression2); -} +Expression operator*(const Expression& e1, const Expression& e2); -/// Construct an array of leaves +/** + * Construct an array of unknown expressions with successive symbol keys + * Example: + * createUnknowns(3,'x') creates unknown expressions for x0,x1,x2 + */ template -std::vector > createUnknowns(size_t n, char c, size_t start = 0) { - std::vector > unknowns; - unknowns.reserve(n); - for (size_t i = start; i < start + n; i++) - unknowns.push_back(Expression(c, i)); - return unknowns; -} +std::vector > createUnknowns(size_t n, char c, size_t start = 0); -} +} // namespace gtsam + +#include diff --git a/gtsam/nonlinear/ExpressionFactor.h b/gtsam/nonlinear/ExpressionFactor.h index 4e0467a3b..b32b84df3 100644 --- a/gtsam/nonlinear/ExpressionFactor.h +++ b/gtsam/nonlinear/ExpressionFactor.h @@ -32,7 +32,9 @@ namespace gtsam { template class ExpressionFactor: public NoiseModelFactor { -protected: + protected: + + typedef ExpressionFactor This; T measurement_; ///< the measurement to be compared with the expression Expression expression_; ///< the expression that is AD enabled @@ -40,7 +42,9 @@ protected: static const int Dim = traits::dimension; -public: + public: + + typedef boost::shared_ptr > shared_ptr; /// Constructor ExpressionFactor(const SharedNoiseModel& noiseModel, // @@ -65,8 +69,8 @@ public: */ virtual Vector unwhitenedError(const Values& x, boost::optional&> H = boost::none) const { - if (H) { - const T value = expression_.value(x, keys_, dims_, *H); + if (H) { + const T value = expression_.valueAndDerivatives(x, keys_, dims_, *H); return traits::Local(measurement_, value); } else { const T value = expression_.value(x); @@ -89,13 +93,13 @@ public: // Wrap keys and VerticalBlockMatrix into structure passed to expression_ VerticalBlockMatrix& Ab = factor->matrixObject(); - JacobianMap jacobianMap(keys_, Ab); + internal::JacobianMap jacobianMap(keys_, Ab); // Zero out Jacobian so we can simply add to it Ab.matrix().setZero(); // Get value and Jacobians, writing directly into JacobianFactor - T value = expression_.value(x, jacobianMap); // <<< Reverse AD happens here ! + T value = expression_.valueAndJacobianMap(x, jacobianMap); // <<< Reverse AD happens here ! // Evaluate error and set RHS vector b Ab(size()).col(0) = -traits::Local(measurement_, value); @@ -106,8 +110,13 @@ public: return factor; } + + /// @return a deep copy of this factor + virtual gtsam::NonlinearFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); } }; // ExpressionFactor -} // \ namespace gtsam +}// \ namespace gtsam diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index 3b3d76285..00ffdccef 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -98,7 +98,7 @@ DoglegOptimizerImpl::TrustRegionAdaptationMode ISAM2DoglegParams::adaptationMode } /* ************************************************************************* */ -ISAM2Params::Factorization ISAM2Params::factorizationTranslator(const std::string& str) const { +ISAM2Params::Factorization ISAM2Params::factorizationTranslator(const std::string& str) { std::string s = str; boost::algorithm::to_upper(s); if (s == "QR") return ISAM2Params::QR; if (s == "CHOLESKY") return ISAM2Params::CHOLESKY; @@ -108,7 +108,7 @@ ISAM2Params::Factorization ISAM2Params::factorizationTranslator(const std::strin } /* ************************************************************************* */ -std::string ISAM2Params::factorizationTranslator(const ISAM2Params::Factorization& value) const { +std::string ISAM2Params::factorizationTranslator(const ISAM2Params::Factorization& value) { std::string s; switch (value) { case ISAM2Params::QR: s = "QR"; break; diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 57a98ca3c..5f5554e91 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -186,6 +186,7 @@ struct GTSAM_EXPORT ISAM2Params { enableDetailedResults(false), enablePartialRelinearizationCheck(false), findUnusedFactorSlots(false) {} + /// print iSAM2 parameters void print(const std::string& str = "") const { std::cout << str << "\n"; if(optimizationParams.type() == typeid(ISAM2GaussNewtonParams)) @@ -214,7 +215,9 @@ struct GTSAM_EXPORT ISAM2Params { std::cout.flush(); } - /** Getters and Setters for all properties */ + /// @name Getters and Setters for all properties + /// @{ + OptimizationParams getOptimizationParams() const { return this->optimizationParams; } RelinearizationThreshold getRelinearizeThreshold() const { return relinearizeThreshold; } int getRelinearizeSkip() const { return relinearizeSkip; } @@ -237,14 +240,21 @@ struct GTSAM_EXPORT ISAM2Params { void setEnableDetailedResults(bool enableDetailedResults) { this->enableDetailedResults = enableDetailedResults; } void setEnablePartialRelinearizationCheck(bool enablePartialRelinearizationCheck) { this->enablePartialRelinearizationCheck = enablePartialRelinearizationCheck; } - Factorization factorizationTranslator(const std::string& str) const; - std::string factorizationTranslator(const Factorization& value) const; - GaussianFactorGraph::Eliminate getEliminationFunction() const { return factorization == CHOLESKY ? (GaussianFactorGraph::Eliminate)EliminatePreferCholesky : (GaussianFactorGraph::Eliminate)EliminateQR; } + + /// @} + + /// @name Some utilities + /// @{ + + static Factorization factorizationTranslator(const std::string& str); + static std::string factorizationTranslator(const Factorization& value); + + /// @} }; @@ -544,8 +554,15 @@ public: boost::optional&> marginalFactorsIndices = boost::none, boost::optional&> deletedFactorsIndices = boost::none); - /** Access the current linearization point */ - const Values& getLinearizationPoint() const { return theta_; } + /// Access the current linearization point + const Values& getLinearizationPoint() const { + return theta_; + } + + /// Check whether variable with given key exists in linearization point + bool valueExists(Key key) const { + return theta_.exists(key); + } /** Compute an estimate from the incomplete linear delta computed during the last update. * This delta is incomplete because it was not updated below wildfire_threshold. If only diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp index 2f8fcfcee..5fb51a243 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp @@ -25,9 +25,10 @@ #include #include +#include +#include #include #include -#include using namespace std; @@ -178,7 +179,7 @@ GaussianFactorGraph::shared_ptr LevenbergMarquardtOptimizer::buildDampedSystem( SharedDiagonal model = noiseModel::Isotropic::Sigma(dim, sigma); damped += boost::make_shared(key_vector.first, A, b, model); - } catch (std::exception e) { + } catch (const std::exception& e) { // Don't attempt any damping if no key found in diagonal continue; } @@ -247,7 +248,7 @@ void LevenbergMarquardtOptimizer::iterate() { double modelFidelity = 0.0; bool step_is_successful = false; bool stopSearchingLambda = false; - double newError; + double newError = numeric_limits::infinity(); Values newValues; VectorValues delta; @@ -255,7 +256,7 @@ void LevenbergMarquardtOptimizer::iterate() { try { delta = solve(dampedSystem, state_.values, params_); systemSolvedSuccessfully = true; - } catch (IndeterminantLinearSystemException) { + } catch (const IndeterminantLinearSystemException& e) { systemSolvedSuccessfully = false; } diff --git a/gtsam/nonlinear/NonlinearEquality.h b/gtsam/nonlinear/NonlinearEquality.h index 1023a2173..86fb34fe0 100644 --- a/gtsam/nonlinear/NonlinearEquality.h +++ b/gtsam/nonlinear/NonlinearEquality.h @@ -21,21 +21,14 @@ #include #include +#include + #include #include #include namespace gtsam { -/** - * Template default compare function that assumes a testable T - */ -template -bool compare(const T& a, const T& b) { - GTSAM_CONCEPT_TESTABLE_TYPE(T); - return a.equals(b); -} - /** * An equality factor that forces either one variable to a constant, * or a set of variables to be equal to each other. @@ -76,7 +69,9 @@ public: /** * Function that compares two values */ - bool (*compare_)(const T& a, const T& b); + typedef boost::function CompareFunction; + CompareFunction compare_; +// bool (*compare_)(const T& a, const T& b); /** default constructor - only for serialization */ NonlinearEquality() { @@ -92,7 +87,7 @@ public: * Constructor - forces exact evaluation */ NonlinearEquality(Key j, const T& feasible, - bool (*_compare)(const T&, const T&) = compare) : + const CompareFunction &_compare = boost::bind(traits::Equals,_1,_2,1e-9)) : Base(noiseModel::Constrained::All(traits::GetDimension(feasible)), j), feasible_(feasible), allow_error_(false), error_gain_(0.0), // compare_(_compare) { @@ -102,7 +97,7 @@ public: * Constructor - allows inexact evaluation */ NonlinearEquality(Key j, const T& feasible, double error_gain, - bool (*_compare)(const T&, const T&) = compare) : + const CompareFunction &_compare = boost::bind(traits::Equals,_1,_2,1e-9)) : Base(noiseModel::Constrained::All(traits::GetDimension(feasible)), j), feasible_(feasible), allow_error_(true), error_gain_(error_gain), // compare_(_compare) { @@ -122,7 +117,7 @@ public: /** Check if two factors are equal */ virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const { const This* e = dynamic_cast(&f); - return e && Base::equals(f) && feasible_.equals(e->feasible_, tol) + return e && Base::equals(f) && traits::Equals(feasible_,e->feasible_, tol) && std::abs(error_gain_ - e->error_gain_) < tol; } diff --git a/gtsam/nonlinear/NonlinearFactorGraph.cpp b/gtsam/nonlinear/NonlinearFactorGraph.cpp index 238d3bc56..f23418d2a 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.cpp +++ b/gtsam/nonlinear/NonlinearFactorGraph.cpp @@ -47,11 +47,12 @@ double NonlinearFactorGraph::probPrime(const Values& c) const { /* ************************************************************************* */ void NonlinearFactorGraph::print(const std::string& str, const KeyFormatter& keyFormatter) const { - cout << str << "size: " << size() << endl; + cout << str << "size: " << size() << endl << endl; for (size_t i = 0; i < factors_.size(); i++) { stringstream ss; - ss << "factor " << i << ": "; + ss << "Factor " << i << ": "; if (factors_[i] != NULL) factors_[i]->print(ss.str(), keyFormatter); + cout << endl; } } @@ -62,12 +63,12 @@ bool NonlinearFactorGraph::equals(const NonlinearFactorGraph& other, double tol) /* ************************************************************************* */ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values, - const GraphvizFormatting& graphvizFormatting, + const GraphvizFormatting& formatting, const KeyFormatter& keyFormatter) const { stm << "graph {\n"; - stm << " size=\"" << graphvizFormatting.figureWidthInches << "," << - graphvizFormatting.figureHeightInches << "\";\n\n"; + stm << " size=\"" << formatting.figureWidthInches << "," << + formatting.figureHeightInches << "\";\n\n"; FastSet keys = this->keys(); @@ -75,72 +76,38 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values, struct { boost::optional operator()( const Value& value, const GraphvizFormatting& graphvizFormatting) { - if(const Pose2* p = dynamic_cast(&value)) { - double x, y; - switch (graphvizFormatting.paperHorizontalAxis) { - case GraphvizFormatting::X: x = p->x(); break; - case GraphvizFormatting::Y: x = p->y(); break; - case GraphvizFormatting::Z: x = 0.0; break; - case GraphvizFormatting::NEGX: x = -p->x(); break; - case GraphvizFormatting::NEGY: x = -p->y(); break; - case GraphvizFormatting::NEGZ: x = 0.0; break; - default: throw std::runtime_error("Invalid enum value"); - } - switch (graphvizFormatting.paperVerticalAxis) { - case GraphvizFormatting::X: y = p->x(); break; - case GraphvizFormatting::Y: y = p->y(); break; - case GraphvizFormatting::Z: y = 0.0; break; - case GraphvizFormatting::NEGX: y = -p->x(); break; - case GraphvizFormatting::NEGY: y = -p->y(); break; - case GraphvizFormatting::NEGZ: y = 0.0; break; - default: throw std::runtime_error("Invalid enum value"); - } - return Point2(x,y); - } else if(const Pose3* p = dynamic_cast(&value)) { - double x, y; - switch (graphvizFormatting.paperHorizontalAxis) { - case GraphvizFormatting::X: x = p->x(); break; - case GraphvizFormatting::Y: x = p->y(); break; - case GraphvizFormatting::Z: x = p->z(); break; - case GraphvizFormatting::NEGX: x = -p->x(); break; - case GraphvizFormatting::NEGY: x = -p->y(); break; - case GraphvizFormatting::NEGZ: x = -p->z(); break; - default: throw std::runtime_error("Invalid enum value"); - } - switch (graphvizFormatting.paperVerticalAxis) { - case GraphvizFormatting::X: y = p->x(); break; - case GraphvizFormatting::Y: y = p->y(); break; - case GraphvizFormatting::Z: y = p->z(); break; - case GraphvizFormatting::NEGX: y = -p->x(); break; - case GraphvizFormatting::NEGY: y = -p->y(); break; - case GraphvizFormatting::NEGZ: y = -p->z(); break; - default: throw std::runtime_error("Invalid enum value"); - } - return Point2(x,y); - } else if(const Point3* p = dynamic_cast(&value)) { - double x, y; - switch (graphvizFormatting.paperHorizontalAxis) { - case GraphvizFormatting::X: x = p->x(); break; - case GraphvizFormatting::Y: x = p->y(); break; - case GraphvizFormatting::Z: x = p->z(); break; - case GraphvizFormatting::NEGX: x = -p->x(); break; - case GraphvizFormatting::NEGY: x = -p->y(); break; - case GraphvizFormatting::NEGZ: x = -p->z(); break; - default: throw std::runtime_error("Invalid enum value"); - } - switch (graphvizFormatting.paperVerticalAxis) { - case GraphvizFormatting::X: y = p->x(); break; - case GraphvizFormatting::Y: y = p->y(); break; - case GraphvizFormatting::Z: y = p->z(); break; - case GraphvizFormatting::NEGX: y = -p->x(); break; - case GraphvizFormatting::NEGY: y = -p->y(); break; - case GraphvizFormatting::NEGZ: y = -p->z(); break; - default: throw std::runtime_error("Invalid enum value"); - } - return Point2(x,y); + Vector3 t; + if (const GenericValue* p = dynamic_cast*>(&value)) { + t << p->value().x(), p->value().y(), 0; + } else if (const GenericValue* p = dynamic_cast*>(&value)) { + t << p->value().x(), p->value().y(), 0; + } else if (const GenericValue* p = dynamic_cast*>(&value)) { + t = p->value().translation().vector(); + } else if (const GenericValue* p = dynamic_cast*>(&value)) { + t = p->value().vector(); } else { return boost::none; } + double x, y; + switch (graphvizFormatting.paperHorizontalAxis) { + case GraphvizFormatting::X: x = t.x(); break; + case GraphvizFormatting::Y: x = t.y(); break; + case GraphvizFormatting::Z: x = t.z(); break; + case GraphvizFormatting::NEGX: x = -t.x(); break; + case GraphvizFormatting::NEGY: x = -t.y(); break; + case GraphvizFormatting::NEGZ: x = -t.z(); break; + default: throw std::runtime_error("Invalid enum value"); + } + switch (graphvizFormatting.paperVerticalAxis) { + case GraphvizFormatting::X: y = t.x(); break; + case GraphvizFormatting::Y: y = t.y(); break; + case GraphvizFormatting::Z: y = t.z(); break; + case GraphvizFormatting::NEGX: y = -t.x(); break; + case GraphvizFormatting::NEGY: y = -t.y(); break; + case GraphvizFormatting::NEGZ: y = -t.z(); break; + default: throw std::runtime_error("Invalid enum value"); + } + return Point2(x,y); }} getXY; // Find bounds @@ -148,7 +115,7 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values, double minY = numeric_limits::infinity(), maxY = -numeric_limits::infinity(); BOOST_FOREACH(Key key, keys) { if(values.exists(key)) { - boost::optional xy = getXY(values.at(key), graphvizFormatting); + boost::optional xy = getXY(values.at(key), formatting); if(xy) { if(xy->x() < minX) minX = xy->x(); @@ -163,33 +130,22 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values, } // Create nodes for each variable in the graph - bool firstTimePoses = true; - Key lastKey; - BOOST_FOREACH(Key key, keys) { + BOOST_FOREACH(Key key, keys){ // Label the node with the label from the KeyFormatter stm << " var" << key << "[label=\"" << keyFormatter(key) << "\""; if(values.exists(key)) { - boost::optional xy = getXY(values.at(key), graphvizFormatting); + boost::optional xy = getXY(values.at(key), formatting); if(xy) - stm << ", pos=\"" << graphvizFormatting.scale*(xy->x() - minX) << "," << graphvizFormatting.scale*(xy->y() - minY) << "!\""; + stm << ", pos=\"" << formatting.scale*(xy->x() - minX) << "," << formatting.scale*(xy->y() - minY) << "!\""; } stm << "];\n"; - - if (firstTimePoses) { - lastKey = key; - firstTimePoses = false; - } else { - stm << " var" << key << "--" << "var" << lastKey << ";\n"; - lastKey = key; - } } stm << "\n"; - - if(graphvizFormatting.mergeSimilarFactors) { + if (formatting.mergeSimilarFactors) { // Remove duplicate factors FastSet > structure; - BOOST_FOREACH(const sharedFactor& factor, *this) { + BOOST_FOREACH(const sharedFactor& factor, *this){ if(factor) { vector factorKeys = factor->keys(); std::sort(factorKeys.begin(), factorKeys.end()); @@ -199,57 +155,65 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values, // Create factors and variable connections size_t i = 0; - BOOST_FOREACH(const vector& factorKeys, structure) { + BOOST_FOREACH(const vector& factorKeys, structure){ // Make each factor a dot stm << " factor" << i << "[label=\"\", shape=point"; { - map::const_iterator pos = graphvizFormatting.factorPositions.find(i); - if(pos != graphvizFormatting.factorPositions.end()) - stm << ", pos=\"" << graphvizFormatting.scale*(pos->second.x() - minX) << "," << graphvizFormatting.scale*(pos->second.y() - minY) << "!\""; + map::const_iterator pos = formatting.factorPositions.find(i); + if(pos != formatting.factorPositions.end()) + stm << ", pos=\"" << formatting.scale*(pos->second.x() - minX) << "," + << formatting.scale*(pos->second.y() - minY) << "!\""; } stm << "];\n"; // Make factor-variable connections BOOST_FOREACH(Key key, factorKeys) { - stm << " var" << key << "--" << "factor" << i << ";\n"; } + stm << " var" << key << "--" << "factor" << i << ";\n"; + } ++ i; } } else { // Create factors and variable connections for(size_t i = 0; i < this->size(); ++i) { - if(graphvizFormatting.plotFactorPoints){ - // Make each factor a dot - stm << " factor" << i << "[label=\"\", shape=point"; - { - map::const_iterator pos = graphvizFormatting.factorPositions.find(i); - if(pos != graphvizFormatting.factorPositions.end()) - stm << ", pos=\"" << graphvizFormatting.scale*(pos->second.x() - minX) << "," << graphvizFormatting.scale*(pos->second.y() - minY) << "!\""; - } - stm << "];\n"; + const NonlinearFactor::shared_ptr& factor = this->at(i); + if(formatting.plotFactorPoints) { + const FastVector& keys = factor->keys(); + if (formatting.binaryEdges && keys.size()==2) { + stm << " var" << keys[0] << "--" << "var" << keys[1] << ";\n"; + } else { + // Make each factor a dot + stm << " factor" << i << "[label=\"\", shape=point"; + { + map::const_iterator pos = formatting.factorPositions.find(i); + if(pos != formatting.factorPositions.end()) + stm << ", pos=\"" << formatting.scale*(pos->second.x() - minX) << "," + << formatting.scale*(pos->second.y() - minY) << "!\""; + } + stm << "];\n"; - // Make factor-variable connections - if(graphvizFormatting.connectKeysToFactor && this->at(i)) { - BOOST_FOREACH(Key key, *this->at(i)) { - stm << " var" << key << "--" << "factor" << i << ";\n"; + // Make factor-variable connections + if(formatting.connectKeysToFactor && factor) { + BOOST_FOREACH(Key key, *factor) { + stm << " var" << key << "--" << "factor" << i << ";\n"; + } + } + } } - } - - } else { - if(this->at(i)) { - Key k; - bool firstTime = true; - BOOST_FOREACH(Key key, *this->at(i)) { - if(firstTime){ - k = key; - firstTime = false; - continue; + if(factor) { + Key k; + bool firstTime = true; + BOOST_FOREACH(Key key, *this->at(i)) { + if(firstTime) { + k = key; + firstTime = false; + continue; + } + stm << " var" << key << "--" << "var" << k << ";\n"; + k = key; + } } - stm << " var" << key << "--" << "var" << k << ";\n"; - k = key; - } - } } } } diff --git a/gtsam/nonlinear/NonlinearFactorGraph.h b/gtsam/nonlinear/NonlinearFactorGraph.h index a69769f48..169d12455 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.h +++ b/gtsam/nonlinear/NonlinearFactorGraph.h @@ -32,6 +32,10 @@ namespace gtsam { class Ordering; class GaussianFactorGraph; class SymbolicFactorGraph; + template + class Expression; + template + class ExpressionFactor; /** * Formatting options when saving in GraphViz format using @@ -47,6 +51,7 @@ namespace gtsam { bool mergeSimilarFactors; ///< Merge multiple factors that have the same connectivity bool plotFactorPoints; ///< Plots each factor as a dot between the variables bool connectKeysToFactor; ///< Draw a line from each key within a factor to the dot of the factor + bool binaryEdges; ///< just use non-dotted edges for binary factors std::map factorPositions; ///< (optional for each factor) Manually specify factor "dot" positions. /// Default constructor sets up robot coordinates. Paper horizontal is robot Y, /// paper vertical is robot X. Default figure size of 5x5 in. @@ -54,7 +59,7 @@ namespace gtsam { paperHorizontalAxis(Y), paperVerticalAxis(X), figureWidthInches(5), figureHeightInches(5), scale(1), mergeSimilarFactors(false), plotFactorPoints(true), - connectKeysToFactor(true) {} + connectKeysToFactor(true), binaryEdges(true) {} }; @@ -150,6 +155,18 @@ namespace gtsam { */ NonlinearFactorGraph rekey(const std::map& rekey_mapping) const; + /** + * Directly add ExpressionFactor that implements |h(x)-z|^2_R + * @param h expression that implements measurement function + * @param z measurement + * @param R model + */ + template + void addExpressionFactor(const SharedNoiseModel& R, const T& z, + const Expression& h) { + push_back(boost::make_shared >(R, z, h)); + } + private: /** Serialization function */ diff --git a/gtsam/nonlinear/expressionTesting.h b/gtsam/nonlinear/expressionTesting.h index ab6703f3a..47f61b8b1 100644 --- a/gtsam/nonlinear/expressionTesting.h +++ b/gtsam/nonlinear/expressionTesting.h @@ -21,9 +21,9 @@ #include #include +#include #include #include -#include #include #include @@ -32,47 +32,6 @@ namespace gtsam { -/** - * Linearize a nonlinear factor using numerical differentiation - * The benefit of this method is that it does not need to know what types are - * involved to evaluate the factor. If all the machinery of gtsam is working - * correctly, we should get the correct numerical derivatives out the other side. - */ -JacobianFactor linearizeNumerically(const NoiseModelFactor& factor, - const Values& values, double delta = 1e-5) { - - // We will fill a map of Jacobians - std::map jacobians; - - // Get size - Eigen::VectorXd e = factor.whitenedError(values); - const size_t rows = e.size(); - - // Loop over all variables - VectorValues dX = values.zeroVectors(); - BOOST_FOREACH(Key key, factor) { - // Compute central differences using the values struct. - const size_t cols = dX.dim(key); - Matrix J = Matrix::Zero(rows, cols); - for (size_t col = 0; col < cols; ++col) { - Eigen::VectorXd dx = Eigen::VectorXd::Zero(cols); - dx[col] = delta; - dX[key] = dx; - Values eval_values = values.retract(dX); - Eigen::VectorXd left = factor.whitenedError(eval_values); - dx[col] = -delta; - dX[key] = dx; - eval_values = values.retract(dX); - Eigen::VectorXd right = factor.whitenedError(eval_values); - J.col(col) = (left - right) * (1.0 / (2.0 * delta)); - } - jacobians[key] = J; - } - - // Next step...return JacobianFactor - return JacobianFactor(jacobians, -e); -} - namespace internal { // CPPUnitLite-style test for linearization of a factor void testFactorJacobians(TestResult& result_, const std::string& name_, @@ -88,7 +47,7 @@ void testFactorJacobians(TestResult& result_, const std::string& name_, // Check cast result and then equality CHECK(actual); - EXPECT( assert_equal(expected, *actual, tolerance)); + EXPECT(assert_equal(expected, *actual, tolerance)); } } @@ -112,7 +71,7 @@ void testExpressionJacobians(TestResult& result_, const std::string& name_, expression.value(values), expression); testFactorJacobians(result_, name_, f, values, nd_step, tolerance); } -} +} // namespace internal } // namespace gtsam /// \brief Check the Jacobians produced by an expression against finite differences. diff --git a/gtsam/nonlinear/expressions.h b/gtsam/nonlinear/expressions.h index a549517e5..2f46d6d74 100644 --- a/gtsam/nonlinear/expressions.h +++ b/gtsam/nonlinear/expressions.h @@ -18,6 +18,12 @@ Expression between(const Expression& t1, const Expression& t2) { return Expression(traits::Between, t1, t2); } +// Generics +template +Expression compose(const Expression& t1, const Expression& t2) { + return Expression(traits::Compose, t1, t2); +} + typedef Expression double_; typedef Expression Vector3_; diff --git a/gtsam/nonlinear/factorTesting.h b/gtsam/nonlinear/factorTesting.h new file mode 100644 index 000000000..442b29382 --- /dev/null +++ b/gtsam/nonlinear/factorTesting.h @@ -0,0 +1,68 @@ +/* ---------------------------------------------------------------------------- + + * 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 factorTesting.h + * @date September 18, 2014 + * @author Frank Dellaert + * @author Paul Furgale + * @brief Evaluate derivatives of a nonlinear factor numerically + */ + +#pragma once + +#include +#include + +namespace gtsam { + +/** + * Linearize a nonlinear factor using numerical differentiation + * The benefit of this method is that it does not need to know what types are + * involved to evaluate the factor. If all the machinery of gtsam is working + * correctly, we should get the correct numerical derivatives out the other side. + */ +JacobianFactor linearizeNumerically(const NoiseModelFactor& factor, + const Values& values, double delta = 1e-5) { + + // We will fill a vector of key/Jacobians pairs (a map would sort) + std::vector > jacobians; + + // Get size + Eigen::VectorXd e = factor.whitenedError(values); + const size_t rows = e.size(); + + // Loop over all variables + VectorValues dX = values.zeroVectors(); + BOOST_FOREACH(Key key, factor) { + // Compute central differences using the values struct. + const size_t cols = dX.dim(key); + Matrix J = Matrix::Zero(rows, cols); + for (size_t col = 0; col < cols; ++col) { + Eigen::VectorXd dx = Eigen::VectorXd::Zero(cols); + dx[col] = delta; + dX[key] = dx; + Values eval_values = values.retract(dX); + Eigen::VectorXd left = factor.whitenedError(eval_values); + dx[col] = -delta; + dX[key] = dx; + eval_values = values.retract(dX); + Eigen::VectorXd right = factor.whitenedError(eval_values); + J.col(col) = (left - right) * (1.0 / (2.0 * delta)); + } + jacobians.push_back(std::make_pair(key,J)); + } + + // Next step...return JacobianFactor + return JacobianFactor(jacobians, -e); +} + +} // namespace gtsam diff --git a/gtsam/nonlinear/CallRecord.h b/gtsam/nonlinear/internal/CallRecord.h similarity index 92% rename from gtsam/nonlinear/CallRecord.h rename to gtsam/nonlinear/internal/CallRecord.h index 159a841e5..90aa8a8be 100644 --- a/gtsam/nonlinear/CallRecord.h +++ b/gtsam/nonlinear/internal/CallRecord.h @@ -20,18 +20,9 @@ #pragma once -#include -#include -#include - -#include +#include namespace gtsam { - -class JacobianMap; -// forward declaration - -//----------------------------------------------------------------------------- namespace internal { /** @@ -64,8 +55,6 @@ struct ConvertToVirtualFunctionSupportedMatrixType { } }; -} // namespace internal - /** * The CallRecord is an abstract base class for the any class that stores * the Jacobians of applying a function with respect to each of its arguments, @@ -94,9 +83,8 @@ struct CallRecord { inline void reverseAD2(const Eigen::MatrixBase & dFdT, JacobianMap& jacobians) const { _reverseAD3( - internal::ConvertToVirtualFunctionSupportedMatrixType< - (Derived::RowsAtCompileTime > 5)>::convert(dFdT), - jacobians); + ConvertToVirtualFunctionSupportedMatrixType< + (Derived::RowsAtCompileTime > 5)>::convert(dFdT), jacobians); } // This overload supports matrices with both rows and columns dynamically sized. @@ -140,7 +128,6 @@ private: */ const int CallRecordMaxVirtualStaticRows = 5; -namespace internal { /** * The CallRecordImplementor implements the CallRecord interface for a Derived class by * delegating to its corresponding (templated) non-virtual methods. @@ -193,5 +180,4 @@ private: }; } // namespace internal - } // gtsam diff --git a/gtsam/nonlinear/internal/ExecutionTrace.h b/gtsam/nonlinear/internal/ExecutionTrace.h new file mode 100644 index 000000000..37ce4dfd5 --- /dev/null +++ b/gtsam/nonlinear/internal/ExecutionTrace.h @@ -0,0 +1,166 @@ +/* ---------------------------------------------------------------------------- + + * 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 ExecutionTrace.h + * @date May 11, 2015 + * @author Frank Dellaert + * @brief Execution trace for expressions + */ + +#pragma once + +#include +#include +#include + +#include + +#include + +namespace gtsam { +namespace internal { + +template struct CallRecord; + +/// Storage type for the execution trace. +/// It enforces the proper alignment in a portable way. +/// Provide a traceSize() sized array of this type to traceExecution as traceStorage. +static const unsigned TraceAlignment = 16; +typedef boost::aligned_storage<1, TraceAlignment>::type ExecutionTraceStorage; + +template +struct UseBlockIf { + static void addToJacobian(const Eigen::MatrixBase& dTdA, + JacobianMap& jacobians, Key key) { + // block makes HUGE difference + jacobians(key).block( + 0, 0) += dTdA; + } +}; + +/// Handle Leaf Case for Dynamic Matrix type (slower) +template +struct UseBlockIf { + static void addToJacobian(const Eigen::MatrixBase& dTdA, + JacobianMap& jacobians, Key key) { + jacobians(key) += dTdA; + } +}; + +/// Handle Leaf Case: reverse AD ends here, by writing a matrix into Jacobians +template +void handleLeafCase(const Eigen::MatrixBase& dTdA, + JacobianMap& jacobians, Key key) { + UseBlockIf< + Derived::RowsAtCompileTime != Eigen::Dynamic + && Derived::ColsAtCompileTime != Eigen::Dynamic, Derived>::addToJacobian( + dTdA, jacobians, key); +} + +/** + * The ExecutionTrace class records a tree-structured expression's execution. + * + * The class looks a bit complicated but it is so for performance. + * It is a tagged union that obviates the need to create + * a ExecutionTrace subclass for Constants and Leaf Expressions. Instead + * the key for the leaf is stored in the space normally used to store a + * CallRecord*. Nothing is stored for a Constant. + * + * A full execution trace of a Binary(Unary(Binary(Leaf,Constant)),Leaf) would be: + * Trace(Function) -> + * BinaryRecord with two traces in it + * trace1(Function) -> + * UnaryRecord with one trace in it + * trace1(Function) -> + * BinaryRecord with two traces in it + * trace1(Leaf) + * trace2(Constant) + * trace2(Leaf) + * Hence, there are three Record structs, written to memory by traceExecution + */ +template +class ExecutionTrace { + static const int Dim = traits::dimension; + enum { + Constant, Leaf, Function + } kind; + union { + Key key; + CallRecord* ptr; + } content; +public: + /// Pointer always starts out as a Constant + ExecutionTrace() : + kind(Constant) { + } + /// Change pointer to a Leaf Record + void setLeaf(Key key) { + kind = Leaf; + content.key = key; + } + /// Take ownership of pointer to a Function Record + void setFunction(CallRecord* record) { + kind = Function; + content.ptr = record; + } + /// Print + void print(const std::string& indent = "") const { + if (kind == Constant) + std::cout << indent << "Constant" << std::endl; + else if (kind == Leaf) + std::cout << indent << "Leaf, key = " << content.key << std::endl; + else if (kind == Function) { + std::cout << indent << "Function" << std::endl; + content.ptr->print(indent + " "); + } + } + /// Return record pointer, quite unsafe, used only for testing + template + boost::optional record() { + if (kind != Function) + return boost::none; + else { + Record* p = dynamic_cast(content.ptr); + return p ? boost::optional(p) : boost::none; + } + } + /** + * *** This is the main entry point for reverse AD, called from Expression *** + * Called only once, either inserts I into Jacobians (Leaf) or starts AD (Function) + */ + typedef Eigen::Matrix JacobianTT; + void startReverseAD1(JacobianMap& jacobians) const { + if (kind == Leaf) { + // This branch will only be called on trivial Leaf expressions, i.e. Priors + static const JacobianTT I = JacobianTT::Identity(); + handleLeafCase(I, jacobians, content.key); + } else if (kind == Function) + // This is the more typical entry point, starting the AD pipeline + // Inside startReverseAD2 the correctly dimensioned pipeline is chosen. + content.ptr->startReverseAD2(jacobians); + } + // Either add to Jacobians (Leaf) or propagate (Function) + template + void reverseAD1(const Eigen::MatrixBase & dTdA, + JacobianMap& jacobians) const { + if (kind == Leaf) + handleLeafCase(dTdA, jacobians, content.key); + else if (kind == Function) + content.ptr->reverseAD2(dTdA, jacobians); + } + + /// Define type so we can apply it as a meta-function + typedef ExecutionTrace type; +}; + +} // namespace internal +} // namespace gtsam diff --git a/gtsam/nonlinear/internal/ExpressionNode.h b/gtsam/nonlinear/internal/ExpressionNode.h new file mode 100644 index 000000000..e7aa34db0 --- /dev/null +++ b/gtsam/nonlinear/internal/ExpressionNode.h @@ -0,0 +1,516 @@ +/* ---------------------------------------------------------------------------- + + * 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 ExpressionNode.h + * @date May 10, 2015 + * @author Frank Dellaert + * @author Paul Furgale + * @brief ExpressionNode class + */ + +#pragma once + +#include +#include +#include + +#include // operator typeid +#include +#include + +class ExpressionFactorBinaryTest; +// Forward declare for testing + +namespace gtsam { +namespace internal { + +template +T & upAlign(T & value, unsigned requiredAlignment = TraceAlignment) { + // right now only word sized types are supported. + // Easy to extend if needed, + // by somehow inferring the unsigned integer of same size + BOOST_STATIC_ASSERT(sizeof(T) == sizeof(size_t)); + size_t & uiValue = reinterpret_cast(value); + size_t misAlignment = uiValue % requiredAlignment; + if (misAlignment) { + uiValue += requiredAlignment - misAlignment; + } + return value; +} +template +T upAligned(T value, unsigned requiredAlignment = TraceAlignment) { + return upAlign(value, requiredAlignment); +} + +//----------------------------------------------------------------------------- + +/** + * Expression node. The superclass for objects that do the heavy lifting + * An Expression has a pointer to an ExpressionNode underneath + * allowing Expressions to have polymorphic behaviour even though they + * are passed by value. This is the same way boost::function works. + * http://loki-lib.sourceforge.net/html/a00652.html + */ +template +class ExpressionNode { + +protected: + + size_t traceSize_; + + /// Constructor, traceSize is size of the execution trace of expression rooted here + ExpressionNode(size_t traceSize = 0) : + traceSize_(traceSize) { + } + +public: + + /// Destructor + virtual ~ExpressionNode() { + } + + /// Streaming + GTSAM_EXPORT + friend std::ostream &operator<<(std::ostream &os, + const ExpressionNode& node) { + os << "Expression of type " << typeid(T).name(); + if (node.traceSize_ > 0) + os << ", trace size = " << node.traceSize_; + os << "\n"; + return os; + } + + /// Return keys that play in this expression as a set + virtual std::set keys() const { + std::set keys; + return keys; + } + + /// Return dimensions for each argument, as a map + virtual void dims(std::map& map) const { + } + + // Return size needed for memory buffer in traceExecution + size_t traceSize() const { + return traceSize_; + } + + /// Return value + virtual T value(const Values& values) const = 0; + + /// Construct an execution trace for reverse AD + virtual T traceExecution(const Values& values, ExecutionTrace& trace, + ExecutionTraceStorage* traceStorage) const = 0; +}; + +//----------------------------------------------------------------------------- +/// Constant Expression +template +class ConstantExpression: public ExpressionNode { + + /// The constant value + T constant_; + + /// Constructor with a value, yielding a constant + ConstantExpression(const T& value) : + constant_(value) { + } + + friend class Expression ; + +public: + + /// Destructor + virtual ~ConstantExpression() { + } + + /// Return value + virtual T value(const Values& values) const { + return constant_; + } + + /// Construct an execution trace for reverse AD + virtual T traceExecution(const Values& values, ExecutionTrace& trace, + ExecutionTraceStorage* traceStorage) const { + return constant_; + } +}; + +//----------------------------------------------------------------------------- +/// Leaf Expression, if no chart is given, assume default chart and value_type is just the plain value +template +class LeafExpression: public ExpressionNode { + typedef T value_type; + + /// The key into values + Key key_; + + /// Constructor with a single key + LeafExpression(Key key) : + key_(key) { + } + + friend class Expression ; + +public: + + /// Destructor + virtual ~LeafExpression() { + } + + /// Return keys that play in this expression + virtual std::set keys() const { + std::set keys; + keys.insert(key_); + return keys; + } + + /// Return dimensions for each argument + virtual void dims(std::map& map) const { + map[key_] = traits::dimension; + } + + /// Return value + virtual T value(const Values& values) const { + return values.at(key_); + } + + /// Construct an execution trace for reverse AD + virtual T traceExecution(const Values& values, ExecutionTrace& trace, + ExecutionTraceStorage* traceStorage) const { + trace.setLeaf(key_); + return values.at(key_); + } + +}; + +//----------------------------------------------------------------------------- +/// meta-function to generate fixed-size JacobianTA type +template +struct Jacobian { + typedef Eigen::Matrix::dimension, traits::dimension> type; +}; + +// Eigen format for printing Jacobians +static const Eigen::IOFormat kMatlabFormat(0, 1, " ", "; ", "", "", "[", "]"); + +//----------------------------------------------------------------------------- +/// Unary Function Expression +template +class UnaryExpression: public ExpressionNode { + + typedef typename Expression::template UnaryFunction::type Function; + boost::shared_ptr > expression1_; + Function function_; + + /// Constructor with a unary function f, and input argument e1 + UnaryExpression(Function f, const Expression& e1) : + expression1_(e1.root()), function_(f) { + ExpressionNode::traceSize_ = upAligned(sizeof(Record)) + e1.traceSize(); + } + + friend class Expression ; + +public: + + /// Destructor + virtual ~UnaryExpression() { + } + + /// Return value + virtual T value(const Values& values) const { + return function_(expression1_->value(values), boost::none); + } + + /// Return keys that play in this expression + virtual std::set keys() const { + return expression1_->keys(); + } + + /// Return dimensions for each argument + virtual void dims(std::map& map) const { + expression1_->dims(map); + } + + // Inner Record Class + struct Record: public CallRecordImplementor::dimension> { + + A1 value1; + ExecutionTrace trace1; + typename Jacobian::type dTdA1; + + /// Print to std::cout + void print(const std::string& indent) const { + std::cout << indent << "UnaryExpression::Record {" << std::endl; + std::cout << indent << dTdA1.format(kMatlabFormat) << std::endl; + trace1.print(indent); + std::cout << indent << "}" << std::endl; + } + + /// Start the reverse AD process + void startReverseAD4(JacobianMap& jacobians) const { + // This is the crucial point where the size of the AD pipeline is selected. + // One pipeline is started for each argument, but the number of rows in each + // pipeline is the same, namely the dimension of the output argument T. + // For example, if the entire expression is rooted by a binary function + // yielding a 2D result, then the matrix dTdA will have 2 rows. + // ExecutionTrace::reverseAD1 just passes this on to CallRecord::reverseAD2 + // which calls the correctly sized CallRecord::reverseAD3, which in turn + // calls reverseAD4 below. + trace1.reverseAD1(dTdA1, jacobians); + } + + /// Given df/dT, multiply in dT/dA and continue reverse AD process + template + void reverseAD4(const SomeMatrix & dFdT, JacobianMap& jacobians) const { + trace1.reverseAD1(dFdT * dTdA1, jacobians); + } + }; + + /// Construct an execution trace for reverse AD + virtual T traceExecution(const Values& values, ExecutionTrace& trace, + ExecutionTraceStorage* ptr) const { + assert(reinterpret_cast(ptr) % TraceAlignment == 0); + + // Create the record at the start of the traceStorage and advance the pointer + Record* record = new (ptr) Record(); + ptr += upAligned(sizeof(Record)); + + // Record the traces for all arguments + // After this, the traceStorage pointer is set to after what was written + // Write an Expression execution trace in record->trace + // Iff Constant or Leaf, this will not write to traceStorage, only to trace. + // Iff the expression is functional, write all Records in traceStorage buffer + // Return value of type T is recorded in record->value + record->value1 = expression1_->traceExecution(values, record->trace1, ptr); + + // ptr is never modified by traceExecution, but if traceExecution has + // written in the buffer, the next caller expects we advance the pointer + ptr += expression1_->traceSize(); + trace.setFunction(record); + + return function_(record->value1, record->dTdA1); + } +}; + +//----------------------------------------------------------------------------- +/// Binary Expression +template +class BinaryExpression: public ExpressionNode { + + typedef typename Expression::template BinaryFunction::type Function; + boost::shared_ptr > expression1_; + boost::shared_ptr > expression2_; + Function function_; + + /// Constructor with a binary function f, and two input arguments + BinaryExpression(Function f, const Expression& e1, + const Expression& e2) : + expression1_(e1.root()), expression2_(e2.root()), function_(f) { + ExpressionNode::traceSize_ = // + upAligned(sizeof(Record)) + e1.traceSize() + e2.traceSize(); + } + + friend class Expression ; + friend class ::ExpressionFactorBinaryTest; + +public: + + /// Destructor + virtual ~BinaryExpression() { + } + + /// Return value + virtual T value(const Values& values) const { + using boost::none; + return function_(expression1_->value(values), expression2_->value(values), + none, none); + } + + /// Return keys that play in this expression + virtual std::set keys() const { + std::set keys = expression1_->keys(); + std::set myKeys = expression2_->keys(); + keys.insert(myKeys.begin(), myKeys.end()); + return keys; + } + + /// Return dimensions for each argument + virtual void dims(std::map& map) const { + expression1_->dims(map); + expression2_->dims(map); + } + + // Inner Record Class + struct Record: public CallRecordImplementor::dimension> { + + A1 value1; + ExecutionTrace trace1; + typename Jacobian::type dTdA1; + + A2 value2; + ExecutionTrace trace2; + typename Jacobian::type dTdA2; + + /// Print to std::cout + void print(const std::string& indent) const { + std::cout << indent << "BinaryExpression::Record {" << std::endl; + std::cout << indent << dTdA1.format(kMatlabFormat) << std::endl; + trace1.print(indent); + std::cout << indent << dTdA2.format(kMatlabFormat) << std::endl; + trace2.print(indent); + std::cout << indent << "}" << std::endl; + } + + /// Start the reverse AD process, see comments in Base + void startReverseAD4(JacobianMap& jacobians) const { + trace1.reverseAD1(dTdA1, jacobians); + trace2.reverseAD1(dTdA2, jacobians); + } + + /// Given df/dT, multiply in dT/dA and continue reverse AD process + template + void reverseAD4(const SomeMatrix & dFdT, JacobianMap& jacobians) const { + trace1.reverseAD1(dFdT * dTdA1, jacobians); + trace2.reverseAD1(dFdT * dTdA2, jacobians); + } + }; + + /// Construct an execution trace for reverse AD, see UnaryExpression for explanation + virtual T traceExecution(const Values& values, ExecutionTrace& trace, + ExecutionTraceStorage* ptr) const { + assert(reinterpret_cast(ptr) % TraceAlignment == 0); + Record* record = new (ptr) Record(); + ptr += upAligned(sizeof(Record)); + record->value1 = expression1_->traceExecution(values, record->trace1, ptr); + record->value2 = expression2_->traceExecution(values, record->trace2, ptr); + ptr += expression1_->traceSize() + expression2_->traceSize(); + trace.setFunction(record); + return function_(record->value1, record->value2, record->dTdA1, + record->dTdA2); + } +}; + +//----------------------------------------------------------------------------- +/// Ternary Expression +template +class TernaryExpression: public ExpressionNode { + + typedef typename Expression::template TernaryFunction::type Function; + boost::shared_ptr > expression1_; + boost::shared_ptr > expression2_; + boost::shared_ptr > expression3_; + Function function_; + + /// Constructor with a ternary function f, and two input arguments + TernaryExpression(Function f, const Expression& e1, + const Expression& e2, const Expression& e3) : + expression1_(e1.root()), expression2_(e2.root()), expression3_(e3.root()), // + function_(f) { + ExpressionNode::traceSize_ = upAligned(sizeof(Record)) + // + e1.traceSize() + e2.traceSize() + e3.traceSize(); + } + + friend class Expression ; + +public: + + /// Destructor + virtual ~TernaryExpression() { + } + + /// Return value + virtual T value(const Values& values) const { + using boost::none; + return function_(expression1_->value(values), expression2_->value(values), + expression3_->value(values), none, none, none); + } + + /// Return keys that play in this expression + virtual std::set keys() const { + std::set keys = expression1_->keys(); + std::set myKeys = expression2_->keys(); + keys.insert(myKeys.begin(), myKeys.end()); + myKeys = expression3_->keys(); + keys.insert(myKeys.begin(), myKeys.end()); + return keys; + } + + /// Return dimensions for each argument + virtual void dims(std::map& map) const { + expression1_->dims(map); + expression2_->dims(map); + expression3_->dims(map); + } + + // Inner Record Class + struct Record: public CallRecordImplementor::dimension> { + + A1 value1; + ExecutionTrace trace1; + typename Jacobian::type dTdA1; + + A2 value2; + ExecutionTrace trace2; + typename Jacobian::type dTdA2; + + A3 value3; + ExecutionTrace trace3; + typename Jacobian::type dTdA3; + + /// Print to std::cout + void print(const std::string& indent) const { + std::cout << indent << "TernaryExpression::Record {" << std::endl; + std::cout << indent << dTdA1.format(kMatlabFormat) << std::endl; + trace1.print(indent); + std::cout << indent << dTdA2.format(kMatlabFormat) << std::endl; + trace2.print(indent); + std::cout << indent << dTdA3.format(kMatlabFormat) << std::endl; + trace3.print(indent); + std::cout << indent << "}" << std::endl; + } + + /// Start the reverse AD process, see comments in Base + void startReverseAD4(JacobianMap& jacobians) const { + trace1.reverseAD1(dTdA1, jacobians); + trace2.reverseAD1(dTdA2, jacobians); + trace3.reverseAD1(dTdA3, jacobians); + } + + /// Given df/dT, multiply in dT/dA and continue reverse AD process + template + void reverseAD4(const SomeMatrix & dFdT, JacobianMap& jacobians) const { + trace1.reverseAD1(dFdT * dTdA1, jacobians); + trace2.reverseAD1(dFdT * dTdA2, jacobians); + trace3.reverseAD1(dFdT * dTdA3, jacobians); + } + }; + + /// Construct an execution trace for reverse AD, see UnaryExpression for explanation + virtual T traceExecution(const Values& values, ExecutionTrace& trace, + ExecutionTraceStorage* ptr) const { + assert(reinterpret_cast(ptr) % TraceAlignment == 0); + Record* record = new (ptr) Record(); + ptr += upAligned(sizeof(Record)); + record->value1 = expression1_->traceExecution(values, record->trace1, ptr); + record->value2 = expression2_->traceExecution(values, record->trace2, ptr); + record->value3 = expression3_->traceExecution(values, record->trace3, ptr); + ptr += expression1_->traceSize() + expression2_->traceSize() + + expression3_->traceSize(); + trace.setFunction(record); + return function_(record->value1, record->value2, record->value3, + record->dTdA1, record->dTdA2, record->dTdA3); + } +}; + +} // namespace internal +} // namespace gtsam diff --git a/gtsam/nonlinear/internal/JacobianMap.h b/gtsam/nonlinear/internal/JacobianMap.h new file mode 100644 index 000000000..f4d2e9565 --- /dev/null +++ b/gtsam/nonlinear/internal/JacobianMap.h @@ -0,0 +1,54 @@ +/* ---------------------------------------------------------------------------- + + * 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 JacobianMap.h + * @date May 11, 2015 + * @author Frank Dellaert + * @brief JacobianMap for returning derivatives from expressions + */ + +#pragma once + +#include +#include +#include + +namespace gtsam { +namespace internal { + +// A JacobianMap is the primary mechanism by which derivatives are returned. +// Expressions are designed to write their derivatives into an already allocated +// Jacobian of the correct size, of type VerticalBlockMatrix. +// The JacobianMap provides a mapping from keys to the underlying blocks. +class JacobianMap { +private: + typedef FastVector Keys; + const FastVector& keys_; + VerticalBlockMatrix& Ab_; + +public: + /// Construct a JacobianMap for writing into a VerticalBlockMatrix Ab + JacobianMap(const Keys& keys, VerticalBlockMatrix& Ab) : + keys_(keys), Ab_(Ab) { + } + + /// Access blocks of via key + VerticalBlockMatrix::Block operator()(Key key) { + Keys::const_iterator it = std::find(keys_.begin(), keys_.end(), key); + DenseIndex block = it - keys_.begin(); + return Ab_(block); + } +}; + +} // namespace internal +} // namespace gtsam + diff --git a/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp b/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp index f3858c818..3f477098a 100644 --- a/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp +++ b/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp @@ -265,9 +265,10 @@ TEST(AdaptAutoDiff, Snavely) { typedef AdaptAutoDiff Adaptor; Expression expression(Adaptor(), P, X); #ifdef GTSAM_USE_QUATERNIONS - EXPECT_LONGS_EQUAL(400,expression.traceSize()); // Todo, should be zero + EXPECT_LONGS_EQUAL(384,expression.traceSize()); // Todo, should be zero #else - EXPECT_LONGS_EQUAL(432,expression.traceSize()); // Todo, should be zero + EXPECT_LONGS_EQUAL(sizeof(internal::BinaryExpression::Record), + expression.traceSize()); // Todo, should be zero #endif set expected = list_of(1)(2); EXPECT(expected == expression.keys()); diff --git a/gtsam/nonlinear/tests/testCallRecord.cpp b/gtsam/nonlinear/tests/testCallRecord.cpp index 483b5ffa9..208f0b284 100644 --- a/gtsam/nonlinear/tests/testCallRecord.cpp +++ b/gtsam/nonlinear/tests/testCallRecord.cpp @@ -18,7 +18,7 @@ * @brief unit tests for CallRecord class */ -#include +#include #include #include @@ -32,7 +32,7 @@ static const int Cols = 3; int dynamicIfAboveMax(int i){ - if(i > CallRecordMaxVirtualStaticRows){ + if(i > internal::CallRecordMaxVirtualStaticRows){ return Eigen::Dynamic; } else return i; @@ -80,13 +80,13 @@ struct Record: public internal::CallRecordImplementor { } void print(const std::string& indent) const { } - void startReverseAD4(JacobianMap& jacobians) const { + void startReverseAD4(internal::JacobianMap& jacobians) const { } mutable CallConfig cc; private: template - void reverseAD4(const SomeMatrix & dFdT, JacobianMap& jacobians) const { + void reverseAD4(const SomeMatrix & dFdT, internal::JacobianMap& jacobians) const { cc.compTimeRows = SomeMatrix::RowsAtCompileTime; cc.compTimeCols = SomeMatrix::ColsAtCompileTime; cc.runTimeRows = dFdT.rows(); @@ -97,7 +97,7 @@ struct Record: public internal::CallRecordImplementor { friend struct internal::CallRecordImplementor; }; -JacobianMap & NJM= *static_cast(NULL); +internal::JacobianMap & NJM= *static_cast(NULL); /* ************************************************************************* */ typedef Eigen::Matrix DynRowMat; diff --git a/gtsam/nonlinear/tests/testExecutionTrace.cpp b/gtsam/nonlinear/tests/testExecutionTrace.cpp new file mode 100644 index 000000000..731f69816 --- /dev/null +++ b/gtsam/nonlinear/tests/testExecutionTrace.cpp @@ -0,0 +1,39 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------1------------------------------------------- */ + +/** + * @file testExecutionTrace.cpp + * @date May 11, 2015 + * @author Frank Dellaert + * @brief unit tests for Expression internals + */ + +#include +#include + +#include + +using namespace std; +using namespace gtsam; + +/* ************************************************************************* */ +// Constant +TEST(ExecutionTrace, construct) { + internal::ExecutionTrace trace; +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ + diff --git a/gtsam/nonlinear/tests/testExpression.cpp b/gtsam/nonlinear/tests/testExpression.cpp index 80100af5e..3e86bcb8c 100644 --- a/gtsam/nonlinear/tests/testExpression.cpp +++ b/gtsam/nonlinear/tests/testExpression.cpp @@ -42,7 +42,7 @@ static const Rot3 someR = Rot3::RzRyRx(1, 2, 3); /* ************************************************************************* */ // Constant -TEST(Expression, constant) { +TEST(Expression, Constant) { Expression R(someR); Values values; Rot3 actual = R.value(values); @@ -68,24 +68,27 @@ TEST(Expression, Leaves) { Point3 somePoint(1, 2, 3); values.insert(Symbol('p', 10), somePoint); std::vector > points = createUnknowns(10, 'p', 1); - EXPECT(assert_equal(somePoint,points.back().value(values))); + EXPECT(assert_equal(somePoint, points.back().value(values))); } /* ************************************************************************* */ // Unary(Leaf) namespace unary { -Point2 f0(const Point3& p, OptionalJacobian<2,3> H) { +Point2 f1(const Point3& p, OptionalJacobian<2, 3> H) { return Point2(); } double f2(const Point3& p, OptionalJacobian<1, 3> H) { return 0.0; } +Vector f3(const Point3& p, OptionalJacobian H) { + return p.vector(); +} Expression p(1); set expected = list_of(1); } -TEST(Expression, Unary0) { +TEST(Expression, Unary1) { using namespace unary; - Expression e(f0, p); + Expression e(f1, p); EXPECT(expected == e.keys()); } TEST(Expression, Unary2) { @@ -94,6 +97,12 @@ TEST(Expression, Unary2) { EXPECT(expected == e.keys()); } /* ************************************************************************* */ +// Unary(Leaf), dynamic +TEST(Expression, Unary3) { + using namespace unary; +// Expression e(f3, p); +} +/* ************************************************************************* */ //Nullary Method TEST(Expression, NullaryMethod) { @@ -118,7 +127,7 @@ TEST(Expression, NullaryMethod) { EXPECT(actual == sqrt(50)); Matrix expected(1, 3); expected << 3.0 / sqrt(50.0), 4.0 / sqrt(50.0), 5.0 / sqrt(50.0); - EXPECT(assert_equal(expected,H[0])); + EXPECT(assert_equal(expected, H[0])); } /* ************************************************************************* */ // Binary(Leaf,Leaf) @@ -149,12 +158,12 @@ TEST(Expression, BinaryKeys) { TEST(Expression, BinaryDimensions) { map actual, expected = map_list_of(1, 6)(2, 3); binary::p_cam.dims(actual); - EXPECT(actual==expected); + EXPECT(actual == expected); } /* ************************************************************************* */ // dimensions TEST(Expression, BinaryTraceSize) { - typedef BinaryExpression Binary; + typedef internal::BinaryExpression Binary; size_t expectedTraceSize = sizeof(Binary::Record); EXPECT_LONGS_EQUAL(expectedTraceSize, binary::p_cam.traceSize()); } @@ -180,17 +189,26 @@ TEST(Expression, TreeKeys) { TEST(Expression, TreeDimensions) { map actual, expected = map_list_of(1, 6)(2, 3)(3, 5); tree::uv_hat.dims(actual); - EXPECT(actual==expected); + EXPECT(actual == expected); } /* ************************************************************************* */ // TraceSize TEST(Expression, TreeTraceSize) { - typedef UnaryExpression Unary; - typedef BinaryExpression Binary1; - typedef BinaryExpression Binary2; - size_t expectedTraceSize = sizeof(Unary::Record) + sizeof(Binary1::Record) - + sizeof(Binary2::Record); - EXPECT_LONGS_EQUAL(expectedTraceSize, tree::uv_hat.traceSize()); + typedef internal::BinaryExpression Binary1; + EXPECT_LONGS_EQUAL(internal::upAligned(sizeof(Binary1::Record)), + tree::p_cam.traceSize()); + + typedef internal::UnaryExpression Unary; + EXPECT_LONGS_EQUAL( + internal::upAligned(sizeof(Unary::Record)) + tree::p_cam.traceSize(), + tree::projection.traceSize()); + + EXPECT_LONGS_EQUAL(0, tree::K.traceSize()); + + typedef internal::BinaryExpression Binary2; + EXPECT_LONGS_EQUAL( + internal::upAligned(sizeof(Binary2::Record)) + tree::K.traceSize() + + tree::projection.traceSize(), tree::uv_hat.traceSize()); } /* ************************************************************************* */ @@ -234,7 +252,8 @@ TEST(Expression, compose3) { /* ************************************************************************* */ // Test with ternary function Rot3 composeThree(const Rot3& R1, const Rot3& R2, const Rot3& R3, - OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2, OptionalJacobian<3, 3> H3) { + OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2, + OptionalJacobian<3, 3> H3) { // return dummy derivatives (not correct, but that's ok for testing here) if (H1) *H1 = eye(3); diff --git a/gtsam/nonlinear/tests/testExpressionFactor.cpp b/gtsam/nonlinear/tests/testExpressionFactor.cpp index 99b8120e3..2fb544edf 100644 --- a/gtsam/nonlinear/tests/testExpressionFactor.cpp +++ b/gtsam/nonlinear/tests/testExpressionFactor.cpp @@ -22,6 +22,7 @@ #include #include #include +#include #include #include @@ -169,7 +170,7 @@ static Point2 myUncal(const Cal3_S2& K, const Point2& p, // Binary(Leaf,Leaf) TEST(ExpressionFactor, Binary) { - typedef BinaryExpression Binary; + typedef internal::BinaryExpression Binary; Cal3_S2_ K_(1); Point2_ p_(2); @@ -184,11 +185,15 @@ TEST(ExpressionFactor, Binary) { EXPECT_LONGS_EQUAL(8, sizeof(double)); EXPECT_LONGS_EQUAL(16, sizeof(Point2)); EXPECT_LONGS_EQUAL(40, sizeof(Cal3_S2)); - EXPECT_LONGS_EQUAL(16, sizeof(ExecutionTrace)); - EXPECT_LONGS_EQUAL(16, sizeof(ExecutionTrace)); - EXPECT_LONGS_EQUAL(2*5*8, sizeof(Jacobian::type)); - EXPECT_LONGS_EQUAL(2*2*8, sizeof(Jacobian::type)); - size_t expectedRecordSize = 16 + 16 + 40 + 2 * 16 + 80 + 32; + EXPECT_LONGS_EQUAL(16, sizeof(internal::ExecutionTrace)); + EXPECT_LONGS_EQUAL(16, sizeof(internal::ExecutionTrace)); + EXPECT_LONGS_EQUAL(2*5*8, sizeof(internal::Jacobian::type)); + EXPECT_LONGS_EQUAL(2*2*8, sizeof(internal::Jacobian::type)); + size_t expectedRecordSize = sizeof(Cal3_S2) + + sizeof(internal::ExecutionTrace) + + +sizeof(internal::Jacobian::type) + sizeof(Point2) + + sizeof(internal::ExecutionTrace) + + sizeof(internal::Jacobian::type); EXPECT_LONGS_EQUAL(expectedRecordSize + 8, sizeof(Binary::Record)); // Check size @@ -197,8 +202,8 @@ TEST(ExpressionFactor, Binary) { EXPECT_LONGS_EQUAL(expectedRecordSize + 8, size); // Use Variable Length Array, allocated on stack by gcc // Note unclear for Clang: http://clang.llvm.org/compatibility.html#vla - ExecutionTraceStorage traceStorage[size]; - ExecutionTrace trace; + internal::ExecutionTraceStorage traceStorage[size]; + internal::ExecutionTrace trace; Point2 value = binary.traceExecution(values, trace, traceStorage); EXPECT(assert_equal(Point2(),value, 1e-9)); // trace.print(); @@ -212,9 +217,8 @@ TEST(ExpressionFactor, Binary) { // Check matrices boost::optional r = trace.record(); CHECK(r); - EXPECT( - assert_equal(expected25, (Matrix) (*r)-> jacobian(), 1e-9)); - EXPECT( assert_equal(expected22, (Matrix) (*r)->jacobian(), 1e-9)); + EXPECT(assert_equal(expected25, (Matrix ) (*r)->dTdA1, 1e-9)); + EXPECT(assert_equal(expected22, (Matrix ) (*r)->dTdA2, 1e-9)); } /* ************************************************************************* */ // Unary(Binary(Leaf,Leaf)) @@ -250,22 +254,22 @@ TEST(ExpressionFactor, Shallow) { LONGS_EQUAL(3,dims[1]); // traceExecution of shallow tree - typedef UnaryExpression Unary; - typedef BinaryExpression Binary; + typedef internal::UnaryExpression Unary; + typedef internal::BinaryExpression Binary; size_t expectedTraceSize = sizeof(Unary::Record) + sizeof(Binary::Record); - EXPECT_LONGS_EQUAL(112, sizeof(Unary::Record)); + EXPECT_LONGS_EQUAL(96, sizeof(Unary::Record)); #ifdef GTSAM_USE_QUATERNIONS EXPECT_LONGS_EQUAL(352, sizeof(Binary::Record)); - LONGS_EQUAL(112+352, expectedTraceSize); + LONGS_EQUAL(96+352, expectedTraceSize); #else - EXPECT_LONGS_EQUAL(400, sizeof(Binary::Record)); - LONGS_EQUAL(112+400, expectedTraceSize); + EXPECT_LONGS_EQUAL(384, sizeof(Binary::Record)); + LONGS_EQUAL(96+384, expectedTraceSize); #endif size_t size = expression.traceSize(); CHECK(size); EXPECT_LONGS_EQUAL(expectedTraceSize, size); - ExecutionTraceStorage traceStorage[size]; - ExecutionTrace trace; + internal::ExecutionTraceStorage traceStorage[size]; + internal::ExecutionTrace trace; Point2 value = expression.traceExecution(values, trace, traceStorage); EXPECT(assert_equal(Point2(),value, 1e-9)); // trace.print(); @@ -277,7 +281,7 @@ TEST(ExpressionFactor, Shallow) { // Check matrices boost::optional r = trace.record(); CHECK(r); - EXPECT(assert_equal(expected23, (Matrix)(*r)->jacobian(), 1e-9)); + EXPECT(assert_equal(expected23, (Matrix)(*r)->dTdA1, 1e-9)); // Linearization ExpressionFactor f2(model, measured, expression); @@ -327,7 +331,7 @@ TEST(ExpressionFactor, tree) { boost::shared_ptr gf2 = f2.linearize(values); EXPECT( assert_equal(*expected, *gf2, 1e-9)); - TernaryExpression::Function fff = project6; + Expression::TernaryFunction::type fff = project6; // Try ternary version ExpressionFactor f3(model, measured, project3(x, p, K)); @@ -493,6 +497,11 @@ TEST(ExpressionFactor, tree_finite_differences) { EXPECT_CORRECT_EXPRESSION_JACOBIANS(uv_hat, values, fd_step, tolerance); } +TEST(ExpressionFactor, push_back) { + NonlinearFactorGraph graph; + graph.addExpressionFactor(model, Point2(0, 0), leaf::p); +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/slam/PriorFactor.h b/gtsam/slam/PriorFactor.h index 9322ed9b2..a738d4cf0 100644 --- a/gtsam/slam/PriorFactor.h +++ b/gtsam/slam/PriorFactor.h @@ -57,6 +57,11 @@ namespace gtsam { Base(model, key), prior_(prior) { } + /** Convenience constructor that takes a full covariance argument */ + PriorFactor(Key key, const VALUE& prior, const Matrix& covariance) : + Base(noiseModel::Gaussian::Covariance(covariance), key), prior_(prior) { + } + /// @return a deep copy of this factor virtual gtsam::NonlinearFactor::shared_ptr clone() const { return boost::static_pointer_cast( diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 0667c1427..851adacf5 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -64,8 +64,8 @@ string findExampleDataFile(const string& name) { throw invalid_argument( "gtsam::findExampleDataFile could not find a matching file in\n" - SOURCE_TREE_DATASET_DIR " or\n" - INSTALLED_DATASET_DIR " named\n" + + GTSAM_SOURCE_TREE_DATASET_DIR " or\n" + GTSAM_INSTALLED_DATASET_DIR " named\n" + name + ", " + name + ".graph, or " + name + ".txt"); } diff --git a/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp b/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp index cfed9ae0c..ac2c31077 100644 --- a/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp +++ b/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp @@ -88,11 +88,13 @@ int main(int argc, char** argv){ } Values initial_pose_values = initial_estimate.filter(); - if(output_poses){ - init_pose3Out.open(init_poseOutput.c_str(),ios::out); - for(int i = 1; i<=initial_pose_values.size(); i++){ - init_pose3Out << i << " " << initial_pose_values.at(Symbol('x',i)).matrix().format(Eigen::IOFormat(Eigen::StreamPrecision, 0, - " ", " ")) << endl; + if (output_poses) { + init_pose3Out.open(init_poseOutput.c_str(), ios::out); + for (size_t i = 1; i <= initial_pose_values.size(); i++) { + init_pose3Out + << i << " " + << initial_pose_values.at(Symbol('x', i)).matrix().format( + Eigen::IOFormat(Eigen::StreamPrecision, 0, " ", " ")) << endl; } } @@ -141,7 +143,7 @@ int main(int argc, char** argv){ if(output_poses){ pose3Out.open(poseOutput.c_str(),ios::out); - for(int i = 1; i<=pose_values.size(); i++){ + for(size_t i = 1; i<=pose_values.size(); i++){ pose3Out << i << " " << pose_values.at(Symbol('x',i)).matrix().format(Eigen::IOFormat(Eigen::StreamPrecision, 0, " ", " ")) << endl; } diff --git a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp index 05b0bb49e..563da4a9f 100644 --- a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp @@ -28,25 +28,34 @@ namespace gtsam { /* ************************************************************************* */ -void BatchFixedLagSmoother::print(const std::string& s, const KeyFormatter& keyFormatter) const { +void BatchFixedLagSmoother::print(const std::string& s, + const KeyFormatter& keyFormatter) const { FixedLagSmoother::print(s, keyFormatter); // TODO: What else to print? } /* ************************************************************************* */ -bool BatchFixedLagSmoother::equals(const FixedLagSmoother& rhs, double tol) const { - const BatchFixedLagSmoother* e = dynamic_cast (&rhs); - return e != NULL - && FixedLagSmoother::equals(*e, tol) - && factors_.equals(e->factors_, tol) - && theta_.equals(e->theta_, tol); +bool BatchFixedLagSmoother::equals(const FixedLagSmoother& rhs, + double tol) const { + const BatchFixedLagSmoother* e = + dynamic_cast(&rhs); + return e != NULL && FixedLagSmoother::equals(*e, tol) + && factors_.equals(e->factors_, tol) && theta_.equals(e->theta_, tol); } /* ************************************************************************* */ -FixedLagSmoother::Result BatchFixedLagSmoother::update(const NonlinearFactorGraph& newFactors, const Values& newTheta, const KeyTimestampMap& timestamps) { +Matrix BatchFixedLagSmoother::marginalCovariance(Key key) const { + throw std::runtime_error( + "BatchFixedLagSmoother::marginalCovariance not implemented"); +} + +/* ************************************************************************* */ +FixedLagSmoother::Result BatchFixedLagSmoother::update( + const NonlinearFactorGraph& newFactors, const Values& newTheta, + const KeyTimestampMap& timestamps) { const bool debug = ISDEBUG("BatchFixedLagSmoother update"); - if(debug) { + if (debug) { std::cout << "BatchFixedLagSmoother::update() START" << std::endl; } @@ -70,11 +79,13 @@ FixedLagSmoother::Result BatchFixedLagSmoother::update(const NonlinearFactorGrap // Get current timestamp double current_timestamp = getCurrentTimestamp(); - if(debug) std::cout << "Current Timestamp: " << current_timestamp << std::endl; + if (debug) + std::cout << "Current Timestamp: " << current_timestamp << std::endl; // Find the set of variables to be marginalized out - std::set marginalizableKeys = findKeysBefore(current_timestamp - smootherLag_); - if(debug) { + std::set marginalizableKeys = findKeysBefore( + current_timestamp - smootherLag_); + if (debug) { std::cout << "Marginalizable Keys: "; BOOST_FOREACH(Key key, marginalizableKeys) { std::cout << DefaultKeyFormatter(key) << " "; @@ -90,19 +101,19 @@ FixedLagSmoother::Result BatchFixedLagSmoother::update(const NonlinearFactorGrap // Optimize gttic(optimize); Result result; - if(factors_.size() > 0) { + if (factors_.size() > 0) { result = optimize(); } gttoc(optimize); // Marginalize out old variables. gttic(marginalize); - if(marginalizableKeys.size() > 0) { + if (marginalizableKeys.size() > 0) { marginalize(marginalizableKeys); } gttoc(marginalize); - if(debug) { + if (debug) { std::cout << "BatchFixedLagSmoother::update() FINISH" << std::endl; } @@ -110,11 +121,12 @@ FixedLagSmoother::Result BatchFixedLagSmoother::update(const NonlinearFactorGrap } /* ************************************************************************* */ -void BatchFixedLagSmoother::insertFactors(const NonlinearFactorGraph& newFactors) { +void BatchFixedLagSmoother::insertFactors( + const NonlinearFactorGraph& newFactors) { BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, newFactors) { Key index; // Insert the factor into an existing hole in the factor graph, if possible - if(availableSlots_.size() > 0) { + if (availableSlots_.size() > 0) { index = availableSlots_.front(); availableSlots_.pop(); factors_.replace(index, factor); @@ -130,9 +142,10 @@ void BatchFixedLagSmoother::insertFactors(const NonlinearFactorGraph& newFactors } /* ************************************************************************* */ -void BatchFixedLagSmoother::removeFactors(const std::set& deleteFactors) { +void BatchFixedLagSmoother::removeFactors( + const std::set& deleteFactors) { BOOST_FOREACH(size_t slot, deleteFactors) { - if(factors_.at(slot)) { + if (factors_.at(slot)) { // Remove references to this factor from the FactorIndex BOOST_FOREACH(Key key, *(factors_.at(slot))) { factorIndex_[key].erase(slot); @@ -143,7 +156,8 @@ void BatchFixedLagSmoother::removeFactors(const std::set& deleteFactors) availableSlots_.push(slot); } else { // TODO: Throw an error?? - std::cout << "Attempting to remove a factor from slot " << slot << ", but it is already NULL." << std::endl; + std::cout << "Attempting to remove a factor from slot " << slot + << ", but it is already NULL." << std::endl; } } } @@ -159,7 +173,7 @@ void BatchFixedLagSmoother::eraseKeys(const std::set& keys) { factorIndex_.erase(key); // Erase the key from the set of linearized keys - if(linearKeys_.exists(key)) { + if (linearKeys_.exists(key)) { linearKeys_.erase(key); } } @@ -178,11 +192,11 @@ void BatchFixedLagSmoother::reorder(const std::set& marginalizeKeys) { const bool debug = ISDEBUG("BatchFixedLagSmoother reorder"); - if(debug) { + if (debug) { std::cout << "BatchFixedLagSmoother::reorder() START" << std::endl; } - if(debug) { + if (debug) { std::cout << "Marginalizable Keys: "; BOOST_FOREACH(Key key, marginalizeKeys) { std::cout << DefaultKeyFormatter(key) << " "; @@ -191,13 +205,14 @@ void BatchFixedLagSmoother::reorder(const std::set& marginalizeKeys) { } // COLAMD groups will be used to place marginalize keys in Group 0, and everything else in Group 1 - ordering_ = Ordering::colamdConstrainedFirst(factors_, std::vector(marginalizeKeys.begin(), marginalizeKeys.end())); + ordering_ = Ordering::colamdConstrainedFirst(factors_, + std::vector(marginalizeKeys.begin(), marginalizeKeys.end())); - if(debug) { + if (debug) { ordering_.print("New Ordering: "); } - if(debug) { + if (debug) { std::cout << "BatchFixedLagSmoother::reorder() FINISH" << std::endl; } } @@ -207,7 +222,7 @@ FixedLagSmoother::Result BatchFixedLagSmoother::optimize() { const bool debug = ISDEBUG("BatchFixedLagSmoother optimize"); - if(debug) { + if (debug) { std::cout << "BatchFixedLagSmoother::optimize() START" << std::endl; } @@ -231,14 +246,19 @@ FixedLagSmoother::Result BatchFixedLagSmoother::optimize() { result.error = factors_.error(evalpoint); // check if we're already close enough - if(result.error <= errorTol) { - if(debug) { std::cout << "BatchFixedLagSmoother::optimize Exiting, as error = " << result.error << " < " << errorTol << std::endl; } + if (result.error <= errorTol) { + if (debug) { + std::cout << "BatchFixedLagSmoother::optimize Exiting, as error = " + << result.error << " < " << errorTol << std::endl; + } return result; } - if(debug) { - std::cout << "BatchFixedLagSmoother::optimize linearValues: " << linearKeys_.size() << std::endl; - std::cout << "BatchFixedLagSmoother::optimize Initial error: " << result.error << std::endl; + if (debug) { + std::cout << "BatchFixedLagSmoother::optimize linearValues: " + << linearKeys_.size() << std::endl; + std::cout << "BatchFixedLagSmoother::optimize Initial error: " + << result.error << std::endl; } // Use a custom optimization loop so the linearization points can be controlled @@ -254,9 +274,12 @@ FixedLagSmoother::Result BatchFixedLagSmoother::optimize() { GaussianFactorGraph linearFactorGraph = *factors_.linearize(theta_); // Keep increasing lambda until we make make progress - while(true) { + while (true) { - if(debug) { std::cout << "BatchFixedLagSmoother::optimize trying lambda = " << lambda << std::endl; } + if (debug) { + std::cout << "BatchFixedLagSmoother::optimize trying lambda = " + << lambda << std::endl; + } // Add prior factors at the current solution gttic(damp); @@ -267,10 +290,11 @@ FixedLagSmoother::Result BatchFixedLagSmoother::optimize() { double sigma = 1.0 / std::sqrt(lambda); BOOST_FOREACH(const VectorValues::KeyValuePair& key_value, delta_) { size_t dim = key_value.second.size(); - Matrix A = Matrix::Identity(dim,dim); + Matrix A = Matrix::Identity(dim, dim); Vector b = key_value.second; SharedDiagonal model = noiseModel::Isotropic::Sigma(dim, sigma); - GaussianFactor::shared_ptr prior(new JacobianFactor(key_value.first, A, b, model)); + GaussianFactor::shared_ptr prior( + new JacobianFactor(key_value.first, A, b, model)); dampedFactorGraph.push_back(prior); } } @@ -279,7 +303,8 @@ FixedLagSmoother::Result BatchFixedLagSmoother::optimize() { gttic(solve); // Solve Damped Gaussian Factor Graph - newDelta = dampedFactorGraph.optimize(ordering_, parameters_.getEliminationFunction()); + newDelta = dampedFactorGraph.optimize(ordering_, + parameters_.getEliminationFunction()); // update the evalpoint with the new delta evalpoint = theta_.retract(newDelta); gttoc(solve); @@ -289,12 +314,14 @@ FixedLagSmoother::Result BatchFixedLagSmoother::optimize() { double error = factors_.error(evalpoint); gttoc(compute_error); - if(debug) { - std::cout << "BatchFixedLagSmoother::optimize linear delta norm = " << newDelta.norm() << std::endl; - std::cout << "BatchFixedLagSmoother::optimize next error = " << error << std::endl; + if (debug) { + std::cout << "BatchFixedLagSmoother::optimize linear delta norm = " + << newDelta.norm() << std::endl; + std::cout << "BatchFixedLagSmoother::optimize next error = " << error + << std::endl; } - if(error < result.error) { + if (error < result.error) { // Keep this change // Update the error value result.error = error; @@ -303,7 +330,7 @@ FixedLagSmoother::Result BatchFixedLagSmoother::optimize() { // Reset the deltas to zeros delta_.setZero(); // Put the linearization points and deltas back for specific variables - if(enforceConsistency_ && (linearKeys_.size() > 0)) { + if (enforceConsistency_ && (linearKeys_.size() > 0)) { theta_.update(linearKeys_); BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, linearKeys_) { delta_.at(key_value.key) = newDelta.at(key_value.key); @@ -311,16 +338,18 @@ FixedLagSmoother::Result BatchFixedLagSmoother::optimize() { } // Decrease lambda for next time lambda /= lambdaFactor; - if(lambda < lambdaLowerBound) { + if (lambda < lambdaLowerBound) { lambda = lambdaLowerBound; } // End this lambda search iteration break; } else { // Reject this change - if(lambda >= lambdaUpperBound) { + if (lambda >= lambdaUpperBound) { // The maximum lambda has been used. Print a warning and end the search. - std::cout << "Warning: Levenberg-Marquardt giving up because cannot decrease error with maximum lambda" << std::endl; + std::cout + << "Warning: Levenberg-Marquardt giving up because cannot decrease error with maximum lambda" + << std::endl; break; } else { // Increase lambda and continue searching @@ -331,15 +360,22 @@ FixedLagSmoother::Result BatchFixedLagSmoother::optimize() { } gttoc(optimizer_iteration); - if(debug) { std::cout << "BatchFixedLagSmoother::optimize using lambda = " << lambda << std::endl; } + if (debug) { + std::cout << "BatchFixedLagSmoother::optimize using lambda = " << lambda + << std::endl; + } result.iterations++; - } while(result.iterations < maxIterations && - !checkConvergence(relativeErrorTol, absoluteErrorTol, errorTol, previousError, result.error, NonlinearOptimizerParams::SILENT)); + } while (result.iterations < maxIterations + && !checkConvergence(relativeErrorTol, absoluteErrorTol, errorTol, + previousError, result.error, NonlinearOptimizerParams::SILENT)); - if(debug) { std::cout << "BatchFixedLagSmoother::optimize newError: " << result.error << std::endl; } + if (debug) { + std::cout << "BatchFixedLagSmoother::optimize newError: " << result.error + << std::endl; + } - if(debug) { + if (debug) { std::cout << "BatchFixedLagSmoother::optimize() FINISH" << std::endl; } @@ -356,9 +392,10 @@ void BatchFixedLagSmoother::marginalize(const std::set& marginalizeKeys) { const bool debug = ISDEBUG("BatchFixedLagSmoother marginalize"); - if(debug) std::cout << "BatchFixedLagSmoother::marginalize Begin" << std::endl; + if (debug) + std::cout << "BatchFixedLagSmoother::marginalize Begin" << std::endl; - if(debug) { + if (debug) { std::cout << "BatchFixedLagSmoother::marginalize Marginalize Keys: "; BOOST_FOREACH(Key key, marginalizeKeys) { std::cout << DefaultKeyFormatter(key) << " "; @@ -374,7 +411,7 @@ void BatchFixedLagSmoother::marginalize(const std::set& marginalizeKeys) { removedFactorSlots.insert(slots.begin(), slots.end()); } - if(debug) { + if (debug) { std::cout << "BatchFixedLagSmoother::marginalize Removed Factor Slots: "; BOOST_FOREACH(size_t slot, removedFactorSlots) { std::cout << slot << " "; @@ -385,20 +422,24 @@ void BatchFixedLagSmoother::marginalize(const std::set& marginalizeKeys) { // Add the removed factors to a factor graph NonlinearFactorGraph removedFactors; BOOST_FOREACH(size_t slot, removedFactorSlots) { - if(factors_.at(slot)) { + if (factors_.at(slot)) { removedFactors.push_back(factors_.at(slot)); } } - if(debug) { - PrintSymbolicGraph(removedFactors, "BatchFixedLagSmoother::marginalize Removed Factors: "); + if (debug) { + PrintSymbolicGraph(removedFactors, + "BatchFixedLagSmoother::marginalize Removed Factors: "); } // Calculate marginal factors on the remaining keys - NonlinearFactorGraph marginalFactors = calculateMarginalFactors(removedFactors, theta_, marginalizeKeys, parameters_.getEliminationFunction()); + NonlinearFactorGraph marginalFactors = calculateMarginalFactors( + removedFactors, theta_, marginalizeKeys, + parameters_.getEliminationFunction()); - if(debug) { - PrintSymbolicGraph(removedFactors, "BatchFixedLagSmoother::marginalize Marginal Factors: "); + if (debug) { + PrintSymbolicGraph(removedFactors, + "BatchFixedLagSmoother::marginalize Marginal Factors: "); } // Remove marginalized factors from the factor graph @@ -412,7 +453,8 @@ void BatchFixedLagSmoother::marginalize(const std::set& marginalizeKeys) { } /* ************************************************************************* */ -void BatchFixedLagSmoother::PrintKeySet(const std::set& keys, const std::string& label) { +void BatchFixedLagSmoother::PrintKeySet(const std::set& keys, + const std::string& label) { std::cout << label; BOOST_FOREACH(gtsam::Key key, keys) { std::cout << " " << gtsam::DefaultKeyFormatter(key); @@ -421,7 +463,8 @@ void BatchFixedLagSmoother::PrintKeySet(const std::set& keys, const std::st } /* ************************************************************************* */ -void BatchFixedLagSmoother::PrintKeySet(const gtsam::FastSet& keys, const std::string& label) { +void BatchFixedLagSmoother::PrintKeySet(const gtsam::FastSet& keys, + const std::string& label) { std::cout << label; BOOST_FOREACH(gtsam::Key key, keys) { std::cout << " " << gtsam::DefaultKeyFormatter(key); @@ -430,9 +473,10 @@ void BatchFixedLagSmoother::PrintKeySet(const gtsam::FastSet& keys, const s } /* ************************************************************************* */ -void BatchFixedLagSmoother::PrintSymbolicFactor(const NonlinearFactor::shared_ptr& factor) { +void BatchFixedLagSmoother::PrintSymbolicFactor( + const NonlinearFactor::shared_ptr& factor) { std::cout << "f("; - if(factor) { + if (factor) { BOOST_FOREACH(Key key, factor->keys()) { std::cout << " " << gtsam::DefaultKeyFormatter(key); } @@ -443,7 +487,8 @@ void BatchFixedLagSmoother::PrintSymbolicFactor(const NonlinearFactor::shared_pt } /* ************************************************************************* */ -void BatchFixedLagSmoother::PrintSymbolicFactor(const GaussianFactor::shared_ptr& factor) { +void BatchFixedLagSmoother::PrintSymbolicFactor( + const GaussianFactor::shared_ptr& factor) { std::cout << "f("; BOOST_FOREACH(Key key, factor->keys()) { std::cout << " " << gtsam::DefaultKeyFormatter(key); @@ -452,7 +497,8 @@ void BatchFixedLagSmoother::PrintSymbolicFactor(const GaussianFactor::shared_ptr } /* ************************************************************************* */ -void BatchFixedLagSmoother::PrintSymbolicGraph(const NonlinearFactorGraph& graph, const std::string& label) { +void BatchFixedLagSmoother::PrintSymbolicGraph( + const NonlinearFactorGraph& graph, const std::string& label) { std::cout << label << std::endl; BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, graph) { PrintSymbolicFactor(factor); @@ -460,59 +506,79 @@ void BatchFixedLagSmoother::PrintSymbolicGraph(const NonlinearFactorGraph& graph } /* ************************************************************************* */ -void BatchFixedLagSmoother::PrintSymbolicGraph(const GaussianFactorGraph& graph, const std::string& label) { +void BatchFixedLagSmoother::PrintSymbolicGraph(const GaussianFactorGraph& graph, + const std::string& label) { std::cout << label << std::endl; BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, graph) { PrintSymbolicFactor(factor); } } - - /* ************************************************************************* */ -NonlinearFactorGraph BatchFixedLagSmoother::calculateMarginalFactors(const NonlinearFactorGraph& graph, const Values& theta, - const std::set& marginalizeKeys, const GaussianFactorGraph::Eliminate& eliminateFunction) { +NonlinearFactorGraph BatchFixedLagSmoother::calculateMarginalFactors( + const NonlinearFactorGraph& graph, const Values& theta, + const std::set& marginalizeKeys, + const GaussianFactorGraph::Eliminate& eliminateFunction) { const bool debug = ISDEBUG("BatchFixedLagSmoother calculateMarginalFactors"); - if(debug) std::cout << "BatchFixedLagSmoother::calculateMarginalFactors START" << std::endl; + if (debug) + std::cout << "BatchFixedLagSmoother::calculateMarginalFactors START" + << std::endl; - if(debug) PrintKeySet(marginalizeKeys, "BatchFixedLagSmoother::calculateMarginalFactors Marginalize Keys: "); + if (debug) + PrintKeySet(marginalizeKeys, + "BatchFixedLagSmoother::calculateMarginalFactors Marginalize Keys: "); // Get the set of all keys involved in the factor graph FastSet allKeys(graph.keys()); - if(debug) PrintKeySet(allKeys, "BatchFixedLagSmoother::calculateMarginalFactors All Keys: "); + if (debug) + PrintKeySet(allKeys, + "BatchFixedLagSmoother::calculateMarginalFactors All Keys: "); // Calculate the set of RemainingKeys = AllKeys \Intersect marginalizeKeys FastSet remainingKeys; - std::set_difference(allKeys.begin(), allKeys.end(), marginalizeKeys.begin(), marginalizeKeys.end(), std::inserter(remainingKeys, remainingKeys.end())); - if(debug) PrintKeySet(remainingKeys, "BatchFixedLagSmoother::calculateMarginalFactors Remaining Keys: "); + std::set_difference(allKeys.begin(), allKeys.end(), marginalizeKeys.begin(), + marginalizeKeys.end(), std::inserter(remainingKeys, remainingKeys.end())); + if (debug) + PrintKeySet(remainingKeys, + "BatchFixedLagSmoother::calculateMarginalFactors Remaining Keys: "); - if(marginalizeKeys.size() == 0) { + if (marginalizeKeys.size() == 0) { // There are no keys to marginalize. Simply return the input factors - if(debug) std::cout << "BatchFixedLagSmoother::calculateMarginalFactors FINISH" << std::endl; + if (debug) + std::cout << "BatchFixedLagSmoother::calculateMarginalFactors FINISH" + << std::endl; return graph; } else { // Create the linear factor graph GaussianFactorGraph linearFactorGraph = *graph.linearize(theta); // .first is the eliminated Bayes tree, while .second is the remaining factor graph - GaussianFactorGraph marginalLinearFactors = *linearFactorGraph.eliminatePartialMultifrontal(std::vector(marginalizeKeys.begin(), marginalizeKeys.end())).second; + GaussianFactorGraph marginalLinearFactors = + *linearFactorGraph.eliminatePartialMultifrontal( + std::vector(marginalizeKeys.begin(), marginalizeKeys.end())).second; // Wrap in nonlinear container factors NonlinearFactorGraph marginalFactors; marginalFactors.reserve(marginalLinearFactors.size()); BOOST_FOREACH(const GaussianFactor::shared_ptr& gaussianFactor, marginalLinearFactors) { - marginalFactors += boost::make_shared(gaussianFactor, theta); - if(debug) { - std::cout << "BatchFixedLagSmoother::calculateMarginalFactors Marginal Factor: "; + marginalFactors += boost::make_shared( + gaussianFactor, theta); + if (debug) { + std::cout + << "BatchFixedLagSmoother::calculateMarginalFactors Marginal Factor: "; PrintSymbolicFactor(marginalFactors.back()); } } - if(debug) PrintSymbolicGraph(marginalFactors, "BatchFixedLagSmoother::calculateMarginalFactors All Marginal Factors: "); + if (debug) + PrintSymbolicGraph(marginalFactors, + "BatchFixedLagSmoother::calculateMarginalFactors All Marginal Factors: "); - if(debug) std::cout << "BatchFixedLagSmoother::calculateMarginalFactors FINISH" << std::endl; + if (debug) + std::cout << "BatchFixedLagSmoother::calculateMarginalFactors FINISH" + << std::endl; return marginalFactors; } diff --git a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h index 5c66d411f..89da3d7e5 100644 --- a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h +++ b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h @@ -100,6 +100,12 @@ public: return delta_; } + /// Calculate marginal covariance on given variable + Matrix marginalCovariance(Key key) const; + + static NonlinearFactorGraph calculateMarginalFactors(const NonlinearFactorGraph& graph, const Values& theta, + const std::set& marginalizeKeys, const GaussianFactorGraph::Eliminate& eliminateFunction); + protected: /** A typedef defining an Key-Factor mapping **/ @@ -134,8 +140,6 @@ protected: /** A cross-reference structure to allow efficient factor lookups by key **/ FactorIndex factorIndex_; - - /** Augment the list of factors with a set of new factors */ void insertFactors(const NonlinearFactorGraph& newFactors); @@ -154,9 +158,6 @@ protected: /** Marginalize out selected variables */ void marginalize(const std::set& marginalizableKeys); - static NonlinearFactorGraph calculateMarginalFactors(const NonlinearFactorGraph& graph, const Values& theta, - const std::set& marginalizeKeys, const GaussianFactorGraph::Eliminate& eliminateFunction); - private: /** Private methods for printing debug information */ static void PrintKeySet(const std::set& keys, const std::string& label); diff --git a/gtsam_unstable/nonlinear/FixedLagSmoother.cpp b/gtsam_unstable/nonlinear/FixedLagSmoother.cpp index 66d367148..369db51c3 100644 --- a/gtsam_unstable/nonlinear/FixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/FixedLagSmoother.cpp @@ -37,7 +37,7 @@ bool FixedLagSmoother::equals(const FixedLagSmoother& rhs, double tol) const { void FixedLagSmoother::updateKeyTimestampMap(const KeyTimestampMap& timestamps) { // Loop through each key and add/update it in the map BOOST_FOREACH(const KeyTimestampMap::value_type& key_timestamp, timestamps) { - // Check to see if this key already exists inthe database + // Check to see if this key already exists in the database KeyTimestampMap::iterator keyIter = keyTimestampMap_.find(key_timestamp.first); // If the key already exists diff --git a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp index b5556994c..23cd42a0a 100644 --- a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp @@ -25,10 +25,13 @@ namespace gtsam { /* ************************************************************************* */ -void recursiveMarkAffectedKeys(const Key& key, const ISAM2Clique::shared_ptr& clique, std::set& additionalKeys) { +void recursiveMarkAffectedKeys(const Key& key, + const ISAM2Clique::shared_ptr& clique, std::set& additionalKeys) { // Check if the separator keys of the current clique contain the specified key - if(std::find(clique->conditional()->beginParents(), clique->conditional()->endParents(), key) != clique->conditional()->endParents()) { + if (std::find(clique->conditional()->beginParents(), + clique->conditional()->endParents(), key) + != clique->conditional()->endParents()) { // Mark the frontal keys of the current clique BOOST_FOREACH(Key i, clique->conditional()->frontals()) { @@ -44,32 +47,36 @@ void recursiveMarkAffectedKeys(const Key& key, const ISAM2Clique::shared_ptr& cl } /* ************************************************************************* */ -void IncrementalFixedLagSmoother::print(const std::string& s, const KeyFormatter& keyFormatter) const { +void IncrementalFixedLagSmoother::print(const std::string& s, + const KeyFormatter& keyFormatter) const { FixedLagSmoother::print(s, keyFormatter); // TODO: What else to print? } /* ************************************************************************* */ -bool IncrementalFixedLagSmoother::equals(const FixedLagSmoother& rhs, double tol) const { - const IncrementalFixedLagSmoother* e = dynamic_cast (&rhs); - return e != NULL - && FixedLagSmoother::equals(*e, tol) +bool IncrementalFixedLagSmoother::equals(const FixedLagSmoother& rhs, + double tol) const { + const IncrementalFixedLagSmoother* e = + dynamic_cast(&rhs); + return e != NULL && FixedLagSmoother::equals(*e, tol) && isam_.equals(e->isam_, tol); } /* ************************************************************************* */ -FixedLagSmoother::Result IncrementalFixedLagSmoother::update(const NonlinearFactorGraph& newFactors, const Values& newTheta, const KeyTimestampMap& timestamps) { +FixedLagSmoother::Result IncrementalFixedLagSmoother::update( + const NonlinearFactorGraph& newFactors, const Values& newTheta, + const KeyTimestampMap& timestamps) { const bool debug = ISDEBUG("IncrementalFixedLagSmoother update"); - if(debug) { + if (debug) { std::cout << "IncrementalFixedLagSmoother::update() Start" << std::endl; PrintSymbolicTree(isam_, "Bayes Tree Before Update:"); std::cout << "END" << std::endl; } FastVector removedFactors; - boost::optional > constrainedKeys = boost::none; + boost::optional > constrainedKeys = boost::none; // Update the Timestamps associated with the factor keys updateKeyTimestampMap(timestamps); @@ -77,12 +84,14 @@ FixedLagSmoother::Result IncrementalFixedLagSmoother::update(const NonlinearFact // Get current timestamp double current_timestamp = getCurrentTimestamp(); - if(debug) std::cout << "Current Timestamp: " << current_timestamp << std::endl; + if (debug) + std::cout << "Current Timestamp: " << current_timestamp << std::endl; // Find the set of variables to be marginalized out - std::set marginalizableKeys = findKeysBefore(current_timestamp - smootherLag_); + std::set marginalizableKeys = findKeysBefore( + current_timestamp - smootherLag_); - if(debug) { + if (debug) { std::cout << "Marginalizable Keys: "; BOOST_FOREACH(Key key, marginalizableKeys) { std::cout << DefaultKeyFormatter(key) << " "; @@ -93,11 +102,13 @@ FixedLagSmoother::Result IncrementalFixedLagSmoother::update(const NonlinearFact // Force iSAM2 to put the marginalizable variables at the beginning createOrderingConstraints(marginalizableKeys, constrainedKeys); - if(debug) { + if (debug) { std::cout << "Constrained Keys: "; - if(constrainedKeys) { - for(FastMap::const_iterator iter = constrainedKeys->begin(); iter != constrainedKeys->end(); ++iter) { - std::cout << DefaultKeyFormatter(iter->first) << "(" << iter->second << ") "; + if (constrainedKeys) { + for (FastMap::const_iterator iter = constrainedKeys->begin(); + iter != constrainedKeys->end(); ++iter) { + std::cout << DefaultKeyFormatter(iter->first) << "(" << iter->second + << ") "; } } std::cout << std::endl; @@ -114,23 +125,26 @@ FixedLagSmoother::Result IncrementalFixedLagSmoother::update(const NonlinearFact KeyList additionalMarkedKeys(additionalKeys.begin(), additionalKeys.end()); // Update iSAM2 - ISAM2Result isamResult = isam_.update(newFactors, newTheta, FastVector(), constrainedKeys, boost::none, additionalMarkedKeys); + ISAM2Result isamResult = isam_.update(newFactors, newTheta, + FastVector(), constrainedKeys, boost::none, additionalMarkedKeys); - if(debug) { - PrintSymbolicTree(isam_, "Bayes Tree After Update, Before Marginalization:"); + if (debug) { + PrintSymbolicTree(isam_, + "Bayes Tree After Update, Before Marginalization:"); std::cout << "END" << std::endl; } // Marginalize out any needed variables - if(marginalizableKeys.size() > 0) { - FastList leafKeys(marginalizableKeys.begin(), marginalizableKeys.end()); + if (marginalizableKeys.size() > 0) { + FastList leafKeys(marginalizableKeys.begin(), + marginalizableKeys.end()); isam_.marginalizeLeaves(leafKeys); } // Remove marginalized keys from the KeyTimestampMap eraseKeyTimestampMap(marginalizableKeys); - if(debug) { + if (debug) { PrintSymbolicTree(isam_, "Final Bayes Tree:"); std::cout << "END" << std::endl; } @@ -142,7 +156,8 @@ FixedLagSmoother::Result IncrementalFixedLagSmoother::update(const NonlinearFact result.nonlinearVariables = 0; result.error = 0; - if(debug) std::cout << "IncrementalFixedLagSmoother::update() Finish" << std::endl; + if (debug) + std::cout << "IncrementalFixedLagSmoother::update() Finish" << std::endl; return result; } @@ -151,30 +166,33 @@ FixedLagSmoother::Result IncrementalFixedLagSmoother::update(const NonlinearFact void IncrementalFixedLagSmoother::eraseKeysBefore(double timestamp) { TimestampKeyMap::iterator end = timestampKeyMap_.lower_bound(timestamp); TimestampKeyMap::iterator iter = timestampKeyMap_.begin(); - while(iter != end) { + while (iter != end) { keyTimestampMap_.erase(iter->second); timestampKeyMap_.erase(iter++); } } /* ************************************************************************* */ -void IncrementalFixedLagSmoother::createOrderingConstraints(const std::set& marginalizableKeys, boost::optional >& constrainedKeys) const { - if(marginalizableKeys.size() > 0) { - constrainedKeys = FastMap(); +void IncrementalFixedLagSmoother::createOrderingConstraints( + const std::set& marginalizableKeys, + boost::optional >& constrainedKeys) const { + if (marginalizableKeys.size() > 0) { + constrainedKeys = FastMap(); // Generate ordering constraints so that the marginalizable variables will be eliminated first // Set all variables to Group1 BOOST_FOREACH(const TimestampKeyMap::value_type& timestamp_key, timestampKeyMap_) { constrainedKeys->operator[](timestamp_key.second) = 1; } // Set marginalizable variables to Group0 - BOOST_FOREACH(Key key, marginalizableKeys){ + BOOST_FOREACH(Key key, marginalizableKeys) { constrainedKeys->operator[](key) = 0; } } } /* ************************************************************************* */ -void IncrementalFixedLagSmoother::PrintKeySet(const std::set& keys, const std::string& label) { +void IncrementalFixedLagSmoother::PrintKeySet(const std::set& keys, + const std::string& label) { std::cout << label; BOOST_FOREACH(gtsam::Key key, keys) { std::cout << " " << gtsam::DefaultKeyFormatter(key); @@ -183,7 +201,8 @@ void IncrementalFixedLagSmoother::PrintKeySet(const std::set& keys, const s } /* ************************************************************************* */ -void IncrementalFixedLagSmoother::PrintSymbolicFactor(const GaussianFactor::shared_ptr& factor) { +void IncrementalFixedLagSmoother::PrintSymbolicFactor( + const GaussianFactor::shared_ptr& factor) { std::cout << "f("; BOOST_FOREACH(gtsam::Key key, factor->keys()) { std::cout << " " << gtsam::DefaultKeyFormatter(key); @@ -192,7 +211,8 @@ void IncrementalFixedLagSmoother::PrintSymbolicFactor(const GaussianFactor::shar } /* ************************************************************************* */ -void IncrementalFixedLagSmoother::PrintSymbolicGraph(const GaussianFactorGraph& graph, const std::string& label) { +void IncrementalFixedLagSmoother::PrintSymbolicGraph( + const GaussianFactorGraph& graph, const std::string& label) { std::cout << label << std::endl; BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, graph) { PrintSymbolicFactor(factor); @@ -200,27 +220,28 @@ void IncrementalFixedLagSmoother::PrintSymbolicGraph(const GaussianFactorGraph& } /* ************************************************************************* */ -void IncrementalFixedLagSmoother::PrintSymbolicTree(const gtsam::ISAM2& isam, const std::string& label) { +void IncrementalFixedLagSmoother::PrintSymbolicTree(const gtsam::ISAM2& isam, + const std::string& label) { std::cout << label << std::endl; - if(!isam.roots().empty()) - { - BOOST_FOREACH(const ISAM2::sharedClique& root, isam.roots()){ - PrintSymbolicTreeHelper(root); + if (!isam.roots().empty()) { + BOOST_FOREACH(const ISAM2::sharedClique& root, isam.roots()) { + PrintSymbolicTreeHelper(root); } - } - else + } else std::cout << "{Empty Tree}" << std::endl; } /* ************************************************************************* */ -void IncrementalFixedLagSmoother::PrintSymbolicTreeHelper(const gtsam::ISAM2Clique::shared_ptr& clique, const std::string indent) { +void IncrementalFixedLagSmoother::PrintSymbolicTreeHelper( + const gtsam::ISAM2Clique::shared_ptr& clique, const std::string indent) { // Print the current clique std::cout << indent << "P( "; BOOST_FOREACH(gtsam::Key key, clique->conditional()->frontals()) { std::cout << gtsam::DefaultKeyFormatter(key) << " "; } - if(clique->conditional()->nrParents() > 0) std::cout << "| "; + if (clique->conditional()->nrParents() > 0) + std::cout << "| "; BOOST_FOREACH(gtsam::Key key, clique->conditional()->parents()) { std::cout << gtsam::DefaultKeyFormatter(key) << " "; } @@ -228,7 +249,7 @@ void IncrementalFixedLagSmoother::PrintSymbolicTreeHelper(const gtsam::ISAM2Cliq // Recursively print all of the children BOOST_FOREACH(const gtsam::ISAM2Clique::shared_ptr& child, clique->children) { - PrintSymbolicTreeHelper(child, indent+" "); + PrintSymbolicTreeHelper(child, indent + " "); } } diff --git a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h index 9f015ef19..31ae20c50 100644 --- a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h +++ b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h @@ -23,7 +23,6 @@ #include #include - namespace gtsam { /** @@ -31,7 +30,7 @@ namespace gtsam { * such that the active states are placed in/near the root. This base class implements a function * to calculate the ordering, and an update function to incorporate new factors into the HMF. */ -class GTSAM_UNSTABLE_EXPORT IncrementalFixedLagSmoother : public FixedLagSmoother { +class GTSAM_UNSTABLE_EXPORT IncrementalFixedLagSmoother: public FixedLagSmoother { public: @@ -39,20 +38,30 @@ public: typedef boost::shared_ptr shared_ptr; /** default constructor */ - IncrementalFixedLagSmoother(double smootherLag = 0.0, const ISAM2Params& parameters = ISAM2Params()) - : FixedLagSmoother(smootherLag), isam_(parameters) {} + IncrementalFixedLagSmoother(double smootherLag = 0.0, + const ISAM2Params& parameters = ISAM2Params()) : + FixedLagSmoother(smootherLag), isam_(parameters) { + } /** destructor */ - virtual ~IncrementalFixedLagSmoother() {} + virtual ~IncrementalFixedLagSmoother() { + } /** Print the factor for debugging and testing (implementing Testable) */ - virtual void print(const std::string& s = "IncrementalFixedLagSmoother:\n", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + virtual void print(const std::string& s = "IncrementalFixedLagSmoother:\n", + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; /** Check if two IncrementalFixedLagSmoother Objects are equal */ virtual bool equals(const FixedLagSmoother& rhs, double tol = 1e-9) const; - /** Add new factors, updating the solution and relinearizing as needed. */ - Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), const Values& newTheta = Values(), + /** + * Add new factors, updating the solution and re-linearizing as needed. + * @param newFactors new factors on old and/or new variables + * @param newTheta new values for new variables only + * @param timestamps an (optional) map from keys to real time stamps + */ + Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), + const Values& newTheta = Values(), // const KeyTimestampMap& timestamps = KeyTimestampMap()); /** Compute an estimate from the incomplete linear delta computed during the last update. @@ -94,6 +103,11 @@ public: return isam_.getDelta(); } + /// Calculate marginal covariance on given variable + Matrix marginalCovariance(Key key) const { + return isam_.marginalCovariance(key); + } + protected: /** An iSAM2 object used to perform inference. The smoother lag is controlled * by what factors are removed each iteration */ @@ -103,16 +117,23 @@ protected: void eraseKeysBefore(double timestamp); /** Fill in an iSAM2 ConstrainedKeys structure such that the provided keys are eliminated before all others */ - void createOrderingConstraints(const std::set& marginalizableKeys, boost::optional >& constrainedKeys) const; + void createOrderingConstraints(const std::set& marginalizableKeys, + boost::optional >& constrainedKeys) const; private: /** Private methods for printing debug information */ - static void PrintKeySet(const std::set& keys, const std::string& label = "Keys:"); + static void PrintKeySet(const std::set& keys, const std::string& label = + "Keys:"); static void PrintSymbolicFactor(const GaussianFactor::shared_ptr& factor); - static void PrintSymbolicGraph(const GaussianFactorGraph& graph, const std::string& label = "Factor Graph:"); - static void PrintSymbolicTree(const gtsam::ISAM2& isam, const std::string& label = "Bayes Tree:"); - static void PrintSymbolicTreeHelper(const gtsam::ISAM2Clique::shared_ptr& clique, const std::string indent = ""); + static void PrintSymbolicGraph(const GaussianFactorGraph& graph, + const std::string& label = "Factor Graph:"); + static void PrintSymbolicTree(const gtsam::ISAM2& isam, + const std::string& label = "Bayes Tree:"); + static void PrintSymbolicTreeHelper( + const gtsam::ISAM2Clique::shared_ptr& clique, const std::string indent = + ""); -}; // IncrementalFixedLagSmoother +}; +// IncrementalFixedLagSmoother -} /// namespace gtsam +}/// namespace gtsam diff --git a/gtsam_unstable/nonlinear/tests/testExpressionMeta.cpp b/gtsam_unstable/nonlinear/tests/testExpressionMeta.cpp deleted file mode 100644 index 7c2f9d9b9..000000000 --- a/gtsam_unstable/nonlinear/tests/testExpressionMeta.cpp +++ /dev/null @@ -1,247 +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 testExpressionMeta.cpp - * @date October 14, 2014 - * @author Frank Dellaert - * @brief Test meta-programming constructs for Expressions - */ - -#include -#include -#include -#include - -#include -#include - -using namespace std; -using namespace gtsam; - -/* ************************************************************************* */ -namespace mpl = boost::mpl; - -#include -#include -template struct Incomplete; - -// Check generation of FunctionalNode -typedef mpl::vector MyTypes; -typedef FunctionalNode::type Generated; -//Incomplete incomplete; - -// Try generating vectors of ExecutionTrace -typedef mpl::vector, ExecutionTrace > ExpectedTraces; - -typedef mpl::transform >::type MyTraces; -BOOST_MPL_ASSERT((mpl::equal< ExpectedTraces, MyTraces >)); - -template -struct MakeTrace { - typedef ExecutionTrace type; -}; -typedef mpl::transform >::type MyTraces1; -BOOST_MPL_ASSERT((mpl::equal< ExpectedTraces, MyTraces1 >)); - -// Try generating vectors of Expression types -typedef mpl::vector, Expression > ExpectedExpressions; -typedef mpl::transform >::type Expressions; -BOOST_MPL_ASSERT((mpl::equal< ExpectedExpressions, Expressions >)); - -// Try generating vectors of Jacobian types -typedef mpl::vector ExpectedJacobians; -typedef mpl::transform >::type Jacobians; -BOOST_MPL_ASSERT((mpl::equal< ExpectedJacobians, Jacobians >)); - -// Try accessing a Jacobian -typedef mpl::at_c::type Jacobian23; // base zero ! -BOOST_MPL_ASSERT((boost::is_same< Matrix23, Jacobian23>)); - -/* ************************************************************************* */ -// Boost Fusion includes allow us to create/access values from MPL vectors -#include -#include - -// Create a value and access it -TEST(ExpressionFactor, JacobiansValue) { - using boost::fusion::at_c; - Matrix23 expected; - Jacobians jacobians; - - at_c<1>(jacobians) << 1, 2, 3, 4, 5, 6; - - Matrix23 actual = at_c<1>(jacobians); - CHECK(actual.cols() == expected.cols()); - CHECK(actual.rows() == expected.rows()); -} - -/* ************************************************************************* */ -// Test out polymorphic transform -#include -#include -#include - -struct triple { - template struct result; // says we will provide result - - template - struct result { - typedef double type; // result for int argument - }; - - template - struct result { - typedef double type; // result for int argument - }; - - template - struct result { - typedef double type; // result for double argument - }; - - template - struct result { - typedef double type; // result for double argument - }; - - // actual function - template - typename result::type operator()(const T& x) const { - return (double) x; - } -}; - -// Test out polymorphic transform -TEST(ExpressionFactor, Triple) { - typedef boost::fusion::vector IntDouble; - IntDouble H = boost::fusion::make_vector(1, 2.0); - - // Only works if I use Double2 - typedef boost::fusion::result_of::transform::type Result; - typedef boost::fusion::vector Double2; - Double2 D = boost::fusion::transform(H, triple()); - - using boost::fusion::at_c; - DOUBLES_EQUAL(1.0, at_c<0>(D), 1e-9); - DOUBLES_EQUAL(2.0, at_c<1>(D), 1e-9); -} - -/* ************************************************************************* */ -#include -#include -#include -#include - -// Test out invoke -TEST(ExpressionFactor, Invoke) { - EXPECT_LONGS_EQUAL(2, invoke(plus(),boost::fusion::make_vector(1,1))); - - // Creating a Pose3 (is there another way?) - boost::fusion::vector pair; - Pose3 pose = boost::fusion::invoke(boost::value_factory(), pair); -} - -/* ************************************************************************* */ -// debug const issue (how to make read/write arguments for invoke) -struct test { - typedef void result_type; - void operator()(int& a, int& b) const { - a = 6; - b = 7; - } -}; - -TEST(ExpressionFactor, ConstIssue) { - int a, b; - boost::fusion::invoke(test(), - boost::fusion::make_vector(boost::ref(a), boost::ref(b))); - LONGS_EQUAL(6, a); - LONGS_EQUAL(7, b); -} - -/* ************************************************************************* */ -// Test out invoke on a given GTSAM function -// then construct prototype for it's derivatives -TEST(ExpressionFactor, InvokeDerivatives) { - // This is the method in Pose3: - // Point3 transform_to(const Point3& p) const; - // Point3 transform_to(const Point3& p, - // boost::optional Dpose, boost::optional Dpoint) const; - - // Let's assign it it to a boost function object - // cast is needed because Pose3::transform_to is overloaded -// typedef boost::function F; -// F f = static_cast(&Pose3::transform_to); -// -// // Create arguments -// Pose3 pose; -// Point3 point; -// typedef boost::fusion::vector Arguments; -// Arguments args = boost::fusion::make_vector(pose, point); -// -// // Create fused function (takes fusion vector) and call it -// boost::fusion::fused g(f); -// Point3 actual = g(args); -// CHECK(assert_equal(point,actual)); -// -// // We can *immediately* do this using invoke -// Point3 actual2 = boost::fusion::invoke(f, args); -// CHECK(assert_equal(point,actual2)); - - // Now, let's create the optional Jacobian arguments - typedef Point3 T; - typedef boost::mpl::vector TYPES; - typedef boost::mpl::transform >::type Optionals; - - // Unfortunately this is moot: we need a pointer to a function with the - // optional derivatives; I don't see a way of calling a function that we - // did not get access to by the caller passing us a pointer. - // Let's test below whether we can have a proxy object -} - -struct proxy { - typedef Point3 result_type; - Point3 operator()(const Pose3& pose, const Point3& point) const { - return pose.transform_to(point); - } - Point3 operator()(const Pose3& pose, const Point3& point, - OptionalJacobian<3,6> Dpose, - OptionalJacobian<3,3> Dpoint) const { - return pose.transform_to(point, Dpose, Dpoint); - } -}; - -TEST(ExpressionFactor, InvokeDerivatives2) { - // without derivatives - Pose3 pose; - Point3 point; - Point3 actual = boost::fusion::invoke(proxy(), - boost::fusion::make_vector(pose, point)); - CHECK(assert_equal(point,actual)); - - // with derivatives, does not work, const issue again - Matrix36 Dpose; - Matrix3 Dpoint; - Point3 actual2 = boost::fusion::invoke(proxy(), - boost::fusion::make_vector(pose, point, boost::ref(Dpose), - boost::ref(Dpoint))); - CHECK(assert_equal(point,actual2)); -} - -/* ************************************************************************* */ -int main() { - TestResult tr; - return TestRegistry::runAllTests(tr); -} -/* ************************************************************************* */ - diff --git a/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp index 832d37d14..0b86a60e9 100644 --- a/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp @@ -17,19 +17,20 @@ */ -#include #include -#include -#include -#include -#include -#include -#include #include #include +#include #include #include +#include +#include +#include +#include #include +#include + +#include using namespace std; using namespace gtsam; diff --git a/gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp b/gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp index 20e96e6bf..99a4f90a4 100644 --- a/gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp +++ b/gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp @@ -73,9 +73,8 @@ TEST( InvDepthFactorVariant1, optimize) { // Optimize the graph to recover the actual landmark position LevenbergMarquardtParams params; Values result = LevenbergMarquardtOptimizer(graph, values, params).optimize(); - Vector6 actual = result.at(landmarkKey); - +// Vector6 actual = result.at(landmarkKey); // values.at(poseKey1).print("Pose1 Before:\n"); // result.at(poseKey1).print("Pose1 After:\n"); // pose1.print("Pose1 Truth:\n"); @@ -116,8 +115,11 @@ TEST( InvDepthFactorVariant1, optimize) { // However, since this is an over-parameterization, there can be // many 6D landmark values that equate to the same 3D world position // Instead, directly test the recovered 3D landmark position - //EXPECT(assert_equal(expected, actual, 1e-9)); EXPECT(assert_equal(landmark, world_landmarkAfter, 1e-9)); + + // Frank asks: why commented out? + //Vector6 actual = result.at(landmarkKey); + //EXPECT(assert_equal(expected, actual, 1e-9)); } diff --git a/wrap/tests/testWrap.cpp b/wrap/tests/testWrap.cpp index 1a9c82143..c03c40444 100644 --- a/wrap/tests/testWrap.cpp +++ b/wrap/tests/testWrap.cpp @@ -41,10 +41,6 @@ static string topdir = "TOPSRCDIR_NOT_CONFIGURED"; // If TOPSRCDIR is not define typedef vector strvec; -// NOTE: as this path is only used to generate makefiles, it is hardcoded here for testing -// In practice, this path will be an absolute system path, which makes testing it more annoying -static const std::string headerPath = "/not_really_a_real_path/borg/gtsam/wrap"; - /* ************************************************************************* */ TEST( wrap, ArgumentList ) { ArgumentList args; diff --git a/wrap/utilities.cpp b/wrap/utilities.cpp index 51dc6f4c3..57a5bce29 100644 --- a/wrap/utilities.cpp +++ b/wrap/utilities.cpp @@ -91,7 +91,8 @@ bool files_equal(const string& expected, const string& actual, bool skipheader) cerr << "<<< DIFF OUTPUT (if none, white-space differences only):\n"; stringstream command; command << "diff --ignore-all-space " << expected << " " << actual << endl; - system(command.str().c_str()); + if (system(command.str().c_str())<0) + throw "command '" + command.str() + "' failed"; cerr << ">>>\n"; } return equal;