diff --git a/gtsam/geometry/EssentialMatrix.h b/gtsam/geometry/EssentialMatrix.h index d616ac0fa..54fd94bbc 100644 --- a/gtsam/geometry/EssentialMatrix.h +++ b/gtsam/geometry/EssentialMatrix.h @@ -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 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.h b/gtsam/geometry/Rot3.h index 980d35de1..ea59fab17 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -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/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/slam/EssentialMatrixFactor.h b/gtsam/slam/EssentialMatrixFactor.h index 142005d3c..c01a72a76 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;