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..090a816ec 100644 --- a/gtsam/slam/EssentialMatrixFactor.h +++ b/gtsam/slam/EssentialMatrixFactor.h @@ -9,6 +9,8 @@ #include #include +#include +#include #include namespace gtsam { @@ -22,6 +24,7 @@ class EssentialMatrixFactor: public NoiseModelFactor1 { Vector vA_, vB_; ///< Homogeneous versions typedef NoiseModelFactor1 Base; + typedef EssentialMatrixFactor This; public: @@ -33,6 +36,12 @@ 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 + > (gtsam::NonlinearFactor::shared_ptr(new This(*this))); + } + /// print virtual void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { @@ -50,5 +59,94 @@ 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 { + + // 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; + + Point3 _1T2 = E.direction().point3(D_1T2_dir); + Point3 d1T2 = d * _1T2; + Point3 dP2 = E.rotation().unrotate(dP1 - d1T2, DdP2_rot, DP2_point); + Point2 pn = SimpleCamera::project_to_camera(dP2, Dpn_dP2); + 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) // efficient backwards computation: + // (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 34a26adbe..57fb47715 100644 --- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp +++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp @@ -18,11 +18,14 @@ 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); Rot3 aRb = data.cameras[1].pose().rotation(); -Point3 aTb = data.cameras[1].pose().translation(); +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; @@ -37,6 +40,8 @@ Vector vB(size_t i) { return EssentialMatrix::Homogeneous(pB(i)); } +Cal3_S2 K; + //************************************************************************* TEST (EssentialMatrixFactor, testData) { CHECK(readOK); @@ -77,18 +82,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)); } } @@ -102,8 +107,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)); @@ -138,6 +142,77 @@ TEST (EssentialMatrixFactor, fromConstraints) { } +//************************************************************************* +TEST (EssentialMatrixFactor2, factor) { + 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), K, model); + + // Check evaluation + 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(); + + Matrix Hactual1, Hactual2; + LieScalar d(baseline / P1.z()); + 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)); + } +} + +//************************************************************************* +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(baseline / 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;