diff --git a/examples/CreateSFMExampleData.cpp b/examples/CreateSFMExampleData.cpp index aa0bde8f6d..97f4c84dc8 100644 --- a/examples/CreateSFMExampleData.cpp +++ b/examples/CreateSFMExampleData.cpp @@ -15,8 +15,8 @@ * @author Frank Dellaert */ -#include #include +#include #include @@ -26,22 +26,16 @@ using namespace gtsam; /* ************************************************************************* */ -void createExampleBALFile(const string& filename, const vector& P, - const Pose3& pose1, const Pose3& pose2, const Cal3Bundler& K = - Cal3Bundler()) { - +void createExampleBALFile(const string& filename, const vector& points, + const Pose3& pose1, const Pose3& pose2, + const Cal3Bundler& K = Cal3Bundler()) { // Class that will gather all data SfmData data; - - // Create two cameras - Rot3 aRb = Rot3::Yaw(M_PI_2); - Point3 aTb(0.1, 0, 0); - Pose3 identity, aPb(aRb, aTb); + // Create two cameras and add them to data data.cameras.push_back(SfmCamera(pose1, K)); data.cameras.push_back(SfmCamera(pose2, K)); - for(const Point3& p: P) { - + for (const Point3& p : points) { // Create the track SfmTrack track; track.p = p; @@ -51,7 +45,7 @@ void createExampleBALFile(const string& filename, const vector& P, // Project points in both cameras for (size_t i = 0; i < 2; i++) - track.measurements.push_back(make_pair(i, data.cameras[i].project(p))); + track.measurements.push_back(make_pair(i, data.cameras[i].project(p))); // Add track to data data.tracks.push_back(track); @@ -63,49 +57,66 @@ void createExampleBALFile(const string& filename, const vector& P, /* ************************************************************************* */ void create5PointExample1() { - // Create two cameras poses Rot3 aRb = Rot3::Yaw(M_PI_2); Point3 aTb(0.1, 0, 0); Pose3 pose1, pose2(aRb, aTb); // Create test data, we need at least 5 points - vector P; - P += Point3(0, 0, 1), Point3(-0.1, 0, 1), Point3(0.1, 0, 1), // - Point3(0, 0.5, 0.5), Point3(0, -0.5, 0.5); + vector points = { + {0, 0, 1}, {-0.1, 0, 1}, {0.1, 0, 1}, {0, 0.5, 0.5}, {0, -0.5, 0.5}}; // Assumes example is run in ${GTSAM_TOP}/build/examples - const string filename = "../../examples/data/5pointExample1.txt"; - createExampleBALFile(filename, P, pose1, pose2); + const string filename = "../../examples/Data/5pointExample1.txt"; + createExampleBALFile(filename, points, pose1, pose2); } /* ************************************************************************* */ void create5PointExample2() { - // Create two cameras poses Rot3 aRb = Rot3::Yaw(M_PI_2); Point3 aTb(10, 0, 0); Pose3 pose1, pose2(aRb, aTb); // Create test data, we need at least 5 points - vector P; - P += Point3(0, 0, 100), Point3(-10, 0, 100), Point3(10, 0, 100), // - Point3(0, 50, 50), Point3(0, -50, 50), Point3(-20, 0, 80), Point3(20, -50, 80); + vector points = {{0, 0, 100}, {-10, 0, 100}, {10, 0, 100}, // + {0, 50, 50}, {0, -50, 50}, {-20, 0, 80}, // + {20, -50, 80}}; // Assumes example is run in ${GTSAM_TOP}/build/examples - const string filename = "../../examples/data/5pointExample2.txt"; + const string filename = "../../examples/Data/5pointExample2.txt"; Cal3Bundler K(500, 0, 0); - createExampleBALFile(filename, P, pose1, pose2,K); + createExampleBALFile(filename, points, pose1, pose2, K); } /* ************************************************************************* */ +void create18PointExample1() { + // Create two cameras poses + Rot3 aRb = Rot3::Yaw(M_PI_2); + Point3 aTb(0.1, 0, 0); + Pose3 pose1, pose2(aRb, aTb); + + // Create test data, we need 15 points + vector points = { + {0, 0, 1}, {-0.1, 0, 1}, {0.1, 0, 1}, // + {0, 0.5, 0.5}, {0, -0.5, 0.5}, {-1, -0.5, 2}, // + {-1, 0.5, 2}, {0.25, -0.5, 1.5}, {0.25, 0.5, 1.5}, // + {-0.1, -0.5, 0.5}, {0.1, -0.5, 1}, {0.1, 0.5, 1}, // + {-0.1, 0, 0.5}, {-0.1, 0.5, 0.5}, {0, 0, 0.5}, // + {0.1, -0.5, 0.5}, {0.1, 0, 0.5}, {0.1, 0.5, 0.5}}; + + // Assumes example is run in ${GTSAM_TOP}/build/examples + const string filename = "../../examples/Data/18pointExample1.txt"; + createExampleBALFile(filename, points, pose1, pose2); +} + int main(int argc, char* argv[]) { create5PointExample1(); create5PointExample2(); + create18PointExample1(); return 0; } /* ************************************************************************* */ - diff --git a/examples/Data/18pointExample1.txt b/examples/Data/18pointExample1.txt new file mode 100644 index 0000000000..484a7944b4 --- /dev/null +++ b/examples/Data/18pointExample1.txt @@ -0,0 +1,131 @@ +2 18 36 + +0 0 0 -0 +1 0 -6.123233995736766344e-18 -0.10000000000000000555 +0 1 -0.10000000000000000555 -0 +1 1 -1.2246467991473532688e-17 -0.2000000000000000111 +0 2 0.10000000000000000555 -0 +1 2 0 -0 +0 3 0 -1 +1 3 1 -0.20000000000000006661 +0 4 0 1 +1 4 -1 -0.19999999999999995559 +0 5 -0.5 0.25 +1 5 -0.25000000000000005551 -0.55000000000000004441 +0 6 -0.5 -0.25 +1 6 0.24999999999999997224 -0.55000000000000004441 +0 7 0.16666666666666665741 0.33333333333333331483 +1 7 -0.33333333333333331483 0.10000000000000000555 +0 8 0.16666666666666665741 -0.33333333333333331483 +1 8 0.33333333333333331483 0.099999999999999977796 +0 9 -0.2000000000000000111 1 +1 9 -1 -0.39999999999999996669 +0 10 0.10000000000000000555 0.5 +1 10 -0.5 3.0616169978683830179e-17 +0 11 0.10000000000000000555 -0.5 +1 11 0.5 -3.0616169978683830179e-17 +0 12 -0.2000000000000000111 -0 +1 12 -2.4492935982947065376e-17 -0.4000000000000000222 +0 13 -0.2000000000000000111 -1 +1 13 1 -0.40000000000000007772 +0 14 0 -0 +1 14 -1.2246467991473532688e-17 -0.2000000000000000111 +0 15 0.2000000000000000111 1 +1 15 -1 6.1232339957367660359e-17 +0 16 0.2000000000000000111 -0 +1 16 0 -0 +0 17 0.2000000000000000111 -1 +1 17 1 -6.1232339957367660359e-17 + +3.141592653589793116 + 0 + 0 +-0 +0 +0 +1 +0 +0 + +2.2214414690791830509 +2.2214414690791826068 + 0 +-6.123233995736766344e-18 +-0.10000000000000000555 +0 +1 +0 +0 + +0 +0 +1 + +-0.10000000000000000555 +0 +1 + +0.10000000000000000555 +0 +1 + +0 +0.5 +0.5 + +0 +-0.5 +0.5 + +-1 +-0.5 +2 + +-1 +0.5 +2 + +0.25 +-0.5 +1.5 + +0.25 +0.5 +1.5 + +-0.10000000000000000555 +-0.5 +0.5 + +0.10000000000000000555 +-0.5 +1 + +0.10000000000000000555 +0.5 +1 + +-0.10000000000000000555 +0 +0.5 + +-0.10000000000000000555 +0.5 +0.5 + +0 +0 +0.5 + +0.10000000000000000555 +-0.5 +0.5 + +0.10000000000000000555 +0 +0.5 + +0.10000000000000000555 +0.5 +0.5 + diff --git a/gtsam/slam/EssentialMatrixFactor.h b/gtsam/slam/EssentialMatrixFactor.h index a99ac9512e..787efac51e 100644 --- a/gtsam/slam/EssentialMatrixFactor.h +++ b/gtsam/slam/EssentialMatrixFactor.h @@ -7,9 +7,10 @@ #pragma once -#include #include #include +#include + #include namespace gtsam { @@ -17,25 +18,24 @@ namespace gtsam { /** * Factor that evaluates epipolar error p'Ep for given essential matrix */ -class EssentialMatrixFactor: public NoiseModelFactor1 { - - Vector3 vA_, vB_; ///< Homogeneous versions, in ideal coordinates +class EssentialMatrixFactor : public NoiseModelFactor1 { + Vector3 vA_, vB_; ///< Homogeneous versions, in ideal coordinates typedef NoiseModelFactor1 Base; typedef EssentialMatrixFactor This; -public: - + public: /** * Constructor * @param key Essential Matrix variable key * @param pA point in first camera, in calibrated coordinates * @param pB point in second camera, in calibrated coordinates - * @param model noise model is about dot product in ideal, homogeneous coordinates + * @param model noise model is about dot product in ideal, homogeneous + * coordinates */ EssentialMatrixFactor(Key key, const Point2& pA, const Point2& pB, - const SharedNoiseModel& model) : - Base(model, key) { + const SharedNoiseModel& model) + : Base(model, key) { vA_ = EssentialMatrix::Homogeneous(pA); vB_ = EssentialMatrix::Homogeneous(pB); } @@ -45,13 +45,15 @@ class EssentialMatrixFactor: public NoiseModelFactor1 { * @param key Essential Matrix variable key * @param pA point in first camera, in pixel coordinates * @param pB point in second camera, in pixel coordinates - * @param model noise model is about dot product in ideal, homogeneous coordinates + * @param model noise model is about dot product in ideal, homogeneous + * coordinates * @param K calibration object, will be used only in constructor */ - template + template EssentialMatrixFactor(Key key, const Point2& pA, const Point2& pB, - const SharedNoiseModel& model, boost::shared_ptr K) : - Base(model, key) { + const SharedNoiseModel& model, + boost::shared_ptr K) + : Base(model, key) { assert(K); vA_ = EssentialMatrix::Homogeneous(K->calibrate(pA)); vB_ = EssentialMatrix::Homogeneous(K->calibrate(pB)); @@ -64,23 +66,25 @@ class EssentialMatrixFactor: public NoiseModelFactor1 { } /// print - void print(const std::string& s = "", + void print( + const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { Base::print(s); std::cout << " EssentialMatrixFactor with measurements\n (" - << vA_.transpose() << ")' and (" << vB_.transpose() << ")'" - << std::endl; + << vA_.transpose() << ")' and (" << vB_.transpose() << ")'" + << std::endl; } /// vector of errors returns 1D vector - Vector evaluateError(const EssentialMatrix& E, boost::optional H = - boost::none) const override { + Vector evaluateError( + const EssentialMatrix& E, + boost::optional H = boost::none) const override { Vector error(1); error << E.error(vA_, vB_, H); return error; } -public: + public: GTSAM_MAKE_ALIGNED_OPERATOR_NEW }; @@ -88,17 +92,16 @@ class EssentialMatrixFactor: public NoiseModelFactor1 { * 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 pn_; ///< Measurement in image 2, in ideal coordinates - double f_; ///< approximate conversion factor for error scaling +class EssentialMatrixFactor2 + : public NoiseModelFactor2 { + Point3 dP1_; ///< 3D point corresponding to measurement in image 1 + Point2 pn_; ///< Measurement in image 2, in ideal coordinates + double f_; ///< approximate conversion factor for error scaling typedef NoiseModelFactor2 Base; typedef EssentialMatrixFactor2 This; -public: - + public: /** * Constructor * @param key1 Essential Matrix variable key @@ -108,8 +111,10 @@ class EssentialMatrixFactor2: public NoiseModelFactor2 * @param model noise model should be in pixels, as well */ EssentialMatrixFactor2(Key key1, Key key2, const Point2& pA, const Point2& pB, - const SharedNoiseModel& model) : - Base(model, key1, key2), dP1_(EssentialMatrix::Homogeneous(pA)), pn_(pB) { + const SharedNoiseModel& model) + : Base(model, key1, key2), + dP1_(EssentialMatrix::Homogeneous(pA)), + pn_(pB) { f_ = 1.0; } @@ -122,11 +127,13 @@ class EssentialMatrixFactor2: public NoiseModelFactor2 * @param K calibration object, will be used only in constructor * @param model noise model should be in pixels, as well */ - template + template EssentialMatrixFactor2(Key key1, Key key2, const Point2& pA, const Point2& pB, - const SharedNoiseModel& model, boost::shared_ptr K) : - Base(model, key1, key2), dP1_( - EssentialMatrix::Homogeneous(K->calibrate(pA))), pn_(K->calibrate(pB)) { + const SharedNoiseModel& model, + boost::shared_ptr K) + : Base(model, key1, key2), + dP1_(EssentialMatrix::Homogeneous(K->calibrate(pA))), + pn_(K->calibrate(pB)) { f_ = 0.5 * (K->fx() + K->fy()); } @@ -137,12 +144,13 @@ class EssentialMatrixFactor2: public NoiseModelFactor2 } /// print - void print(const std::string& s = "", + void print( + const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { Base::print(s); std::cout << " EssentialMatrixFactor2 with measurements\n (" - << dP1_.transpose() << ")' and (" << pn_.transpose() - << ")'" << std::endl; + << dP1_.transpose() << ")' and (" << pn_.transpose() << ")'" + << std::endl; } /* @@ -150,30 +158,28 @@ class EssentialMatrixFactor2: public NoiseModelFactor2 * @param E essential matrix * @param d inverse depth d */ - Vector evaluateError(const EssentialMatrix& E, const double& d, - boost::optional DE = boost::none, boost::optional Dd = - boost::none) const override { - + Vector evaluateError( + const EssentialMatrix& E, const double& d, + boost::optional DE = boost::none, + boost::optional Dd = boost::none) const override { // We have point x,y in image 1 // Given a depth Z, the corresponding 3D point P1 = Z*(x,y,1) = (x,y,1)/d // We then convert to second camera by P2 = 1R2'*(P1-1T2) // The homogeneous coordinates of can be written as // 2R1*(P1-1T2) == 2R1*d*(P1-1T2) == 2R1*((x,y,1)-d*1T2) - // where we multiplied with d which yields equivalent homogeneous coordinates. - // Note that this is just the homography 2R1 for d==0 - // The point d*P1 = (x,y,1) is computed in constructor as dP1_ + // where we multiplied with d which yields equivalent homogeneous + // coordinates. Note that this is just the homography 2R1 for d==0 The point + // d*P1 = (x,y,1) is computed in constructor as dP1_ // Project to normalized image coordinates, then uncalibrate - Point2 pn(0,0); + Point2 pn(0, 0); if (!DE && !Dd) { - Point3 _1T2 = E.direction().point3(); Point3 d1T2 = d * _1T2; - Point3 dP2 = E.rotation().unrotate(dP1_ - d1T2); // 2R1*((x,y,1)-d*1T2) + Point3 dP2 = E.rotation().unrotate(dP1_ - d1T2); // 2R1*((x,y,1)-d*1T2) pn = PinholeBase::Project(dP2); } else { - // Calculate derivatives. TODO if slow: optimize with Mathematica // 3*2 3*3 3*3 Matrix D_1T2_dir, DdP2_rot, DP2_point; @@ -187,20 +193,19 @@ class EssentialMatrixFactor2: public NoiseModelFactor2 if (DE) { Matrix DdP2_E(3, 5); - DdP2_E << DdP2_rot, -DP2_point * d * D_1T2_dir; // (3*3), (3*3) * (3*2) - *DE = f_ * Dpn_dP2 * DdP2_E; // (2*3) * (3*5) + DdP2_E << DdP2_rot, -DP2_point * d * D_1T2_dir; // (3*3), (3*3) * (3*2) + *DE = f_ * Dpn_dP2 * DdP2_E; // (2*3) * (3*5) } - if (Dd) // efficient backwards computation: + if (Dd) // efficient backwards computation: // (2*3) * (3*3) * (3*1) *Dd = -f_ * (Dpn_dP2 * (DP2_point * _1T2)); - } Point2 reprojectionError = pn - pn_; return f_ * reprojectionError; } -public: + public: GTSAM_MAKE_ALIGNED_OPERATOR_NEW }; // EssentialMatrixFactor2 @@ -210,15 +215,13 @@ class EssentialMatrixFactor2: public NoiseModelFactor2 * in image 2 is perfect, and returns re-projection error in image 1 * This version takes an extrinsic rotation to allow for omni-directional rigs */ -class EssentialMatrixFactor3: public EssentialMatrixFactor2 { - +class EssentialMatrixFactor3 : public EssentialMatrixFactor2 { typedef EssentialMatrixFactor2 Base; typedef EssentialMatrixFactor3 This; - Rot3 cRb_; ///< Rotation from body to camera frame - -public: + Rot3 cRb_; ///< Rotation from body to camera frame + public: /** * Constructor * @param key1 Essential Matrix variable key @@ -229,9 +232,8 @@ class EssentialMatrixFactor3: public EssentialMatrixFactor2 { * @param model noise model should be in calibrated coordinates, as well */ EssentialMatrixFactor3(Key key1, Key key2, const Point2& pA, const Point2& pB, - const Rot3& cRb, const SharedNoiseModel& model) : - EssentialMatrixFactor2(key1, key2, pA, pB, model), cRb_(cRb) { - } + const Rot3& cRb, const SharedNoiseModel& model) + : EssentialMatrixFactor2(key1, key2, pA, pB, model), cRb_(cRb) {} /** * Constructor @@ -242,12 +244,11 @@ class EssentialMatrixFactor3: public EssentialMatrixFactor2 { * @param K calibration object, will be used only in constructor * @param model noise model should be in pixels, as well */ - template + 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) { - } + 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 gtsam::NonlinearFactor::shared_ptr clone() const override { @@ -256,7 +257,8 @@ class EssentialMatrixFactor3: public EssentialMatrixFactor2 { } /// print - void print(const std::string& s = "", + void print( + const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { Base::print(s); std::cout << " EssentialMatrixFactor3 with rotation " << cRb_ << std::endl; @@ -267,9 +269,10 @@ class EssentialMatrixFactor3: public EssentialMatrixFactor2 { * @param E essential matrix * @param d inverse depth d */ - Vector evaluateError(const EssentialMatrix& E, const double& d, - boost::optional DE = boost::none, boost::optional Dd = - boost::none) const override { + Vector evaluateError( + const EssentialMatrix& E, const double& d, + boost::optional DE = boost::none, + boost::optional Dd = boost::none) const override { if (!DE) { // Convert E from body to camera frame EssentialMatrix cameraE = cRb_ * E; @@ -277,18 +280,117 @@ class EssentialMatrixFactor3: public EssentialMatrixFactor2 { return Base::evaluateError(cameraE, d, boost::none, Dd); } else { // Version with derivatives - Matrix D_e_cameraE, D_cameraE_E; // 2*5, 5*5 + Matrix D_e_cameraE, D_cameraE_E; // 2*5, 5*5 EssentialMatrix cameraE = E.rotate(cRb_, D_cameraE_E); Vector e = Base::evaluateError(cameraE, d, D_e_cameraE, Dd); - *DE = D_e_cameraE * D_cameraE_E; // (2*5) * (5*5) + *DE = D_e_cameraE * D_cameraE_E; // (2*5) * (5*5) return e; } } -public: + public: GTSAM_MAKE_ALIGNED_OPERATOR_NEW }; // EssentialMatrixFactor3 -}// gtsam +/** + * Binary factor that optimizes for E and calibration K using the algebraic + * epipolar error (K^-1 pA)'E (K^-1 pB). The calibration is shared between two + * images. + * + * Note: As correspondences between 2d coordinates can only recover 7 DoF, + * this factor should always be used with a prior factor on calibration. + * Even with a prior, we can only optimize 2 DoF in the calibration. So the + * prior should have a noise model with very low sigma in the remaining + * dimensions. This has been tested to work on Cal3_S2. With Cal3Bundler, it + * works only with a strong prior (low sigma noisemodel) on all degrees of + * freedom. + */ +template +class EssentialMatrixFactor4 + : public NoiseModelFactor2 { + private: + Point2 pA_, pB_; ///< points in pixel coordinates + + typedef NoiseModelFactor2 Base; + typedef EssentialMatrixFactor4 This; + + static constexpr int DimK = FixedDimension::value; + typedef Eigen::Matrix JacobianCalibration; + + public: + /** + * Constructor + * @param keyE Essential Matrix (from camera B to A) variable key + * @param keyK Calibration variable key (common for both cameras) + * @param pA point in first camera, in pixel coordinates + * @param pB point in second camera, in pixel coordinates + * @param model noise model is about dot product in ideal, homogeneous + * coordinates + */ + EssentialMatrixFactor4(Key keyE, Key keyK, const Point2& pA, const Point2& pB, + const SharedNoiseModel& model) + : Base(model, keyE, keyK), pA_(pA), pB_(pB) {} + + /// @return a deep copy of this factor + gtsam::NonlinearFactor::shared_ptr clone() const override { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); + } + + /// print + void print( + const std::string& s = "", + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { + Base::print(s); + std::cout << " EssentialMatrixFactor4 with measurements\n (" + << pA_.transpose() << ")' and (" << pB_.transpose() << ")'" + << std::endl; + } + + /** + * @brief Calculate the algebraic epipolar error pA' (K^-1)' E K pB. + * + * @param E essential matrix for key keyE + * @param K calibration (common for both images) for key keyK + * @param H1 optional jacobian of error w.r.t E + * @param H2 optional jacobian of error w.r.t K + * @return * Vector 1D vector of algebraic error + */ + Vector evaluateError( + const EssentialMatrix& E, const CALIBRATION& K, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none) const override { + // converting from pixel coordinates to normalized coordinates cA and cB + JacobianCalibration cA_H_K; // dcA/dK + JacobianCalibration cB_H_K; // dcB/dK + Point2 cA = K.calibrate(pA_, H2 ? &cA_H_K : 0, boost::none); + Point2 cB = K.calibrate(pB_, H2 ? &cB_H_K : 0, boost::none); + + // convert to homogeneous coordinates + Vector3 vA = EssentialMatrix::Homogeneous(cA); + Vector3 vB = EssentialMatrix::Homogeneous(cB); + + if (H2) { + // compute the jacobian of error w.r.t K + + // error function f = vA.T * E * vB + // H2 = df/dK = vB.T * E.T * dvA/dK + vA.T * E * dvB/dK + // where dvA/dK = dvA/dcA * dcA/dK, dVB/dK = dvB/dcB * dcB/dK + // and dvA/dcA = dvB/dcB = [[1, 0], [0, 1], [0, 0]] + *H2 = vB.transpose() * E.matrix().transpose().leftCols<2>() * cA_H_K + + vA.transpose() * E.matrix().leftCols<2>() * cB_H_K; + } + + Vector error(1); + error << E.error(vA, vB, H1); + + return error; + } + + public: + GTSAM_MAKE_ALIGNED_OPERATOR_NEW +}; +// EssentialMatrixFactor4 +} // namespace gtsam diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp index 95db611d76..78857262fb 100644 --- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp +++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp @@ -5,26 +5,24 @@ * @date December 17, 2013 */ -#include - -#include -#include +#include +#include +#include +#include #include -#include #include -#include -#include -#include -#include - -#include +#include +#include +#include +#include +#include using namespace std; using namespace gtsam; // Noise model for first type of factor is evaluating algebraic error -noiseModel::Isotropic::shared_ptr model1 = noiseModel::Isotropic::Sigma(1, - 0.01); +noiseModel::Isotropic::shared_ptr model1 = + noiseModel::Isotropic::Sigma(1, 0.01); // Noise model for second type of factor is evaluating pixel coordinates noiseModel::Unit::shared_ptr model2 = noiseModel::Unit::Create(2); @@ -34,39 +32,33 @@ gtsam::Rot3 cRb = gtsam::Rot3(bX, bZ, -bY).inverse(); namespace example1 { -const string filename = findExampleDataFile("5pointExample1.txt"); +const string filename = findExampleDataFile("18pointExample1.txt"); SfmData data; bool readOK = readBAL(filename, data); Rot3 c1Rc2 = data.cameras[1].pose().rotation(); Point3 c1Tc2 = data.cameras[1].pose().translation(); -PinholeCamera camera2(data.cameras[1].pose(), Cal3_S2()); +// TODO: maybe default value not good; assert with 0th +Cal3_S2 trueK = Cal3_S2(); +PinholeCamera camera2(data.cameras[1].pose(), trueK); Rot3 trueRotation(c1Rc2); Unit3 trueDirection(c1Tc2); EssentialMatrix trueE(trueRotation, trueDirection); -double baseline = 0.1; // actual baseline of the camera +double baseline = 0.1; // actual baseline of the camera -Point2 pA(size_t i) { - return data.tracks[i].measurements[0].second; -} -Point2 pB(size_t i) { - return data.tracks[i].measurements[1].second; -} -Vector vA(size_t i) { - return EssentialMatrix::Homogeneous(pA(i)); -} -Vector vB(size_t i) { - return EssentialMatrix::Homogeneous(pB(i)); -} +Point2 pA(size_t i) { return data.tracks[i].measurements[0].second; } +Point2 pB(size_t i) { return data.tracks[i].measurements[1].second; } +Vector vA(size_t i) { return EssentialMatrix::Homogeneous(pA(i)); } +Vector vB(size_t i) { return EssentialMatrix::Homogeneous(pB(i)); } //************************************************************************* -TEST (EssentialMatrixFactor, testData) { +TEST(EssentialMatrixFactor, testData) { CHECK(readOK); // Check E matrix Matrix expected(3, 3); expected << 0, 0, 0, 0, 0, -0.1, 0.1, 0, 0; - Matrix aEb_matrix = skewSymmetric(c1Tc2.x(), c1Tc2.y(), c1Tc2.z()) - * c1Rc2.matrix(); + Matrix aEb_matrix = + skewSymmetric(c1Tc2.x(), c1Tc2.y(), c1Tc2.z()) * c1Rc2.matrix(); EXPECT(assert_equal(expected, aEb_matrix, 1e-8)); // Check some projections @@ -88,7 +80,7 @@ TEST (EssentialMatrixFactor, testData) { } //************************************************************************* -TEST (EssentialMatrixFactor, factor) { +TEST(EssentialMatrixFactor, factor) { Key key(1); for (size_t i = 0; i < 5; i++) { EssentialMatrixFactor factor(key, pA(i), pB(i), model1); @@ -96,19 +88,12 @@ TEST (EssentialMatrixFactor, factor) { // Check evaluation Vector expected(1); expected << 0; - Matrix Hactual; - Vector actual = factor.evaluateError(trueE, Hactual); + Vector actual = factor.evaluateError(trueE); EXPECT(assert_equal(expected, actual, 1e-7)); - // Use numerical derivatives to calculate the expected Jacobian - Matrix Hexpected; - typedef Eigen::Matrix Vector1; - Hexpected = numericalDerivative11( - boost::bind(&EssentialMatrixFactor::evaluateError, &factor, _1, - boost::none), trueE); - - // Verify the Jacobian is correct - EXPECT(assert_equal(Hexpected, Hactual, 1e-8)); + Values val; + val.insert(key, trueE); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, val, 1e-5, 1e-7); } } @@ -116,10 +101,10 @@ TEST (EssentialMatrixFactor, factor) { TEST(EssentialMatrixFactor, ExpressionFactor) { Key key(1); for (size_t i = 0; i < 5; i++) { - boost::function)> f = + boost::function)> f = boost::bind(&EssentialMatrix::error, _1, vA(i), vB(i), _2); - Expression E_(key); // leaf expression - Expression expr(f, E_); // unary expression + Expression E_(key); // leaf expression + Expression expr(f, E_); // unary expression // Test the derivatives using Paul's magic Values values; @@ -142,13 +127,16 @@ TEST(EssentialMatrixFactor, ExpressionFactor) { TEST(EssentialMatrixFactor, ExpressionFactorRotationOnly) { Key key(1); for (size_t i = 0; i < 5; i++) { - boost::function)> f = + boost::function)> f = boost::bind(&EssentialMatrix::error, _1, vA(i), vB(i), _2); - boost::function, - OptionalJacobian<5, 2>)> g; + boost::function, + OptionalJacobian<5, 2>)> + g; Expression R_(key); Expression d_(trueDirection); - Expression E_(&EssentialMatrix::FromRotationAndDirection, R_, d_); + Expression E_(&EssentialMatrix::FromRotationAndDirection, + R_, d_); Expression expr(f, E_); // Test the derivatives using Paul's magic @@ -169,7 +157,7 @@ TEST(EssentialMatrixFactor, ExpressionFactorRotationOnly) { } //************************************************************************* -TEST (EssentialMatrixFactor, minimization) { +TEST(EssentialMatrixFactor, minimization) { // Here we want to optimize directly on essential matrix constraints // Yi Ma's algorithm (Ma01ijcv) is a bit cumbersome to implement, // but GTSAM does the equivalent anyway, provided we give the right @@ -188,8 +176,8 @@ TEST (EssentialMatrixFactor, minimization) { // Check error at initial estimate Values initial; - EssentialMatrix initialE = trueE.retract( - (Vector(5) << 0.1, -0.1, 0.1, 0.1, -0.1).finished()); + EssentialMatrix initialE = + trueE.retract((Vector(5) << 0.1, -0.1, 0.1, 0.1, -0.1).finished()); initial.insert(1, initialE); #if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) EXPECT_DOUBLES_EQUAL(643.26, graph.error(initial), 1e-2); @@ -212,11 +200,10 @@ TEST (EssentialMatrixFactor, minimization) { // Check errors individually for (size_t i = 0; i < 5; i++) EXPECT_DOUBLES_EQUAL(0, actual.error(vA(i), vB(i)), 1e-6); - } //************************************************************************* -TEST (EssentialMatrixFactor2, factor) { +TEST(EssentialMatrixFactor2, factor) { for (size_t i = 0; i < 5; i++) { EssentialMatrixFactor2 factor(100, i, pA(i), pB(i), model2); @@ -230,22 +217,15 @@ TEST (EssentialMatrixFactor2, factor) { Vector actual = factor.evaluateError(trueE, d, Hactual1, Hactual2); EXPECT(assert_equal(expected, actual, 1e-7)); - // Use numerical derivatives to calculate the expected Jacobian - Matrix Hexpected1, Hexpected2; - boost::function 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-8)); - EXPECT(assert_equal(Hexpected2, Hactual2, 1e-8)); + Values val; + val.insert(100, trueE); + val.insert(i, d); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, val, 1e-5, 1e-7); } } //************************************************************************* -TEST (EssentialMatrixFactor2, minimization) { +TEST(EssentialMatrixFactor2, minimization) { // Here we want to optimize for E and inverse depths at the same time // We start with a factor graph and add constraints to it @@ -288,8 +268,7 @@ TEST (EssentialMatrixFactor2, minimization) { EssentialMatrix bodyE = cRb.inverse() * trueE; //************************************************************************* -TEST (EssentialMatrixFactor3, factor) { - +TEST(EssentialMatrixFactor3, factor) { for (size_t i = 0; i < 5; i++) { EssentialMatrixFactor3 factor(100, i, pA(i), pB(i), cRb, model2); @@ -303,28 +282,21 @@ TEST (EssentialMatrixFactor3, factor) { Vector actual = factor.evaluateError(bodyE, d, Hactual1, Hactual2); EXPECT(assert_equal(expected, actual, 1e-7)); - // Use numerical derivatives to calculate the expected Jacobian - Matrix Hexpected1, Hexpected2; - boost::function 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)); + Values val; + val.insert(100, bodyE); + val.insert(i, d); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, val, 1e-6, 1e-7); } } //************************************************************************* -TEST (EssentialMatrixFactor3, minimization) { - +TEST(EssentialMatrixFactor3, minimization) { // As before, we start with a factor graph and add constraints to it NonlinearFactorGraph graph; for (size_t i = 0; i < 5; i++) // but now we specify the rotation bRc - graph.emplace_shared(100, i, pA(i), pB(i), cRb, model2); + graph.emplace_shared(100, i, pA(i), pB(i), cRb, + model2); // Check error at ground truth Values truth; @@ -351,7 +323,214 @@ TEST (EssentialMatrixFactor3, minimization) { EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4); } -} // namespace example1 +//************************************************************************* +TEST(EssentialMatrixFactor4, factor) { + Key keyE(1); + Key keyK(2); + for (size_t i = 0; i < 5; i++) { + EssentialMatrixFactor4 factor(keyE, keyK, pA(i), pB(i), model1); + + // Check evaluation + Vector1 expected; + expected << 0; + Vector actual = factor.evaluateError(trueE, trueK); + EXPECT(assert_equal(expected, actual, 1e-7)); + + Values truth; + truth.insert(keyE, trueE); + truth.insert(keyK, trueK); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, truth, 1e-6, 1e-7); + } +} + +//************************************************************************* +TEST(EssentialMatrixFactor4, evaluateErrorJacobiansCal3S2) { + Key keyE(1); + Key keyK(2); + // initialize essential matrix + Rot3 r = Rot3::Expmap(Vector3(M_PI / 6, M_PI / 3, M_PI / 9)); + Unit3 t(Point3(2, -1, 0.5)); + EssentialMatrix E = EssentialMatrix::FromRotationAndDirection(r, t); + Cal3_S2 K(200, 1, 1, 10, 10); + Values val; + val.insert(keyE, E); + val.insert(keyK, K); + + Point2 pA(10.0, 20.0); + Point2 pB(12.0, 15.0); + + EssentialMatrixFactor4 f(keyE, keyK, pA, pB, model1); + EXPECT_CORRECT_FACTOR_JACOBIANS(f, val, 1e-5, 1e-6); +} + +//************************************************************************* +TEST(EssentialMatrixFactor4, evaluateErrorJacobiansCal3Bundler) { + Key keyE(1); + Key keyK(2); + // initialize essential matrix + Rot3 r = Rot3::Expmap(Vector3(0, 0, M_PI_2)); + Unit3 t(Point3(0.1, 0, 0)); + EssentialMatrix E = EssentialMatrix::FromRotationAndDirection(r, t); + Cal3Bundler K; + Values val; + val.insert(keyE, E); + val.insert(keyK, K); + + Point2 pA(-0.1, 0.5); + Point2 pB(-0.5, -0.2); + + EssentialMatrixFactor4 f(keyE, keyK, pA, pB, model1); + EXPECT_CORRECT_FACTOR_JACOBIANS(f, val, 1e-5, 1e-5); +} + +//************************************************************************* +TEST(EssentialMatrixFactor4, minimizationWithStrongCal3S2Prior) { + NonlinearFactorGraph graph; + for (size_t i = 0; i < 5; i++) + graph.emplace_shared>(1, 2, pA(i), pB(i), + model1); + + // Check error at ground truth + Values truth; + truth.insert(1, trueE); + truth.insert(2, trueK); + EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8); + + // Initialization + Values initial; + EssentialMatrix initialE = + trueE.retract((Vector(5) << 0.1, -0.1, 0.1, 0.1, -0.1).finished()); + initial.insert(1, initialE); + initial.insert(2, trueK); + + // add prior factor for calibration + Vector5 priorNoiseModelSigma; + priorNoiseModelSigma << 10, 10, 10, 10, 10; + graph.emplace_shared>( + 2, trueK, noiseModel::Diagonal::Sigmas(priorNoiseModelSigma)); + + LevenbergMarquardtOptimizer optimizer(graph, initial); + Values result = optimizer.optimize(); + + // Check result + EssentialMatrix actualE = result.at(1); + Cal3_S2 actualK = result.at(2); + EXPECT(assert_equal(trueE, actualE, 1e-1)); + EXPECT(assert_equal(trueK, actualK, 1e-2)); + + // Check error at result + EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4); + + // Check errors individually + for (size_t i = 0; i < 5; i++) + EXPECT_DOUBLES_EQUAL( + 0, + actualE.error(EssentialMatrix::Homogeneous(actualK.calibrate(pA(i))), + EssentialMatrix::Homogeneous(actualK.calibrate(pB(i)))), + 1e-6); +} + +//************************************************************************* +TEST(EssentialMatrixFactor4, minimizationWithWeakCal3S2Prior) { + // We need 7 points here as the prior on the focal length parameters is weak + // and the initialization is noisy. So in total we are trying to optimize 7 + // DOF, with a strong prior on the remaining 3 DOF. + NonlinearFactorGraph graph; + for (size_t i = 0; i < 7; i++) + graph.emplace_shared>(1, 2, pA(i), pB(i), + model1); + + // Check error at ground truth + Values truth; + truth.insert(1, trueE); + truth.insert(2, trueK); + EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8); + + // Initialization + Values initial; + EssentialMatrix initialE = + trueE.retract((Vector(5) << 0.1, -0.1, 0.1, 0.1, -0.1).finished()); + Cal3_S2 initialK = + trueK.retract((Vector(5) << 0.1, -0.1, 0.0, -0.0, 0.0).finished()); + initial.insert(1, initialE); + initial.insert(2, initialK); + + // add prior factor for calibration + Vector5 priorNoiseModelSigma; + priorNoiseModelSigma << 20, 20, 1, 1, 1; + graph.emplace_shared>( + 2, initialK, noiseModel::Diagonal::Sigmas(priorNoiseModelSigma)); + + LevenbergMarquardtOptimizer optimizer(graph, initial); + Values result = optimizer.optimize(); + + // Check result + EssentialMatrix actualE = result.at(1); + Cal3_S2 actualK = result.at(2); + EXPECT(assert_equal(trueE, actualE, 1e-1)); + EXPECT(assert_equal(trueK, actualK, 1e-2)); + + // Check error at result + EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4); + + // Check errors individually + for (size_t i = 0; i < 7; i++) + EXPECT_DOUBLES_EQUAL( + 0, + actualE.error(EssentialMatrix::Homogeneous(actualK.calibrate(pA(i))), + EssentialMatrix::Homogeneous(actualK.calibrate(pB(i)))), + 1e-5); +} + +//************************************************************************* +TEST(EssentialMatrixFactor4, minimizationWithStrongCal3BundlerPrior) { + NonlinearFactorGraph graph; + for (size_t i = 0; i < 5; i++) + graph.emplace_shared>(1, 2, pA(i), + pB(i), model1); + Cal3Bundler trueK(1, 0, 0, 0, 0, /*tolerance=*/5e-3); + // Check error at ground truth + Values truth; + truth.insert(1, trueE); + truth.insert(2, trueK); + EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8); + + // Check error at initial estimate + Values initial; + EssentialMatrix initialE = + trueE.retract((Vector(5) << 0.1, -0.1, 0.1, 0.1, -0.1).finished()); + Cal3Bundler initialK = trueK; + initial.insert(1, initialE); + initial.insert(2, initialK); + + // add prior factor for calibration + Vector3 priorNoiseModelSigma; + priorNoiseModelSigma << 0.1, 0.1, 0.1; + graph.emplace_shared>( + 2, trueK, noiseModel::Diagonal::Sigmas(priorNoiseModelSigma)); + + LevenbergMarquardtOptimizer optimizer(graph, initial); + Values result = optimizer.optimize(); + + // Check result + EssentialMatrix actualE = result.at(1); + Cal3Bundler actualK = result.at(2); + EXPECT(assert_equal(trueE, actualE, 1e-1)); + EXPECT(assert_equal(trueK, actualK, 1e-2)); + + // Check error at result + EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4); + + // Check errors individually + for (size_t i = 0; i < 5; i++) + EXPECT_DOUBLES_EQUAL( + 0, + actualE.error(EssentialMatrix::Homogeneous(actualK.calibrate(pA(i))), + EssentialMatrix::Homogeneous(actualK.calibrate(pB(i)))), + 1e-6); +} + +} // namespace example1 //************************************************************************* @@ -364,30 +543,26 @@ Rot3 aRb = data.cameras[1].pose().rotation(); Point3 aTb = data.cameras[1].pose().translation(); EssentialMatrix trueE(aRb, Unit3(aTb)); -double baseline = 10; // actual baseline of the camera +double baseline = 10; // actual baseline of the camera -Point2 pA(size_t i) { - return data.tracks[i].measurements[0].second; -} -Point2 pB(size_t i) { - return data.tracks[i].measurements[1].second; -} +Point2 pA(size_t i) { return data.tracks[i].measurements[0].second; } +Point2 pB(size_t i) { return data.tracks[i].measurements[1].second; } -boost::shared_ptr // -K = boost::make_shared(500, 0, 0); -PinholeCamera camera2(data.cameras[1].pose(), *K); +Cal3Bundler trueK = Cal3Bundler(500, 0, 0); +boost::shared_ptr K = boost::make_shared(trueK); +PinholeCamera camera2(data.cameras[1].pose(), trueK); Vector vA(size_t i) { - Point2 xy = K->calibrate(pA(i)); + Point2 xy = trueK.calibrate(pA(i)); return EssentialMatrix::Homogeneous(xy); } Vector vB(size_t i) { - Point2 xy = K->calibrate(pB(i)); + Point2 xy = trueK.calibrate(pB(i)); return EssentialMatrix::Homogeneous(xy); } //************************************************************************* -TEST (EssentialMatrixFactor, extraMinimization) { +TEST(EssentialMatrixFactor, extraMinimization) { // Additional test with camera moving in positive X direction NonlinearFactorGraph graph; @@ -401,8 +576,8 @@ TEST (EssentialMatrixFactor, extraMinimization) { // Check error at initial estimate Values initial; - EssentialMatrix initialE = trueE.retract( - (Vector(5) << 0.1, -0.1, 0.1, 0.1, -0.1).finished()); + EssentialMatrix initialE = + trueE.retract((Vector(5) << 0.1, -0.1, 0.1, 0.1, -0.1).finished()); initial.insert(1, initialE); #if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) @@ -426,11 +601,10 @@ TEST (EssentialMatrixFactor, extraMinimization) { // Check errors individually for (size_t i = 0; i < 5; i++) EXPECT_DOUBLES_EQUAL(0, actual.error(vA(i), vB(i)), 1e-6); - } //************************************************************************* -TEST (EssentialMatrixFactor2, extraTest) { +TEST(EssentialMatrixFactor2, extraTest) { for (size_t i = 0; i < 5; i++) { EssentialMatrixFactor2 factor(100, i, pA(i), pB(i), model2, K); @@ -439,34 +613,27 @@ TEST (EssentialMatrixFactor2, extraTest) { const Point2 pi = camera2.project(P1); Point2 expected(pi - pB(i)); - Matrix Hactual1, Hactual2; double d(baseline / P1.z()); - Vector actual = factor.evaluateError(trueE, d, Hactual1, Hactual2); + Vector actual = factor.evaluateError(trueE, d); EXPECT(assert_equal(expected, actual, 1e-7)); - // Use numerical derivatives to calculate the expected Jacobian - Matrix Hexpected1, Hexpected2; - boost::function 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)); + Values val; + val.insert(100, trueE); + val.insert(i, d); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, val, 1e-5, 1e-6); } } //************************************************************************* -TEST (EssentialMatrixFactor2, extraMinimization) { +TEST(EssentialMatrixFactor2, extraMinimization) { // Additional test with camera moving in positive X direction // We start with a factor graph and add constraints to it // Noise sigma is 1, assuming pixel measurements NonlinearFactorGraph graph; for (size_t i = 0; i < data.number_tracks(); i++) - graph.emplace_shared(100, i, pA(i), pB(i), model2, K); + graph.emplace_shared(100, i, pA(i), pB(i), model2, + K); // Check error at ground truth Values truth; @@ -494,8 +661,7 @@ TEST (EssentialMatrixFactor2, extraMinimization) { } //************************************************************************* -TEST (EssentialMatrixFactor3, extraTest) { - +TEST(EssentialMatrixFactor3, extraTest) { // The "true E" in the body frame is EssentialMatrix bodyE = cRb.inverse() * trueE; @@ -507,26 +673,18 @@ TEST (EssentialMatrixFactor3, extraTest) { const Point2 pi = camera2.project(P1); Point2 expected(pi - pB(i)); - Matrix Hactual1, Hactual2; double d(baseline / P1.z()); - Vector actual = factor.evaluateError(bodyE, d, Hactual1, Hactual2); + Vector actual = factor.evaluateError(bodyE, d); EXPECT(assert_equal(expected, actual, 1e-7)); - // Use numerical derivatives to calculate the expected Jacobian - Matrix Hexpected1, Hexpected2; - boost::function 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)); + Values val; + val.insert(100, bodyE); + val.insert(i, d); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, val, 1e-5, 1e-6); } } -} // namespace example2 +} // namespace example2 /* ************************************************************************* */ int main() { @@ -534,4 +692,3 @@ int main() { return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ -