diff --git a/.cproject b/.cproject index bba3082a5..59c321651 100644 --- a/.cproject +++ b/.cproject @@ -15,35 +15,35 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -300,7 +300,7 @@ make - +-j2 install true true @@ -308,7 +308,7 @@ make - +-j2 check true true @@ -324,7 +324,7 @@ make - +-j2 testSimpleCamera.run true true @@ -347,7 +347,7 @@ make - +-j2 testCalibratedCamera.run true true @@ -362,7 +362,7 @@ make - +-j2 testPose2.run true true @@ -370,7 +370,7 @@ make - +-j2 testRot3.run true true @@ -385,7 +385,7 @@ make - +-j2 testGaussianFactor.run true true @@ -393,7 +393,7 @@ make - +-j2 testGaussianFactorGraph.run true true @@ -408,7 +408,7 @@ make - +-j2 testPose3.run true true @@ -430,7 +430,7 @@ make - +-j2 testNonlinearFactor.run true true @@ -438,7 +438,7 @@ make - +-j2 timeGaussianFactor.run true true @@ -446,7 +446,7 @@ make - +-j2 timeGaussianFactorGraph.run true true @@ -454,7 +454,7 @@ make - +-j2 testGaussianBayesNet.run true true @@ -484,7 +484,7 @@ make - +-j2 testVector.run true true @@ -492,7 +492,7 @@ make - +-j2 testMatrix.run true true @@ -514,7 +514,7 @@ make - +-j2 testSQP.run true true @@ -529,7 +529,7 @@ make - +-j2 testSQPOptimizer.run true true @@ -537,7 +537,7 @@ make - +-j2 testVSLAMConfig.run true true @@ -545,7 +545,7 @@ make - +-j2 testControlConfig.run true true @@ -553,7 +553,7 @@ make - +-j2 testControlPoint.run true true @@ -568,7 +568,7 @@ make - +-j2 testOrdering.run true true @@ -583,7 +583,7 @@ make - +-j2 testRot2.run true true @@ -591,7 +591,7 @@ make - +-j2 testGaussianBayesTree.run true true @@ -627,7 +627,7 @@ make - +-j2 testUrbanOdometry.run true true @@ -649,7 +649,7 @@ make - +-j2 testSubgraphPreconditioner.run true true @@ -657,7 +657,7 @@ make - +-j2 testBayesNetPreconditioner.run true true @@ -684,10 +684,10 @@ true true - + make --j2 -testLieConfig.run + +testPose2Graph.run true true true @@ -718,6 +718,6 @@ - - + + diff --git a/cpp/BetweenFactor.h b/cpp/BetweenFactor.h index 54e2f3d21..4e1f7ed83 100644 --- a/cpp/BetweenFactor.h +++ b/cpp/BetweenFactor.h @@ -63,10 +63,12 @@ namespace gtsam { /** implement functions needed to derive from Factor */ /** vector of errors */ - Vector error_vector(const Config& config) const { + Vector error_vector(const Config& x) const { //z-h - T p1 = config.get(key1_), p2 = config.get(key2_); - return -logmap(between(p1, p2), measured_); + T p1 = x.get(key1_), p2 = x.get(key2_); + T hx = between(p1,p2); + // manifold equivalent of z-h(x) -> log(h(x),z) + return square_root_inverse_covariance_ * logmap(hx,measured_); } /** keys as a list */ @@ -76,14 +78,14 @@ namespace gtsam { inline std::size_t size() const { return 2;} /** linearize */ - boost::shared_ptr linearize(const Config& config) const { - T p1 = config.get(key1_), p2 = config.get(key2_); - Vector b = -logmap(between(p1, p2), measured_); - Matrix H1 = Dbetween1(p1, p2); - Matrix H2 = Dbetween2(p1, p2); - boost::shared_ptr linearized(new GaussianFactor(key1_, - square_root_inverse_covariance_ * H1, key2_, - square_root_inverse_covariance_ * H2, b, 1.0)); + boost::shared_ptr linearize(const Config& x0) const { + T p1 = x0.get(key1_), p2 = x0.get(key2_); + Matrix A1 = Dbetween1(p1, p2); + Matrix A2 = Dbetween2(p1, p2); + Vector b = error_vector(x0); // already has sigmas in ! + boost::shared_ptr linearized(new GaussianFactor( + key1_, square_root_inverse_covariance_ * A1, + key2_, square_root_inverse_covariance_ * A2, b, 1.0)); return linearized; } }; diff --git a/cpp/Pose3Factor.h b/cpp/Pose3Factor.h index 23cdcc6d2..72aacd2a8 100644 --- a/cpp/Pose3Factor.h +++ b/cpp/Pose3Factor.h @@ -11,6 +11,9 @@ namespace gtsam { + /** + * A config specifically for 3D poses + */ class Pose3Config: public std::map { public: @@ -23,7 +26,10 @@ namespace gtsam { }; - /** This is just a typedef now */ + /** + * A Factor for 3D pose measurements + * This is just a typedef now + */ typedef BetweenFactor Pose3Factor; } /// namespace gtsam diff --git a/cpp/testPose2Factor.cpp b/cpp/testPose2Factor.cpp index 2f9a93616..4f0fab95c 100644 --- a/cpp/testPose2Factor.cpp +++ b/cpp/testPose2Factor.cpp @@ -1,10 +1,9 @@ /** - * @file testPose2Constraint.cpp + * @file testPose2Factor.cpp * @brief Unit tests for Pose2Factor Class * @authors Frank Dellaert, Viorela Ila **/ -/*STL/C++*/ #include #include #include @@ -12,6 +11,8 @@ using namespace boost::assign; #include +#include "Vector.h" +#include "numericalDerivative.h" #include "NonlinearOptimizer-inl.h" #include "NonlinearEquality.h" #include "Pose2Factor.h" @@ -20,51 +21,124 @@ using namespace boost::assign; using namespace std; using namespace gtsam; +// Common measurement covariance +static double sx=0.5, sy=0.5,st=0.1; +static Matrix covariance = Matrix_(3,3, + sx*sx, 0.0, 0.0, + 0.0, sy*sy, 0.0, + 0.0, 0.0, st*st + ); + +/* ************************************************************************* */ +// Very simple test establishing Ax-b \approx h(x)-z +TEST( Pose2Factor, error ) +{ + // Choose a linearization point + Pose2 p1; // robot at origin + Pose2 p2(1, 0, 0); // robot at (1,0) + Pose2Config x0; + x0.insert("p1", p1); + x0.insert("p2", p2); + + // Create factor + Pose2 z = between(p1,p2); + Pose2Factor factor("p1", "p2", z, covariance); + + // Actual linearization + boost::shared_ptr linear = factor.linearize(x0); + + // Check error at x0 = zero ! + VectorConfig delta; + delta.insert("p1", zero(3)); + delta.insert("p2", zero(3)); + Vector error_at_zero = Vector_(3,0.0,0.0,0.0); + CHECK(assert_equal(error_at_zero,factor.error_vector(x0))); + CHECK(assert_equal(-error_at_zero,linear->error_vector(delta))); + + // Check error after increasing p2 + VectorConfig plus = delta + VectorConfig("p2", Vector_(3, 0.1, 0.0, 0.0)); + Pose2Config x1 = expmap(x0, plus); + Vector error_at_plus = Vector_(3,-0.1/sx,0.0,0.0); + CHECK(assert_equal(error_at_plus,factor.error_vector(x1))); + CHECK(assert_equal(-error_at_plus,linear->error_vector(plus))); +} + +/* ************************************************************************* */ +// common Pose2Factor for tests below +static Pose2 measured(2,2,M_PI_2); +static Pose2Factor factor("p1","p2",measured, covariance); + +/* ************************************************************************* */ +TEST( Pose2Factor, rhs ) +{ + // Choose a linearization point + Pose2 p1(1.1,2,M_PI_2); // robot at (1.1,2) looking towards y (ground truth is at 1,2, see testPose2) + Pose2 p2(-1,4.1,M_PI); // robot at (-1,4.1) looking at negative (ground truth is at -1,4) + Pose2Config x0; + x0.insert("p1",p1); + x0.insert("p2",p2); + + // Actual linearization + boost::shared_ptr linear = factor.linearize(x0); + + // Check RHS + Pose2 hx0 = between(p1,p2); + CHECK(assert_equal(Pose2(2.1, 2.1, M_PI_2),hx0)); + Vector expected_b = Vector_(3, -0.1/sx, 0.1/sy, 0.0); + CHECK(assert_equal(expected_b,factor.error_vector(x0))); + CHECK(assert_equal(expected_b,linear->get_b())); +} + +/* ************************************************************************* */ +// The error |A*dx-b| approximates (h(x0+dx)-z) = -error_vector +// Hence i.e., b = approximates z-h(x0) = error_vector(x0) +Vector h(const Pose2& p1,const Pose2& p2) { + Pose2Config x; + x.insert("p1",p1); + x.insert("p2",p2); + return -factor.error_vector(x); +} + /* ************************************************************************* */ TEST( Pose2Factor, linearize ) { - // create a factor between unknown poses p1 and p2 - Pose2 measured(2,2,M_PI_2); - Matrix measurement_covariance = Matrix_(3,3, - 0.25, 0.0, 0.0, - 0.0, 0.25, 0.0, - 0.0, 0.0, 0.01 - ); - Pose2Factor constraint("p1","p2",measured, measurement_covariance); - - // Choose a linearization point - Pose2 p1(1.1,2,M_PI_2); // robot at (1.1,2) looking towards y (ground truth is at 1,2, see testPose2) - Pose2 p2(-1,4.1,M_PI); // robot at (-1,4.1) looking at negative (ground truth is at 4.1,2) - Pose2Config config; - config.insert("p1",p1); - config.insert("p2",p2); + // Choose a linearization point at ground truth + Pose2 p1(1,2,M_PI_2); + Pose2 p2(-1,4,M_PI); + Pose2Config x0; + x0.insert("p1",p1); + x0.insert("p2",p2); // expected linearization - // we need the minus signs below as "inverse_square_root" - // uses SVD and the sign is simply arbitrary (but still a square root!) Matrix square_root_inverse_covariance = Matrix_(3,3, - -2.0, 0.0, 0.0, - 0.0, -2.0, 0.0, - 0.0, 0.0, -10.0 + 2.0, 0.0, 0.0, + 0.0, 2.0, 0.0, + 0.0, 0.0, 10.0 ); - Matrix expectedH1 = Matrix_(3,3, - 0.0,-1.0,-2.1, - 1.0, 0.0,-2.1, + Matrix expectedH1 = square_root_inverse_covariance*Matrix_(3,3, + 0.0,-1.0,-2.0, + 1.0, 0.0,-2.0, 0.0, 0.0,-1.0 ); - Matrix expectedH2 = Matrix_(3,3, + Matrix expectedH2 = square_root_inverse_covariance*Matrix_(3,3, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0 ); - GaussianFactor expected( - "p1", square_root_inverse_covariance*expectedH1, - "p2", square_root_inverse_covariance*expectedH2, - Vector_(3, 0.1, -0.1, 0.0), 1.0); + Vector expected_b = Vector_(3, 0.0, 0.0, 0.0); + + // expected linear factor + GaussianFactor expected("p1", expectedH1, "p2", expectedH2, expected_b, 1.0); // Actual linearization - boost::shared_ptr actual = constraint.linearize(config); + boost::shared_ptr actual = factor.linearize(x0); CHECK(assert_equal(expected,*actual)); + + // Numerical do not work out because BetweenFactor is approximate ? + Matrix numericalH1 = numericalDerivative21(h, p1, p2, 1e-5); + CHECK(assert_equal(expectedH1,numericalH1)); + Matrix numericalH2 = numericalDerivative22(h, p1, p2, 1e-5); + CHECK(assert_equal(expectedH2,numericalH2)); } /* ************************************************************************* */ @@ -84,10 +158,7 @@ TEST(Pose2Factor, optimize) { fg.push_back(Pose2Graph::sharedFactor( new NonlinearEquality("p0", feasible, dim(Pose2()), poseCompare))); fg.push_back(Pose2Graph::sharedFactor( - new Pose2Factor("p0", "p1", Pose2(1,2,M_PI_2), Matrix_(3,3, - 0.5, 0.0, 0.0, - 0.0, 0.5, 0.0, - 0.0, 0.0, 0.5)))); + new Pose2Factor("p0", "p1", Pose2(1,2,M_PI_2), covariance))); // Create initial config boost::shared_ptr initial = @@ -98,8 +169,11 @@ TEST(Pose2Factor, optimize) { // Choose an ordering and optimize Ordering ordering; ordering += "p0","p1"; - NonlinearOptimizer optimizer(fg, ordering, initial); - optimizer = optimizer.levenbergMarquardt(1e-15, 1e-15); + typedef NonlinearOptimizer Optimizer; + Optimizer optimizer(fg, ordering, initial); + Optimizer::verbosityLevel verbosity = Optimizer::SILENT; + //Optimizer::verbosityLevel verbosity = Optimizer::ERROR; + optimizer = optimizer.levenbergMarquardt(1e-15, 1e-15, verbosity); // Check with expected config Pose2Config expected; diff --git a/cpp/testPose2Graph.cpp b/cpp/testPose2Graph.cpp index 44b94b8d6..ccb1ff2a9 100644 --- a/cpp/testPose2Graph.cpp +++ b/cpp/testPose2Graph.cpp @@ -1,5 +1,8 @@ +/** + * @file testPose2Graph.cpp + * @authors Frank Dellaert, Viorela Ila + **/ -/*STL/C++*/ #include #include @@ -10,19 +13,22 @@ using namespace std; using namespace gtsam; +// common measurement covariance +static double sx=0.5, sy=0.5,st=0.1; +static Matrix covariance = Matrix_(3,3, + sx*sx, 0.0, 0.0, + 0.0, sy*sy, 0.0, + 0.0, 0.0, st*st + ); +/* ************************************************************************* */ TEST( Pose2Graph, constructor ) { // create a factor between unknown poses p1 and p2 Pose2 measured(2,2,M_PI_2); - Matrix measurement_covariance = Matrix_(3,3, - 0.25, 0.0, 0.0, - 0.0, 0.25, 0.0, - 0.0, 0.0, 0.01 - ); - Pose2Factor constraint("x1","x2",measured, measurement_covariance); + Pose2Factor constraint("x1","x2",measured, covariance); Pose2Graph graph; - graph.push_back(Pose2Factor::shared_ptr(new Pose2Factor("x1","x2",measured, measurement_covariance))); + graph.push_back(Pose2Factor::shared_ptr(new Pose2Factor("x1","x2",measured, covariance))); // get the size of the graph size_t actual = graph.size(); // verify @@ -30,18 +36,15 @@ TEST( Pose2Graph, constructor ) CHECK(actual == expected); } + +/* ************************************************************************* */ TEST( Pose2Graph, linerization ) { // create a factor between unknown poses p1 and p2 Pose2 measured(2,2,M_PI_2); - Matrix measurement_covariance = Matrix_(3,3, - 0.25, 0.0, 0.0, - 0.0, 0.25, 0.0, - 0.0, 0.0, 0.01 - ); - Pose2Factor constraint("x1","x2",measured, measurement_covariance); + Pose2Factor constraint("x1","x2",measured, covariance); Pose2Graph graph; - graph.push_back(Pose2Factor::shared_ptr(new Pose2Factor("x1","x2",measured, measurement_covariance))); + graph.push_back(Pose2Factor::shared_ptr(new Pose2Factor("x1","x2",measured, covariance))); // Choose a linearization point Pose2 p1(1.1,2,M_PI_2); // robot at (1.1,2) looking towards y (ground truth is at 1,2, see testPose2) @@ -56,24 +59,22 @@ TEST( Pose2Graph, linerization ) // the expected linear factor GaussianFactorGraph lfg_expected; Matrix A1 = Matrix_(3,3, - 0.0, 2.0, 4.2, - -2.0, 0.0, 4.2, - 0.0, 0.0, 10.0); + 0.0,-2.0, -4.2, + 2.0, 0.0, -4.2, + 0.0, 0.0,-10.0); Matrix A2 = Matrix_(3,3, - -2.0, 0.0, 0.0, - 0.0,-2.0, 0.0, - 0.0, 0.0, -10.0); - + 2.0, 0.0, 0.0, + 0.0, 2.0, 0.0, + 0.0, 0.0, 10.0); double sigma = 1; - Vector b = Vector_(3,0.1,-0.1,0.0); + Vector b = Vector_(3,-0.1/sx,0.1/sy,0.0); lfg_expected.add("x1", A1, "x2", A2, b, sigma); - CHECK(assert_equal(lfg_expected, lfg_linearized)); - } + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/cpp/testPose3Factor.cpp b/cpp/testPose3Factor.cpp index 1d2a3b643..26a384432 100644 --- a/cpp/testPose3Factor.cpp +++ b/cpp/testPose3Factor.cpp @@ -5,6 +5,8 @@ **/ #include +#include +using namespace boost::assign; #include #include "Pose3Factor.h" @@ -12,11 +14,26 @@ using namespace std; using namespace gtsam; /* ************************************************************************* */ -TEST( Pose3Factor, constructor ) +TEST( Pose3Factor, error ) { - Pose3 measured; - Matrix measurement_covariance; - Pose3Factor("x1", "x2", measured, measurement_covariance); + // Create example + Pose3 t1; // origin + Pose3 t2(rodriguez(0.1,0.2,0.3),Point3(0,1,0)); + Pose3 z(rodriguez(0.2,0.2,0.3),Point3(0,1.1,0));; + + // Create factor + Matrix measurement_covariance = eye(6); + Pose3Factor factor("t1", "t2", z, measurement_covariance); + + // Create config + Pose3Config x; + x.insert(make_pair("t1",t1)); + x.insert(make_pair("t2",t2)); + + // Get error z-h(x) -> logmap(h(x),z) = logmap(between(t1,t2),z) + Vector actual = factor.error_vector(x); + Vector expected = logmap(between(t1,t2),z); + CHECK(assert_equal(expected,actual)); } /* ************************************************************************* */