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

Merged in feature/2.4.0/extrinsicE (pull request #2) 

A third factor for essential matrices, now with an extrinsic calibration (rotation only)
release/4.3a0
Frank Dellaert 2014-01-03 23:18:08 -05:00
parent 3afc4eb651 fe3177c257
commit d8b68c6fab
5 changed files with 487 additions and 101 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

55
gtsam/geometry/EssentialMatrix.h
View File

@ -88,7 +88,7 @@ public:
/// Retract delta to manifold
virtual EssentialMatrix retract(const Vector& xi) const {
assert(xi.size()==5);
assert(xi.size() == 5);
Vector3 omega(sub(xi, 0, 3));
Vector2 z(sub(xi, 3, 5));
Rot3 R = aRb_.retract(omega);
@ -97,8 +97,9 @@ public:
}
/// Compute the coordinates in the tangent space
virtual Vector localCoordinates(const EssentialMatrix& value) const {
return Vector(5) << 0, 0, 0, 0, 0;
virtual Vector localCoordinates(const EssentialMatrix& other) const {
return Vector(5) << //
aRb_.localCoordinates(other.aRb_), aTb_.localCoordinates(other.aTb_);
}
/// @}
@ -139,12 +140,58 @@ public:
// The derivative of translation with respect to a 2D sphere delta is 3*2 aTb_.basis()
// Duy made an educated guess that this needs to be rotated to the local frame
Matrix H(3, 5);
H << DE->block < 3, 3 > (0, 0), -aRb_.transpose() * aTb_.basis();
H << DE->block<3, 3>(0, 0), -aRb_.transpose() * aTb_.basis();
*DE = H;
}
return q;
}
/**
* Given essential matrix E in camera frame B, convert to body frame C
* @param cRb rotation from body frame to camera frame
* @param E essential matrix E in camera frame C
*/
EssentialMatrix rotate(const Rot3& cRb, boost::optional<Matrix&> HE =
boost::none, boost::optional<Matrix&> HR = boost::none) const {
// The rotation must be conjugated to act in the camera frame
Rot3 c1Rc2 = aRb_.conjugate(cRb);
if (!HE && !HR) {
// Rotate translation direction and return
Sphere2 c1Tc2 = cRb * aTb_;
return EssentialMatrix(c1Rc2, c1Tc2);
} else {
// Calculate derivatives
Matrix D_c1Tc2_cRb, D_c1Tc2_aTb; // 2*3 and 2*2
Sphere2 c1Tc2 = cRb.rotate(aTb_, D_c1Tc2_cRb, D_c1Tc2_aTb);
if (HE) {
*HE = zeros(5, 5);
HE->block<3, 3>(0, 0) << cRb.matrix(); // a change in aRb_ will yield a rotated change in c1Rc2
HE->block<2, 2>(3, 3) << D_c1Tc2_aTb; // (2*2)
}
if (HR) {
throw std::runtime_error(
"EssentialMatrix::rotate: derivative HR not implemented yet");
/*
HR->resize(5, 3);
HR->block<3, 3>(0, 0) << zeros(3, 3); // a change in the rotation yields ?
HR->block<2, 3>(3, 0) << zeros(2, 3); // (2*3) * (3*3) ?
*/
}
return EssentialMatrix(c1Rc2, c1Tc2);
}
}
/**
* Given essential matrix E in camera frame B, convert to body frame C
* @param cRb rotation from body frame to camera frame
* @param E essential matrix E in camera frame C
*/
friend EssentialMatrix operator*(const Rot3& cRb, const EssentialMatrix& E) {
return E.rotate(cRb);
}
/// epipolar error, algebraic
double error(const Vector& vA, const Vector& vB, //
boost::optional<Matrix&> H = boost::none) const {

10
gtsam/geometry/Rot3.h
View File

@ -225,6 +225,16 @@ namespace gtsam {
/** compose two rotations */
Rot3 operator*(const Rot3& R2) const;
/**
* Conjugation: given a rotation acting in frame B, compute rotation c1Rc2 acting in a frame C
* @param cRb rotation from B frame to C frame
* @return c1Rc2 = cRb * b1Rb2 * cRb'
*/
Rot3 conjugate(const Rot3& cRb) const {
// TODO: do more efficiently by using Eigen or quaternion properties
return cRb * (*this) * cRb.inverse();
}
/**
* Return relative rotation D s.t. R2=D*R1, i.e. D=R2*R1'
*/

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

@ -19,48 +19,82 @@ GTSAM_CONCEPT_MANIFOLD_INST(EssentialMatrix)
//*************************************************************************
// Create two cameras and corresponding essential matrix E
Rot3 aRb = Rot3::yaw(M_PI_2);
Point3 aTb(0.1, 0, 0);
Rot3 c1Rc2 = Rot3::yaw(M_PI_2);
Point3 c1Tc2(0.1, 0, 0);
EssentialMatrix trueE(c1Rc2, c1Tc2);
//*************************************************************************
TEST (EssentialMatrix, equality) {
EssentialMatrix actual(aRb, aTb), expected(aRb, aTb);
EssentialMatrix actual(c1Rc2, c1Tc2), expected(c1Rc2, c1Tc2);
EXPECT(assert_equal(expected, actual));
}
//*************************************************************************
TEST (EssentialMatrix, retract1) {
EssentialMatrix expected(aRb.retract((Vector(3) << 0.1, 0, 0)), aTb);
EssentialMatrix trueE(aRb, aTb);
EssentialMatrix expected(c1Rc2.retract((Vector(3) << 0.1, 0, 0)), c1Tc2);
EssentialMatrix actual = trueE.retract((Vector(5) << 0.1, 0, 0, 0, 0));
EXPECT(assert_equal(expected, actual));
}
//*************************************************************************
TEST (EssentialMatrix, retract2) {
EssentialMatrix expected(aRb, Sphere2(aTb).retract((Vector(2) << 0.1, 0)));
EssentialMatrix trueE(aRb, aTb);
EssentialMatrix expected(c1Rc2,
Sphere2(c1Tc2).retract((Vector(2) << 0.1, 0)));
EssentialMatrix actual = trueE.retract((Vector(5) << 0, 0, 0, 0.1, 0));
EXPECT(assert_equal(expected, actual));
}
//*************************************************************************
Point3 transform_to_(const EssentialMatrix& E, const Point3& point) { return E.transform_to(point); }
Point3 transform_to_(const EssentialMatrix& E, const Point3& point) {
return E.transform_to(point);
}
TEST (EssentialMatrix, transform_to) {
// test with a more complicated EssentialMatrix
Rot3 aRb2 = Rot3::yaw(M_PI/3.0)*Rot3::pitch(M_PI_4)*Rot3::roll(M_PI/6.0);
Rot3 aRb2 = Rot3::yaw(M_PI / 3.0) * Rot3::pitch(M_PI_4)
* Rot3::roll(M_PI / 6.0);
Point3 aTb2(19.2, 3.7, 5.9);
EssentialMatrix E(aRb2, aTb2);
//EssentialMatrix E(aRb, Sphere2(aTb).retract((Vector(2) << 0.1, 0)));
static Point3 P(0.2,0.7,-2);
static Point3 P(0.2, 0.7, -2);
Matrix actH1, actH2;
E.transform_to(P,actH1,actH2);
Matrix expH1 = numericalDerivative21(transform_to_, E,P),
expH2 = numericalDerivative22(transform_to_, E,P);
E.transform_to(P, actH1, actH2);
Matrix expH1 = numericalDerivative21(transform_to_, E, P), //
expH2 = numericalDerivative22(transform_to_, E, P);
EXPECT(assert_equal(expH1, actH1, 1e-8));
EXPECT(assert_equal(expH2, actH2, 1e-8));
}
//*************************************************************************
EssentialMatrix rotate_(const EssentialMatrix& E, const Rot3& cRb) {
return E.rotate(cRb);
}
TEST (EssentialMatrix, rotate) {
// Suppose the essential matrix is specified in a body coordinate frame B
// which is rotated with respect to the camera frame C, via rotation bRc.
// The rotation between body and camera is:
Point3 bX(1, 0, 0), bY(0, 1, 0), bZ(0, 0, 1);
Rot3 bRc(bX, bZ, -bY), cRb = bRc.inverse();
// Let's compute the ground truth E in body frame:
Rot3 b1Rb2 = bRc * c1Rc2 * cRb;
Point3 b1Tb2 = bRc * c1Tc2;
EssentialMatrix bodyE(b1Rb2, b1Tb2);
EXPECT(assert_equal(bodyE, bRc * trueE, 1e-8));
EXPECT(assert_equal(bodyE, trueE.rotate(bRc), 1e-8));
// Let's go back to camera frame:
EXPECT(assert_equal(trueE, cRb * bodyE, 1e-8));
EXPECT(assert_equal(trueE, bodyE.rotate(cRb), 1e-8));
// Derivatives
Matrix actH1, actH2;
try { bodyE.rotate(cRb, actH1, actH2);} catch(exception e) {} // avoid exception
Matrix expH1 = numericalDerivative21(rotate_, bodyE, cRb), //
expH2 = numericalDerivative22(rotate_, bodyE, cRb);
EXPECT(assert_equal(expH1, actH1, 1e-8));
// Does not work yet EXPECT(assert_equal(expH2, actH2, 1e-8));
}
/* ************************************************************************* */
int main() {
TestResult tr;

203
gtsam/slam/EssentialMatrixFactor.h
View File

@ -20,26 +20,46 @@ namespace gtsam {
*/
class EssentialMatrixFactor: public NoiseModelFactor1<EssentialMatrix> {
Point2 pA_, pB_; ///< Measurements in image A and B
Vector vA_, vB_; ///< Homogeneous versions
Vector vA_, vB_; ///< Homogeneous versions, in ideal coordinates
typedef NoiseModelFactor1<EssentialMatrix> Base;
typedef EssentialMatrixFactor This;
public:
/// Constructor
/**
* Constructor
* @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
*/
EssentialMatrixFactor(Key key, const Point2& pA, const Point2& pB,
const SharedNoiseModel& model) :
Base(model, key), pA_(pA), pB_(pB), //
vA_(EssentialMatrix::Homogeneous(pA)), //
vB_(EssentialMatrix::Homogeneous(pB)) {
Base(model, key) {
vA_ = EssentialMatrix::Homogeneous(pA);
vB_ = EssentialMatrix::Homogeneous(pB);
}
/**
* Constructor
* @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 K calibration object, will be used only in constructor
*/
template<class CALIBRATION>
EssentialMatrixFactor(Key key, const Point2& pA, const Point2& pB,
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));
}
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
return boost::static_pointer_cast < gtsam::NonlinearFactor
> (gtsam::NonlinearFactor::shared_ptr(new This(*this)));
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
}
/// print
@ -47,46 +67,72 @@ public:
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
Base::print(s);
std::cout << " EssentialMatrixFactor with measurements\n ("
<< pA_.vector().transpose() << ")' and (" << pB_.vector().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 {
return (Vector(1) << E.error(vA_, vB_, H));
Vector error(1);
error << E.error(vA_, vB_, H);
return error;
}
};
/**
* Binary factor that optimizes for E and inverse depth: assumes measurement
* 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,
LieScalar> {
Point3 dP1_; ///< 3D point corresponding to measurement in image 1
Point2 p1_, p2_; ///< Measurements in image 1 and image 2
Cal3_S2 K_; ///< Calibration
Point2 pn_; ///< Measurement in image 2, in ideal coordinates
double f_; ///< approximate conversion factor for error scaling
typedef NoiseModelFactor2<EssentialMatrix, LieScalar> Base;
typedef EssentialMatrixFactor2 This;
public:
/// Constructor
/**
* Constructor
* @param pA point in first camera, in calibrated coordinates
* @param pB point in second camera, in calibrated coordinates
* @param model noise model should be in pixels, as well
*/
EssentialMatrixFactor2(Key key1, Key key2, const Point2& pA, const Point2& pB,
const Cal3_S2& K, const SharedNoiseModel& model) :
Base(model, key1, key2), p1_(pA), p2_(pB), K_(K) {
Point2 xy = K_.calibrate(p1_);
dP1_ = Point3(xy.x(), xy.y(), 1);
const SharedNoiseModel& model) :
Base(model, key1, key2) {
dP1_ = Point3(pA.x(), pA.y(), 1);
pn_ = pB;
f_ = 1.0;
}
/**
* Constructor
* @param pA point in first camera, in pixel coordinates
* @param pB point in second camera, in pixel coordinates
* @param K calibration object, will be used only in constructor
* @param model noise model should be in pixels, as well
*/
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) {
assert(K);
Point2 p1 = K->calibrate(pA);
dP1_ = Point3(p1.x(), p1.y(), 1); // d*P1 = (x,y,1)
pn_ = K->calibrate(pB);
f_ = 0.5 * (K->fx() + K->fy());
}
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
return boost::static_pointer_cast < gtsam::NonlinearFactor
> (gtsam::NonlinearFactor::shared_ptr(new This(*this)));
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
}
/// print
@ -94,62 +140,143 @@ public:
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
Base::print(s);
std::cout << " EssentialMatrixFactor2 with measurements\n ("
<< p1_.vector().transpose() << ")' and (" << p2_.vector().transpose()
<< dP1_.vector().transpose() << ")' and (" << pn_.vector().transpose()
<< ")'" << std::endl;
}
/// vector of errors returns 1D vector
/*
* Vector of errors returns 2D vector
* @param E essential matrix
* @param d inverse depth d
*/
Vector evaluateError(const EssentialMatrix& E, const LieScalar& d,
boost::optional<Matrix&> DE = boost::none, boost::optional<Matrix&> Dd =
boost::none) const {
// 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 first camera by 2P = 1R2Õ*(P1-1T2)
// 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)
// Note that this is just a homography for d==0
// 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 pi;
Point2 pn;
if (!DE && !Dd) {
Point3 _1T2 = E.direction().point3();
Point3 d1T2 = d * _1T2;
Point3 dP2 = E.rotation().unrotate(dP1_ - d1T2);
Point2 pn = SimpleCamera::project_to_camera(dP2);
pi = K_.uncalibrate(pn);
Point3 dP2 = E.rotation().unrotate(dP1_ - d1T2); // 2R1*((x,y,1)-d*1T2)
pn = SimpleCamera::project_to_camera(dP2);
} else {
// Calculate derivatives. TODO if slow: optimize with Mathematica
// 3*2 3*3 3*3 2*3 2*2
Matrix D_1T2_dir, DdP2_rot, DP2_point, Dpn_dP2, Dpi_pn;
// 3*2 3*3 3*3 2*3
Matrix D_1T2_dir, DdP2_rot, DP2_point, Dpn_dP2;
Point3 _1T2 = E.direction().point3(D_1T2_dir);
Point3 d1T2 = d * _1T2;
Point3 dP2 = E.rotation().unrotate(dP1_ - d1T2, DdP2_rot, DP2_point);
Point2 pn = SimpleCamera::project_to_camera(dP2, Dpn_dP2);
pi = K_.uncalibrate(pn, boost::none, Dpi_pn);
pn = SimpleCamera::project_to_camera(dP2, Dpn_dP2);
if (DE) {
Matrix DdP2_E(3, 5);
DdP2_E << DdP2_rot, -DP2_point * d * D_1T2_dir; // (3*3), (3*3) * (3*2)
*DE = Dpi_pn * (Dpn_dP2 * DdP2_E); // (2*2) * (2*3) * (3*5)
*DE = f_ * Dpn_dP2 * DdP2_E; // (2*3) * (3*5)
}
if (Dd) // efficient backwards computation:
// (2*2) * (2*3) * (3*3) * (3*1)
*Dd = -(Dpi_pn * (Dpn_dP2 * (DP2_point * _1T2.vector())));
// (2*3) * (3*3) * (3*1)
*Dd = -f_ * (Dpn_dP2 * (DP2_point * _1T2.vector()));
}
Point2 reprojectionError = pi - p2_;
return reprojectionError.vector();
Point2 reprojectionError = pn - pn_;
return f_ * reprojectionError.vector();
}
};
// EssentialMatrixFactor2
/**
* 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
* This version takes an extrinsic rotation to allow for omni-directional rigs
*/
class EssentialMatrixFactor3: public EssentialMatrixFactor2 {
typedef EssentialMatrixFactor2 Base;
typedef EssentialMatrixFactor3 This;
Rot3 cRb_; ///< Rotation from body to camera frame
public:
/**
* Constructor
* @param pA point in first camera, in calibrated coordinates
* @param pB point in second camera, in calibrated coordinates
* @param bRc extra rotation between "body" and "camera" frame
* @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) {
}
/**
* Constructor
* @param pA point in first camera, in pixel coordinates
* @param pB point in second camera, in pixel coordinates
* @param K calibration object, will be used only in constructor
* @param model noise model should be in pixels, as well
*/
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) {
}
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
}
/// print
virtual void print(const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
Base::print(s);
std::cout << " EssentialMatrixFactor3 with rotation " << cRb_ << std::endl;
}
/*
* Vector of errors returns 2D vector
* @param E essential matrix
* @param d inverse depth d
*/
Vector evaluateError(const EssentialMatrix& E, const LieScalar& d,
boost::optional<Matrix&> DE = boost::none, boost::optional<Matrix&> Dd =
boost::none) const {
if (!DE) {
// Convert E from body to camera frame
EssentialMatrix cameraE = cRb_ * E;
// Evaluate error
return Base::evaluateError(cameraE, d, boost::none, Dd);
} else {
// Version with derivatives
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)
return e;
}
}
};
// EssentialMatrixFactor3
}// gtsam

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

@ -6,6 +6,7 @@
*/
#include <gtsam/slam/EssentialMatrixFactor.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
@ -18,15 +19,25 @@
using namespace std;
using namespace gtsam;
typedef noiseModel::Isotropic::shared_ptr Model;
// 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");
SfM_data data;
bool readOK = readBAL(filename, data);
Rot3 aRb = data.cameras[1].pose().rotation();
Point3 aTb = data.cameras[1].pose().translation();
Rot3 c1Rc2 = data.cameras[1].pose().rotation();
Point3 c1Tc2 = data.cameras[1].pose().translation();
PinholeCamera<Cal3_S2> camera2(data.cameras[1].pose(),Cal3_S2());
EssentialMatrix trueE(c1Rc2, c1Tc2);
double baseline = 0.1; // actual baseline of the camera
Point2 pA(size_t i) {
@ -42,8 +53,6 @@ Vector vB(size_t i) {
return EssentialMatrix::Homogeneous(pB(i));
}
Cal3_S2 K;
//*************************************************************************
TEST (EssentialMatrixFactor, testData) {
CHECK(readOK);
@ -51,35 +60,32 @@ TEST (EssentialMatrixFactor, testData) {
// Check E matrix
Matrix expected(3, 3);
expected << 0, 0, 0, 0, 0, -0.1, 0.1, 0, 0;
Matrix aEb_matrix = skewSymmetric(aTb.x(), aTb.y(), aTb.z()) * aRb.matrix();
EXPECT(assert_equal(expected, aEb_matrix,1e-8));
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));
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((Vector(3) << -1,0.2,1),vB(4),1e-8));
EXPECT(assert_equal((Vector(3) << -1, 0.2, 1), vB(4), 1e-8));
// Check epipolar constraint
for (size_t i = 0; i < 5; i++)
EXPECT_DOUBLES_EQUAL(0, vA(i).transpose() * aEb_matrix * vB(i), 1e-8);
// Check epipolar constraint
EssentialMatrix trueE(aRb, aTb);
for (size_t i = 0; i < 5; i++)
EXPECT_DOUBLES_EQUAL(0, trueE.error(vA(i),vB(i)), 1e-7);
EXPECT_DOUBLES_EQUAL(0, trueE.error(vA(i), vB(i)), 1e-7);
}
//*************************************************************************
TEST (EssentialMatrixFactor, factor) {
EssentialMatrix trueE(aRb, aTb);
noiseModel::Unit::shared_ptr model = noiseModel::Unit::Create(1);
for (size_t i = 0; i < 5; i++) {
EssentialMatrixFactor factor(1, pA(i), pB(i), model);
EssentialMatrixFactor factor(1, pA(i), pB(i), model1);
// Check evaluation
Vector expected(1);
@ -100,7 +106,7 @@ TEST (EssentialMatrixFactor, factor) {
}
//*************************************************************************
TEST (EssentialMatrixFactor, fromConstraints) {
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
@ -109,13 +115,11 @@ TEST (EssentialMatrixFactor, fromConstraints) {
// We start with a factor graph and add constraints to it
// Noise sigma is 1cm, assuming metric measurements
NonlinearFactorGraph graph;
Model model = noiseModel::Isotropic::Sigma(1, 0.01);
for (size_t i = 0; i < 5; i++)
graph.add(EssentialMatrixFactor(1, pA(i), pB(i), model));
graph.add(EssentialMatrixFactor(1, pA(i), pB(i), model1));
// Check error at ground truth
Values truth;
EssentialMatrix trueE(aRb, aTb);
truth.insert(1, trueE);
EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8);
@ -133,35 +137,31 @@ TEST (EssentialMatrixFactor, fromConstraints) {
// Check result
EssentialMatrix actual = result.at<EssentialMatrix>(1);
EXPECT(assert_equal(trueE, actual,1e-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);
EXPECT_DOUBLES_EQUAL(0, actual.error(vA(i), vB(i)), 1e-6);
}
//*************************************************************************
TEST (EssentialMatrixFactor2, factor) {
EssentialMatrix E(aRb, aTb);
noiseModel::Unit::shared_ptr model = noiseModel::Unit::Create(1);
for (size_t i = 0; i < 5; i++) {
EssentialMatrixFactor2 factor(100, i, pA(i), pB(i), K, model);
EssentialMatrixFactor2 factor(100, i, pA(i), pB(i), model2);
// Check evaluation
Point3 P1 = data.tracks[i].p, P2 = data.cameras[1].pose().transform_to(P1);
const Point2 pn = SimpleCamera::project_to_camera(P2);
const Point2 pi = K.uncalibrate(pn);
Point3 P1 = data.tracks[i].p;
const Point2 pi = camera2.project(P1);
Point2 reprojectionError(pi - pB(i));
Vector expected = reprojectionError.vector();
Matrix Hactual1, Hactual2;
LieScalar d(baseline / P1.z());
Vector actual = factor.evaluateError(E, d, Hactual1, Hactual2);
Vector actual = factor.evaluateError(trueE, d, Hactual1, Hactual2);
EXPECT(assert_equal(expected, actual, 1e-7));
// Use numerical derivatives to calculate the expected Jacobian
@ -169,8 +169,8 @@ TEST (EssentialMatrixFactor2, factor) {
boost::function<Vector(const EssentialMatrix&, const LieScalar&)> f =
boost::bind(&EssentialMatrixFactor2::evaluateError, &factor, _1, _2,
boost::none, boost::none);
Hexpected1 = numericalDerivative21<EssentialMatrix>(f, E, d);
Hexpected2 = numericalDerivative22<EssentialMatrix>(f, E, d);
Hexpected1 = numericalDerivative21<EssentialMatrix>(f, trueE, d);
Hexpected2 = numericalDerivative22<EssentialMatrix>(f, trueE, d);
// Verify the Jacobian is correct
EXPECT(assert_equal(Hexpected1, Hactual1, 1e-8));
@ -185,13 +185,11 @@ TEST (EssentialMatrixFactor2, minimization) {
// We start with a factor graph and add constraints to it
// Noise sigma is 1cm, assuming metric measurements
NonlinearFactorGraph graph;
Model model = noiseModel::Isotropic::Sigma(2, 0.01);
for (size_t i = 0; i < 5; i++)
graph.add(EssentialMatrixFactor2(100, i, pA(i), pB(i), K, model));
graph.add(EssentialMatrixFactor2(100, i, pA(i), pB(i), model2));
// Check error at ground truth
Values truth;
EssentialMatrix trueE(aRb, aTb);
truth.insert(100, trueE);
for (size_t i = 0; i < 5; i++) {
Point3 P1 = data.tracks[i].p;
@ -207,14 +205,72 @@ TEST (EssentialMatrixFactor2, minimization) {
// Check result
EssentialMatrix actual = result.at<EssentialMatrix>(100);
EXPECT(assert_equal(trueE, actual,1e-1));
EXPECT(assert_equal(trueE, actual, 1e-1));
for (size_t i = 0; i < 5; i++)
EXPECT(assert_equal(truth.at<LieScalar>(i),result.at<LieScalar>(i),1e-1));
EXPECT(assert_equal(truth.at<LieScalar>(i), result.at<LieScalar>(i), 1e-1));
// Check error at result
EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4);
}
//*************************************************************************
// Below we want to optimize for an essential matrix specified in a
// body coordinate frame B which is rotated with respect to the camera
// frame C, via the rotation bRc.
// The "true E" in the body frame is then
EssentialMatrix bodyE = cRb.inverse() * trueE;
//*************************************************************************
TEST (EssentialMatrixFactor3, factor) {
for (size_t i = 0; i < 5; i++) {
EssentialMatrixFactor3 factor(100, i, pA(i), pB(i), cRb, model2);
// Check evaluation
Point3 P1 = data.tracks[i].p;
const Point2 pi = camera2.project(P1);
Point2 reprojectionError(pi - pB(i));
Vector expected = reprojectionError.vector();
Matrix Hactual1, Hactual2;
LieScalar 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&, const LieScalar&)> f =
boost::bind(&EssentialMatrixFactor3::evaluateError, &factor, _1, _2,
boost::none, boost::none);
Hexpected1 = numericalDerivative21<EssentialMatrix>(f, bodyE, d);
Hexpected2 = numericalDerivative22<EssentialMatrix>(f, bodyE, d);
// Verify the Jacobian is correct
EXPECT(assert_equal(Hexpected1, Hactual1, 1e-8));
EXPECT(assert_equal(Hexpected2, Hactual2, 1e-8));
}
}
//*************************************************************************
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.add(EssentialMatrixFactor3(100, i, pA(i), pB(i), cRb, model2));
// Check error at ground truth
Values truth;
truth.insert(100, bodyE);
for (size_t i = 0; i < 5; i++) {
Point3 P1 = data.tracks[i].p;
truth.insert(i, LieScalar(baseline / P1.z()));
}
EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8);
}
} // namespace example1
//*************************************************************************
@ -226,6 +282,8 @@ SfM_data data;
bool readOK = readBAL(filename, data);
Rot3 aRb = data.cameras[1].pose().rotation();
Point3 aTb = data.cameras[1].pose().translation();
EssentialMatrix trueE(aRb, aTb);
double baseline = 10; // actual baseline of the camera
Point2 pA(size_t i) {
@ -235,23 +293,99 @@ Point2 pB(size_t i) {
return data.tracks[i].measurements[1].second;
}
// Matches Cal3Bundler K(500, 0, 0);
Cal3_S2 K(500, 500, 0, 0, 0);
boost::shared_ptr<Cal3Bundler> //
K = boost::make_shared<Cal3Bundler>(500, 0, 0);
PinholeCamera<Cal3Bundler> 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 (EssentialMatrixFactor, extraMinimization) {
// Additional test with camera moving in positive X direction
NonlinearFactorGraph graph;
for (size_t i = 0; i < 5; i++)
graph.add(EssentialMatrixFactor(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));
initial.insert(1, initialE);
EXPECT_DOUBLES_EQUAL(640, graph.error(initial), 1e-2);
// 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 reprojectionError(pi - pB(i));
Vector expected = reprojectionError.vector();
Matrix Hactual1, Hactual2;
LieScalar 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&, const LieScalar&)> f =
boost::bind(&EssentialMatrixFactor2::evaluateError, &factor, _1, _2,
boost::none, boost::none);
Hexpected1 = numericalDerivative21<EssentialMatrix>(f, trueE, d);
Hexpected2 = numericalDerivative22<EssentialMatrix>(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;
Model model = noiseModel::Isotropic::Sigma(2, 1);
for (size_t i = 0; i < data.number_tracks(); i++)
graph.add(EssentialMatrixFactor2(100, i, pA(i), pB(i), K, model));
graph.add(EssentialMatrixFactor2(100, i, pA(i), pB(i), model2, K));
// Check error at ground truth
Values truth;
EssentialMatrix trueE(aRb, aTb);
truth.insert(100, trueE);
for (size_t i = 0; i < data.number_tracks(); i++) {
Point3 P1 = data.tracks[i].p;
@ -267,16 +401,50 @@ TEST (EssentialMatrixFactor2, extraTest) {
// Check result
EssentialMatrix actual = result.at<EssentialMatrix>(100);
EXPECT(assert_equal(trueE, actual,1e-1));
EXPECT(assert_equal(trueE, actual, 1e-1));
for (size_t i = 0; i < data.number_tracks(); i++)
EXPECT(assert_equal(truth.at<LieScalar>(i),result.at<LieScalar>(i),1e-1));
EXPECT(assert_equal(truth.at<LieScalar>(i), result.at<LieScalar>(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 reprojectionError(pi - pB(i));
Vector expected = reprojectionError.vector();
Matrix Hactual1, Hactual2;
LieScalar 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&, const LieScalar&)> f =
boost::bind(&EssentialMatrixFactor3::evaluateError, &factor, _1, _2,
boost::none, boost::none);
Hexpected1 = numericalDerivative21<EssentialMatrix>(f, bodyE, d);
Hexpected2 = numericalDerivative22<EssentialMatrix>(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;