From 96e63fb48008ffe181b2cc0613cef783c35898b9 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 21 Feb 2015 14:32:36 +0100 Subject: [PATCH 1/5] Added test for RegularHessianFactor creation --- gtsam/slam/RegularHessianFactor.h | 7 +++ .../tests/testSmartProjectionPoseFactor.cpp | 61 +++++++++++++++++++ 2 files changed, 68 insertions(+) diff --git a/gtsam/slam/RegularHessianFactor.h b/gtsam/slam/RegularHessianFactor.h index 8c3df87cf..a738cc256 100644 --- a/gtsam/slam/RegularHessianFactor.h +++ b/gtsam/slam/RegularHessianFactor.h @@ -172,6 +172,13 @@ public: } } + /* ************************************************************************* */ + +}; // end class RegularHessianFactor + +// traits +template struct traits > : public Testable< + RegularHessianFactor > { }; } diff --git a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp index c2855f09b..2fa3f20f4 100644 --- a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp @@ -380,6 +380,67 @@ TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ){ EXPECT(assert_equal(bodyPose3,result.at(x3))); } +/* *************************************************************************/ +TEST( SmartProjectionPoseFactor, FactorsWithSensorTransform ){ + + // Default cameras for simple derivatives + SimpleCamera cam1, cam2; + + // create arbitrary body_Pose_sensor (transforms from sensor to body) + Pose3 sensor_to_body = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(1, 1, 1)); // Pose3(); // + + // one landmarks ~5 meters infront of camera + Point3 landmark1(5, 0.5, 1.2); + + vector measurements_cam1; + + // Project 2 landmarks into 2 cameras + measurements_cam1.push_back(cam1.project(landmark1)); + measurements_cam1.push_back(cam2.project(landmark1)); + + // Create smart factors + std::vector views; + views.push_back(x1); + views.push_back(x2); + + double rankTol = 1; + double linThreshold = -1; + bool manageDegeneracy = false; + bool enableEPI = false; + + SmartFactor::shared_ptr smartFactor1(new SmartFactor(rankTol,linThreshold,manageDegeneracy,enableEPI,sensor_to_body)); + smartFactor1->add(measurements_cam1, views, model, K); + + SmartFactor::Cameras cameras; + cameras.push_back(cam1); + cameras.push_back(cam2); + + { + // RegularHessianFactor<6> + Matrix66 G11; G11.setZero(); + Matrix66 G12; G12.setZero(); + Matrix66 G22; G22.setZero(); + + Vector6 g1; g1.setZero(); + Vector6 g2; g2.setZero(); + + double f = 10; + + std::vector js; + js.push_back(x1); js.push_back(x2); + std::vector Gs; + Gs.push_back(G11); Gs.push_back(G12); Gs.push_back(G22); + std::vector gs; + gs.push_back(g1); gs.push_back(g2); + RegularHessianFactor<6> expected(js, Gs, gs, f); + + boost::shared_ptr > actual = + smartFactor1->createHessianFactor(cameras, 0.0); + CHECK(assert_equal(expected,*actual)); + } + +} + /* *************************************************************************/ TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ){ From 5120e4b77aa3ee0b04d21fe491df8afccfabeef4 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 21 Feb 2015 16:39:31 +0100 Subject: [PATCH 2/5] Constructor --- gtsam/slam/RegularHessianFactor.h | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/gtsam/slam/RegularHessianFactor.h b/gtsam/slam/RegularHessianFactor.h index a738cc256..6401eda90 100644 --- a/gtsam/slam/RegularHessianFactor.h +++ b/gtsam/slam/RegularHessianFactor.h @@ -43,6 +43,15 @@ public: HessianFactor(js, Gs, gs, f) { } + /** Construct a binary factor. Gxx are the upper-triangle blocks of the + * quadratic term (the Hessian matrix), gx the pieces of the linear vector + * term, and f the constant term. + */ + RegularHessianFactor(Key j1, Key j2, const Matrix& G11, const Matrix& G12, + const Vector& g1, const Matrix& G22, const Vector& g2, double f) : + HessianFactor(j1, j2, G11, G12, g1, G22, g2, f) { + } + /** Constructor with an arbitrary number of keys and with the augmented information matrix * specified as a block matrix. */ template From c3c5c0d80c802d1aba45f6c353470a1587d3e28d Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 21 Feb 2015 16:39:39 +0100 Subject: [PATCH 3/5] Equals --- gtsam/slam/RegularImplicitSchurFactor.h | 19 +++++++++++++++---- 1 file changed, 15 insertions(+), 4 deletions(-) diff --git a/gtsam/slam/RegularImplicitSchurFactor.h b/gtsam/slam/RegularImplicitSchurFactor.h index 75b2d44ba..95a61dcfc 100644 --- a/gtsam/slam/RegularImplicitSchurFactor.h +++ b/gtsam/slam/RegularImplicitSchurFactor.h @@ -96,11 +96,17 @@ public: /// equals bool equals(const GaussianFactor& lf, double tol) const { - if (!dynamic_cast(&lf)) - return false; - else { + const This* f = dynamic_cast(&lf); + if (!f) return false; + for (size_t pos = 0; pos < size(); ++pos) { + if (keys_[pos] != f->keys_[pos]) return false; + if (Fblocks_[pos].first != f->Fblocks_[pos].first) return false; + if (!equal_with_abs_tol(Fblocks_[pos].second,f->Fblocks_[pos].second,tol)) return false; } + return equal_with_abs_tol(PointCovariance_, f->PointCovariance_, tol) + && equal_with_abs_tol(E_, f->E_, tol) + && equal_with_abs_tol(b_, f->b_, tol); } /// Degrees of freedom of camera @@ -460,7 +466,12 @@ public: }; -// RegularImplicitSchurFactor +// end class RegularImplicitSchurFactor + +// traits +template struct traits > : public Testable< + RegularImplicitSchurFactor > { +}; } From 00d7e707a5b26a2cdd7d0a1a5b393d205c37dbf0 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 21 Feb 2015 17:09:31 +0100 Subject: [PATCH 4/5] printing --- gtsam/slam/RegularImplicitSchurFactor.h | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/gtsam/slam/RegularImplicitSchurFactor.h b/gtsam/slam/RegularImplicitSchurFactor.h index 95a61dcfc..bde928723 100644 --- a/gtsam/slam/RegularImplicitSchurFactor.h +++ b/gtsam/slam/RegularImplicitSchurFactor.h @@ -89,9 +89,12 @@ public: const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { std::cout << " RegularImplicitSchurFactor " << std::endl; Factor::print(s); - std::cout << " PointCovariance_ \n" << PointCovariance_ << std::endl; - std::cout << " E_ \n" << E_ << std::endl; - std::cout << " b_ \n" << b_.transpose() << std::endl; + for (size_t pos = 0; pos < size(); ++pos) { + std::cout << "Fblock:\n" << Fblocks_[pos].second << std::endl; + } + std::cout << "PointCovariance:\n" << PointCovariance_ << std::endl; + std::cout << "E:\n" << E_ << std::endl; + std::cout << "b:\n" << b_.transpose() << std::endl; } /// equals From 3ccd02e8b108e80368bd2f8b0babdefc3756f2ef Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 21 Feb 2015 17:09:43 +0100 Subject: [PATCH 5/5] Two factors unit tested --- .../tests/testSmartProjectionPoseFactor.cpp | 65 ++++++++++++------- 1 file changed, 41 insertions(+), 24 deletions(-) diff --git a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp index 2fa3f20f4..38722d88b 100644 --- a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp @@ -381,20 +381,21 @@ TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ){ } /* *************************************************************************/ -TEST( SmartProjectionPoseFactor, FactorsWithSensorTransform ){ +TEST( SmartProjectionPoseFactor, Factors ){ // Default cameras for simple derivatives - SimpleCamera cam1, cam2; + Rot3 R; + static Cal3_S2::shared_ptr K(new Cal3_S2(100, 100, 0, 0, 0)); + SimpleCamera cam1(Pose3(R,Point3(0,0,0)),*K), cam2(Pose3(R,Point3(1,0,0)),*K); - // create arbitrary body_Pose_sensor (transforms from sensor to body) - Pose3 sensor_to_body = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(1, 1, 1)); // Pose3(); // - - // one landmarks ~5 meters infront of camera - Point3 landmark1(5, 0.5, 1.2); + // one landmarks 1m in front of camera + Point3 landmark1(0,0,10); vector measurements_cam1; // Project 2 landmarks into 2 cameras + cout << cam1.project(landmark1) << endl; + cout << cam2.project(landmark1) << endl; measurements_cam1.push_back(cam1.project(landmark1)); measurements_cam1.push_back(cam2.project(landmark1)); @@ -403,39 +404,55 @@ TEST( SmartProjectionPoseFactor, FactorsWithSensorTransform ){ views.push_back(x1); views.push_back(x2); - double rankTol = 1; - double linThreshold = -1; - bool manageDegeneracy = false; - bool enableEPI = false; - - SmartFactor::shared_ptr smartFactor1(new SmartFactor(rankTol,linThreshold,manageDegeneracy,enableEPI,sensor_to_body)); + SmartFactor::shared_ptr smartFactor1 = boost::make_shared(); smartFactor1->add(measurements_cam1, views, model, K); SmartFactor::Cameras cameras; cameras.push_back(cam1); cameras.push_back(cam2); + // Make sure triangulation works + LONGS_EQUAL(2,smartFactor1->triangulateSafe(cameras)); + CHECK(!smartFactor1->isDegenerate()); + CHECK(!smartFactor1->isPointBehindCamera()); + boost::optional p = smartFactor1->point(); + CHECK(p); + EXPECT(assert_equal(landmark1,*p)); + { // RegularHessianFactor<6> - Matrix66 G11; G11.setZero(); - Matrix66 G12; G12.setZero(); - Matrix66 G22; G22.setZero(); + Matrix66 G11; G11.setZero(); G11(0,0) = 100; G11(0,4) = -10; G11(4,0) = -10; G11(4,4) = 1; + Matrix66 G12; G12 = -G11; G12(0,2) = -10; G12(4,2) = 1; + Matrix66 G22; G22 = G11; G22(0,2) = 10; G22(2,0) = 10; G22(2,2) = 1; G22(2,4) = -1; G22(4,2) = -1; Vector6 g1; g1.setZero(); Vector6 g2; g2.setZero(); - double f = 10; + double f = 0; - std::vector js; - js.push_back(x1); js.push_back(x2); - std::vector Gs; - Gs.push_back(G11); Gs.push_back(G12); Gs.push_back(G22); - std::vector gs; - gs.push_back(g1); gs.push_back(g2); - RegularHessianFactor<6> expected(js, Gs, gs, f); + RegularHessianFactor<6> expected(x1, x2, 50 * G11, 50 * G12, g1, 50 * G22, g2, f); boost::shared_ptr > actual = smartFactor1->createHessianFactor(cameras, 0.0); + CHECK(assert_equal(expected.information(),actual->information(),1e-8)); + CHECK(assert_equal(expected,*actual,1e-8)); + } + + { + // RegularImplicitSchurFactor<6> + Matrix26 F1; F1.setZero(); F1(0,1)=-100; F1(0,3)=-10; F1(1,0)=100; F1(1,4)=-10; + Matrix26 F2; F2.setZero(); F2(0,1)=-101; F2(0,3)=-10; F2(0,5)=-1; F2(1,0)=100; F2(1,2)=10; F2(1,4)=-10; + Matrix43 E; E.setZero(); E(0,0)=10; E(1,1)=10; E(2,0)=10; E(2,2)=1;E(3,1)=10; + const vector > Fblocks = list_of > // + (make_pair(x1, F1))(make_pair(x2, F2)); + Matrix3 P = (E.transpose() * E).inverse(); + Vector4 b; b.setZero(); + + RegularImplicitSchurFactor<6> expected(Fblocks, E, P, b); + + boost::shared_ptr > actual = + smartFactor1->createRegularImplicitSchurFactor(cameras, 0.0); + CHECK(actual); CHECK(assert_equal(expected,*actual)); }