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

Merge branch 'origin/release/2.4.0' 

Mainly improvements in EssentialMatrix stuff, but also has the new precisions in the diagonal noise model.
release/4.3a0
Frank Dellaert 2014-01-04 00:29:32 -05:00
parent 7761796036 d8b68c6fab
commit a3003a0736
14 changed files with 598 additions and 154 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

61
gtsam/geometry/EssentialMatrix.h
View File

@ -48,6 +48,12 @@ public:
aRb_(aRb), aTb_(aTb), E_(aTb_.skew() * aRb_.matrix()) {
}
/// Random, using Rot3::Random and Sphere2::Random
template<typename Engine>
static EssentialMatrix Random(Engine & rng) {
return EssentialMatrix(Rot3::Random(rng), Sphere2::Random(rng));
}
/// @}
/// @name Testable
@ -82,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);
@ -91,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_);
}
/// @}
@ -133,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.cpp
View File

@ -20,6 +20,7 @@
#include <gtsam/geometry/Rot3.h>
#include <boost/math/constants/constants.hpp>
#include <boost/random.hpp>
#include <cmath>
using namespace std;
@ -43,6 +44,15 @@ Rot3 Rot3::rodriguez(const Sphere2& w, double theta) {
return rodriguez(w.point3(),theta);
}
/* ************************************************************************* */
Rot3 Rot3::Random(boost::random::mt19937 & rng) {
// TODO allow any engine without including all of boost :-(
Sphere2 w = Sphere2::Random(rng);
boost::random::uniform_real_distribution<double> randomAngle(-M_PI,M_PI);
double angle = randomAngle(rng);
return rodriguez(w,angle);
}
/* ************************************************************************* */
Rot3 Rot3::rodriguez(const Vector& w) {
double t = w.norm();

20
gtsam/geometry/Rot3.h
View File

@ -79,12 +79,12 @@ namespace gtsam {
Rot3();
/**
* Constructor from columns
* Constructor from *columns*
* @param r1 X-axis of rotated frame
* @param r2 Y-axis of rotated frame
* @param r3 Z-axis of rotated frame
*/
Rot3(const Point3& r1, const Point3& r2, const Point3& r3);
Rot3(const Point3& col1, const Point3& col2, const Point3& col3);
/** constructor from a rotation matrix, as doubles in *row-major* order !!! */
Rot3(double R11, double R12, double R13,
@ -97,15 +97,15 @@ namespace gtsam {
/** constructor from a rotation matrix */
Rot3(const Matrix& R);
// /** constructor from a fixed size rotation matrix */
// Rot3(const Matrix3& R);
/** Constructor from a quaternion. This can also be called using a plain
* Vector, due to implicit conversion from Vector to Quaternion
* @param q The quaternion
*/
Rot3(const Quaternion& q);
/// Random, generates a random axis, then random angle \in [-p,pi]
static Rot3 Random(boost::random::mt19937 & rng);
/** Virtual destructor */
virtual ~Rot3() {}
@ -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'
*/

8
gtsam/geometry/Rot3M.cpp
View File

@ -36,10 +36,10 @@ static const Matrix3 I3 = Matrix3::Identity();
Rot3::Rot3() : rot_(Matrix3::Identity()) {}
/* ************************************************************************* */
Rot3::Rot3(const Point3& r1, const Point3& r2, const Point3& r3) {
rot_.col(0) = r1.vector();
rot_.col(1) = r2.vector();
rot_.col(2) = r3.vector();
Rot3::Rot3(const Point3& col1, const Point3& col2, const Point3& col3) {
rot_.col(0) = col1.vector();
rot_.col(1) = col2.vector();
rot_.col(2) = col3.vector();
}
/* ************************************************************************* */

8
gtsam/geometry/Rot3Q.cpp
View File

@ -32,11 +32,11 @@ namespace gtsam {
Rot3::Rot3() : quaternion_(Quaternion::Identity()) {}
/* ************************************************************************* */
Rot3::Rot3(const Point3& r1, const Point3& r2, const Point3& r3) :
Rot3::Rot3(const Point3& col1, const Point3& col2, const Point3& col3) :
quaternion_((Eigen::Matrix3d() <<
r1.x(), r2.x(), r3.x(),
r1.y(), r2.y(), r3.y(),
r1.z(), r2.z(), r3.z()).finished()) {}
col1.x(), col2.x(), col3.x(),
col1.y(), col2.y(), col3.y(),
col1.z(), col2.z(), col3.z()).finished()) {}
/* ************************************************************************* */
Rot3::Rot3(double R11, double R12, double R13,

14
gtsam/geometry/Sphere2.cpp
View File

@ -19,12 +19,24 @@
#include <gtsam/geometry/Sphere2.h>
#include <gtsam/geometry/Point2.h>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_on_sphere.hpp>
#include <iostream>
using namespace std;
namespace gtsam {
/* ************************************************************************* */
Sphere2 Sphere2::Random(boost::random::mt19937 & rng) {
// TODO allow any engine without including all of boost :-(
boost::random::uniform_on_sphere<double> randomDirection(3);
vector<double> d = randomDirection(rng);
Sphere2 result;
result.p_ = Point3(d[0], d[1], d[2]);
return result;
}
/* ************************************************************************* */
Matrix Sphere2::basis() const {
@ -78,7 +90,7 @@ Vector Sphere2::error(const Sphere2& q, boost::optional<Matrix&> H) const {
/* ************************************************************************* */
double Sphere2::distance(const Sphere2& q, boost::optional<Matrix&> H) const {
Vector xi = error(q,H);
Vector xi = error(q, H);
double theta = xi.norm();
if (H)
*H = (xi.transpose() / theta) * (*H);

15
gtsam/geometry/Sphere2.h
View File

@ -22,6 +22,18 @@
#include <gtsam/geometry/Point3.h>
#include <gtsam/base/DerivedValue.h>
// (Cumbersome) forward declaration for random generator
namespace boost {
namespace random {
template<class UIntType, std::size_t w, std::size_t n, std::size_t m,
std::size_t r, UIntType a, std::size_t u, UIntType d, std::size_t s,
UIntType b, std::size_t t, UIntType c, std::size_t l, UIntType f>
class mersenne_twister_engine;
typedef mersenne_twister_engine<uint32_t, 32, 624, 397, 31, 0x9908b0df, 11,
0xffffffff, 7, 0x9d2c5680, 15, 0xefc60000, 18, 1812433253> mt19937;
}
}
namespace gtsam {
/// Represents a 3D point on a unit sphere.
@ -53,6 +65,9 @@ public:
p_ = p_ / p_.norm();
}
/// Random direction, using boost::uniform_on_sphere
static Sphere2 Random(boost::random::mt19937 & rng);
/// @}
/// @name Testable

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;

20
gtsam/geometry/tests/testSphere2.cpp
View File

@ -23,6 +23,7 @@
#include <CppUnitLite/TestHarness.h>
#include <boost/bind.hpp>
#include <boost/foreach.hpp>
#include <boost/random.hpp>
#include <boost/assign/std/vector.hpp>
using namespace boost::assign;
@ -63,12 +64,12 @@ TEST(Sphere2, rotate) {
Matrix actualH, expectedH;
// Use numerical derivatives to calculate the expected Jacobian
{
expectedH = numericalDerivative21(rotate_,R,p);
expectedH = numericalDerivative21(rotate_, R, p);
R.rotate(p, actualH, boost::none);
EXPECT(assert_equal(expectedH, actualH, 1e-9));
}
{
expectedH = numericalDerivative22(rotate_,R,p);
expectedH = numericalDerivative22(rotate_, R, p);
R.rotate(p, boost::none, actualH);
EXPECT(assert_equal(expectedH, actualH, 1e-9));
}
@ -237,6 +238,21 @@ TEST(Sphere2, localCoordinates_retract) {
//
//}
//*******************************************************************************
TEST(Sphere2, Random) {
boost::random::mt19937 rng(42);
// Check that is deterministic given same random seed
Point3 expected(-0.667578, 0.671447, 0.321713);
Point3 actual = Sphere2::Random(rng).point3();
EXPECT(assert_equal(expected,actual,1e-5));
// Check that means are all zero at least
Point3 expectedMean, actualMean;
for (size_t i = 0; i < 100; i++)
actualMean = actualMean + Sphere2::Random(rng).point3();
actualMean = actualMean/100;
EXPECT(assert_equal(expectedMean,actualMean,0.1));
}
/* ************************************************************************* */
int main() {
srand(time(NULL));

23
gtsam/linear/NoiseModel.cpp
View File

@ -166,15 +166,12 @@ void Gaussian::WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const
// Diagonal
/* ************************************************************************* */
Diagonal::Diagonal() :
Gaussian(1), sigmas_(ones(1)), invsigmas_(ones(1)) {
Gaussian(1), sigmas_(ones(1)), invsigmas_(ones(1)), precisions_(ones(1)) {
}
Diagonal::Diagonal(const Vector& sigmas, bool initialize_invsigmas):
Gaussian(sigmas.size()), sigmas_(sigmas) {
if (initialize_invsigmas)
invsigmas_ = reciprocal(sigmas);
else
invsigmas_ = boost::none;
Diagonal::Diagonal(const Vector& sigmas) :
Gaussian(sigmas.size()), sigmas_(sigmas), invsigmas_(reciprocal(sigmas)), precisions_(
emul(invsigmas_, invsigmas_)) {
}
/* ************************************************************************* */
@ -205,18 +202,6 @@ void Diagonal::print(const string& name) const {
gtsam::print(sigmas_, name + "diagonal sigmas");
}
/* ************************************************************************* */
Vector Diagonal::invsigmas() const {
if (invsigmas_) return *invsigmas_;
else return reciprocal(sigmas_);
}
/* ************************************************************************* */
double Diagonal::invsigma(size_t i) const {
if (invsigmas_) return (*invsigmas_)(i);
else return 1.0/sigmas_(i);
}
/* ************************************************************************* */
Vector Diagonal::whiten(const Vector& v) const {
return emul(v, invsigmas());

28
gtsam/linear/NoiseModel.h
View File

@ -250,19 +250,19 @@ namespace gtsam {
class GTSAM_EXPORT Diagonal : public Gaussian {
protected:
/** sigmas and reciprocal */
Vector sigmas_;
private:
boost::optional<Vector> invsigmas_; ///< optional to allow for constraints
/**
* Standard deviations (sigmas), their inverse and inverse square (weights/precisions)
* These are all computed at construction: the idea is to use one shared model
* where computation is done only once, the common use case in many problems.
*/
Vector sigmas_, invsigmas_, precisions_;
protected:
/** protected constructor takes sigmas */
Diagonal();
/** constructor to allow for disabling initializaion of invsigmas */
Diagonal(const Vector& sigmas, bool initialize_invsigmas=true);
Diagonal(const Vector& sigmas);
public:
@ -308,8 +308,14 @@ namespace gtsam {
/**
* Return sqrt precisions
*/
Vector invsigmas() const;
double invsigma(size_t i) const;
inline const Vector& invsigmas() const { return invsigmas_; }
inline double invsigma(size_t i) const {return invsigmas_(i);}
/**
* Return precisions
*/
inline const Vector& precisions() const { return precisions_; }
inline double precision(size_t i) const {return precisions_(i);}
/**
* Return R itself, but note that Whiten(H) is cheaper than R*H
@ -354,12 +360,12 @@ namespace gtsam {
/** protected constructor takes sigmas */
// Keeps only sigmas and calculates invsigmas when necessary
Constrained(const Vector& sigmas = zero(1)) :
Diagonal(sigmas, false), mu_(repeat(sigmas.size(), 1000.0)) {}
Diagonal(sigmas), mu_(repeat(sigmas.size(), 1000.0)) {}
// Keeps only sigmas and calculates invsigmas when necessary
// allows for specifying mu
Constrained(const Vector& mu, const Vector& sigmas) :
Diagonal(sigmas, false), mu_(mu) {}
Diagonal(sigmas), mu_(mu) {}
public:

22
gtsam/linear/tests/testNoiseModel.cpp
View File

@ -17,18 +17,21 @@
*/
#include <iostream>
#include <boost/foreach.hpp>
#include <boost/assign/std/vector.hpp>
using namespace boost::assign;
#include <gtsam/linear/NoiseModel.h>
#include <gtsam/base/TestableAssertions.h>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/linear/NoiseModel.h>
#include <boost/foreach.hpp>
#include <boost/assign/std/vector.hpp>
#include <iostream>
#include <limits>
using namespace std;
using namespace gtsam;
using namespace noiseModel;
using namespace boost::assign;
static double sigma = 2, s_1=1.0/sigma, var = sigma*sigma, prc = 1.0/var;
static Matrix R = (Matrix(3, 3) <<
@ -40,7 +43,7 @@ static Matrix Sigma = (Matrix(3, 3) <<
0.0, var, 0.0,
0.0, 0.0, var);
//static double inf = std::numeric_limits<double>::infinity();
//static double inf = numeric_limits<double>::infinity();
/* ************************************************************************* */
TEST(NoiseModel, constructors)
@ -155,7 +158,12 @@ TEST(NoiseModel, ConstrainedConstructors )
Vector sigmas = (Vector(3) << sigma, 0.0, 0.0);
Vector mu = (Vector(3) << 200.0, 300.0, 400.0);
actual = Constrained::All(d);
// TODO: why should this be a thousand ??? Dummy variable?
EXPECT(assert_equal(gtsam::repeat(d, 1000.0), actual->mu()));
EXPECT(assert_equal(gtsam::repeat(d, 0), actual->sigmas()));
double Inf = numeric_limits<double>::infinity();
EXPECT(assert_equal(gtsam::repeat(d, Inf), actual->invsigmas()));
EXPECT(assert_equal(gtsam::repeat(d, Inf), actual->precisions()));
actual = Constrained::All(d, m);
EXPECT(assert_equal(gtsam::repeat(d, m), actual->mu()));

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<52>*(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;