diff --git a/gtsam/slam/EssentialMatrixFactor.h b/gtsam/slam/EssentialMatrixFactor.h index 142005d3c..0ab43aa4f 100644 --- a/gtsam/slam/EssentialMatrixFactor.h +++ b/gtsam/slam/EssentialMatrixFactor.h @@ -20,26 +20,46 @@ namespace gtsam { */ class EssentialMatrixFactor: public NoiseModelFactor1 { - Point2 pA_, pB_; ///< Measurements in image A and B - Vector vA_, vB_; ///< Homogeneous versions + Vector vA_, vB_; ///< Homogeneous versions, in ideal coordinates typedef NoiseModelFactor1 Base; typedef EssentialMatrixFactor This; public: - /// Constructor + /** + * Constructor + * @param pA point in first camera, in calibrated coordinates + * @param pB point in second camera, in calibrated coordinates + * @param model noise model is about dot product in ideal, homogeneous coordinates + */ EssentialMatrixFactor(Key key, const Point2& pA, const Point2& pB, const SharedNoiseModel& model) : - Base(model, key), pA_(pA), pB_(pB), // - vA_(EssentialMatrix::Homogeneous(pA)), // - vB_(EssentialMatrix::Homogeneous(pB)) { + Base(model, key) { + vA_ = EssentialMatrix::Homogeneous(pA); + vB_ = EssentialMatrix::Homogeneous(pB); + } + + /** + * Constructor + * @param pA point in first camera, in pixel coordinates + * @param pB point in second camera, in pixel coordinates + * @param model noise model is about dot product in ideal, homogeneous coordinates + * @param K calibration object, will be used only in constructor + */ + template + EssentialMatrixFactor(Key key, const Point2& pA, const Point2& pB, + const SharedNoiseModel& model, boost::shared_ptr K) : + Base(model, key) { + assert(K); + vA_ = EssentialMatrix::Homogeneous(K->calibrate(pA)); + vB_ = EssentialMatrix::Homogeneous(K->calibrate(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))); + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); } /// print @@ -47,14 +67,16 @@ public: const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { Base::print(s); std::cout << " EssentialMatrixFactor with measurements\n (" - << pA_.vector().transpose() << ")' and (" << pB_.vector().transpose() - << ")'" << std::endl; + << vA_.transpose() << ")' and (" << vB_.transpose() << ")'" + << std::endl; } /// vector of errors returns 1D vector Vector evaluateError(const EssentialMatrix& E, boost::optional H = boost::none) const { - return (Vector(1) << E.error(vA_, vB_, H)); + Vector error(1); + error << E.error(vA_, vB_, H); + return error; } }; @@ -67,26 +89,50 @@ class EssentialMatrixFactor2: public NoiseModelFactor2 { Point3 dP1_; ///< 3D point corresponding to measurement in image 1 - Point2 p1_, p2_; ///< Measurements in image 1 and image 2 - Cal3_S2 K_; ///< Calibration + Point2 pn_; ///< Measurement in image 2, in ideal coordinates + double f_; ///< approximate conversion factor for error scaling typedef NoiseModelFactor2 Base; typedef EssentialMatrixFactor2 This; public: - /// Constructor + /** + * Constructor + * @param pA point in first camera, in calibrated coordinates + * @param pB point in second camera, in calibrated coordinates + * @param model noise model should be in pixels, as well + */ EssentialMatrixFactor2(Key key1, Key key2, const Point2& pA, const Point2& pB, - const Cal3_S2& K, const SharedNoiseModel& model) : - Base(model, key1, key2), p1_(pA), p2_(pB), K_(K) { - Point2 xy = K_.calibrate(p1_); - dP1_ = Point3(xy.x(), xy.y(), 1); + const SharedNoiseModel& model) : + Base(model, key1, key2) { + dP1_ = Point3(pA.x(), pA.y(), 1); + pn_ = pB; + f_ = 1.0; + } + + /** + * Constructor + * @param pA point in first camera, in pixel coordinates + * @param pB point in second camera, in pixel coordinates + * @param K calibration object, will be used only in constructor + * @param model noise model should be in pixels, as well + */ + template + EssentialMatrixFactor2(Key key1, Key key2, const Point2& pA, const Point2& pB, + const SharedNoiseModel& model, boost::shared_ptr K) : + Base(model, key1, key2) { + assert(K); + Point2 p1 = K->calibrate(pA); + dP1_ = Point3(p1.x(), p1.y(), 1); + pn_ = K->calibrate(pB); + f_ = 0.5 * (K->fx() + K->fy()); } /// @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))); + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); } /// print @@ -94,7 +140,7 @@ public: const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { Base::print(s); std::cout << " EssentialMatrixFactor2 with measurements\n (" - << p1_.vector().transpose() << ")' and (" << p2_.vector().transpose() + << dP1_.vector().transpose() << ")' and (" << pn_.vector().transpose() << ")'" << std::endl; } @@ -112,40 +158,38 @@ public: // The point d*P1 = (x,y,1) is computed in constructor as dP1_ // Project to normalized image coordinates, then uncalibrate - Point2 pi; + Point2 pn; 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); + pn = SimpleCamera::project_to_camera(dP2); } 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; + // 3*2 3*3 3*3 2*3 + Matrix D_1T2_dir, DdP2_rot, DP2_point, Dpn_dP2; 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); + pn = SimpleCamera::project_to_camera(dP2, Dpn_dP2); 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) + *DE = f_ * Dpn_dP2 * DdP2_E; // (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()))); + // (2*3) * (3*3) * (3*1) + *Dd = -f_ * (Dpn_dP2 * (DP2_point * _1T2.vector())); } - Point2 reprojectionError = pi - p2_; - return reprojectionError.vector(); + Point2 reprojectionError = pn - pn_; + return f_ * reprojectionError.vector(); } }; diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp index 5f9233bb3..fabcd47cf 100644 --- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp +++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp @@ -18,7 +18,11 @@ using namespace std; using namespace gtsam; -typedef noiseModel::Isotropic::shared_ptr Model; +// Noise model for first type of factor is evaluating algebraic error +noiseModel::Isotropic::shared_ptr model1 = noiseModel::Isotropic::Sigma(1, + 0.01); +// Noise model for second type of factor is evaluating pixel coordinates +noiseModel::Unit::shared_ptr model2 = noiseModel::Unit::Create(2); namespace example1 { @@ -42,8 +46,6 @@ Vector vB(size_t i) { return EssentialMatrix::Homogeneous(pB(i)); } -Cal3_S2 K; - //************************************************************************* TEST (EssentialMatrixFactor, testData) { CHECK(readOK); @@ -76,10 +78,9 @@ TEST (EssentialMatrixFactor, testData) { //************************************************************************* TEST (EssentialMatrixFactor, factor) { EssentialMatrix trueE(aRb, aTb); - noiseModel::Unit::shared_ptr model = noiseModel::Unit::Create(1); for (size_t i = 0; i < 5; i++) { - EssentialMatrixFactor factor(1, pA(i), pB(i), model); + EssentialMatrixFactor factor(1, pA(i), pB(i), model1); // Check evaluation Vector expected(1); @@ -100,7 +101,7 @@ TEST (EssentialMatrixFactor, factor) { } //************************************************************************* -TEST (EssentialMatrixFactor, fromConstraints) { +TEST (EssentialMatrixFactor, minimization) { // Here we want to optimize directly on essential matrix constraints // Yi Ma's algorithm (Ma01ijcv) is a bit cumbersome to implement, // but GTSAM does the equivalent anyway, provided we give the right @@ -109,9 +110,8 @@ TEST (EssentialMatrixFactor, fromConstraints) { // 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(1, 0.01); for (size_t i = 0; i < 5; i++) - graph.add(EssentialMatrixFactor(1, pA(i), pB(i), model)); + graph.add(EssentialMatrixFactor(1, pA(i), pB(i), model1)); // Check error at ground truth Values truth; @@ -147,15 +147,13 @@ 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); + EssentialMatrixFactor2 factor(100, i, pA(i), pB(i), model2); // 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); + const Point2 pi = SimpleCamera::project_to_camera(P2); Point2 reprojectionError(pi - pB(i)); Vector expected = reprojectionError.vector(); @@ -185,9 +183,8 @@ TEST (EssentialMatrixFactor2, minimization) { // 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)); + graph.add(EssentialMatrixFactor2(100, i, pA(i), pB(i), model2)); // Check error at ground truth Values truth; @@ -235,8 +232,56 @@ Point2 pB(size_t i) { return data.tracks[i].measurements[1].second; } -// Matches Cal3Bundler K(500, 0, 0); -Cal3_S2 K(500, 500, 0, 0, 0); +boost::shared_ptr // +K = boost::make_shared < Cal3Bundler > (500, 0, 0); + +Vector vA(size_t i) { + Point2 xy = K->calibrate(pA(i)); + return EssentialMatrix::Homogeneous(xy); +} +Vector vB(size_t i) { + Point2 xy = K->calibrate(pB(i)); + return EssentialMatrix::Homogeneous(xy); +} + +//************************************************************************* +TEST (EssentialMatrixFactor, extraTest) { + // Additional test with camera moving in positive X direction + + NonlinearFactorGraph graph; + for (size_t i = 0; i < 5; i++) + graph.add(EssentialMatrixFactor(1, pA(i), pB(i), model1, K)); + + // Check error at ground truth + Values truth; + EssentialMatrix trueE(aRb, aTb); + truth.insert(1, trueE); + EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8); + + // Check error at initial estimate + Values initial; + EssentialMatrix initialE = trueE.retract( + (Vector(5) << 0.1, -0.1, 0.1, 0.1, -0.1)); + initial.insert(1, initialE); + EXPECT_DOUBLES_EQUAL(640, graph.error(initial), 1e-2); + + // Optimize + LevenbergMarquardtParams parameters; + LevenbergMarquardtOptimizer optimizer(graph, initial, parameters); + Values result = optimizer.optimize(); + + // Check result + EssentialMatrix actual = result.at(1); + EXPECT(assert_equal(trueE, actual,1e-1)); + + // Check error at result + EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4); + + // Check errors individually + for (size_t i = 0; i < 5; i++) + EXPECT_DOUBLES_EQUAL(0, actual.error(vA(i),vB(i)), 1e-6); + +} //************************************************************************* TEST (EssentialMatrixFactor2, extraTest) { @@ -245,9 +290,8 @@ TEST (EssentialMatrixFactor2, extraTest) { // We start with a factor graph and add constraints to it // Noise sigma is 1, assuming pixel measurements NonlinearFactorGraph graph; - Model model = noiseModel::Isotropic::Sigma(2, 1); for (size_t i = 0; i < data.number_tracks(); i++) - graph.add(EssentialMatrixFactor2(100, i, pA(i), pB(i), K, model)); + graph.add(EssentialMatrixFactor2(100, i, pA(i), pB(i), model2, K)); // Check error at ground truth Values truth;