12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

fixing tests by moving to Cal3_S2

release/4.3a0
Ayush Baid 2021-04-24 10:57:28 -04:00
parent 8ca7f1ff1d
commit f60e9e9365
2 changed files with 113 additions and 96 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

44
gtsam/slam/EssentialMatrixWithCalibrationFactor.h
View File

@ -12,7 +12,8 @@
/** /**
* @file EssentialMatrixWithCalibrationFactor.h * @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 Ayush Baid
* @author Akshay Krishnan * @author Akshay Krishnan
@ -21,38 +22,40 @@
#pragma once #pragma once
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/geometry/EssentialMatrix.h> #include <gtsam/geometry/EssentialMatrix.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <iostream> #include <iostream>
namespace gtsam { namespace gtsam {
/** /**
* Factor that evaluates algebraic epipolar error (K^-1 p)'E (K^-1 p) for given essential matrix and calibration shared * Factor that evaluates algebraic epipolar error (K^-1 p)'E (K^-1 p) for given
* between two images. * essential matrix and calibration shared between two images.
*/ */
template <class CALIBRATION> template <class CALIBRATION>
class EssentialMatrixWithCalibrationFactor: public NoiseModelFactor2<EssentialMatrix, CALIBRATION > { class EssentialMatrixWithCalibrationFactor
: public NoiseModelFactor2<EssentialMatrix, CALIBRATION> {
Point2 pA_, pB_; ///< points in pixel coordinates Point2 pA_, pB_; ///< points in pixel coordinates
typedef NoiseModelFactor2<EssentialMatrix, CALIBRATION> Base; typedef NoiseModelFactor2<EssentialMatrix, CALIBRATION> Base;
typedef EssentialMatrixWithCalibrationFactor This; typedef EssentialMatrixWithCalibrationFactor This;
public: public:
/** /**
* Constructor * Constructor
* @param essentialMatrixKey Essential Matrix variable key * @param essentialMatrixKey Essential Matrix variable key
* @param calibrationKey Calibration variable key * @param calibrationKey Calibration variable key
* @param pA point in first camera, in pixel coordinates * @param pA point in first camera, in pixel coordinates
* @param pB point in second 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, EssentialMatrixWithCalibrationFactor(Key essentialMatrixKey,
const SharedNoiseModel& model) : Key calibrationKey, const Point2& pA,
Base(model, essentialMatrixKey, calibrationKey), pA_(pA), pB_(pB) {} const Point2& pB,
const SharedNoiseModel& model)
: Base(model, essentialMatrixKey, calibrationKey), pA_(pA), pB_(pB) {}
/// @return a deep copy of this factor /// @return a deep copy of this factor
gtsam::NonlinearFactor::shared_ptr clone() const override { gtsam::NonlinearFactor::shared_ptr clone() const override {
@ -61,7 +64,8 @@ public:
} }
/// print /// print
void print(const std::string& s = "", void print(
const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
Base::print(s); Base::print(s);
std::cout << " EssentialMatrixWithCalibrationFactor with measurements\n (" std::cout << " EssentialMatrixWithCalibrationFactor with measurements\n ("
@ -79,8 +83,10 @@ public:
* @param H2 optional jacobian in K * @param H2 optional jacobian in K
* @return * Vector * @return * Vector
*/ */
Vector evaluateError(const EssentialMatrix& E, const CALIBRATION& K, Vector evaluateError(
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) const override { const EssentialMatrix& E, const CALIBRATION& K,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const override {
Vector error(1); Vector error(1);
// converting from pixel coordinates to normalized coordinates cA and cB // converting from pixel coordinates to normalized coordinates cA and cB
Matrix cA_H_K; // dcA/dK Matrix cA_H_K; // dcA/dK
@ -96,7 +102,8 @@ public:
// compute the jacobian of error w.r.t K // compute the jacobian of error w.r.t K
// dvX / dcX [3x2] = [1, 0], [0, 1], [0, 0] // dvX / dcX [3x2] = [1, 0], [0, 1], [0, 0]
Matrix v_H_c = (Matrix(3, 2) << 1.0, 0.0, 0.0, 1.0, 0.0, 0.0).finished(); // [3x2] Matrix v_H_c =
(Matrix(3, 2) << 1.0, 0.0, 0.0, 1.0, 0.0, 0.0).finished(); // [3x2]
// computing dvA/dK = dvA/dcA * dcA/dK and dVB/dK = dvB/dcB * dcB/dK // 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 vA_H_K = v_H_c * cA_H_K;
@ -104,7 +111,8 @@ public:
// error function f = vB.T * E * vA // error function f = vB.T * E * vA
// H2 = df/dK = vB.T * E.T * dvA/dK + vA.T * E * dvB/dK // H2 = df/dK = vB.T * E.T * dvA/dK + vA.T * E * dvB/dK
*H2 = vB.transpose() * E.matrix().transpose() * vA_H_K + vA.transpose() * E.matrix() * vB_H_K; *H2 = vB.transpose() * E.matrix().transpose() * vA_H_K +
vA.transpose() * E.matrix() * vB_H_K;
} }
error << E.error(vA, vB, H1); error << E.error(vA, vB, H1);
@ -116,4 +124,4 @@ public:
GTSAM_MAKE_ALIGNED_OPERATOR_NEW GTSAM_MAKE_ALIGNED_OPERATOR_NEW
}; };
}// gtsam } // namespace gtsam

133
gtsam/slam/tests/testEssentialMatrixWithCalibrationFactor.cpp
View File

@ -6,27 +6,25 @@
* @date April 22, 2021 * @date April 22, 2021
*/ */
#include <gtsam/slam/EssentialMatrixWithCalibrationFactor.h> #include <CppUnitLite/TestHarness.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/nonlinear/expressionTesting.h>
#include <gtsam/nonlinear/ExpressionFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/geometry/CalibratedCamera.h>
#include <gtsam/geometry/Cal3Bundler.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/base/Testable.h> #include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h> #include <gtsam/base/numericalDerivative.h>
#include <gtsam/geometry/Cal3Bundler.h>
#include <CppUnitLite/TestHarness.h> #include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/CalibratedCamera.h>
#include <gtsam/nonlinear/ExpressionFactor.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/expressionTesting.h>
#include <gtsam/slam/EssentialMatrixWithCalibrationFactor.h>
#include <gtsam/slam/dataset.h>
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
// Noise model for first type of factor is evaluating algebraic error // Noise model for first type of factor is evaluating algebraic error
noiseModel::Isotropic::shared_ptr model1 = noiseModel::Isotropic::Sigma(1, noiseModel::Isotropic::shared_ptr model1 =
0.01); noiseModel::Isotropic::Sigma(1, 0.01);
// Noise model for second type of factor is evaluating pixel coordinates // Noise model for second type of factor is evaluating pixel coordinates
noiseModel::Unit::shared_ptr model2 = noiseModel::Unit::Create(2); noiseModel::Unit::shared_ptr model2 = noiseModel::Unit::Create(2);
@ -41,24 +39,21 @@ SfmData data;
bool readOK = readBAL(filename, data); bool readOK = readBAL(filename, data);
Rot3 c1Rc2 = data.cameras[1].pose().rotation(); Rot3 c1Rc2 = data.cameras[1].pose().rotation();
Point3 c1Tc2 = data.cameras[1].pose().translation(); Point3 c1Tc2 = data.cameras[1].pose().translation();
Cal3Bundler trueK = data.cameras[1].calibration(); // TODO: maybe default value not good; assert with 0th // TODO: maybe default value not good; assert with 0th
// PinholeCamera<Cal3Bundler> camera2(data.cameras[1].pose(), trueK); Cal3_S2 trueK = Cal3_S2();
// PinholeCamera<Cal3_S2> camera2(data.cameras[1].pose(), trueK);
Rot3 trueRotation(c1Rc2); Rot3 trueRotation(c1Rc2);
Unit3 trueDirection(c1Tc2); Unit3 trueDirection(c1Tc2);
EssentialMatrix trueE(trueRotation, trueDirection); 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) { Point2 pA(size_t i) { return data.tracks[i].measurements[0].second; }
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) {
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))); 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))); return EssentialMatrix::Homogeneous(K.calibrate(pB(i)));
} }
@ -69,8 +64,8 @@ TEST (EssentialMatrixWithCalibrationFactor, testData) {
// Check E matrix // Check E matrix
Matrix expected(3, 3); Matrix expected(3, 3);
expected << 0, 0, 0, 0, 0, -0.1, 0.1, 0, 0; expected << 0, 0, 0, 0, 0, -0.1, 0.1, 0, 0;
Matrix aEb_matrix = skewSymmetric(c1Tc2.x(), c1Tc2.y(), c1Tc2.z()) Matrix aEb_matrix =
* c1Rc2.matrix(); skewSymmetric(c1Tc2.x(), c1Tc2.y(), c1Tc2.z()) * c1Rc2.matrix();
EXPECT(assert_equal(expected, aEb_matrix, 1e-8)); EXPECT(assert_equal(expected, aEb_matrix, 1e-8));
// Check some projections // Check some projections
@ -83,13 +78,13 @@ TEST (EssentialMatrixWithCalibrationFactor, testData) {
EXPECT(assert_equal(Vector3(-1, 0.2, 1), vB(4, trueK), 1e-8)); EXPECT(assert_equal(Vector3(-1, 0.2, 1), vB(4, trueK), 1e-8));
// check the calibration // check the calibration
Cal3Bundler expectedK; Cal3_S2 expectedK(1, 1, 0, 0, 0);
EXPECT(assert_equal(expectedK, trueK)); EXPECT(assert_equal(expectedK, trueK));
// Check epipolar constraint // Check epipolar constraint
for (size_t i = 0; i < 5; i++) 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 // Check epipolar constraint
for (size_t i = 0; i < 5; i++) for (size_t i = 0; i < 5; i++)
@ -101,7 +96,8 @@ TEST (EssentialMatrixWithCalibrationFactor, factor) {
Key keyE(1); Key keyE(1);
Key keyK(1); Key keyK(1);
for (size_t i = 0; i < 5; i++) { for (size_t i = 0; i < 5; i++) {
EssentialMatrixWithCalibrationFactor<Cal3Bundler> factor(keyE, keyK, pA(i), pB(i), model1); EssentialMatrixWithCalibrationFactor<Cal3_S2> factor(keyE, keyK, pA(i),
pB(i), model1);
// Check evaluation // Check evaluation
Vector expected(1); Vector expected(1);
@ -116,14 +112,18 @@ TEST (EssentialMatrixWithCalibrationFactor, factor) {
Matrix HKexpected; Matrix HKexpected;
typedef Eigen::Matrix<double, 1, 1> Vector1; typedef Eigen::Matrix<double, 1, 1> Vector1;
// TODO: fix this // TODO: fix this
boost::function<Vector(const EssentialMatrix&, const Cal3Bundler&)> f = boost::bind( boost::function<Vector(const EssentialMatrix &, const Cal3_S2 &)> f =
&EssentialMatrixWithCalibrationFactor<Cal3Bundler>::evaluateError, factor, _1, _2, boost::none, boost::none); boost::bind(
HEexpected = numericalDerivative21<Vector1, EssentialMatrix, Cal3Bundler>(f, trueE, trueK); &EssentialMatrixWithCalibrationFactor<Cal3_S2>::evaluateError,
HKexpected = numericalDerivative22<Vector1, EssentialMatrix, Cal3Bundler>(f, trueE, trueK); factor, _1, _2, boost::none, boost::none);
HEexpected = numericalDerivative21<Vector1, EssentialMatrix, Cal3_S2>(
f, trueE, trueK);
HKexpected = numericalDerivative22<Vector1, EssentialMatrix, Cal3_S2>(
f, trueE, trueK);
// Verify the Jacobian is correct // Verify the Jacobian is correct
EXPECT(assert_equal(HEexpected, HEactual, 1e-8)); 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 keyE(1);
// Key keyK(2); // Key keyK(2);
// for (size_t i = 0; i < 5; i++) { // for (size_t i = 0; i < 5; i++) {
// boost::function<double(const EssentialMatrix&, const Cal3Bundler&, // boost::function<double(const EssentialMatrix&, const Cal3_S2&,
// OptionalJacobian<1, 5>, OptionalJacobian<1, 3>)> f = // OptionalJacobian<1, 5>, OptionalJacobian<1, 3>)> f =
// boost::bind(&EssentialMatrix::error, _1, pA(i), pB(i), _2); // boost::bind(&EssentialMatrix::error, _1, pA(i), pB(i), _2);
// Expression<EssentialMatrix> E_(keyE); // leaf expression // Expression<EssentialMatrix> E_(keyE); // leaf expression
// Expression<Cal3Bundler> K_(keyK); // leaf expression // Expression<Cal3_S2> K_(keyK); // leaf expression
// Expression<double> expr(f, E_, K_); // unary expression // Expression<double> expr(f, E_, K_); // unary expression
// // Test the derivatives using Paul's magic // // Test the derivatives using Paul's magic
@ -162,13 +162,16 @@ TEST (EssentialMatrixWithCalibrationFactor, factor) {
// Key keyE(1); // Key keyE(1);
// Key keyK(1); // Key keyK(1);
// for (size_t i = 0; i < 5; i++) { // for (size_t i = 0; i < 5; i++) {
// boost::function<double(const EssentialMatrix&, OptionalJacobian<1, 5>)> f = // boost::function<double(const EssentialMatrix&, OptionalJacobian<1, 5>)> f
// =
// boost::bind(&EssentialMatrix::error, _1, vA(i), vB(i), _2); // boost::bind(&EssentialMatrix::error, _1, vA(i), vB(i), _2);
// boost::function<EssentialMatrix(const Rot3&, const Unit3&, OptionalJacobian<5, 3>, // boost::function<EssentialMatrix(const Rot3&, const Unit3&,
// OptionalJacobian<5, 3>,
// OptionalJacobian<5, 2>)> g; // OptionalJacobian<5, 2>)> g;
// Expression<Rot3> R_(key); // Expression<Rot3> R_(key);
// Expression<Unit3> d_(trueDirection); // Expression<Unit3> d_(trueDirection);
// Expression<EssentialMatrix> E_(&EssentialMatrix::FromRotationAndDirection, R_, d_); // Expression<EssentialMatrix>
// E_(&EssentialMatrix::FromRotationAndDirection, R_, d_);
// Expression<double> expr(f, E_); // Expression<double> expr(f, E_);
// // Test the derivatives using Paul's magic // // Test the derivatives using Paul's magic
@ -219,7 +222,8 @@ TEST (EssentialMatrixWithCalibrationFactor, minimization) {
// Noise sigma is 1cm, assuming metric measurements // Noise sigma is 1cm, assuming metric measurements
NonlinearFactorGraph graph; NonlinearFactorGraph graph;
for (size_t i = 0; i < 5; i++) for (size_t i = 0; i < 5; i++)
graph.emplace_shared<EssentialMatrixWithCalibrationFactor<Cal3Bundler>>(1, 2, pA(i), pB(i), model1); graph.emplace_shared<EssentialMatrixWithCalibrationFactor<Cal3_S2>>(
1, 2, pA(i), pB(i), model1);
// Check error at ground truth // Check error at ground truth
Values truth; Values truth;
@ -229,16 +233,17 @@ TEST (EssentialMatrixWithCalibrationFactor, minimization) {
// Check error at initial estimate // Check error at initial estimate
Values initial; Values initial;
EssentialMatrix initialE = trueE.retract( EssentialMatrix initialE =
(Vector(5) << 0.1, -0.1, 0.1, 0.1, -0.1).finished()); trueE.retract((Vector(5) << 0.1, -0.1, 0.1, 0.1, -0.1).finished());
Cal3Bundler initialK = trueK.retract( Cal3_S2 initialK =
(Vector(3) << 0.1, -1e-1, 3e-2).finished()); trueK.retract((Vector(5) << 0.1, -0.1, 0.03, -0.2, 0.2).finished());
initial.insert(1, initialE); initial.insert(1, initialE);
initial.insert(2, initialK); initial.insert(2, trueK);
#if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) #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 #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 #endif
// Optimize // Optimize
@ -248,17 +253,17 @@ TEST (EssentialMatrixWithCalibrationFactor, minimization) {
// Check result // Check result
EssentialMatrix actualE = result.at<EssentialMatrix>(1); EssentialMatrix actualE = result.at<EssentialMatrix>(1);
Cal3Bundler actualK = result.at<Cal3Bundler>(2); Cal3_S2 actualK = result.at<Cal3_S2>(2);
EXPECT(assert_equal(trueE, actualE, 1e-1)); EXPECT(assert_equal(trueE, actualE, 1e-2));
EXPECT(assert_equal(trueK, actualK, 1e-1)); EXPECT(assert_equal(trueK, actualK, 1e-2));
// Check error at result // Check error at result
EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4); EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4);
// Check errors individually // Check errors individually
for (size_t i = 0; i < 5; i++) 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; // return data.tracks[i].measurements[1].second;
// } // }
// boost::shared_ptr<Cal3Bundler> // // boost::shared_ptr<Cal3_S2> //
// K = boost::make_shared<Cal3Bundler>(500, 0, 0); // K = boost::make_shared<Cal3_S2>(500, 0, 0);
// PinholeCamera<Cal3Bundler> camera2(data.cameras[1].pose(), *K); // PinholeCamera<Cal3_S2> camera2(data.cameras[1].pose(), *K);
// Vector vA(size_t i) { // Vector vA(size_t i) {
// Point2 xy = K->calibrate(pA(i)); // Point2 xy = K->calibrate(pA(i));
@ -302,7 +307,8 @@ TEST (EssentialMatrixWithCalibrationFactor, minimization) {
// NonlinearFactorGraph graph; // NonlinearFactorGraph graph;
// for (size_t i = 0; i < 5; i++) // for (size_t i = 0; i < 5; i++)
// graph.emplace_shared<EssentialMatrixWithCalibrationFactor>(1, pA(i), pB(i), model1, K); // graph.emplace_shared<EssentialMatrixWithCalibrationFactor>(1, pA(i),
// pB(i), model1, K);
// // Check error at ground truth // // Check error at ground truth
// Values truth; // Values truth;
@ -359,8 +365,9 @@ TEST (EssentialMatrixWithCalibrationFactor, minimization) {
// boost::function<Vector(const EssentialMatrix&, double)> f = boost::bind( // boost::function<Vector(const EssentialMatrix&, double)> f = boost::bind(
// &EssentialMatrixFactor2::evaluateError, &factor, _1, _2, boost::none, // &EssentialMatrixFactor2::evaluateError, &factor, _1, _2, boost::none,
// boost::none); // boost::none);
// Hexpected1 = numericalDerivative21<Vector2, EssentialMatrix, double>(f, trueE, d); // Hexpected1 = numericalDerivative21<Vector2, EssentialMatrix, double>(f,
// Hexpected2 = numericalDerivative22<Vector2, EssentialMatrix, double>(f, trueE, d); // trueE, d); Hexpected2 = numericalDerivative22<Vector2, EssentialMatrix,
// double>(f, trueE, d);
// // Verify the Jacobian is correct // // Verify the Jacobian is correct
// EXPECT(assert_equal(Hexpected1, Hactual1, 1e-6)); // EXPECT(assert_equal(Hexpected1, Hactual1, 1e-6));
@ -376,7 +383,8 @@ TEST (EssentialMatrixWithCalibrationFactor, minimization) {
// // Noise sigma is 1, assuming pixel measurements // // Noise sigma is 1, assuming pixel measurements
// NonlinearFactorGraph graph; // NonlinearFactorGraph graph;
// for (size_t i = 0; i < data.number_tracks(); i++) // for (size_t i = 0; i < data.number_tracks(); i++)
// graph.emplace_shared<EssentialMatrixFactor2>(100, i, pA(i), pB(i), model2, K); // graph.emplace_shared<EssentialMatrixFactor2>(100, i, pA(i), pB(i),
// model2, K);
// // Check error at ground truth // // Check error at ground truth
// Values truth; // Values truth;
@ -427,8 +435,9 @@ TEST (EssentialMatrixWithCalibrationFactor, minimization) {
// boost::function<Vector(const EssentialMatrix&, double)> f = boost::bind( // boost::function<Vector(const EssentialMatrix&, double)> f = boost::bind(
// &EssentialMatrixFactor3::evaluateError, &factor, _1, _2, boost::none, // &EssentialMatrixFactor3::evaluateError, &factor, _1, _2, boost::none,
// boost::none); // boost::none);
// Hexpected1 = numericalDerivative21<Vector2, EssentialMatrix, double>(f, bodyE, d); // Hexpected1 = numericalDerivative21<Vector2, EssentialMatrix, double>(f,
// Hexpected2 = numericalDerivative22<Vector2, EssentialMatrix, double>(f, bodyE, d); // bodyE, d); Hexpected2 = numericalDerivative22<Vector2, EssentialMatrix,
// double>(f, bodyE, d);
// // Verify the Jacobian is correct // // Verify the Jacobian is correct
// EXPECT(assert_equal(Hexpected1, Hactual1, 1e-6)); // EXPECT(assert_equal(Hexpected1, Hactual1, 1e-6));