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

Merge pull request #753 from borglab/feature/essential-mat-with-approx-k 

Adding factor which considers the essential matrix and camera calibration as variable
release/4.3a0
Akshay Krishnan 2021-06-21 09:14:06 -07:00 committed by GitHub
parent 3b4eeebb8c 01561bc217
commit aec2cf06a5
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
4 changed files with 633 additions and 232 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

63
examples/CreateSFMExampleData.cpp
View File

@ -15,8 +15,8 @@
* @author Frank Dellaert
*/
#include <gtsam/slam/dataset.h>
#include <gtsam/geometry/CalibratedCamera.h>
#include <gtsam/slam/dataset.h>
#include <boost/assign/std/vector.hpp>
@ -26,22 +26,16 @@ using namespace gtsam;
/* ************************************************************************* */
void createExampleBALFile(const string& filename, const vector<Point3>& P,
const Pose3& pose1, const Pose3& pose2, const Cal3Bundler& K =
Cal3Bundler()) {
void createExampleBALFile(const string& filename, const vector<Point3>& points,
const Pose3& pose1, const Pose3& pose2,
const Cal3Bundler& K = Cal3Bundler()) {
// 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));
for(const Point3& p: P) {
for (const Point3& p : points) {
// Create the track
SfmTrack track;
track.p = p;
@ -51,7 +45,7 @@ void createExampleBALFile(const string& filename, const vector<Point3>& 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);
@ -63,49 +57,66 @@ void createExampleBALFile(const string& filename, const vector<Point3>& P,
/* ************************************************************************* */
void create5PointExample1() {
// 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 at least 5 points
vector<Point3> 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<Point3> 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);
const string filename = "../../examples/Data/5pointExample1.txt";
createExampleBALFile(filename, points, pose1, pose2);
}
/* ************************************************************************* */
void create5PointExample2() {
// 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 at least 5 points
vector<Point3> 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<Point3> 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";
const string filename = "../../examples/Data/5pointExample2.txt";
Cal3Bundler K(500, 0, 0);
createExampleBALFile(filename, P, pose1, pose2,K);
createExampleBALFile(filename, points, 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<Point3> 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, points, pose1, pose2);
}
int main(int argc, char* argv[]) {
create5PointExample1();
create5PointExample2();
create18PointExample1();
return 0;
}
/* ************************************************************************* */

131
examples/Data/18pointExample1.txt Normal file
View File

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

246
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,117 @@ 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
/**
* 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,
* 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 CALIBRATION>
class EssentialMatrixFactor4
: public NoiseModelFactor2<EssentialMatrix, CALIBRATION> {
private:
Point2 pA_, pB_; ///< points in pixel coordinates
typedef NoiseModelFactor2<EssentialMatrix, CALIBRATION> Base;
typedef EssentialMatrixFactor4 This;
static constexpr int DimK = FixedDimension<CALIBRATION>::value;
typedef Eigen::Matrix<double, 2, DimK> JacobianCalibration;
public:
/**
* Constructor
* @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,
const SharedNoiseModel& model)
: Base(model, keyE, keyK), 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;
}
/**
* @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
* @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,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const override {
// 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, boost::none);
Point2 cB = K.calibrate(pB_, H2 ? &cB_H_K : 0, boost::none);
// 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
// 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 = 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;
}
public:
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
};
// EssentialMatrixFactor4
} // namespace gtsam

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

@ -5,26 +5,24 @@
* @date December 17, 2013
*/
#include <gtsam/slam/EssentialMatrixFactor.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/Cal3_S2.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.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/nonlinear/factorTesting.h>
#include <gtsam/slam/EssentialMatrixFactor.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);
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);
@ -34,39 +32,33 @@ 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();
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);
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
@ -88,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);
@ -96,19 +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<double,1,1> Vector1;
Hexpected = numericalDerivative11<Vector1, EssentialMatrix>(
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);
}
}
@ -116,10 +101,10 @@ TEST (EssentialMatrixFactor, factor) {
TEST(EssentialMatrixFactor, ExpressionFactor) {
Key key(1);
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);
Expression<EssentialMatrix> E_(key); // leaf expression
Expression<double> expr(f, E_); // unary expression
Expression<EssentialMatrix> E_(key); // leaf expression
Expression<double> expr(f, E_); // unary expression
// Test the derivatives using Paul's magic
Values values;
@ -142,13 +127,16 @@ TEST(EssentialMatrixFactor, ExpressionFactor) {
TEST(EssentialMatrixFactor, ExpressionFactorRotationOnly) {
Key key(1);
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::function<EssentialMatrix(const Rot3&, const Unit3&, OptionalJacobian<5, 3>,
OptionalJacobian<5, 2>)> g;
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<EssentialMatrix> E_(&EssentialMatrix::FromRotationAndDirection,
R_, d_);
Expression<double> expr(f, E_);
// Test the derivatives using Paul's magic
@ -169,7 +157,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
@ -188,8 +176,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);
@ -212,11 +200,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);
@ -230,22 +217,15 @@ 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<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-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);
}
}
//*************************************************************************
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
@ -288,8 +268,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);
@ -303,28 +282,21 @@ 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<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-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);
}
}
//*************************************************************************
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<EssentialMatrixFactor3>(100, i, pA(i), pB(i), cRb, model2);
graph.emplace_shared<EssentialMatrixFactor3>(100, i, pA(i), pB(i), cRb,
model2);
// Check error at ground truth
Values truth;
@ -351,7 +323,214 @@ TEST (EssentialMatrixFactor3, minimization) {
EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4);
}
} // namespace example1
//*************************************************************************
TEST(EssentialMatrixFactor4, factor) {
Key keyE(1);
Key keyK(2);
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;
Vector actual = factor.evaluateError(trueE, trueK);
EXPECT(assert_equal(expected, actual, 1e-7));
Values truth;
truth.insert(keyE, trueE);
truth.insert(keyK, trueK);
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, truth, 1e-6, 1e-7);
}
}
//*************************************************************************
TEST(EssentialMatrixFactor4, evaluateErrorJacobiansCal3S2) {
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, 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<Cal3Bundler> f(keyE, keyK, pA, pB, model1);
EXPECT_CORRECT_FACTOR_JACOBIANS(f, val, 1e-5, 1e-5);
}
//*************************************************************************
TEST(EssentialMatrixFactor4, minimizationWithStrongCal3S2Prior) {
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);
// 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<PriorFactor<Cal3_S2>>(
2, trueK, noiseModel::Diagonal::Sigmas(priorNoiseModelSigma));
LevenbergMarquardtOptimizer optimizer(graph, initial);
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));
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<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);
// Initialization
Values initial;
EssentialMatrix initialE =
trueE.retract((Vector(5) << 0.1, -0.1, 0.1, 0.1, -0.1).finished());
Cal3_S2 initialK =
trueK.retract((Vector(5) << 0.1, -0.1, 0.0, -0.0, 0.0).finished());
initial.insert(1, initialE);
initial.insert(2, initialK);
// add prior factor for calibration
Vector5 priorNoiseModelSigma;
priorNoiseModelSigma << 20, 20, 1, 1, 1;
graph.emplace_shared<PriorFactor<Cal3_S2>>(
2, initialK, noiseModel::Diagonal::Sigmas(priorNoiseModelSigma));
LevenbergMarquardtOptimizer optimizer(graph, initial);
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));
EXPECT(assert_equal(trueK, actualK, 1e-2));
// Check error at result
EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4);
// Check errors individually
for (size_t i = 0; i < 7; i++)
EXPECT_DOUBLES_EQUAL(
0,
actualE.error(EssentialMatrix::Homogeneous(actualK.calibrate(pA(i))),
EssentialMatrix::Homogeneous(actualK.calibrate(pB(i)))),
1e-5);
}
//*************************************************************************
TEST(EssentialMatrixFactor4, minimizationWithStrongCal3BundlerPrior) {
NonlinearFactorGraph graph;
for (size_t i = 0; i < 5; i++)
graph.emplace_shared<EssentialMatrixFactor4<Cal3Bundler>>(1, 2, pA(i),
pB(i), model1);
Cal3Bundler trueK(1, 0, 0, 0, 0, /*tolerance=*/5e-3);
// 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 << 0.1, 0.1, 0.1;
graph.emplace_shared<PriorFactor<Cal3Bundler>>(
2, trueK, noiseModel::Diagonal::Sigmas(priorNoiseModelSigma));
LevenbergMarquardtOptimizer optimizer(graph, initial);
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));
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
//*************************************************************************
@ -364,30 +543,26 @@ 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; }
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;
@ -401,8 +576,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)
@ -426,11 +601,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);
@ -439,34 +613,27 @@ 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<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));
Values val;
val.insert(100, trueE);
val.insert(i, d);
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, val, 1e-5, 1e-6);
}
}
//*************************************************************************
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<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
Values truth;
@ -494,8 +661,7 @@ TEST (EssentialMatrixFactor2, extraMinimization) {
}
//*************************************************************************
TEST (EssentialMatrixFactor3, extraTest) {
TEST(EssentialMatrixFactor3, extraTest) {
// The "true E" in the body frame is
EssentialMatrix bodyE = cRb.inverse() * trueE;
@ -507,26 +673,18 @@ 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<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));
Values val;
val.insert(100, bodyE);
val.insert(i, d);
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, val, 1e-5, 1e-6);
}
}
} // namespace example2
} // namespace example2
/* ************************************************************************* */
int main() {
@ -534,4 +692,3 @@ int main() {
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */