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

using fixed size matrix, and adding jacobian in homogeneous conversion

release/4.3a0
Ayush Baid 2021-04-26 20:50:22 -04:00
parent f60e9e9365
commit 64ff24b656
6 changed files with 358 additions and 666 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
gtsam/geometry/EssentialMatrix.h
View File

@ -7,10 +7,10 @@
#pragma once
#include <gtsam/base/Manifold.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Unit3.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/base/Manifold.h>
#include <iosfwd>
#include <string>
@ -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);
}

12
gtsam/geometry/tests/testEssentialMatrix.cpp
View File

@ -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;

243
gtsam/slam/EssentialMatrixFactor.h
View File

@ -7,9 +7,10 @@
#pragma once
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/geometry/EssentialMatrix.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <iostream>
namespace gtsam {
@ -17,25 +18,24 @@ namespace gtsam {
/**
* Factor that evaluates epipolar error p'Ep for given essential matrix
*/
class EssentialMatrixFactor: public NoiseModelFactor1<EssentialMatrix> {
Vector3 vA_, vB_; ///< Homogeneous versions, in ideal coordinates
class EssentialMatrixFactor : public NoiseModelFactor1<EssentialMatrix> {
Vector3 vA_, vB_; ///< Homogeneous versions, in ideal coordinates
typedef NoiseModelFactor1<EssentialMatrix> Base;
typedef EssentialMatrixFactor This;
public:
public:
/**
* Constructor
* @param key Essential Matrix variable key
* @param pA point in first camera, in calibrated coordinates
* @param pB point in second camera, in calibrated coordinates
* @param model noise model is about dot product in ideal, homogeneous coordinates
* @param model noise model is about dot product in ideal, homogeneous
* coordinates
*/
EssentialMatrixFactor(Key key, const Point2& pA, const Point2& pB,
const SharedNoiseModel& model) :
Base(model, key) {
const SharedNoiseModel& model)
: Base(model, key) {
vA_ = EssentialMatrix::Homogeneous(pA);
vB_ = EssentialMatrix::Homogeneous(pB);
}
@ -45,13 +45,15 @@ public:
* @param key Essential Matrix variable key
* @param pA point in first camera, in pixel coordinates
* @param pB point in second camera, in pixel coordinates
* @param model noise model is about dot product in ideal, homogeneous coordinates
* @param model noise model is about dot product in ideal, homogeneous
* coordinates
* @param K calibration object, will be used only in constructor
*/
template<class CALIBRATION>
template <class CALIBRATION>
EssentialMatrixFactor(Key key, const Point2& pA, const Point2& pB,
const SharedNoiseModel& model, boost::shared_ptr<CALIBRATION> K) :
Base(model, key) {
const SharedNoiseModel& model,
boost::shared_ptr<CALIBRATION> K)
: Base(model, key) {
assert(K);
vA_ = EssentialMatrix::Homogeneous(K->calibrate(pA));
vB_ = EssentialMatrix::Homogeneous(K->calibrate(pB));
@ -64,23 +66,25 @@ public:
}
/// print
void print(const std::string& s = "",
void print(
const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
Base::print(s);
std::cout << " EssentialMatrixFactor with measurements\n ("
<< vA_.transpose() << ")' and (" << vB_.transpose() << ")'"
<< std::endl;
<< vA_.transpose() << ")' and (" << vB_.transpose() << ")'"
<< std::endl;
}
/// vector of errors returns 1D vector
Vector evaluateError(const EssentialMatrix& E, boost::optional<Matrix&> H =
boost::none) const override {
Vector evaluateError(
const EssentialMatrix& E,
boost::optional<Matrix&> H = boost::none) const override {
Vector error(1);
error << E.error(vA_, vB_, H);
return error;
}
public:
public:
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
};
@ -88,17 +92,16 @@ public:
* Binary factor that optimizes for E and inverse depth d: assumes measurement
* in image 2 is perfect, and returns re-projection error in image 1
*/
class EssentialMatrixFactor2: public NoiseModelFactor2<EssentialMatrix, double> {
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<EssentialMatrix, double> {
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<EssentialMatrix, double> Base;
typedef EssentialMatrixFactor2 This;
public:
public:
/**
* Constructor
* @param key1 Essential Matrix variable key
@ -108,8 +111,10 @@ public:
* @param model noise model should be in pixels, as well
*/
EssentialMatrixFactor2(Key key1, Key key2, const Point2& pA, const Point2& pB,
const SharedNoiseModel& model) :
Base(model, key1, key2), dP1_(EssentialMatrix::Homogeneous(pA)), pn_(pB) {
const SharedNoiseModel& model)
: Base(model, key1, key2),
dP1_(EssentialMatrix::Homogeneous(pA)),
pn_(pB) {
f_ = 1.0;
}
@ -122,11 +127,13 @@ public:
* @param K calibration object, will be used only in constructor
* @param model noise model should be in pixels, as well
*/
template<class CALIBRATION>
template <class CALIBRATION>
EssentialMatrixFactor2(Key key1, Key key2, const Point2& pA, const Point2& pB,
const SharedNoiseModel& model, boost::shared_ptr<CALIBRATION> K) :
Base(model, key1, key2), dP1_(
EssentialMatrix::Homogeneous(K->calibrate(pA))), pn_(K->calibrate(pB)) {
const SharedNoiseModel& model,
boost::shared_ptr<CALIBRATION> K)
: Base(model, key1, key2),
dP1_(EssentialMatrix::Homogeneous(K->calibrate(pA))),
pn_(K->calibrate(pB)) {
f_ = 0.5 * (K->fx() + K->fy());
}
@ -137,12 +144,13 @@ public:
}
/// print
void print(const std::string& s = "",
void print(
const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
Base::print(s);
std::cout << " EssentialMatrixFactor2 with measurements\n ("
<< dP1_.transpose() << ")' and (" << pn_.transpose()
<< ")'" << std::endl;
<< dP1_.transpose() << ")' and (" << pn_.transpose() << ")'"
<< std::endl;
}
/*
@ -150,30 +158,28 @@ public:
* @param E essential matrix
* @param d inverse depth d
*/
Vector evaluateError(const EssentialMatrix& E, const double& d,
boost::optional<Matrix&> DE = boost::none, boost::optional<Matrix&> Dd =
boost::none) const override {
Vector evaluateError(
const EssentialMatrix& E, const double& d,
boost::optional<Matrix&> DE = boost::none,
boost::optional<Matrix&> Dd = boost::none) const override {
// We have point x,y in image 1
// Given a depth Z, the corresponding 3D point P1 = Z*(x,y,1) = (x,y,1)/d
// We then convert to second camera by P2 = 1R2'*(P1-1T2)
// The homogeneous coordinates of can be written as
// 2R1*(P1-1T2) == 2R1*d*(P1-1T2) == 2R1*((x,y,1)-d*1T2)
// where we multiplied with d which yields equivalent homogeneous coordinates.
// Note that this is just the homography 2R1 for d==0
// The point d*P1 = (x,y,1) is computed in constructor as dP1_
// where we multiplied with d which yields equivalent homogeneous
// coordinates. Note that this is just the homography 2R1 for d==0 The point
// d*P1 = (x,y,1) is computed in constructor as dP1_
// Project to normalized image coordinates, then uncalibrate
Point2 pn(0,0);
Point2 pn(0, 0);
if (!DE && !Dd) {
Point3 _1T2 = E.direction().point3();
Point3 d1T2 = d * _1T2;
Point3 dP2 = E.rotation().unrotate(dP1_ - d1T2); // 2R1*((x,y,1)-d*1T2)
Point3 dP2 = E.rotation().unrotate(dP1_ - d1T2); // 2R1*((x,y,1)-d*1T2)
pn = PinholeBase::Project(dP2);
} else {
// Calculate derivatives. TODO if slow: optimize with Mathematica
// 3*2 3*3 3*3
Matrix D_1T2_dir, DdP2_rot, DP2_point;
@ -187,20 +193,19 @@ public:
if (DE) {
Matrix DdP2_E(3, 5);
DdP2_E << DdP2_rot, -DP2_point * d * D_1T2_dir; // (3*3), (3*3) * (3*2)
*DE = f_ * Dpn_dP2 * DdP2_E; // (2*3) * (3*5)
DdP2_E << DdP2_rot, -DP2_point * d * D_1T2_dir; // (3*3), (3*3) * (3*2)
*DE = f_ * Dpn_dP2 * DdP2_E; // (2*3) * (3*5)
}
if (Dd) // efficient backwards computation:
if (Dd) // efficient backwards computation:
// (2*3) * (3*3) * (3*1)
*Dd = -f_ * (Dpn_dP2 * (DP2_point * _1T2));
}
Point2 reprojectionError = pn - pn_;
return f_ * reprojectionError;
}
public:
public:
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
};
// EssentialMatrixFactor2
@ -210,15 +215,13 @@ public:
* in image 2 is perfect, and returns re-projection error in image 1
* This version takes an extrinsic rotation to allow for omni-directional rigs
*/
class EssentialMatrixFactor3: public EssentialMatrixFactor2 {
class EssentialMatrixFactor3 : public EssentialMatrixFactor2 {
typedef EssentialMatrixFactor2 Base;
typedef EssentialMatrixFactor3 This;
Rot3 cRb_; ///< Rotation from body to camera frame
public:
Rot3 cRb_; ///< Rotation from body to camera frame
public:
/**
* Constructor
* @param key1 Essential Matrix variable key
@ -229,9 +232,8 @@ public:
* @param model noise model should be in calibrated coordinates, as well
*/
EssentialMatrixFactor3(Key key1, Key key2, const Point2& pA, const Point2& pB,
const Rot3& cRb, const SharedNoiseModel& model) :
EssentialMatrixFactor2(key1, key2, pA, pB, model), cRb_(cRb) {
}
const Rot3& cRb, const SharedNoiseModel& model)
: EssentialMatrixFactor2(key1, key2, pA, pB, model), cRb_(cRb) {}
/**
* Constructor
@ -242,12 +244,11 @@ public:
* @param K calibration object, will be used only in constructor
* @param model noise model should be in pixels, as well
*/
template<class CALIBRATION>
template <class CALIBRATION>
EssentialMatrixFactor3(Key key1, Key key2, const Point2& pA, const Point2& pB,
const Rot3& cRb, const SharedNoiseModel& model,
boost::shared_ptr<CALIBRATION> K) :
EssentialMatrixFactor2(key1, key2, pA, pB, model, K), cRb_(cRb) {
}
const Rot3& cRb, const SharedNoiseModel& model,
boost::shared_ptr<CALIBRATION> K)
: EssentialMatrixFactor2(key1, key2, pA, pB, model, K), cRb_(cRb) {}
/// @return a deep copy of this factor
gtsam::NonlinearFactor::shared_ptr clone() const override {
@ -256,7 +257,8 @@ public:
}
/// print
void print(const std::string& s = "",
void print(
const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
Base::print(s);
std::cout << " EssentialMatrixFactor3 with rotation " << cRb_ << std::endl;
@ -267,9 +269,10 @@ public:
* @param E essential matrix
* @param d inverse depth d
*/
Vector evaluateError(const EssentialMatrix& E, const double& d,
boost::optional<Matrix&> DE = boost::none, boost::optional<Matrix&> Dd =
boost::none) const override {
Vector evaluateError(
const EssentialMatrix& E, const double& d,
boost::optional<Matrix&> DE = boost::none,
boost::optional<Matrix&> Dd = boost::none) const override {
if (!DE) {
// Convert E from body to camera frame
EssentialMatrix cameraE = cRb_ * E;
@ -277,18 +280,114 @@ public:
return Base::evaluateError(cameraE, d, boost::none, Dd);
} else {
// Version with derivatives
Matrix D_e_cameraE, D_cameraE_E; // 2*5, 5*5
Matrix D_e_cameraE, D_cameraE_E; // 2*5, 5*5
EssentialMatrix cameraE = E.rotate(cRb_, D_cameraE_E);
Vector e = Base::evaluateError(cameraE, d, D_e_cameraE, Dd);
*DE = D_e_cameraE * D_cameraE_E; // (2*5) * (5*5)
*DE = D_e_cameraE * D_cameraE_E; // (2*5) * (5*5)
return e;
}
}
public:
public:
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
};
// EssentialMatrixFactor3
}// gtsam
/**
* Factor that evaluates algebraic epipolar error (K^-1 p)'E (K^-1 p) for given
* essential matrix and calibration. The calibration is shared between two
* images.
*/
template <class CALIBRATION>
class EssentialMatrixFactor4
: public NoiseModelFactor2<EssentialMatrix, CALIBRATION> {
private:
Point2 pA_, pB_; ///< points in pixel coordinates
typedef NoiseModelFactor2<EssentialMatrix, CALIBRATION> Base;
typedef EssentialMatrixFactor4 This;
static const int DimK = FixedDimension<CALIBRATION>::value;
typedef Eigen::Matrix<double, 2, DimK> 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>(
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<Matrix&> H1 = boost::none,
boost::optional<Matrix&> 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

127
gtsam/slam/EssentialMatrixWithCalibrationFactor.h
View File

@ -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 <gtsam/geometry/EssentialMatrix.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <iostream>
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 CALIBRATION>
class EssentialMatrixWithCalibrationFactor
: public NoiseModelFactor2<EssentialMatrix, CALIBRATION> {
Point2 pA_, pB_; ///< points in pixel coordinates
typedef NoiseModelFactor2<EssentialMatrix, CALIBRATION> 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>(
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<Matrix&> H1 = boost::none,
boost::optional<Matrix&> 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

177
gtsam/slam/tests/testEssentialMatrixFactor.cpp
View File

@ -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<Cal3_S2> camera2(data.cameras[1].pose(), Cal3_S2());
// TODO: maybe default value not good; assert with 0th
Cal3_S2 trueK = Cal3_S2();
PinholeCamera<Cal3_S2> 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<Cal3_S2> 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<double, 1, 1> Vector1;
boost::function<Vector(const EssentialMatrix &, const Cal3_S2 &)> f =
boost::bind(&EssentialMatrixFactor4<Cal3_S2>::evaluateError, 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
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<Cal3_S2> 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<EssentialMatrixFactor4<Cal3_S2>>(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<EssentialMatrix>(1);
Cal3_S2 actualK = result.at<Cal3_S2>(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<Cal3Bundler> //
K = boost::make_shared<Cal3Bundler>(500, 0, 0);
PinholeCamera<Cal3Bundler> camera2(data.cameras[1].pose(), *K);
Cal3Bundler trueK = Cal3Bundler(500, 0, 0);
boost::shared_ptr<Cal3Bundler> K = boost::make_shared<Cal3Bundler>(trueK);
PinholeCamera<Cal3Bundler> 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<EssentialMatrixFactor4<Cal3Bundler>>(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<EssentialMatrix>(1);
Cal3Bundler actualK = result.at<Cal3Bundler>(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() {

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

@ -1,455 +0,0 @@
/**
* @file testEssentialMatrixWithCalibrationFactor.cpp
* @brief Test EssentialMatrixWithCalibrationFactor class
* @author Ayush Baid
* @author Akshay Krishnan
* @date April 22, 2021
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/geometry/Cal3Bundler.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 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<Cal3_S2> 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<Cal3_S2> 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<double, 1, 1> Vector1;
// TODO: fix this
boost::function<Vector(const EssentialMatrix &, const Cal3_S2 &)> f =
boost::bind(
&EssentialMatrixWithCalibrationFactor<Cal3_S2>::evaluateError,
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
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<double(const EssentialMatrix&, const Cal3_S2&,
// OptionalJacobian<1, 5>, OptionalJacobian<1, 3>)> f =
// boost::bind(&EssentialMatrix::error, _1, pA(i), pB(i), _2);
// Expression<EssentialMatrix> E_(keyE); // leaf expression
// Expression<Cal3_S2> K_(keyK); // leaf expression
// Expression<double> 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<double> factor(model1, 0, expr);
// // Check evaluation
// Vector expected(1);
// expected << 0;
// vector<Matrix> 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<double(const EssentialMatrix&, OptionalJacobian<1, 5>)> f
// =
// boost::bind(&EssentialMatrix::error, _1, vA(i), vB(i), _2);
// boost::function<EssentialMatrix(const Rot3&, const Unit3&,
// OptionalJacobian<5, 3>,
// OptionalJacobian<5, 2>)> g;
// Expression<Rot3> R_(key);
// Expression<Unit3> d_(trueDirection);
// Expression<EssentialMatrix>
// E_(&EssentialMatrix::FromRotationAndDirection, R_, d_);
// Expression<double> 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<double> factor(model1, 0, expr);
// // Check evaluation
// Vector expected(1);
// expected << 0;
// vector<Matrix> 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<Cal3_S2> 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<EssentialMatrixWithCalibrationFactor<Cal3_S2>>(
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<EssentialMatrix>(1);
Cal3_S2 actualK = result.at<Cal3_S2>(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<Cal3_S2> //
// K = boost::make_shared<Cal3_S2>(500, 0, 0);
// PinholeCamera<Cal3_S2> 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<EssentialMatrixWithCalibrationFactor>(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<EssentialMatrix>(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<Vector(const EssentialMatrix&, double)> f = boost::bind(
// &EssentialMatrixFactor2::evaluateError, &factor, _1, _2, boost::none,
// boost::none);
// Hexpected1 = numericalDerivative21<Vector2, EssentialMatrix, double>(f,
// trueE, d); Hexpected2 = numericalDerivative22<Vector2, EssentialMatrix,
// double>(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<EssentialMatrixFactor2>(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<EssentialMatrix>(100);
// EXPECT(assert_equal(trueE, actual, 1e-1));
// for (size_t i = 0; i < data.number_tracks(); i++)
// EXPECT_DOUBLES_EQUAL(truth.at<double>(i), result.at<double>(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<Vector(const EssentialMatrix&, double)> f = boost::bind(
// &EssentialMatrixFactor3::evaluateError, &factor, _1, _2, boost::none,
// boost::none);
// Hexpected1 = numericalDerivative21<Vector2, EssentialMatrix, double>(f,
// bodyE, d); Hexpected2 = numericalDerivative22<Vector2, EssentialMatrix,
// double>(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);
}
/* ************************************************************************* */