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/RotateFactor.h b/gtsam/slam/RotateFactor.h index f30392d62..0b6dde95d 100644 --- a/gtsam/slam/RotateFactor.h +++ b/gtsam/slam/RotateFactor.h @@ -25,6 +25,7 @@ class RotateFactor: public NoiseModelFactor1 { Point3 p_, z_; ///< Predicted and measured directions, p = iRc * z typedef NoiseModelFactor1 Base; + typedef RotateFactor This; public: @@ -34,6 +35,11 @@ public: Base(model, key), p_(Rot3::Logmap(P)), z_(Rot3::Logmap(Z)) { } + /// @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 { @@ -63,6 +69,7 @@ class RotateDirectionsFactor: public NoiseModelFactor1 { Sphere2 p_, z_; ///< Predicted and measured directions, p = iRc * z typedef NoiseModelFactor1 Base; + typedef RotateDirectionsFactor This; public: @@ -72,6 +79,11 @@ public: Base(model, key), p_(p), z_(z) { } + /// @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..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;