From 8ca7f1ff1d69d68c083a716dad9e0a0eb36172fc Mon Sep 17 00:00:00 2001 From: Ayush Baid Date: Fri, 23 Apr 2021 16:29:03 -0400 Subject: [PATCH 01/30] Adding factor with shared calibration as a variable --- .../EssentialMatrixWithCalibrationFactor.h | 119 +++++ ...stEssentialMatrixWithCalibrationFactor.cpp | 446 ++++++++++++++++++ 2 files changed, 565 insertions(+) create mode 100644 gtsam/slam/EssentialMatrixWithCalibrationFactor.h create mode 100644 gtsam/slam/tests/testEssentialMatrixWithCalibrationFactor.cpp diff --git a/gtsam/slam/EssentialMatrixWithCalibrationFactor.h b/gtsam/slam/EssentialMatrixWithCalibrationFactor.h new file mode 100644 index 000000000..17dbe98a0 --- /dev/null +++ b/gtsam/slam/EssentialMatrixWithCalibrationFactor.h @@ -0,0 +1,119 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file EssentialMatrixWithCalibrationFactor.h + * + * @brief A factor evaluating algebraic epipolar error with essential matrix and calibration as variables. + * + * @author Ayush Baid + * @author Akshay Krishnan + * @date April 23, 2021 + */ + +#pragma once + +#include +#include +#include + +namespace gtsam { + +/** + * Factor that evaluates algebraic epipolar error (K^-1 p)'E (K^-1 p) for given essential matrix and calibration shared + * between two images. + */ +template +class EssentialMatrixWithCalibrationFactor: public NoiseModelFactor2 { + + Point2 pA_, pB_; ///< points in pixel coordinates + + typedef NoiseModelFactor2 Base; + typedef EssentialMatrixWithCalibrationFactor This; + +public: + + /** + * Constructor + * @param essentialMatrixKey Essential Matrix variable key + * @param calibrationKey Calibration variable key + * @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 + */ + EssentialMatrixWithCalibrationFactor(Key essentialMatrixKey, Key calibrationKey, const Point2& pA, const Point2& pB, + const SharedNoiseModel& model) : + Base(model, essentialMatrixKey, calibrationKey), pA_(pA), pB_(pB) {} + + + /// @return a deep copy of this factor + gtsam::NonlinearFactor::shared_ptr clone() const override { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); + } + + /// print + void print(const std::string& s = "", + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { + Base::print(s); + std::cout << " EssentialMatrixWithCalibrationFactor with measurements\n (" + << pA_.transpose() << ")' and (" << pB_.transpose() << ")'" + << std::endl; + } + + /// vector of errors returns 1D vector + /** + * @brief Calculate the algebraic epipolar error p' (K^-1)' E K p. + * + * @param E essential matrix for key essentialMatrixKey + * @param K calibration (common for both images) for key calibrationKey + * @param H1 optional jacobian in E + * @param H2 optional jacobian in K + * @return * Vector + */ + Vector evaluateError(const EssentialMatrix& E, const CALIBRATION& K, + boost::optional H1 = boost::none, boost::optional H2 = boost::none) const override { + Vector error(1); + // converting from pixel coordinates to normalized coordinates cA and cB + Matrix cA_H_K; // dcA/dK + Matrix cB_H_K; // dcB/dK + Point2 cA = K.calibrate(pA_, cA_H_K); + Point2 cB = K.calibrate(pB_, cB_H_K); + + // Homogeneous the coordinates + Vector3 vA = EssentialMatrix::Homogeneous(cA); + Vector3 vB = EssentialMatrix::Homogeneous(cB); + + if (H2){ + // compute the jacobian of error w.r.t K + + // dvX / dcX [3x2] = [1, 0], [0, 1], [0, 0] + Matrix v_H_c = (Matrix(3, 2) << 1.0, 0.0, 0.0, 1.0, 0.0, 0.0).finished(); // [3x2] + + // computing dvA/dK = dvA/dcA * dcA/dK and dVB/dK = dvB/dcB * dcB/dK + Matrix vA_H_K = v_H_c * cA_H_K; + Matrix vB_H_K = v_H_c * cB_H_K; + + // error function f = vB.T * E * vA + // H2 = df/dK = vB.T * E.T * dvA/dK + vA.T * E * dvB/dK + *H2 = vB.transpose() * E.matrix().transpose() * vA_H_K + vA.transpose() * E.matrix() * vB_H_K; + } + + error << E.error(vA, vB, H1); + + return error; + } + +public: + GTSAM_MAKE_ALIGNED_OPERATOR_NEW +}; + +}// gtsam diff --git a/gtsam/slam/tests/testEssentialMatrixWithCalibrationFactor.cpp b/gtsam/slam/tests/testEssentialMatrixWithCalibrationFactor.cpp new file mode 100644 index 000000000..b156df01e --- /dev/null +++ b/gtsam/slam/tests/testEssentialMatrixWithCalibrationFactor.cpp @@ -0,0 +1,446 @@ +/** + * @file testEssentialMatrixWithCalibrationFactor.cpp + * @brief Test EssentialMatrixWithCalibrationFactor class + * @author Ayush Baid + * @author Akshay Krishnan + * @date April 22, 2021 + */ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +using namespace std; +using namespace gtsam; + +// 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); + +// The rotation between body and camera is: +gtsam::Point3 bX(1, 0, 0), bY(0, 1, 0), bZ(0, 0, 1); +gtsam::Rot3 cRb = gtsam::Rot3(bX, bZ, -bY).inverse(); + +namespace example1 { + +const string filename = findExampleDataFile("5pointExample1.txt"); +SfmData data; +bool readOK = readBAL(filename, data); +Rot3 c1Rc2 = data.cameras[1].pose().rotation(); +Point3 c1Tc2 = data.cameras[1].pose().translation(); +Cal3Bundler trueK = data.cameras[1].calibration(); // TODO: maybe default value not good; assert with 0th +// PinholeCamera camera2(data.cameras[1].pose(), trueK); +Rot3 trueRotation(c1Rc2); +Unit3 trueDirection(c1Tc2); +EssentialMatrix trueE(trueRotation, trueDirection); + +double baseline = 0.1; // actual baseline of the camera + +Point2 pA(size_t i) { + return data.tracks[i].measurements[0].second; +} +Point2 pB(size_t i) { + return data.tracks[i].measurements[1].second; +} +Vector vA(size_t i, Cal3Bundler K) { + return EssentialMatrix::Homogeneous(K.calibrate(pA(i))); +} +Vector vB(size_t i, Cal3Bundler K) { + return EssentialMatrix::Homogeneous(K.calibrate(pB(i))); +} + +//************************************************************************* +TEST (EssentialMatrixWithCalibrationFactor, testData) { + CHECK(readOK); + + // Check E matrix + Matrix expected(3, 3); + expected << 0, 0, 0, 0, 0, -0.1, 0.1, 0, 0; + Matrix aEb_matrix = skewSymmetric(c1Tc2.x(), c1Tc2.y(), c1Tc2.z()) + * c1Rc2.matrix(); + EXPECT(assert_equal(expected, aEb_matrix, 1e-8)); + + // Check some projections + EXPECT(assert_equal(Point2(0, 0), pA(0), 1e-8)); + EXPECT(assert_equal(Point2(0, 0.1), pB(0), 1e-8)); + EXPECT(assert_equal(Point2(0, -1), pA(4), 1e-8)); + EXPECT(assert_equal(Point2(-1, 0.2), pB(4), 1e-8)); + + // Check homogeneous version + EXPECT(assert_equal(Vector3(-1, 0.2, 1), vB(4, trueK), 1e-8)); + + // check the calibration + Cal3Bundler expectedK; + EXPECT(assert_equal(expectedK, trueK)); + + + // Check epipolar constraint + for (size_t i = 0; i < 5; i++) + EXPECT_DOUBLES_EQUAL(0, vA(i, trueK).transpose() * aEb_matrix * vB(i, trueK), 1e-8); + + // Check epipolar constraint + for (size_t i = 0; i < 5; i++) + EXPECT_DOUBLES_EQUAL(0, trueE.error(vA(i, trueK), vB(i, trueK)), 1e-7); +} + +//************************************************************************* +TEST (EssentialMatrixWithCalibrationFactor, factor) { + Key keyE(1); + Key keyK(1); + for (size_t i = 0; i < 5; i++) { + EssentialMatrixWithCalibrationFactor factor(keyE, keyK, pA(i), pB(i), model1); + + // Check evaluation + Vector expected(1); + expected << 0; + Matrix HEactual; + Matrix HKactual; + Vector actual = factor.evaluateError(trueE, trueK, HEactual, HKactual); + EXPECT(assert_equal(expected, actual, 1e-7)); + + // Use numerical derivatives to calculate the expected Jacobian + Matrix HEexpected; + Matrix HKexpected; + typedef Eigen::Matrix Vector1; + // TODO: fix this + boost::function f = boost::bind( + &EssentialMatrixWithCalibrationFactor::evaluateError, factor, _1, _2, boost::none, boost::none); + HEexpected = numericalDerivative21(f, trueE, trueK); + HKexpected = numericalDerivative22(f, trueE, trueK); + + // Verify the Jacobian is correct + EXPECT(assert_equal(HEexpected, HEactual, 1e-8)); + EXPECT(assert_equal(HKexpected, HKactual, 1e-5)); + } +} + +// //************************************************************************* +// TEST(EssentialMatrixWithCalibrationFactor, ExpressionFactor) { +// Key keyE(1); +// Key keyK(2); +// for (size_t i = 0; i < 5; i++) { +// boost::function, OptionalJacobian<1, 3>)> f = +// boost::bind(&EssentialMatrix::error, _1, pA(i), pB(i), _2); +// Expression E_(keyE); // leaf expression +// Expression K_(keyK); // leaf expression +// Expression expr(f, E_, K_); // unary expression + +// // Test the derivatives using Paul's magic +// Values values; +// values.insert(keyE, trueE); +// values.insert(keyK, trueK); +// EXPECT_CORRECT_EXPRESSION_JACOBIANS(expr, values, 1e-5, 1e-9); + +// // Create the factor +// ExpressionFactor factor(model1, 0, expr); + +// // Check evaluation +// Vector expected(1); +// expected << 0; +// vector Hactual(1); +// Vector actual = factor.unwhitenedError(values, Hactual); +// EXPECT(assert_equal(expected, actual, 1e-7)); +// } +// } + +//************************************************************************* +// TEST(EssentialMatrixWithCalibrationFactor, ExpressionFactorRotationOnly) { +// Key keyE(1); +// Key keyK(1); +// for (size_t i = 0; i < 5; i++) { +// boost::function)> f = +// boost::bind(&EssentialMatrix::error, _1, vA(i), vB(i), _2); +// boost::function, +// OptionalJacobian<5, 2>)> g; +// Expression R_(key); +// Expression d_(trueDirection); +// Expression E_(&EssentialMatrix::FromRotationAndDirection, R_, d_); +// Expression expr(f, E_); + +// // Test the derivatives using Paul's magic +// Values values; +// values.insert(key, trueRotation); +// EXPECT_CORRECT_EXPRESSION_JACOBIANS(expr, values, 1e-5, 1e-9); + +// // Create the factor +// ExpressionFactor factor(model1, 0, expr); + +// // Check evaluation +// Vector expected(1); +// expected << 0; +// vector Hactual(1); +// Vector actual = factor.unwhitenedError(values, Hactual); +// EXPECT(assert_equal(expected, actual, 1e-7)); +// } +// } + +//************************************************************************* +TEST(EssentialMatrixWithCalibrationFactor, evaluateErrorJacobians) { + Key keyE(1); + Key keyK(2); + // initialize essential matrix + Rot3 r = Rot3::Expmap(Vector3(M_PI/6, M_PI / 3, M_PI/9)); + Unit3 t(Point3(2, -1, 0.5)); + EssentialMatrix E = EssentialMatrix::FromRotationAndDirection(r, t); + Cal3_S2 K(200, 1, 1, 10, 10); + Values val; + val.insert(keyE, E); + val.insert(keyK, K); + + Point2 pA(10.0, 20.0); + Point2 pB(12.0, 15.0); + + EssentialMatrixWithCalibrationFactor f(keyE, keyK, pA, pB, model1); + EXPECT_CORRECT_FACTOR_JACOBIANS(f, val, 1e-5, 1e-6); +} + +//************************************************************************* +TEST (EssentialMatrixWithCalibrationFactor, 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 + // factors. In this case, the factors are the constraints. + + // We start with a factor graph and add constraints to it + // Noise sigma is 1cm, assuming metric measurements + NonlinearFactorGraph graph; + for (size_t i = 0; i < 5; i++) + graph.emplace_shared>(1, 2, pA(i), pB(i), model1); + + // Check error at ground truth + Values truth; + truth.insert(1, trueE); + truth.insert(2, trueK); + 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).finished()); + Cal3Bundler initialK = trueK.retract( + (Vector(3) << 0.1, -1e-1, 3e-2).finished()); + initial.insert(1, initialE); + initial.insert(2, initialK); +#if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) + EXPECT_DOUBLES_EQUAL(618.94, graph.error(initial), 1e-2); +#else + EXPECT_DOUBLES_EQUAL(639.84, graph.error(initial), 1e-2); +#endif + + // Optimize + LevenbergMarquardtParams parameters; + LevenbergMarquardtOptimizer optimizer(graph, initial, parameters); + Values result = optimizer.optimize(); + + // Check result + EssentialMatrix actualE = result.at(1); + Cal3Bundler actualK = result.at(2); + EXPECT(assert_equal(trueE, actualE, 1e-1)); + EXPECT(assert_equal(trueK, actualK, 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, actualE.error(vA(i, actualK), vB(i, actualK)), 1e-6); + +} + +} // namespace example1 + +//************************************************************************* + +// namespace example2 { + +// const string filename = findExampleDataFile("5pointExample2.txt"); +// SfmData data; +// bool readOK = readBAL(filename, data); +// Rot3 aRb = data.cameras[1].pose().rotation(); +// Point3 aTb = data.cameras[1].pose().translation(); +// EssentialMatrix trueE(aRb, Unit3(aTb)); + +// double baseline = 10; // actual baseline of the camera + +// Point2 pA(size_t i) { +// return data.tracks[i].measurements[0].second; +// } +// Point2 pB(size_t i) { +// return data.tracks[i].measurements[1].second; +// } + +// boost::shared_ptr // +// K = boost::make_shared(500, 0, 0); +// PinholeCamera camera2(data.cameras[1].pose(), *K); + +// 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 (EssentialWithMatrixCalibrationFactor, extraMinimization) { +// // Additional test with camera moving in positive X direction + +// NonlinearFactorGraph graph; +// for (size_t i = 0; i < 5; i++) +// graph.emplace_shared(1, pA(i), pB(i), model1, K); + +// // Check error at ground truth +// Values truth; +// 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).finished()); +// initial.insert(1, initialE); + +// #if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) +// EXPECT_DOUBLES_EQUAL(643.26, graph.error(initial), 1e-2); +// #else +// EXPECT_DOUBLES_EQUAL(639.84, graph.error(initial), 1e-2); +// #endif + +// // 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) { +// for (size_t i = 0; i < 5; i++) { +// EssentialMatrixFactor2 factor(100, i, pA(i), pB(i), model2, K); + +// // Check evaluation +// Point3 P1 = data.tracks[i].p; +// const Point2 pi = camera2.project(P1); +// Point2 expected(pi - pB(i)); + +// Matrix Hactual1, Hactual2; +// double d(baseline / P1.z()); +// 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-6)); +// EXPECT(assert_equal(Hexpected2, Hactual2, 1e-8)); +// } +// } + +// //************************************************************************* +// TEST (EssentialMatrixFactor2, extraMinimization) { +// // Additional test with camera moving in positive X direction + +// // We start with a factor graph and add constraints to it +// // Noise sigma is 1, assuming pixel measurements +// NonlinearFactorGraph graph; +// for (size_t i = 0; i < data.number_tracks(); i++) +// graph.emplace_shared(100, i, pA(i), pB(i), model2, K); + +// // Check error at ground truth +// Values truth; +// truth.insert(100, trueE); +// for (size_t i = 0; i < data.number_tracks(); i++) { +// Point3 P1 = data.tracks[i].p; +// truth.insert(i, double(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 < data.number_tracks(); i++) +// EXPECT_DOUBLES_EQUAL(truth.at(i), result.at(i), 1e-1); + +// // Check error at result +// EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4); +// } + +// //************************************************************************* +// TEST (EssentialMatrixFactor3, extraTest) { + +// // The "true E" in the body frame is +// EssentialMatrix bodyE = cRb.inverse() * trueE; + +// for (size_t i = 0; i < 5; i++) { +// EssentialMatrixFactor3 factor(100, i, pA(i), pB(i), cRb, model2, K); + +// // Check evaluation +// Point3 P1 = data.tracks[i].p; +// const Point2 pi = camera2.project(P1); +// Point2 expected(pi - pB(i)); + +// Matrix Hactual1, Hactual2; +// double d(baseline / P1.z()); +// Vector actual = factor.evaluateError(bodyE, 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( +// &EssentialMatrixFactor3::evaluateError, &factor, _1, _2, boost::none, +// boost::none); +// Hexpected1 = numericalDerivative21(f, bodyE, d); +// Hexpected2 = numericalDerivative22(f, bodyE, d); + +// // Verify the Jacobian is correct +// EXPECT(assert_equal(Hexpected1, Hactual1, 1e-6)); +// EXPECT(assert_equal(Hexpected2, Hactual2, 1e-8)); +// } +// } + +// } // namespace example2 + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ From f60e9e93659c6f731db57723e2cfd1a6a7bb96e7 Mon Sep 17 00:00:00 2001 From: Ayush Baid Date: Sat, 24 Apr 2021 10:57:28 -0400 Subject: [PATCH 02/30] fixing tests by moving to Cal3_S2 --- .../EssentialMatrixWithCalibrationFactor.h | 62 ++++---- ...stEssentialMatrixWithCalibrationFactor.cpp | 147 ++++++++++-------- 2 files changed, 113 insertions(+), 96 deletions(-) diff --git a/gtsam/slam/EssentialMatrixWithCalibrationFactor.h b/gtsam/slam/EssentialMatrixWithCalibrationFactor.h index 17dbe98a0..568a94585 100644 --- a/gtsam/slam/EssentialMatrixWithCalibrationFactor.h +++ b/gtsam/slam/EssentialMatrixWithCalibrationFactor.h @@ -12,7 +12,8 @@ /** * @file EssentialMatrixWithCalibrationFactor.h * - * @brief A factor evaluating algebraic epipolar error with essential matrix and calibration as variables. + * @brief A factor evaluating algebraic epipolar error with essential matrix and + * calibration as variables. * * @author Ayush Baid * @author Akshay Krishnan @@ -21,38 +22,40 @@ #pragma once -#include #include +#include + #include namespace gtsam { /** - * Factor that evaluates algebraic epipolar error (K^-1 p)'E (K^-1 p) for given essential matrix and calibration shared - * between two images. + * Factor that evaluates algebraic epipolar error (K^-1 p)'E (K^-1 p) for given + * essential matrix and calibration shared between two images. */ -template -class EssentialMatrixWithCalibrationFactor: public NoiseModelFactor2 { - - Point2 pA_, pB_; ///< points in pixel coordinates +template +class EssentialMatrixWithCalibrationFactor + : public NoiseModelFactor2 { + Point2 pA_, pB_; ///< points in pixel coordinates typedef NoiseModelFactor2 Base; typedef EssentialMatrixWithCalibrationFactor This; -public: - + public: /** * Constructor * @param essentialMatrixKey Essential Matrix variable key * @param calibrationKey Calibration variable key * @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 model noise model is about dot product in ideal, homogeneous + * coordinates */ - EssentialMatrixWithCalibrationFactor(Key essentialMatrixKey, Key calibrationKey, const Point2& pA, const Point2& pB, - const SharedNoiseModel& model) : - Base(model, essentialMatrixKey, calibrationKey), pA_(pA), pB_(pB) {} - + EssentialMatrixWithCalibrationFactor(Key essentialMatrixKey, + Key calibrationKey, const Point2& pA, + const Point2& pB, + const SharedNoiseModel& model) + : Base(model, essentialMatrixKey, calibrationKey), pA_(pA), pB_(pB) {} /// @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { @@ -61,12 +64,13 @@ public: } /// print - void print(const std::string& s = "", + void print( + const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { Base::print(s); std::cout << " EssentialMatrixWithCalibrationFactor with measurements\n (" - << pA_.transpose() << ")' and (" << pB_.transpose() << ")'" - << std::endl; + << pA_.transpose() << ")' and (" << pB_.transpose() << ")'" + << std::endl; } /// vector of errors returns 1D vector @@ -79,12 +83,14 @@ public: * @param H2 optional jacobian in K * @return * Vector */ - Vector evaluateError(const EssentialMatrix& E, const CALIBRATION& K, - boost::optional H1 = boost::none, boost::optional H2 = boost::none) const override { + Vector evaluateError( + const EssentialMatrix& E, const CALIBRATION& K, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none) const override { Vector error(1); // converting from pixel coordinates to normalized coordinates cA and cB - Matrix cA_H_K; // dcA/dK - Matrix cB_H_K; // dcB/dK + Matrix cA_H_K; // dcA/dK + Matrix cB_H_K; // dcB/dK Point2 cA = K.calibrate(pA_, cA_H_K); Point2 cB = K.calibrate(pB_, cB_H_K); @@ -92,11 +98,12 @@ public: Vector3 vA = EssentialMatrix::Homogeneous(cA); Vector3 vB = EssentialMatrix::Homogeneous(cB); - if (H2){ + if (H2) { // compute the jacobian of error w.r.t K // dvX / dcX [3x2] = [1, 0], [0, 1], [0, 0] - Matrix v_H_c = (Matrix(3, 2) << 1.0, 0.0, 0.0, 1.0, 0.0, 0.0).finished(); // [3x2] + Matrix v_H_c = + (Matrix(3, 2) << 1.0, 0.0, 0.0, 1.0, 0.0, 0.0).finished(); // [3x2] // computing dvA/dK = dvA/dcA * dcA/dK and dVB/dK = dvB/dcB * dcB/dK Matrix vA_H_K = v_H_c * cA_H_K; @@ -104,7 +111,8 @@ public: // error function f = vB.T * E * vA // H2 = df/dK = vB.T * E.T * dvA/dK + vA.T * E * dvB/dK - *H2 = vB.transpose() * E.matrix().transpose() * vA_H_K + vA.transpose() * E.matrix() * vB_H_K; + *H2 = vB.transpose() * E.matrix().transpose() * vA_H_K + + vA.transpose() * E.matrix() * vB_H_K; } error << E.error(vA, vB, H1); @@ -112,8 +120,8 @@ public: return error; } -public: + public: GTSAM_MAKE_ALIGNED_OPERATOR_NEW }; -}// gtsam +} // namespace gtsam diff --git a/gtsam/slam/tests/testEssentialMatrixWithCalibrationFactor.cpp b/gtsam/slam/tests/testEssentialMatrixWithCalibrationFactor.cpp index b156df01e..4d77e1fcd 100644 --- a/gtsam/slam/tests/testEssentialMatrixWithCalibrationFactor.cpp +++ b/gtsam/slam/tests/testEssentialMatrixWithCalibrationFactor.cpp @@ -6,27 +6,25 @@ * @date April 22, 2021 */ -#include - -#include -#include -#include -#include -#include -#include -#include -#include +#include #include #include - -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include using namespace std; using namespace gtsam; // Noise model for first type of factor is evaluating algebraic error -noiseModel::Isotropic::shared_ptr model1 = noiseModel::Isotropic::Sigma(1, - 0.01); +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); @@ -41,36 +39,33 @@ SfmData data; bool readOK = readBAL(filename, data); Rot3 c1Rc2 = data.cameras[1].pose().rotation(); Point3 c1Tc2 = data.cameras[1].pose().translation(); -Cal3Bundler trueK = data.cameras[1].calibration(); // TODO: maybe default value not good; assert with 0th -// PinholeCamera camera2(data.cameras[1].pose(), trueK); +// TODO: maybe default value not good; assert with 0th +Cal3_S2 trueK = Cal3_S2(); +// PinholeCamera camera2(data.cameras[1].pose(), trueK); Rot3 trueRotation(c1Rc2); Unit3 trueDirection(c1Tc2); EssentialMatrix trueE(trueRotation, trueDirection); -double baseline = 0.1; // actual baseline of the camera +double baseline = 0.1; // actual baseline of the camera -Point2 pA(size_t i) { - return data.tracks[i].measurements[0].second; -} -Point2 pB(size_t i) { - return data.tracks[i].measurements[1].second; -} -Vector vA(size_t i, Cal3Bundler K) { +Point2 pA(size_t i) { return data.tracks[i].measurements[0].second; } +Point2 pB(size_t i) { return data.tracks[i].measurements[1].second; } +Vector vA(size_t i, Cal3_S2 K) { return EssentialMatrix::Homogeneous(K.calibrate(pA(i))); } -Vector vB(size_t i, Cal3Bundler K) { +Vector vB(size_t i, Cal3_S2 K) { return EssentialMatrix::Homogeneous(K.calibrate(pB(i))); } //************************************************************************* -TEST (EssentialMatrixWithCalibrationFactor, testData) { +TEST(EssentialMatrixWithCalibrationFactor, testData) { CHECK(readOK); // Check E matrix Matrix expected(3, 3); expected << 0, 0, 0, 0, 0, -0.1, 0.1, 0, 0; - Matrix aEb_matrix = skewSymmetric(c1Tc2.x(), c1Tc2.y(), c1Tc2.z()) - * c1Rc2.matrix(); + Matrix aEb_matrix = + skewSymmetric(c1Tc2.x(), c1Tc2.y(), c1Tc2.z()) * c1Rc2.matrix(); EXPECT(assert_equal(expected, aEb_matrix, 1e-8)); // Check some projections @@ -83,13 +78,13 @@ TEST (EssentialMatrixWithCalibrationFactor, testData) { EXPECT(assert_equal(Vector3(-1, 0.2, 1), vB(4, trueK), 1e-8)); // check the calibration - Cal3Bundler expectedK; + Cal3_S2 expectedK(1, 1, 0, 0, 0); EXPECT(assert_equal(expectedK, trueK)); - // Check epipolar constraint for (size_t i = 0; i < 5; i++) - EXPECT_DOUBLES_EQUAL(0, vA(i, trueK).transpose() * aEb_matrix * vB(i, trueK), 1e-8); + EXPECT_DOUBLES_EQUAL( + 0, vA(i, trueK).transpose() * aEb_matrix * vB(i, trueK), 1e-8); // Check epipolar constraint for (size_t i = 0; i < 5; i++) @@ -97,11 +92,12 @@ TEST (EssentialMatrixWithCalibrationFactor, testData) { } //************************************************************************* -TEST (EssentialMatrixWithCalibrationFactor, factor) { +TEST(EssentialMatrixWithCalibrationFactor, factor) { Key keyE(1); Key keyK(1); for (size_t i = 0; i < 5; i++) { - EssentialMatrixWithCalibrationFactor factor(keyE, keyK, pA(i), pB(i), model1); + EssentialMatrixWithCalibrationFactor factor(keyE, keyK, pA(i), + pB(i), model1); // Check evaluation Vector expected(1); @@ -114,16 +110,20 @@ TEST (EssentialMatrixWithCalibrationFactor, factor) { // Use numerical derivatives to calculate the expected Jacobian Matrix HEexpected; Matrix HKexpected; - typedef Eigen::Matrix Vector1; + typedef Eigen::Matrix Vector1; // TODO: fix this - boost::function f = boost::bind( - &EssentialMatrixWithCalibrationFactor::evaluateError, factor, _1, _2, boost::none, boost::none); - HEexpected = numericalDerivative21(f, trueE, trueK); - HKexpected = numericalDerivative22(f, trueE, trueK); + boost::function f = + boost::bind( + &EssentialMatrixWithCalibrationFactor::evaluateError, + factor, _1, _2, boost::none, boost::none); + HEexpected = numericalDerivative21( + f, trueE, trueK); + HKexpected = numericalDerivative22( + f, trueE, trueK); // Verify the Jacobian is correct EXPECT(assert_equal(HEexpected, HEactual, 1e-8)); - EXPECT(assert_equal(HKexpected, HKactual, 1e-5)); + EXPECT(assert_equal(HKexpected, HKactual, 1e-8)); } } @@ -132,11 +132,11 @@ TEST (EssentialMatrixWithCalibrationFactor, factor) { // Key keyE(1); // Key keyK(2); // for (size_t i = 0; i < 5; i++) { -// boost::function, OptionalJacobian<1, 3>)> f = // boost::bind(&EssentialMatrix::error, _1, pA(i), pB(i), _2); // Expression E_(keyE); // leaf expression -// Expression K_(keyK); // leaf expression +// Expression K_(keyK); // leaf expression // Expression expr(f, E_, K_); // unary expression // // Test the derivatives using Paul's magic @@ -162,13 +162,16 @@ TEST (EssentialMatrixWithCalibrationFactor, factor) { // Key keyE(1); // Key keyK(1); // for (size_t i = 0; i < 5; i++) { -// boost::function)> f = +// boost::function)> f +// = // boost::bind(&EssentialMatrix::error, _1, vA(i), vB(i), _2); -// boost::function, +// boost::function, // OptionalJacobian<5, 2>)> g; // Expression R_(key); // Expression d_(trueDirection); -// Expression E_(&EssentialMatrix::FromRotationAndDirection, R_, d_); +// Expression +// E_(&EssentialMatrix::FromRotationAndDirection, R_, d_); // Expression expr(f, E_); // // Test the derivatives using Paul's magic @@ -193,7 +196,7 @@ TEST(EssentialMatrixWithCalibrationFactor, evaluateErrorJacobians) { Key keyE(1); Key keyK(2); // initialize essential matrix - Rot3 r = Rot3::Expmap(Vector3(M_PI/6, M_PI / 3, M_PI/9)); + Rot3 r = Rot3::Expmap(Vector3(M_PI / 6, M_PI / 3, M_PI / 9)); Unit3 t(Point3(2, -1, 0.5)); EssentialMatrix E = EssentialMatrix::FromRotationAndDirection(r, t); Cal3_S2 K(200, 1, 1, 10, 10); @@ -209,7 +212,7 @@ TEST(EssentialMatrixWithCalibrationFactor, evaluateErrorJacobians) { } //************************************************************************* -TEST (EssentialMatrixWithCalibrationFactor, minimization) { +TEST(EssentialMatrixWithCalibrationFactor, 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 @@ -219,7 +222,8 @@ TEST (EssentialMatrixWithCalibrationFactor, minimization) { // Noise sigma is 1cm, assuming metric measurements NonlinearFactorGraph graph; for (size_t i = 0; i < 5; i++) - graph.emplace_shared>(1, 2, pA(i), pB(i), model1); + graph.emplace_shared>( + 1, 2, pA(i), pB(i), model1); // Check error at ground truth Values truth; @@ -229,16 +233,17 @@ TEST (EssentialMatrixWithCalibrationFactor, minimization) { // Check error at initial estimate Values initial; - EssentialMatrix initialE = trueE.retract( - (Vector(5) << 0.1, -0.1, 0.1, 0.1, -0.1).finished()); - Cal3Bundler initialK = trueK.retract( - (Vector(3) << 0.1, -1e-1, 3e-2).finished()); + EssentialMatrix initialE = + trueE.retract((Vector(5) << 0.1, -0.1, 0.1, 0.1, -0.1).finished()); + Cal3_S2 initialK = + trueK.retract((Vector(5) << 0.1, -0.1, 0.03, -0.2, 0.2).finished()); initial.insert(1, initialE); - initial.insert(2, initialK); + initial.insert(2, trueK); #if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) - EXPECT_DOUBLES_EQUAL(618.94, graph.error(initial), 1e-2); + EXPECT_DOUBLES_EQUAL(643.26, graph.error(initial), 1e-2); #else - EXPECT_DOUBLES_EQUAL(639.84, graph.error(initial), 1e-2); + EXPECT_DOUBLES_EQUAL(639.84, graph.error(initial), + 1e-2); // TODO: update this value too #endif // Optimize @@ -248,20 +253,20 @@ TEST (EssentialMatrixWithCalibrationFactor, minimization) { // Check result EssentialMatrix actualE = result.at(1); - Cal3Bundler actualK = result.at(2); - EXPECT(assert_equal(trueE, actualE, 1e-1)); - EXPECT(assert_equal(trueK, actualK, 1e-1)); + Cal3_S2 actualK = result.at(2); + EXPECT(assert_equal(trueE, actualE, 1e-2)); + EXPECT(assert_equal(trueK, actualK, 1e-2)); // 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, actualE.error(vA(i, actualK), vB(i, actualK)), 1e-6); - + EXPECT_DOUBLES_EQUAL(0, actualE.error(vA(i, actualK), vB(i, actualK)), + 1e-6); } -} // namespace example1 +} // namespace example1 //************************************************************************* @@ -283,9 +288,9 @@ TEST (EssentialMatrixWithCalibrationFactor, minimization) { // return data.tracks[i].measurements[1].second; // } -// boost::shared_ptr // -// K = boost::make_shared(500, 0, 0); -// PinholeCamera camera2(data.cameras[1].pose(), *K); +// boost::shared_ptr // +// K = boost::make_shared(500, 0, 0); +// PinholeCamera camera2(data.cameras[1].pose(), *K); // Vector vA(size_t i) { // Point2 xy = K->calibrate(pA(i)); @@ -302,7 +307,8 @@ TEST (EssentialMatrixWithCalibrationFactor, minimization) { // NonlinearFactorGraph graph; // for (size_t i = 0; i < 5; i++) -// graph.emplace_shared(1, pA(i), pB(i), model1, K); +// graph.emplace_shared(1, pA(i), +// pB(i), model1, K); // // Check error at ground truth // Values truth; @@ -359,8 +365,9 @@ TEST (EssentialMatrixWithCalibrationFactor, minimization) { // boost::function f = boost::bind( // &EssentialMatrixFactor2::evaluateError, &factor, _1, _2, boost::none, // boost::none); -// Hexpected1 = numericalDerivative21(f, trueE, d); -// Hexpected2 = numericalDerivative22(f, trueE, d); +// Hexpected1 = numericalDerivative21(f, +// trueE, d); Hexpected2 = numericalDerivative22(f, trueE, d); // // Verify the Jacobian is correct // EXPECT(assert_equal(Hexpected1, Hactual1, 1e-6)); @@ -376,7 +383,8 @@ TEST (EssentialMatrixWithCalibrationFactor, minimization) { // // Noise sigma is 1, assuming pixel measurements // NonlinearFactorGraph graph; // for (size_t i = 0; i < data.number_tracks(); i++) -// graph.emplace_shared(100, i, pA(i), pB(i), model2, K); +// graph.emplace_shared(100, i, pA(i), pB(i), +// model2, K); // // Check error at ground truth // Values truth; @@ -427,8 +435,9 @@ TEST (EssentialMatrixWithCalibrationFactor, minimization) { // boost::function f = boost::bind( // &EssentialMatrixFactor3::evaluateError, &factor, _1, _2, boost::none, // boost::none); -// Hexpected1 = numericalDerivative21(f, bodyE, d); -// Hexpected2 = numericalDerivative22(f, bodyE, d); +// Hexpected1 = numericalDerivative21(f, +// bodyE, d); Hexpected2 = numericalDerivative22(f, bodyE, d); // // Verify the Jacobian is correct // EXPECT(assert_equal(Hexpected1, Hactual1, 1e-6)); From 64ff24b656cfca9633f7088a32fe20b6ea5fd140 Mon Sep 17 00:00:00 2001 From: Ayush Baid Date: Mon, 26 Apr 2021 20:50:22 -0400 Subject: [PATCH 03/30] using fixed size matrix, and adding jacobian in homogeneous conversion --- gtsam/geometry/EssentialMatrix.h | 10 +- gtsam/geometry/tests/testEssentialMatrix.cpp | 12 + gtsam/slam/EssentialMatrixFactor.h | 243 +++++++--- .../EssentialMatrixWithCalibrationFactor.h | 127 ----- .../slam/tests/testEssentialMatrixFactor.cpp | 177 ++++++- ...stEssentialMatrixWithCalibrationFactor.cpp | 455 ------------------ 6 files changed, 358 insertions(+), 666 deletions(-) delete mode 100644 gtsam/slam/EssentialMatrixWithCalibrationFactor.h delete mode 100644 gtsam/slam/tests/testEssentialMatrixWithCalibrationFactor.cpp diff --git a/gtsam/geometry/EssentialMatrix.h b/gtsam/geometry/EssentialMatrix.h index 909576aa0..5eb11f8ed 100644 --- a/gtsam/geometry/EssentialMatrix.h +++ b/gtsam/geometry/EssentialMatrix.h @@ -7,10 +7,10 @@ #pragma once +#include +#include #include #include -#include -#include #include #include @@ -31,7 +31,11 @@ class EssentialMatrix { public: /// Static function to convert Point2 to homogeneous coordinates - static Vector3 Homogeneous(const Point2& p) { + static Vector3 Homogeneous(const Point2& p, + OptionalJacobian<3, 2> H = boost::none) { + if (H) { + H->setIdentity(); + } return Vector3(p.x(), p.y(), 1); } diff --git a/gtsam/geometry/tests/testEssentialMatrix.cpp b/gtsam/geometry/tests/testEssentialMatrix.cpp index 86a498cdc..fd3fb64f0 100644 --- a/gtsam/geometry/tests/testEssentialMatrix.cpp +++ b/gtsam/geometry/tests/testEssentialMatrix.cpp @@ -241,6 +241,18 @@ TEST (EssentialMatrix, epipoles) { EXPECT(assert_equal(e2, E.epipole_b())); } +//************************************************************************* +TEST(EssentialMatrix, Homogeneous) { + Point2 input(5.0, 1.3); + Vector3 expected(5.0, 1.3, 1.0); + Matrix32 expectedH; + expectedH << 1.0, 0.0, 0.0, 1.0, 0.0, 0.0; + Matrix32 actualH; + Vector3 actual = EssentialMatrix::Homogeneous(input, actualH); + EXPECT(assert_equal(actual, expected)); + EXPECT(assert_equal(actualH, expectedH)); +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/slam/EssentialMatrixFactor.h b/gtsam/slam/EssentialMatrixFactor.h index a99ac9512..786b9596b 100644 --- a/gtsam/slam/EssentialMatrixFactor.h +++ b/gtsam/slam/EssentialMatrixFactor.h @@ -7,9 +7,10 @@ #pragma once -#include #include #include +#include + #include namespace gtsam { @@ -17,25 +18,24 @@ namespace gtsam { /** * Factor that evaluates epipolar error p'Ep for given essential matrix */ -class EssentialMatrixFactor: public NoiseModelFactor1 { - - Vector3 vA_, vB_; ///< Homogeneous versions, in ideal coordinates +class EssentialMatrixFactor : public NoiseModelFactor1 { + Vector3 vA_, vB_; ///< Homogeneous versions, in ideal coordinates typedef NoiseModelFactor1 Base; typedef EssentialMatrixFactor This; -public: - + public: /** * Constructor * @param key Essential Matrix variable key * @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 + * @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) { + const SharedNoiseModel& model) + : Base(model, key) { vA_ = EssentialMatrix::Homogeneous(pA); vB_ = EssentialMatrix::Homogeneous(pB); } @@ -45,13 +45,15 @@ public: * @param key Essential Matrix variable key * @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 model noise model is about dot product in ideal, homogeneous + * coordinates * @param K calibration object, will be used only in constructor */ - template + template EssentialMatrixFactor(Key key, const Point2& pA, const Point2& pB, - const SharedNoiseModel& model, boost::shared_ptr K) : - Base(model, key) { + const SharedNoiseModel& model, + boost::shared_ptr K) + : Base(model, key) { assert(K); vA_ = EssentialMatrix::Homogeneous(K->calibrate(pA)); vB_ = EssentialMatrix::Homogeneous(K->calibrate(pB)); @@ -64,23 +66,25 @@ public: } /// print - void print(const std::string& s = "", + void print( + const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { Base::print(s); std::cout << " EssentialMatrixFactor with measurements\n (" - << vA_.transpose() << ")' and (" << vB_.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 override { + Vector evaluateError( + const EssentialMatrix& E, + boost::optional H = boost::none) const override { Vector error(1); error << E.error(vA_, vB_, H); return error; } -public: + public: GTSAM_MAKE_ALIGNED_OPERATOR_NEW }; @@ -88,17 +92,16 @@ public: * Binary factor that optimizes for E and inverse depth d: assumes measurement * in image 2 is perfect, and returns re-projection error in image 1 */ -class EssentialMatrixFactor2: public NoiseModelFactor2 { - - Point3 dP1_; ///< 3D point corresponding to measurement in image 1 - Point2 pn_; ///< Measurement in image 2, in ideal coordinates - double f_; ///< approximate conversion factor for error scaling +class EssentialMatrixFactor2 + : public NoiseModelFactor2 { + Point3 dP1_; ///< 3D point corresponding to measurement in image 1 + Point2 pn_; ///< Measurement in image 2, in ideal coordinates + double f_; ///< approximate conversion factor for error scaling typedef NoiseModelFactor2 Base; typedef EssentialMatrixFactor2 This; -public: - + public: /** * Constructor * @param key1 Essential Matrix variable key @@ -108,8 +111,10 @@ public: * @param model noise model should be in pixels, as well */ EssentialMatrixFactor2(Key key1, Key key2, const Point2& pA, const Point2& pB, - const SharedNoiseModel& model) : - Base(model, key1, key2), dP1_(EssentialMatrix::Homogeneous(pA)), pn_(pB) { + const SharedNoiseModel& model) + : Base(model, key1, key2), + dP1_(EssentialMatrix::Homogeneous(pA)), + pn_(pB) { f_ = 1.0; } @@ -122,11 +127,13 @@ public: * @param K calibration object, will be used only in constructor * @param model noise model should be in pixels, as well */ - template + template EssentialMatrixFactor2(Key key1, Key key2, const Point2& pA, const Point2& pB, - const SharedNoiseModel& model, boost::shared_ptr K) : - Base(model, key1, key2), dP1_( - EssentialMatrix::Homogeneous(K->calibrate(pA))), pn_(K->calibrate(pB)) { + const SharedNoiseModel& model, + boost::shared_ptr K) + : Base(model, key1, key2), + dP1_(EssentialMatrix::Homogeneous(K->calibrate(pA))), + pn_(K->calibrate(pB)) { f_ = 0.5 * (K->fx() + K->fy()); } @@ -137,12 +144,13 @@ public: } /// print - void print(const std::string& s = "", + void print( + const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { Base::print(s); std::cout << " EssentialMatrixFactor2 with measurements\n (" - << dP1_.transpose() << ")' and (" << pn_.transpose() - << ")'" << std::endl; + << dP1_.transpose() << ")' and (" << pn_.transpose() << ")'" + << std::endl; } /* @@ -150,30 +158,28 @@ public: * @param E essential matrix * @param d inverse depth d */ - Vector evaluateError(const EssentialMatrix& E, const double& d, - boost::optional DE = boost::none, boost::optional Dd = - boost::none) const override { - + Vector evaluateError( + const EssentialMatrix& E, const double& d, + boost::optional DE = boost::none, + boost::optional Dd = boost::none) const override { // 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 second camera by P2 = 1R2'*(P1-1T2) // The homogeneous coordinates of can be written as // 2R1*(P1-1T2) == 2R1*d*(P1-1T2) == 2R1*((x,y,1)-d*1T2) - // where we multiplied with d which yields equivalent homogeneous coordinates. - // Note that this is just the homography 2R1 for d==0 - // The point d*P1 = (x,y,1) is computed in constructor as dP1_ + // where we multiplied with d which yields equivalent homogeneous + // coordinates. Note that this is just the homography 2R1 for d==0 The point + // d*P1 = (x,y,1) is computed in constructor as dP1_ // Project to normalized image coordinates, then uncalibrate - Point2 pn(0,0); + Point2 pn(0, 0); if (!DE && !Dd) { - Point3 _1T2 = E.direction().point3(); Point3 d1T2 = d * _1T2; - Point3 dP2 = E.rotation().unrotate(dP1_ - d1T2); // 2R1*((x,y,1)-d*1T2) + Point3 dP2 = E.rotation().unrotate(dP1_ - d1T2); // 2R1*((x,y,1)-d*1T2) pn = PinholeBase::Project(dP2); } else { - // Calculate derivatives. TODO if slow: optimize with Mathematica // 3*2 3*3 3*3 Matrix D_1T2_dir, DdP2_rot, DP2_point; @@ -187,20 +193,19 @@ public: if (DE) { Matrix DdP2_E(3, 5); - DdP2_E << DdP2_rot, -DP2_point * d * D_1T2_dir; // (3*3), (3*3) * (3*2) - *DE = f_ * Dpn_dP2 * DdP2_E; // (2*3) * (3*5) + DdP2_E << DdP2_rot, -DP2_point * d * D_1T2_dir; // (3*3), (3*3) * (3*2) + *DE = f_ * Dpn_dP2 * DdP2_E; // (2*3) * (3*5) } - if (Dd) // efficient backwards computation: + if (Dd) // efficient backwards computation: // (2*3) * (3*3) * (3*1) *Dd = -f_ * (Dpn_dP2 * (DP2_point * _1T2)); - } Point2 reprojectionError = pn - pn_; return f_ * reprojectionError; } -public: + public: GTSAM_MAKE_ALIGNED_OPERATOR_NEW }; // EssentialMatrixFactor2 @@ -210,15 +215,13 @@ public: * in image 2 is perfect, and returns re-projection error in image 1 * This version takes an extrinsic rotation to allow for omni-directional rigs */ -class EssentialMatrixFactor3: public EssentialMatrixFactor2 { - +class EssentialMatrixFactor3 : public EssentialMatrixFactor2 { typedef EssentialMatrixFactor2 Base; typedef EssentialMatrixFactor3 This; - Rot3 cRb_; ///< Rotation from body to camera frame - -public: + Rot3 cRb_; ///< Rotation from body to camera frame + public: /** * Constructor * @param key1 Essential Matrix variable key @@ -229,9 +232,8 @@ public: * @param model noise model should be in calibrated coordinates, as well */ EssentialMatrixFactor3(Key key1, Key key2, const Point2& pA, const Point2& pB, - const Rot3& cRb, const SharedNoiseModel& model) : - EssentialMatrixFactor2(key1, key2, pA, pB, model), cRb_(cRb) { - } + const Rot3& cRb, const SharedNoiseModel& model) + : EssentialMatrixFactor2(key1, key2, pA, pB, model), cRb_(cRb) {} /** * Constructor @@ -242,12 +244,11 @@ public: * @param K calibration object, will be used only in constructor * @param model noise model should be in pixels, as well */ - template + template EssentialMatrixFactor3(Key key1, Key key2, const Point2& pA, const Point2& pB, - const Rot3& cRb, const SharedNoiseModel& model, - boost::shared_ptr K) : - EssentialMatrixFactor2(key1, key2, pA, pB, model, K), cRb_(cRb) { - } + const Rot3& cRb, const SharedNoiseModel& model, + boost::shared_ptr K) + : EssentialMatrixFactor2(key1, key2, pA, pB, model, K), cRb_(cRb) {} /// @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { @@ -256,7 +257,8 @@ public: } /// print - void print(const std::string& s = "", + void print( + const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { Base::print(s); std::cout << " EssentialMatrixFactor3 with rotation " << cRb_ << std::endl; @@ -267,9 +269,10 @@ public: * @param E essential matrix * @param d inverse depth d */ - Vector evaluateError(const EssentialMatrix& E, const double& d, - boost::optional DE = boost::none, boost::optional Dd = - boost::none) const override { + Vector evaluateError( + const EssentialMatrix& E, const double& d, + boost::optional DE = boost::none, + boost::optional Dd = boost::none) const override { if (!DE) { // Convert E from body to camera frame EssentialMatrix cameraE = cRb_ * E; @@ -277,18 +280,114 @@ public: return Base::evaluateError(cameraE, d, boost::none, Dd); } else { // Version with derivatives - Matrix D_e_cameraE, D_cameraE_E; // 2*5, 5*5 + Matrix D_e_cameraE, D_cameraE_E; // 2*5, 5*5 EssentialMatrix cameraE = E.rotate(cRb_, D_cameraE_E); Vector e = Base::evaluateError(cameraE, d, D_e_cameraE, Dd); - *DE = D_e_cameraE * D_cameraE_E; // (2*5) * (5*5) + *DE = D_e_cameraE * D_cameraE_E; // (2*5) * (5*5) return e; } } -public: + public: GTSAM_MAKE_ALIGNED_OPERATOR_NEW }; // EssentialMatrixFactor3 -}// gtsam +/** + * Factor that evaluates algebraic epipolar error (K^-1 p)'E (K^-1 p) for given + * essential matrix and calibration. The calibration is shared between two + * images. + */ +template +class EssentialMatrixFactor4 + : public NoiseModelFactor2 { + private: + Point2 pA_, pB_; ///< points in pixel coordinates + typedef NoiseModelFactor2 Base; + typedef EssentialMatrixFactor4 This; + + static const int DimK = FixedDimension::value; + typedef Eigen::Matrix JacobianCalibration; + + public: + /** + * Constructor + * @param essentialMatrixKey Essential Matrix variable key + * @param calibrationKey Calibration variable key + * @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 + */ + EssentialMatrixFactor4(Key essentialMatrixKey, Key calibrationKey, + const Point2& pA, const Point2& pB, + const SharedNoiseModel& model) + : Base(model, essentialMatrixKey, calibrationKey), pA_(pA), pB_(pB) {} + + /// @return a deep copy of this factor + gtsam::NonlinearFactor::shared_ptr clone() const override { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); + } + + /// print + void print( + const std::string& s = "", + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { + Base::print(s); + std::cout << " EssentialMatrixFactor4 with measurements\n (" + << pA_.transpose() << ")' and (" << pB_.transpose() << ")'" + << std::endl; + } + + /// vector of errors returns 1D vector + /** + * @brief Calculate the algebraic epipolar error p' (K^-1)' E K p. + * + * @param E essential matrix for key essentialMatrixKey + * @param K calibration (common for both images) for key calibrationKey + * @param H1 optional jacobian in E + * @param H2 optional jacobian in K + * @return * Vector + */ + Vector evaluateError( + const EssentialMatrix& E, const CALIBRATION& K, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none) const override { + Vector error(1); + // converting from pixel coordinates to normalized coordinates cA and cB + JacobianCalibration cA_H_K; // dcA/dK + JacobianCalibration cB_H_K; // dcB/dK + Point2 cA = K.calibrate(pA_, H2 ? &cA_H_K : 0); + Point2 cB = K.calibrate(pB_, H2 ? &cB_H_K : 0); + + // Homogeneous the coordinates + Matrix32 vA_H_cA, vB_H_cB; + Vector3 vA = EssentialMatrix::Homogeneous(cA, H2 ? &vA_H_cA : 0); + Vector3 vB = EssentialMatrix::Homogeneous(cB, H2 ? &vB_H_cB : 0); + + if (H2) { + // compute the jacobian of error w.r.t K + + // using dvA/dK = dvA/dcA * dcA/dK and dVB/dK = dvB/dcB * dcB/dK + // Matrix vA_H_K = vA_H_cA * cA_H_K; + // Matrix vB_H_K = vB_H_cB * cB_H_K; + + // error function f = vA.T * E * vB + // H2 = df/dK = vB.T * E.T * dvA/dK + vA.T * E * dvB/dK + *H2 = vB.transpose() * E.matrix().transpose() * vA_H_cA * cA_H_K + + vA.transpose() * E.matrix() * vB_H_cB * cB_H_K; + } + + error << E.error(vA, vB, H1); + + return error; + } + + public: + GTSAM_MAKE_ALIGNED_OPERATOR_NEW +}; +// EssentialMatrixFactor4 + +} // namespace gtsam diff --git a/gtsam/slam/EssentialMatrixWithCalibrationFactor.h b/gtsam/slam/EssentialMatrixWithCalibrationFactor.h deleted file mode 100644 index 568a94585..000000000 --- a/gtsam/slam/EssentialMatrixWithCalibrationFactor.h +++ /dev/null @@ -1,127 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file EssentialMatrixWithCalibrationFactor.h - * - * @brief A factor evaluating algebraic epipolar error with essential matrix and - * calibration as variables. - * - * @author Ayush Baid - * @author Akshay Krishnan - * @date April 23, 2021 - */ - -#pragma once - -#include -#include - -#include - -namespace gtsam { - -/** - * Factor that evaluates algebraic epipolar error (K^-1 p)'E (K^-1 p) for given - * essential matrix and calibration shared between two images. - */ -template -class EssentialMatrixWithCalibrationFactor - : public NoiseModelFactor2 { - Point2 pA_, pB_; ///< points in pixel coordinates - - typedef NoiseModelFactor2 Base; - typedef EssentialMatrixWithCalibrationFactor This; - - public: - /** - * Constructor - * @param essentialMatrixKey Essential Matrix variable key - * @param calibrationKey Calibration variable key - * @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 - */ - EssentialMatrixWithCalibrationFactor(Key essentialMatrixKey, - Key calibrationKey, const Point2& pA, - const Point2& pB, - const SharedNoiseModel& model) - : Base(model, essentialMatrixKey, calibrationKey), pA_(pA), pB_(pB) {} - - /// @return a deep copy of this factor - gtsam::NonlinearFactor::shared_ptr clone() const override { - return boost::static_pointer_cast( - gtsam::NonlinearFactor::shared_ptr(new This(*this))); - } - - /// print - void print( - const std::string& s = "", - const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { - Base::print(s); - std::cout << " EssentialMatrixWithCalibrationFactor with measurements\n (" - << pA_.transpose() << ")' and (" << pB_.transpose() << ")'" - << std::endl; - } - - /// vector of errors returns 1D vector - /** - * @brief Calculate the algebraic epipolar error p' (K^-1)' E K p. - * - * @param E essential matrix for key essentialMatrixKey - * @param K calibration (common for both images) for key calibrationKey - * @param H1 optional jacobian in E - * @param H2 optional jacobian in K - * @return * Vector - */ - Vector evaluateError( - const EssentialMatrix& E, const CALIBRATION& K, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const override { - Vector error(1); - // converting from pixel coordinates to normalized coordinates cA and cB - Matrix cA_H_K; // dcA/dK - Matrix cB_H_K; // dcB/dK - Point2 cA = K.calibrate(pA_, cA_H_K); - Point2 cB = K.calibrate(pB_, cB_H_K); - - // Homogeneous the coordinates - Vector3 vA = EssentialMatrix::Homogeneous(cA); - Vector3 vB = EssentialMatrix::Homogeneous(cB); - - if (H2) { - // compute the jacobian of error w.r.t K - - // dvX / dcX [3x2] = [1, 0], [0, 1], [0, 0] - Matrix v_H_c = - (Matrix(3, 2) << 1.0, 0.0, 0.0, 1.0, 0.0, 0.0).finished(); // [3x2] - - // computing dvA/dK = dvA/dcA * dcA/dK and dVB/dK = dvB/dcB * dcB/dK - Matrix vA_H_K = v_H_c * cA_H_K; - Matrix vB_H_K = v_H_c * cB_H_K; - - // error function f = vB.T * E * vA - // H2 = df/dK = vB.T * E.T * dvA/dK + vA.T * E * dvB/dK - *H2 = vB.transpose() * E.matrix().transpose() * vA_H_K + - vA.transpose() * E.matrix() * vB_H_K; - } - - error << E.error(vA, vB, H1); - - return error; - } - - public: - GTSAM_MAKE_ALIGNED_OPERATOR_NEW -}; - -} // namespace gtsam diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp index 95db611d7..3a53157f6 100644 --- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp +++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp @@ -39,7 +39,9 @@ SfmData data; bool readOK = readBAL(filename, data); Rot3 c1Rc2 = data.cameras[1].pose().rotation(); Point3 c1Tc2 = data.cameras[1].pose().translation(); -PinholeCamera camera2(data.cameras[1].pose(), Cal3_S2()); +// TODO: maybe default value not good; assert with 0th +Cal3_S2 trueK = Cal3_S2(); +PinholeCamera camera2(data.cameras[1].pose(), trueK); Rot3 trueRotation(c1Rc2); Unit3 trueDirection(c1Tc2); EssentialMatrix trueE(trueRotation, trueDirection); @@ -351,7 +353,112 @@ TEST (EssentialMatrixFactor3, minimization) { EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4); } -} // namespace example1 +//************************************************************************* +TEST(EssentialMatrixFactor4, factor) { + Key keyE(1); + Key keyK(1); + for (size_t i = 0; i < 5; i++) { + EssentialMatrixFactor4 factor(keyE, keyK, pA(i), pB(i), model1); + + // Check evaluation + Vector1 expected; + expected << 0; + Matrix HEactual; + Matrix HKactual; + Vector actual = factor.evaluateError(trueE, trueK, HEactual, HKactual); + EXPECT(assert_equal(expected, actual, 1e-7)); + + // Use numerical derivatives to calculate the expected Jacobian + Matrix HEexpected; + Matrix HKexpected; + typedef Eigen::Matrix Vector1; + boost::function f = + boost::bind(&EssentialMatrixFactor4::evaluateError, factor, _1, + _2, boost::none, boost::none); + HEexpected = numericalDerivative21( + f, trueE, trueK); + HKexpected = numericalDerivative22( + f, trueE, trueK); + + // Verify the Jacobian is correct + EXPECT(assert_equal(HEexpected, HEactual, 1e-8)); + EXPECT(assert_equal(HKexpected, HKactual, 1e-8)); + } +} + +//************************************************************************* +TEST(EssentialMatrixFactor4, evaluateErrorJacobians) { + Key keyE(1); + Key keyK(2); + // initialize essential matrix + Rot3 r = Rot3::Expmap(Vector3(M_PI / 6, M_PI / 3, M_PI / 9)); + Unit3 t(Point3(2, -1, 0.5)); + EssentialMatrix E = EssentialMatrix::FromRotationAndDirection(r, t); + Cal3_S2 K(200, 1, 1, 10, 10); + Values val; + val.insert(keyE, E); + val.insert(keyK, K); + + Point2 pA(10.0, 20.0); + Point2 pB(12.0, 15.0); + + EssentialMatrixFactor4 f(keyE, keyK, pA, pB, model1); + EXPECT_CORRECT_FACTOR_JACOBIANS(f, val, 1e-5, 1e-6); +} + +//************************************************************************* +TEST(EssentialMatrixFactor4, minimization) { + // As before, we start with a factor graph and add constraints to it + NonlinearFactorGraph graph; + for (size_t i = 0; i < 5; i++) + graph.emplace_shared>(1, 2, pA(i), pB(i), + model1); + + // Check error at ground truth + Values truth; + truth.insert(1, trueE); + truth.insert(2, trueK); + 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).finished()); + Cal3_S2 initialK = + trueK.retract((Vector(5) << 0.1, -0.1, 0.03, -0.2, 0.2).finished()); + initial.insert(1, initialE); + initial.insert(2, trueK); +#if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) + EXPECT_DOUBLES_EQUAL(643.26, graph.error(initial), 1e-2); +#else + EXPECT_DOUBLES_EQUAL(639.84, graph.error(initial), + 1e-2); // TODO: update this value too +#endif + + // Optimize + LevenbergMarquardtParams parameters; + LevenbergMarquardtOptimizer optimizer(graph, initial, parameters); + Values result = optimizer.optimize(); + + // Check result + EssentialMatrix actualE = result.at(1); + Cal3_S2 actualK = result.at(2); + EXPECT(assert_equal(trueE, actualE, 1e-1)); // TODO: fix the tolerance + EXPECT(assert_equal(trueK, actualK, 1e-1)); // TODO: fix the tolerance + + // 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, + actualE.error(EssentialMatrix::Homogeneous(actualK.calibrate(pA(i))), + EssentialMatrix::Homogeneous(actualK.calibrate(pB(i)))), + 1e-6); +} + +} // namespace example1 //************************************************************************* @@ -373,21 +480,21 @@ Point2 pB(size_t i) { return data.tracks[i].measurements[1].second; } -boost::shared_ptr // -K = boost::make_shared(500, 0, 0); -PinholeCamera camera2(data.cameras[1].pose(), *K); +Cal3Bundler trueK = Cal3Bundler(500, 0, 0); +boost::shared_ptr K = boost::make_shared(trueK); +PinholeCamera camera2(data.cameras[1].pose(), trueK); Vector vA(size_t i) { - Point2 xy = K->calibrate(pA(i)); + Point2 xy = trueK.calibrate(pA(i)); return EssentialMatrix::Homogeneous(xy); } Vector vB(size_t i) { - Point2 xy = K->calibrate(pB(i)); + Point2 xy = trueK.calibrate(pB(i)); return EssentialMatrix::Homogeneous(xy); } //************************************************************************* -TEST (EssentialMatrixFactor, extraMinimization) { +TEST(EssentialMatrixFactor, extraMinimization) { // Additional test with camera moving in positive X direction NonlinearFactorGraph graph; @@ -526,7 +633,59 @@ TEST (EssentialMatrixFactor3, extraTest) { } } -} // namespace example2 +TEST(EssentialMatrixFactor4, extraMinimization) { + // Additional test with camera moving in positive X direction + + NonlinearFactorGraph graph; + for (size_t i = 0; i < 5; i++) + graph.emplace_shared>(1, 2, pA(i), + pB(i), model1); + + // Check error at ground truth + Values truth; + truth.insert(1, trueE); + truth.insert(2, trueK); + 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).finished()); + Cal3Bundler initialK = + trueK.retract((Vector(3) << 0.1, -0.02, 0.03).finished()); + initial.insert(1, initialE); + initial.insert(2, initialK); + +#if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) + EXPECT_DOUBLES_EQUAL(633.71, graph.error(initial), 1e-2); +#else + EXPECT_DOUBLES_EQUAL(639.84, graph.error(initial), 1e-2); // TODO: fix this +#endif + + // Optimize + LevenbergMarquardtParams parameters; + LevenbergMarquardtOptimizer optimizer(graph, initial, parameters); + Values result = optimizer.optimize(); + + // Check result + EssentialMatrix actualE = result.at(1); + Cal3Bundler actualK = result.at(2); + EXPECT(assert_equal(trueE, actualE, 1e-1)); // TODO: tighten tolerance + EXPECT(assert_equal(trueK, actualK, 1e-1)); // TODO: tighten tolerance + + // 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, + actualE.error(EssentialMatrix::Homogeneous(actualK.calibrate(pA(i))), + EssentialMatrix::Homogeneous(actualK.calibrate(pB(i)))), + 1e-6); +} + +} // namespace example2 /* ************************************************************************* */ int main() { diff --git a/gtsam/slam/tests/testEssentialMatrixWithCalibrationFactor.cpp b/gtsam/slam/tests/testEssentialMatrixWithCalibrationFactor.cpp deleted file mode 100644 index 4d77e1fcd..000000000 --- a/gtsam/slam/tests/testEssentialMatrixWithCalibrationFactor.cpp +++ /dev/null @@ -1,455 +0,0 @@ -/** - * @file testEssentialMatrixWithCalibrationFactor.cpp - * @brief Test EssentialMatrixWithCalibrationFactor class - * @author Ayush Baid - * @author Akshay Krishnan - * @date April 22, 2021 - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -using namespace std; -using namespace gtsam; - -// 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); - -// The rotation between body and camera is: -gtsam::Point3 bX(1, 0, 0), bY(0, 1, 0), bZ(0, 0, 1); -gtsam::Rot3 cRb = gtsam::Rot3(bX, bZ, -bY).inverse(); - -namespace example1 { - -const string filename = findExampleDataFile("5pointExample1.txt"); -SfmData data; -bool readOK = readBAL(filename, data); -Rot3 c1Rc2 = data.cameras[1].pose().rotation(); -Point3 c1Tc2 = data.cameras[1].pose().translation(); -// TODO: maybe default value not good; assert with 0th -Cal3_S2 trueK = Cal3_S2(); -// PinholeCamera camera2(data.cameras[1].pose(), trueK); -Rot3 trueRotation(c1Rc2); -Unit3 trueDirection(c1Tc2); -EssentialMatrix trueE(trueRotation, trueDirection); - -double baseline = 0.1; // actual baseline of the camera - -Point2 pA(size_t i) { return data.tracks[i].measurements[0].second; } -Point2 pB(size_t i) { return data.tracks[i].measurements[1].second; } -Vector vA(size_t i, Cal3_S2 K) { - return EssentialMatrix::Homogeneous(K.calibrate(pA(i))); -} -Vector vB(size_t i, Cal3_S2 K) { - return EssentialMatrix::Homogeneous(K.calibrate(pB(i))); -} - -//************************************************************************* -TEST(EssentialMatrixWithCalibrationFactor, testData) { - CHECK(readOK); - - // Check E matrix - Matrix expected(3, 3); - expected << 0, 0, 0, 0, 0, -0.1, 0.1, 0, 0; - Matrix aEb_matrix = - skewSymmetric(c1Tc2.x(), c1Tc2.y(), c1Tc2.z()) * c1Rc2.matrix(); - EXPECT(assert_equal(expected, aEb_matrix, 1e-8)); - - // Check some projections - EXPECT(assert_equal(Point2(0, 0), pA(0), 1e-8)); - EXPECT(assert_equal(Point2(0, 0.1), pB(0), 1e-8)); - EXPECT(assert_equal(Point2(0, -1), pA(4), 1e-8)); - EXPECT(assert_equal(Point2(-1, 0.2), pB(4), 1e-8)); - - // Check homogeneous version - EXPECT(assert_equal(Vector3(-1, 0.2, 1), vB(4, trueK), 1e-8)); - - // check the calibration - Cal3_S2 expectedK(1, 1, 0, 0, 0); - EXPECT(assert_equal(expectedK, trueK)); - - // Check epipolar constraint - for (size_t i = 0; i < 5; i++) - EXPECT_DOUBLES_EQUAL( - 0, vA(i, trueK).transpose() * aEb_matrix * vB(i, trueK), 1e-8); - - // Check epipolar constraint - for (size_t i = 0; i < 5; i++) - EXPECT_DOUBLES_EQUAL(0, trueE.error(vA(i, trueK), vB(i, trueK)), 1e-7); -} - -//************************************************************************* -TEST(EssentialMatrixWithCalibrationFactor, factor) { - Key keyE(1); - Key keyK(1); - for (size_t i = 0; i < 5; i++) { - EssentialMatrixWithCalibrationFactor factor(keyE, keyK, pA(i), - pB(i), model1); - - // Check evaluation - Vector expected(1); - expected << 0; - Matrix HEactual; - Matrix HKactual; - Vector actual = factor.evaluateError(trueE, trueK, HEactual, HKactual); - EXPECT(assert_equal(expected, actual, 1e-7)); - - // Use numerical derivatives to calculate the expected Jacobian - Matrix HEexpected; - Matrix HKexpected; - typedef Eigen::Matrix Vector1; - // TODO: fix this - boost::function f = - boost::bind( - &EssentialMatrixWithCalibrationFactor::evaluateError, - factor, _1, _2, boost::none, boost::none); - HEexpected = numericalDerivative21( - f, trueE, trueK); - HKexpected = numericalDerivative22( - f, trueE, trueK); - - // Verify the Jacobian is correct - EXPECT(assert_equal(HEexpected, HEactual, 1e-8)); - EXPECT(assert_equal(HKexpected, HKactual, 1e-8)); - } -} - -// //************************************************************************* -// TEST(EssentialMatrixWithCalibrationFactor, ExpressionFactor) { -// Key keyE(1); -// Key keyK(2); -// for (size_t i = 0; i < 5; i++) { -// boost::function, OptionalJacobian<1, 3>)> f = -// boost::bind(&EssentialMatrix::error, _1, pA(i), pB(i), _2); -// Expression E_(keyE); // leaf expression -// Expression K_(keyK); // leaf expression -// Expression expr(f, E_, K_); // unary expression - -// // Test the derivatives using Paul's magic -// Values values; -// values.insert(keyE, trueE); -// values.insert(keyK, trueK); -// EXPECT_CORRECT_EXPRESSION_JACOBIANS(expr, values, 1e-5, 1e-9); - -// // Create the factor -// ExpressionFactor factor(model1, 0, expr); - -// // Check evaluation -// Vector expected(1); -// expected << 0; -// vector Hactual(1); -// Vector actual = factor.unwhitenedError(values, Hactual); -// EXPECT(assert_equal(expected, actual, 1e-7)); -// } -// } - -//************************************************************************* -// TEST(EssentialMatrixWithCalibrationFactor, ExpressionFactorRotationOnly) { -// Key keyE(1); -// Key keyK(1); -// for (size_t i = 0; i < 5; i++) { -// boost::function)> f -// = -// boost::bind(&EssentialMatrix::error, _1, vA(i), vB(i), _2); -// boost::function, -// OptionalJacobian<5, 2>)> g; -// Expression R_(key); -// Expression d_(trueDirection); -// Expression -// E_(&EssentialMatrix::FromRotationAndDirection, R_, d_); -// Expression expr(f, E_); - -// // Test the derivatives using Paul's magic -// Values values; -// values.insert(key, trueRotation); -// EXPECT_CORRECT_EXPRESSION_JACOBIANS(expr, values, 1e-5, 1e-9); - -// // Create the factor -// ExpressionFactor factor(model1, 0, expr); - -// // Check evaluation -// Vector expected(1); -// expected << 0; -// vector Hactual(1); -// Vector actual = factor.unwhitenedError(values, Hactual); -// EXPECT(assert_equal(expected, actual, 1e-7)); -// } -// } - -//************************************************************************* -TEST(EssentialMatrixWithCalibrationFactor, evaluateErrorJacobians) { - Key keyE(1); - Key keyK(2); - // initialize essential matrix - Rot3 r = Rot3::Expmap(Vector3(M_PI / 6, M_PI / 3, M_PI / 9)); - Unit3 t(Point3(2, -1, 0.5)); - EssentialMatrix E = EssentialMatrix::FromRotationAndDirection(r, t); - Cal3_S2 K(200, 1, 1, 10, 10); - Values val; - val.insert(keyE, E); - val.insert(keyK, K); - - Point2 pA(10.0, 20.0); - Point2 pB(12.0, 15.0); - - EssentialMatrixWithCalibrationFactor f(keyE, keyK, pA, pB, model1); - EXPECT_CORRECT_FACTOR_JACOBIANS(f, val, 1e-5, 1e-6); -} - -//************************************************************************* -TEST(EssentialMatrixWithCalibrationFactor, 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 - // factors. In this case, the factors are the constraints. - - // We start with a factor graph and add constraints to it - // Noise sigma is 1cm, assuming metric measurements - NonlinearFactorGraph graph; - for (size_t i = 0; i < 5; i++) - graph.emplace_shared>( - 1, 2, pA(i), pB(i), model1); - - // Check error at ground truth - Values truth; - truth.insert(1, trueE); - truth.insert(2, trueK); - 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).finished()); - Cal3_S2 initialK = - trueK.retract((Vector(5) << 0.1, -0.1, 0.03, -0.2, 0.2).finished()); - initial.insert(1, initialE); - initial.insert(2, trueK); -#if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) - EXPECT_DOUBLES_EQUAL(643.26, graph.error(initial), 1e-2); -#else - EXPECT_DOUBLES_EQUAL(639.84, graph.error(initial), - 1e-2); // TODO: update this value too -#endif - - // Optimize - LevenbergMarquardtParams parameters; - LevenbergMarquardtOptimizer optimizer(graph, initial, parameters); - Values result = optimizer.optimize(); - - // Check result - EssentialMatrix actualE = result.at(1); - Cal3_S2 actualK = result.at(2); - EXPECT(assert_equal(trueE, actualE, 1e-2)); - EXPECT(assert_equal(trueK, actualK, 1e-2)); - - // 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, actualE.error(vA(i, actualK), vB(i, actualK)), - 1e-6); -} - -} // namespace example1 - -//************************************************************************* - -// namespace example2 { - -// const string filename = findExampleDataFile("5pointExample2.txt"); -// SfmData data; -// bool readOK = readBAL(filename, data); -// Rot3 aRb = data.cameras[1].pose().rotation(); -// Point3 aTb = data.cameras[1].pose().translation(); -// EssentialMatrix trueE(aRb, Unit3(aTb)); - -// double baseline = 10; // actual baseline of the camera - -// Point2 pA(size_t i) { -// return data.tracks[i].measurements[0].second; -// } -// Point2 pB(size_t i) { -// return data.tracks[i].measurements[1].second; -// } - -// boost::shared_ptr // -// K = boost::make_shared(500, 0, 0); -// PinholeCamera camera2(data.cameras[1].pose(), *K); - -// 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 (EssentialWithMatrixCalibrationFactor, extraMinimization) { -// // Additional test with camera moving in positive X direction - -// NonlinearFactorGraph graph; -// for (size_t i = 0; i < 5; i++) -// graph.emplace_shared(1, pA(i), -// pB(i), model1, K); - -// // Check error at ground truth -// Values truth; -// 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).finished()); -// initial.insert(1, initialE); - -// #if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) -// EXPECT_DOUBLES_EQUAL(643.26, graph.error(initial), 1e-2); -// #else -// EXPECT_DOUBLES_EQUAL(639.84, graph.error(initial), 1e-2); -// #endif - -// // 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) { -// for (size_t i = 0; i < 5; i++) { -// EssentialMatrixFactor2 factor(100, i, pA(i), pB(i), model2, K); - -// // Check evaluation -// Point3 P1 = data.tracks[i].p; -// const Point2 pi = camera2.project(P1); -// Point2 expected(pi - pB(i)); - -// Matrix Hactual1, Hactual2; -// double d(baseline / P1.z()); -// 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-6)); -// EXPECT(assert_equal(Hexpected2, Hactual2, 1e-8)); -// } -// } - -// //************************************************************************* -// TEST (EssentialMatrixFactor2, extraMinimization) { -// // Additional test with camera moving in positive X direction - -// // We start with a factor graph and add constraints to it -// // Noise sigma is 1, assuming pixel measurements -// NonlinearFactorGraph graph; -// for (size_t i = 0; i < data.number_tracks(); i++) -// graph.emplace_shared(100, i, pA(i), pB(i), -// model2, K); - -// // Check error at ground truth -// Values truth; -// truth.insert(100, trueE); -// for (size_t i = 0; i < data.number_tracks(); i++) { -// Point3 P1 = data.tracks[i].p; -// truth.insert(i, double(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 < data.number_tracks(); i++) -// EXPECT_DOUBLES_EQUAL(truth.at(i), result.at(i), 1e-1); - -// // Check error at result -// EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4); -// } - -// //************************************************************************* -// TEST (EssentialMatrixFactor3, extraTest) { - -// // The "true E" in the body frame is -// EssentialMatrix bodyE = cRb.inverse() * trueE; - -// for (size_t i = 0; i < 5; i++) { -// EssentialMatrixFactor3 factor(100, i, pA(i), pB(i), cRb, model2, K); - -// // Check evaluation -// Point3 P1 = data.tracks[i].p; -// const Point2 pi = camera2.project(P1); -// Point2 expected(pi - pB(i)); - -// Matrix Hactual1, Hactual2; -// double d(baseline / P1.z()); -// Vector actual = factor.evaluateError(bodyE, 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( -// &EssentialMatrixFactor3::evaluateError, &factor, _1, _2, boost::none, -// boost::none); -// Hexpected1 = numericalDerivative21(f, -// bodyE, d); Hexpected2 = numericalDerivative22(f, bodyE, d); - -// // Verify the Jacobian is correct -// EXPECT(assert_equal(Hexpected1, Hactual1, 1e-6)); -// EXPECT(assert_equal(Hexpected2, Hactual2, 1e-8)); -// } -// } - -// } // namespace example2 - -/* ************************************************************************* */ -int main() { - TestResult tr; - return TestRegistry::runAllTests(tr); -} -/* ************************************************************************* */ From b0fb6a3908efac0a3992e29b96862732210f117b Mon Sep 17 00:00:00 2001 From: Ayush Baid Date: Mon, 26 Apr 2021 20:52:17 -0400 Subject: [PATCH 04/30] renaming key variable --- gtsam/slam/EssentialMatrixFactor.h | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/gtsam/slam/EssentialMatrixFactor.h b/gtsam/slam/EssentialMatrixFactor.h index 786b9596b..01d623332 100644 --- a/gtsam/slam/EssentialMatrixFactor.h +++ b/gtsam/slam/EssentialMatrixFactor.h @@ -313,17 +313,17 @@ class EssentialMatrixFactor4 public: /** * Constructor - * @param essentialMatrixKey Essential Matrix variable key - * @param calibrationKey Calibration variable key + * @param keyE Essential Matrix variable key + * @param keyK Calibration variable key * @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 */ - EssentialMatrixFactor4(Key essentialMatrixKey, Key calibrationKey, + EssentialMatrixFactor4(Key keyE, Key keyK, const Point2& pA, const Point2& pB, const SharedNoiseModel& model) - : Base(model, essentialMatrixKey, calibrationKey), pA_(pA), pB_(pB) {} + : Base(model, keyE, keyK), pA_(pA), pB_(pB) {} /// @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { @@ -345,8 +345,8 @@ class EssentialMatrixFactor4 /** * @brief Calculate the algebraic epipolar error p' (K^-1)' E K p. * - * @param E essential matrix for key essentialMatrixKey - * @param K calibration (common for both images) for key calibrationKey + * @param E essential matrix for key keyE + * @param K calibration (common for both images) for key keyK * @param H1 optional jacobian in E * @param H2 optional jacobian in K * @return * Vector From bd0838c0c96b78ccee6a3d1e811f43f4315c876a Mon Sep 17 00:00:00 2001 From: Ayush Baid Date: Mon, 26 Apr 2021 20:55:25 -0400 Subject: [PATCH 05/30] fixing docstring --- gtsam/slam/EssentialMatrixFactor.h | 12 ++++-------- 1 file changed, 4 insertions(+), 8 deletions(-) diff --git a/gtsam/slam/EssentialMatrixFactor.h b/gtsam/slam/EssentialMatrixFactor.h index 01d623332..0c81cfc4b 100644 --- a/gtsam/slam/EssentialMatrixFactor.h +++ b/gtsam/slam/EssentialMatrixFactor.h @@ -341,15 +341,14 @@ class EssentialMatrixFactor4 << std::endl; } - /// vector of errors returns 1D vector /** * @brief Calculate the algebraic epipolar error p' (K^-1)' E K p. * * @param E essential matrix for key keyE * @param K calibration (common for both images) for key keyK - * @param H1 optional jacobian in E - * @param H2 optional jacobian in K - * @return * Vector + * @param H1 optional jacobian of error w.r.t E + * @param H2 optional jacobian of error w.r.t K + * @return * Vector 1D vector of algebraic error */ Vector evaluateError( const EssentialMatrix& E, const CALIBRATION& K, @@ -370,12 +369,9 @@ class EssentialMatrixFactor4 if (H2) { // compute the jacobian of error w.r.t K - // using dvA/dK = dvA/dcA * dcA/dK and dVB/dK = dvB/dcB * dcB/dK - // Matrix vA_H_K = vA_H_cA * cA_H_K; - // Matrix vB_H_K = vB_H_cB * cB_H_K; - // error function f = vA.T * E * vB // H2 = df/dK = vB.T * E.T * dvA/dK + vA.T * E * dvB/dK + // where dvA/dK = dvA/dcA * dcA/dK, dVB/dK = dvB/dcB * dcB/dK *H2 = vB.transpose() * E.matrix().transpose() * vA_H_cA * cA_H_K + vA.transpose() * E.matrix() * vB_H_cB * cB_H_K; } From 2cf76daf3cdbf17ba2e66cd916450e2a0c1cbb8b Mon Sep 17 00:00:00 2001 From: Ayush Baid Date: Tue, 27 Apr 2021 00:44:15 -0400 Subject: [PATCH 06/30] reverting jacobian computation from homogeneous function --- gtsam/geometry/EssentialMatrix.h | 10 +++------- gtsam/geometry/tests/testEssentialMatrix.cpp | 12 ------------ gtsam/slam/EssentialMatrixFactor.h | 14 +++++++------- 3 files changed, 10 insertions(+), 26 deletions(-) diff --git a/gtsam/geometry/EssentialMatrix.h b/gtsam/geometry/EssentialMatrix.h index 5eb11f8ed..909576aa0 100644 --- a/gtsam/geometry/EssentialMatrix.h +++ b/gtsam/geometry/EssentialMatrix.h @@ -7,10 +7,10 @@ #pragma once -#include -#include #include #include +#include +#include #include #include @@ -31,11 +31,7 @@ class EssentialMatrix { public: /// Static function to convert Point2 to homogeneous coordinates - static Vector3 Homogeneous(const Point2& p, - OptionalJacobian<3, 2> H = boost::none) { - if (H) { - H->setIdentity(); - } + static Vector3 Homogeneous(const Point2& p) { return Vector3(p.x(), p.y(), 1); } diff --git a/gtsam/geometry/tests/testEssentialMatrix.cpp b/gtsam/geometry/tests/testEssentialMatrix.cpp index fd3fb64f0..86a498cdc 100644 --- a/gtsam/geometry/tests/testEssentialMatrix.cpp +++ b/gtsam/geometry/tests/testEssentialMatrix.cpp @@ -241,18 +241,6 @@ TEST (EssentialMatrix, epipoles) { EXPECT(assert_equal(e2, E.epipole_b())); } -//************************************************************************* -TEST(EssentialMatrix, Homogeneous) { - Point2 input(5.0, 1.3); - Vector3 expected(5.0, 1.3, 1.0); - Matrix32 expectedH; - expectedH << 1.0, 0.0, 0.0, 1.0, 0.0, 0.0; - Matrix32 actualH; - Vector3 actual = EssentialMatrix::Homogeneous(input, actualH); - EXPECT(assert_equal(actual, expected)); - EXPECT(assert_equal(actualH, expectedH)); -} - /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/slam/EssentialMatrixFactor.h b/gtsam/slam/EssentialMatrixFactor.h index 0c81cfc4b..9d51852ed 100644 --- a/gtsam/slam/EssentialMatrixFactor.h +++ b/gtsam/slam/EssentialMatrixFactor.h @@ -354,17 +354,15 @@ class EssentialMatrixFactor4 const EssentialMatrix& E, const CALIBRATION& K, boost::optional H1 = boost::none, boost::optional H2 = boost::none) const override { - Vector error(1); // converting from pixel coordinates to normalized coordinates cA and cB JacobianCalibration cA_H_K; // dcA/dK JacobianCalibration cB_H_K; // dcB/dK Point2 cA = K.calibrate(pA_, H2 ? &cA_H_K : 0); Point2 cB = K.calibrate(pB_, H2 ? &cB_H_K : 0); - // Homogeneous the coordinates - Matrix32 vA_H_cA, vB_H_cB; - Vector3 vA = EssentialMatrix::Homogeneous(cA, H2 ? &vA_H_cA : 0); - Vector3 vB = EssentialMatrix::Homogeneous(cB, H2 ? &vB_H_cB : 0); + // convert to homogeneous coordinates + Vector3 vA = EssentialMatrix::Homogeneous(cA); + Vector3 vB = EssentialMatrix::Homogeneous(cB); if (H2) { // compute the jacobian of error w.r.t K @@ -372,10 +370,12 @@ class EssentialMatrixFactor4 // error function f = vA.T * E * vB // H2 = df/dK = vB.T * E.T * dvA/dK + vA.T * E * dvB/dK // where dvA/dK = dvA/dcA * dcA/dK, dVB/dK = dvB/dcB * dcB/dK - *H2 = vB.transpose() * E.matrix().transpose() * vA_H_cA * cA_H_K + - vA.transpose() * E.matrix() * vB_H_cB * cB_H_K; + // and dvA/dcA = dvB/dcB = [[1, 0], [0, 1], [0, 0]] + *H2 = vB.transpose() * E.matrix().transpose().leftCols<2>() * cA_H_K + + vA.transpose() * E.matrix().leftCols<2>() * cB_H_K; } + Vector error(1); error << E.error(vA, vB, H1); return error; From 4572282debb257b903f8700c009f3448b41589de Mon Sep 17 00:00:00 2001 From: Ayush Baid Date: Wed, 28 Apr 2021 18:49:07 -0400 Subject: [PATCH 07/30] adding prior on calibrations --- gtsam/slam/EssentialMatrixFactor.h | 7 +++++-- .../slam/tests/testEssentialMatrixFactor.cpp | 20 ++++++++++++++----- 2 files changed, 20 insertions(+), 7 deletions(-) diff --git a/gtsam/slam/EssentialMatrixFactor.h b/gtsam/slam/EssentialMatrixFactor.h index 9d51852ed..3e8c45ece 100644 --- a/gtsam/slam/EssentialMatrixFactor.h +++ b/gtsam/slam/EssentialMatrixFactor.h @@ -297,6 +297,9 @@ class EssentialMatrixFactor3 : public EssentialMatrixFactor2 { * Factor that evaluates algebraic epipolar error (K^-1 p)'E (K^-1 p) for given * essential matrix and calibration. The calibration is shared between two * images. + * + * Note: as correspondences between 2d coordinates can only recover 7 DoF, + * this factor should always be used with a prior factor on calibration. */ template class EssentialMatrixFactor4 @@ -357,8 +360,8 @@ class EssentialMatrixFactor4 // converting from pixel coordinates to normalized coordinates cA and cB JacobianCalibration cA_H_K; // dcA/dK JacobianCalibration cB_H_K; // dcB/dK - Point2 cA = K.calibrate(pA_, H2 ? &cA_H_K : 0); - Point2 cB = K.calibrate(pB_, H2 ? &cB_H_K : 0); + Point2 cA = K.calibrate(pA_, H2 ? &cA_H_K : 0, boost::none); + Point2 cB = K.calibrate(pB_, H2 ? &cB_H_K : 0, boost::none); // convert to homogeneous coordinates Vector3 vA = EssentialMatrix::Homogeneous(cA); diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp index 3a53157f6..08908d499 100644 --- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp +++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp @@ -425,9 +425,9 @@ TEST(EssentialMatrixFactor4, minimization) { EssentialMatrix initialE = trueE.retract((Vector(5) << 0.1, -0.1, 0.1, 0.1, -0.1).finished()); Cal3_S2 initialK = - trueK.retract((Vector(5) << 0.1, -0.1, 0.03, -0.2, 0.2).finished()); + trueK.retract((Vector(5) << 0.1, -0.08, 0.01, -0.05, 0.06).finished()); initial.insert(1, initialE); - initial.insert(2, trueK); + initial.insert(2, initialK); #if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) EXPECT_DOUBLES_EQUAL(643.26, graph.error(initial), 1e-2); #else @@ -435,6 +435,11 @@ TEST(EssentialMatrixFactor4, minimization) { 1e-2); // TODO: update this value too #endif + // add prior factor for calibration + Vector5 priorNoiseModelSigma; + priorNoiseModelSigma << 0.1, 0.1, 0.01, 0.1, 0.1; + graph.emplace_shared>(2, initialK, noiseModel::Diagonal::Sigmas(priorNoiseModelSigma)); + // Optimize LevenbergMarquardtParams parameters; LevenbergMarquardtOptimizer optimizer(graph, initial, parameters); @@ -444,7 +449,7 @@ TEST(EssentialMatrixFactor4, minimization) { EssentialMatrix actualE = result.at(1); Cal3_S2 actualK = result.at(2); EXPECT(assert_equal(trueE, actualE, 1e-1)); // TODO: fix the tolerance - EXPECT(assert_equal(trueK, actualK, 1e-1)); // TODO: fix the tolerance + EXPECT(assert_equal(trueK, actualK, 1e-2)); // TODO: fix the tolerance // Check error at result EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4); @@ -652,16 +657,21 @@ TEST(EssentialMatrixFactor4, extraMinimization) { EssentialMatrix initialE = trueE.retract((Vector(5) << 0.1, -0.1, 0.1, 0.1, -0.1).finished()); Cal3Bundler initialK = - trueK.retract((Vector(3) << 0.1, -0.02, 0.03).finished()); + trueK.retract((Vector(3) << 0.1, -0.01, 0.01).finished()); initial.insert(1, initialE); initial.insert(2, initialK); #if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) - EXPECT_DOUBLES_EQUAL(633.71, graph.error(initial), 1e-2); + EXPECT_DOUBLES_EQUAL(643.62, graph.error(initial), 1e-2); #else EXPECT_DOUBLES_EQUAL(639.84, graph.error(initial), 1e-2); // TODO: fix this #endif + // add prior factor on calibration + Vector3 priorNoiseModelSigma; + priorNoiseModelSigma << 0.3, 0.03, 0.03; + graph.emplace_shared>(2, initialK, noiseModel::Diagonal::Sigmas(priorNoiseModelSigma)); + // Optimize LevenbergMarquardtParams parameters; LevenbergMarquardtOptimizer optimizer(graph, initial, parameters); From b3601ef1c12c6d91180a4475122cdde9d0b5b29f Mon Sep 17 00:00:00 2001 From: Ayush Baid Date: Tue, 8 Jun 2021 13:14:29 -0700 Subject: [PATCH 08/30] updating tests --- gtsam/slam/tests/testEssentialMatrixFactor.cpp | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp index 08908d499..6904ba4f5 100644 --- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp +++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp @@ -12,6 +12,7 @@ #include #include #include +#include #include #include #include @@ -34,7 +35,7 @@ gtsam::Rot3 cRb = gtsam::Rot3(bX, bZ, -bY).inverse(); namespace example1 { -const string filename = findExampleDataFile("5pointExample1.txt"); +const string filename = findExampleDataFile("18pointExample1.txt"); SfmData data; bool readOK = readBAL(filename, data); Rot3 c1Rc2 = data.cameras[1].pose().rotation(); @@ -72,13 +73,13 @@ TEST (EssentialMatrixFactor, testData) { EXPECT(assert_equal(expected, aEb_matrix, 1e-8)); // Check some projections - EXPECT(assert_equal(Point2(0, 0), pA(0), 1e-8)); - EXPECT(assert_equal(Point2(0, 0.1), pB(0), 1e-8)); - EXPECT(assert_equal(Point2(0, -1), pA(4), 1e-8)); - EXPECT(assert_equal(Point2(-1, 0.2), pB(4), 1e-8)); + EXPECT(assert_equal(Point2(-0.1, -0.5), pA(0), 1e-8)); + EXPECT(assert_equal(Point2(-0.5, 0.2), pB(0), 1e-8)); + EXPECT(assert_equal(Point2(0, 0), pA(4), 1e-8)); + EXPECT(assert_equal(Point2(0, 0.1), pB(4), 1e-8)); // Check homogeneous version - EXPECT(assert_equal(Vector3(-1, 0.2, 1), vB(4), 1e-8)); + EXPECT(assert_equal(Vector3(0, 0.1, 1), vB(4), 1e-8)); // Check epipolar constraint for (size_t i = 0; i < 5; i++) @@ -194,7 +195,7 @@ TEST (EssentialMatrixFactor, minimization) { (Vector(5) << 0.1, -0.1, 0.1, 0.1, -0.1).finished()); initial.insert(1, initialE); #if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) - EXPECT_DOUBLES_EQUAL(643.26, graph.error(initial), 1e-2); + EXPECT_DOUBLES_EQUAL(419.07, graph.error(initial), 1e-2); #else EXPECT_DOUBLES_EQUAL(639.84, graph.error(initial), 1e-2); #endif From 4fbd98df3a23fa3c485fc649944cc8a3144ce9d5 Mon Sep 17 00:00:00 2001 From: Ayush Baid Date: Tue, 8 Jun 2021 13:14:56 -0700 Subject: [PATCH 09/30] creating 18 point example --- examples/CreateSFMExampleData.cpp | 25 ++++++ examples/data/18pointExample1.txt | 131 ++++++++++++++++++++++++++++++ 2 files changed, 156 insertions(+) create mode 100644 examples/data/18pointExample1.txt diff --git a/examples/CreateSFMExampleData.cpp b/examples/CreateSFMExampleData.cpp index aa0bde8f6..650b3c731 100644 --- a/examples/CreateSFMExampleData.cpp +++ b/examples/CreateSFMExampleData.cpp @@ -99,11 +99,36 @@ void create5PointExample2() { createExampleBALFile(filename, P, pose1, pose2,K); } + +/* ************************************************************************* */ + +void create18PointExample1() { + + // Create two cameras poses + Rot3 aRb = Rot3::Yaw(M_PI_2); + Point3 aTb(0.1, 0, 0); + Pose3 pose1, pose2(aRb, aTb); + + // Create test data, we need 15 points + vector P; + P += Point3(-0.1, -0.5, 1), Point3(-0.1, 0, 1), Point3(-0.1, 0.5, 1), + Point3(0, -0.5, 1), Point3(0, 0, 1), Point3(0, 0.5, 1), + Point3(0.1, -0.5, 1), Point3(0.1, 0, 1), Point3(0.1, 0.5, 1), + Point3(-0.1, -0.5, 0.5), Point3(-0.1, 0, 0.5), Point3(-0.1, 0.5, 0.5), + Point3(0, -0.5, 0.5), Point3(0, 0, 0.5), Point3(0, 0.5, 0.5), + Point3(0.1, -0.5, 0.5), Point3(0.1, 0, 0.5), Point3(0.1, 0.5, 0.5); + + // Assumes example is run in ${GTSAM_TOP}/build/examples + const string filename = "../../examples/data/18pointExample1.txt"; + createExampleBALFile(filename, P, pose1, pose2); +} + /* ************************************************************************* */ int main(int argc, char* argv[]) { create5PointExample1(); create5PointExample2(); + create18PointExample1(); return 0; } diff --git a/examples/data/18pointExample1.txt b/examples/data/18pointExample1.txt new file mode 100644 index 000000000..e7d186294 --- /dev/null +++ b/examples/data/18pointExample1.txt @@ -0,0 +1,131 @@ +2 18 36 + +0 0 -0.10000000000000000555 0.5 +1 0 -0.5 -0.19999999999999998335 +0 1 -0.10000000000000000555 -0 +1 1 -1.2246467991473532688e-17 -0.2000000000000000111 +0 2 -0.10000000000000000555 -0.5 +1 2 0.5 -0.20000000000000003886 +0 3 0 0.5 +1 3 -0.5 -0.099999999999999977796 +0 4 0 -0 +1 4 -6.123233995736766344e-18 -0.10000000000000000555 +0 5 0 -0.5 +1 5 0.5 -0.10000000000000003331 +0 6 0.10000000000000000555 0.5 +1 6 -0.5 3.0616169978683830179e-17 +0 7 0.10000000000000000555 -0 +1 7 0 -0 +0 8 0.10000000000000000555 -0.5 +1 8 0.5 -3.0616169978683830179e-17 +0 9 -0.2000000000000000111 1 +1 9 -1 -0.39999999999999996669 +0 10 -0.2000000000000000111 -0 +1 10 -2.4492935982947065376e-17 -0.4000000000000000222 +0 11 -0.2000000000000000111 -1 +1 11 1 -0.40000000000000007772 +0 12 0 1 +1 12 -1 -0.19999999999999995559 +0 13 0 -0 +1 13 -1.2246467991473532688e-17 -0.2000000000000000111 +0 14 0 -1 +1 14 1 -0.20000000000000006661 +0 15 0.2000000000000000111 1 +1 15 -1 6.1232339957367660359e-17 +0 16 0.2000000000000000111 -0 +1 16 0 -0 +0 17 0.2000000000000000111 -1 +1 17 1 -6.1232339957367660359e-17 + +3.141592653589793116 + 0 + 0 +-0 +0 +0 +1 +0 +0 + +2.2214414690791830509 +2.2214414690791826068 + 0 +-6.123233995736766344e-18 +-0.10000000000000000555 +0 +1 +0 +0 + +-0.10000000000000000555 +-0.5 +1 + +-0.10000000000000000555 +0 +1 + +-0.10000000000000000555 +0.5 +1 + +0 +-0.5 +1 + +0 +0 +1 + +0 +0.5 +1 + +0.10000000000000000555 +-0.5 +1 + +0.10000000000000000555 +0 +1 + +0.10000000000000000555 +0.5 +1 + +-0.10000000000000000555 +-0.5 +0.5 + +-0.10000000000000000555 +0 +0.5 + +-0.10000000000000000555 +0.5 +0.5 + +0 +-0.5 +0.5 + +0 +0 +0.5 + +0 +0.5 +0.5 + +0.10000000000000000555 +-0.5 +0.5 + +0.10000000000000000555 +0 +0.5 + +0.10000000000000000555 +0.5 +0.5 + From 8a2ce7e11809b8d5f20513ebda7120041a43d269 Mon Sep 17 00:00:00 2001 From: Ayush Baid Date: Tue, 8 Jun 2021 21:42:14 -0700 Subject: [PATCH 10/30] switching to sampson point line error --- gtsam/geometry/EssentialMatrix.cpp | 51 ++++++++++++++++-- gtsam/geometry/EssentialMatrix.h | 2 +- gtsam/geometry/tests/testEssentialMatrix.cpp | 56 ++++++++++++++++++++ 3 files changed, 104 insertions(+), 5 deletions(-) diff --git a/gtsam/geometry/EssentialMatrix.cpp b/gtsam/geometry/EssentialMatrix.cpp index 020c94fdb..446228fcc 100644 --- a/gtsam/geometry/EssentialMatrix.cpp +++ b/gtsam/geometry/EssentialMatrix.cpp @@ -103,14 +103,57 @@ EssentialMatrix EssentialMatrix::rotate(const Rot3& cRb, /* ************************************************************************* */ double EssentialMatrix::error(const Vector3& vA, const Vector3& vB, // OptionalJacobian<1, 5> H) const { + + // compute the epipolar lines + Point3 lB = E_ * vB; + Point3 lA = E_.transpose() * vA; + + + // compute the algebraic error as a simple dot product. + double algebraic_error = dot(vA, lB); + + // compute the line-norms for the two lines + Matrix23 I; + I.setIdentity(); + Matrix21 nA = I * lA; + Matrix21 nB = I * lB; + double nA_sq_norm = nA.squaredNorm(); + double nB_sq_norm = nB.squaredNorm(); + + // compute the normalizing denominator and finally the sampson error + double denominator = sqrt(nA_sq_norm + nB_sq_norm); + double sampson_error = algebraic_error / denominator; + if (H) { // See math.lyx - Matrix13 HR = vA.transpose() * E_ * skewSymmetric(-vB); - Matrix12 HD = vA.transpose() * skewSymmetric(-rotation().matrix() * vB) + // computing the derivatives of the numerator w.r.t. E + Matrix13 numerator_H_R = vA.transpose() * E_ * skewSymmetric(-vB); + Matrix12 numerator_H_D = vA.transpose() * skewSymmetric(-rotation().matrix() * vB) * direction().basis(); - *H << HR, HD; + + + // computing the derivatives of the denominator w.r.t. E + Matrix12 denominator_H_nA = nA.transpose() / denominator; + Matrix12 denominator_H_nB = nB.transpose() / denominator; + Matrix13 denominator_H_lA = denominator_H_nA * I; + Matrix13 denominator_H_lB = denominator_H_nB * I; + Matrix33 lB_H_R = E_ * skewSymmetric(-vB); + Matrix32 lB_H_D = skewSymmetric(-rotation().matrix() * vB) * direction().basis(); + Matrix33 lA_H_R = skewSymmetric(E_.matrix().transpose() * vA) * + rotation().matrix().transpose(); + Matrix32 lA_H_D = rotation().inverse().matrix() * skewSymmetric(vA) * direction().basis(); + + Matrix13 denominator_H_R = denominator_H_lA * lA_H_R + denominator_H_lB * lB_H_R; + Matrix12 denominator_H_D = denominator_H_lA * lA_H_D + denominator_H_lB * lB_H_D; + + Matrix15 denominator_H; + denominator_H << denominator_H_R, denominator_H_D; + Matrix15 numerator_H; + numerator_H << numerator_H_R, numerator_H_D; + + *H = numerator_H / denominator - algebraic_error * denominator_H / (denominator * denominator); } - return dot(vA, E_ * vB); + return sampson_error; } /* ************************************************************************* */ diff --git a/gtsam/geometry/EssentialMatrix.h b/gtsam/geometry/EssentialMatrix.h index 909576aa0..4f698047c 100644 --- a/gtsam/geometry/EssentialMatrix.h +++ b/gtsam/geometry/EssentialMatrix.h @@ -159,7 +159,7 @@ class EssentialMatrix { return E.rotate(cRb); } - /// epipolar error, algebraic + /// epipolar error, sampson GTSAM_EXPORT double error(const Vector3& vA, const Vector3& vB, OptionalJacobian<1, 5> H = boost::none) const; diff --git a/gtsam/geometry/tests/testEssentialMatrix.cpp b/gtsam/geometry/tests/testEssentialMatrix.cpp index 86a498cdc..71ea44bd1 100644 --- a/gtsam/geometry/tests/testEssentialMatrix.cpp +++ b/gtsam/geometry/tests/testEssentialMatrix.cpp @@ -241,6 +241,62 @@ TEST (EssentialMatrix, epipoles) { EXPECT(assert_equal(e2, E.epipole_b())); } +//************************************************************************* +TEST (EssentialMatrix, errorValue) { + // Use two points to get error + Point3 a(1, -2, 1); + Point3 b(3, 1, 1); + + // compute the expected error + // E = [0, 0, 0; 0, 0, -1; 1, 0, 0] + // line for b = [0, -1, 3] + // line for a = [1, 0, 2] + // algebraic error = 5 + // norm of line for b = 1 + // norm of line for a = 1 + // sampson error = 5 / sqrt(1^2 + 1^2) + double expected = 3.535533906; + + // check the error + double actual = trueE.error(a, b); + EXPECT(assert_equal(expected, actual, 1e-6)); +} + +//************************************************************************* +double error_(const Rot3& R, const Unit3& t){ + // Use two points to get error + Point3 a(1, -2, 1); + Point3 b(3, 1, 1); + + EssentialMatrix E = EssentialMatrix::FromRotationAndDirection(R, t); + return E.error(a, b); +} +TEST (EssentialMatrix, errorJacobians) { + // Use two points to get error + Point3 a(1, -2, 1); + Point3 b(3, 1, 1); + + Rot3 c1Rc2 = Rot3::Ypr(0.1, -0.2, 0.3); + Point3 c1Tc2(0.4, 0.5, 0.6); + EssentialMatrix E(c1Rc2, Unit3(c1Tc2)); + + // Use numerical derivatives to calculate the expected Jacobian + Matrix13 HRexpected; + Matrix12 HDexpected; + HRexpected = numericalDerivative21( + error_, E.rotation(), E.direction(), 1e-8); + HDexpected = numericalDerivative22( + error_, E.rotation(), E.direction(), 1e-8); + Matrix15 HEexpected; + HEexpected << HRexpected, HDexpected; + + Matrix15 HEactual; + E.error(a, b, HEactual); + + // Verify the Jacobian is correct + EXPECT(assert_equal(HEexpected, HEactual, 1e-8)); +} + /* ************************************************************************* */ int main() { TestResult tr; From 0ecd8ab63d60f7f382637b0bf227c384cc2f86e1 Mon Sep 17 00:00:00 2001 From: Ayush Baid Date: Wed, 9 Jun 2021 09:49:32 -0700 Subject: [PATCH 11/30] fixing jacobians and reformatting --- gtsam/geometry/EssentialMatrix.cpp | 34 +++++++++++--------- gtsam/geometry/tests/testEssentialMatrix.cpp | 19 ++++++----- 2 files changed, 27 insertions(+), 26 deletions(-) diff --git a/gtsam/geometry/EssentialMatrix.cpp b/gtsam/geometry/EssentialMatrix.cpp index 446228fcc..10d387338 100644 --- a/gtsam/geometry/EssentialMatrix.cpp +++ b/gtsam/geometry/EssentialMatrix.cpp @@ -101,17 +101,15 @@ EssentialMatrix EssentialMatrix::rotate(const Rot3& cRb, } /* ************************************************************************* */ -double EssentialMatrix::error(const Vector3& vA, const Vector3& vB, // - OptionalJacobian<1, 5> H) const { - +double EssentialMatrix::error(const Vector3& vA, const Vector3& vB, // + OptionalJacobian<1, 5> H) const { // compute the epipolar lines Point3 lB = E_ * vB; Point3 lA = E_.transpose() * vA; - // compute the algebraic error as a simple dot product. - double algebraic_error = dot(vA, lB); - + double algebraic_error = dot(vA, lB); + // compute the line-norms for the two lines Matrix23 I; I.setIdentity(); @@ -128,9 +126,9 @@ double EssentialMatrix::error(const Vector3& vA, const Vector3& vB, // // See math.lyx // computing the derivatives of the numerator w.r.t. E Matrix13 numerator_H_R = vA.transpose() * E_ * skewSymmetric(-vB); - Matrix12 numerator_H_D = vA.transpose() * skewSymmetric(-rotation().matrix() * vB) - * direction().basis(); - + Matrix12 numerator_H_D = vA.transpose() * + skewSymmetric(-rotation().matrix() * vB) * + direction().basis(); // computing the derivatives of the denominator w.r.t. E Matrix12 denominator_H_nA = nA.transpose() / denominator; @@ -138,20 +136,24 @@ double EssentialMatrix::error(const Vector3& vA, const Vector3& vB, // Matrix13 denominator_H_lA = denominator_H_nA * I; Matrix13 denominator_H_lB = denominator_H_nB * I; Matrix33 lB_H_R = E_ * skewSymmetric(-vB); - Matrix32 lB_H_D = skewSymmetric(-rotation().matrix() * vB) * direction().basis(); - Matrix33 lA_H_R = skewSymmetric(E_.matrix().transpose() * vA) * - rotation().matrix().transpose(); - Matrix32 lA_H_D = rotation().inverse().matrix() * skewSymmetric(vA) * direction().basis(); + Matrix32 lB_H_D = + skewSymmetric(-rotation().matrix() * vB) * direction().basis(); + Matrix33 lA_H_R = skewSymmetric(E_.matrix().transpose() * vA); + Matrix32 lA_H_D = + rotation().inverse().matrix() * skewSymmetric(vA) * direction().basis(); - Matrix13 denominator_H_R = denominator_H_lA * lA_H_R + denominator_H_lB * lB_H_R; - Matrix12 denominator_H_D = denominator_H_lA * lA_H_D + denominator_H_lB * lB_H_D; + Matrix13 denominator_H_R = + denominator_H_lA * lA_H_R + denominator_H_lB * lB_H_R; + Matrix12 denominator_H_D = + denominator_H_lA * lA_H_D + denominator_H_lB * lB_H_D; Matrix15 denominator_H; denominator_H << denominator_H_R, denominator_H_D; Matrix15 numerator_H; numerator_H << numerator_H_R, numerator_H_D; - *H = numerator_H / denominator - algebraic_error * denominator_H / (denominator * denominator); + *H = numerator_H / denominator - + algebraic_error * denominator_H / (denominator * denominator); } return sampson_error; } diff --git a/gtsam/geometry/tests/testEssentialMatrix.cpp b/gtsam/geometry/tests/testEssentialMatrix.cpp index 71ea44bd1..acb817ae7 100644 --- a/gtsam/geometry/tests/testEssentialMatrix.cpp +++ b/gtsam/geometry/tests/testEssentialMatrix.cpp @@ -242,7 +242,7 @@ TEST (EssentialMatrix, epipoles) { } //************************************************************************* -TEST (EssentialMatrix, errorValue) { +TEST(EssentialMatrix, errorValue) { // Use two points to get error Point3 a(1, -2, 1); Point3 b(3, 1, 1); @@ -254,7 +254,7 @@ TEST (EssentialMatrix, errorValue) { // algebraic error = 5 // norm of line for b = 1 // norm of line for a = 1 - // sampson error = 5 / sqrt(1^2 + 1^2) + // sampson error = 5 / sqrt(1^2 + 1^2) double expected = 3.535533906; // check the error @@ -263,7 +263,7 @@ TEST (EssentialMatrix, errorValue) { } //************************************************************************* -double error_(const Rot3& R, const Unit3& t){ +double error_(const Rot3& R, const Unit3& t) { // Use two points to get error Point3 a(1, -2, 1); Point3 b(3, 1, 1); @@ -271,7 +271,7 @@ double error_(const Rot3& R, const Unit3& t){ EssentialMatrix E = EssentialMatrix::FromRotationAndDirection(R, t); return E.error(a, b); } -TEST (EssentialMatrix, errorJacobians) { +TEST(EssentialMatrix, errorJacobians) { // Use two points to get error Point3 a(1, -2, 1); Point3 b(3, 1, 1); @@ -283,10 +283,10 @@ TEST (EssentialMatrix, errorJacobians) { // Use numerical derivatives to calculate the expected Jacobian Matrix13 HRexpected; Matrix12 HDexpected; - HRexpected = numericalDerivative21( - error_, E.rotation(), E.direction(), 1e-8); - HDexpected = numericalDerivative22( - error_, E.rotation(), E.direction(), 1e-8); + HRexpected = numericalDerivative21(error_, E.rotation(), + E.direction()); + HDexpected = numericalDerivative22(error_, E.rotation(), + E.direction()); Matrix15 HEexpected; HEexpected << HRexpected, HDexpected; @@ -294,7 +294,7 @@ TEST (EssentialMatrix, errorJacobians) { E.error(a, b, HEactual); // Verify the Jacobian is correct - EXPECT(assert_equal(HEexpected, HEactual, 1e-8)); + EXPECT(assert_equal(HEexpected, HEactual, 1e-5)); } /* ************************************************************************* */ @@ -303,4 +303,3 @@ int main() { return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ - From ae69e5f0158b79c4d6e943128026775488359376 Mon Sep 17 00:00:00 2001 From: Ayush Baid Date: Wed, 9 Jun 2021 10:02:39 -0700 Subject: [PATCH 12/30] changing error values in test --- gtsam/slam/tests/testEssentialMatrixFactor.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp index 95db611d7..977528b53 100644 --- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp +++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp @@ -192,7 +192,7 @@ TEST (EssentialMatrixFactor, minimization) { (Vector(5) << 0.1, -0.1, 0.1, 0.1, -0.1).finished()); initial.insert(1, initialE); #if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) - EXPECT_DOUBLES_EQUAL(643.26, graph.error(initial), 1e-2); + EXPECT_DOUBLES_EQUAL(313.85, graph.error(initial), 1e-2); #else EXPECT_DOUBLES_EQUAL(639.84, graph.error(initial), 1e-2); #endif @@ -406,7 +406,7 @@ TEST (EssentialMatrixFactor, extraMinimization) { initial.insert(1, initialE); #if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) - EXPECT_DOUBLES_EQUAL(643.26, graph.error(initial), 1e-2); + EXPECT_DOUBLES_EQUAL(313.85, graph.error(initial), 1e-2); #else EXPECT_DOUBLES_EQUAL(639.84, graph.error(initial), 1e-2); #endif From 15f8b416cdbb543fd8debea195643196ed3ace6c Mon Sep 17 00:00:00 2001 From: Ayush Baid Date: Wed, 9 Jun 2021 15:22:08 -0700 Subject: [PATCH 13/30] adding jacobians on input points --- gtsam/geometry/EssentialMatrix.cpp | 25 ++++++++++++++--- gtsam/geometry/EssentialMatrix.h | 4 ++- gtsam/geometry/tests/testEssentialMatrix.cpp | 29 ++++++++++++-------- 3 files changed, 41 insertions(+), 17 deletions(-) diff --git a/gtsam/geometry/EssentialMatrix.cpp b/gtsam/geometry/EssentialMatrix.cpp index 10d387338..cfe76ff04 100644 --- a/gtsam/geometry/EssentialMatrix.cpp +++ b/gtsam/geometry/EssentialMatrix.cpp @@ -101,8 +101,10 @@ EssentialMatrix EssentialMatrix::rotate(const Rot3& cRb, } /* ************************************************************************* */ -double EssentialMatrix::error(const Vector3& vA, const Vector3& vB, // - OptionalJacobian<1, 5> H) const { +double EssentialMatrix::error(const Vector3& vA, const Vector3& vB, + OptionalJacobian<1, 5> HE, + OptionalJacobian<1, 3> HvA, + OptionalJacobian<1, 3> HvB) const { // compute the epipolar lines Point3 lB = E_ * vB; Point3 lA = E_.transpose() * vA; @@ -122,7 +124,7 @@ double EssentialMatrix::error(const Vector3& vA, const Vector3& vB, // double denominator = sqrt(nA_sq_norm + nB_sq_norm); double sampson_error = algebraic_error / denominator; - if (H) { + if (HE) { // See math.lyx // computing the derivatives of the numerator w.r.t. E Matrix13 numerator_H_R = vA.transpose() * E_ * skewSymmetric(-vB); @@ -152,9 +154,24 @@ double EssentialMatrix::error(const Vector3& vA, const Vector3& vB, // Matrix15 numerator_H; numerator_H << numerator_H_R, numerator_H_D; - *H = numerator_H / denominator - + *HE = numerator_H / denominator - algebraic_error * denominator_H / (denominator * denominator); } + + if (HvA){ + Matrix13 numerator_H_vA = vB.transpose() * matrix().transpose(); + Matrix13 denominator_H_vA = nA.transpose() * I * matrix().transpose() / denominator; + + *HvA = numerator_H_vA / denominator - algebraic_error * denominator_H_vA / (denominator * denominator); + } + + if (HvB){ + Matrix13 numerator_H_vB = vA.transpose() * matrix(); + Matrix13 denominator_H_vB = nB.transpose() * I * matrix() / denominator; + + *HvB = numerator_H_vB / denominator - algebraic_error * denominator_H_vB / (denominator * denominator); + } + return sampson_error; } diff --git a/gtsam/geometry/EssentialMatrix.h b/gtsam/geometry/EssentialMatrix.h index 4f698047c..0f8403bc8 100644 --- a/gtsam/geometry/EssentialMatrix.h +++ b/gtsam/geometry/EssentialMatrix.h @@ -161,7 +161,9 @@ class EssentialMatrix { /// epipolar error, sampson GTSAM_EXPORT double error(const Vector3& vA, const Vector3& vB, - OptionalJacobian<1, 5> H = boost::none) const; + OptionalJacobian<1, 5> HE = boost::none, + OptionalJacobian<1, 3> HvA = boost::none, + OptionalJacobian<1, 3> HvB = boost::none) const; /// @} diff --git a/gtsam/geometry/tests/testEssentialMatrix.cpp b/gtsam/geometry/tests/testEssentialMatrix.cpp index acb817ae7..f079e15c5 100644 --- a/gtsam/geometry/tests/testEssentialMatrix.cpp +++ b/gtsam/geometry/tests/testEssentialMatrix.cpp @@ -263,18 +263,14 @@ TEST(EssentialMatrix, errorValue) { } //************************************************************************* -double error_(const Rot3& R, const Unit3& t) { - // Use two points to get error - Point3 a(1, -2, 1); - Point3 b(3, 1, 1); - +double error_(const Rot3& R, const Unit3& t, const Point3& a, const Point3& b) { EssentialMatrix E = EssentialMatrix::FromRotationAndDirection(R, t); return E.error(a, b); } TEST(EssentialMatrix, errorJacobians) { // Use two points to get error - Point3 a(1, -2, 1); - Point3 b(3, 1, 1); + Point3 vA(1, -2, 1); + Point3 vB(3, 1, 1); Rot3 c1Rc2 = Rot3::Ypr(0.1, -0.2, 0.3); Point3 c1Tc2(0.4, 0.5, 0.6); @@ -283,18 +279,27 @@ TEST(EssentialMatrix, errorJacobians) { // Use numerical derivatives to calculate the expected Jacobian Matrix13 HRexpected; Matrix12 HDexpected; - HRexpected = numericalDerivative21(error_, E.rotation(), - E.direction()); - HDexpected = numericalDerivative22(error_, E.rotation(), - E.direction()); + Matrix13 HvAexpected; + Matrix13 HvBexpected; + HRexpected = numericalDerivative41(error_, E.rotation(), + E.direction(), vA, vB); + HDexpected = numericalDerivative42(error_, E.rotation(), + E.direction(), vA, vB); + HvAexpected = numericalDerivative43(error_, E.rotation(), + E.direction(), vA, vB); + HvBexpected = numericalDerivative44(error_, E.rotation(), + E.direction(), vA, vB); Matrix15 HEexpected; HEexpected << HRexpected, HDexpected; Matrix15 HEactual; - E.error(a, b, HEactual); + Matrix13 HvAactual, HvBactual; + E.error(vA, vB, HEactual, HvAactual, HvBactual); // Verify the Jacobian is correct EXPECT(assert_equal(HEexpected, HEactual, 1e-5)); + EXPECT(assert_equal(HvAexpected, HvAactual, 1e-5)); + EXPECT(assert_equal(HvBexpected, HvBactual, 1e-5)); } /* ************************************************************************* */ From e9738c70a62b7a5503eeff7ec1d06ee9674b2519 Mon Sep 17 00:00:00 2001 From: Ayush Baid Date: Wed, 9 Jun 2021 15:22:08 -0700 Subject: [PATCH 14/30] adding jacobians on input points --- gtsam/geometry/EssentialMatrix.cpp | 25 +++++++++++++--- gtsam/geometry/EssentialMatrix.h | 4 ++- gtsam/geometry/tests/testEssentialMatrix.cpp | 29 +++++++++++-------- .../slam/tests/testEssentialMatrixFactor.cpp | 12 +++++--- 4 files changed, 49 insertions(+), 21 deletions(-) diff --git a/gtsam/geometry/EssentialMatrix.cpp b/gtsam/geometry/EssentialMatrix.cpp index 10d387338..cfe76ff04 100644 --- a/gtsam/geometry/EssentialMatrix.cpp +++ b/gtsam/geometry/EssentialMatrix.cpp @@ -101,8 +101,10 @@ EssentialMatrix EssentialMatrix::rotate(const Rot3& cRb, } /* ************************************************************************* */ -double EssentialMatrix::error(const Vector3& vA, const Vector3& vB, // - OptionalJacobian<1, 5> H) const { +double EssentialMatrix::error(const Vector3& vA, const Vector3& vB, + OptionalJacobian<1, 5> HE, + OptionalJacobian<1, 3> HvA, + OptionalJacobian<1, 3> HvB) const { // compute the epipolar lines Point3 lB = E_ * vB; Point3 lA = E_.transpose() * vA; @@ -122,7 +124,7 @@ double EssentialMatrix::error(const Vector3& vA, const Vector3& vB, // double denominator = sqrt(nA_sq_norm + nB_sq_norm); double sampson_error = algebraic_error / denominator; - if (H) { + if (HE) { // See math.lyx // computing the derivatives of the numerator w.r.t. E Matrix13 numerator_H_R = vA.transpose() * E_ * skewSymmetric(-vB); @@ -152,9 +154,24 @@ double EssentialMatrix::error(const Vector3& vA, const Vector3& vB, // Matrix15 numerator_H; numerator_H << numerator_H_R, numerator_H_D; - *H = numerator_H / denominator - + *HE = numerator_H / denominator - algebraic_error * denominator_H / (denominator * denominator); } + + if (HvA){ + Matrix13 numerator_H_vA = vB.transpose() * matrix().transpose(); + Matrix13 denominator_H_vA = nA.transpose() * I * matrix().transpose() / denominator; + + *HvA = numerator_H_vA / denominator - algebraic_error * denominator_H_vA / (denominator * denominator); + } + + if (HvB){ + Matrix13 numerator_H_vB = vA.transpose() * matrix(); + Matrix13 denominator_H_vB = nB.transpose() * I * matrix() / denominator; + + *HvB = numerator_H_vB / denominator - algebraic_error * denominator_H_vB / (denominator * denominator); + } + return sampson_error; } diff --git a/gtsam/geometry/EssentialMatrix.h b/gtsam/geometry/EssentialMatrix.h index 4f698047c..0f8403bc8 100644 --- a/gtsam/geometry/EssentialMatrix.h +++ b/gtsam/geometry/EssentialMatrix.h @@ -161,7 +161,9 @@ class EssentialMatrix { /// epipolar error, sampson GTSAM_EXPORT double error(const Vector3& vA, const Vector3& vB, - OptionalJacobian<1, 5> H = boost::none) const; + OptionalJacobian<1, 5> HE = boost::none, + OptionalJacobian<1, 3> HvA = boost::none, + OptionalJacobian<1, 3> HvB = boost::none) const; /// @} diff --git a/gtsam/geometry/tests/testEssentialMatrix.cpp b/gtsam/geometry/tests/testEssentialMatrix.cpp index acb817ae7..f079e15c5 100644 --- a/gtsam/geometry/tests/testEssentialMatrix.cpp +++ b/gtsam/geometry/tests/testEssentialMatrix.cpp @@ -263,18 +263,14 @@ TEST(EssentialMatrix, errorValue) { } //************************************************************************* -double error_(const Rot3& R, const Unit3& t) { - // Use two points to get error - Point3 a(1, -2, 1); - Point3 b(3, 1, 1); - +double error_(const Rot3& R, const Unit3& t, const Point3& a, const Point3& b) { EssentialMatrix E = EssentialMatrix::FromRotationAndDirection(R, t); return E.error(a, b); } TEST(EssentialMatrix, errorJacobians) { // Use two points to get error - Point3 a(1, -2, 1); - Point3 b(3, 1, 1); + Point3 vA(1, -2, 1); + Point3 vB(3, 1, 1); Rot3 c1Rc2 = Rot3::Ypr(0.1, -0.2, 0.3); Point3 c1Tc2(0.4, 0.5, 0.6); @@ -283,18 +279,27 @@ TEST(EssentialMatrix, errorJacobians) { // Use numerical derivatives to calculate the expected Jacobian Matrix13 HRexpected; Matrix12 HDexpected; - HRexpected = numericalDerivative21(error_, E.rotation(), - E.direction()); - HDexpected = numericalDerivative22(error_, E.rotation(), - E.direction()); + Matrix13 HvAexpected; + Matrix13 HvBexpected; + HRexpected = numericalDerivative41(error_, E.rotation(), + E.direction(), vA, vB); + HDexpected = numericalDerivative42(error_, E.rotation(), + E.direction(), vA, vB); + HvAexpected = numericalDerivative43(error_, E.rotation(), + E.direction(), vA, vB); + HvBexpected = numericalDerivative44(error_, E.rotation(), + E.direction(), vA, vB); Matrix15 HEexpected; HEexpected << HRexpected, HDexpected; Matrix15 HEactual; - E.error(a, b, HEactual); + Matrix13 HvAactual, HvBactual; + E.error(vA, vB, HEactual, HvAactual, HvBactual); // Verify the Jacobian is correct EXPECT(assert_equal(HEexpected, HEactual, 1e-5)); + EXPECT(assert_equal(HvAexpected, HvAactual, 1e-5)); + EXPECT(assert_equal(HvBexpected, HvBactual, 1e-5)); } /* ************************************************************************* */ diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp index 977528b53..36029932d 100644 --- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp +++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp @@ -117,7 +117,8 @@ TEST(EssentialMatrixFactor, ExpressionFactor) { Key key(1); for (size_t i = 0; i < 5; i++) { boost::function)> f = - boost::bind(&EssentialMatrix::error, _1, vA(i), vB(i), _2); + boost::bind(&EssentialMatrix::error, _1, vA(i), vB(i), _2, boost::none, + boost::none); Expression E_(key); // leaf expression Expression expr(f, E_); // unary expression @@ -143,9 +144,12 @@ TEST(EssentialMatrixFactor, ExpressionFactorRotationOnly) { Key key(1); for (size_t i = 0; i < 5; i++) { boost::function)> f = - boost::bind(&EssentialMatrix::error, _1, vA(i), vB(i), _2); - boost::function, - OptionalJacobian<5, 2>)> g; + boost::bind(&EssentialMatrix::error, _1, vA(i), vB(i), _2, boost::none, + boost::none); + boost::function, + OptionalJacobian<5, 2>)> + g; Expression R_(key); Expression d_(trueDirection); Expression E_(&EssentialMatrix::FromRotationAndDirection, R_, d_); From 65211f8b0a88a8b3545fb90f4d8aa52c24e831cd Mon Sep 17 00:00:00 2001 From: Ayush Baid Date: Wed, 9 Jun 2021 16:18:14 -0700 Subject: [PATCH 15/30] moving to squared sampson error --- gtsam/geometry/EssentialMatrix.cpp | 12 +++++++----- gtsam/geometry/EssentialMatrix.h | 2 +- gtsam/geometry/tests/testEssentialMatrix.cpp | 4 ++-- gtsam/slam/tests/testEssentialMatrixFactor.cpp | 8 ++++---- 4 files changed, 14 insertions(+), 12 deletions(-) diff --git a/gtsam/geometry/EssentialMatrix.cpp b/gtsam/geometry/EssentialMatrix.cpp index cfe76ff04..586fcfbb7 100644 --- a/gtsam/geometry/EssentialMatrix.cpp +++ b/gtsam/geometry/EssentialMatrix.cpp @@ -154,25 +154,27 @@ double EssentialMatrix::error(const Vector3& vA, const Vector3& vB, Matrix15 numerator_H; numerator_H << numerator_H_R, numerator_H_D; - *HE = numerator_H / denominator - - algebraic_error * denominator_H / (denominator * denominator); + *HE = 2 * sampson_error * (numerator_H / denominator - + algebraic_error * denominator_H / (denominator * denominator)); } if (HvA){ Matrix13 numerator_H_vA = vB.transpose() * matrix().transpose(); Matrix13 denominator_H_vA = nA.transpose() * I * matrix().transpose() / denominator; - *HvA = numerator_H_vA / denominator - algebraic_error * denominator_H_vA / (denominator * denominator); + *HvA = 2 * sampson_error * (numerator_H_vA / denominator - + algebraic_error * denominator_H_vA / (denominator * denominator)); } if (HvB){ Matrix13 numerator_H_vB = vA.transpose() * matrix(); Matrix13 denominator_H_vB = nB.transpose() * I * matrix() / denominator; - *HvB = numerator_H_vB / denominator - algebraic_error * denominator_H_vB / (denominator * denominator); + *HvB = 2 * sampson_error * (numerator_H_vB / denominator - + algebraic_error * denominator_H_vB / (denominator * denominator)); } - return sampson_error; + return sampson_error * sampson_error; } /* ************************************************************************* */ diff --git a/gtsam/geometry/EssentialMatrix.h b/gtsam/geometry/EssentialMatrix.h index 0f8403bc8..5293f1c7f 100644 --- a/gtsam/geometry/EssentialMatrix.h +++ b/gtsam/geometry/EssentialMatrix.h @@ -159,7 +159,7 @@ class EssentialMatrix { return E.rotate(cRb); } - /// epipolar error, sampson + /// epipolar error, sampson squared GTSAM_EXPORT double error(const Vector3& vA, const Vector3& vB, OptionalJacobian<1, 5> HE = boost::none, OptionalJacobian<1, 3> HvA = boost::none, diff --git a/gtsam/geometry/tests/testEssentialMatrix.cpp b/gtsam/geometry/tests/testEssentialMatrix.cpp index f079e15c5..999bf34b9 100644 --- a/gtsam/geometry/tests/testEssentialMatrix.cpp +++ b/gtsam/geometry/tests/testEssentialMatrix.cpp @@ -254,8 +254,8 @@ TEST(EssentialMatrix, errorValue) { // algebraic error = 5 // norm of line for b = 1 // norm of line for a = 1 - // sampson error = 5 / sqrt(1^2 + 1^2) - double expected = 3.535533906; + // sampson error = 5^2 / 1^2 + 1^2 + double expected = 12.5; // check the error double actual = trueE.error(a, b); diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp index 36029932d..d69eb6d2d 100644 --- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp +++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp @@ -24,7 +24,7 @@ using namespace gtsam; // Noise model for first type of factor is evaluating algebraic error noiseModel::Isotropic::shared_ptr model1 = noiseModel::Isotropic::Sigma(1, - 0.01); + 1e-4); // Noise model for second type of factor is evaluating pixel coordinates noiseModel::Unit::shared_ptr model2 = noiseModel::Unit::Create(2); @@ -196,9 +196,9 @@ TEST (EssentialMatrixFactor, minimization) { (Vector(5) << 0.1, -0.1, 0.1, 0.1, -0.1).finished()); initial.insert(1, initialE); #if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) - EXPECT_DOUBLES_EQUAL(313.85, graph.error(initial), 1e-2); + EXPECT_DOUBLES_EQUAL(59403.51, graph.error(initial), 1e-2); #else - EXPECT_DOUBLES_EQUAL(639.84, graph.error(initial), 1e-2); + EXPECT_DOUBLES_EQUAL(639.84, graph.error(initial), 1e-2); # TODO: redo this error #endif // Optimize @@ -410,7 +410,7 @@ TEST (EssentialMatrixFactor, extraMinimization) { initial.insert(1, initialE); #if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) - EXPECT_DOUBLES_EQUAL(313.85, graph.error(initial), 1e-2); + EXPECT_DOUBLES_EQUAL(59403.51, graph.error(initial), 1e-2); #else EXPECT_DOUBLES_EQUAL(639.84, graph.error(initial), 1e-2); #endif From 2e8bfd60fce7fb127cedc12c3302daa0baa4cde6 Mon Sep 17 00:00:00 2001 From: Ayush Baid Date: Thu, 10 Jun 2021 12:21:32 -0700 Subject: [PATCH 16/30] using correct jacobian computation for calibration --- gtsam/slam/EssentialMatrixFactor.h | 20 +++++++++++--------- 1 file changed, 11 insertions(+), 9 deletions(-) diff --git a/gtsam/slam/EssentialMatrixFactor.h b/gtsam/slam/EssentialMatrixFactor.h index 3e8c45ece..ec4351b86 100644 --- a/gtsam/slam/EssentialMatrixFactor.h +++ b/gtsam/slam/EssentialMatrixFactor.h @@ -80,7 +80,7 @@ class EssentialMatrixFactor : public NoiseModelFactor1 { const EssentialMatrix& E, boost::optional H = boost::none) const override { Vector error(1); - error << E.error(vA_, vB_, H); + error << E.error(vA_, vB_, H, boost::none, boost::none); return error; } @@ -367,20 +367,22 @@ class EssentialMatrixFactor4 Vector3 vA = EssentialMatrix::Homogeneous(cA); Vector3 vB = EssentialMatrix::Homogeneous(cB); + + Matrix13 error_H_vA, error_H_vB; + Vector error(1); + error << E.error(vA, vB, H1, H2 ? &error_H_vA : 0, H2 ? &error_H_vB : 0); + if (H2) { // compute the jacobian of error w.r.t K - // error function f = vA.T * E * vB - // H2 = df/dK = vB.T * E.T * dvA/dK + vA.T * E * dvB/dK + // error function f + // H2 = df/dK = df/dvA * dvA/dK + df/dvB * dvB/dK // where dvA/dK = dvA/dcA * dcA/dK, dVB/dK = dvB/dcB * dcB/dK // and dvA/dcA = dvB/dcB = [[1, 0], [0, 1], [0, 0]] - *H2 = vB.transpose() * E.matrix().transpose().leftCols<2>() * cA_H_K + - vA.transpose() * E.matrix().leftCols<2>() * cB_H_K; + *H2 = error_H_vA.leftCols<2>() * cA_H_K + + error_H_vB.leftCols<2>() * cB_H_K; } - - Vector error(1); - error << E.error(vA, vB, H1); - + return error; } From 91e58f50165a426a1720bb8ee6a3598304fe2baf Mon Sep 17 00:00:00 2001 From: Ayush Baid Date: Thu, 10 Jun 2021 12:21:44 -0700 Subject: [PATCH 17/30] fixing unit tests --- gtsam/slam/tests/testEssentialMatrixFactor.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp index 4c6c950da..42b5c4919 100644 --- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp +++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp @@ -199,7 +199,7 @@ TEST (EssentialMatrixFactor, minimization) { (Vector(5) << 0.1, -0.1, 0.1, 0.1, -0.1).finished()); initial.insert(1, initialE); #if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) - EXPECT_DOUBLES_EQUAL(59403.51, graph.error(initial), 1e-2); + EXPECT_DOUBLES_EQUAL(18161.79, graph.error(initial), 1e-2); #else EXPECT_DOUBLES_EQUAL(639.84, graph.error(initial), 1e-2); # TODO: redo this error #endif @@ -361,7 +361,7 @@ TEST (EssentialMatrixFactor3, minimization) { //************************************************************************* TEST(EssentialMatrixFactor4, factor) { Key keyE(1); - Key keyK(1); + Key keyK(2); for (size_t i = 0; i < 5; i++) { EssentialMatrixFactor4 factor(keyE, keyK, pA(i), pB(i), model1); @@ -408,7 +408,7 @@ TEST(EssentialMatrixFactor4, evaluateErrorJacobians) { Point2 pB(12.0, 15.0); EssentialMatrixFactor4 f(keyE, keyK, pA, pB, model1); - EXPECT_CORRECT_FACTOR_JACOBIANS(f, val, 1e-5, 1e-6); + EXPECT_CORRECT_FACTOR_JACOBIANS(f, val, 1e-5, 1e-5); } //************************************************************************* @@ -434,16 +434,16 @@ TEST(EssentialMatrixFactor4, minimization) { initial.insert(1, initialE); initial.insert(2, initialK); #if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) - EXPECT_DOUBLES_EQUAL(643.26, graph.error(initial), 1e-2); + EXPECT_DOUBLES_EQUAL(2914.92, graph.error(initial), 1e-2); #else EXPECT_DOUBLES_EQUAL(639.84, graph.error(initial), 1e-2); // TODO: update this value too #endif // add prior factor for calibration - Vector5 priorNoiseModelSigma; - priorNoiseModelSigma << 0.1, 0.1, 0.01, 0.1, 0.1; - graph.emplace_shared>(2, initialK, noiseModel::Diagonal::Sigmas(priorNoiseModelSigma)); + // Vector5 priorNoiseModelSigma; + // priorNoiseModelSigma << 0.1, 0.1, 0.01, 0.1, 0.1; + // graph.emplace_shared>(2, initialK, noiseModel::Diagonal::Sigmas(priorNoiseModelSigma)); // Optimize LevenbergMarquardtParams parameters; @@ -667,7 +667,7 @@ TEST(EssentialMatrixFactor4, extraMinimization) { initial.insert(2, initialK); #if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) - EXPECT_DOUBLES_EQUAL(643.62, graph.error(initial), 1e-2); + EXPECT_DOUBLES_EQUAL(59418.96, graph.error(initial), 1e-2); #else EXPECT_DOUBLES_EQUAL(639.84, graph.error(initial), 1e-2); // TODO: fix this #endif From bce9050672ddf73e15a2b32e83074084f534a21c Mon Sep 17 00:00:00 2001 From: Ayush Baid Date: Thu, 10 Jun 2021 14:47:43 -0700 Subject: [PATCH 18/30] adding 11 point example for cal3bundler --- examples/CreateSFMExampleData.cpp | 34 +++++++++++++++++++++++++------ 1 file changed, 28 insertions(+), 6 deletions(-) diff --git a/examples/CreateSFMExampleData.cpp b/examples/CreateSFMExampleData.cpp index 650b3c731..3f46ae8b4 100644 --- a/examples/CreateSFMExampleData.cpp +++ b/examples/CreateSFMExampleData.cpp @@ -111,24 +111,46 @@ void create18PointExample1() { // Create test data, we need 15 points vector P; - P += Point3(-0.1, -0.5, 1), Point3(-0.1, 0, 1), Point3(-0.1, 0.5, 1), - Point3(0, -0.5, 1), Point3(0, 0, 1), Point3(0, 0.5, 1), - Point3(0.1, -0.5, 1), Point3(0.1, 0, 1), Point3(0.1, 0.5, 1), - Point3(-0.1, -0.5, 0.5), Point3(-0.1, 0, 0.5), Point3(-0.1, 0.5, 0.5), - Point3(0, -0.5, 0.5), Point3(0, 0, 0.5), Point3(0, 0.5, 0.5), - Point3(0.1, -0.5, 0.5), Point3(0.1, 0, 0.5), Point3(0.1, 0.5, 0.5); + P += Point3(-0.1, -0.5, 1), Point3(-0.1, 0, 1), Point3(-0.1, 0.5, 1), // + Point3(0, -0.5, 1), Point3(0, 0, 1), Point3(0, 0.5, 1), // + Point3(0.1, -0.5, 1), Point3(0.1, 0, 1), Point3(0.1, 0.5, 1), // + Point3(-0.1, -0.5, 0.5), Point3(-0.1, 0, 0.5), Point3(-0.1, 0.5, 0.5), // + Point3(0, -0.5, 0.5), Point3(0, 0, 0.5), Point3(0, 0.5, 0.5), // + Point3(0.1, -0.5, 0.5), Point3(0.1, 0, 0.5), Point3(0.1, 0.5, 0.5); // Assumes example is run in ${GTSAM_TOP}/build/examples const string filename = "../../examples/data/18pointExample1.txt"; createExampleBALFile(filename, P, pose1, pose2); } +/* ************************************************************************* */ +void create11PointExample2() { + // Create two cameras poses + Rot3 aRb = Rot3::Yaw(M_PI_2); + Point3 aTb(10, 0, 0); + Pose3 pose1, pose2(aRb, aTb); + + // Create test data, we need 11 points + vector P; + P += Point3(0, 0, 100), Point3(0, 0, 100), Point3(0, 0, 100), // + Point3(-10, 0, 100), Point3(10, 0, 100), // + Point3(0, 50, 50), Point3(0, -50, 50), // + Point3(-10, 50, 50), Point3(10, -50, 50), // + Point3(-20, 0, 80), Point3(20, -50, 80); + + // Assumes example is run in ${GTSAM_TOP}/build/examples + const string filename = "../../examples/data/11pointExample2.txt"; + Cal3Bundler K(500, 0, 0); + createExampleBALFile(filename, P, pose1, pose2, K); +} + /* ************************************************************************* */ int main(int argc, char* argv[]) { create5PointExample1(); create5PointExample2(); create18PointExample1(); + create11PointExample2(); return 0; } From 620bcf76cbeb7743ab48246a35925e85049911a9 Mon Sep 17 00:00:00 2001 From: Ayush Baid Date: Thu, 10 Jun 2021 14:51:41 -0700 Subject: [PATCH 19/30] fixing test cases --- .../slam/tests/testEssentialMatrixFactor.cpp | 61 +++++++++++-------- 1 file changed, 35 insertions(+), 26 deletions(-) diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp index 42b5c4919..2d2a25cd7 100644 --- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp +++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp @@ -201,7 +201,8 @@ TEST (EssentialMatrixFactor, minimization) { #if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) EXPECT_DOUBLES_EQUAL(18161.79, graph.error(initial), 1e-2); #else - EXPECT_DOUBLES_EQUAL(639.84, graph.error(initial), 1e-2); # TODO: redo this error + // TODO : redo this error + EXPECT_DOUBLES_EQUAL(639.84, graph.error(initial), 1e-2); #endif // Optimize @@ -408,14 +409,15 @@ TEST(EssentialMatrixFactor4, evaluateErrorJacobians) { Point2 pB(12.0, 15.0); EssentialMatrixFactor4 f(keyE, keyK, pA, pB, model1); - EXPECT_CORRECT_FACTOR_JACOBIANS(f, val, 1e-5, 1e-5); + EXPECT_CORRECT_FACTOR_JACOBIANS(f, val, 1e-5, 3e-5); } //************************************************************************* TEST(EssentialMatrixFactor4, minimization) { // As before, we start with a factor graph and add constraints to it NonlinearFactorGraph graph; - for (size_t i = 0; i < 5; i++) + size_t numberPoints = 11; + for (size_t i = 0; i < numberPoints; i++) graph.emplace_shared>(1, 2, pA(i), pB(i), model1); @@ -434,16 +436,17 @@ TEST(EssentialMatrixFactor4, minimization) { initial.insert(1, initialE); initial.insert(2, initialK); #if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) - EXPECT_DOUBLES_EQUAL(2914.92, graph.error(initial), 1e-2); + EXPECT_DOUBLES_EQUAL(6246.35, graph.error(initial), 1e-2); #else - EXPECT_DOUBLES_EQUAL(639.84, graph.error(initial), - 1e-2); // TODO: update this value too + // TODO: update this value too + EXPECT_DOUBLES_EQUAL(639.84, graph.error(initial), 1e-2); #endif // add prior factor for calibration - // Vector5 priorNoiseModelSigma; - // priorNoiseModelSigma << 0.1, 0.1, 0.01, 0.1, 0.1; - // graph.emplace_shared>(2, initialK, noiseModel::Diagonal::Sigmas(priorNoiseModelSigma)); + Vector5 priorNoiseModelSigma; + priorNoiseModelSigma << 0.3, 0.3, 0.05, 0.3, 0.3; + graph.emplace_shared>( + 2, initialK, noiseModel::Diagonal::Sigmas(priorNoiseModelSigma)); // Optimize LevenbergMarquardtParams parameters; @@ -453,19 +456,19 @@ TEST(EssentialMatrixFactor4, minimization) { // Check result EssentialMatrix actualE = result.at(1); Cal3_S2 actualK = result.at(2); - EXPECT(assert_equal(trueE, actualE, 1e-1)); // TODO: fix the tolerance - EXPECT(assert_equal(trueK, actualK, 1e-2)); // TODO: fix the tolerance + EXPECT(assert_equal(trueE, actualE, 1e-1)); // TODO: fix the tolerance + EXPECT(assert_equal(trueK, actualK, 1e-1)); // TODO: fix the tolerance // Check error at result - EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4); + EXPECT_DOUBLES_EQUAL(0, graph.error(result), 0.3); // Check errors individually - for (size_t i = 0; i < 5; i++) + for (size_t i = 0; i < numberPoints; i++) EXPECT_DOUBLES_EQUAL( 0, actualE.error(EssentialMatrix::Homogeneous(actualK.calibrate(pA(i))), EssentialMatrix::Homogeneous(actualK.calibrate(pB(i)))), - 1e-6); + 1e-5); } } // namespace example1 @@ -474,7 +477,7 @@ TEST(EssentialMatrixFactor4, minimization) { namespace example2 { -const string filename = findExampleDataFile("5pointExample2.txt"); +const string filename = findExampleDataFile("11pointExample2.txt"); SfmData data; bool readOK = readBAL(filename, data); Rot3 aRb = data.cameras[1].pose().rotation(); @@ -523,9 +526,10 @@ TEST(EssentialMatrixFactor, extraMinimization) { initial.insert(1, initialE); #if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) - EXPECT_DOUBLES_EQUAL(59403.51, graph.error(initial), 1e-2); + EXPECT_DOUBLES_EQUAL(7850.88, graph.error(initial), 1e-2); #else - EXPECT_DOUBLES_EQUAL(639.84, graph.error(initial), 1e-2); + // TODO: redo this error + EXPECT_DOUBLES_EQUAL(639.84, graph.error(initial), 1e-2); #endif // Optimize @@ -643,11 +647,13 @@ TEST (EssentialMatrixFactor3, extraTest) { } } +//************************************************************************* TEST(EssentialMatrixFactor4, extraMinimization) { // Additional test with camera moving in positive X direction + size_t numberPoints = 11; NonlinearFactorGraph graph; - for (size_t i = 0; i < 5; i++) + for (size_t i = 0; i < numberPoints; i++) graph.emplace_shared>(1, 2, pA(i), pB(i), model1); @@ -662,20 +668,23 @@ TEST(EssentialMatrixFactor4, extraMinimization) { EssentialMatrix initialE = trueE.retract((Vector(5) << 0.1, -0.1, 0.1, 0.1, -0.1).finished()); Cal3Bundler initialK = - trueK.retract((Vector(3) << 0.1, -0.01, 0.01).finished()); + trueK.retract((Vector(3) << -50, -0.003, 0.003).finished()); initial.insert(1, initialE); initial.insert(2, initialK); #if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) - EXPECT_DOUBLES_EQUAL(59418.96, graph.error(initial), 1e-2); + EXPECT_DOUBLES_EQUAL(242630.09, graph.error(initial), 1e-2); #else - EXPECT_DOUBLES_EQUAL(639.84, graph.error(initial), 1e-2); // TODO: fix this + // TODO: redo this error + EXPECT_DOUBLES_EQUAL(639.84, graph.error(initial), 1e-2); #endif // add prior factor on calibration Vector3 priorNoiseModelSigma; - priorNoiseModelSigma << 0.3, 0.03, 0.03; - graph.emplace_shared>(2, initialK, noiseModel::Diagonal::Sigmas(priorNoiseModelSigma)); + priorNoiseModelSigma << 100, 0.01, 0.01; + // TODO: fix this to work with the prior on initialK + graph.emplace_shared>( + 2, trueK, noiseModel::Diagonal::Sigmas(priorNoiseModelSigma)); // Optimize LevenbergMarquardtParams parameters; @@ -685,14 +694,14 @@ TEST(EssentialMatrixFactor4, extraMinimization) { // Check result EssentialMatrix actualE = result.at(1); Cal3Bundler actualK = result.at(2); - EXPECT(assert_equal(trueE, actualE, 1e-1)); // TODO: tighten tolerance - EXPECT(assert_equal(trueK, actualK, 1e-1)); // TODO: tighten tolerance + EXPECT(assert_equal(trueE, actualE, 1e-1)); // TODO: tighten tolerance + EXPECT(assert_equal(trueK, actualK, 1e-1)); // TODO: tighten tolerance // Check error at result EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4); // Check errors individually - for (size_t i = 0; i < 5; i++) + for (size_t i = 0; i < numberPoints; i++) EXPECT_DOUBLES_EQUAL( 0, actualE.error(EssentialMatrix::Homogeneous(actualK.calibrate(pA(i))), From 14f8b8aa6279e5d407e46282f7ee82d044129adc Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Mon, 14 Jun 2021 01:30:00 +0000 Subject: [PATCH 20/30] removing Sampson error + some tests cleanup --- examples/CreateSFMExampleData.cpp | 37 ++- examples/Data/18pointExample1.txt | 131 ++++++++++ gtsam/geometry/EssentialMatrix.cpp | 80 +----- gtsam/geometry/EssentialMatrix.h | 6 +- gtsam/geometry/tests/testEssentialMatrix.cpp | 62 +---- gtsam/slam/EssentialMatrixFactor.h | 20 +- .../slam/tests/testEssentialMatrixFactor.cpp | 229 +++++++++++++----- 7 files changed, 329 insertions(+), 236 deletions(-) create mode 100644 examples/Data/18pointExample1.txt diff --git a/examples/CreateSFMExampleData.cpp b/examples/CreateSFMExampleData.cpp index 3f46ae8b4..f8f625b17 100644 --- a/examples/CreateSFMExampleData.cpp +++ b/examples/CreateSFMExampleData.cpp @@ -32,11 +32,7 @@ void createExampleBALFile(const string& filename, const vector& P, // Class that will gather all data SfmData data; - - // Create two cameras - Rot3 aRb = Rot3::Yaw(M_PI_2); - Point3 aTb(0.1, 0, 0); - Pose3 identity, aPb(aRb, aTb); + // Create two cameras and add them to data data.cameras.push_back(SfmCamera(pose1, K)); data.cameras.push_back(SfmCamera(pose2, K)); @@ -75,7 +71,7 @@ void create5PointExample1() { Point3(0, 0.5, 0.5), Point3(0, -0.5, 0.5); // Assumes example is run in ${GTSAM_TOP}/build/examples - const string filename = "../../examples/data/5pointExample1.txt"; + const string filename = "../../examples/Data/5pointExample1.txt"; createExampleBALFile(filename, P, pose1, pose2); } @@ -94,7 +90,7 @@ void create5PointExample2() { Point3(0, 50, 50), Point3(0, -50, 50), Point3(-20, 0, 80), Point3(20, -50, 80); // Assumes example is run in ${GTSAM_TOP}/build/examples - const string filename = "../../examples/data/5pointExample2.txt"; + const string filename = "../../examples/Data/5pointExample2.txt"; Cal3Bundler K(500, 0, 0); createExampleBALFile(filename, P, pose1, pose2,K); } @@ -111,20 +107,20 @@ void create18PointExample1() { // Create test data, we need 15 points vector P; - P += Point3(-0.1, -0.5, 1), Point3(-0.1, 0, 1), Point3(-0.1, 0.5, 1), // - Point3(0, -0.5, 1), Point3(0, 0, 1), Point3(0, 0.5, 1), // - Point3(0.1, -0.5, 1), Point3(0.1, 0, 1), Point3(0.1, 0.5, 1), // - Point3(-0.1, -0.5, 0.5), Point3(-0.1, 0, 0.5), Point3(-0.1, 0.5, 0.5), // - Point3(0, -0.5, 0.5), Point3(0, 0, 0.5), Point3(0, 0.5, 0.5), // + P += Point3(0, 0, 1), Point3(-0.1, 0, 1), Point3(0.1, 0, 1), // + Point3(0, 0.5, 0.5), Point3(0, -0.5, 0.5), Point3(-1, -0.5, 2), // + Point3(-1, 0.5, 2), Point3(0.25, -0.5, 1.5), Point3(0.25, 0.5, 1.5),// + Point3(-0.1, -0.5, 0.5), Point3(0.1, -0.5, 1), Point3(0.1, 0.5, 1), // + Point3(-0.1, 0, 0.5), Point3(-0.1, 0.5, 0.5), Point3(0, 0, 0.5), // Point3(0.1, -0.5, 0.5), Point3(0.1, 0, 0.5), Point3(0.1, 0.5, 0.5); // Assumes example is run in ${GTSAM_TOP}/build/examples - const string filename = "../../examples/data/18pointExample1.txt"; + const string filename = "../../examples/Data/18pointExample1.txt"; createExampleBALFile(filename, P, pose1, pose2); } /* ************************************************************************* */ -void create11PointExample2() { +void create11PointExample1() { // Create two cameras poses Rot3 aRb = Rot3::Yaw(M_PI_2); Point3 aTb(10, 0, 0); @@ -132,14 +128,13 @@ void create11PointExample2() { // Create test data, we need 11 points vector P; - P += Point3(0, 0, 100), Point3(0, 0, 100), Point3(0, 0, 100), // - Point3(-10, 0, 100), Point3(10, 0, 100), // - Point3(0, 50, 50), Point3(0, -50, 50), // - Point3(-10, 50, 50), Point3(10, -50, 50), // - Point3(-20, 0, 80), Point3(20, -50, 80); + P += Point3(0, 0, 100), Point3(-10, 0, 100), Point3(10, 0, 100), // + Point3(0, 50, 50), Point3(0, -50, 50), Point3(-20, 0, 80), // + Point3(20, -50, 80), Point3(0, 0, 100), Point3(0, 0, 100), // + Point3(-10, 50, 50), Point3(10, -50, 50); // Assumes example is run in ${GTSAM_TOP}/build/examples - const string filename = "../../examples/data/11pointExample2.txt"; + const string filename = "../../examples/Data/11pointExample1.txt"; Cal3Bundler K(500, 0, 0); createExampleBALFile(filename, P, pose1, pose2, K); } @@ -150,7 +145,7 @@ int main(int argc, char* argv[]) { create5PointExample1(); create5PointExample2(); create18PointExample1(); - create11PointExample2(); + create11PointExample1(); return 0; } diff --git a/examples/Data/18pointExample1.txt b/examples/Data/18pointExample1.txt new file mode 100644 index 000000000..484a7944b --- /dev/null +++ b/examples/Data/18pointExample1.txt @@ -0,0 +1,131 @@ +2 18 36 + +0 0 0 -0 +1 0 -6.123233995736766344e-18 -0.10000000000000000555 +0 1 -0.10000000000000000555 -0 +1 1 -1.2246467991473532688e-17 -0.2000000000000000111 +0 2 0.10000000000000000555 -0 +1 2 0 -0 +0 3 0 -1 +1 3 1 -0.20000000000000006661 +0 4 0 1 +1 4 -1 -0.19999999999999995559 +0 5 -0.5 0.25 +1 5 -0.25000000000000005551 -0.55000000000000004441 +0 6 -0.5 -0.25 +1 6 0.24999999999999997224 -0.55000000000000004441 +0 7 0.16666666666666665741 0.33333333333333331483 +1 7 -0.33333333333333331483 0.10000000000000000555 +0 8 0.16666666666666665741 -0.33333333333333331483 +1 8 0.33333333333333331483 0.099999999999999977796 +0 9 -0.2000000000000000111 1 +1 9 -1 -0.39999999999999996669 +0 10 0.10000000000000000555 0.5 +1 10 -0.5 3.0616169978683830179e-17 +0 11 0.10000000000000000555 -0.5 +1 11 0.5 -3.0616169978683830179e-17 +0 12 -0.2000000000000000111 -0 +1 12 -2.4492935982947065376e-17 -0.4000000000000000222 +0 13 -0.2000000000000000111 -1 +1 13 1 -0.40000000000000007772 +0 14 0 -0 +1 14 -1.2246467991473532688e-17 -0.2000000000000000111 +0 15 0.2000000000000000111 1 +1 15 -1 6.1232339957367660359e-17 +0 16 0.2000000000000000111 -0 +1 16 0 -0 +0 17 0.2000000000000000111 -1 +1 17 1 -6.1232339957367660359e-17 + +3.141592653589793116 + 0 + 0 +-0 +0 +0 +1 +0 +0 + +2.2214414690791830509 +2.2214414690791826068 + 0 +-6.123233995736766344e-18 +-0.10000000000000000555 +0 +1 +0 +0 + +0 +0 +1 + +-0.10000000000000000555 +0 +1 + +0.10000000000000000555 +0 +1 + +0 +0.5 +0.5 + +0 +-0.5 +0.5 + +-1 +-0.5 +2 + +-1 +0.5 +2 + +0.25 +-0.5 +1.5 + +0.25 +0.5 +1.5 + +-0.10000000000000000555 +-0.5 +0.5 + +0.10000000000000000555 +-0.5 +1 + +0.10000000000000000555 +0.5 +1 + +-0.10000000000000000555 +0 +0.5 + +-0.10000000000000000555 +0.5 +0.5 + +0 +0 +0.5 + +0.10000000000000000555 +-0.5 +0.5 + +0.10000000000000000555 +0 +0.5 + +0.10000000000000000555 +0.5 +0.5 + diff --git a/gtsam/geometry/EssentialMatrix.cpp b/gtsam/geometry/EssentialMatrix.cpp index cf8f2a4b8..020c94fdb 100644 --- a/gtsam/geometry/EssentialMatrix.cpp +++ b/gtsam/geometry/EssentialMatrix.cpp @@ -101,80 +101,16 @@ EssentialMatrix EssentialMatrix::rotate(const Rot3& cRb, } /* ************************************************************************* */ -double EssentialMatrix::error(const Vector3& vA, const Vector3& vB, - OptionalJacobian<1, 5> HE, - OptionalJacobian<1, 3> HvA, - OptionalJacobian<1, 3> HvB) const { - // compute the epipolar lines - Point3 lB = E_ * vB; - Point3 lA = E_.transpose() * vA; - - // compute the algebraic error as a simple dot product. - double algebraic_error = dot(vA, lB); - - // compute the line-norms for the two lines - Matrix23 I; - I.setIdentity(); - Matrix21 nA = I * lA; - Matrix21 nB = I * lB; - double nA_sq_norm = nA.squaredNorm(); - double nB_sq_norm = nB.squaredNorm(); - - // compute the normalizing denominator and finally the sampson error - double denominator = sqrt(nA_sq_norm + nB_sq_norm); - double sampson_error = algebraic_error / denominator; - - if (HE) { +double EssentialMatrix::error(const Vector3& vA, const Vector3& vB, // + OptionalJacobian<1, 5> H) const { + if (H) { // See math.lyx - // computing the derivatives of the numerator w.r.t. E - Matrix13 numerator_H_R = vA.transpose() * E_ * skewSymmetric(-vB); - Matrix12 numerator_H_D = vA.transpose() * - skewSymmetric(-rotation().matrix() * vB) * - direction().basis(); - - // computing the derivatives of the denominator w.r.t. E - Matrix12 denominator_H_nA = nA.transpose() / denominator; - Matrix12 denominator_H_nB = nB.transpose() / denominator; - Matrix13 denominator_H_lA = denominator_H_nA * I; - Matrix13 denominator_H_lB = denominator_H_nB * I; - Matrix33 lB_H_R = E_ * skewSymmetric(-vB); - Matrix32 lB_H_D = - skewSymmetric(-rotation().matrix() * vB) * direction().basis(); - Matrix33 lA_H_R = skewSymmetric(E_.matrix().transpose() * vA); - Matrix32 lA_H_D = - rotation().inverse().matrix() * skewSymmetric(vA) * direction().basis(); - - Matrix13 denominator_H_R = - denominator_H_lA * lA_H_R + denominator_H_lB * lB_H_R; - Matrix12 denominator_H_D = - denominator_H_lA * lA_H_D + denominator_H_lB * lB_H_D; - - Matrix15 denominator_H; - denominator_H << denominator_H_R, denominator_H_D; - Matrix15 numerator_H; - numerator_H << numerator_H_R, numerator_H_D; - - *HE = 2 * sampson_error * (numerator_H / denominator - - algebraic_error * denominator_H / (denominator * denominator)); + Matrix13 HR = vA.transpose() * E_ * skewSymmetric(-vB); + Matrix12 HD = vA.transpose() * skewSymmetric(-rotation().matrix() * vB) + * direction().basis(); + *H << HR, HD; } - - if (HvA){ - Matrix13 numerator_H_vA = vB.transpose() * matrix().transpose(); - Matrix13 denominator_H_vA = nA.transpose() * I * matrix().transpose() / denominator; - - *HvA = 2 * sampson_error * (numerator_H_vA / denominator - - algebraic_error * denominator_H_vA / (denominator * denominator)); - } - - if (HvB){ - Matrix13 numerator_H_vB = vA.transpose() * matrix(); - Matrix13 denominator_H_vB = nB.transpose() * I * matrix() / denominator; - - *HvB = 2 * sampson_error * (numerator_H_vB / denominator - - algebraic_error * denominator_H_vB / (denominator * denominator)); - } - - return sampson_error * sampson_error; + return dot(vA, E_ * vB); } /* ************************************************************************* */ diff --git a/gtsam/geometry/EssentialMatrix.h b/gtsam/geometry/EssentialMatrix.h index 5293f1c7f..909576aa0 100644 --- a/gtsam/geometry/EssentialMatrix.h +++ b/gtsam/geometry/EssentialMatrix.h @@ -159,11 +159,9 @@ class EssentialMatrix { return E.rotate(cRb); } - /// epipolar error, sampson squared + /// epipolar error, algebraic GTSAM_EXPORT double error(const Vector3& vA, const Vector3& vB, - OptionalJacobian<1, 5> HE = boost::none, - OptionalJacobian<1, 3> HvA = boost::none, - OptionalJacobian<1, 3> HvB = boost::none) const; + OptionalJacobian<1, 5> H = boost::none) const; /// @} diff --git a/gtsam/geometry/tests/testEssentialMatrix.cpp b/gtsam/geometry/tests/testEssentialMatrix.cpp index 999bf34b9..86a498cdc 100644 --- a/gtsam/geometry/tests/testEssentialMatrix.cpp +++ b/gtsam/geometry/tests/testEssentialMatrix.cpp @@ -241,70 +241,10 @@ TEST (EssentialMatrix, epipoles) { EXPECT(assert_equal(e2, E.epipole_b())); } -//************************************************************************* -TEST(EssentialMatrix, errorValue) { - // Use two points to get error - Point3 a(1, -2, 1); - Point3 b(3, 1, 1); - - // compute the expected error - // E = [0, 0, 0; 0, 0, -1; 1, 0, 0] - // line for b = [0, -1, 3] - // line for a = [1, 0, 2] - // algebraic error = 5 - // norm of line for b = 1 - // norm of line for a = 1 - // sampson error = 5^2 / 1^2 + 1^2 - double expected = 12.5; - - // check the error - double actual = trueE.error(a, b); - EXPECT(assert_equal(expected, actual, 1e-6)); -} - -//************************************************************************* -double error_(const Rot3& R, const Unit3& t, const Point3& a, const Point3& b) { - EssentialMatrix E = EssentialMatrix::FromRotationAndDirection(R, t); - return E.error(a, b); -} -TEST(EssentialMatrix, errorJacobians) { - // Use two points to get error - Point3 vA(1, -2, 1); - Point3 vB(3, 1, 1); - - Rot3 c1Rc2 = Rot3::Ypr(0.1, -0.2, 0.3); - Point3 c1Tc2(0.4, 0.5, 0.6); - EssentialMatrix E(c1Rc2, Unit3(c1Tc2)); - - // Use numerical derivatives to calculate the expected Jacobian - Matrix13 HRexpected; - Matrix12 HDexpected; - Matrix13 HvAexpected; - Matrix13 HvBexpected; - HRexpected = numericalDerivative41(error_, E.rotation(), - E.direction(), vA, vB); - HDexpected = numericalDerivative42(error_, E.rotation(), - E.direction(), vA, vB); - HvAexpected = numericalDerivative43(error_, E.rotation(), - E.direction(), vA, vB); - HvBexpected = numericalDerivative44(error_, E.rotation(), - E.direction(), vA, vB); - Matrix15 HEexpected; - HEexpected << HRexpected, HDexpected; - - Matrix15 HEactual; - Matrix13 HvAactual, HvBactual; - E.error(vA, vB, HEactual, HvAactual, HvBactual); - - // Verify the Jacobian is correct - EXPECT(assert_equal(HEexpected, HEactual, 1e-5)); - EXPECT(assert_equal(HvAexpected, HvAactual, 1e-5)); - EXPECT(assert_equal(HvBexpected, HvBactual, 1e-5)); -} - /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ + diff --git a/gtsam/slam/EssentialMatrixFactor.h b/gtsam/slam/EssentialMatrixFactor.h index ec4351b86..3e8c45ece 100644 --- a/gtsam/slam/EssentialMatrixFactor.h +++ b/gtsam/slam/EssentialMatrixFactor.h @@ -80,7 +80,7 @@ class EssentialMatrixFactor : public NoiseModelFactor1 { const EssentialMatrix& E, boost::optional H = boost::none) const override { Vector error(1); - error << E.error(vA_, vB_, H, boost::none, boost::none); + error << E.error(vA_, vB_, H); return error; } @@ -367,22 +367,20 @@ class EssentialMatrixFactor4 Vector3 vA = EssentialMatrix::Homogeneous(cA); Vector3 vB = EssentialMatrix::Homogeneous(cB); - - Matrix13 error_H_vA, error_H_vB; - Vector error(1); - error << E.error(vA, vB, H1, H2 ? &error_H_vA : 0, H2 ? &error_H_vB : 0); - if (H2) { // compute the jacobian of error w.r.t K - // error function f - // H2 = df/dK = df/dvA * dvA/dK + df/dvB * dvB/dK + // error function f = vA.T * E * vB + // H2 = df/dK = vB.T * E.T * dvA/dK + vA.T * E * dvB/dK // where dvA/dK = dvA/dcA * dcA/dK, dVB/dK = dvB/dcB * dcB/dK // and dvA/dcA = dvB/dcB = [[1, 0], [0, 1], [0, 0]] - *H2 = error_H_vA.leftCols<2>() * cA_H_K - + error_H_vB.leftCols<2>() * cB_H_K; + *H2 = vB.transpose() * E.matrix().transpose().leftCols<2>() * cA_H_K + + vA.transpose() * E.matrix().leftCols<2>() * cB_H_K; } - + + Vector error(1); + error << E.error(vA, vB, H1); + return error; } diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp index 2d2a25cd7..563482651 100644 --- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp +++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp @@ -12,7 +12,6 @@ #include #include #include -#include #include #include #include @@ -25,7 +24,7 @@ using namespace gtsam; // Noise model for first type of factor is evaluating algebraic error noiseModel::Isotropic::shared_ptr model1 = noiseModel::Isotropic::Sigma(1, - 1e-4); + 0.01); // Noise model for second type of factor is evaluating pixel coordinates noiseModel::Unit::shared_ptr model2 = noiseModel::Unit::Create(2); @@ -73,13 +72,13 @@ TEST (EssentialMatrixFactor, testData) { EXPECT(assert_equal(expected, aEb_matrix, 1e-8)); // Check some projections - EXPECT(assert_equal(Point2(-0.1, -0.5), pA(0), 1e-8)); - EXPECT(assert_equal(Point2(-0.5, 0.2), pB(0), 1e-8)); - EXPECT(assert_equal(Point2(0, 0), pA(4), 1e-8)); - EXPECT(assert_equal(Point2(0, 0.1), pB(4), 1e-8)); + EXPECT(assert_equal(Point2(0, 0), pA(0), 1e-8)); + EXPECT(assert_equal(Point2(0, 0.1), pB(0), 1e-8)); + EXPECT(assert_equal(Point2(0, -1), pA(4), 1e-8)); + EXPECT(assert_equal(Point2(-1, 0.2), pB(4), 1e-8)); // Check homogeneous version - EXPECT(assert_equal(Vector3(0, 0.1, 1), vB(4), 1e-8)); + EXPECT(assert_equal(Vector3(-1, 0.2, 1), vB(4), 1e-8)); // Check epipolar constraint for (size_t i = 0; i < 5; i++) @@ -120,8 +119,7 @@ TEST(EssentialMatrixFactor, ExpressionFactor) { Key key(1); for (size_t i = 0; i < 5; i++) { boost::function)> f = - boost::bind(&EssentialMatrix::error, _1, vA(i), vB(i), _2, boost::none, - boost::none); + boost::bind(&EssentialMatrix::error, _1, vA(i), vB(i), _2); Expression E_(key); // leaf expression Expression expr(f, E_); // unary expression @@ -147,12 +145,9 @@ TEST(EssentialMatrixFactor, ExpressionFactorRotationOnly) { Key key(1); for (size_t i = 0; i < 5; i++) { boost::function)> f = - boost::bind(&EssentialMatrix::error, _1, vA(i), vB(i), _2, boost::none, - boost::none); - boost::function, - OptionalJacobian<5, 2>)> - g; + boost::bind(&EssentialMatrix::error, _1, vA(i), vB(i), _2); + boost::function, + OptionalJacobian<5, 2>)> g; Expression R_(key); Expression d_(trueDirection); Expression E_(&EssentialMatrix::FromRotationAndDirection, R_, d_); @@ -199,9 +194,8 @@ TEST (EssentialMatrixFactor, minimization) { (Vector(5) << 0.1, -0.1, 0.1, 0.1, -0.1).finished()); initial.insert(1, initialE); #if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) - EXPECT_DOUBLES_EQUAL(18161.79, graph.error(initial), 1e-2); + EXPECT_DOUBLES_EQUAL(643.26, graph.error(initial), 1e-2); #else - // TODO : redo this error EXPECT_DOUBLES_EQUAL(639.84, graph.error(initial), 1e-2); #endif @@ -374,6 +368,7 @@ TEST(EssentialMatrixFactor4, factor) { Vector actual = factor.evaluateError(trueE, trueK, HEactual, HKactual); EXPECT(assert_equal(expected, actual, 1e-7)); + // Use numerical derivatives to calculate the expected Jacobian Matrix HEexpected; Matrix HKexpected; @@ -393,7 +388,7 @@ TEST(EssentialMatrixFactor4, factor) { } //************************************************************************* -TEST(EssentialMatrixFactor4, evaluateErrorJacobians) { +TEST(EssentialMatrixFactor4, evaluateErrorJacobiansCal3S2) { Key keyE(1); Key keyK(2); // initialize essential matrix @@ -409,15 +404,33 @@ TEST(EssentialMatrixFactor4, evaluateErrorJacobians) { Point2 pB(12.0, 15.0); EssentialMatrixFactor4 f(keyE, keyK, pA, pB, model1); - EXPECT_CORRECT_FACTOR_JACOBIANS(f, val, 1e-5, 3e-5); + EXPECT_CORRECT_FACTOR_JACOBIANS(f, val, 1e-5, 1e-6); } //************************************************************************* -TEST(EssentialMatrixFactor4, minimization) { - // As before, we start with a factor graph and add constraints to it +TEST(EssentialMatrixFactor4, evaluateErrorJacobiansCal3Bundler) { + Key keyE(1); + Key keyK(2); + // initialize essential matrix + Rot3 r = Rot3::Expmap(Vector3(0, 0, M_PI_2)); + Unit3 t(Point3(0.1, 0, 0)); + EssentialMatrix E = EssentialMatrix::FromRotationAndDirection(r, t); + Cal3Bundler K; + Values val; + val.insert(keyE, E); + val.insert(keyK, K); + + Point2 pA(-0.1, 0.5); + Point2 pB(-0.5, -0.2); + + EssentialMatrixFactor4 f(keyE, keyK, pA, pB, model1); + EXPECT_CORRECT_FACTOR_JACOBIANS(f, val, 1e-5, 1e-5); +} + +//************************************************************************* +TEST(EssentialMatrixFactor4, minimizationWithStrongCal3S2Prior) { NonlinearFactorGraph graph; - size_t numberPoints = 11; - for (size_t i = 0; i < numberPoints; i++) + for (size_t i = 0; i < 5; i++) graph.emplace_shared>(1, 2, pA(i), pB(i), model1); @@ -427,57 +440,145 @@ TEST(EssentialMatrixFactor4, minimization) { truth.insert(2, trueK); EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8); + // Initialization + Values initial; + EssentialMatrix initialE = + trueE.retract((Vector(5) << 0.1, -0.1, 0.1, 0.1, -0.1).finished()); + initial.insert(1, initialE); + initial.insert(2, trueK); + + // add prior factor for calibration + Vector5 priorNoiseModelSigma; + priorNoiseModelSigma << 10, 10, 10, 10, 10; + graph.emplace_shared>(2, trueK, noiseModel::Diagonal::Sigmas(priorNoiseModelSigma)); + + LevenbergMarquardtOptimizer optimizer(graph, initial); + Values result = optimizer.optimize(); + + // Check result + EssentialMatrix actualE = result.at(1); + Cal3_S2 actualK = result.at(2); + EXPECT(assert_equal(trueE, actualE, 1e-1)); + EXPECT(assert_equal(trueK, actualK, 1e-2)); + + // 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, + actualE.error(EssentialMatrix::Homogeneous(actualK.calibrate(pA(i))), + EssentialMatrix::Homogeneous(actualK.calibrate(pB(i)))), + 1e-6); +} + +//************************************************************************* +TEST(EssentialMatrixFactor4, minimizationWithWeakCal3S2Prior) { + // We need 7 points here as the prior on the focal length parameters is weak + // and the initialization is noisy. So in total we are trying to optimize 7 DOF, + // with a strong prior on the remaining 3 DOF. + NonlinearFactorGraph graph; + for (size_t i = 0; i < 7; i++) + graph.emplace_shared>(1, 2, pA(i), pB(i), + model1); + + // Check error at ground truth + Values truth; + truth.insert(1, trueE); + truth.insert(2, trueK); + EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8); + + // Initialization + Values initial; + EssentialMatrix initialE = + trueE.retract((Vector(5) << 0.1, -0.1, 0.1, 0.1, -0.1).finished()); + Cal3_S2 initialK = + trueK.retract((Vector(5) << 0.1, -0.1, 0.0, -0.0, 0.0).finished()); + initial.insert(1, initialE); + initial.insert(2, initialK); + + // add prior factor for calibration + Vector5 priorNoiseModelSigma; + priorNoiseModelSigma << 20, 20, 1, 1, 1; + graph.emplace_shared>(2, initialK, noiseModel::Diagonal::Sigmas(priorNoiseModelSigma)); + + LevenbergMarquardtOptimizer optimizer(graph, initial); + Values result = optimizer.optimize(); + + // Check result + EssentialMatrix actualE = result.at(1); + Cal3_S2 actualK = result.at(2); + EXPECT(assert_equal(trueE, actualE, 1e-1)); + EXPECT(assert_equal(trueK, actualK, 1e-2)); + + // Check error at result + EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4); + + // Check errors individually + for (size_t i = 0; i < 7; i++) + EXPECT_DOUBLES_EQUAL( + 0, + actualE.error(EssentialMatrix::Homogeneous(actualK.calibrate(pA(i))), + EssentialMatrix::Homogeneous(actualK.calibrate(pB(i)))), + 1e-5); +} + +//************************************************************************* +TEST(EssentialMatrixFactor4, minimizationWithStrongCal3BundlerPrior) { + NonlinearFactorGraph graph; + for (size_t i = 0; i < 5; i++) + graph.emplace_shared>(1, 2, pA(i), pB(i), + model1); + Cal3Bundler trueK; + // Check error at ground truth + Values truth; + truth.insert(1, trueE); + truth.insert(2, trueK); + 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).finished()); - Cal3_S2 initialK = - trueK.retract((Vector(5) << 0.1, -0.08, 0.01, -0.05, 0.06).finished()); + Cal3Bundler initialK = trueK; initial.insert(1, initialE); initial.insert(2, initialK); -#if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) - EXPECT_DOUBLES_EQUAL(6246.35, graph.error(initial), 1e-2); -#else - // TODO: update this value too - EXPECT_DOUBLES_EQUAL(639.84, graph.error(initial), 1e-2); -#endif // add prior factor for calibration - Vector5 priorNoiseModelSigma; - priorNoiseModelSigma << 0.3, 0.3, 0.05, 0.3, 0.3; - graph.emplace_shared>( - 2, initialK, noiseModel::Diagonal::Sigmas(priorNoiseModelSigma)); - - // Optimize - LevenbergMarquardtParams parameters; - LevenbergMarquardtOptimizer optimizer(graph, initial, parameters); + Vector3 priorNoiseModelSigma; + priorNoiseModelSigma << 1, 1, 1; + graph.emplace_shared>(2, trueK, noiseModel::Diagonal::Sigmas(priorNoiseModelSigma)); + + LevenbergMarquardtOptimizer optimizer(graph, initial); Values result = optimizer.optimize(); // Check result EssentialMatrix actualE = result.at(1); - Cal3_S2 actualK = result.at(2); - EXPECT(assert_equal(trueE, actualE, 1e-1)); // TODO: fix the tolerance - EXPECT(assert_equal(trueK, actualK, 1e-1)); // TODO: fix the tolerance + Cal3Bundler actualK = result.at(2); + EXPECT(assert_equal(trueE, actualE, 1e-1)); + EXPECT(assert_equal(trueK, actualK, 1e-2)); // Check error at result - EXPECT_DOUBLES_EQUAL(0, graph.error(result), 0.3); + EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4); // Check errors individually - for (size_t i = 0; i < numberPoints; i++) + for (size_t i = 0; i < 5; i++) EXPECT_DOUBLES_EQUAL( 0, actualE.error(EssentialMatrix::Homogeneous(actualK.calibrate(pA(i))), EssentialMatrix::Homogeneous(actualK.calibrate(pB(i)))), - 1e-5); + 1e-6); } + } // namespace example1 //************************************************************************* namespace example2 { -const string filename = findExampleDataFile("11pointExample2.txt"); +const string filename = findExampleDataFile("5pointExample2.txt"); SfmData data; bool readOK = readBAL(filename, data); Rot3 aRb = data.cameras[1].pose().rotation(); @@ -526,10 +627,9 @@ TEST(EssentialMatrixFactor, extraMinimization) { initial.insert(1, initialE); #if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) - EXPECT_DOUBLES_EQUAL(7850.88, graph.error(initial), 1e-2); + EXPECT_DOUBLES_EQUAL(643.26, graph.error(initial), 1e-2); #else - // TODO: redo this error - EXPECT_DOUBLES_EQUAL(639.84, graph.error(initial), 1e-2); + EXPECT_DOUBLES_EQUAL(639.84, graph.error(initial), 1e-2); #endif // Optimize @@ -647,13 +747,12 @@ TEST (EssentialMatrixFactor3, extraTest) { } } -//************************************************************************* -TEST(EssentialMatrixFactor4, extraMinimization) { +/* +TEST(EssentialMatrixFactor4, minimizationWithStrongCal3BundlerPrior2) { // Additional test with camera moving in positive X direction - size_t numberPoints = 11; NonlinearFactorGraph graph; - for (size_t i = 0; i < numberPoints; i++) + for (size_t i = 0; i < 5; i++) graph.emplace_shared>(1, 2, pA(i), pB(i), model1); @@ -667,47 +766,43 @@ TEST(EssentialMatrixFactor4, extraMinimization) { Values initial; EssentialMatrix initialE = trueE.retract((Vector(5) << 0.1, -0.1, 0.1, 0.1, -0.1).finished()); - Cal3Bundler initialK = - trueK.retract((Vector(3) << -50, -0.003, 0.003).finished()); + Cal3Bundler initialK = trueK.retract((Vector(3) << 0.1, 0.1, 0.1).finished()); initial.insert(1, initialE); initial.insert(2, initialK); #if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) - EXPECT_DOUBLES_EQUAL(242630.09, graph.error(initial), 1e-2); + EXPECT_DOUBLES_EQUAL(643.62, graph.error(initial), 1e-2); #else - // TODO: redo this error - EXPECT_DOUBLES_EQUAL(639.84, graph.error(initial), 1e-2); + EXPECT_DOUBLES_EQUAL(639.84, graph.error(initial), 1e-2); // TODO: fix this #endif // add prior factor on calibration Vector3 priorNoiseModelSigma; - priorNoiseModelSigma << 100, 0.01, 0.01; - // TODO: fix this to work with the prior on initialK - graph.emplace_shared>( - 2, trueK, noiseModel::Diagonal::Sigmas(priorNoiseModelSigma)); + priorNoiseModelSigma << 1, 1, 1; + graph.emplace_shared>(2, trueK, noiseModel::Diagonal::Sigmas(priorNoiseModelSigma)); // Optimize - LevenbergMarquardtParams parameters; - LevenbergMarquardtOptimizer optimizer(graph, initial, parameters); + LevenbergMarquardtOptimizer optimizer(graph, initial); Values result = optimizer.optimize(); // Check result EssentialMatrix actualE = result.at(1); Cal3Bundler actualK = result.at(2); - EXPECT(assert_equal(trueE, actualE, 1e-1)); // TODO: tighten tolerance - EXPECT(assert_equal(trueK, actualK, 1e-1)); // TODO: tighten tolerance + EXPECT(assert_equal(trueE, actualE, 1e-1)); // TODO: tighten tolerance + EXPECT(assert_equal(trueK, actualK, 1e-1)); // TODO: tighten tolerance // Check error at result EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4); // Check errors individually - for (size_t i = 0; i < numberPoints; i++) + for (size_t i = 0; i < 5; i++) EXPECT_DOUBLES_EQUAL( 0, actualE.error(EssentialMatrix::Homogeneous(actualK.calibrate(pA(i))), EssentialMatrix::Homogeneous(actualK.calibrate(pB(i)))), 1e-6); } +*/ } // namespace example2 From 285f0413a8c91de59d83b1e4173fc61fcf5d8da3 Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Mon, 14 Jun 2021 01:57:59 +0000 Subject: [PATCH 21/30] increasing calibrate() tolerance --- gtsam/slam/tests/testEssentialMatrixFactor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp index 563482651..54b12a292 100644 --- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp +++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp @@ -530,7 +530,7 @@ TEST(EssentialMatrixFactor4, minimizationWithStrongCal3BundlerPrior) { for (size_t i = 0; i < 5; i++) graph.emplace_shared>(1, 2, pA(i), pB(i), model1); - Cal3Bundler trueK; + Cal3Bundler trueK(1, 0, 0, 0, 0, /*tolerance=*/5e-3); // Check error at ground truth Values truth; truth.insert(1, trueE); From 373b109228f925c2fe9647e64d13ac5f1595c505 Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Mon, 14 Jun 2021 02:02:01 +0000 Subject: [PATCH 22/30] small covariance change --- gtsam/slam/tests/testEssentialMatrixFactor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp index 54b12a292..19234bec2 100644 --- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp +++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp @@ -547,7 +547,7 @@ TEST(EssentialMatrixFactor4, minimizationWithStrongCal3BundlerPrior) { // add prior factor for calibration Vector3 priorNoiseModelSigma; - priorNoiseModelSigma << 1, 1, 1; + priorNoiseModelSigma << 0.1, 0.1, 0.1; graph.emplace_shared>(2, trueK, noiseModel::Diagonal::Sigmas(priorNoiseModelSigma)); LevenbergMarquardtOptimizer optimizer(graph, initial); From 01515d10013d94b6987144f47fc3db2738c5c204 Mon Sep 17 00:00:00 2001 From: akrishnan86 Date: Sun, 13 Jun 2021 20:30:04 -0700 Subject: [PATCH 23/30] formatting changes --- examples/CreateSFMExampleData.cpp | 44 ++-- .../slam/tests/testEssentialMatrixFactor.cpp | 204 +++++++++--------- 2 files changed, 118 insertions(+), 130 deletions(-) diff --git a/examples/CreateSFMExampleData.cpp b/examples/CreateSFMExampleData.cpp index f8f625b17..e99d4ed3e 100644 --- a/examples/CreateSFMExampleData.cpp +++ b/examples/CreateSFMExampleData.cpp @@ -15,8 +15,8 @@ * @author Frank Dellaert */ -#include #include +#include #include @@ -27,17 +27,15 @@ using namespace gtsam; /* ************************************************************************* */ void createExampleBALFile(const string& filename, const vector& P, - const Pose3& pose1, const Pose3& pose2, const Cal3Bundler& K = - Cal3Bundler()) { - + const Pose3& pose1, const Pose3& pose2, + const Cal3Bundler& K = Cal3Bundler()) { // Class that will gather all data SfmData data; // Create two cameras and add them to data data.cameras.push_back(SfmCamera(pose1, K)); data.cameras.push_back(SfmCamera(pose2, K)); - for(const Point3& p: P) { - + for (const Point3& p : P) { // Create the track SfmTrack track; track.p = p; @@ -47,7 +45,7 @@ void createExampleBALFile(const string& filename, const vector& P, // Project points in both cameras for (size_t i = 0; i < 2; i++) - track.measurements.push_back(make_pair(i, data.cameras[i].project(p))); + track.measurements.push_back(make_pair(i, data.cameras[i].project(p))); // Add track to data data.tracks.push_back(track); @@ -59,7 +57,6 @@ void createExampleBALFile(const string& filename, const vector& P, /* ************************************************************************* */ void create5PointExample1() { - // Create two cameras poses Rot3 aRb = Rot3::Yaw(M_PI_2); Point3 aTb(0.1, 0, 0); @@ -67,8 +64,8 @@ void create5PointExample1() { // Create test data, we need at least 5 points vector P; - P += Point3(0, 0, 1), Point3(-0.1, 0, 1), Point3(0.1, 0, 1), // - Point3(0, 0.5, 0.5), Point3(0, -0.5, 0.5); + P += Point3(0, 0, 1), Point3(-0.1, 0, 1), Point3(0.1, 0, 1), // + Point3(0, 0.5, 0.5), Point3(0, -0.5, 0.5); // Assumes example is run in ${GTSAM_TOP}/build/examples const string filename = "../../examples/Data/5pointExample1.txt"; @@ -78,7 +75,6 @@ void create5PointExample1() { /* ************************************************************************* */ void create5PointExample2() { - // Create two cameras poses Rot3 aRb = Rot3::Yaw(M_PI_2); Point3 aTb(10, 0, 0); @@ -86,20 +82,19 @@ void create5PointExample2() { // Create test data, we need at least 5 points vector P; - P += Point3(0, 0, 100), Point3(-10, 0, 100), Point3(10, 0, 100), // - Point3(0, 50, 50), Point3(0, -50, 50), Point3(-20, 0, 80), Point3(20, -50, 80); + P += Point3(0, 0, 100), Point3(-10, 0, 100), Point3(10, 0, 100), // + Point3(0, 50, 50), Point3(0, -50, 50), Point3(-20, 0, 80), + Point3(20, -50, 80); // Assumes example is run in ${GTSAM_TOP}/build/examples const string filename = "../../examples/Data/5pointExample2.txt"; Cal3Bundler K(500, 0, 0); - createExampleBALFile(filename, P, pose1, pose2,K); + createExampleBALFile(filename, P, pose1, pose2, K); } - /* ************************************************************************* */ void create18PointExample1() { - // Create two cameras poses Rot3 aRb = Rot3::Yaw(M_PI_2); Point3 aTb(0.1, 0, 0); @@ -107,11 +102,11 @@ void create18PointExample1() { // Create test data, we need 15 points vector P; - P += Point3(0, 0, 1), Point3(-0.1, 0, 1), Point3(0.1, 0, 1), // - Point3(0, 0.5, 0.5), Point3(0, -0.5, 0.5), Point3(-1, -0.5, 2), // - Point3(-1, 0.5, 2), Point3(0.25, -0.5, 1.5), Point3(0.25, 0.5, 1.5),// - Point3(-0.1, -0.5, 0.5), Point3(0.1, -0.5, 1), Point3(0.1, 0.5, 1), // - Point3(-0.1, 0, 0.5), Point3(-0.1, 0.5, 0.5), Point3(0, 0, 0.5), // + P += Point3(0, 0, 1), Point3(-0.1, 0, 1), Point3(0.1, 0, 1), // + Point3(0, 0.5, 0.5), Point3(0, -0.5, 0.5), Point3(-1, -0.5, 2), // + Point3(-1, 0.5, 2), Point3(0.25, -0.5, 1.5), Point3(0.25, 0.5, 1.5), // + Point3(-0.1, -0.5, 0.5), Point3(0.1, -0.5, 1), Point3(0.1, 0.5, 1), // + Point3(-0.1, 0, 0.5), Point3(-0.1, 0.5, 0.5), Point3(0, 0, 0.5), // Point3(0.1, -0.5, 0.5), Point3(0.1, 0, 0.5), Point3(0.1, 0.5, 0.5); // Assumes example is run in ${GTSAM_TOP}/build/examples @@ -128,9 +123,9 @@ void create11PointExample1() { // Create test data, we need 11 points vector P; - P += Point3(0, 0, 100), Point3(-10, 0, 100), Point3(10, 0, 100), // - Point3(0, 50, 50), Point3(0, -50, 50), Point3(-20, 0, 80), // - Point3(20, -50, 80), Point3(0, 0, 100), Point3(0, 0, 100), // + P += Point3(0, 0, 100), Point3(-10, 0, 100), Point3(10, 0, 100), // + Point3(0, 50, 50), Point3(0, -50, 50), Point3(-20, 0, 80), // + Point3(20, -50, 80), Point3(0, 0, 100), Point3(0, 0, 100), // Point3(-10, 50, 50), Point3(10, -50, 50); // Assumes example is run in ${GTSAM_TOP}/build/examples @@ -150,4 +145,3 @@ int main(int argc, char* argv[]) { } /* ************************************************************************* */ - diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp index 19234bec2..433684f69 100644 --- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp +++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp @@ -5,26 +5,24 @@ * @date December 17, 2013 */ -#include - -#include -#include -#include -#include -#include -#include -#include +#include #include #include - -#include +#include +#include +#include +#include +#include +#include +#include +#include using namespace std; using namespace gtsam; // Noise model for first type of factor is evaluating algebraic error -noiseModel::Isotropic::shared_ptr model1 = noiseModel::Isotropic::Sigma(1, - 0.01); +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); @@ -45,30 +43,22 @@ PinholeCamera camera2(data.cameras[1].pose(), trueK); Rot3 trueRotation(c1Rc2); Unit3 trueDirection(c1Tc2); EssentialMatrix trueE(trueRotation, trueDirection); -double baseline = 0.1; // actual baseline of the camera +double baseline = 0.1; // actual baseline of the camera -Point2 pA(size_t i) { - return data.tracks[i].measurements[0].second; -} -Point2 pB(size_t i) { - return data.tracks[i].measurements[1].second; -} -Vector vA(size_t i) { - return EssentialMatrix::Homogeneous(pA(i)); -} -Vector vB(size_t i) { - return EssentialMatrix::Homogeneous(pB(i)); -} +Point2 pA(size_t i) { return data.tracks[i].measurements[0].second; } +Point2 pB(size_t i) { return data.tracks[i].measurements[1].second; } +Vector vA(size_t i) { return EssentialMatrix::Homogeneous(pA(i)); } +Vector vB(size_t i) { return EssentialMatrix::Homogeneous(pB(i)); } //************************************************************************* -TEST (EssentialMatrixFactor, testData) { +TEST(EssentialMatrixFactor, testData) { CHECK(readOK); // Check E matrix Matrix expected(3, 3); expected << 0, 0, 0, 0, 0, -0.1, 0.1, 0, 0; - Matrix aEb_matrix = skewSymmetric(c1Tc2.x(), c1Tc2.y(), c1Tc2.z()) - * c1Rc2.matrix(); + Matrix aEb_matrix = + skewSymmetric(c1Tc2.x(), c1Tc2.y(), c1Tc2.z()) * c1Rc2.matrix(); EXPECT(assert_equal(expected, aEb_matrix, 1e-8)); // Check some projections @@ -90,7 +80,7 @@ TEST (EssentialMatrixFactor, testData) { } //************************************************************************* -TEST (EssentialMatrixFactor, factor) { +TEST(EssentialMatrixFactor, factor) { Key key(1); for (size_t i = 0; i < 5; i++) { EssentialMatrixFactor factor(key, pA(i), pB(i), model1); @@ -104,10 +94,11 @@ TEST (EssentialMatrixFactor, factor) { // Use numerical derivatives to calculate the expected Jacobian Matrix Hexpected; - typedef Eigen::Matrix Vector1; + typedef Eigen::Matrix Vector1; Hexpected = numericalDerivative11( boost::bind(&EssentialMatrixFactor::evaluateError, &factor, _1, - boost::none), trueE); + boost::none), + trueE); // Verify the Jacobian is correct EXPECT(assert_equal(Hexpected, Hactual, 1e-8)); @@ -118,10 +109,10 @@ TEST (EssentialMatrixFactor, factor) { TEST(EssentialMatrixFactor, ExpressionFactor) { Key key(1); for (size_t i = 0; i < 5; i++) { - boost::function)> f = + boost::function)> f = boost::bind(&EssentialMatrix::error, _1, vA(i), vB(i), _2); - Expression E_(key); // leaf expression - Expression expr(f, E_); // unary expression + Expression E_(key); // leaf expression + Expression expr(f, E_); // unary expression // Test the derivatives using Paul's magic Values values; @@ -144,13 +135,16 @@ TEST(EssentialMatrixFactor, ExpressionFactor) { TEST(EssentialMatrixFactor, ExpressionFactorRotationOnly) { Key key(1); for (size_t i = 0; i < 5; i++) { - boost::function)> f = + boost::function)> f = boost::bind(&EssentialMatrix::error, _1, vA(i), vB(i), _2); - boost::function, - OptionalJacobian<5, 2>)> g; + boost::function, + OptionalJacobian<5, 2>)> + g; Expression R_(key); Expression d_(trueDirection); - Expression E_(&EssentialMatrix::FromRotationAndDirection, R_, d_); + Expression E_(&EssentialMatrix::FromRotationAndDirection, + R_, d_); Expression expr(f, E_); // Test the derivatives using Paul's magic @@ -171,7 +165,7 @@ TEST(EssentialMatrixFactor, ExpressionFactorRotationOnly) { } //************************************************************************* -TEST (EssentialMatrixFactor, minimization) { +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 @@ -190,8 +184,8 @@ TEST (EssentialMatrixFactor, minimization) { // Check error at initial estimate Values initial; - EssentialMatrix initialE = trueE.retract( - (Vector(5) << 0.1, -0.1, 0.1, 0.1, -0.1).finished()); + EssentialMatrix initialE = + trueE.retract((Vector(5) << 0.1, -0.1, 0.1, 0.1, -0.1).finished()); initial.insert(1, initialE); #if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) EXPECT_DOUBLES_EQUAL(643.26, graph.error(initial), 1e-2); @@ -214,11 +208,10 @@ TEST (EssentialMatrixFactor, minimization) { // 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, factor) { +TEST(EssentialMatrixFactor2, factor) { for (size_t i = 0; i < 5; i++) { EssentialMatrixFactor2 factor(100, i, pA(i), pB(i), model2); @@ -234,11 +227,13 @@ TEST (EssentialMatrixFactor2, factor) { // 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); + 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)); @@ -247,7 +242,7 @@ TEST (EssentialMatrixFactor2, factor) { } //************************************************************************* -TEST (EssentialMatrixFactor2, minimization) { +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 @@ -290,8 +285,7 @@ TEST (EssentialMatrixFactor2, minimization) { EssentialMatrix bodyE = cRb.inverse() * trueE; //************************************************************************* -TEST (EssentialMatrixFactor3, factor) { - +TEST(EssentialMatrixFactor3, factor) { for (size_t i = 0; i < 5; i++) { EssentialMatrixFactor3 factor(100, i, pA(i), pB(i), cRb, model2); @@ -307,11 +301,13 @@ TEST (EssentialMatrixFactor3, factor) { // Use numerical derivatives to calculate the expected Jacobian Matrix Hexpected1, Hexpected2; - boost::function f = boost::bind( - &EssentialMatrixFactor3::evaluateError, &factor, _1, _2, boost::none, - boost::none); - Hexpected1 = numericalDerivative21(f, bodyE, d); - Hexpected2 = numericalDerivative22(f, bodyE, d); + boost::function f = + boost::bind(&EssentialMatrixFactor3::evaluateError, &factor, _1, _2, + boost::none, boost::none); + Hexpected1 = + numericalDerivative21(f, bodyE, d); + Hexpected2 = + numericalDerivative22(f, bodyE, d); // Verify the Jacobian is correct EXPECT(assert_equal(Hexpected1, Hactual1, 1e-8)); @@ -320,13 +316,13 @@ TEST (EssentialMatrixFactor3, factor) { } //************************************************************************* -TEST (EssentialMatrixFactor3, minimization) { - +TEST(EssentialMatrixFactor3, minimization) { // As before, we start with a factor graph and add constraints to it NonlinearFactorGraph graph; for (size_t i = 0; i < 5; i++) // but now we specify the rotation bRc - graph.emplace_shared(100, i, pA(i), pB(i), cRb, model2); + graph.emplace_shared(100, i, pA(i), pB(i), cRb, + model2); // Check error at ground truth Values truth; @@ -368,7 +364,6 @@ TEST(EssentialMatrixFactor4, factor) { Vector actual = factor.evaluateError(trueE, trueK, HEactual, HKactual); EXPECT(assert_equal(expected, actual, 1e-7)); - // Use numerical derivatives to calculate the expected Jacobian Matrix HEexpected; Matrix HKexpected; @@ -445,13 +440,14 @@ TEST(EssentialMatrixFactor4, minimizationWithStrongCal3S2Prior) { EssentialMatrix initialE = trueE.retract((Vector(5) << 0.1, -0.1, 0.1, 0.1, -0.1).finished()); initial.insert(1, initialE); - initial.insert(2, trueK); + initial.insert(2, trueK); // add prior factor for calibration Vector5 priorNoiseModelSigma; priorNoiseModelSigma << 10, 10, 10, 10, 10; - graph.emplace_shared>(2, trueK, noiseModel::Diagonal::Sigmas(priorNoiseModelSigma)); - + graph.emplace_shared>( + 2, trueK, noiseModel::Diagonal::Sigmas(priorNoiseModelSigma)); + LevenbergMarquardtOptimizer optimizer(graph, initial); Values result = optimizer.optimize(); @@ -476,8 +472,8 @@ TEST(EssentialMatrixFactor4, minimizationWithStrongCal3S2Prior) { //************************************************************************* TEST(EssentialMatrixFactor4, minimizationWithWeakCal3S2Prior) { // We need 7 points here as the prior on the focal length parameters is weak - // and the initialization is noisy. So in total we are trying to optimize 7 DOF, - // with a strong prior on the remaining 3 DOF. + // and the initialization is noisy. So in total we are trying to optimize 7 + // DOF, with a strong prior on the remaining 3 DOF. NonlinearFactorGraph graph; for (size_t i = 0; i < 7; i++) graph.emplace_shared>(1, 2, pA(i), pB(i), @@ -501,8 +497,9 @@ TEST(EssentialMatrixFactor4, minimizationWithWeakCal3S2Prior) { // add prior factor for calibration Vector5 priorNoiseModelSigma; priorNoiseModelSigma << 20, 20, 1, 1, 1; - graph.emplace_shared>(2, initialK, noiseModel::Diagonal::Sigmas(priorNoiseModelSigma)); - + graph.emplace_shared>( + 2, initialK, noiseModel::Diagonal::Sigmas(priorNoiseModelSigma)); + LevenbergMarquardtOptimizer optimizer(graph, initial); Values result = optimizer.optimize(); @@ -528,8 +525,8 @@ TEST(EssentialMatrixFactor4, minimizationWithWeakCal3S2Prior) { TEST(EssentialMatrixFactor4, minimizationWithStrongCal3BundlerPrior) { NonlinearFactorGraph graph; for (size_t i = 0; i < 5; i++) - graph.emplace_shared>(1, 2, pA(i), pB(i), - model1); + graph.emplace_shared>(1, 2, pA(i), + pB(i), model1); Cal3Bundler trueK(1, 0, 0, 0, 0, /*tolerance=*/5e-3); // Check error at ground truth Values truth; @@ -548,8 +545,9 @@ TEST(EssentialMatrixFactor4, minimizationWithStrongCal3BundlerPrior) { // add prior factor for calibration Vector3 priorNoiseModelSigma; priorNoiseModelSigma << 0.1, 0.1, 0.1; - graph.emplace_shared>(2, trueK, noiseModel::Diagonal::Sigmas(priorNoiseModelSigma)); - + graph.emplace_shared>( + 2, trueK, noiseModel::Diagonal::Sigmas(priorNoiseModelSigma)); + LevenbergMarquardtOptimizer optimizer(graph, initial); Values result = optimizer.optimize(); @@ -571,7 +569,6 @@ TEST(EssentialMatrixFactor4, minimizationWithStrongCal3BundlerPrior) { 1e-6); } - } // namespace example1 //************************************************************************* @@ -585,14 +582,10 @@ Rot3 aRb = data.cameras[1].pose().rotation(); Point3 aTb = data.cameras[1].pose().translation(); EssentialMatrix trueE(aRb, Unit3(aTb)); -double baseline = 10; // actual baseline of the camera +double baseline = 10; // actual baseline of the camera -Point2 pA(size_t i) { - return data.tracks[i].measurements[0].second; -} -Point2 pB(size_t i) { - return data.tracks[i].measurements[1].second; -} +Point2 pA(size_t i) { return data.tracks[i].measurements[0].second; } +Point2 pB(size_t i) { return data.tracks[i].measurements[1].second; } Cal3Bundler trueK = Cal3Bundler(500, 0, 0); boost::shared_ptr K = boost::make_shared(trueK); @@ -622,8 +615,8 @@ TEST(EssentialMatrixFactor, extraMinimization) { // Check error at initial estimate Values initial; - EssentialMatrix initialE = trueE.retract( - (Vector(5) << 0.1, -0.1, 0.1, 0.1, -0.1).finished()); + EssentialMatrix initialE = + trueE.retract((Vector(5) << 0.1, -0.1, 0.1, 0.1, -0.1).finished()); initial.insert(1, initialE); #if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) @@ -647,11 +640,10 @@ TEST(EssentialMatrixFactor, extraMinimization) { // 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) { +TEST(EssentialMatrixFactor2, extraTest) { for (size_t i = 0; i < 5; i++) { EssentialMatrixFactor2 factor(100, i, pA(i), pB(i), model2, K); @@ -667,11 +659,13 @@ TEST (EssentialMatrixFactor2, extraTest) { // 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); + 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-6)); @@ -680,14 +674,15 @@ TEST (EssentialMatrixFactor2, extraTest) { } //************************************************************************* -TEST (EssentialMatrixFactor2, extraMinimization) { +TEST(EssentialMatrixFactor2, extraMinimization) { // Additional test with camera moving in positive X direction // We start with a factor graph and add constraints to it // Noise sigma is 1, assuming pixel measurements NonlinearFactorGraph graph; for (size_t i = 0; i < data.number_tracks(); i++) - graph.emplace_shared(100, i, pA(i), pB(i), model2, K); + graph.emplace_shared(100, i, pA(i), pB(i), model2, + K); // Check error at ground truth Values truth; @@ -715,8 +710,7 @@ TEST (EssentialMatrixFactor2, extraMinimization) { } //************************************************************************* -TEST (EssentialMatrixFactor3, extraTest) { - +TEST(EssentialMatrixFactor3, extraTest) { // The "true E" in the body frame is EssentialMatrix bodyE = cRb.inverse() * trueE; @@ -735,18 +729,19 @@ TEST (EssentialMatrixFactor3, extraTest) { // Use numerical derivatives to calculate the expected Jacobian Matrix Hexpected1, Hexpected2; - boost::function f = boost::bind( - &EssentialMatrixFactor3::evaluateError, &factor, _1, _2, boost::none, - boost::none); - Hexpected1 = numericalDerivative21(f, bodyE, d); - Hexpected2 = numericalDerivative22(f, bodyE, d); + boost::function f = + boost::bind(&EssentialMatrixFactor3::evaluateError, &factor, _1, _2, + boost::none, boost::none); + Hexpected1 = + numericalDerivative21(f, bodyE, d); + Hexpected2 = + numericalDerivative22(f, bodyE, d); // Verify the Jacobian is correct EXPECT(assert_equal(Hexpected1, Hactual1, 1e-6)); EXPECT(assert_equal(Hexpected2, Hactual2, 1e-8)); } } - /* TEST(EssentialMatrixFactor4, minimizationWithStrongCal3BundlerPrior2) { // Additional test with camera moving in positive X direction @@ -773,13 +768,14 @@ TEST(EssentialMatrixFactor4, minimizationWithStrongCal3BundlerPrior2) { #if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) EXPECT_DOUBLES_EQUAL(643.62, graph.error(initial), 1e-2); #else - EXPECT_DOUBLES_EQUAL(639.84, graph.error(initial), 1e-2); // TODO: fix this + EXPECT_DOUBLES_EQUAL(639.84, graph.error(initial), 1e-2); // TODO: fix this #endif // add prior factor on calibration Vector3 priorNoiseModelSigma; priorNoiseModelSigma << 1, 1, 1; - graph.emplace_shared>(2, trueK, noiseModel::Diagonal::Sigmas(priorNoiseModelSigma)); + graph.emplace_shared>( + 2, trueK, noiseModel::Diagonal::Sigmas(priorNoiseModelSigma)); // Optimize LevenbergMarquardtOptimizer optimizer(graph, initial); @@ -788,8 +784,8 @@ TEST(EssentialMatrixFactor4, minimizationWithStrongCal3BundlerPrior2) { // Check result EssentialMatrix actualE = result.at(1); Cal3Bundler actualK = result.at(2); - EXPECT(assert_equal(trueE, actualE, 1e-1)); // TODO: tighten tolerance - EXPECT(assert_equal(trueK, actualK, 1e-1)); // TODO: tighten tolerance + EXPECT(assert_equal(trueE, actualE, 1e-1)); // TODO: tighten tolerance + EXPECT(assert_equal(trueK, actualK, 1e-1)); // TODO: tighten tolerance // Check error at result EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4); @@ -803,7 +799,6 @@ TEST(EssentialMatrixFactor4, minimizationWithStrongCal3BundlerPrior2) { 1e-6); } */ - } // namespace example2 /* ************************************************************************* */ @@ -812,4 +807,3 @@ int main() { return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ - From c4582941bf1120a380d872b023543cc7db8cc60f Mon Sep 17 00:00:00 2001 From: akrishnan86 Date: Sun, 13 Jun 2021 20:33:37 -0700 Subject: [PATCH 24/30] removing duplicate data file --- examples/data/18pointExample1.txt | 131 ------------------------------ 1 file changed, 131 deletions(-) delete mode 100644 examples/data/18pointExample1.txt diff --git a/examples/data/18pointExample1.txt b/examples/data/18pointExample1.txt deleted file mode 100644 index e7d186294..000000000 --- a/examples/data/18pointExample1.txt +++ /dev/null @@ -1,131 +0,0 @@ -2 18 36 - -0 0 -0.10000000000000000555 0.5 -1 0 -0.5 -0.19999999999999998335 -0 1 -0.10000000000000000555 -0 -1 1 -1.2246467991473532688e-17 -0.2000000000000000111 -0 2 -0.10000000000000000555 -0.5 -1 2 0.5 -0.20000000000000003886 -0 3 0 0.5 -1 3 -0.5 -0.099999999999999977796 -0 4 0 -0 -1 4 -6.123233995736766344e-18 -0.10000000000000000555 -0 5 0 -0.5 -1 5 0.5 -0.10000000000000003331 -0 6 0.10000000000000000555 0.5 -1 6 -0.5 3.0616169978683830179e-17 -0 7 0.10000000000000000555 -0 -1 7 0 -0 -0 8 0.10000000000000000555 -0.5 -1 8 0.5 -3.0616169978683830179e-17 -0 9 -0.2000000000000000111 1 -1 9 -1 -0.39999999999999996669 -0 10 -0.2000000000000000111 -0 -1 10 -2.4492935982947065376e-17 -0.4000000000000000222 -0 11 -0.2000000000000000111 -1 -1 11 1 -0.40000000000000007772 -0 12 0 1 -1 12 -1 -0.19999999999999995559 -0 13 0 -0 -1 13 -1.2246467991473532688e-17 -0.2000000000000000111 -0 14 0 -1 -1 14 1 -0.20000000000000006661 -0 15 0.2000000000000000111 1 -1 15 -1 6.1232339957367660359e-17 -0 16 0.2000000000000000111 -0 -1 16 0 -0 -0 17 0.2000000000000000111 -1 -1 17 1 -6.1232339957367660359e-17 - -3.141592653589793116 - 0 - 0 --0 -0 -0 -1 -0 -0 - -2.2214414690791830509 -2.2214414690791826068 - 0 --6.123233995736766344e-18 --0.10000000000000000555 -0 -1 -0 -0 - --0.10000000000000000555 --0.5 -1 - --0.10000000000000000555 -0 -1 - --0.10000000000000000555 -0.5 -1 - -0 --0.5 -1 - -0 -0 -1 - -0 -0.5 -1 - -0.10000000000000000555 --0.5 -1 - -0.10000000000000000555 -0 -1 - -0.10000000000000000555 -0.5 -1 - --0.10000000000000000555 --0.5 -0.5 - --0.10000000000000000555 -0 -0.5 - --0.10000000000000000555 -0.5 -0.5 - -0 --0.5 -0.5 - -0 -0 -0.5 - -0 -0.5 -0.5 - -0.10000000000000000555 --0.5 -0.5 - -0.10000000000000000555 -0 -0.5 - -0.10000000000000000555 -0.5 -0.5 - From 545dfd0be732a3a4be7d3c2c9f411ab9a79fec95 Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Thu, 17 Jun 2021 01:36:57 +0000 Subject: [PATCH 25/30] removing failing test and unused data --- examples/CreateSFMExampleData.cpp | 21 ------- .../slam/tests/testEssentialMatrixFactor.cpp | 57 ------------------- 2 files changed, 78 deletions(-) diff --git a/examples/CreateSFMExampleData.cpp b/examples/CreateSFMExampleData.cpp index f8f625b17..eae806607 100644 --- a/examples/CreateSFMExampleData.cpp +++ b/examples/CreateSFMExampleData.cpp @@ -119,33 +119,12 @@ void create18PointExample1() { createExampleBALFile(filename, P, pose1, pose2); } -/* ************************************************************************* */ -void create11PointExample1() { - // Create two cameras poses - Rot3 aRb = Rot3::Yaw(M_PI_2); - Point3 aTb(10, 0, 0); - Pose3 pose1, pose2(aRb, aTb); - - // Create test data, we need 11 points - vector P; - P += Point3(0, 0, 100), Point3(-10, 0, 100), Point3(10, 0, 100), // - Point3(0, 50, 50), Point3(0, -50, 50), Point3(-20, 0, 80), // - Point3(20, -50, 80), Point3(0, 0, 100), Point3(0, 0, 100), // - Point3(-10, 50, 50), Point3(10, -50, 50); - - // Assumes example is run in ${GTSAM_TOP}/build/examples - const string filename = "../../examples/Data/11pointExample1.txt"; - Cal3Bundler K(500, 0, 0); - createExampleBALFile(filename, P, pose1, pose2, K); -} - /* ************************************************************************* */ int main(int argc, char* argv[]) { create5PointExample1(); create5PointExample2(); create18PointExample1(); - create11PointExample1(); return 0; } diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp index 19234bec2..b58d4f9d3 100644 --- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp +++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp @@ -747,63 +747,6 @@ TEST (EssentialMatrixFactor3, extraTest) { } } -/* -TEST(EssentialMatrixFactor4, minimizationWithStrongCal3BundlerPrior2) { - // Additional test with camera moving in positive X direction - - NonlinearFactorGraph graph; - for (size_t i = 0; i < 5; i++) - graph.emplace_shared>(1, 2, pA(i), - pB(i), model1); - - // Check error at ground truth - Values truth; - truth.insert(1, trueE); - truth.insert(2, trueK); - 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).finished()); - Cal3Bundler initialK = trueK.retract((Vector(3) << 0.1, 0.1, 0.1).finished()); - initial.insert(1, initialE); - initial.insert(2, initialK); - -#if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) - EXPECT_DOUBLES_EQUAL(643.62, graph.error(initial), 1e-2); -#else - EXPECT_DOUBLES_EQUAL(639.84, graph.error(initial), 1e-2); // TODO: fix this -#endif - - // add prior factor on calibration - Vector3 priorNoiseModelSigma; - priorNoiseModelSigma << 1, 1, 1; - graph.emplace_shared>(2, trueK, noiseModel::Diagonal::Sigmas(priorNoiseModelSigma)); - - // Optimize - LevenbergMarquardtOptimizer optimizer(graph, initial); - Values result = optimizer.optimize(); - - // Check result - EssentialMatrix actualE = result.at(1); - Cal3Bundler actualK = result.at(2); - EXPECT(assert_equal(trueE, actualE, 1e-1)); // TODO: tighten tolerance - EXPECT(assert_equal(trueK, actualK, 1e-1)); // TODO: tighten tolerance - - // 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, - actualE.error(EssentialMatrix::Homogeneous(actualK.calibrate(pA(i))), - EssentialMatrix::Homogeneous(actualK.calibrate(pB(i)))), - 1e-6); -} -*/ - } // namespace example2 /* ************************************************************************* */ From 47f9f30b391ccffdcd91db57bd1ceec21ef97a5b Mon Sep 17 00:00:00 2001 From: akrishnan86 Date: Wed, 16 Jun 2021 19:08:43 -0700 Subject: [PATCH 26/30] updating documentation for factor --- gtsam/slam/EssentialMatrixFactor.h | 22 +++++++++++++--------- 1 file changed, 13 insertions(+), 9 deletions(-) diff --git a/gtsam/slam/EssentialMatrixFactor.h b/gtsam/slam/EssentialMatrixFactor.h index 3e8c45ece..62d7531d9 100644 --- a/gtsam/slam/EssentialMatrixFactor.h +++ b/gtsam/slam/EssentialMatrixFactor.h @@ -294,12 +294,17 @@ class EssentialMatrixFactor3 : public EssentialMatrixFactor2 { // EssentialMatrixFactor3 /** - * Factor that evaluates algebraic epipolar error (K^-1 p)'E (K^-1 p) for given - * essential matrix and calibration. The calibration is shared between two + * Binary factor that optimizes for E and calibration K using the algebraic + * epipolar error (K^-1 pA)'E (K^-1 pB). The calibration is shared between two * images. - * - * Note: as correspondences between 2d coordinates can only recover 7 DoF, + * + * Note: As correspondences between 2d coordinates can only recover 7 DoF, * this factor should always be used with a prior factor on calibration. + * Even with a prior, we can only optimize 2 DoF in the calibration. So the + * prior should have a noise model with very low sigma in the remaining + * dimensions. This has been tested to work on Cal3_S2. With Cal3Bundler, it + * works only with a strong prior (low sigma noisemodel) on all degrees of + * freedom. */ template class EssentialMatrixFactor4 @@ -316,15 +321,14 @@ class EssentialMatrixFactor4 public: /** * Constructor - * @param keyE Essential Matrix variable key - * @param keyK Calibration variable key + * @param keyE Essential Matrix (from camera B to A) variable key + * @param keyK Calibration variable key (common for both cameras) * @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 */ - EssentialMatrixFactor4(Key keyE, Key keyK, - const Point2& pA, const Point2& pB, + EssentialMatrixFactor4(Key keyE, Key keyK, const Point2& pA, const Point2& pB, const SharedNoiseModel& model) : Base(model, keyE, keyK), pA_(pA), pB_(pB) {} @@ -345,7 +349,7 @@ class EssentialMatrixFactor4 } /** - * @brief Calculate the algebraic epipolar error p' (K^-1)' E K p. + * @brief Calculate the algebraic epipolar error pA' (K^-1)' E K pB. * * @param E essential matrix for key keyE * @param K calibration (common for both images) for key keyK From e3b6c8308aada2c470974140fa24be5d8ad6db7b Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Mon, 21 Jun 2021 03:47:10 +0000 Subject: [PATCH 27/30] updating points name, constexpr --- examples/CreateSFMExampleData.cpp | 35 ++++++++++++++---------------- gtsam/slam/EssentialMatrixFactor.h | 2 +- 2 files changed, 17 insertions(+), 20 deletions(-) diff --git a/examples/CreateSFMExampleData.cpp b/examples/CreateSFMExampleData.cpp index 15c009036..3a1e905e8 100644 --- a/examples/CreateSFMExampleData.cpp +++ b/examples/CreateSFMExampleData.cpp @@ -26,7 +26,7 @@ using namespace gtsam; /* ************************************************************************* */ -void createExampleBALFile(const string& filename, const vector& P, +void createExampleBALFile(const string& filename, const vector& points, const Pose3& pose1, const Pose3& pose2, const Cal3Bundler& K = Cal3Bundler()) { // Class that will gather all data @@ -35,7 +35,7 @@ void createExampleBALFile(const string& filename, const vector& P, data.cameras.push_back(SfmCamera(pose1, K)); data.cameras.push_back(SfmCamera(pose2, K)); - for (const Point3& p : P) { + for (const Point3& p : points) { // Create the track SfmTrack track; track.p = p; @@ -63,13 +63,12 @@ void create5PointExample1() { Pose3 pose1, pose2(aRb, aTb); // Create test data, we need at least 5 points - vector P; - P += Point3(0, 0, 1), Point3(-0.1, 0, 1), Point3(0.1, 0, 1), // - Point3(0, 0.5, 0.5), Point3(0, -0.5, 0.5); + vector points = {{0, 0, 1}, {-0.1, 0, 1}, {0.1, 0, 1}, // + {0, 0.5, 0.5}, {0, -0.5, 0.5}}; // Assumes example is run in ${GTSAM_TOP}/build/examples const string filename = "../../examples/Data/5pointExample1.txt"; - createExampleBALFile(filename, P, pose1, pose2); + createExampleBALFile(filename, points, pose1, pose2); } /* ************************************************************************* */ @@ -81,15 +80,14 @@ void create5PointExample2() { Pose3 pose1, pose2(aRb, aTb); // Create test data, we need at least 5 points - vector P; - P += Point3(0, 0, 100), Point3(-10, 0, 100), Point3(10, 0, 100), // - Point3(0, 50, 50), Point3(0, -50, 50), Point3(-20, 0, 80), - Point3(20, -50, 80); + vector points = {{0, 0, 100}, {-10, 0, 100}, {10, 0, 100}, // + {0, 50, 50}, {0, -50, 50}, {-20, 0, 80}, + {20, -50, 80}}; // Assumes example is run in ${GTSAM_TOP}/build/examples const string filename = "../../examples/Data/5pointExample2.txt"; Cal3Bundler K(500, 0, 0); - createExampleBALFile(filename, P, pose1, pose2, K); + createExampleBALFile(filename, points, pose1, pose2, K); } /* ************************************************************************* */ @@ -101,17 +99,16 @@ void create18PointExample1() { Pose3 pose1, pose2(aRb, aTb); // Create test data, we need 15 points - vector P; - P += Point3(0, 0, 1), Point3(-0.1, 0, 1), Point3(0.1, 0, 1), // - Point3(0, 0.5, 0.5), Point3(0, -0.5, 0.5), Point3(-1, -0.5, 2), // - Point3(-1, 0.5, 2), Point3(0.25, -0.5, 1.5), Point3(0.25, 0.5, 1.5), // - Point3(-0.1, -0.5, 0.5), Point3(0.1, -0.5, 1), Point3(0.1, 0.5, 1), // - Point3(-0.1, 0, 0.5), Point3(-0.1, 0.5, 0.5), Point3(0, 0, 0.5), // - Point3(0.1, -0.5, 0.5), Point3(0.1, 0, 0.5), Point3(0.1, 0.5, 0.5); + vector points = {{0, 0, 1}, {-0.1, 0, 1}, {0.1, 0, 1}, // + {0, 0.5, 0.5}, {0, -0.5, 0.5}, {-1, -0.5, 2}, // + {-1, 0.5, 2}, {0.25, -0.5, 1.5}, {0.25, 0.5, 1.5}, // + {-0.1, -0.5, 0.5}, {0.1, -0.5, 1}, {0.1, 0.5, 1}, // + {-0.1, 0, 0.5}, {-0.1, 0.5, 0.5}, {0, 0, 0.5}, // + {0.1, -0.5, 0.5}, {0.1, 0, 0.5}, {0.1, 0.5, 0.5}}; // Assumes example is run in ${GTSAM_TOP}/build/examples const string filename = "../../examples/Data/18pointExample1.txt"; - createExampleBALFile(filename, P, pose1, pose2); + createExampleBALFile(filename, points, pose1, pose2); } int main(int argc, char* argv[]) { diff --git a/gtsam/slam/EssentialMatrixFactor.h b/gtsam/slam/EssentialMatrixFactor.h index 62d7531d9..787efac51 100644 --- a/gtsam/slam/EssentialMatrixFactor.h +++ b/gtsam/slam/EssentialMatrixFactor.h @@ -315,7 +315,7 @@ class EssentialMatrixFactor4 typedef NoiseModelFactor2 Base; typedef EssentialMatrixFactor4 This; - static const int DimK = FixedDimension::value; + static constexpr int DimK = FixedDimension::value; typedef Eigen::Matrix JacobianCalibration; public: From a69f9e59b4272eddc5790855fd1c500774eedff8 Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Mon, 21 Jun 2021 03:47:44 +0000 Subject: [PATCH 28/30] changing to macro EssenstialMatrixfactor4 --- .../slam/tests/testEssentialMatrixFactor.cpp | 24 +++++-------------- 1 file changed, 6 insertions(+), 18 deletions(-) diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp index 89ed8f0ae..d4f7b2365 100644 --- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp +++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp @@ -14,6 +14,7 @@ #include #include #include +#include #include #include @@ -359,26 +360,13 @@ TEST(EssentialMatrixFactor4, factor) { // Check evaluation Vector1 expected; expected << 0; - Matrix HEactual; - Matrix HKactual; - Vector actual = factor.evaluateError(trueE, trueK, HEactual, HKactual); + Vector actual = factor.evaluateError(trueE, trueK); EXPECT(assert_equal(expected, actual, 1e-7)); - // Use numerical derivatives to calculate the expected Jacobian - Matrix HEexpected; - Matrix HKexpected; - typedef Eigen::Matrix Vector1; - boost::function f = - boost::bind(&EssentialMatrixFactor4::evaluateError, factor, _1, - _2, boost::none, boost::none); - HEexpected = numericalDerivative21( - f, trueE, trueK); - HKexpected = numericalDerivative22( - f, trueE, trueK); - - // Verify the Jacobian is correct - EXPECT(assert_equal(HEexpected, HEactual, 1e-8)); - EXPECT(assert_equal(HKexpected, HKactual, 1e-8)); + Values truth; + truth.insert(keyE, trueE); + truth.insert(keyK, trueK); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, truth, 1e-6, 1e-7); } } From 119e43aeac695a492b23960439070d66abf24d92 Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Mon, 21 Jun 2021 05:21:19 +0000 Subject: [PATCH 29/30] all jacobian tests for essential matrix use macro --- .../slam/tests/testEssentialMatrixFactor.cpp | 91 +++++-------------- 1 file changed, 22 insertions(+), 69 deletions(-) diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp index d4f7b2365..78857262f 100644 --- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp +++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp @@ -7,7 +7,6 @@ #include #include -#include #include #include #include @@ -89,20 +88,12 @@ TEST(EssentialMatrixFactor, factor) { // Check evaluation Vector expected(1); expected << 0; - Matrix Hactual; - Vector actual = factor.evaluateError(trueE, Hactual); + Vector actual = factor.evaluateError(trueE); EXPECT(assert_equal(expected, actual, 1e-7)); - // Use numerical derivatives to calculate the expected Jacobian - Matrix Hexpected; - typedef Eigen::Matrix Vector1; - Hexpected = numericalDerivative11( - boost::bind(&EssentialMatrixFactor::evaluateError, &factor, _1, - boost::none), - trueE); - - // Verify the Jacobian is correct - EXPECT(assert_equal(Hexpected, Hactual, 1e-8)); + Values val; + val.insert(key, trueE); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, val, 1e-5, 1e-7); } } @@ -226,19 +217,10 @@ TEST(EssentialMatrixFactor2, factor) { 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)); + Values val; + val.insert(100, trueE); + val.insert(i, d); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, val, 1e-5, 1e-7); } } @@ -300,19 +282,10 @@ TEST(EssentialMatrixFactor3, factor) { Vector actual = factor.evaluateError(bodyE, 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(&EssentialMatrixFactor3::evaluateError, &factor, _1, _2, - boost::none, boost::none); - Hexpected1 = - numericalDerivative21(f, bodyE, d); - Hexpected2 = - numericalDerivative22(f, bodyE, d); - - // Verify the Jacobian is correct - EXPECT(assert_equal(Hexpected1, Hactual1, 1e-8)); - EXPECT(assert_equal(Hexpected2, Hactual2, 1e-8)); + Values val; + val.insert(100, bodyE); + val.insert(i, d); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, val, 1e-6, 1e-7); } } @@ -640,24 +613,14 @@ TEST(EssentialMatrixFactor2, extraTest) { const Point2 pi = camera2.project(P1); Point2 expected(pi - pB(i)); - Matrix Hactual1, Hactual2; double d(baseline / P1.z()); - Vector actual = factor.evaluateError(trueE, d, Hactual1, Hactual2); + Vector actual = factor.evaluateError(trueE, d); 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-6)); - EXPECT(assert_equal(Hexpected2, Hactual2, 1e-8)); + Values val; + val.insert(100, trueE); + val.insert(i, d); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, val, 1e-5, 1e-6); } } @@ -710,24 +673,14 @@ TEST(EssentialMatrixFactor3, extraTest) { const Point2 pi = camera2.project(P1); Point2 expected(pi - pB(i)); - Matrix Hactual1, Hactual2; double d(baseline / P1.z()); - Vector actual = factor.evaluateError(bodyE, d, Hactual1, Hactual2); + Vector actual = factor.evaluateError(bodyE, d); EXPECT(assert_equal(expected, actual, 1e-7)); - // Use numerical derivatives to calculate the expected Jacobian - Matrix Hexpected1, Hexpected2; - boost::function f = - boost::bind(&EssentialMatrixFactor3::evaluateError, &factor, _1, _2, - boost::none, boost::none); - Hexpected1 = - numericalDerivative21(f, bodyE, d); - Hexpected2 = - numericalDerivative22(f, bodyE, d); - - // Verify the Jacobian is correct - EXPECT(assert_equal(Hexpected1, Hactual1, 1e-6)); - EXPECT(assert_equal(Hexpected2, Hactual2, 1e-8)); + Values val; + val.insert(100, bodyE); + val.insert(i, d); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, val, 1e-5, 1e-6); } } From 01561bc217921cf7fa0658dabef25e9cd69a82a0 Mon Sep 17 00:00:00 2001 From: akrishnan86 Date: Sun, 20 Jun 2021 22:26:19 -0700 Subject: [PATCH 30/30] formatting example --- examples/CreateSFMExampleData.cpp | 23 ++++++++++++----------- 1 file changed, 12 insertions(+), 11 deletions(-) diff --git a/examples/CreateSFMExampleData.cpp b/examples/CreateSFMExampleData.cpp index 3a1e905e8..97f4c84dc 100644 --- a/examples/CreateSFMExampleData.cpp +++ b/examples/CreateSFMExampleData.cpp @@ -63,8 +63,8 @@ void create5PointExample1() { Pose3 pose1, pose2(aRb, aTb); // Create test data, we need at least 5 points - vector points = {{0, 0, 1}, {-0.1, 0, 1}, {0.1, 0, 1}, // - {0, 0.5, 0.5}, {0, -0.5, 0.5}}; + vector points = { + {0, 0, 1}, {-0.1, 0, 1}, {0.1, 0, 1}, {0, 0.5, 0.5}, {0, -0.5, 0.5}}; // Assumes example is run in ${GTSAM_TOP}/build/examples const string filename = "../../examples/Data/5pointExample1.txt"; @@ -80,9 +80,9 @@ void create5PointExample2() { Pose3 pose1, pose2(aRb, aTb); // Create test data, we need at least 5 points - vector points = {{0, 0, 100}, {-10, 0, 100}, {10, 0, 100}, // - {0, 50, 50}, {0, -50, 50}, {-20, 0, 80}, - {20, -50, 80}}; + vector points = {{0, 0, 100}, {-10, 0, 100}, {10, 0, 100}, // + {0, 50, 50}, {0, -50, 50}, {-20, 0, 80}, // + {20, -50, 80}}; // Assumes example is run in ${GTSAM_TOP}/build/examples const string filename = "../../examples/Data/5pointExample2.txt"; @@ -99,12 +99,13 @@ void create18PointExample1() { Pose3 pose1, pose2(aRb, aTb); // Create test data, we need 15 points - vector points = {{0, 0, 1}, {-0.1, 0, 1}, {0.1, 0, 1}, // - {0, 0.5, 0.5}, {0, -0.5, 0.5}, {-1, -0.5, 2}, // - {-1, 0.5, 2}, {0.25, -0.5, 1.5}, {0.25, 0.5, 1.5}, // - {-0.1, -0.5, 0.5}, {0.1, -0.5, 1}, {0.1, 0.5, 1}, // - {-0.1, 0, 0.5}, {-0.1, 0.5, 0.5}, {0, 0, 0.5}, // - {0.1, -0.5, 0.5}, {0.1, 0, 0.5}, {0.1, 0.5, 0.5}}; + vector points = { + {0, 0, 1}, {-0.1, 0, 1}, {0.1, 0, 1}, // + {0, 0.5, 0.5}, {0, -0.5, 0.5}, {-1, -0.5, 2}, // + {-1, 0.5, 2}, {0.25, -0.5, 1.5}, {0.25, 0.5, 1.5}, // + {-0.1, -0.5, 0.5}, {0.1, -0.5, 1}, {0.1, 0.5, 1}, // + {-0.1, 0, 0.5}, {-0.1, 0.5, 0.5}, {0, 0, 0.5}, // + {0.1, -0.5, 0.5}, {0.1, 0, 0.5}, {0.1, 0.5, 0.5}}; // Assumes example is run in ${GTSAM_TOP}/build/examples const string filename = "../../examples/Data/18pointExample1.txt";