From ce7cf524ea7f0ad5be990d4ed8710756363c8acf Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 23 Dec 2013 19:36:24 -0500 Subject: [PATCH 1/8] Initial unit test --- .cproject | 356 +++++++++--------- gtsam/slam/EssentialMatrixFactor.h | 6 + .../slam/tests/testEssentialMatrixFactor.cpp | 89 ++++- 3 files changed, 266 insertions(+), 185 deletions(-) diff --git a/.cproject b/.cproject index 790f70daa..21ab6965c 100644 --- a/.cproject +++ b/.cproject @@ -365,6 +365,38 @@ true true + + make + -j2 + all + true + true + true + + + make + -j2 + testNonlinearConstraint.run + true + true + true + + + make + -j2 + testLieConfig.run + true + true + true + + + make + -j2 + testConstraintOptimizer.run + true + true + true + make -j5 @@ -445,38 +477,6 @@ true true - - make - -j2 - all - true - true - true - - - make - -j2 - testNonlinearConstraint.run - true - true - true - - - make - -j2 - testLieConfig.run - true - true - true - - - make - -j2 - testConstraintOptimizer.run - true - true - true - make -j2 @@ -551,7 +551,6 @@ make - tests/testBayesTree.run true false @@ -559,7 +558,6 @@ make - testBinaryBayesNet.run true false @@ -607,7 +605,6 @@ make - testSymbolicBayesNet.run true false @@ -615,7 +612,6 @@ make - tests/testSymbolicFactor.run true false @@ -623,7 +619,6 @@ make - testSymbolicFactorGraph.run true false @@ -639,12 +634,19 @@ make - tests/testBayesTree true false true + + make + -j2 + testVSLAMGraph + true + true + true + make -j2 @@ -727,6 +729,7 @@ make + testSimulated2DOriented.run true false @@ -766,6 +769,7 @@ make + testSimulated2D.run true false @@ -773,6 +777,7 @@ make + testSimulated3D.run true false @@ -786,14 +791,6 @@ true true - - make - -j2 - testVSLAMGraph - true - true - true - make -j2 @@ -833,21 +830,6 @@ false true - - make - -j2 - check - true - true - true - - - make - tests/testGaussianISAM2 - true - false - true - make -j5 @@ -986,7 +968,6 @@ make - testGraph.run true false @@ -994,7 +975,6 @@ make - testJunctionTree.run true false @@ -1002,7 +982,6 @@ make - testSymbolicBayesNetB.run true false @@ -1064,6 +1043,22 @@ true true + + make + -j2 + check + true + true + true + + + make + + tests/testGaussianISAM2 + true + false + true + make -j2 @@ -1424,6 +1419,22 @@ true true + + make + -j5 + testEssentialMatrixFactor.run + true + true + true + + + make + -j2 + testGaussianFactor.run + true + true + true + make -j2 @@ -1504,10 +1515,66 @@ true true - + make -j2 - testGaussianFactor.run + check + true + true + true + + + make + -j2 + testClusterTree.run + true + true + true + + + make + -j2 + testJunctionTree.run + true + true + true + + + make + -j2 + tests/testEliminationTree.run + true + true + true + + + make + -j2 + tests/testSymbolicFactor.run + true + true + true + + + make + -j2 + tests/testVariableSlots.run + true + true + true + + + make + -j2 + tests/testConditional.run + true + true + true + + + make + -j2 + tests/testSymbolicFactorGraph.run true true true @@ -1608,86 +1675,6 @@ true true - - make - -j2 - check - true - true - true - - - make - -j2 - testClusterTree.run - true - true - true - - - make - -j2 - testJunctionTree.run - true - true - true - - - make - -j2 - tests/testEliminationTree.run - true - true - true - - - make - -j2 - tests/testSymbolicFactor.run - true - true - true - - - make - -j2 - tests/testVariableSlots.run - true - true - true - - - make - -j2 - tests/testConditional.run - true - true - true - - - make - -j2 - tests/testSymbolicFactorGraph.run - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - make -j5 @@ -1752,6 +1739,22 @@ true true + + make + -j2 + all + true + true + true + + + make + -j2 + clean + true + true + true + make -j2 @@ -2185,7 +2188,6 @@ cpack - -G DEB true false @@ -2193,7 +2195,6 @@ cpack - -G RPM true false @@ -2201,7 +2202,6 @@ cpack - -G TGZ true false @@ -2209,7 +2209,6 @@ cpack - --config CPackSourceConfig.cmake true false @@ -2383,30 +2382,6 @@ true true - - make - -j2 - check - true - true - true - - - make - -j2 - tests/testSPQRUtil.run - true - true - true - - - make - -j2 - clean - true - true - true - make -j5 @@ -2447,18 +2422,26 @@ true true - + make -j2 - tests/testPose2.run + check true true true - + make -j2 - tests/testPose3.run + tests/testSPQRUtil.run + true + true + true + + + make + -j2 + clean true true true @@ -2553,12 +2536,27 @@ make - testErrors.run true false true + + make + -j2 + tests/testPose2.run + true + true + true + + + make + -j2 + tests/testPose3.run + true + true + true + diff --git a/gtsam/slam/EssentialMatrixFactor.h b/gtsam/slam/EssentialMatrixFactor.h index d3f560b32..7ac992d79 100644 --- a/gtsam/slam/EssentialMatrixFactor.h +++ b/gtsam/slam/EssentialMatrixFactor.h @@ -22,6 +22,7 @@ class EssentialMatrixFactor: public NoiseModelFactor1 { Vector vA_, vB_; ///< Homogeneous versions typedef NoiseModelFactor1 Base; + typedef EssentialMatrixFactor This; public: @@ -33,6 +34,11 @@ public: vB_(EssentialMatrix::Homogeneous(pB)) { } + /// @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))); } + /// print virtual void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp index 34a26adbe..e7cbb0ada 100644 --- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp +++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp @@ -6,6 +6,53 @@ */ #include +#include + +namespace gtsam { + +/** + * Binary factor that optimizes for E and inverse depth: assumes measurement + * in image 1 is perfect, and returns re-projection error in image 2 + */ +class EssentialMatrixFactor2: public NoiseModelFactor2 { + + Point2 pA_, pB_; ///< Measurements in image A and B + + typedef NoiseModelFactor2 Base; + +public: + + /// Constructor + EssentialMatrixFactor2(Key key1, Key key2, const Point2& pA, const Point2& pB, + const SharedNoiseModel& model) : + Base(model, key1, key2), pA_(pA), pB_(pB) { + } + + /// print + virtual void print(const std::string& s = "", + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + Base::print(s); + std::cout << " EssentialMatrixFactor2 with measurements\n (" + << pA_.vector().transpose() << ")' and (" << pB_.vector().transpose() + << ")'" << std::endl; + } + + /// vector of errors returns 1D vector + Vector evaluateError(const EssentialMatrix& E, const LieScalar& d, + boost::optional H1 = boost::none, boost::optional H2 = + boost::none) const { + if (H1) + *H1 = zeros(2, 5); + if (H2) + *H2 = zeros(2, 1); + return (Vector(2) << 0,0); + } + +}; + +} // gtsam + #include #include #include @@ -22,7 +69,7 @@ const string filename = findExampleDataFile("5pointExample1.txt"); SfM_data data; bool readOK = readBAL(filename, data); Rot3 aRb = data.cameras[1].pose().rotation(); -Point3 aTb = data.cameras[1].pose().translation(); +Point3 aTb = data.cameras[1].pose().translation(); Point2 pA(size_t i) { return data.tracks[i].measurements[0].second; @@ -77,18 +124,18 @@ TEST (EssentialMatrixFactor, factor) { // Check evaluation Vector expected(1); expected << 0; - Matrix HActual; - Vector actual = factor.evaluateError(trueE, HActual); + Matrix Hactual; + Vector actual = factor.evaluateError(trueE, Hactual); EXPECT(assert_equal(expected, actual, 1e-7)); // Use numerical derivatives to calculate the expected Jacobian - Matrix HExpected; - HExpected = numericalDerivative11( + Matrix Hexpected; + Hexpected = numericalDerivative11( boost::bind(&EssentialMatrixFactor::evaluateError, &factor, _1, boost::none), trueE); // Verify the Jacobian is correct - EXPECT(assert_equal(HExpected, HActual, 1e-8)); + EXPECT(assert_equal(Hexpected, Hactual, 1e-8)); } } @@ -138,6 +185,36 @@ TEST (EssentialMatrixFactor, fromConstraints) { } +//************************************************************************* +TEST (EssentialMatrixFactor2, factor) { + EssentialMatrix trueE(aRb, aTb); + LieScalar d(5); + noiseModel::Unit::shared_ptr model = noiseModel::Unit::Create(1); + + for (size_t i = 0; i < 5; i++) { + EssentialMatrixFactor2 factor(100, i, pA(i), pB(i), model); + + // Check evaluation + Vector expected(1); + expected << 0; + Matrix Hactual1, Hactual2; + Vector actual = factor.evaluateError(trueE, d, Hactual1, Hactual2); + EXPECT(assert_equal(expected, actual, 1e-7)); + + // Use numerical derivatives to calculate the expected Jacobian + Matrix Hexpected1, Hexpected2; + boost::function f = + boost::bind(&EssentialMatrixFactor2::evaluateError, &factor, _1, _2, + boost::none, boost::none); + Hexpected1 = numericalDerivative21(f, trueE, d); + Hexpected2 = numericalDerivative22(f, trueE, d); + + // Verify the Jacobian is correct + EXPECT(assert_equal(Hexpected1, Hactual1, 1e-8)); + EXPECT(assert_equal(Hexpected2, Hactual2, 1e-8)); + } +} + /* ************************************************************************* */ int main() { TestResult tr; From 92c1398cd2e5c33dfd026a51e884c669e70e0d8c Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 23 Dec 2013 22:29:09 -0500 Subject: [PATCH 2/8] Checked evaluation and calculation --- .../slam/tests/testEssentialMatrixFactor.cpp | 79 +++++++++++++------ 1 file changed, 57 insertions(+), 22 deletions(-) diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp index e7cbb0ada..be96af06c 100644 --- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp +++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp @@ -6,27 +6,30 @@ */ #include +#include #include namespace gtsam { /** * Binary factor that optimizes for E and inverse depth: assumes measurement - * in image 1 is perfect, and returns re-projection error in image 2 + * in image 2 is perfect, and returns re-projection error in image 1 */ class EssentialMatrixFactor2: public NoiseModelFactor2 { Point2 pA_, pB_; ///< Measurements in image A and B + Cal3_S2 K_; ///< Calibration typedef NoiseModelFactor2 Base; + typedef EssentialMatrixFactor2 This; public: /// Constructor EssentialMatrixFactor2(Key key1, Key key2, const Point2& pA, const Point2& pB, - const SharedNoiseModel& model) : - Base(model, key1, key2), pA_(pA), pB_(pB) { + const Cal3_S2& K, const SharedNoiseModel& model) : + Base(model, key1, key2), pA_(pA), pB_(pB), K_(K) { } /// print @@ -42,16 +45,32 @@ public: Vector evaluateError(const EssentialMatrix& E, const LieScalar& d, boost::optional H1 = boost::none, boost::optional H2 = boost::none) const { + // We have point x,y in image 1 + // Given a depth Z, the corresponding 3D point P1 = Z*(x,y,1) = (x,y,1)/d + // We then convert to first camera by 2P = 1R2Õ*(P1-1T2) + // The homogeneous coordinates of can be written as + // 2R1*(P1-1T2) == 2R1*d*(P1-1T2) == 2R1*((x,y,1)-d*1T2) + // Note that this is just a homography for d==0 + Point3 dP1(pA_.x(), pA_.y(), 1); + Point3 P2 = E.rotation().unrotate(dP1 - d * E.direction().point3()); + + // Project to normalized image coordinates, then uncalibrate + const Point2 pn = SimpleCamera::project_to_camera(P2); + const Point2 pi = K_.uncalibrate(pn); + Point2 reprojectionError(pi - pB_); + if (H1) *H1 = zeros(2, 5); if (H2) *H2 = zeros(2, 1); - return (Vector(2) << 0,0); + + return reprojectionError.vector(); } }; -} // gtsam +} +// gtsam #include #include @@ -187,31 +206,47 @@ TEST (EssentialMatrixFactor, fromConstraints) { //************************************************************************* TEST (EssentialMatrixFactor2, factor) { - EssentialMatrix trueE(aRb, aTb); - LieScalar d(5); + EssentialMatrix E(aRb, aTb); noiseModel::Unit::shared_ptr model = noiseModel::Unit::Create(1); for (size_t i = 0; i < 5; i++) { - EssentialMatrixFactor2 factor(100, i, pA(i), pB(i), model); + Cal3_S2 K; + EssentialMatrixFactor2 factor(100, i, pA(i), pB(i), K, model); // Check evaluation - Vector expected(1); - expected << 0; + Point3 P1 = data.tracks[i].p, P2 = E.transform_to(P1); + LieScalar d(1.0 / P1.z()); + const Point2 pn = SimpleCamera::project_to_camera(P2); + const Point2 pi = K.uncalibrate(pn); + Point2 reprojectionError(pi - pB(i)); + Vector expected = reprojectionError.vector(); + + { + // Check calculations + Point3 dP1(pA(i).x(), pA(i).y(), 1); + EXPECT_DOUBLES_EQUAL(pA(i).x(), P1.x()/P1.z(), 1e-8); + EXPECT_DOUBLES_EQUAL(pA(i).y(), P1.y()/P1.z(), 1e-8); + EXPECT(assert_equal(P1,dP1/d)); + Point3 otherP2 = E.rotation().unrotate( + d * P1 - d * E.direction().point3()); + EXPECT(assert_equal(P2,otherP2/d)); + } + Matrix Hactual1, Hactual2; - Vector actual = factor.evaluateError(trueE, d, Hactual1, Hactual2); + Vector actual = factor.evaluateError(E, d, Hactual1, Hactual2); EXPECT(assert_equal(expected, actual, 1e-7)); - // Use numerical derivatives to calculate the expected Jacobian - Matrix Hexpected1, Hexpected2; - boost::function f = - boost::bind(&EssentialMatrixFactor2::evaluateError, &factor, _1, _2, - boost::none, boost::none); - Hexpected1 = numericalDerivative21(f, trueE, d); - Hexpected2 = numericalDerivative22(f, trueE, d); - - // Verify the Jacobian is correct - EXPECT(assert_equal(Hexpected1, Hactual1, 1e-8)); - EXPECT(assert_equal(Hexpected2, Hactual2, 1e-8)); +// // Use numerical derivatives to calculate the expected Jacobian +// Matrix Hexpected1, Hexpected2; +// boost::function f = +// boost::bind(&EssentialMatrixFactor2::evaluateError, &factor, _1, _2, +// boost::none, boost::none); +// Hexpected1 = numericalDerivative21(f, E, d); +// Hexpected2 = numericalDerivative22(f, E, d); +// +// // Verify the Jacobian is correct +// EXPECT(assert_equal(Hexpected1, Hactual1, 1e-8)); +// EXPECT(assert_equal(Hexpected2, Hactual2, 1e-8)); } } From 44d03e638ecfca12f6575500800df75e9f1d1fe4 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 24 Dec 2013 00:19:08 -0500 Subject: [PATCH 3/8] Tedious derivatives, but right from the start! --- .../slam/tests/testEssentialMatrixFactor.cpp | 69 +++++++++++++------ 1 file changed, 49 insertions(+), 20 deletions(-) diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp index be96af06c..86284de50 100644 --- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp +++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp @@ -43,8 +43,9 @@ public: /// vector of errors returns 1D vector Vector evaluateError(const EssentialMatrix& E, const LieScalar& d, - boost::optional H1 = boost::none, boost::optional H2 = + boost::optional DE = boost::none, boost::optional Dd = boost::none) const { + // We have point x,y in image 1 // Given a depth Z, the corresponding 3D point P1 = Z*(x,y,1) = (x,y,1)/d // We then convert to first camera by 2P = 1R2Õ*(P1-1T2) @@ -52,18 +53,46 @@ public: // 2R1*(P1-1T2) == 2R1*d*(P1-1T2) == 2R1*((x,y,1)-d*1T2) // Note that this is just a homography for d==0 Point3 dP1(pA_.x(), pA_.y(), 1); - Point3 P2 = E.rotation().unrotate(dP1 - d * E.direction().point3()); // Project to normalized image coordinates, then uncalibrate - const Point2 pn = SimpleCamera::project_to_camera(P2); - const Point2 pi = K_.uncalibrate(pn); - Point2 reprojectionError(pi - pB_); + Point2 pi; + if (!DE && !Dd) { + Point3 d1T2 = d * E.direction().point3(); + Point3 dP2 = E.rotation().unrotate(dP1 - d1T2); + Point2 pn = SimpleCamera::project_to_camera(dP2); + pi = K_.uncalibrate(pn); + } else { + // TODO, clean up this expensive mess w Mathematica + Matrix D_1T2_dir; // 3*2 + Point3 _1T2 = E.direction().point3(D_1T2_dir); - if (H1) - *H1 = zeros(2, 5); - if (H2) - *H2 = zeros(2, 1); + Matrix Dd1T2_dir = d * D_1T2_dir; // 3*2 + Matrix Dd1T2_d = _1T2.vector(); // 3*1 + Point3 d1T2 = d * _1T2; + Matrix Dpoint_dir = -Dd1T2_dir; // 3*2 + Matrix Dpoint_d = -Dd1T2_d; // 3*1 + Point3 point = dP1 - d1T2; + + Matrix DdP2_rot, DP2_point; + Point3 dP2 = E.rotation().unrotate(point, DdP2_rot, DP2_point); + Matrix DdP2_E(3, 5); + DdP2_E << DdP2_rot, DP2_point * Dpoint_dir; // (3*3), (3*3) * (3*2) + Matrix DdP2_d = DP2_point * Dpoint_d; // (3*3) * (3*1) + + Matrix Dpn_dP2; // 2*3 + Point2 pn = SimpleCamera::project_to_camera(dP2, Dpn_dP2); + Matrix Dpn_E = Dpn_dP2 * DdP2_E; // (2*3) * (3*5) + Matrix Dpn_d = Dpn_dP2 * DdP2_d; // (2*3) * (3*1) + + Matrix Dpi_pn; // 2*2 + pi = K_.uncalibrate(pn, boost::none, Dpi_pn); + if (DE) + *DE = Dpi_pn * Dpn_E; // (2*2) * (2*5) + if (Dd) + *Dd = Dpi_pn * Dpn_d; // (2*2) * (2*1) + } + Point2 reprojectionError = pi - pB_; return reprojectionError.vector(); } @@ -236,17 +265,17 @@ TEST (EssentialMatrixFactor2, factor) { Vector actual = factor.evaluateError(E, d, Hactual1, Hactual2); EXPECT(assert_equal(expected, actual, 1e-7)); -// // Use numerical derivatives to calculate the expected Jacobian -// Matrix Hexpected1, Hexpected2; -// boost::function f = -// boost::bind(&EssentialMatrixFactor2::evaluateError, &factor, _1, _2, -// boost::none, boost::none); -// Hexpected1 = numericalDerivative21(f, E, d); -// Hexpected2 = numericalDerivative22(f, E, d); -// -// // Verify the Jacobian is correct -// EXPECT(assert_equal(Hexpected1, Hactual1, 1e-8)); -// EXPECT(assert_equal(Hexpected2, Hactual2, 1e-8)); + // Use numerical derivatives to calculate the expected Jacobian + Matrix Hexpected1, Hexpected2; + boost::function f = + boost::bind(&EssentialMatrixFactor2::evaluateError, &factor, _1, _2, + boost::none, boost::none); + Hexpected1 = numericalDerivative21(f, E, d); + Hexpected2 = numericalDerivative22(f, E, d); + + // Verify the Jacobian is correct + EXPECT(assert_equal(Hexpected1, Hactual1, 1e-8)); + EXPECT(assert_equal(Hexpected2, Hactual2, 1e-8)); } } From d6edd1db07febfc9341424e617c7b0f149706153 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 24 Dec 2013 00:36:54 -0500 Subject: [PATCH 4/8] Sanitized/collapsed derivatives --- .../slam/tests/testEssentialMatrixFactor.cpp | 35 ++++++++++--------- 1 file changed, 18 insertions(+), 17 deletions(-) diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp index 86284de50..001fb15fe 100644 --- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp +++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp @@ -57,40 +57,41 @@ public: // Project to normalized image coordinates, then uncalibrate Point2 pi; if (!DE && !Dd) { - Point3 d1T2 = d * E.direction().point3(); + + Point3 _1T2 = E.direction().point3(); + Point3 d1T2 = d * _1T2; Point3 dP2 = E.rotation().unrotate(dP1 - d1T2); Point2 pn = SimpleCamera::project_to_camera(dP2); pi = K_.uncalibrate(pn); + } else { + // TODO, clean up this expensive mess w Mathematica + Matrix D_1T2_dir; // 3*2 Point3 _1T2 = E.direction().point3(D_1T2_dir); - Matrix Dd1T2_dir = d * D_1T2_dir; // 3*2 - Matrix Dd1T2_d = _1T2.vector(); // 3*1 Point3 d1T2 = d * _1T2; - Matrix Dpoint_dir = -Dd1T2_dir; // 3*2 - Matrix Dpoint_d = -Dd1T2_d; // 3*1 - Point3 point = dP1 - d1T2; - Matrix DdP2_rot, DP2_point; - Point3 dP2 = E.rotation().unrotate(point, DdP2_rot, DP2_point); - Matrix DdP2_E(3, 5); - DdP2_E << DdP2_rot, DP2_point * Dpoint_dir; // (3*3), (3*3) * (3*2) - Matrix DdP2_d = DP2_point * Dpoint_d; // (3*3) * (3*1) + Point3 dP2 = E.rotation().unrotate(dP1 - d1T2, DdP2_rot, DP2_point); Matrix Dpn_dP2; // 2*3 Point2 pn = SimpleCamera::project_to_camera(dP2, Dpn_dP2); - Matrix Dpn_E = Dpn_dP2 * DdP2_E; // (2*3) * (3*5) - Matrix Dpn_d = Dpn_dP2 * DdP2_d; // (2*3) * (3*1) - + Matrix Dpi_pn; // 2*2 pi = K_.uncalibrate(pn, boost::none, Dpi_pn); - if (DE) - *DE = Dpi_pn * Dpn_E; // (2*2) * (2*5) + + if (DE) { + Matrix DdP2_E(3, 5); + DdP2_E << DdP2_rot, - DP2_point * d * D_1T2_dir; // (3*3), (3*3) * (3*2) + *DE = Dpi_pn * (Dpn_dP2 * DdP2_E); // (2*2) * (2*3) * (3*5) + } + if (Dd) - *Dd = Dpi_pn * Dpn_d; // (2*2) * (2*1) + // (2*2) * (2*3) * (3*3) * (3*1) + *Dd = - (Dpi_pn * (Dpn_dP2 * (DP2_point * _1T2.vector()))); + } Point2 reprojectionError = pi - pB_; return reprojectionError.vector(); From bba8368218024c794b205e62069d2b6fdf63d923 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 24 Dec 2013 00:48:33 -0500 Subject: [PATCH 5/8] Added clone, minimization test; converges, but not to grund truth (which has high error) --- .../slam/tests/testEssentialMatrixFactor.cpp | 55 +++++++++++++++++-- 1 file changed, 50 insertions(+), 5 deletions(-) diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp index 001fb15fe..fca3015b6 100644 --- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp +++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp @@ -32,6 +32,12 @@ public: Base(model, key1, key2), pA_(pA), pB_(pB), K_(K) { } + /// @return a deep copy of this factor + virtual gtsam::NonlinearFactor::shared_ptr clone() const { + return boost::static_pointer_cast < gtsam::NonlinearFactor + > (gtsam::NonlinearFactor::shared_ptr(new This(*this))); + } + /// print virtual void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { @@ -84,13 +90,13 @@ public: if (DE) { Matrix DdP2_E(3, 5); - DdP2_E << DdP2_rot, - DP2_point * d * D_1T2_dir; // (3*3), (3*3) * (3*2) + DdP2_E << DdP2_rot, -DP2_point * d * D_1T2_dir; // (3*3), (3*3) * (3*2) *DE = Dpi_pn * (Dpn_dP2 * DdP2_E); // (2*2) * (2*3) * (3*5) } if (Dd) // (2*2) * (2*3) * (3*3) * (3*1) - *Dd = - (Dpi_pn * (Dpn_dP2 * (DP2_point * _1T2.vector()))); + *Dd = -(Dpi_pn * (Dpn_dP2 * (DP2_point * _1T2.vector()))); } Point2 reprojectionError = pi - pB_; @@ -114,6 +120,8 @@ public: using namespace std; using namespace gtsam; +typedef noiseModel::Isotropic::shared_ptr Model; + const string filename = findExampleDataFile("5pointExample1.txt"); SfM_data data; bool readOK = readBAL(filename, data); @@ -133,6 +141,8 @@ Vector vB(size_t i) { return EssentialMatrix::Homogeneous(pB(i)); } +Cal3_S2 K; + //************************************************************************* TEST (EssentialMatrixFactor, testData) { CHECK(readOK); @@ -198,8 +208,7 @@ TEST (EssentialMatrixFactor, fromConstraints) { // We start with a factor graph and add constraints to it // Noise sigma is 1cm, assuming metric measurements NonlinearFactorGraph graph; - noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(1, - 0.01); + Model model = noiseModel::Isotropic::Sigma(1, 0.01); for (size_t i = 0; i < 5; i++) graph.add(EssentialMatrixFactor(1, pA(i), pB(i), model)); @@ -240,7 +249,6 @@ TEST (EssentialMatrixFactor2, factor) { noiseModel::Unit::shared_ptr model = noiseModel::Unit::Create(1); for (size_t i = 0; i < 5; i++) { - Cal3_S2 K; EssentialMatrixFactor2 factor(100, i, pA(i), pB(i), K, model); // Check evaluation @@ -280,6 +288,43 @@ TEST (EssentialMatrixFactor2, factor) { } } +//************************************************************************* +TEST (EssentialMatrixFactor2, minimization) { + // Here we want to optimize for E and inverse depths at the same time + + // We start with a factor graph and add constraints to it + // Noise sigma is 1cm, assuming metric measurements + NonlinearFactorGraph graph; + Model model = noiseModel::Isotropic::Sigma(2, 0.01); + for (size_t i = 0; i < 5; i++) + graph.add(EssentialMatrixFactor2(100, i, pA(i), pB(i), K, model)); + + // Check error at ground truth + Values truth; + EssentialMatrix trueE(aRb, aTb); + truth.insert(100, trueE); + for (size_t i = 0; i < 5; i++) { + Point3 P1 = data.tracks[i].p; + truth.insert(i, LieScalar(1.0 / P1.z())); + } + EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8); + + // Optimize + LevenbergMarquardtParams parameters; + // parameters.setVerbosity("ERROR"); + LevenbergMarquardtOptimizer optimizer(graph, truth, parameters); + Values result = optimizer.optimize(); + + // Check result + EssentialMatrix actual = result.at(100); + EXPECT(assert_equal(trueE, actual,1e-1)); + for (size_t i = 0; i < 5; i++) + EXPECT(assert_equal(result.at(i), truth.at(i),1e-1)); + + // Check error at result + EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4); +} + /* ************************************************************************* */ int main() { TestResult tr; From 708665ca04aff61c10a68903132607b2afad7493 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 24 Dec 2013 00:52:24 -0500 Subject: [PATCH 6/8] Moved new factor to header; Still fails! --- gtsam/slam/EssentialMatrixFactor.h | 104 +++++++++++++++++- .../slam/tests/testEssentialMatrixFactor.cpp | 102 ----------------- 2 files changed, 101 insertions(+), 105 deletions(-) diff --git a/gtsam/slam/EssentialMatrixFactor.h b/gtsam/slam/EssentialMatrixFactor.h index 7ac992d79..e78c16c85 100644 --- a/gtsam/slam/EssentialMatrixFactor.h +++ b/gtsam/slam/EssentialMatrixFactor.h @@ -9,6 +9,8 @@ #include #include +#include +#include #include namespace gtsam { @@ -36,8 +38,9 @@ public: /// @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))); } + return boost::static_pointer_cast < gtsam::NonlinearFactor + > (gtsam::NonlinearFactor::shared_ptr(new This(*this))); + } /// print virtual void print(const std::string& s = "", @@ -56,5 +59,100 @@ public: }; -} // gtsam +/** + * Binary factor that optimizes for E and inverse depth: assumes measurement + * in image 2 is perfect, and returns re-projection error in image 1 + */ +class EssentialMatrixFactor2: public NoiseModelFactor2 { + + Point2 pA_, pB_; ///< Measurements in image A and B + Cal3_S2 K_; ///< Calibration + + typedef NoiseModelFactor2 Base; + typedef EssentialMatrixFactor2 This; + +public: + + /// Constructor + EssentialMatrixFactor2(Key key1, Key key2, const Point2& pA, const Point2& pB, + const Cal3_S2& K, const SharedNoiseModel& model) : + Base(model, key1, key2), pA_(pA), pB_(pB), K_(K) { + } + + /// @return a deep copy of this factor + virtual gtsam::NonlinearFactor::shared_ptr clone() const { + return boost::static_pointer_cast < gtsam::NonlinearFactor + > (gtsam::NonlinearFactor::shared_ptr(new This(*this))); + } + + /// print + virtual void print(const std::string& s = "", + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + Base::print(s); + std::cout << " EssentialMatrixFactor2 with measurements\n (" + << pA_.vector().transpose() << ")' and (" << pB_.vector().transpose() + << ")'" << std::endl; + } + + /// vector of errors returns 1D vector + Vector evaluateError(const EssentialMatrix& E, const LieScalar& d, + boost::optional DE = boost::none, boost::optional Dd = + boost::none) const { + + // We have point x,y in image 1 + // Given a depth Z, the corresponding 3D point P1 = Z*(x,y,1) = (x,y,1)/d + // We then convert to first camera by 2P = 1R2Õ*(P1-1T2) + // The homogeneous coordinates of can be written as + // 2R1*(P1-1T2) == 2R1*d*(P1-1T2) == 2R1*((x,y,1)-d*1T2) + // Note that this is just a homography for d==0 + Point3 dP1(pA_.x(), pA_.y(), 1); + + // Project to normalized image coordinates, then uncalibrate + Point2 pi; + if (!DE && !Dd) { + + Point3 _1T2 = E.direction().point3(); + Point3 d1T2 = d * _1T2; + Point3 dP2 = E.rotation().unrotate(dP1 - d1T2); + Point2 pn = SimpleCamera::project_to_camera(dP2); + pi = K_.uncalibrate(pn); + + } else { + + // TODO, clean up this expensive mess w Mathematica + + Matrix D_1T2_dir; // 3*2 + Point3 _1T2 = E.direction().point3(D_1T2_dir); + + Point3 d1T2 = d * _1T2; + + Matrix DdP2_rot, DP2_point; + Point3 dP2 = E.rotation().unrotate(dP1 - d1T2, DdP2_rot, DP2_point); + + Matrix Dpn_dP2; // 2*3 + Point2 pn = SimpleCamera::project_to_camera(dP2, Dpn_dP2); + + Matrix Dpi_pn; // 2*2 + pi = K_.uncalibrate(pn, boost::none, Dpi_pn); + + if (DE) { + Matrix DdP2_E(3, 5); + DdP2_E << DdP2_rot, -DP2_point * d * D_1T2_dir; // (3*3), (3*3) * (3*2) + *DE = Dpi_pn * (Dpn_dP2 * DdP2_E); // (2*2) * (2*3) * (3*5) + } + + if (Dd) + // (2*2) * (2*3) * (3*3) * (3*1) + *Dd = -(Dpi_pn * (Dpn_dP2 * (DP2_point * _1T2.vector()))); + + } + Point2 reprojectionError = pi - pB_; + return reprojectionError.vector(); + } + +}; +// EssentialMatrixFactor2 + +}// gtsam diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp index fca3015b6..4fb24b240 100644 --- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp +++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp @@ -6,108 +6,6 @@ */ #include -#include -#include - -namespace gtsam { - -/** - * Binary factor that optimizes for E and inverse depth: assumes measurement - * in image 2 is perfect, and returns re-projection error in image 1 - */ -class EssentialMatrixFactor2: public NoiseModelFactor2 { - - Point2 pA_, pB_; ///< Measurements in image A and B - Cal3_S2 K_; ///< Calibration - - typedef NoiseModelFactor2 Base; - typedef EssentialMatrixFactor2 This; - -public: - - /// Constructor - EssentialMatrixFactor2(Key key1, Key key2, const Point2& pA, const Point2& pB, - const Cal3_S2& K, const SharedNoiseModel& model) : - Base(model, key1, key2), pA_(pA), pB_(pB), K_(K) { - } - - /// @return a deep copy of this factor - virtual gtsam::NonlinearFactor::shared_ptr clone() const { - return boost::static_pointer_cast < gtsam::NonlinearFactor - > (gtsam::NonlinearFactor::shared_ptr(new This(*this))); - } - - /// print - virtual void print(const std::string& s = "", - const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { - Base::print(s); - std::cout << " EssentialMatrixFactor2 with measurements\n (" - << pA_.vector().transpose() << ")' and (" << pB_.vector().transpose() - << ")'" << std::endl; - } - - /// vector of errors returns 1D vector - Vector evaluateError(const EssentialMatrix& E, const LieScalar& d, - boost::optional DE = boost::none, boost::optional Dd = - boost::none) const { - - // We have point x,y in image 1 - // Given a depth Z, the corresponding 3D point P1 = Z*(x,y,1) = (x,y,1)/d - // We then convert to first camera by 2P = 1R2Õ*(P1-1T2) - // The homogeneous coordinates of can be written as - // 2R1*(P1-1T2) == 2R1*d*(P1-1T2) == 2R1*((x,y,1)-d*1T2) - // Note that this is just a homography for d==0 - Point3 dP1(pA_.x(), pA_.y(), 1); - - // Project to normalized image coordinates, then uncalibrate - Point2 pi; - if (!DE && !Dd) { - - Point3 _1T2 = E.direction().point3(); - Point3 d1T2 = d * _1T2; - Point3 dP2 = E.rotation().unrotate(dP1 - d1T2); - Point2 pn = SimpleCamera::project_to_camera(dP2); - pi = K_.uncalibrate(pn); - - } else { - - // TODO, clean up this expensive mess w Mathematica - - Matrix D_1T2_dir; // 3*2 - Point3 _1T2 = E.direction().point3(D_1T2_dir); - - Point3 d1T2 = d * _1T2; - - Matrix DdP2_rot, DP2_point; - Point3 dP2 = E.rotation().unrotate(dP1 - d1T2, DdP2_rot, DP2_point); - - Matrix Dpn_dP2; // 2*3 - Point2 pn = SimpleCamera::project_to_camera(dP2, Dpn_dP2); - - Matrix Dpi_pn; // 2*2 - pi = K_.uncalibrate(pn, boost::none, Dpi_pn); - - if (DE) { - Matrix DdP2_E(3, 5); - DdP2_E << DdP2_rot, -DP2_point * d * D_1T2_dir; // (3*3), (3*3) * (3*2) - *DE = Dpi_pn * (Dpn_dP2 * DdP2_E); // (2*2) * (2*3) * (3*5) - } - - if (Dd) - // (2*2) * (2*3) * (3*3) * (3*1) - *Dd = -(Dpi_pn * (Dpn_dP2 * (DP2_point * _1T2.vector()))); - - } - Point2 reprojectionError = pi - pB_; - return reprojectionError.vector(); - } - -}; - -} -// gtsam - #include #include #include From adac9d209f8615df9d487b13908f7e0835bb6fcb Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 24 Dec 2013 01:25:13 -0500 Subject: [PATCH 7/8] Fixed unit test (baseline of test-case was not 1, corrected for it) --- gtsam/slam/tests/testEssentialMatrixFactor.cpp | 18 ++++-------------- 1 file changed, 4 insertions(+), 14 deletions(-) diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp index 4fb24b240..57fb47715 100644 --- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp +++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp @@ -25,6 +25,7 @@ SfM_data data; bool readOK = readBAL(filename, data); Rot3 aRb = data.cameras[1].pose().rotation(); Point3 aTb = data.cameras[1].pose().translation(); +double baseline = 0.1; // actual baseline of the camera Point2 pA(size_t i) { return data.tracks[i].measurements[0].second; @@ -150,25 +151,14 @@ TEST (EssentialMatrixFactor2, factor) { EssentialMatrixFactor2 factor(100, i, pA(i), pB(i), K, model); // Check evaluation - Point3 P1 = data.tracks[i].p, P2 = E.transform_to(P1); - LieScalar d(1.0 / P1.z()); + Point3 P1 = data.tracks[i].p, P2 = data.cameras[1].pose().transform_to(P1); const Point2 pn = SimpleCamera::project_to_camera(P2); const Point2 pi = K.uncalibrate(pn); Point2 reprojectionError(pi - pB(i)); Vector expected = reprojectionError.vector(); - { - // Check calculations - Point3 dP1(pA(i).x(), pA(i).y(), 1); - EXPECT_DOUBLES_EQUAL(pA(i).x(), P1.x()/P1.z(), 1e-8); - EXPECT_DOUBLES_EQUAL(pA(i).y(), P1.y()/P1.z(), 1e-8); - EXPECT(assert_equal(P1,dP1/d)); - Point3 otherP2 = E.rotation().unrotate( - d * P1 - d * E.direction().point3()); - EXPECT(assert_equal(P2,otherP2/d)); - } - Matrix Hactual1, Hactual2; + LieScalar d(baseline / P1.z()); Vector actual = factor.evaluateError(E, d, Hactual1, Hactual2); EXPECT(assert_equal(expected, actual, 1e-7)); @@ -203,7 +193,7 @@ TEST (EssentialMatrixFactor2, minimization) { truth.insert(100, trueE); for (size_t i = 0; i < 5; i++) { Point3 P1 = data.tracks[i].p; - truth.insert(i, LieScalar(1.0 / P1.z())); + truth.insert(i, LieScalar(baseline / P1.z())); } EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8); From e6a64ec5b25507a8780d55e03c22607082e74816 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 24 Dec 2013 01:25:52 -0500 Subject: [PATCH 8/8] Cleaned up derivatives somewhat more --- gtsam/slam/EssentialMatrixFactor.h | 16 +++++----------- 1 file changed, 5 insertions(+), 11 deletions(-) diff --git a/gtsam/slam/EssentialMatrixFactor.h b/gtsam/slam/EssentialMatrixFactor.h index e78c16c85..090a816ec 100644 --- a/gtsam/slam/EssentialMatrixFactor.h +++ b/gtsam/slam/EssentialMatrixFactor.h @@ -120,20 +120,14 @@ public: } else { - // TODO, clean up this expensive mess w Mathematica + // Calculate derivatives. TODO if slow: optimize with Mathematica + // 3*2 3*3 3*3 2*3 2*2 + Matrix D_1T2_dir, DdP2_rot, DP2_point, Dpn_dP2, Dpi_pn; - Matrix D_1T2_dir; // 3*2 Point3 _1T2 = E.direction().point3(D_1T2_dir); - Point3 d1T2 = d * _1T2; - - Matrix DdP2_rot, DP2_point; Point3 dP2 = E.rotation().unrotate(dP1 - d1T2, DdP2_rot, DP2_point); - - Matrix Dpn_dP2; // 2*3 Point2 pn = SimpleCamera::project_to_camera(dP2, Dpn_dP2); - - Matrix Dpi_pn; // 2*2 pi = K_.uncalibrate(pn, boost::none, Dpi_pn); if (DE) { @@ -142,8 +136,8 @@ public: *DE = Dpi_pn * (Dpn_dP2 * DdP2_E); // (2*2) * (2*3) * (3*5) } - if (Dd) - // (2*2) * (2*3) * (3*3) * (3*1) + if (Dd) // efficient backwards computation: + // (2*2) * (2*3) * (3*3) * (3*1) *Dd = -(Dpi_pn * (Dpn_dP2 * (DP2_point * _1T2.vector()))); }