diff --git a/gtsam/geometry/EssentialMatrix.h b/gtsam/geometry/EssentialMatrix.h index 4257b867b..54fd94bbc 100644 --- a/gtsam/geometry/EssentialMatrix.h +++ b/gtsam/geometry/EssentialMatrix.h @@ -48,6 +48,12 @@ public: aRb_(aRb), aTb_(aTb), E_(aTb_.skew() * aRb_.matrix()) { } + /// Random, using Rot3::Random and Sphere2::Random + template + 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 HE = + boost::none, boost::optional 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 H = boost::none) const { diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp index 0a3b4fd9f..8dcf14d4b 100644 --- a/gtsam/geometry/Rot3.cpp +++ b/gtsam/geometry/Rot3.cpp @@ -20,6 +20,7 @@ #include #include +#include #include 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 randomAngle(-M_PI,M_PI); + double angle = randomAngle(rng); + return rodriguez(w,angle); +} + /* ************************************************************************* */ Rot3 Rot3::rodriguez(const Vector& w) { double t = w.norm(); diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 5bb382a3e..ea59fab17 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -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' */ diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index 03e7c0e0d..99c5c619e 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -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(); } /* ************************************************************************* */ diff --git a/gtsam/geometry/Rot3Q.cpp b/gtsam/geometry/Rot3Q.cpp index 837972805..c5990153a 100644 --- a/gtsam/geometry/Rot3Q.cpp +++ b/gtsam/geometry/Rot3Q.cpp @@ -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, diff --git a/gtsam/geometry/Sphere2.cpp b/gtsam/geometry/Sphere2.cpp index a5c8c07ea..b6cae287c 100644 --- a/gtsam/geometry/Sphere2.cpp +++ b/gtsam/geometry/Sphere2.cpp @@ -19,12 +19,24 @@ #include #include +#include +#include #include 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 randomDirection(3); + vector 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 H) const { /* ************************************************************************* */ double Sphere2::distance(const Sphere2& q, boost::optional H) const { - Vector xi = error(q,H); + Vector xi = error(q, H); double theta = xi.norm(); if (H) *H = (xi.transpose() / theta) * (*H); diff --git a/gtsam/geometry/Sphere2.h b/gtsam/geometry/Sphere2.h index 5d1bbd7b2..0c549ed0b 100644 --- a/gtsam/geometry/Sphere2.h +++ b/gtsam/geometry/Sphere2.h @@ -22,6 +22,18 @@ #include #include +// (Cumbersome) forward declaration for random generator +namespace boost { +namespace random { +template +class mersenne_twister_engine; +typedef mersenne_twister_engine 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 diff --git a/gtsam/geometry/tests/testEssentialMatrix.cpp b/gtsam/geometry/tests/testEssentialMatrix.cpp index 1a2965089..da5240b15 100644 --- a/gtsam/geometry/tests/testEssentialMatrix.cpp +++ b/gtsam/geometry/tests/testEssentialMatrix.cpp @@ -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; diff --git a/gtsam/geometry/tests/testSphere2.cpp b/gtsam/geometry/tests/testSphere2.cpp index af435bcea..c6e519a44 100644 --- a/gtsam/geometry/tests/testSphere2.cpp +++ b/gtsam/geometry/tests/testSphere2.cpp @@ -23,6 +23,7 @@ #include #include #include +#include #include 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)); diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index ccc61c508..359ce9968 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -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()); diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index 4e09e4dbd..e3bf5a10b 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -250,19 +250,19 @@ namespace gtsam { class GTSAM_EXPORT Diagonal : public Gaussian { protected: - /** sigmas and reciprocal */ - Vector sigmas_; - - private: - - boost::optional 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: diff --git a/gtsam/linear/tests/testNoiseModel.cpp b/gtsam/linear/tests/testNoiseModel.cpp index 99c301dbb..5c1b81401 100644 --- a/gtsam/linear/tests/testNoiseModel.cpp +++ b/gtsam/linear/tests/testNoiseModel.cpp @@ -17,18 +17,21 @@ */ -#include -#include -#include -using namespace boost::assign; +#include +#include #include -#include -#include + +#include +#include + +#include +#include 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::infinity(); +//static double inf = numeric_limits::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::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())); diff --git a/gtsam/slam/EssentialMatrixFactor.h b/gtsam/slam/EssentialMatrixFactor.h index 362fac619..9e6f3f8ba 100644 --- a/gtsam/slam/EssentialMatrixFactor.h +++ b/gtsam/slam/EssentialMatrixFactor.h @@ -20,26 +20,46 @@ namespace gtsam { */ class EssentialMatrixFactor: public NoiseModelFactor1 { - Point2 pA_, pB_; ///< Measurements in image A and B - Vector vA_, vB_; ///< Homogeneous versions + Vector vA_, vB_; ///< Homogeneous versions, in ideal coordinates typedef NoiseModelFactor1 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 + EssentialMatrixFactor(Key key, const Point2& pA, const Point2& pB, + const SharedNoiseModel& model, boost::shared_ptr 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::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 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 { 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 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 + EssentialMatrixFactor2(Key key1, Key key2, const Point2& pA, const Point2& pB, + const SharedNoiseModel& model, boost::shared_ptr 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::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 DE = boost::none, boost::optional 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 + EssentialMatrixFactor3(Key key1, Key key2, const Point2& pA, const Point2& pB, + const Rot3& cRb, const SharedNoiseModel& model, boost::shared_ptr 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::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 DE = boost::none, boost::optional 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 diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp index 5f9233bb3..a521e43fb 100644 --- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp +++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp @@ -6,6 +6,7 @@ */ #include + #include #include #include @@ -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 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(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 f = boost::bind(&EssentialMatrixFactor2::evaluateError, &factor, _1, _2, boost::none, boost::none); - Hexpected1 = numericalDerivative21(f, E, d); - Hexpected2 = numericalDerivative22(f, E, d); + Hexpected1 = numericalDerivative21(f, trueE, d); + Hexpected2 = numericalDerivative22(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(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(i),result.at(i),1e-1)); + EXPECT(assert_equal(truth.at(i), result.at(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 f = + boost::bind(&EssentialMatrixFactor3::evaluateError, &factor, _1, _2, + boost::none, boost::none); + Hexpected1 = numericalDerivative21(f, bodyE, d); + Hexpected2 = numericalDerivative22(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 // +K = boost::make_shared(500, 0, 0); +PinholeCamera 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(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 f = + boost::bind(&EssentialMatrixFactor2::evaluateError, &factor, _1, _2, + boost::none, boost::none); + Hexpected1 = numericalDerivative21(f, trueE, d); + Hexpected2 = numericalDerivative22(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(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(i),result.at(i),1e-1)); + EXPECT(assert_equal(truth.at(i), result.at(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 f = + boost::bind(&EssentialMatrixFactor3::evaluateError, &factor, _1, _2, + boost::none, boost::none); + Hexpected1 = numericalDerivative21(f, bodyE, d); + Hexpected2 = numericalDerivative22(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;