From 8ca7f1ff1d69d68c083a716dad9e0a0eb36172fc Mon Sep 17 00:00:00 2001 From: Ayush Baid Date: Fri, 23 Apr 2021 16:29:03 -0400 Subject: [PATCH 01/40] 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 0000000000..17dbe98a04 --- /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 0000000000..b156df01e2 --- /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/40] 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 17dbe98a04..568a94585f 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 @@ class EssentialMatrixWithCalibrationFactor: public NoiseModelFactor2 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 @@ class EssentialMatrixWithCalibrationFactor: public NoiseModelFactor2 - -#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/40] 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 909576aa01..5eb11f8ed6 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 86a498cdc7..fd3fb64f08 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 a99ac9512e..786b9596b4 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 @@ class EssentialMatrixFactor: public NoiseModelFactor1 { * @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 @@ class EssentialMatrixFactor: public NoiseModelFactor1 { } /// 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 @@ class EssentialMatrixFactor: public NoiseModelFactor1 { * 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 @@ class EssentialMatrixFactor2: public NoiseModelFactor2 * @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 @@ class EssentialMatrixFactor2: public NoiseModelFactor2 * @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 @@ class EssentialMatrixFactor2: public NoiseModelFactor2 } /// 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 @@ class EssentialMatrixFactor2: public NoiseModelFactor2 * @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 @@ class EssentialMatrixFactor2: public NoiseModelFactor2 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 @@ class EssentialMatrixFactor2: public NoiseModelFactor2 * 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 @@ class EssentialMatrixFactor3: public EssentialMatrixFactor2 { * @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 @@ class EssentialMatrixFactor3: public EssentialMatrixFactor2 { * @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 @@ class EssentialMatrixFactor3: public EssentialMatrixFactor2 { } /// 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 @@ class EssentialMatrixFactor3: public EssentialMatrixFactor2 { * @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 @@ class EssentialMatrixFactor3: public EssentialMatrixFactor2 { 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 568a94585f..0000000000 --- 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 95db611d76..3a53157f68 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 4d77e1fcd3..0000000000 --- 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/40] 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 786b9596b4..01d6233323 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/40] 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 01d6233323..0c81cfc4b5 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/40] 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 5eb11f8ed6..909576aa01 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 fd3fb64f08..86a498cdc7 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 0c81cfc4b5..9d51852edb 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/40] 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 9d51852edb..3e8c45ecea 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 3a53157f68..08908d4995 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/40] 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 08908d4995..6904ba4f58 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/40] 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 aa0bde8f6d..650b3c7317 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 0000000000..e7d1862941 --- /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/40] 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 020c94fdb0..446228fcc6 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 909576aa01..4f698047c8 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 86a498cdc7..71ea44bd10 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/40] 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 446228fcc6..10d3873385 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 71ea44bd10..acb817ae73 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/40] 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 95db611d76..977528b53b 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/40] 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 10d3873385..cfe76ff044 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 4f698047c8..0f8403bc82 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 acb817ae73..f079e15c55 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/40] 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 10d3873385..cfe76ff044 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 4f698047c8..0f8403bc82 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 acb817ae73..f079e15c55 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 977528b53b..36029932da 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/40] 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 cfe76ff044..586fcfbb78 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 0f8403bc82..5293f1c7fa 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 f079e15c55..999bf34b9b 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 36029932da..d69eb6d2dc 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/40] 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 3e8c45ecea..ec4351b860 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/40] 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 4c6c950da2..42b5c49198 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/40] 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 650b3c7317..3f46ae8b40 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/40] 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 42b5c49198..2d2a25cd77 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 342ab73ecc4f9e9d4775a3587e162400c8c5ccc8 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 13 Jun 2021 10:22:51 -0400 Subject: [PATCH 20/40] merge double into Values templates --- gtsam/gtsam.i | 29 ++++++++++++++++++++++++----- 1 file changed, 24 insertions(+), 5 deletions(-) diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index 400e98a8e5..94d10953bb 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -2257,6 +2257,7 @@ class Values { void insert(size_t j, const gtsam::PinholeCamera& camera); void insert(size_t j, const gtsam::imuBias::ConstantBias& constant_bias); void insert(size_t j, const gtsam::NavState& nav_state); + void insert(size_t j, double c); void update(size_t j, const gtsam::Point2& point2); void update(size_t j, const gtsam::Point3& point3); @@ -2278,13 +2279,31 @@ class Values { void update(size_t j, const gtsam::NavState& nav_state); void update(size_t j, Vector vector); void update(size_t j, Matrix matrix); - - template, gtsam::imuBias::ConstantBias, gtsam::NavState, Vector, Matrix}> + void update(size_t j, double c); + + template , + gtsam::imuBias::ConstantBias, + gtsam::NavState, + Vector, + Matrix, + double}> T at(size_t j); - /// version for double - void insertDouble(size_t j, double c); - double atDouble(size_t j) const; }; #include From 52bf1cd765f52941cc2ea2394832d2f6a5ae622e Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 13 Jun 2021 10:24:15 -0400 Subject: [PATCH 21/40] add cmake command to run GTSAM python tests --- python/CMakeLists.txt | 10 ++++++++++ python/README.md | 8 ++------ 2 files changed, 12 insertions(+), 6 deletions(-) diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index fb4d891488..5f51368e6a 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -142,3 +142,13 @@ add_custom_target(${GTSAM_PYTHON_INSTALL_TARGET} COMMAND ${PYTHON_EXECUTABLE} ${GTSAM_PYTHON_BUILD_DIRECTORY}/setup.py install DEPENDS ${GTSAM_PYTHON_DEPENDENCIES} WORKING_DIRECTORY ${GTSAM_PYTHON_BUILD_DIRECTORY}) + +# Custom make command to run all GTSAM Python tests +add_custom_target( + python-test + COMMAND + ${CMAKE_COMMAND} -E env # add package to python path so no need to install + "PYTHONPATH=${GTSAM_PYTHON_BUILD_DIRECTORY}/$ENV{PYTHONPATH}" + ${PYTHON_EXECUTABLE} -m unittest discover + DEPENDS ${GTSAM_PYTHON_DEPENDENCIES} + WORKING_DIRECTORY ${GTSAM_PYTHON_BUILD_DIRECTORY}/gtsam/tests) diff --git a/python/README.md b/python/README.md index c485d12bdc..ec9808ce0d 100644 --- a/python/README.md +++ b/python/README.md @@ -35,12 +35,8 @@ For instructions on updating the version of the [wrap library](https://github.co ## Unit Tests The Python toolbox also has a small set of unit tests located in the -test directory. To run them: - - ```bash - cd /python/gtsam/tests - python -m unittest discover - ``` +test directory. +To run them, use `make python-test`. ## Utils From 14f8b8aa6279e5d407e46282f7ee82d044129adc Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Mon, 14 Jun 2021 01:30:00 +0000 Subject: [PATCH 22/40] 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 | 223 +++++++++++++----- 7 files changed, 326 insertions(+), 233 deletions(-) create mode 100644 examples/Data/18pointExample1.txt diff --git a/examples/CreateSFMExampleData.cpp b/examples/CreateSFMExampleData.cpp index 3f46ae8b40..f8f625b179 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 0000000000..484a7944b4 --- /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 cf8f2a4b83..020c94fdb0 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 5293f1c7fa..909576aa01 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 999bf34b9b..86a498cdc7 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 ec4351b860..3e8c45ecea 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 2d2a25cd77..5634826510 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,43 +440,83 @@ TEST(EssentialMatrixFactor4, minimization) { truth.insert(2, trueK); EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8); - // Check error at initial estimate + // 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.08, 0.01, -0.05, 0.06).finished()); + trueK.retract((Vector(5) << 0.1, -0.1, 0.0, -0.0, 0.0).finished()); 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); + 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)); // TODO: fix the tolerance - EXPECT(assert_equal(trueK, actualK, 1e-1)); // TODO: fix the tolerance + 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 < 7; i++) EXPECT_DOUBLES_EQUAL( 0, actualE.error(EssentialMatrix::Homogeneous(actualK.calibrate(pA(i))), @@ -471,13 +524,61 @@ TEST(EssentialMatrixFactor4, minimization) { 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()); + Cal3Bundler initialK = trueK; + initial.insert(1, initialE); + initial.insert(2, initialK); + + // add prior factor for calibration + 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); + 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), 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 //************************************************************************* 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 23/40] 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 5634826510..54b12a2928 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 24/40] 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 54b12a2928..19234bec29 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 25/40] 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 f8f625b179..e99d4ed3ed 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 19234bec29..433684f694 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 26/40] 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 e7d1862941..0000000000 --- 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 a60bac2bc7926c074f03b29372ecfddd9d937f53 Mon Sep 17 00:00:00 2001 From: acxz <17132214+acxz@users.noreply.github.com> Date: Mon, 14 Jun 2021 02:31:35 -0400 Subject: [PATCH 27/40] use size_t variable type --- gtsam/nonlinear/NonlinearFactorGraph.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/nonlinear/NonlinearFactorGraph.cpp b/gtsam/nonlinear/NonlinearFactorGraph.cpp index 42fe5ae57d..8e4cf277c2 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.cpp +++ b/gtsam/nonlinear/NonlinearFactorGraph.cpp @@ -354,7 +354,7 @@ GaussianFactorGraph::shared_ptr NonlinearFactorGraph::linearize(const Values& li _LinearizeOneFactor(*this, linearizationPoint, *linearFG)); // Linearize all non-sendable factors - for(int i = 0; i < size(); i++) { + for(size_t i = 0; i < size(); i++) { auto& factor = (*this)[i]; if(factor && !(factor->sendable())) { (*linearFG)[i] = factor->linearize(linearizationPoint); From 7bdaff3cd8afa4d920b9fed81ad38db150753beb Mon Sep 17 00:00:00 2001 From: acxz <17132214+acxz@users.noreply.github.com> Date: Mon, 14 Jun 2021 15:33:17 -0400 Subject: [PATCH 28/40] update timeLago.cpp with newer Sampler interface --- timing/timeLago.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/timing/timeLago.cpp b/timing/timeLago.cpp index 7aaf37e924..c3ee6ff4bc 100644 --- a/timing/timeLago.cpp +++ b/timing/timeLago.cpp @@ -41,11 +41,11 @@ int main(int argc, char *argv[]) { // add noise to create initial estimate Values initial; - Sampler sampler(42u); Values::ConstFiltered poses = solution->filter(); SharedDiagonal noise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.5, 0.5, 15.0 * M_PI / 180.0).finished()); + Sampler sampler(noise); for(const Values::ConstFiltered::KeyValuePair& it: poses) - initial.insert(it.key, it.value.retract(sampler.sampleNewModel(noise))); + initial.insert(it.key, it.value.retract(sampler.sample())); // Add prior on the pose having index (key) = 0 noiseModel::Diagonal::shared_ptr priorModel = // From ad7d8f183a09d7a9e5fa828b6aeb58aaab84abc7 Mon Sep 17 00:00:00 2001 From: acxz <17132214+acxz@users.noreply.github.com> Date: Mon, 14 Jun 2021 16:25:15 -0400 Subject: [PATCH 29/40] use size_t variable type --- gtsam_unstable/timing/timeShonanAveraging.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam_unstable/timing/timeShonanAveraging.cpp b/gtsam_unstable/timing/timeShonanAveraging.cpp index d42635404b..e932b64007 100644 --- a/gtsam_unstable/timing/timeShonanAveraging.cpp +++ b/gtsam_unstable/timing/timeShonanAveraging.cpp @@ -52,7 +52,7 @@ void saveResult(string name, const Values& values) { myfile.open("shonan_result_of_" + name + ".dat"); size_t nrSO3 = values.count(); myfile << "#Type SO3 Number " << nrSO3 << "\n"; - for (int i = 0; i < nrSO3; ++i) { + for (size_t i = 0; i < nrSO3; ++i) { Matrix R = values.at(i).matrix(); // Check if the result of R.Transpose*R satisfy orthogonal constraint checkR(R); @@ -72,7 +72,7 @@ void saveG2oResult(string name, const Values& values, std::map poses ofstream myfile; myfile.open("shonan_result_of_" + name + ".g2o"); size_t nrSO3 = values.count(); - for (int i = 0; i < nrSO3; ++i) { + for (size_t i = 0; i < nrSO3; ++i) { Matrix R = values.at(i).matrix(); // Check if the result of R.Transpose*R satisfy orthogonal constraint checkR(R); @@ -92,7 +92,7 @@ void saveResultQuat(const Values& values) { ofstream myfile; myfile.open("shonan_result.dat"); size_t nrSOn = values.count(); - for (int i = 0; i < nrSOn; ++i) { + for (size_t i = 0; i < nrSOn; ++i) { GTSAM_PRINT(values.at(i)); Rot3 R = Rot3(values.at(i).matrix()); float x = R.toQuaternion().x(); From 56bede07cd96d81462936fdce51f5ae81d699850 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 16 Jun 2021 13:51:43 -0400 Subject: [PATCH 30/40] Squashed 'wrap/' changes from 0124bcc45..07330d100 07330d100 Merge pull request #113 from borglab/fix/reserved-keywords ec6b8f037 update test f022ba516 update and ignore reserved keywords for both functions and methods 4f988e5ad Merge pull request #112 from borglab/fix/cleanup 55bba2e6d fix variable annotations 61720ca0b support python 3.5 in the CI 0975d6529 version bump e8109917c use args.list() instead of args.args_list 6d0a91d7d renames args_names to names 4ce060b44 Merge pull request #111 from borglab/fix/default-bug ce7eea318 updated tests to capture bug b7650ec07 Fix bug for default in methods 4108854c7 Merge pull request #107 from borglab/feature/print 528ee64ce Merge pull request #110 from borglab/fix/variable-annotation e069f8bfc use old style variable annotation 5fd300116 update test fixture a25c2df0f use separate function to wrap global functions 58499a74b Merge pull request #106 from borglab/feature/consistent_names 2fe92b693 rename from cpp_class to to_cpp 3a3ba5963 Merge pull request #105 from borglab/cleanup e27a7b833 unskip tests 0db1839c4 Merge pull request #104 from borglab/feature/forward-class-declaration e3e7fbb27 remove unused imports a3c125065 encapsulate parsing and instantiation within wrap method in a functional way 69bbbe992 wrap instantiated declaration dbc44e7d5 added test for forward declaration typedef 6bec3cb8b add template instantation for typedefs of forward declarations 8d70c15ed updated Declaration to allow for wrapping 0637c2b3f remove print debugging statement deb8291ac allow forward declarations to be used for typedefs 69d660899 Merge pull request #101 from borglab/feature/object-default-parameters ec5555e56 formatting and docs cdaabc043 Merge branch 'feature/object-default-parameters' of github.com:borglab/wrap into feature/object-default-parameters 8ab0b0fa7 new parsing rule 0638a1937 update DEFAULT_ARG rule to support vector initializer list 83d2b761c update tests 7bb8c5494 more tests 1eaf6ba4a refactor default arg feature and add more tests 94f373ca9 tests 534e8a6dd support object types as default arguments 05e8ea855 Merge pull request #99 from borglab/fix/default_arg_0 1fdfeae6a address review comments 25b109c3f fix matlab unit test 6bb1b0c46 fix declaration order in unit test "expected" 7ee2d5fa4 don't unquote QuotedStrings that way we don't have to deal with strings manually. c915f4963 failing unit test : literals are not wrapped properly d47b6e8be default arg of 0 - interface_parser unit test ccf693641 fix for allow default arg of 0 3534c06e9 unit tests for default arg of 0 git-subtree-dir: wrap git-subtree-split: 07330d10022130e4284743341ac9d54a0dcb3d9f --- .github/workflows/linux-ci.yml | 2 +- .github/workflows/macos-ci.yml | 2 +- DOCS.md | 3 +- gtwrap/interface_parser/classes.py | 2 +- gtwrap/interface_parser/declaration.py | 13 +- gtwrap/interface_parser/function.py | 30 ++- gtwrap/interface_parser/namespace.py | 2 +- gtwrap/interface_parser/tokens.py | 28 ++- gtwrap/interface_parser/variable.py | 16 +- gtwrap/matlab_wrapper.py | 78 ++++--- gtwrap/pybind_wrapper.py | 158 +++++++++---- gtwrap/template_instantiator.py | 95 ++++++-- scripts/matlab_wrap.py | 14 +- scripts/pybind_wrap.py | 21 +- setup.py | 2 +- .../matlab/MultipleTemplatesIntDouble.m | 4 +- .../matlab/MultipleTemplatesIntFloat.m | 4 +- tests/expected/matlab/MyFactorPosePoint2.m | 8 +- tests/expected/matlab/MyVector12.m | 6 +- tests/expected/matlab/MyVector3.m | 6 +- tests/expected/matlab/PrimitiveRefDouble.m | 8 +- tests/expected/matlab/TemplatedFunctionRot3.m | 2 +- tests/expected/matlab/Test.m | 51 +++-- tests/expected/matlab/class_wrapper.cpp | 211 ++++++++++++------ tests/expected/matlab/functions_wrapper.cpp | 49 +++- tests/expected/matlab/geometry_wrapper.cpp | 8 + tests/expected/matlab/inheritance_wrapper.cpp | 18 +- tests/expected/matlab/namespaces_wrapper.cpp | 98 ++++++++ .../expected/matlab/special_cases_wrapper.cpp | 17 ++ tests/expected/python/class_pybind.cpp | 9 +- tests/expected/python/functions_pybind.cpp | 5 +- tests/expected/python/namespaces_pybind.cpp | 12 +- tests/fixtures/class.i | 14 ++ tests/fixtures/functions.i | 7 +- tests/fixtures/namespaces.i | 11 + tests/test_docs.py | 2 - tests/test_interface_parser.py | 116 +++++++--- tests/test_matlab_wrapper.py | 49 +--- tests/test_pybind_wrapper.py | 15 +- 39 files changed, 820 insertions(+), 376 deletions(-) diff --git a/.github/workflows/linux-ci.yml b/.github/workflows/linux-ci.yml index 34623385ed..0ca9ba8f5b 100644 --- a/.github/workflows/linux-ci.yml +++ b/.github/workflows/linux-ci.yml @@ -10,7 +10,7 @@ jobs: strategy: fail-fast: false matrix: - python-version: [3.6, 3.7, 3.8, 3.9] + python-version: [3.5, 3.6, 3.7, 3.8, 3.9] steps: - name: Checkout diff --git a/.github/workflows/macos-ci.yml b/.github/workflows/macos-ci.yml index 3910d28d8a..b0ccb3fbe9 100644 --- a/.github/workflows/macos-ci.yml +++ b/.github/workflows/macos-ci.yml @@ -10,7 +10,7 @@ jobs: strategy: fail-fast: false matrix: - python-version: [3.6, 3.7, 3.8, 3.9] + python-version: [3.5, 3.6, 3.7, 3.8, 3.9] steps: - name: Checkout diff --git a/DOCS.md b/DOCS.md index a5ca3fb0cb..8537ddd276 100644 --- a/DOCS.md +++ b/DOCS.md @@ -100,7 +100,8 @@ The python wrapper supports keyword arguments for functions/methods. Hence, the ``` - Using classes defined in other modules - - If you are using a class `OtherClass` not wrapped in an interface file, add `class OtherClass;` as a forward declaration to avoid a dependency error. `OtherClass` should be in the same project. + - If you are using a class `OtherClass` not wrapped in an interface file, add `class OtherClass;` as a forward declaration to avoid a dependency error. + - `OtherClass` may not be in the same project. If this is the case, include the header for the appropriate project `#include `. - Virtual inheritance - Specify fully-qualified base classes, i.e. `virtual class Derived : ns::Base {` where `ns` is the namespace. diff --git a/gtwrap/interface_parser/classes.py b/gtwrap/interface_parser/classes.py index ee4a9725cb..ea7a3b3c38 100644 --- a/gtwrap/interface_parser/classes.py +++ b/gtwrap/interface_parser/classes.py @@ -189,7 +189,7 @@ def __init__(self, # Check to ensure arg and return type are the same. if len(args) == 1 and self.operator not in ("()", "[]"): - assert args.args_list[0].ctype.typename.name == return_type.type1.typename.name, \ + assert args.list()[0].ctype.typename.name == return_type.type1.typename.name, \ "Mixed type overloading not supported. Both arg and return type must be the same." def __repr__(self) -> str: diff --git a/gtwrap/interface_parser/declaration.py b/gtwrap/interface_parser/declaration.py index 2a916899d1..292d6aeaa6 100644 --- a/gtwrap/interface_parser/declaration.py +++ b/gtwrap/interface_parser/declaration.py @@ -15,6 +15,7 @@ from .tokens import (CLASS, COLON, INCLUDE, LOPBRACK, ROPBRACK, SEMI_COLON, VIRTUAL) from .type import Typename +from .utils import collect_namespaces class Include: @@ -42,11 +43,12 @@ class ForwardDeclaration: t.name, t.parent_type, t.is_virtual)) def __init__(self, - name: Typename, + typename: Typename, parent_type: str, is_virtual: str, parent: str = ''): - self.name = name + self.name = typename.name + self.typename = typename if parent_type: self.parent_type = parent_type else: @@ -55,6 +57,9 @@ def __init__(self, self.is_virtual = is_virtual self.parent = parent + def namespaces(self) -> list: + """Get the namespaces which this class is nested under as a list.""" + return collect_namespaces(self) + def __repr__(self) -> str: - return "ForwardDeclaration: {} {}({})".format(self.is_virtual, - self.name, self.parent) + return "ForwardDeclaration: {} {}".format(self.is_virtual, self.name) diff --git a/gtwrap/interface_parser/function.py b/gtwrap/interface_parser/function.py index bf9b15256b..3b9a5d4ada 100644 --- a/gtwrap/interface_parser/function.py +++ b/gtwrap/interface_parser/function.py @@ -29,11 +29,13 @@ class Argument: void sayHello(/*`s` is the method argument with type `const string&`*/ const string& s); ``` """ - rule = ((Type.rule ^ TemplatedType.rule)("ctype") + IDENT("name") + \ - Optional(EQUAL + (DEFAULT_ARG ^ Type.rule ^ TemplatedType.rule) + \ - Optional(LPAREN + RPAREN) # Needed to parse the parens for default constructors - )("default") - ).setParseAction(lambda t: Argument(t.ctype, t.name, t.default)) + rule = ((Type.rule ^ TemplatedType.rule)("ctype") # + + IDENT("name") # + + Optional(EQUAL + DEFAULT_ARG)("default") + ).setParseAction(lambda t: Argument( + t.ctype, # + t.name, # + t.default[0] if isinstance(t.default, ParseResults) else None)) def __init__(self, ctype: Union[Type, TemplatedType], @@ -44,18 +46,8 @@ def __init__(self, else: self.ctype = ctype self.name = name - # If the length is 1, it's a regular type, - if len(default) == 1: - default = default[0] - # This means a tuple has been passed so we convert accordingly - elif len(default) > 1: - default = tuple(default.asList()) - else: - # set to None explicitly so we can support empty strings - default = None self.default = default - - self.parent: Union[ArgumentList, None] = None + self.parent = None # type: Union[ArgumentList, None] def __repr__(self) -> str: return self.to_cpp() @@ -94,10 +86,14 @@ def __repr__(self) -> str: def __len__(self) -> int: return len(self.args_list) - def args_names(self) -> List[str]: + def names(self) -> List[str]: """Return a list of the names of all the arguments.""" return [arg.name for arg in self.args_list] + def list(self) -> List[Argument]: + """Return a list of the names of all the arguments.""" + return self.args_list + def to_cpp(self, use_boost: bool) -> List[str]: """Generate the C++ code for wrapping.""" return [arg.ctype.to_cpp(use_boost) for arg in self.args_list] diff --git a/gtwrap/interface_parser/namespace.py b/gtwrap/interface_parser/namespace.py index 8aa2e71cc1..575d982371 100644 --- a/gtwrap/interface_parser/namespace.py +++ b/gtwrap/interface_parser/namespace.py @@ -102,7 +102,7 @@ def find_class_or_function( res = [] for namespace in found_namespaces: classes_and_funcs = (c for c in namespace.content - if isinstance(c, (Class, GlobalFunction))) + if isinstance(c, (Class, GlobalFunction, ForwardDeclaration))) res += [c for c in classes_and_funcs if c.name == typename.name] if not res: raise ValueError("Cannot find class {} in module!".format( diff --git a/gtwrap/interface_parser/tokens.py b/gtwrap/interface_parser/tokens.py index c6a40bc311..4eba95900a 100644 --- a/gtwrap/interface_parser/tokens.py +++ b/gtwrap/interface_parser/tokens.py @@ -10,9 +10,9 @@ Author: Duy Nguyen Ta, Fan Jiang, Matthew Sklar, Varun Agrawal, and Frank Dellaert """ -from pyparsing import (Keyword, Literal, Or, QuotedString, Suppress, Word, - alphanums, alphas, delimitedList, nums, - pyparsing_common) +from pyparsing import (Keyword, Literal, OneOrMore, Or, QuotedString, Suppress, + Word, alphanums, alphas, nestedExpr, nums, + originalTextFor, printables) # rule for identifiers (e.g. variable names) IDENT = Word(alphas + '_', alphanums + '_') ^ Word(nums) @@ -22,16 +22,20 @@ LPAREN, RPAREN, LBRACE, RBRACE, COLON, SEMI_COLON = map(Suppress, "(){}:;") LOPBRACK, ROPBRACK, COMMA, EQUAL = map(Suppress, "<>,=") -# Encapsulating type for numbers, and single and double quoted strings. -# The pyparsing_common utilities ensure correct coversion to the corresponding type. -# E.g. pyparsing_common.number will convert 3.1415 to a float type. -NUMBER_OR_STRING = (pyparsing_common.number ^ QuotedString('"') ^ QuotedString("'")) - -# A python tuple, e.g. (1, 9, "random", 3.1415) -TUPLE = (LPAREN + delimitedList(NUMBER_OR_STRING) + RPAREN) - # Default argument passed to functions/methods. -DEFAULT_ARG = (NUMBER_OR_STRING ^ pyparsing_common.identifier ^ TUPLE) +# Allow anything up to ',' or ';' except when they +# appear inside matched expressions such as +# (a, b) {c, b} "hello, world", templates, initializer lists, etc. +DEFAULT_ARG = originalTextFor( + OneOrMore( + QuotedString('"') ^ # parse double quoted strings + QuotedString("'") ^ # parse single quoted strings + Word(printables, excludeChars="(){}[]<>,;") ^ # parse arbitrary words + nestedExpr(opener='(', closer=')') ^ # parse expression in parentheses + nestedExpr(opener='[', closer=']') ^ # parse expression in brackets + nestedExpr(opener='{', closer='}') ^ # parse expression in braces + nestedExpr(opener='<', closer='>') # parse template expressions + )) CONST, VIRTUAL, CLASS, STATIC, PAIR, TEMPLATE, TYPEDEF, INCLUDE = map( Keyword, diff --git a/gtwrap/interface_parser/variable.py b/gtwrap/interface_parser/variable.py index dffa2de126..fcb02666f7 100644 --- a/gtwrap/interface_parser/variable.py +++ b/gtwrap/interface_parser/variable.py @@ -12,7 +12,7 @@ from pyparsing import Optional, ParseResults -from .tokens import DEFAULT_ARG, EQUAL, IDENT, SEMI_COLON, STATIC +from .tokens import DEFAULT_ARG, EQUAL, IDENT, SEMI_COLON from .type import TemplatedType, Type @@ -32,10 +32,12 @@ class Hello { """ rule = ((Type.rule ^ TemplatedType.rule)("ctype") # + IDENT("name") # - #TODO(Varun) Add support for non-basic types - + Optional(EQUAL + (DEFAULT_ARG))("default") # + + Optional(EQUAL + DEFAULT_ARG)("default") # + SEMI_COLON # - ).setParseAction(lambda t: Variable(t.ctype, t.name, t.default)) + ).setParseAction(lambda t: Variable( + t.ctype, # + t.name, # + t.default[0] if isinstance(t.default, ParseResults) else None)) def __init__(self, ctype: Type, @@ -44,11 +46,7 @@ def __init__(self, parent=''): self.ctype = ctype[0] # ParseResult is a list self.name = name - if default: - self.default = default[0] - else: - self.default = None - + self.default = default self.parent = parent def __repr__(self) -> str: diff --git a/gtwrap/matlab_wrapper.py b/gtwrap/matlab_wrapper.py index 1f9f3146fd..de6221bbcf 100755 --- a/gtwrap/matlab_wrapper.py +++ b/gtwrap/matlab_wrapper.py @@ -57,7 +57,7 @@ class MatlabWrapper(object): # Methods that should be ignored ignore_methods = ['pickle'] # Datatypes that do not need to be checked in methods - not_check_type: list = [] + not_check_type = [] # type: list # Data types that are primitive types not_ptr_type = ['int', 'double', 'bool', 'char', 'unsigned char', 'size_t'] # Ignore the namespace for these datatypes @@ -65,16 +65,18 @@ class MatlabWrapper(object): # The amount of times the wrapper has created a call to geometry_wrapper wrapper_id = 0 # Map each wrapper id to what its collector function namespace, class, type, and string format - wrapper_map: dict = {} + wrapper_map = {} # Set of all the includes in the namespace - includes: Dict[parser.Include, int] = {} + includes = {} # type: Dict[parser.Include, int] # Set of all classes in the namespace - classes: List[Union[parser.Class, instantiator.InstantiatedClass]] = [] - classes_elems: Dict[Union[parser.Class, instantiator.InstantiatedClass], int] = {} + classes = [ + ] # type: List[Union[parser.Class, instantiator.InstantiatedClass]] + classes_elems = { + } # type: Dict[Union[parser.Class, instantiator.InstantiatedClass], int] # Id for ordering global functions in the wrapper global_function_id = 0 # Files and their content - content: List[str] = [] + content = [] # type: List[str] # Ensure the template file is always picked up from the correct directory. dir_path = osp.dirname(osp.realpath(__file__)) @@ -82,11 +84,9 @@ class MatlabWrapper(object): wrapper_file_header = f.read() def __init__(self, - module, module_name, top_module_namespace='', ignore_classes=()): - self.module = module self.module_name = module_name self.top_module_namespace = top_module_namespace self.ignore_classes = ignore_classes @@ -374,14 +374,14 @@ def _wrap_args(self, args): """ arg_wrap = '' - for i, arg in enumerate(args.args_list, 1): + for i, arg in enumerate(args.list(), 1): c_type = self._format_type_name(arg.ctype.typename, include_namespace=False) arg_wrap += '{c_type} {arg_name}{comma}'.format( c_type=c_type, arg_name=arg.name, - comma='' if i == len(args.args_list) else ', ') + comma='' if i == len(args.list()) else ', ') return arg_wrap @@ -396,7 +396,7 @@ def _wrap_variable_arguments(self, args, wrap_datatypes=True): """ var_arg_wrap = '' - for i, arg in enumerate(args.args_list, 1): + for i, arg in enumerate(args.list(), 1): name = arg.ctype.typename.name if name in self.not_check_type: continue @@ -442,7 +442,7 @@ def _wrap_list_variable_arguments(self, args): var_list_wrap = '' first = True - for i in range(1, len(args.args_list) + 1): + for i in range(1, len(args.list()) + 1): if first: var_list_wrap += 'varargin{{{}}}'.format(i) first = False @@ -462,9 +462,9 @@ def _wrap_method_check_statement(self, args): if check_statement == '': check_statement = \ 'if length(varargin) == {param_count}'.format( - param_count=len(args.args_list)) + param_count=len(args.list())) - for _, arg in enumerate(args.args_list): + for _, arg in enumerate(args.list()): name = arg.ctype.typename.name if name in self.not_check_type: @@ -516,7 +516,7 @@ def _wrapper_unwrap_arguments(self, args, arg_id=0, constructor=False): params = '' body_args = '' - for arg in args.args_list: + for arg in args.list(): if params != '': params += ',' @@ -725,10 +725,10 @@ def wrap_global_function(self, function): param_wrap += ' if' if i == 0 else ' elseif' param_wrap += ' length(varargin) == ' - if len(overload.args.args_list) == 0: + if len(overload.args.list()) == 0: param_wrap += '0\n' else: - param_wrap += str(len(overload.args.args_list)) \ + param_wrap += str(len(overload.args.list())) \ + self._wrap_variable_arguments(overload.args, False) + '\n' # Determine format of return and varargout statements @@ -825,14 +825,14 @@ def wrap_class_constructors(self, namespace_name, inst_class, parent_name, methods_wrap += textwrap.indent(textwrap.dedent('''\ elseif nargin == {len}{varargin} {ptr}{wrapper}({num}{comma}{var_arg}); - ''').format(len=len(ctor.args.args_list), + ''').format(len=len(ctor.args.list()), varargin=self._wrap_variable_arguments( ctor.args, False), ptr=wrapper_return, wrapper=self._wrapper_name(), num=self._update_wrapper_id( (namespace_name, inst_class, 'constructor', ctor)), - comma='' if len(ctor.args.args_list) == 0 else ', ', + comma='' if len(ctor.args.list()) == 0 else ', ', var_arg=self._wrap_list_variable_arguments(ctor.args)), prefix=' ') @@ -938,7 +938,7 @@ def wrap_class_methods(self, namespace_name, inst_class, methods, - serialize=(False,)): + serialize=(False, )): """Wrap the methods in the class. Args: @@ -1075,7 +1075,7 @@ def wrap_static_methods(self, namespace_name, instantiated_class, static_overload.return_type, include_namespace=True, separator="."), - length=len(static_overload.args.args_list), + length=len(static_overload.args.list()), var_args_list=self._wrap_variable_arguments( static_overload.args), check_statement=check_statement, @@ -1097,7 +1097,8 @@ def wrap_static_methods(self, namespace_name, instantiated_class, method_text += textwrap.indent(textwrap.dedent("""\ end\n - """), prefix=" ") + """), + prefix=" ") if serialize: method_text += textwrap.indent(textwrap.dedent("""\ @@ -1349,14 +1350,14 @@ def wrap_collector_function_return(self, method): else: if isinstance(method.parent, instantiator.InstantiatedClass): - method_name = method.parent.cpp_class() + "::" + method_name = method.parent.to_cpp() + "::" else: method_name = self._format_static_method(method, '::') method_name += method.name if "MeasureRange" in method_name: self._debug("method: {}, method: {}, inst: {}".format( - method_name, method.name, method.parent.cpp_class())) + method_name, method.name, method.parent.to_cpp())) obj = ' ' if return_1_name == 'void' else '' obj += '{}{}({})'.format(obj_start, method_name, params) @@ -1447,7 +1448,7 @@ def generate_collector_function(self, func_id): extra = collector_func[4] class_name = collector_func[0] + collector_func[1].name - class_name_separated = collector_func[1].cpp_class() + class_name_separated = collector_func[1].to_cpp() is_method = isinstance(extra, parser.Method) is_static_method = isinstance(extra, parser.StaticMethod) @@ -1510,12 +1511,12 @@ def generate_collector_function(self, func_id): elif extra == 'serialize': body += self.wrap_collector_function_serialize( collector_func[1].name, - full_name=collector_func[1].cpp_class(), + full_name=collector_func[1].to_cpp(), namespace=collector_func[0]) elif extra == 'deserialize': body += self.wrap_collector_function_deserialize( collector_func[1].name, - full_name=collector_func[1].cpp_class(), + full_name=collector_func[1].to_cpp(), namespace=collector_func[0]) elif is_method or is_static_method: method_name = '' @@ -1548,7 +1549,7 @@ def generate_collector_function(self, func_id): min1='-1' if is_method else '', shared_obj=shared_obj, method_name=method_name, - num_args=len(extra.args.args_list), + num_args=len(extra.args.list()), body_args=body_args, return_body=return_body) @@ -1565,10 +1566,11 @@ def generate_collector_function(self, func_id): checkArguments("{function_name}",nargout,nargin,{len}); ''').format(function_name=collector_func[1].name, id=self.global_function_id, - len=len(collector_func[1].args.args_list)) + len=len(collector_func[1].args.list())) body += self._wrapper_unwrap_arguments(collector_func[1].args)[1] - body += self.wrap_collector_function_return(collector_func[1]) + '\n}\n' + body += self.wrap_collector_function_return( + collector_func[1]) + '\n}\n' collector_function += body @@ -1723,7 +1725,7 @@ def _has_serialization(cls): cls_insts += self._format_type_name(inst) typedef_instances += 'typedef {original_class_name} {class_name_sep};\n' \ - .format(original_class_name=cls.cpp_class(), + .format(original_class_name=cls.to_cpp(), class_name_sep=cls.name) class_name_sep = cls.name @@ -1734,7 +1736,7 @@ def _has_serialization(cls): boost_class_export_guid += 'BOOST_CLASS_EXPORT_GUID({}, "{}");\n'.format( class_name_sep, class_name) else: - class_name_sep = cls.cpp_class() + class_name_sep = cls.to_cpp() class_name = self._format_class_name(cls) if len(cls.original.namespaces()) > 1 and _has_serialization( @@ -1780,7 +1782,7 @@ def _has_serialization(cls): if queue_set_next_case: ptr_ctor_frag += self.wrap_collector_function_upcast_from_void( - id_val[1].name, idx, id_val[1].cpp_class()) + id_val[1].name, idx, id_val[1].to_cpp()) wrapper_file += textwrap.dedent('''\ {typedef_instances} @@ -1872,10 +1874,14 @@ def wrap_collector_function_deserialize(self, namespace=namespace), prefix=' ') - def wrap(self): + def wrap(self, content): """High level function to wrap the project.""" - self.wrap_namespace(self.module) - self.generate_wrapper(self.module) + # Parse the contents of the interface file + parsed_result = parser.Module.parseString(content) + # Instantiate the module + module = instantiator.instantiate_namespace(parsed_result) + self.wrap_namespace(module) + self.generate_wrapper(module) return self.content diff --git a/gtwrap/pybind_wrapper.py b/gtwrap/pybind_wrapper.py index 8f8dde224e..0e1b3c7eaa 100755 --- a/gtwrap/pybind_wrapper.py +++ b/gtwrap/pybind_wrapper.py @@ -23,48 +23,49 @@ class PybindWrapper: Class to generate binding code for Pybind11 specifically. """ def __init__(self, - module, module_name, top_module_namespaces='', use_boost=False, ignore_classes=(), module_template=""): - self.module = module self.module_name = module_name self.top_module_namespaces = top_module_namespaces self.use_boost = use_boost self.ignore_classes = ignore_classes self._serializing_classes = list() self.module_template = module_template - self.python_keywords = ['print', 'lambda'] + self.python_keywords = [ + 'lambda', 'False', 'def', 'if', 'raise', 'None', 'del', 'import', + 'return', 'True', 'elif', 'in', 'try', 'and', 'else', 'is', + 'while', 'as', 'except', 'lambda', 'with', 'assert', 'finally', + 'nonlocal', 'yield', 'break', 'for', 'not', 'class', 'from', 'or', + 'continue', 'global', 'pass' + ] # amount of indentation to add before each function/method declaration. self.method_indent = '\n' + (' ' * 8) - def _py_args_names(self, args_list): + def _py_args_names(self, args): """Set the argument names in Pybind11 format.""" - names = args_list.args_names() + names = args.names() if names: py_args = [] - for arg in args_list.args_list: - if isinstance(arg.default, str) and arg.default is not None: - # string default arg - arg.default = ' = "{arg.default}"'.format(arg=arg) - elif arg.default: # Other types - arg.default = ' = {arg.default}'.format(arg=arg) + for arg in args.list(): + if arg.default is not None: + default = ' = {arg.default}'.format(arg=arg) else: - arg.default = '' + default = '' argument = 'py::arg("{name}"){default}'.format( - name=arg.name, default='{0}'.format(arg.default)) + name=arg.name, default='{0}'.format(default)) py_args.append(argument) return ", " + ", ".join(py_args) else: return '' - def _method_args_signature_with_names(self, args_list): - """Define the method signature types with the argument names.""" - cpp_types = args_list.to_cpp(self.use_boost) - names = args_list.args_names() + def _method_args_signature(self, args): + """Generate the argument types and names as per the method signature.""" + cpp_types = args.to_cpp(self.use_boost) + names = args.names() types_names = [ "{} {}".format(ctype, name) for ctype, name in zip(cpp_types, names) @@ -99,7 +100,8 @@ def _wrap_method(self, serialize_method = self.method_indent + \ ".def(\"serialize\", []({class_inst} self){{ return gtsam::serialize(*self); }})".format(class_inst=cpp_class + '*') deserialize_method = self.method_indent + \ - ".def(\"deserialize\", []({class_inst} self, string serialized){{ gtsam::deserialize(serialized, *self); }}, py::arg(\"serialized\"))" \ + '.def("deserialize", []({class_inst} self, string serialized)' \ + '{{ gtsam::deserialize(serialized, *self); }}, py::arg("serialized"))' \ .format(class_inst=cpp_class + '*') return serialize_method + deserialize_method @@ -112,20 +114,23 @@ def _wrap_method(self, return pickle_method.format(cpp_class=cpp_class, indent=self.method_indent) + # Add underscore to disambiguate if the method name matches a python keyword + if py_method in self.python_keywords: + py_method = py_method + "_" + is_method = isinstance(method, instantiator.InstantiatedMethod) is_static = isinstance(method, parser.StaticMethod) return_void = method.return_type.is_void() - args_names = method.args.args_names() + args_names = method.args.names() py_args_names = self._py_args_names(method.args) - args_signature_with_names = self._method_args_signature_with_names( - method.args) + args_signature_with_names = self._method_args_signature(method.args) caller = cpp_class + "::" if not is_method else "self->" - function_call = ('{opt_return} {caller}{function_name}' + function_call = ('{opt_return} {caller}{method_name}' '({args_names});'.format( opt_return='return' if not return_void else '', caller=caller, - function_name=cpp_method, + method_name=cpp_method, args_names=', '.join(args_names), )) @@ -136,8 +141,7 @@ def _wrap_method(self, '{py_args_names}){suffix}'.format( prefix=prefix, cdef="def_static" if is_static else "def", - py_method=py_method if not py_method in self.python_keywords - else py_method + "_", + py_method=py_method, opt_self="{cpp_class}* self".format( cpp_class=cpp_class) if is_method else "", opt_comma=', ' if is_method and args_names else '', @@ -181,15 +185,13 @@ def wrap_methods(self, suffix=''): """ Wrap all the methods in the `cpp_class`. - - This function is also used to wrap global functions. """ res = "" for method in methods: - # To avoid type confusion for insert, currently unused + # To avoid type confusion for insert if method.name == 'insert' and cpp_class == 'gtsam::Values': - name_list = method.args.args_names() + name_list = method.args.names() type_list = method.args.to_cpp(self.use_boost) # inserting non-wrapped value types if type_list[0].strip() == 'size_t': @@ -214,7 +216,8 @@ def wrap_variable(self, module_var, variable, prefix='\n' + ' ' * 8): - """Wrap a variable that's not part of a class (i.e. global) + """ + Wrap a variable that's not part of a class (i.e. global) """ variable_value = "" if variable.default is None: @@ -288,23 +291,20 @@ def wrap_enum(self, enum, class_name='', module=None, prefix=' ' * 4): def wrap_enums(self, enums, instantiated_class, prefix=' ' * 4): """Wrap multiple enums defined in a class.""" - cpp_class = instantiated_class.cpp_class() + cpp_class = instantiated_class.to_cpp() module_var = instantiated_class.name.lower() res = '' for enum in enums: res += "\n" + self.wrap_enum( - enum, - class_name=cpp_class, - module=module_var, - prefix=prefix) + enum, class_name=cpp_class, module=module_var, prefix=prefix) return res def wrap_instantiated_class( self, instantiated_class: instantiator.InstantiatedClass): """Wrap the class.""" module_var = self._gen_module_var(instantiated_class.namespaces()) - cpp_class = instantiated_class.cpp_class() + cpp_class = instantiated_class.to_cpp() if cpp_class in self.ignore_classes: return "" if instantiated_class.parent_class: @@ -356,10 +356,27 @@ def wrap_instantiated_class( wrapped_operators=self.wrap_operators( instantiated_class.operators, cpp_class))) + def wrap_instantiated_declaration( + self, instantiated_decl: instantiator.InstantiatedDeclaration): + """Wrap the class.""" + module_var = self._gen_module_var(instantiated_decl.namespaces()) + cpp_class = instantiated_decl.to_cpp() + if cpp_class in self.ignore_classes: + return "" + + res = ( + '\n py::class_<{cpp_class}, ' + '{shared_ptr_type}::shared_ptr<{cpp_class}>>({module_var}, "{class_name}")' + ).format(shared_ptr_type=('boost' if self.use_boost else 'std'), + cpp_class=cpp_class, + class_name=instantiated_decl.name, + module_var=module_var) + return res + def wrap_stl_class(self, stl_class): """Wrap STL containers.""" module_var = self._gen_module_var(stl_class.namespaces()) - cpp_class = stl_class.cpp_class() + cpp_class = stl_class.to_cpp() if cpp_class in self.ignore_classes: return "" @@ -385,6 +402,59 @@ def wrap_stl_class(self, stl_class): stl_class.properties, cpp_class), )) + def wrap_functions(self, + functions, + namespace, + prefix='\n' + ' ' * 8, + suffix=''): + """ + Wrap all the global functions. + """ + res = "" + for function in functions: + + function_name = function.name + + # Add underscore to disambiguate if the function name matches a python keyword + python_keywords = self.python_keywords + ['print'] + if function_name in python_keywords: + function_name = function_name + "_" + + cpp_method = function.to_cpp() + + is_static = isinstance(function, parser.StaticMethod) + return_void = function.return_type.is_void() + args_names = function.args.names() + py_args_names = self._py_args_names(function.args) + args_signature = self._method_args_signature(function.args) + + caller = namespace + "::" + function_call = ('{opt_return} {caller}{function_name}' + '({args_names});'.format( + opt_return='return' + if not return_void else '', + caller=caller, + function_name=cpp_method, + args_names=', '.join(args_names), + )) + + ret = ('{prefix}.{cdef}("{function_name}",' + '[]({args_signature}){{' + '{function_call}' + '}}' + '{py_args_names}){suffix}'.format( + prefix=prefix, + cdef="def_static" if is_static else "def", + function_name=function_name, + args_signature=args_signature, + function_call=function_call, + py_args_names=py_args_names, + suffix=suffix)) + + res += ret + + return res + def _partial_match(self, namespaces1, namespaces2): for i in range(min(len(namespaces1), len(namespaces2))): if namespaces1[i] != namespaces2[i]: @@ -460,6 +530,9 @@ def wrap_namespace(self, namespace): wrapped += self.wrap_instantiated_class(element) wrapped += self.wrap_enums(element.enums, element) + elif isinstance(element, instantiator.InstantiatedDeclaration): + wrapped += self.wrap_instantiated_declaration(element) + elif isinstance(element, parser.Variable): variable_namespace = self._add_namespaces('', namespaces) wrapped += self.wrap_variable(namespace=variable_namespace, @@ -476,7 +549,7 @@ def wrap_namespace(self, namespace): if isinstance(func, (parser.GlobalFunction, instantiator.InstantiatedGlobalFunction)) ] - wrapped += self.wrap_methods( + wrapped += self.wrap_functions( all_funcs, self._add_namespaces('', namespaces)[:-2], prefix='\n' + ' ' * 4 + module_var, @@ -484,9 +557,14 @@ def wrap_namespace(self, namespace): ) return wrapped, includes - def wrap(self): + def wrap(self, content): """Wrap the code in the interface file.""" - wrapped_namespace, includes = self.wrap_namespace(self.module) + # Parse the contents of the interface file + module = parser.Module.parseString(content) + # Instantiate all templates + module = instantiator.instantiate_namespace(module) + + wrapped_namespace, includes = self.wrap_namespace(module) # Export classes for serialization. boost_class_export = "" diff --git a/gtwrap/template_instantiator.py b/gtwrap/template_instantiator.py index a66fa95445..c474246489 100644 --- a/gtwrap/template_instantiator.py +++ b/gtwrap/template_instantiator.py @@ -95,10 +95,9 @@ def instantiate_args_list(args_list, template_typenames, instantiations, for arg in args_list: new_type = instantiate_type(arg.ctype, template_typenames, instantiations, cpp_typename) - default = [arg.default] if isinstance(arg, parser.Argument) else '' - instantiated_args.append(parser.Argument(name=arg.name, - ctype=new_type, - default=default)) + instantiated_args.append( + parser.Argument(name=arg.name, ctype=new_type, + default=arg.default)) return instantiated_args @@ -131,7 +130,6 @@ def instantiate_name(original_name, instantiations): TODO(duy): To avoid conflicts, we should include the instantiation's namespaces, but I find that too verbose. """ - inst_name = '' instantiated_names = [] for inst in instantiations: # Ensure the first character of the type is capitalized @@ -172,7 +170,7 @@ def __init__(self, original, instantiations=(), new_name=''): cpp_typename='', ) instantiated_args = instantiate_args_list( - original.args.args_list, + original.args.list(), self.original.template.typenames, self.instantiations, # Keyword type name `This` should already be replaced in the @@ -206,7 +204,13 @@ def __repr__(self): class InstantiatedMethod(parser.Method): """ - We can only instantiate template methods with a single template parameter. + Instantiate method with template parameters. + + E.g. + class A { + template + void func(X x, Y y); + } """ def __init__(self, original, instantiations: List[parser.Typename] = ''): self.original = original @@ -216,7 +220,7 @@ def __init__(self, original, instantiations: List[parser.Typename] = ''): self.parent = original.parent # Check for typenames if templated. - # This way, we can gracefully handle bot templated and non-templated methois. + # This way, we can gracefully handle both templated and non-templated methods. typenames = self.original.template.typenames if self.original.template else [] self.name = instantiate_name(original.name, self.instantiations) self.return_type = instantiate_return_type( @@ -229,7 +233,7 @@ def __init__(self, original, instantiations: List[parser.Typename] = ''): ) instantiated_args = instantiate_args_list( - original.args.args_list, + original.args.list(), typenames, self.instantiations, # Keyword type name `This` should already be replaced in the @@ -342,7 +346,7 @@ def __repr__(self): "{ctors}\n{static_methods}\n{methods}".format( virtual="virtual" if self.is_virtual else '', name=self.name, - cpp_class=self.cpp_class(), + cpp_class=self.to_cpp(), parent_class=self.parent, ctors="\n".join([repr(ctor) for ctor in self.ctors]), methods="\n".join([repr(m) for m in self.methods]), @@ -364,7 +368,7 @@ def instantiate_ctors(self, typenames): for ctor in self.original.ctors: instantiated_args = instantiate_args_list( - ctor.args.args_list, + ctor.args.list(), typenames, self.instantiations, self.cpp_typename(), @@ -389,7 +393,7 @@ def instantiate_static_methods(self, typenames): instantiated_static_methods = [] for static_method in self.original.static_methods: instantiated_args = instantiate_args_list( - static_method.args.args_list, typenames, self.instantiations, + static_method.args.list(), typenames, self.instantiations, self.cpp_typename()) instantiated_static_methods.append( parser.StaticMethod( @@ -426,7 +430,7 @@ class Greeter{ class_instantiated_methods = [] for method in self.original.methods: instantiated_args = instantiate_args_list( - method.args.args_list, + method.args.list(), typenames, self.instantiations, self.cpp_typename(), @@ -459,7 +463,7 @@ def instantiate_operators(self, typenames): instantiated_operators = [] for operator in self.original.operators: instantiated_args = instantiate_args_list( - operator.args.args_list, + operator.args.list(), typenames, self.instantiations, self.cpp_typename(), @@ -497,10 +501,6 @@ def instantiate_properties(self, typenames): ) return instantiated_properties - def cpp_class(self): - """Generate the C++ code for wrapping.""" - return self.cpp_typename().to_cpp() - def cpp_typename(self): """ Return a parser.Typename including namespaces and cpp name of this @@ -516,8 +516,53 @@ def cpp_typename(self): namespaces_name.append(name) return parser.Typename(namespaces_name) + def to_cpp(self): + """Generate the C++ code for wrapping.""" + return self.cpp_typename().to_cpp() + + +class InstantiatedDeclaration(parser.ForwardDeclaration): + """ + Instantiate typedefs of forward declarations. + This is useful when we wish to typedef a templated class + which is not defined in the current project. + + E.g. + class FactorFromAnotherMother; + + typedef FactorFromAnotherMother FactorWeCanUse; + """ + def __init__(self, original, instantiations=(), new_name=''): + super().__init__(original.typename, + original.parent_type, + original.is_virtual, + parent=original.parent) + + self.original = original + self.instantiations = instantiations + self.parent = original.parent + + self.name = instantiate_name( + original.name, instantiations) if not new_name else new_name + + def to_cpp(self): + """Generate the C++ code for wrapping.""" + instantiated_names = [ + inst.qualified_name() for inst in self.instantiations + ] + name = "{}<{}>".format(self.original.name, + ",".join(instantiated_names)) + namespaces_name = self.namespaces() + namespaces_name.append(name) + # Leverage Typename to generate the fully qualified C++ name + return parser.Typename(namespaces_name).to_cpp() + + def __repr__(self): + return "Instantiated {}".format( + super(InstantiatedDeclaration, self).__repr__()) + -def instantiate_namespace_inplace(namespace): +def instantiate_namespace(namespace): """ Instantiate the classes and other elements in the `namespace` content and assign it back to the namespace content attribute. @@ -567,7 +612,8 @@ def instantiate_namespace_inplace(namespace): original_element = top_level.find_class_or_function( typedef_inst.typename) - # Check if element is a typedef'd class or function. + # Check if element is a typedef'd class, function or + # forward declaration from another project. if isinstance(original_element, parser.Class): typedef_content.append( InstantiatedClass(original_element, @@ -578,12 +624,19 @@ def instantiate_namespace_inplace(namespace): InstantiatedGlobalFunction( original_element, typedef_inst.typename.instantiations, typedef_inst.new_name)) + elif isinstance(original_element, parser.ForwardDeclaration): + typedef_content.append( + InstantiatedDeclaration( + original_element, typedef_inst.typename.instantiations, + typedef_inst.new_name)) elif isinstance(element, parser.Namespace): - instantiate_namespace_inplace(element) + element = instantiate_namespace(element) instantiated_content.append(element) else: instantiated_content.append(element) instantiated_content.extend(typedef_content) namespace.content = instantiated_content + + return namespace diff --git a/scripts/matlab_wrap.py b/scripts/matlab_wrap.py index 232e934905..be6043947f 100644 --- a/scripts/matlab_wrap.py +++ b/scripts/matlab_wrap.py @@ -8,9 +8,8 @@ import argparse import os +import sys -import gtwrap.interface_parser as parser -import gtwrap.template_instantiator as instantiator from gtwrap.matlab_wrapper import MatlabWrapper, generate_content if __name__ == "__main__": @@ -51,18 +50,11 @@ if not os.path.exists(args.src): os.mkdir(args.src) - module = parser.Module.parseString(content) - - instantiator.instantiate_namespace_inplace(module) - - import sys - print("Ignoring classes: {}".format(args.ignore), file=sys.stderr) - wrapper = MatlabWrapper(module=module, - module_name=args.module_name, + wrapper = MatlabWrapper(module_name=args.module_name, top_module_namespace=top_module_namespaces, ignore_classes=args.ignore) - cc_content = wrapper.wrap() + cc_content = wrapper.wrap(content) generate_content(cc_content, args.out) diff --git a/scripts/pybind_wrap.py b/scripts/pybind_wrap.py index 26e63d51c3..7f2f8d4192 100644 --- a/scripts/pybind_wrap.py +++ b/scripts/pybind_wrap.py @@ -1,5 +1,4 @@ #!/usr/bin/env python3 - """ Helper script to wrap C++ to Python with Pybind. This script is installed via CMake to the user's binary directory @@ -10,8 +9,6 @@ import argparse -import gtwrap.interface_parser as parser -import gtwrap.template_instantiator as instantiator from gtwrap.pybind_wrapper import PybindWrapper @@ -19,11 +16,10 @@ def main(): """Main runner.""" arg_parser = argparse.ArgumentParser( formatter_class=argparse.ArgumentDefaultsHelpFormatter) - arg_parser.add_argument( - "--src", - type=str, - required=True, - help="Input interface .i/.h file") + arg_parser.add_argument("--src", + type=str, + required=True, + help="Input interface .i/.h file") arg_parser.add_argument( "--module_name", type=str, @@ -62,7 +58,8 @@ def main(): help="A space-separated list of classes to ignore. " "Class names must include their full namespaces.", ) - arg_parser.add_argument("--template", type=str, + arg_parser.add_argument("--template", + type=str, help="The module template file") args = arg_parser.parse_args() @@ -74,14 +71,10 @@ def main(): with open(args.src, "r") as f: content = f.read() - module = parser.Module.parseString(content) - instantiator.instantiate_namespace_inplace(module) - with open(args.template, "r") as f: template_content = f.read() wrapper = PybindWrapper( - module=module, module_name=args.module_name, use_boost=args.use_boost, top_module_namespaces=top_module_namespaces, @@ -90,7 +83,7 @@ def main(): ) # Wrap the code and get back the cpp/cc code. - cc_content = wrapper.wrap() + cc_content = wrapper.wrap(content) # Generate the C++ code which Pybind11 will use. with open(args.out, "w") as f: diff --git a/setup.py b/setup.py index 8ef61f3125..e8962f175f 100644 --- a/setup.py +++ b/setup.py @@ -10,7 +10,7 @@ setup( name='gtwrap', description='Library to wrap C++ with Python and Matlab', - version='1.1.0', + version='2.0.0', author="Frank Dellaert et. al.", author_email="dellaert@gatech.edu", license='BSD', diff --git a/tests/expected/matlab/MultipleTemplatesIntDouble.m b/tests/expected/matlab/MultipleTemplatesIntDouble.m index 85de8002fa..1a00572e04 100644 --- a/tests/expected/matlab/MultipleTemplatesIntDouble.m +++ b/tests/expected/matlab/MultipleTemplatesIntDouble.m @@ -9,7 +9,7 @@ function obj = MultipleTemplatesIntDouble(varargin) if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682) my_ptr = varargin{2}; - class_wrapper(48, my_ptr); + class_wrapper(49, my_ptr); else error('Arguments do not match any overload of MultipleTemplatesIntDouble constructor'); end @@ -17,7 +17,7 @@ end function delete(obj) - class_wrapper(49, obj.ptr_MultipleTemplatesIntDouble); + class_wrapper(50, obj.ptr_MultipleTemplatesIntDouble); end function display(obj), obj.print(''); end diff --git a/tests/expected/matlab/MultipleTemplatesIntFloat.m b/tests/expected/matlab/MultipleTemplatesIntFloat.m index 90b79d560e..6239b1bd18 100644 --- a/tests/expected/matlab/MultipleTemplatesIntFloat.m +++ b/tests/expected/matlab/MultipleTemplatesIntFloat.m @@ -9,7 +9,7 @@ function obj = MultipleTemplatesIntFloat(varargin) if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682) my_ptr = varargin{2}; - class_wrapper(50, my_ptr); + class_wrapper(51, my_ptr); else error('Arguments do not match any overload of MultipleTemplatesIntFloat constructor'); end @@ -17,7 +17,7 @@ end function delete(obj) - class_wrapper(51, obj.ptr_MultipleTemplatesIntFloat); + class_wrapper(52, obj.ptr_MultipleTemplatesIntFloat); end function display(obj), obj.print(''); end diff --git a/tests/expected/matlab/MyFactorPosePoint2.m b/tests/expected/matlab/MyFactorPosePoint2.m index ea2e335c71..2dd4b5428c 100644 --- a/tests/expected/matlab/MyFactorPosePoint2.m +++ b/tests/expected/matlab/MyFactorPosePoint2.m @@ -15,9 +15,9 @@ function obj = MyFactorPosePoint2(varargin) if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682) my_ptr = varargin{2}; - class_wrapper(52, my_ptr); + class_wrapper(56, my_ptr); elseif nargin == 4 && isa(varargin{1},'numeric') && isa(varargin{2},'numeric') && isa(varargin{3},'double') && isa(varargin{4},'gtsam.noiseModel.Base') - my_ptr = class_wrapper(53, varargin{1}, varargin{2}, varargin{3}, varargin{4}); + my_ptr = class_wrapper(57, varargin{1}, varargin{2}, varargin{3}, varargin{4}); else error('Arguments do not match any overload of MyFactorPosePoint2 constructor'); end @@ -25,7 +25,7 @@ end function delete(obj) - class_wrapper(54, obj.ptr_MyFactorPosePoint2); + class_wrapper(58, obj.ptr_MyFactorPosePoint2); end function display(obj), obj.print(''); end @@ -36,7 +36,7 @@ function delete(obj) % PRINT usage: print(string s, KeyFormatter keyFormatter) : returns void % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 2 && isa(varargin{1},'char') && isa(varargin{2},'gtsam.KeyFormatter') - class_wrapper(55, this, varargin{:}); + class_wrapper(59, this, varargin{:}); return end error('Arguments do not match any overload of function MyFactorPosePoint2.print'); diff --git a/tests/expected/matlab/MyVector12.m b/tests/expected/matlab/MyVector12.m index c95136cc9e..00a8f19652 100644 --- a/tests/expected/matlab/MyVector12.m +++ b/tests/expected/matlab/MyVector12.m @@ -12,9 +12,9 @@ function obj = MyVector12(varargin) if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682) my_ptr = varargin{2}; - class_wrapper(45, my_ptr); + class_wrapper(46, my_ptr); elseif nargin == 0 - my_ptr = class_wrapper(46); + my_ptr = class_wrapper(47); else error('Arguments do not match any overload of MyVector12 constructor'); end @@ -22,7 +22,7 @@ end function delete(obj) - class_wrapper(47, obj.ptr_MyVector12); + class_wrapper(48, obj.ptr_MyVector12); end function display(obj), obj.print(''); end diff --git a/tests/expected/matlab/MyVector3.m b/tests/expected/matlab/MyVector3.m index fe0ec9c5f7..5d4a80cd81 100644 --- a/tests/expected/matlab/MyVector3.m +++ b/tests/expected/matlab/MyVector3.m @@ -12,9 +12,9 @@ function obj = MyVector3(varargin) if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682) my_ptr = varargin{2}; - class_wrapper(42, my_ptr); + class_wrapper(43, my_ptr); elseif nargin == 0 - my_ptr = class_wrapper(43); + my_ptr = class_wrapper(44); else error('Arguments do not match any overload of MyVector3 constructor'); end @@ -22,7 +22,7 @@ end function delete(obj) - class_wrapper(44, obj.ptr_MyVector3); + class_wrapper(45, obj.ptr_MyVector3); end function display(obj), obj.print(''); end diff --git a/tests/expected/matlab/PrimitiveRefDouble.m b/tests/expected/matlab/PrimitiveRefDouble.m index 8e8e94dc67..14f04a825c 100644 --- a/tests/expected/matlab/PrimitiveRefDouble.m +++ b/tests/expected/matlab/PrimitiveRefDouble.m @@ -19,9 +19,9 @@ function obj = PrimitiveRefDouble(varargin) if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682) my_ptr = varargin{2}; - class_wrapper(38, my_ptr); + class_wrapper(39, my_ptr); elseif nargin == 0 - my_ptr = class_wrapper(39); + my_ptr = class_wrapper(40); else error('Arguments do not match any overload of PrimitiveRefDouble constructor'); end @@ -29,7 +29,7 @@ end function delete(obj) - class_wrapper(40, obj.ptr_PrimitiveRefDouble); + class_wrapper(41, obj.ptr_PrimitiveRefDouble); end function display(obj), obj.print(''); end @@ -43,7 +43,7 @@ function delete(obj) % BRUTAL usage: Brutal(double t) : returns PrimitiveRefdouble % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 1 && isa(varargin{1},'double') - varargout{1} = class_wrapper(41, varargin{:}); + varargout{1} = class_wrapper(42, varargin{:}); return end diff --git a/tests/expected/matlab/TemplatedFunctionRot3.m b/tests/expected/matlab/TemplatedFunctionRot3.m index 5b90c24733..4216201b49 100644 --- a/tests/expected/matlab/TemplatedFunctionRot3.m +++ b/tests/expected/matlab/TemplatedFunctionRot3.m @@ -1,6 +1,6 @@ function varargout = TemplatedFunctionRot3(varargin) if length(varargin) == 1 && isa(varargin{1},'gtsam.Rot3') - functions_wrapper(11, varargin{:}); + functions_wrapper(14, varargin{:}); else error('Arguments do not match any overload of function TemplatedFunctionRot3'); end diff --git a/tests/expected/matlab/Test.m b/tests/expected/matlab/Test.m index 4c7b5eeaba..c39882a93f 100644 --- a/tests/expected/matlab/Test.m +++ b/tests/expected/matlab/Test.m @@ -10,6 +10,7 @@ %create_MixedPtrs() : returns pair< Test, Test > %create_ptrs() : returns pair< Test, Test > %get_container() : returns std::vector +%lambda() : returns void %print() : returns void %return_Point2Ptr(bool value) : returns Point2 %return_Test(Test value) : returns Test @@ -98,11 +99,21 @@ function delete(obj) error('Arguments do not match any overload of function Test.get_container'); end + function varargout = lambda(this, varargin) + % LAMBDA usage: lambda() : returns void + % Doxygen can be found at https://gtsam.org/doxygen/ + if length(varargin) == 0 + class_wrapper(18, this, varargin{:}); + return + end + error('Arguments do not match any overload of function Test.lambda'); + end + function varargout = print(this, varargin) % PRINT usage: print() : returns void % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 0 - class_wrapper(18, this, varargin{:}); + class_wrapper(19, this, varargin{:}); return end error('Arguments do not match any overload of function Test.print'); @@ -112,7 +123,7 @@ function delete(obj) % RETURN_POINT2PTR usage: return_Point2Ptr(bool value) : returns Point2 % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 1 && isa(varargin{1},'logical') - varargout{1} = class_wrapper(19, this, varargin{:}); + varargout{1} = class_wrapper(20, this, varargin{:}); return end error('Arguments do not match any overload of function Test.return_Point2Ptr'); @@ -122,7 +133,7 @@ function delete(obj) % RETURN_TEST usage: return_Test(Test value) : returns Test % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 1 && isa(varargin{1},'Test') - varargout{1} = class_wrapper(20, this, varargin{:}); + varargout{1} = class_wrapper(21, this, varargin{:}); return end error('Arguments do not match any overload of function Test.return_Test'); @@ -132,7 +143,7 @@ function delete(obj) % RETURN_TESTPTR usage: return_TestPtr(Test value) : returns Test % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 1 && isa(varargin{1},'Test') - varargout{1} = class_wrapper(21, this, varargin{:}); + varargout{1} = class_wrapper(22, this, varargin{:}); return end error('Arguments do not match any overload of function Test.return_TestPtr'); @@ -142,7 +153,7 @@ function delete(obj) % RETURN_BOOL usage: return_bool(bool value) : returns bool % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 1 && isa(varargin{1},'logical') - varargout{1} = class_wrapper(22, this, varargin{:}); + varargout{1} = class_wrapper(23, this, varargin{:}); return end error('Arguments do not match any overload of function Test.return_bool'); @@ -152,7 +163,7 @@ function delete(obj) % RETURN_DOUBLE usage: return_double(double value) : returns double % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 1 && isa(varargin{1},'double') - varargout{1} = class_wrapper(23, this, varargin{:}); + varargout{1} = class_wrapper(24, this, varargin{:}); return end error('Arguments do not match any overload of function Test.return_double'); @@ -162,7 +173,7 @@ function delete(obj) % RETURN_FIELD usage: return_field(Test t) : returns bool % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 1 && isa(varargin{1},'Test') - varargout{1} = class_wrapper(24, this, varargin{:}); + varargout{1} = class_wrapper(25, this, varargin{:}); return end error('Arguments do not match any overload of function Test.return_field'); @@ -172,7 +183,7 @@ function delete(obj) % RETURN_INT usage: return_int(int value) : returns int % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 1 && isa(varargin{1},'numeric') - varargout{1} = class_wrapper(25, this, varargin{:}); + varargout{1} = class_wrapper(26, this, varargin{:}); return end error('Arguments do not match any overload of function Test.return_int'); @@ -182,7 +193,7 @@ function delete(obj) % RETURN_MATRIX1 usage: return_matrix1(Matrix value) : returns Matrix % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 1 && isa(varargin{1},'double') - varargout{1} = class_wrapper(26, this, varargin{:}); + varargout{1} = class_wrapper(27, this, varargin{:}); return end error('Arguments do not match any overload of function Test.return_matrix1'); @@ -192,7 +203,7 @@ function delete(obj) % RETURN_MATRIX2 usage: return_matrix2(Matrix value) : returns Matrix % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 1 && isa(varargin{1},'double') - varargout{1} = class_wrapper(27, this, varargin{:}); + varargout{1} = class_wrapper(28, this, varargin{:}); return end error('Arguments do not match any overload of function Test.return_matrix2'); @@ -202,13 +213,13 @@ function delete(obj) % RETURN_PAIR usage: return_pair(Vector v, Matrix A) : returns pair< Vector, Matrix > % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 2 && isa(varargin{1},'double') && size(varargin{1},2)==1 && isa(varargin{2},'double') - [ varargout{1} varargout{2} ] = class_wrapper(28, this, varargin{:}); + [ varargout{1} varargout{2} ] = class_wrapper(29, this, varargin{:}); return end % RETURN_PAIR usage: return_pair(Vector v) : returns pair< Vector, Matrix > % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 1 && isa(varargin{1},'double') && size(varargin{1},2)==1 - [ varargout{1} varargout{2} ] = class_wrapper(29, this, varargin{:}); + [ varargout{1} varargout{2} ] = class_wrapper(30, this, varargin{:}); return end error('Arguments do not match any overload of function Test.return_pair'); @@ -218,7 +229,7 @@ function delete(obj) % RETURN_PTRS usage: return_ptrs(Test p1, Test p2) : returns pair< Test, Test > % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 2 && isa(varargin{1},'Test') && isa(varargin{2},'Test') - [ varargout{1} varargout{2} ] = class_wrapper(30, this, varargin{:}); + [ varargout{1} varargout{2} ] = class_wrapper(31, this, varargin{:}); return end error('Arguments do not match any overload of function Test.return_ptrs'); @@ -228,7 +239,7 @@ function delete(obj) % RETURN_SIZE_T usage: return_size_t(size_t value) : returns size_t % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 1 && isa(varargin{1},'numeric') - varargout{1} = class_wrapper(31, this, varargin{:}); + varargout{1} = class_wrapper(32, this, varargin{:}); return end error('Arguments do not match any overload of function Test.return_size_t'); @@ -238,7 +249,7 @@ function delete(obj) % RETURN_STRING usage: return_string(string value) : returns string % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 1 && isa(varargin{1},'char') - varargout{1} = class_wrapper(32, this, varargin{:}); + varargout{1} = class_wrapper(33, this, varargin{:}); return end error('Arguments do not match any overload of function Test.return_string'); @@ -248,7 +259,7 @@ function delete(obj) % RETURN_VECTOR1 usage: return_vector1(Vector value) : returns Vector % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 1 && isa(varargin{1},'double') && size(varargin{1},2)==1 - varargout{1} = class_wrapper(33, this, varargin{:}); + varargout{1} = class_wrapper(34, this, varargin{:}); return end error('Arguments do not match any overload of function Test.return_vector1'); @@ -258,7 +269,7 @@ function delete(obj) % RETURN_VECTOR2 usage: return_vector2(Vector value) : returns Vector % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 1 && isa(varargin{1},'double') && size(varargin{1},2)==1 - varargout{1} = class_wrapper(34, this, varargin{:}); + varargout{1} = class_wrapper(35, this, varargin{:}); return end error('Arguments do not match any overload of function Test.return_vector2'); @@ -268,19 +279,19 @@ function delete(obj) % SET_CONTAINER usage: set_container(vector container) : returns void % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 1 && isa(varargin{1},'std.vectorTest') - class_wrapper(35, this, varargin{:}); + class_wrapper(36, this, varargin{:}); return end % SET_CONTAINER usage: set_container(vector container) : returns void % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 1 && isa(varargin{1},'std.vectorTest') - class_wrapper(36, this, varargin{:}); + class_wrapper(37, this, varargin{:}); return end % SET_CONTAINER usage: set_container(vector container) : returns void % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 1 && isa(varargin{1},'std.vectorTest') - class_wrapper(37, this, varargin{:}); + class_wrapper(38, this, varargin{:}); return end error('Arguments do not match any overload of function Test.set_container'); diff --git a/tests/expected/matlab/class_wrapper.cpp b/tests/expected/matlab/class_wrapper.cpp index 3fc2e5dafd..e644ac00f2 100644 --- a/tests/expected/matlab/class_wrapper.cpp +++ b/tests/expected/matlab/class_wrapper.cpp @@ -33,6 +33,8 @@ typedef std::set*> Collector_Multi static Collector_MultipleTemplatesIntDouble collector_MultipleTemplatesIntDouble; typedef std::set*> Collector_MultipleTemplatesIntFloat; static Collector_MultipleTemplatesIntFloat collector_MultipleTemplatesIntFloat; +typedef std::set*> Collector_ForwardKinematics; +static Collector_ForwardKinematics collector_ForwardKinematics; typedef std::set*> Collector_MyFactorPosePoint2; static Collector_MyFactorPosePoint2 collector_MyFactorPosePoint2; @@ -90,6 +92,12 @@ void _deleteAllObjects() collector_MultipleTemplatesIntFloat.erase(iter++); anyDeleted = true; } } + { for(Collector_ForwardKinematics::iterator iter = collector_ForwardKinematics.begin(); + iter != collector_ForwardKinematics.end(); ) { + delete *iter; + collector_ForwardKinematics.erase(iter++); + anyDeleted = true; + } } { for(Collector_MyFactorPosePoint2::iterator iter = collector_MyFactorPosePoint2.begin(); iter != collector_MyFactorPosePoint2.end(); ) { delete *iter; @@ -304,14 +312,21 @@ void Test_get_container_17(int nargout, mxArray *out[], int nargin, const mxArra out[0] = wrap_shared_ptr(boost::make_shared>(obj->get_container()),"std.vectorTest", false); } -void Test_print_18(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_lambda_18(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + checkArguments("lambda",nargout,nargin-1,0); + auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); + obj->lambda(); +} + +void Test_print_19(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("print",nargout,nargin-1,0); auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); obj->print(); } -void Test_return_Point2Ptr_19(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_return_Point2Ptr_20(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_Point2Ptr",nargout,nargin-1,1); auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); @@ -322,7 +337,7 @@ void Test_return_Point2Ptr_19(int nargout, mxArray *out[], int nargin, const mxA } } -void Test_return_Test_20(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_return_Test_21(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_Test",nargout,nargin-1,1); auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); @@ -330,7 +345,7 @@ void Test_return_Test_20(int nargout, mxArray *out[], int nargin, const mxArray out[0] = wrap_shared_ptr(boost::make_shared(obj->return_Test(value)),"Test", false); } -void Test_return_TestPtr_21(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_return_TestPtr_22(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_TestPtr",nargout,nargin-1,1); auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); @@ -338,7 +353,7 @@ void Test_return_TestPtr_21(int nargout, mxArray *out[], int nargin, const mxArr out[0] = wrap_shared_ptr(obj->return_TestPtr(value),"Test", false); } -void Test_return_bool_22(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_return_bool_23(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_bool",nargout,nargin-1,1); auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); @@ -346,7 +361,7 @@ void Test_return_bool_22(int nargout, mxArray *out[], int nargin, const mxArray out[0] = wrap< bool >(obj->return_bool(value)); } -void Test_return_double_23(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_return_double_24(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_double",nargout,nargin-1,1); auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); @@ -354,7 +369,7 @@ void Test_return_double_23(int nargout, mxArray *out[], int nargin, const mxArra out[0] = wrap< double >(obj->return_double(value)); } -void Test_return_field_24(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_return_field_25(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_field",nargout,nargin-1,1); auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); @@ -362,7 +377,7 @@ void Test_return_field_24(int nargout, mxArray *out[], int nargin, const mxArray out[0] = wrap< bool >(obj->return_field(t)); } -void Test_return_int_25(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_return_int_26(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_int",nargout,nargin-1,1); auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); @@ -370,7 +385,7 @@ void Test_return_int_25(int nargout, mxArray *out[], int nargin, const mxArray * out[0] = wrap< int >(obj->return_int(value)); } -void Test_return_matrix1_26(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_return_matrix1_27(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_matrix1",nargout,nargin-1,1); auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); @@ -378,7 +393,7 @@ void Test_return_matrix1_26(int nargout, mxArray *out[], int nargin, const mxArr out[0] = wrap< Matrix >(obj->return_matrix1(value)); } -void Test_return_matrix2_27(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_return_matrix2_28(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_matrix2",nargout,nargin-1,1); auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); @@ -386,7 +401,7 @@ void Test_return_matrix2_27(int nargout, mxArray *out[], int nargin, const mxArr out[0] = wrap< Matrix >(obj->return_matrix2(value)); } -void Test_return_pair_28(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_return_pair_29(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_pair",nargout,nargin-1,2); auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); @@ -397,7 +412,7 @@ void Test_return_pair_28(int nargout, mxArray *out[], int nargin, const mxArray out[1] = wrap< Matrix >(pairResult.second); } -void Test_return_pair_29(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_return_pair_30(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_pair",nargout,nargin-1,1); auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); @@ -407,7 +422,7 @@ void Test_return_pair_29(int nargout, mxArray *out[], int nargin, const mxArray out[1] = wrap< Matrix >(pairResult.second); } -void Test_return_ptrs_30(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_return_ptrs_31(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_ptrs",nargout,nargin-1,2); auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); @@ -418,7 +433,7 @@ void Test_return_ptrs_30(int nargout, mxArray *out[], int nargin, const mxArray out[1] = wrap_shared_ptr(pairResult.second,"Test", false); } -void Test_return_size_t_31(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_return_size_t_32(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_size_t",nargout,nargin-1,1); auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); @@ -426,7 +441,7 @@ void Test_return_size_t_31(int nargout, mxArray *out[], int nargin, const mxArra out[0] = wrap< size_t >(obj->return_size_t(value)); } -void Test_return_string_32(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_return_string_33(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_string",nargout,nargin-1,1); auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); @@ -434,7 +449,7 @@ void Test_return_string_32(int nargout, mxArray *out[], int nargin, const mxArra out[0] = wrap< string >(obj->return_string(value)); } -void Test_return_vector1_33(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_return_vector1_34(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_vector1",nargout,nargin-1,1); auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); @@ -442,7 +457,7 @@ void Test_return_vector1_33(int nargout, mxArray *out[], int nargin, const mxArr out[0] = wrap< Vector >(obj->return_vector1(value)); } -void Test_return_vector2_34(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_return_vector2_35(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("return_vector2",nargout,nargin-1,1); auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); @@ -450,7 +465,7 @@ void Test_return_vector2_34(int nargout, mxArray *out[], int nargin, const mxArr out[0] = wrap< Vector >(obj->return_vector2(value)); } -void Test_set_container_35(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_set_container_36(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("set_container",nargout,nargin-1,1); auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); @@ -458,7 +473,7 @@ void Test_set_container_35(int nargout, mxArray *out[], int nargin, const mxArra obj->set_container(*container); } -void Test_set_container_36(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_set_container_37(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("set_container",nargout,nargin-1,1); auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); @@ -466,7 +481,7 @@ void Test_set_container_36(int nargout, mxArray *out[], int nargin, const mxArra obj->set_container(*container); } -void Test_set_container_37(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_set_container_38(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("set_container",nargout,nargin-1,1); auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); @@ -474,7 +489,7 @@ void Test_set_container_37(int nargout, mxArray *out[], int nargin, const mxArra obj->set_container(*container); } -void PrimitiveRefDouble_collectorInsertAndMakeBase_38(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void PrimitiveRefDouble_collectorInsertAndMakeBase_39(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); typedef boost::shared_ptr> Shared; @@ -483,7 +498,7 @@ void PrimitiveRefDouble_collectorInsertAndMakeBase_38(int nargout, mxArray *out[ collector_PrimitiveRefDouble.insert(self); } -void PrimitiveRefDouble_constructor_39(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void PrimitiveRefDouble_constructor_40(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); typedef boost::shared_ptr> Shared; @@ -494,7 +509,7 @@ void PrimitiveRefDouble_constructor_39(int nargout, mxArray *out[], int nargin, *reinterpret_cast (mxGetData(out[0])) = self; } -void PrimitiveRefDouble_deconstructor_40(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void PrimitiveRefDouble_deconstructor_41(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { typedef boost::shared_ptr> Shared; checkArguments("delete_PrimitiveRefDouble",nargout,nargin,1); @@ -507,14 +522,14 @@ void PrimitiveRefDouble_deconstructor_40(int nargout, mxArray *out[], int nargin } } -void PrimitiveRefDouble_Brutal_41(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void PrimitiveRefDouble_Brutal_42(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("PrimitiveRefDouble.Brutal",nargout,nargin,1); double t = unwrap< double >(in[0]); out[0] = wrap_shared_ptr(boost::make_shared>(PrimitiveRef::Brutal(t)),"PrimitiveRefdouble", false); } -void MyVector3_collectorInsertAndMakeBase_42(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void MyVector3_collectorInsertAndMakeBase_43(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); typedef boost::shared_ptr> Shared; @@ -523,7 +538,7 @@ void MyVector3_collectorInsertAndMakeBase_42(int nargout, mxArray *out[], int na collector_MyVector3.insert(self); } -void MyVector3_constructor_43(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void MyVector3_constructor_44(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); typedef boost::shared_ptr> Shared; @@ -534,7 +549,7 @@ void MyVector3_constructor_43(int nargout, mxArray *out[], int nargin, const mxA *reinterpret_cast (mxGetData(out[0])) = self; } -void MyVector3_deconstructor_44(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void MyVector3_deconstructor_45(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { typedef boost::shared_ptr> Shared; checkArguments("delete_MyVector3",nargout,nargin,1); @@ -547,7 +562,7 @@ void MyVector3_deconstructor_44(int nargout, mxArray *out[], int nargin, const m } } -void MyVector12_collectorInsertAndMakeBase_45(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void MyVector12_collectorInsertAndMakeBase_46(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); typedef boost::shared_ptr> Shared; @@ -556,7 +571,7 @@ void MyVector12_collectorInsertAndMakeBase_45(int nargout, mxArray *out[], int n collector_MyVector12.insert(self); } -void MyVector12_constructor_46(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void MyVector12_constructor_47(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); typedef boost::shared_ptr> Shared; @@ -567,7 +582,7 @@ void MyVector12_constructor_46(int nargout, mxArray *out[], int nargin, const mx *reinterpret_cast (mxGetData(out[0])) = self; } -void MyVector12_deconstructor_47(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void MyVector12_deconstructor_48(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { typedef boost::shared_ptr> Shared; checkArguments("delete_MyVector12",nargout,nargin,1); @@ -580,7 +595,7 @@ void MyVector12_deconstructor_47(int nargout, mxArray *out[], int nargin, const } } -void MultipleTemplatesIntDouble_collectorInsertAndMakeBase_48(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void MultipleTemplatesIntDouble_collectorInsertAndMakeBase_49(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); typedef boost::shared_ptr> Shared; @@ -589,7 +604,7 @@ void MultipleTemplatesIntDouble_collectorInsertAndMakeBase_48(int nargout, mxArr collector_MultipleTemplatesIntDouble.insert(self); } -void MultipleTemplatesIntDouble_deconstructor_49(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void MultipleTemplatesIntDouble_deconstructor_50(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { typedef boost::shared_ptr> Shared; checkArguments("delete_MultipleTemplatesIntDouble",nargout,nargin,1); @@ -602,7 +617,7 @@ void MultipleTemplatesIntDouble_deconstructor_49(int nargout, mxArray *out[], in } } -void MultipleTemplatesIntFloat_collectorInsertAndMakeBase_50(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void MultipleTemplatesIntFloat_collectorInsertAndMakeBase_51(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); typedef boost::shared_ptr> Shared; @@ -611,7 +626,7 @@ void MultipleTemplatesIntFloat_collectorInsertAndMakeBase_50(int nargout, mxArra collector_MultipleTemplatesIntFloat.insert(self); } -void MultipleTemplatesIntFloat_deconstructor_51(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void MultipleTemplatesIntFloat_deconstructor_52(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { typedef boost::shared_ptr> Shared; checkArguments("delete_MultipleTemplatesIntFloat",nargout,nargin,1); @@ -624,7 +639,45 @@ void MultipleTemplatesIntFloat_deconstructor_51(int nargout, mxArray *out[], int } } -void MyFactorPosePoint2_collectorInsertAndMakeBase_52(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void ForwardKinematics_collectorInsertAndMakeBase_53(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr Shared; + + Shared *self = *reinterpret_cast (mxGetData(in[0])); + collector_ForwardKinematics.insert(self); +} + +void ForwardKinematics_constructor_54(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr Shared; + + gtdynamics::Robot& robot = *unwrap_shared_ptr< gtdynamics::Robot >(in[0], "ptr_gtdynamicsRobot"); + string& start_link_name = *unwrap_shared_ptr< string >(in[1], "ptr_string"); + string& end_link_name = *unwrap_shared_ptr< string >(in[2], "ptr_string"); + gtsam::Values& joint_angles = *unwrap_shared_ptr< gtsam::Values >(in[3], "ptr_gtsamValues"); + gtsam::Pose3& l2Tp = *unwrap_shared_ptr< gtsam::Pose3 >(in[4], "ptr_gtsamPose3"); + Shared *self = new Shared(new ForwardKinematics(robot,start_link_name,end_link_name,joint_angles,l2Tp)); + collector_ForwardKinematics.insert(self); + out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + *reinterpret_cast (mxGetData(out[0])) = self; +} + +void ForwardKinematics_deconstructor_55(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr Shared; + checkArguments("delete_ForwardKinematics",nargout,nargin,1); + Shared *self = *reinterpret_cast(mxGetData(in[0])); + Collector_ForwardKinematics::iterator item; + item = collector_ForwardKinematics.find(self); + if(item != collector_ForwardKinematics.end()) { + delete self; + collector_ForwardKinematics.erase(item); + } +} + +void MyFactorPosePoint2_collectorInsertAndMakeBase_56(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); typedef boost::shared_ptr> Shared; @@ -633,7 +686,7 @@ void MyFactorPosePoint2_collectorInsertAndMakeBase_52(int nargout, mxArray *out[ collector_MyFactorPosePoint2.insert(self); } -void MyFactorPosePoint2_constructor_53(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void MyFactorPosePoint2_constructor_57(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); typedef boost::shared_ptr> Shared; @@ -648,7 +701,7 @@ void MyFactorPosePoint2_constructor_53(int nargout, mxArray *out[], int nargin, *reinterpret_cast (mxGetData(out[0])) = self; } -void MyFactorPosePoint2_deconstructor_54(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void MyFactorPosePoint2_deconstructor_58(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { typedef boost::shared_ptr> Shared; checkArguments("delete_MyFactorPosePoint2",nargout,nargin,1); @@ -661,7 +714,7 @@ void MyFactorPosePoint2_deconstructor_54(int nargout, mxArray *out[], int nargin } } -void MyFactorPosePoint2_print_55(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void MyFactorPosePoint2_print_59(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("print",nargout,nargin-1,2); auto obj = unwrap_shared_ptr>(in[0], "ptr_MyFactorPosePoint2"); @@ -737,58 +790,58 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) Test_get_container_17(nargout, out, nargin-1, in+1); break; case 18: - Test_print_18(nargout, out, nargin-1, in+1); + Test_lambda_18(nargout, out, nargin-1, in+1); break; case 19: - Test_return_Point2Ptr_19(nargout, out, nargin-1, in+1); + Test_print_19(nargout, out, nargin-1, in+1); break; case 20: - Test_return_Test_20(nargout, out, nargin-1, in+1); + Test_return_Point2Ptr_20(nargout, out, nargin-1, in+1); break; case 21: - Test_return_TestPtr_21(nargout, out, nargin-1, in+1); + Test_return_Test_21(nargout, out, nargin-1, in+1); break; case 22: - Test_return_bool_22(nargout, out, nargin-1, in+1); + Test_return_TestPtr_22(nargout, out, nargin-1, in+1); break; case 23: - Test_return_double_23(nargout, out, nargin-1, in+1); + Test_return_bool_23(nargout, out, nargin-1, in+1); break; case 24: - Test_return_field_24(nargout, out, nargin-1, in+1); + Test_return_double_24(nargout, out, nargin-1, in+1); break; case 25: - Test_return_int_25(nargout, out, nargin-1, in+1); + Test_return_field_25(nargout, out, nargin-1, in+1); break; case 26: - Test_return_matrix1_26(nargout, out, nargin-1, in+1); + Test_return_int_26(nargout, out, nargin-1, in+1); break; case 27: - Test_return_matrix2_27(nargout, out, nargin-1, in+1); + Test_return_matrix1_27(nargout, out, nargin-1, in+1); break; case 28: - Test_return_pair_28(nargout, out, nargin-1, in+1); + Test_return_matrix2_28(nargout, out, nargin-1, in+1); break; case 29: Test_return_pair_29(nargout, out, nargin-1, in+1); break; case 30: - Test_return_ptrs_30(nargout, out, nargin-1, in+1); + Test_return_pair_30(nargout, out, nargin-1, in+1); break; case 31: - Test_return_size_t_31(nargout, out, nargin-1, in+1); + Test_return_ptrs_31(nargout, out, nargin-1, in+1); break; case 32: - Test_return_string_32(nargout, out, nargin-1, in+1); + Test_return_size_t_32(nargout, out, nargin-1, in+1); break; case 33: - Test_return_vector1_33(nargout, out, nargin-1, in+1); + Test_return_string_33(nargout, out, nargin-1, in+1); break; case 34: - Test_return_vector2_34(nargout, out, nargin-1, in+1); + Test_return_vector1_34(nargout, out, nargin-1, in+1); break; case 35: - Test_set_container_35(nargout, out, nargin-1, in+1); + Test_return_vector2_35(nargout, out, nargin-1, in+1); break; case 36: Test_set_container_36(nargout, out, nargin-1, in+1); @@ -797,58 +850,70 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) Test_set_container_37(nargout, out, nargin-1, in+1); break; case 38: - PrimitiveRefDouble_collectorInsertAndMakeBase_38(nargout, out, nargin-1, in+1); + Test_set_container_38(nargout, out, nargin-1, in+1); break; case 39: - PrimitiveRefDouble_constructor_39(nargout, out, nargin-1, in+1); + PrimitiveRefDouble_collectorInsertAndMakeBase_39(nargout, out, nargin-1, in+1); break; case 40: - PrimitiveRefDouble_deconstructor_40(nargout, out, nargin-1, in+1); + PrimitiveRefDouble_constructor_40(nargout, out, nargin-1, in+1); break; case 41: - PrimitiveRefDouble_Brutal_41(nargout, out, nargin-1, in+1); + PrimitiveRefDouble_deconstructor_41(nargout, out, nargin-1, in+1); break; case 42: - MyVector3_collectorInsertAndMakeBase_42(nargout, out, nargin-1, in+1); + PrimitiveRefDouble_Brutal_42(nargout, out, nargin-1, in+1); break; case 43: - MyVector3_constructor_43(nargout, out, nargin-1, in+1); + MyVector3_collectorInsertAndMakeBase_43(nargout, out, nargin-1, in+1); break; case 44: - MyVector3_deconstructor_44(nargout, out, nargin-1, in+1); + MyVector3_constructor_44(nargout, out, nargin-1, in+1); break; case 45: - MyVector12_collectorInsertAndMakeBase_45(nargout, out, nargin-1, in+1); + MyVector3_deconstructor_45(nargout, out, nargin-1, in+1); break; case 46: - MyVector12_constructor_46(nargout, out, nargin-1, in+1); + MyVector12_collectorInsertAndMakeBase_46(nargout, out, nargin-1, in+1); break; case 47: - MyVector12_deconstructor_47(nargout, out, nargin-1, in+1); + MyVector12_constructor_47(nargout, out, nargin-1, in+1); break; case 48: - MultipleTemplatesIntDouble_collectorInsertAndMakeBase_48(nargout, out, nargin-1, in+1); + MyVector12_deconstructor_48(nargout, out, nargin-1, in+1); break; case 49: - MultipleTemplatesIntDouble_deconstructor_49(nargout, out, nargin-1, in+1); + MultipleTemplatesIntDouble_collectorInsertAndMakeBase_49(nargout, out, nargin-1, in+1); break; case 50: - MultipleTemplatesIntFloat_collectorInsertAndMakeBase_50(nargout, out, nargin-1, in+1); + MultipleTemplatesIntDouble_deconstructor_50(nargout, out, nargin-1, in+1); break; case 51: - MultipleTemplatesIntFloat_deconstructor_51(nargout, out, nargin-1, in+1); + MultipleTemplatesIntFloat_collectorInsertAndMakeBase_51(nargout, out, nargin-1, in+1); break; case 52: - MyFactorPosePoint2_collectorInsertAndMakeBase_52(nargout, out, nargin-1, in+1); + MultipleTemplatesIntFloat_deconstructor_52(nargout, out, nargin-1, in+1); break; case 53: - MyFactorPosePoint2_constructor_53(nargout, out, nargin-1, in+1); + ForwardKinematics_collectorInsertAndMakeBase_53(nargout, out, nargin-1, in+1); break; case 54: - MyFactorPosePoint2_deconstructor_54(nargout, out, nargin-1, in+1); + ForwardKinematics_constructor_54(nargout, out, nargin-1, in+1); break; case 55: - MyFactorPosePoint2_print_55(nargout, out, nargin-1, in+1); + ForwardKinematics_deconstructor_55(nargout, out, nargin-1, in+1); + break; + case 56: + MyFactorPosePoint2_collectorInsertAndMakeBase_56(nargout, out, nargin-1, in+1); + break; + case 57: + MyFactorPosePoint2_constructor_57(nargout, out, nargin-1, in+1); + break; + case 58: + MyFactorPosePoint2_deconstructor_58(nargout, out, nargin-1, in+1); + break; + case 59: + MyFactorPosePoint2_print_59(nargout, out, nargin-1, in+1); break; } } catch(const std::exception& e) { diff --git a/tests/expected/matlab/functions_wrapper.cpp b/tests/expected/matlab/functions_wrapper.cpp index 536733bdcc..ae7f49c410 100644 --- a/tests/expected/matlab/functions_wrapper.cpp +++ b/tests/expected/matlab/functions_wrapper.cpp @@ -33,6 +33,8 @@ typedef std::set*> Collector_Multi static Collector_MultipleTemplatesIntDouble collector_MultipleTemplatesIntDouble; typedef std::set*> Collector_MultipleTemplatesIntFloat; static Collector_MultipleTemplatesIntFloat collector_MultipleTemplatesIntFloat; +typedef std::set*> Collector_ForwardKinematics; +static Collector_ForwardKinematics collector_ForwardKinematics; typedef std::set*> Collector_MyFactorPosePoint2; static Collector_MyFactorPosePoint2 collector_MyFactorPosePoint2; @@ -90,6 +92,12 @@ void _deleteAllObjects() collector_MultipleTemplatesIntFloat.erase(iter++); anyDeleted = true; } } + { for(Collector_ForwardKinematics::iterator iter = collector_ForwardKinematics.begin(); + iter != collector_ForwardKinematics.end(); ) { + delete *iter; + collector_ForwardKinematics.erase(iter++); + anyDeleted = true; + } } { for(Collector_MyFactorPosePoint2::iterator iter = collector_MyFactorPosePoint2.begin(); iter != collector_MyFactorPosePoint2.end(); ) { delete *iter; @@ -198,9 +206,10 @@ void MultiTemplatedFunctionDoubleSize_tDouble_7(int nargout, mxArray *out[], int } void DefaultFuncInt_8(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - checkArguments("DefaultFuncInt",nargout,nargin,1); + checkArguments("DefaultFuncInt",nargout,nargin,2); int a = unwrap< int >(in[0]); - DefaultFuncInt(a); + int b = unwrap< int >(in[1]); + DefaultFuncInt(a,b); } void DefaultFuncString_9(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { @@ -215,7 +224,30 @@ void DefaultFuncObj_10(int nargout, mxArray *out[], int nargin, const mxArray *i gtsam::KeyFormatter& keyFormatter = *unwrap_shared_ptr< gtsam::KeyFormatter >(in[0], "ptr_gtsamKeyFormatter"); DefaultFuncObj(keyFormatter); } -void TemplatedFunctionRot3_11(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void DefaultFuncZero_11(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + checkArguments("DefaultFuncZero",nargout,nargin,5); + int a = unwrap< int >(in[0]); + int b = unwrap< int >(in[1]); + double c = unwrap< double >(in[2]); + bool d = unwrap< bool >(in[3]); + bool e = unwrap< bool >(in[4]); + DefaultFuncZero(a,b,c,d,e); +} +void DefaultFuncVector_12(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + checkArguments("DefaultFuncVector",nargout,nargin,2); + std::vector& i = *unwrap_shared_ptr< std::vector >(in[0], "ptr_stdvectorint"); + std::vector& s = *unwrap_shared_ptr< std::vector >(in[1], "ptr_stdvectorstring"); + DefaultFuncVector(i,s); +} +void setPose_13(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + checkArguments("setPose",nargout,nargin,1); + gtsam::Pose3& pose = *unwrap_shared_ptr< gtsam::Pose3 >(in[0], "ptr_gtsamPose3"); + setPose(pose); +} +void TemplatedFunctionRot3_14(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("TemplatedFunctionRot3",nargout,nargin,1); gtsam::Rot3& t = *unwrap_shared_ptr< gtsam::Rot3 >(in[0], "ptr_gtsamRot3"); @@ -267,7 +299,16 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) DefaultFuncObj_10(nargout, out, nargin-1, in+1); break; case 11: - TemplatedFunctionRot3_11(nargout, out, nargin-1, in+1); + DefaultFuncZero_11(nargout, out, nargin-1, in+1); + break; + case 12: + DefaultFuncVector_12(nargout, out, nargin-1, in+1); + break; + case 13: + setPose_13(nargout, out, nargin-1, in+1); + break; + case 14: + TemplatedFunctionRot3_14(nargout, out, nargin-1, in+1); break; } } catch(const std::exception& e) { diff --git a/tests/expected/matlab/geometry_wrapper.cpp b/tests/expected/matlab/geometry_wrapper.cpp index 766c64bb31..4d8a7c7893 100644 --- a/tests/expected/matlab/geometry_wrapper.cpp +++ b/tests/expected/matlab/geometry_wrapper.cpp @@ -36,6 +36,8 @@ typedef std::set*> Collector_Multi static Collector_MultipleTemplatesIntDouble collector_MultipleTemplatesIntDouble; typedef std::set*> Collector_MultipleTemplatesIntFloat; static Collector_MultipleTemplatesIntFloat collector_MultipleTemplatesIntFloat; +typedef std::set*> Collector_ForwardKinematics; +static Collector_ForwardKinematics collector_ForwardKinematics; typedef std::set*> Collector_MyFactorPosePoint2; static Collector_MyFactorPosePoint2 collector_MyFactorPosePoint2; typedef std::set*> Collector_gtsamPoint2; @@ -97,6 +99,12 @@ void _deleteAllObjects() collector_MultipleTemplatesIntFloat.erase(iter++); anyDeleted = true; } } + { for(Collector_ForwardKinematics::iterator iter = collector_ForwardKinematics.begin(); + iter != collector_ForwardKinematics.end(); ) { + delete *iter; + collector_ForwardKinematics.erase(iter++); + anyDeleted = true; + } } { for(Collector_MyFactorPosePoint2::iterator iter = collector_MyFactorPosePoint2.begin(); iter != collector_MyFactorPosePoint2.end(); ) { delete *iter; diff --git a/tests/expected/matlab/inheritance_wrapper.cpp b/tests/expected/matlab/inheritance_wrapper.cpp index e68b3a6e4a..077df48305 100644 --- a/tests/expected/matlab/inheritance_wrapper.cpp +++ b/tests/expected/matlab/inheritance_wrapper.cpp @@ -38,6 +38,8 @@ typedef std::set*> Collector_Multi static Collector_MultipleTemplatesIntDouble collector_MultipleTemplatesIntDouble; typedef std::set*> Collector_MultipleTemplatesIntFloat; static Collector_MultipleTemplatesIntFloat collector_MultipleTemplatesIntFloat; +typedef std::set*> Collector_ForwardKinematics; +static Collector_ForwardKinematics collector_ForwardKinematics; typedef std::set*> Collector_MyFactorPosePoint2; static Collector_MyFactorPosePoint2 collector_MyFactorPosePoint2; typedef std::set*> Collector_gtsamPoint2; @@ -107,6 +109,12 @@ void _deleteAllObjects() collector_MultipleTemplatesIntFloat.erase(iter++); anyDeleted = true; } } + { for(Collector_ForwardKinematics::iterator iter = collector_ForwardKinematics.begin(); + iter != collector_ForwardKinematics.end(); ) { + delete *iter; + collector_ForwardKinematics.erase(iter++); + anyDeleted = true; + } } { for(Collector_MyFactorPosePoint2::iterator iter = collector_MyFactorPosePoint2.begin(); iter != collector_MyFactorPosePoint2.end(); ) { delete *iter; @@ -564,12 +572,12 @@ void MyTemplateMatrix_Level_34(int nargout, mxArray *out[], int nargin, const mx out[0] = wrap_shared_ptr(boost::make_shared>(MyTemplate::Level(K)),"MyTemplateMatrix", false); } -void Test_set_container_35(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void Test_return_vector2_35(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - checkArguments("set_container",nargout,nargin-1,1); + checkArguments("return_vector2",nargout,nargin-1,1); auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); - boost::shared_ptr> container = unwrap_shared_ptr< std::vector >(in[1], "ptr_stdvectorTest"); - obj->set_container(*container); + Vector value = unwrap< Vector >(in[1]); + out[0] = wrap< Vector >(obj->return_vector2(value)); } void ForwardKinematicsFactor_collectorInsertAndMakeBase_35(int nargout, mxArray *out[], int nargin, const mxArray *in[]) @@ -716,7 +724,7 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) MyTemplateMatrix_Level_34(nargout, out, nargin-1, in+1); break; case 35: - Test_set_container_35(nargout, out, nargin-1, in+1); + Test_return_vector2_35(nargout, out, nargin-1, in+1); break; case 36: ForwardKinematicsFactor_collectorInsertAndMakeBase_35(nargout, out, nargin-1, in+1); diff --git a/tests/expected/matlab/namespaces_wrapper.cpp b/tests/expected/matlab/namespaces_wrapper.cpp index aba5c49eaa..8f6e415e2d 100644 --- a/tests/expected/matlab/namespaces_wrapper.cpp +++ b/tests/expected/matlab/namespaces_wrapper.cpp @@ -8,6 +8,7 @@ #include #include #include +#include #include #include #include @@ -43,6 +44,8 @@ typedef std::set*> Collector_Multi static Collector_MultipleTemplatesIntDouble collector_MultipleTemplatesIntDouble; typedef std::set*> Collector_MultipleTemplatesIntFloat; static Collector_MultipleTemplatesIntFloat collector_MultipleTemplatesIntFloat; +typedef std::set*> Collector_ForwardKinematics; +static Collector_ForwardKinematics collector_ForwardKinematics; typedef std::set*> Collector_MyFactorPosePoint2; static Collector_MyFactorPosePoint2 collector_MyFactorPosePoint2; typedef std::set*> Collector_gtsamPoint2; @@ -69,6 +72,8 @@ typedef std::set*> Collector_ns2ClassC; static Collector_ns2ClassC collector_ns2ClassC; typedef std::set*> Collector_ClassD; static Collector_ClassD collector_ClassD; +typedef std::set*> Collector_gtsamValues; +static Collector_gtsamValues collector_gtsamValues; void _deleteAllObjects() { @@ -124,6 +129,12 @@ void _deleteAllObjects() collector_MultipleTemplatesIntFloat.erase(iter++); anyDeleted = true; } } + { for(Collector_ForwardKinematics::iterator iter = collector_ForwardKinematics.begin(); + iter != collector_ForwardKinematics.end(); ) { + delete *iter; + collector_ForwardKinematics.erase(iter++); + anyDeleted = true; + } } { for(Collector_MyFactorPosePoint2::iterator iter = collector_MyFactorPosePoint2.begin(); iter != collector_MyFactorPosePoint2.end(); ) { delete *iter; @@ -202,6 +213,12 @@ void _deleteAllObjects() collector_ClassD.erase(iter++); anyDeleted = true; } } + { for(Collector_gtsamValues::iterator iter = collector_gtsamValues.begin(); + iter != collector_gtsamValues.end(); ) { + delete *iter; + collector_gtsamValues.erase(iter++); + anyDeleted = true; + } } if(anyDeleted) cout << "WARNING: Wrap modules with variables in the workspace have been reloaded due to\n" @@ -491,6 +508,69 @@ void ClassD_deconstructor_25(int nargout, mxArray *out[], int nargin, const mxAr } } +void gtsamValues_collectorInsertAndMakeBase_26(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr Shared; + + Shared *self = *reinterpret_cast (mxGetData(in[0])); + collector_gtsamValues.insert(self); +} + +void gtsamValues_constructor_27(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr Shared; + + Shared *self = new Shared(new gtsam::Values()); + collector_gtsamValues.insert(self); + out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + *reinterpret_cast (mxGetData(out[0])) = self; +} + +void gtsamValues_constructor_28(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef boost::shared_ptr Shared; + + gtsam::Values& other = *unwrap_shared_ptr< gtsam::Values >(in[0], "ptr_gtsamValues"); + Shared *self = new Shared(new gtsam::Values(other)); + collector_gtsamValues.insert(self); + out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + *reinterpret_cast (mxGetData(out[0])) = self; +} + +void gtsamValues_deconstructor_29(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef boost::shared_ptr Shared; + checkArguments("delete_gtsamValues",nargout,nargin,1); + Shared *self = *reinterpret_cast(mxGetData(in[0])); + Collector_gtsamValues::iterator item; + item = collector_gtsamValues.find(self); + if(item != collector_gtsamValues.end()) { + delete self; + collector_gtsamValues.erase(item); + } +} + +void gtsamValues_insert_30(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + checkArguments("insert",nargout,nargin-1,2); + auto obj = unwrap_shared_ptr(in[0], "ptr_gtsamValues"); + size_t j = unwrap< size_t >(in[1]); + Vector vector = unwrap< Vector >(in[2]); + obj->insert(j,vector); +} + +void gtsamValues_insert_31(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + checkArguments("insert",nargout,nargin-1,2); + auto obj = unwrap_shared_ptr(in[0], "ptr_gtsamValues"); + size_t j = unwrap< size_t >(in[1]); + Matrix matrix = unwrap< Matrix >(in[2]); + obj->insert(j,matrix); +} + void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { @@ -581,6 +661,24 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) case 25: ClassD_deconstructor_25(nargout, out, nargin-1, in+1); break; + case 26: + gtsamValues_collectorInsertAndMakeBase_26(nargout, out, nargin-1, in+1); + break; + case 27: + gtsamValues_constructor_27(nargout, out, nargin-1, in+1); + break; + case 28: + gtsamValues_constructor_28(nargout, out, nargin-1, in+1); + break; + case 29: + gtsamValues_deconstructor_29(nargout, out, nargin-1, in+1); + break; + case 30: + gtsamValues_insert_30(nargout, out, nargin-1, in+1); + break; + case 31: + gtsamValues_insert_31(nargout, out, nargin-1, in+1); + break; } } catch(const std::exception& e) { mexErrMsgTxt(("Exception from gtsam:\n" + std::string(e.what()) + "\n").c_str()); diff --git a/tests/expected/matlab/special_cases_wrapper.cpp b/tests/expected/matlab/special_cases_wrapper.cpp index d79a41ace5..056ce80973 100644 --- a/tests/expected/matlab/special_cases_wrapper.cpp +++ b/tests/expected/matlab/special_cases_wrapper.cpp @@ -9,6 +9,7 @@ #include #include #include +#include #include #include #include @@ -46,6 +47,8 @@ typedef std::set*> Collector_Multi static Collector_MultipleTemplatesIntDouble collector_MultipleTemplatesIntDouble; typedef std::set*> Collector_MultipleTemplatesIntFloat; static Collector_MultipleTemplatesIntFloat collector_MultipleTemplatesIntFloat; +typedef std::set*> Collector_ForwardKinematics; +static Collector_ForwardKinematics collector_ForwardKinematics; typedef std::set*> Collector_MyFactorPosePoint2; static Collector_MyFactorPosePoint2 collector_MyFactorPosePoint2; typedef std::set*> Collector_gtsamPoint2; @@ -72,6 +75,8 @@ typedef std::set*> Collector_ns2ClassC; static Collector_ns2ClassC collector_ns2ClassC; typedef std::set*> Collector_ClassD; static Collector_ClassD collector_ClassD; +typedef std::set*> Collector_gtsamValues; +static Collector_gtsamValues collector_gtsamValues; typedef std::set*> Collector_gtsamNonlinearFactorGraph; static Collector_gtsamNonlinearFactorGraph collector_gtsamNonlinearFactorGraph; typedef std::set*> Collector_gtsamSfmTrack; @@ -135,6 +140,12 @@ void _deleteAllObjects() collector_MultipleTemplatesIntFloat.erase(iter++); anyDeleted = true; } } + { for(Collector_ForwardKinematics::iterator iter = collector_ForwardKinematics.begin(); + iter != collector_ForwardKinematics.end(); ) { + delete *iter; + collector_ForwardKinematics.erase(iter++); + anyDeleted = true; + } } { for(Collector_MyFactorPosePoint2::iterator iter = collector_MyFactorPosePoint2.begin(); iter != collector_MyFactorPosePoint2.end(); ) { delete *iter; @@ -213,6 +224,12 @@ void _deleteAllObjects() collector_ClassD.erase(iter++); anyDeleted = true; } } + { for(Collector_gtsamValues::iterator iter = collector_gtsamValues.begin(); + iter != collector_gtsamValues.end(); ) { + delete *iter; + collector_gtsamValues.erase(iter++); + anyDeleted = true; + } } { for(Collector_gtsamNonlinearFactorGraph::iterator iter = collector_gtsamNonlinearFactorGraph.begin(); iter != collector_gtsamNonlinearFactorGraph.end(); ) { delete *iter; diff --git a/tests/expected/python/class_pybind.cpp b/tests/expected/python/class_pybind.cpp index 961daeffe4..4c2371a42c 100644 --- a/tests/expected/python/class_pybind.cpp +++ b/tests/expected/python/class_pybind.cpp @@ -55,13 +55,14 @@ PYBIND11_MODULE(class_py, m_) { .def("create_ptrs",[](Test* self){return self->create_ptrs();}) .def("create_MixedPtrs",[](Test* self){return self->create_MixedPtrs();}) .def("return_ptrs",[](Test* self, std::shared_ptr p1, std::shared_ptr p2){return self->return_ptrs(p1, p2);}, py::arg("p1"), py::arg("p2")) - .def("print_",[](Test* self){ py::scoped_ostream_redirect output; self->print();}) + .def("print",[](Test* self){ py::scoped_ostream_redirect output; self->print();}) .def("__repr__", [](const Test& self){ gtsam::RedirectCout redirect; self.print(); return redirect.str(); }) + .def("lambda_",[](Test* self){ self->lambda();}) .def("set_container",[](Test* self, std::vector container){ self->set_container(container);}, py::arg("container")) .def("set_container",[](Test* self, std::vector> container){ self->set_container(container);}, py::arg("container")) .def("set_container",[](Test* self, std::vector container){ self->set_container(container);}, py::arg("container")) @@ -82,9 +83,12 @@ PYBIND11_MODULE(class_py, m_) { py::class_, std::shared_ptr>>(m_, "MultipleTemplatesIntFloat"); + py::class_>(m_, "ForwardKinematics") + .def(py::init(), py::arg("robot"), py::arg("start_link_name"), py::arg("end_link_name"), py::arg("joint_angles"), py::arg("l2Tp") = gtsam::Pose3()); + py::class_, std::shared_ptr>>(m_, "MyFactorPosePoint2") .def(py::init>(), py::arg("key1"), py::arg("key2"), py::arg("measured"), py::arg("noiseModel")) - .def("print_",[](MyFactor* self, const string& s, const gtsam::KeyFormatter& keyFormatter){ py::scoped_ostream_redirect output; self->print(s, keyFormatter);}, py::arg("s") = "factor: ", py::arg("keyFormatter") = gtsam::DefaultKeyFormatter) + .def("print",[](MyFactor* self, const string& s, const gtsam::KeyFormatter& keyFormatter){ py::scoped_ostream_redirect output; self->print(s, keyFormatter);}, py::arg("s") = "factor: ", py::arg("keyFormatter") = gtsam::DefaultKeyFormatter) .def("__repr__", [](const MyFactor& self, const string& s, const gtsam::KeyFormatter& keyFormatter){ gtsam::RedirectCout redirect; @@ -92,6 +96,7 @@ PYBIND11_MODULE(class_py, m_) { return redirect.str(); }, py::arg("s") = "factor: ", py::arg("keyFormatter") = gtsam::DefaultKeyFormatter); + py::class_, std::shared_ptr>>(m_, "SuperCoolFactorPose3") #include "python/specializations.h" diff --git a/tests/expected/python/functions_pybind.cpp b/tests/expected/python/functions_pybind.cpp index 47c540bc09..bee95ec03f 100644 --- a/tests/expected/python/functions_pybind.cpp +++ b/tests/expected/python/functions_pybind.cpp @@ -30,9 +30,12 @@ PYBIND11_MODULE(functions_py, m_) { m_.def("overloadedGlobalFunction",[](int a, double b){return ::overloadedGlobalFunction(a, b);}, py::arg("a"), py::arg("b")); m_.def("MultiTemplatedFunctionStringSize_tDouble",[](const T& x, size_t y){return ::MultiTemplatedFunction(x, y);}, py::arg("x"), py::arg("y")); m_.def("MultiTemplatedFunctionDoubleSize_tDouble",[](const T& x, size_t y){return ::MultiTemplatedFunction(x, y);}, py::arg("x"), py::arg("y")); - m_.def("DefaultFuncInt",[](int a){ ::DefaultFuncInt(a);}, py::arg("a") = 123); + m_.def("DefaultFuncInt",[](int a, int b){ ::DefaultFuncInt(a, b);}, py::arg("a") = 123, py::arg("b") = 0); m_.def("DefaultFuncString",[](const string& s, const string& name){ ::DefaultFuncString(s, name);}, py::arg("s") = "hello", py::arg("name") = ""); m_.def("DefaultFuncObj",[](const gtsam::KeyFormatter& keyFormatter){ ::DefaultFuncObj(keyFormatter);}, py::arg("keyFormatter") = gtsam::DefaultKeyFormatter); + m_.def("DefaultFuncZero",[](int a, int b, double c, bool d, bool e){ ::DefaultFuncZero(a, b, c, d, e);}, py::arg("a") = 0, py::arg("b"), py::arg("c") = 0.0, py::arg("d") = false, py::arg("e")); + m_.def("DefaultFuncVector",[](const std::vector& i, const std::vector& s){ ::DefaultFuncVector(i, s);}, py::arg("i") = {1, 2, 3}, py::arg("s") = {"borglab", "gtsam"}); + m_.def("setPose",[](const gtsam::Pose3& pose){ ::setPose(pose);}, py::arg("pose") = gtsam::Pose3()); m_.def("TemplatedFunctionRot3",[](const gtsam::Rot3& t){ ::TemplatedFunction(t);}, py::arg("t")); #include "python/specializations.h" diff --git a/tests/expected/python/namespaces_pybind.cpp b/tests/expected/python/namespaces_pybind.cpp index b09fe36eb6..53e9d186a0 100644 --- a/tests/expected/python/namespaces_pybind.cpp +++ b/tests/expected/python/namespaces_pybind.cpp @@ -11,6 +11,7 @@ #include "path/to/ns2.h" #include "path/to/ns2/ClassA.h" #include "path/to/ns3.h" +#include "gtsam/nonlinear/Values.h" #include "wrap/serialization.h" #include @@ -57,7 +58,16 @@ PYBIND11_MODULE(namespaces_py, m_) { py::class_>(m_, "ClassD") .def(py::init<>()); - m_.attr("aGlobalVar") = aGlobalVar; + m_.attr("aGlobalVar") = aGlobalVar; pybind11::module m_gtsam = m_.def_submodule("gtsam", "gtsam submodule"); + + py::class_>(m_gtsam, "Values") + .def(py::init<>()) + .def(py::init(), py::arg("other")) + .def("insert_vector",[](gtsam::Values* self, size_t j, const gtsam::Vector& vector){ self->insert(j, vector);}, py::arg("j"), py::arg("vector")) + .def("insert",[](gtsam::Values* self, size_t j, const gtsam::Vector& vector){ self->insert(j, vector);}, py::arg("j"), py::arg("vector")) + .def("insert_matrix",[](gtsam::Values* self, size_t j, const gtsam::Matrix& matrix){ self->insert(j, matrix);}, py::arg("j"), py::arg("matrix")) + .def("insert",[](gtsam::Values* self, size_t j, const gtsam::Matrix& matrix){ self->insert(j, matrix);}, py::arg("j"), py::arg("matrix")); + #include "python/specializations.h" diff --git a/tests/fixtures/class.i b/tests/fixtures/class.i index f49725ffa9..9e30b17b5c 100644 --- a/tests/fixtures/class.i +++ b/tests/fixtures/class.i @@ -61,7 +61,10 @@ class Test { pair create_MixedPtrs () const; pair return_ptrs (Test* p1, Test* p2) const; + // This should be callable as .print() in python void print() const; + // Since this is a reserved keyword, it should be updated to `lambda_` + void lambda() const; void set_container(std::vector container); void set_container(std::vector container); @@ -106,3 +109,14 @@ class MyVector { // Class with multiple instantiated templates template class MultipleTemplates {}; + +// Test for default args in constructor +class ForwardKinematics { + ForwardKinematics(const gtdynamics::Robot& robot, + const string& start_link_name, const string& end_link_name, + const gtsam::Values& joint_angles, + const gtsam::Pose3& l2Tp = gtsam::Pose3()); +}; + +class SuperCoolFactor; +typedef SuperCoolFactor SuperCoolFactorPose3; diff --git a/tests/fixtures/functions.i b/tests/fixtures/functions.i index 2980286913..0852a3e1e9 100644 --- a/tests/fixtures/functions.i +++ b/tests/fixtures/functions.i @@ -28,6 +28,11 @@ void TemplatedFunction(const T& t); typedef TemplatedFunction TemplatedFunctionRot3; // Check default arguments -void DefaultFuncInt(int a = 123); +void DefaultFuncInt(int a = 123, int b = 0); void DefaultFuncString(const string& s = "hello", const string& name = ""); void DefaultFuncObj(const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter); +void DefaultFuncZero(int a = 0, int b, double c = 0.0, bool d = false, bool e); +void DefaultFuncVector(const std::vector &i = {1, 2, 3}, const std::vector &s = {"borglab", "gtsam"}); + +// Test for non-trivial default constructor +void setPose(const gtsam::Pose3& pose = gtsam::Pose3()); diff --git a/tests/fixtures/namespaces.i b/tests/fixtures/namespaces.i index 5c277801d5..871087a378 100644 --- a/tests/fixtures/namespaces.i +++ b/tests/fixtures/namespaces.i @@ -60,3 +60,14 @@ class ClassD { }; int aGlobalVar; + +namespace gtsam { + #include +class Values { + Values(); + Values(const gtsam::Values& other); + + void insert(size_t j, Vector vector); + void insert(size_t j, Matrix matrix); +}; +} \ No newline at end of file diff --git a/tests/test_docs.py b/tests/test_docs.py index 622d6d14f0..ca8cdbdde8 100644 --- a/tests/test_docs.py +++ b/tests/test_docs.py @@ -41,7 +41,6 @@ class TestDocument(unittest.TestCase): OUTPUT_XML_DIR_PATH = path.abspath(path.join(DIR_NAME, OUTPUT_XML_DIR)) EXPECTED_XML_DIR_PATH = path.abspath(path.join(DIR_NAME, EXPECTED_XML_DIR)) - @unittest.skip("DOC_DIR_PATH doesn't exist") def test_generate_xml(self): """Test parse_xml.generate_xml""" if path.exists(self.OUTPUT_XML_DIR_PATH): @@ -65,7 +64,6 @@ def test_generate_xml(self): self.assertTrue(not dircmp.diff_files and not dircmp.funny_files) - @unittest.skip("DOC_DIR_PATH doesn't exist") def test_parse(self): """Test the parsing of the XML generated by Doxygen.""" docs = parser.ParseDoxygenXML(self.DOC_DIR_PATH, diff --git a/tests/test_interface_parser.py b/tests/test_interface_parser.py index 70f044f04a..95987f42f2 100644 --- a/tests/test_interface_parser.py +++ b/tests/test_interface_parser.py @@ -142,9 +142,9 @@ def test_argument_list(self): "const C6* c6" args = ArgumentList.rule.parseString(arg_string)[0] - self.assertEqual(7, len(args.args_list)) + self.assertEqual(7, len(args.list())) self.assertEqual(['a', 'c1', 'c2', 'c3', 'c4', 'c5', 'c6'], - args.args_names()) + args.names()) def test_argument_list_qualifiers(self): """ @@ -153,7 +153,7 @@ def test_argument_list_qualifiers(self): """ arg_string = "double x1, double* x2, double& x3, double@ x4, " \ "const double x5, const double* x6, const double& x7, const double@ x8" - args = ArgumentList.rule.parseString(arg_string)[0].args_list + args = ArgumentList.rule.parseString(arg_string)[0].list() self.assertEqual(8, len(args)) self.assertFalse(args[1].ctype.is_ptr and args[1].ctype.is_shared_ptr and args[1].ctype.is_ref) @@ -169,7 +169,7 @@ def test_argument_list_templated(self): """Test arguments list where the arguments can be templated.""" arg_string = "std::pair steps, vector vector_of_pointers" args = ArgumentList.rule.parseString(arg_string)[0] - args_list = args.args_list + args_list = args.list() self.assertEqual(2, len(args_list)) self.assertEqual("std::pair", args_list[0].ctype.to_cpp(False)) @@ -180,30 +180,62 @@ def test_argument_list_templated(self): def test_default_arguments(self): """Tests any expression that is a valid default argument""" - args = ArgumentList.rule.parseString( - "string c = \"\", string s=\"hello\", int a=3, " - "int b, double pi = 3.1415, " - "gtsam::KeyFormatter kf = gtsam::DefaultKeyFormatter, " - "std::vector p = std::vector(), " - "std::vector l = (1, 2, 'name', \"random\", 3.1415)" - )[0].args_list + args = ArgumentList.rule.parseString(""" + string c = "", int z = 0, double z2 = 0.0, bool f = false, + string s="hello"+"goodbye", char c='a', int a=3, + int b, double pi = 3.1415""")[0].list() # Test for basic types - self.assertEqual(args[0].default, "") - self.assertEqual(args[1].default, "hello") - self.assertEqual(args[2].default, 3) + self.assertEqual(args[0].default, '""') + self.assertEqual(args[1].default, '0') + self.assertEqual(args[2].default, '0.0') + self.assertEqual(args[3].default, "false") + self.assertEqual(args[4].default, '"hello"+"goodbye"') + self.assertEqual(args[5].default, "'a'") + self.assertEqual(args[6].default, '3') # No default argument should set `default` to None - self.assertIsNone(args[3].default) - - self.assertEqual(args[4].default, 3.1415) + self.assertIsNone(args[7].default) + self.assertEqual(args[8].default, '3.1415') + + arg0 = 'gtsam::DefaultKeyFormatter' + arg1 = 'std::vector()' + arg2 = '{1, 2}' + arg3 = '[&c1, &c2](string s=5, int a){return s+"hello"+a+c1+c2;}' + arg4 = 'gtsam::Pose3()' + arg5 = 'Factor()' + arg6 = 'gtsam::Point3(1, 2, 3)' + arg7 = 'ns::Class(3, 2, 1, "name")' + + argument_list = """ + gtsam::KeyFormatter kf = {arg0}, + std::vector v = {arg1}, + std::vector l = {arg2}, + gtsam::KeyFormatter lambda = {arg3}, + gtsam::Pose3 p = {arg4}, + Factor x = {arg5}, + gtsam::Point3 x = {arg6}, + ns::Class obj = {arg7} + """.format(arg0=arg0, + arg1=arg1, + arg2=arg2, + arg3=arg3, + arg4=arg4, + arg5=arg5, + arg6=arg6, + arg7=arg7) + args = ArgumentList.rule.parseString(argument_list)[0].list() # Test non-basic type - self.assertEqual(repr(args[5].default.typename), - 'gtsam::DefaultKeyFormatter') + self.assertEqual(args[0].default, arg0) # Test templated type - self.assertEqual(repr(args[6].default.typename), 'std::vector') - # Test for allowing list as default argument - self.assertEqual(args[7].default, (1, 2, 'name', "random", 3.1415)) + self.assertEqual(args[1].default, arg1) + self.assertEqual(args[2].default, arg2) + self.assertEqual(args[3].default, arg3) + self.assertEqual(args[4].default, arg4) + self.assertEqual(args[5].default, arg5) + self.assertEqual(args[6].default, arg6) + # Test for default argument with multiple templates and params + self.assertEqual(args[7].default, arg7) def test_return_type(self): """Test ReturnType""" @@ -273,6 +305,15 @@ def test_constructor(self): self.assertEqual("f", ret.name) self.assertEqual(3, len(ret.args)) + ret = Constructor.rule.parseString( + """ForwardKinematics(const gtdynamics::Robot& robot, + const string& start_link_name, const string& end_link_name, + const gtsam::Values& joint_angles, + const gtsam::Pose3& l2Tp = gtsam::Pose3());""")[0] + self.assertEqual("ForwardKinematics", ret.name) + self.assertEqual(5, len(ret.args)) + self.assertEqual("gtsam::Pose3()", ret.args.list()[4].default) + def test_operator_overload(self): """Test for operator overloading.""" # Unary operator @@ -296,7 +337,7 @@ def test_operator_overload(self): ret.return_type.type1.typename.to_cpp()) self.assertTrue(len(ret.args) == 1) self.assertEqual("const gtsam::Vector2 &", - repr(ret.args.args_list[0].ctype)) + repr(ret.args.list()[0].ctype)) self.assertTrue(not ret.is_unary) def test_typedef_template_instantiation(self): @@ -392,6 +433,16 @@ class SymbolicFactorGraph { self.assertEqual(0, len(ret.properties)) self.assertTrue(not ret.is_virtual) + def test_templated_class(self): + """Test a templated class.""" + ret = Class.rule.parseString(""" + template + class MyFactor {}; + """)[0] + + self.assertEqual("MyFactor", ret.name) + self.assertEqual("", repr(ret.template)) + def test_class_inheritance(self): """Test for class inheritance.""" ret = Class.rule.parseString(""" @@ -446,8 +497,7 @@ def test_forward_declaration(self): fwd = ForwardDeclaration.rule.parseString( "virtual class Test:gtsam::Point3;")[0] - fwd_name = fwd.name - self.assertEqual("Test", fwd_name.name) + self.assertEqual("Test", fwd.name) self.assertTrue(fwd.is_virtual) def test_function(self): @@ -469,14 +519,26 @@ def test_global_variable(self): variable = Variable.rule.parseString("string kGravity = 9.81;")[0] self.assertEqual(variable.name, "kGravity") self.assertEqual(variable.ctype.typename.name, "string") - self.assertEqual(variable.default, 9.81) + self.assertEqual(variable.default, "9.81") variable = Variable.rule.parseString( "const string kGravity = 9.81;")[0] self.assertEqual(variable.name, "kGravity") self.assertEqual(variable.ctype.typename.name, "string") self.assertTrue(variable.ctype.is_const) - self.assertEqual(variable.default, 9.81) + self.assertEqual(variable.default, "9.81") + + variable = Variable.rule.parseString( + "gtsam::Pose3 wTc = gtsam::Pose3();")[0] + self.assertEqual(variable.name, "wTc") + self.assertEqual(variable.ctype.typename.name, "Pose3") + self.assertEqual(variable.default, "gtsam::Pose3()") + + variable = Variable.rule.parseString( + "gtsam::Pose3 wTc = gtsam::Pose3(1, 2, 0);")[0] + self.assertEqual(variable.name, "wTc") + self.assertEqual(variable.ctype.typename.name, "Pose3") + self.assertEqual(variable.default, "gtsam::Pose3(1, 2, 0)") def test_enumerator(self): """Test for enumerator.""" diff --git a/tests/test_matlab_wrapper.py b/tests/test_matlab_wrapper.py index 02d40cb457..b321c4e151 100644 --- a/tests/test_matlab_wrapper.py +++ b/tests/test_matlab_wrapper.py @@ -15,8 +15,6 @@ sys.path.append(osp.dirname(osp.dirname(osp.abspath(__file__)))) -import gtwrap.interface_parser as parser -import gtwrap.template_instantiator as instantiator from gtwrap.matlab_wrapper import MatlabWrapper @@ -117,19 +115,14 @@ def test_geometry(self): if not osp.exists(self.MATLAB_ACTUAL_DIR): os.mkdir(self.MATLAB_ACTUAL_DIR) - module = parser.Module.parseString(content) - - instantiator.instantiate_namespace_inplace(module) - # Create MATLAB wrapper instance wrapper = MatlabWrapper( - module=module, module_name='geometry', top_module_namespace=['gtsam'], ignore_classes=[''], ) - cc_content = wrapper.wrap() + cc_content = wrapper.wrap(content) self.generate_content(cc_content) @@ -148,18 +141,13 @@ def test_functions(self): if not osp.exists(self.MATLAB_ACTUAL_DIR): os.mkdir(self.MATLAB_ACTUAL_DIR) - module = parser.Module.parseString(content) - - instantiator.instantiate_namespace_inplace(module) - wrapper = MatlabWrapper( - module=module, module_name='functions', top_module_namespace=['gtsam'], ignore_classes=[''], ) - cc_content = wrapper.wrap() + cc_content = wrapper.wrap(content) self.generate_content(cc_content) @@ -181,18 +169,13 @@ def test_class(self): if not osp.exists(self.MATLAB_ACTUAL_DIR): os.mkdir(self.MATLAB_ACTUAL_DIR) - module = parser.Module.parseString(content) - - instantiator.instantiate_namespace_inplace(module) - wrapper = MatlabWrapper( - module=module, module_name='class', top_module_namespace=['gtsam'], ignore_classes=[''], ) - cc_content = wrapper.wrap() + cc_content = wrapper.wrap(content) self.generate_content(cc_content) @@ -214,18 +197,13 @@ def test_inheritance(self): if not osp.exists(self.MATLAB_ACTUAL_DIR): os.mkdir(self.MATLAB_ACTUAL_DIR) - module = parser.Module.parseString(content) - - instantiator.instantiate_namespace_inplace(module) - wrapper = MatlabWrapper( - module=module, module_name='inheritance', top_module_namespace=['gtsam'], ignore_classes=[''], ) - cc_content = wrapper.wrap() + cc_content = wrapper.wrap(content) self.generate_content(cc_content) @@ -237,7 +215,7 @@ def test_inheritance(self): for file in files: self.compare_and_diff(file) - def test_namspaces(self): + def test_namespaces(self): """ Test interface file with full namespace definition. """ @@ -247,18 +225,13 @@ def test_namspaces(self): if not osp.exists(self.MATLAB_ACTUAL_DIR): os.mkdir(self.MATLAB_ACTUAL_DIR) - module = parser.Module.parseString(content) - - instantiator.instantiate_namespace_inplace(module) - wrapper = MatlabWrapper( - module=module, module_name='namespaces', top_module_namespace=['gtsam'], ignore_classes=[''], ) - cc_content = wrapper.wrap() + cc_content = wrapper.wrap(content) self.generate_content(cc_content) @@ -282,29 +255,25 @@ def test_special_cases(self): if not osp.exists(self.MATLAB_ACTUAL_DIR): os.mkdir(self.MATLAB_ACTUAL_DIR) - module = parser.Module.parseString(content) - - instantiator.instantiate_namespace_inplace(module) - wrapper = MatlabWrapper( - module=module, module_name='special_cases', top_module_namespace=['gtsam'], ignore_classes=[''], ) - cc_content = wrapper.wrap() + cc_content = wrapper.wrap(content) self.generate_content(cc_content) files = [ 'special_cases_wrapper.cpp', '+gtsam/PinholeCameraCal3Bundler.m', - '+gtsam/NonlinearFactorGraph.m', + '+gtsam/NonlinearFactorGraph.m', ] for file in files: self.compare_and_diff(file) + if __name__ == '__main__': unittest.main() diff --git a/tests/test_pybind_wrapper.py b/tests/test_pybind_wrapper.py index fe5e1950e0..77c884b622 100644 --- a/tests/test_pybind_wrapper.py +++ b/tests/test_pybind_wrapper.py @@ -16,8 +16,6 @@ sys.path.append( osp.normpath(osp.abspath(osp.join(__file__, '../../../build/wrap')))) -import gtwrap.interface_parser as parser -import gtwrap.template_instantiator as instantiator from gtwrap.pybind_wrapper import PybindWrapper sys.path.append(osp.dirname(osp.dirname(osp.abspath(__file__)))) @@ -37,23 +35,18 @@ def wrap_content(self, content, module_name, output_dir): """ Common function to wrap content. """ - module = parser.Module.parseString(content) - - instantiator.instantiate_namespace_inplace(module) - with open(osp.join(self.TEST_DIR, "pybind_wrapper.tpl")) as template_file: module_template = template_file.read() # Create Pybind wrapper instance - wrapper = PybindWrapper(module=module, - module_name=module_name, + wrapper = PybindWrapper(module_name=module_name, use_boost=False, top_module_namespaces=[''], ignore_classes=[''], module_template=module_template) - cc_content = wrapper.wrap() + cc_content = wrapper.wrap(content) output = osp.join(self.TEST_DIR, output_dir, module_name + ".cpp") @@ -165,10 +158,10 @@ def test_enum(self): with open(osp.join(self.INTERFACE_DIR, 'enum.i'), 'r') as f: content = f.read() - output = self.wrap_content(content, 'enum_py', - self.PYTHON_ACTUAL_DIR) + output = self.wrap_content(content, 'enum_py', self.PYTHON_ACTUAL_DIR) self.compare_and_diff('enum_pybind.cpp', output) + if __name__ == '__main__': unittest.main() From 545dfd0be732a3a4be7d3c2c9f411ab9a79fec95 Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Thu, 17 Jun 2021 01:36:57 +0000 Subject: [PATCH 31/40] 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 f8f625b179..eae806607b 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 19234bec29..b58d4f9d36 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 32/40] 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 3e8c45ecea..62d7531d9f 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 18c068d87c4cf5a77cdbcddbfe671f9ed603f16d Mon Sep 17 00:00:00 2001 From: acxz <17132214+acxz@users.noreply.github.com> Date: Sat, 31 Oct 2020 10:00:52 -0400 Subject: [PATCH 33/40] replace deprecated tbb functionality Useful tbb migration guide: https://docs.oneapi.com/versions/latest/onetbb/tbb_userguide/Migration_Guide/Task_API.html add mutable keyward to isPostOrderPhase as that is a variable we change in the const operator function --- gtsam/base/treeTraversal-inst.h | 5 +- .../treeTraversal/parallelTraversalTasks.h | 69 +++++++------------ 2 files changed, 28 insertions(+), 46 deletions(-) diff --git a/gtsam/base/treeTraversal-inst.h b/gtsam/base/treeTraversal-inst.h index 7a88f72eb6..30cec3b9a4 100644 --- a/gtsam/base/treeTraversal-inst.h +++ b/gtsam/base/treeTraversal-inst.h @@ -158,9 +158,8 @@ void DepthFirstForestParallel(FOREST& forest, DATA& rootData, // Typedefs typedef typename FOREST::Node Node; - tbb::task::spawn_root_and_wait( - internal::CreateRootTask(forest.roots(), rootData, visitorPre, - visitorPost, problemSizeThreshold)); + internal::CreateRootTask(forest.roots(), rootData, visitorPre, + visitorPost, problemSizeThreshold); #else DepthFirstForest(forest, rootData, visitorPre, visitorPost); #endif diff --git a/gtsam/base/treeTraversal/parallelTraversalTasks.h b/gtsam/base/treeTraversal/parallelTraversalTasks.h index 87d5b0d4c3..5501abe465 100644 --- a/gtsam/base/treeTraversal/parallelTraversalTasks.h +++ b/gtsam/base/treeTraversal/parallelTraversalTasks.h @@ -22,7 +22,7 @@ #include #ifdef GTSAM_USE_TBB -#include // tbb::task, tbb::task_list +#include // tbb::task_group #include // tbb::scalable_allocator namespace gtsam { @@ -34,7 +34,7 @@ namespace gtsam { /* ************************************************************************* */ template - class PreOrderTask : public tbb::task + class PreOrderTask { public: const boost::shared_ptr& treeNode; @@ -42,28 +42,29 @@ namespace gtsam { VISITOR_PRE& visitorPre; VISITOR_POST& visitorPost; int problemSizeThreshold; + tbb::task_group& tg; bool makeNewTasks; - bool isPostOrderPhase; + mutable bool isPostOrderPhase; PreOrderTask(const boost::shared_ptr& treeNode, const boost::shared_ptr& myData, VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost, int problemSizeThreshold, - bool makeNewTasks = true) + tbb::task_group& tg, bool makeNewTasks = true) : treeNode(treeNode), myData(myData), visitorPre(visitorPre), visitorPost(visitorPost), problemSizeThreshold(problemSizeThreshold), + tg(tg), makeNewTasks(makeNewTasks), isPostOrderPhase(false) {} - tbb::task* execute() override + void operator()() const { if(isPostOrderPhase) { // Run the post-order visitor since this task was recycled to run the post-order visitor (void) visitorPost(treeNode, *myData); - return nullptr; } else { @@ -71,14 +72,10 @@ namespace gtsam { { if(!treeNode->children.empty()) { - // Allocate post-order task as a continuation - isPostOrderPhase = true; - recycle_as_continuation(); - bool overThreshold = (treeNode->problemSize() >= problemSizeThreshold); - tbb::task* firstChild = 0; - tbb::task_list childTasks; + // If we have child tasks, start subtasks and wait for them to complete + tbb::task_group ctg; for(const boost::shared_ptr& child: treeNode->children) { // Process child in a subtask. Important: Run visitorPre before calling @@ -86,37 +83,30 @@ namespace gtsam { // allocated an extra child, this causes a TBB error. boost::shared_ptr childData = boost::allocate_shared( tbb::scalable_allocator(), visitorPre(child, *myData)); - tbb::task* childTask = - new (allocate_child()) PreOrderTask(child, childData, visitorPre, visitorPost, - problemSizeThreshold, overThreshold); - if (firstChild) - childTasks.push_back(*childTask); - else - firstChild = childTask; + ctg.run(PreOrderTask(child, childData, visitorPre, visitorPost, + problemSizeThreshold, ctg, overThreshold)); } + ctg.wait(); - // If we have child tasks, start subtasks and wait for them to complete - set_ref_count((int)treeNode->children.size()); - spawn(childTasks); - return firstChild; + // Allocate post-order task as a continuation + isPostOrderPhase = true; + tg.run(*this); } else { // Run the post-order visitor in this task if we have no children (void) visitorPost(treeNode, *myData); - return nullptr; } } else { // Process this node and its children in this task processNodeRecursively(treeNode, *myData); - return nullptr; } } } - void processNodeRecursively(const boost::shared_ptr& node, DATA& myData) + void processNodeRecursively(const boost::shared_ptr& node, DATA& myData) const { for(const boost::shared_ptr& child: node->children) { @@ -131,7 +121,7 @@ namespace gtsam { /* ************************************************************************* */ template - class RootTask : public tbb::task + class RootTask { public: const ROOTS& roots; @@ -139,38 +129,31 @@ namespace gtsam { VISITOR_PRE& visitorPre; VISITOR_POST& visitorPost; int problemSizeThreshold; + tbb::task_group& tg; RootTask(const ROOTS& roots, DATA& myData, VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost, - int problemSizeThreshold) : + int problemSizeThreshold, tbb::task_group& tg) : roots(roots), myData(myData), visitorPre(visitorPre), visitorPost(visitorPost), - problemSizeThreshold(problemSizeThreshold) {} + problemSizeThreshold(problemSizeThreshold), tg(tg) {} - tbb::task* execute() override + void operator()() const { typedef PreOrderTask PreOrderTask; // Create data and tasks for our children - tbb::task_list tasks; for(const boost::shared_ptr& root: roots) { boost::shared_ptr rootData = boost::allocate_shared(tbb::scalable_allocator(), visitorPre(root, myData)); - tasks.push_back(*new(allocate_child()) - PreOrderTask(root, rootData, visitorPre, visitorPost, problemSizeThreshold)); + tg.run(PreOrderTask(root, rootData, visitorPre, visitorPost, problemSizeThreshold, tg)); } - // Set TBB ref count - set_ref_count(1 + (int) roots.size()); - // Spawn tasks - spawn_and_wait_for_all(tasks); - // Return nullptr - return nullptr; } }; template - RootTask& - CreateRootTask(const ROOTS& roots, DATA& rootData, VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost, int problemSizeThreshold) + void CreateRootTask(const ROOTS& roots, DATA& rootData, VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost, int problemSizeThreshold) { typedef RootTask RootTask; - return *new(tbb::task::allocate_root()) RootTask(roots, rootData, visitorPre, visitorPost, problemSizeThreshold); - } + tbb::task_group tg; + tg.run_and_wait(RootTask(roots, rootData, visitorPre, visitorPost, problemSizeThreshold, tg)); + } } From e57fe4ab2ffd9a85e123367aa2ccb6c8979089f8 Mon Sep 17 00:00:00 2001 From: acxz <17132214+acxz@users.noreply.github.com> Date: Sun, 20 Jun 2021 19:03:33 -0400 Subject: [PATCH 34/40] add comment for purpose of variable --- gtsam/base/treeTraversal/parallelTraversalTasks.h | 1 + 1 file changed, 1 insertion(+) diff --git a/gtsam/base/treeTraversal/parallelTraversalTasks.h b/gtsam/base/treeTraversal/parallelTraversalTasks.h index 5501abe465..dc1b459066 100644 --- a/gtsam/base/treeTraversal/parallelTraversalTasks.h +++ b/gtsam/base/treeTraversal/parallelTraversalTasks.h @@ -45,6 +45,7 @@ namespace gtsam { tbb::task_group& tg; bool makeNewTasks; + // Keep track of order phase across multiple calls to the same functor mutable bool isPostOrderPhase; PreOrderTask(const boost::shared_ptr& treeNode, const boost::shared_ptr& myData, From e3b6c8308aada2c470974140fa24be5d8ad6db7b Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Mon, 21 Jun 2021 03:47:10 +0000 Subject: [PATCH 35/40] 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 15c0090363..3a1e905e80 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 62d7531d9f..787efac51e 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 36/40] 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 89ed8f0ae0..d4f7b23659 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 37/40] 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 d4f7b23659..78857262fb 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 38/40] formatting example --- examples/CreateSFMExampleData.cpp | 23 ++++++++++++----------- 1 file changed, 12 insertions(+), 11 deletions(-) diff --git a/examples/CreateSFMExampleData.cpp b/examples/CreateSFMExampleData.cpp index 3a1e905e80..97f4c84dc8 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"; From 3244679dd1f9a86f17c8a76dbdd7cc9d81a5b03a Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 21 Jun 2021 23:25:03 -0400 Subject: [PATCH 39/40] update the pgp servers to get the LLVM GPG key --- .github/workflows/build-linux.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/build-linux.yml b/.github/workflows/build-linux.yml index 9ca3a8fe52..5483c32cfd 100644 --- a/.github/workflows/build-linux.yml +++ b/.github/workflows/build-linux.yml @@ -57,7 +57,7 @@ jobs: # 15CF4D18AF4F7421 is the GPG key for the LLVM apt repository # This key is not in the keystore by default for Ubuntu so we need to add it. LLVM_KEY=15CF4D18AF4F7421 - gpg --keyserver ipv4.pool.sks-keyservers.net --recv-key $LLVM_KEY || gpg --keyserver ha.pool.sks-keyservers.net --recv-key $LLVM_KEY + gpg --keyserver keyserver.ubuntu.com --recv-key $LLVM_KEY || gpg --keyserver hkp://keyserver.ubuntu.com:80 --recv-key $LLVM_KEY gpg -a --export $LLVM_KEY | sudo apt-key add - sudo add-apt-repository "deb http://apt.llvm.org/bionic/ llvm-toolchain-bionic-9 main" fi From 04621b02756b15315cf5bb25a6d45af242f3a940 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 22 Jun 2021 07:50:54 -0400 Subject: [PATCH 40/40] update key server in other workflow files --- .github/workflows/build-python.yml | 2 +- .github/workflows/build-special.yml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/build-python.yml b/.github/workflows/build-python.yml index df11c5e953..1f87b5119a 100644 --- a/.github/workflows/build-python.yml +++ b/.github/workflows/build-python.yml @@ -77,7 +77,7 @@ jobs: # 15CF4D18AF4F7421 is the GPG key for the LLVM apt repository # This key is not in the keystore by default for Ubuntu so we need to add it. LLVM_KEY=15CF4D18AF4F7421 - gpg --keyserver ipv4.pool.sks-keyservers.net --recv-key $LLVM_KEY || gpg --keyserver ha.pool.sks-keyservers.net --recv-key $LLVM_KEY + gpg --keyserver keyserver.ubuntu.com --recv-key $LLVM_KEY || gpg --keyserver hkp://keyserver.ubuntu.com:80 --recv-key $LLVM_KEY gpg -a --export $LLVM_KEY | sudo apt-key add - sudo add-apt-repository "deb http://apt.llvm.org/bionic/ llvm-toolchain-bionic-9 main" fi diff --git a/.github/workflows/build-special.yml b/.github/workflows/build-special.yml index 3ac88bdc80..6427e13bc8 100644 --- a/.github/workflows/build-special.yml +++ b/.github/workflows/build-special.yml @@ -64,7 +64,7 @@ jobs: run: | # LLVM 9 is not in Bionic's repositories so we add the official LLVM repository. if [ "${{ matrix.compiler }}" = "clang" ] && [ "${{ matrix.version }}" = "9" ]; then - gpg --keyserver pool.sks-keyservers.net --recv-key 15CF4D18AF4F7421 + gpg --keyserver keyserver.ubuntu.com --recv-key $LLVM_KEY || gpg --keyserver hkp://keyserver.ubuntu.com:80 --recv-key $LLVM_KEY gpg -a --export 15CF4D18AF4F7421 | sudo apt-key add - sudo add-apt-repository "deb http://apt.llvm.org/bionic/ llvm-toolchain-bionic-9 main" fi