diff --git a/.cproject b/.cproject index 86952742b..1134896ad 100644 --- a/.cproject +++ b/.cproject @@ -5,13 +5,13 @@ - - + + @@ -60,13 +60,13 @@ - - + + @@ -116,13 +116,13 @@ - - + + @@ -819,10 +819,18 @@ false true - + make - -j4 - SmartStereoProjectionFactorExample.run + -j5 + SmartProjectionFactorExample_kitti_nonbatch.run + true + true + true + + + make + -j5 + SmartProjectionFactorExample_kitti.run true true true @@ -835,14 +843,6 @@ true true - - make - -j4 - SmartProjectionFactorExample.run - true - true - true - make -j2 @@ -1035,30 +1035,6 @@ true true - - make - -j4 - testCameraSet.run - true - true - true - - - make - -j4 - testTriangulation.run - true - true - true - - - make - -j4 - testPinholeSet.run - true - true - true - make -j2 @@ -1147,6 +1123,14 @@ true true + + make + -j5 + testSmartProjectionFactor.run + true + true + true + make -j5 @@ -1546,10 +1530,10 @@ true true - + make - -j4 - testRegularImplicitSchurFactor.run + -j5 + testImplicitSchurFactor.run true true true @@ -1578,30 +1562,6 @@ true true - - make - -j4 - testSmartProjectionCameraFactor.run - true - true - true - - - make - -j4 - testSmartFactorBase.run - true - true - true - - - make - -j4 - testTriangulationFactor.run - true - true - true - make -j3 @@ -2503,14 +2463,6 @@ true true - - make - -j4 - SFMExample_SmartFactorPCG.run - true - true - true - make -j2 @@ -3103,22 +3055,6 @@ true true - - make - -j4 - testRegularHessianFactor.run - true - true - true - - - make - -j4 - testRegularJacobianFactor.run - true - true - true - make -j5 diff --git a/gtsam/slam/SmartProjectionFactor.h b/gtsam/slam/SmartProjectionFactor.h index aad1a27f6..c7b2962e4 100644 --- a/gtsam/slam/SmartProjectionFactor.h +++ b/gtsam/slam/SmartProjectionFactor.h @@ -31,6 +31,16 @@ namespace gtsam { +/// Linearization mode: what factor to linearize to +enum LinearizationMode { + HESSIAN, IMPLICIT_SCHUR, JACOBIAN_Q, JACOBIAN_SVD +}; + +/// How to manage degeneracy +enum DegeneracyMode { + IGNORE_DEGENERACY, ZERO_ON_DEGENERACY, HANDLE_INFINITY +}; + /** * SmartProjectionFactor: triangulates point and keeps an estimate of it around. */ @@ -39,16 +49,6 @@ class SmartProjectionFactor: public SmartFactorBase { public: - /// Linearization mode: what factor to linearize to - enum LinearizationMode { - HESSIAN, IMPLICIT_SCHUR, JACOBIAN_Q, JACOBIAN_SVD - }; - - /// How to manage degeneracy - enum DegeneracyMode { - IGNORE_DEGENERACY, ZERO_ON_DEGENERACY, HANDLE_INFINITY - }; - private: typedef SmartFactorBase Base; typedef SmartProjectionFactor This; diff --git a/gtsam/slam/SmartProjectionPoseFactor.h b/gtsam/slam/SmartProjectionPoseFactor.h new file mode 100644 index 000000000..7a5137bfc --- /dev/null +++ b/gtsam/slam/SmartProjectionPoseFactor.h @@ -0,0 +1,160 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file SmartProjectionPoseFactor.h + * @brief Produces an Hessian factors on POSES from monocular measurements of a single landmark + * @author Luca Carlone + * @author Chris Beall + * @author Zsolt Kira + */ + +#pragma once + +#include + +namespace gtsam { +/** + * + * @addtogroup SLAM + * + * If you are using the factor, please cite: + * L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating conditionally + * independent sets in factor graphs: a unifying perspective based on smart factors, + * Int. Conf. on Robotics and Automation (ICRA), 2014. + * + */ + +/** + * The calibration is known here. The factor only constraints poses (variable dimension is 6) + * @addtogroup SLAM + */ +template +class SmartProjectionPoseFactor: public SmartProjectionFactor< + PinholePose > { + +private: + typedef PinholePose Camera; + typedef SmartProjectionFactor Base; + typedef SmartProjectionPoseFactor This; + +protected: + + boost::optional body_P_sensor_; ///< Pose of the camera in the body frame + std::vector > sharedKs_; ///< shared pointer to calibration object (one for each camera) + +public: + + /// shorthand for a smart pointer to a factor + typedef boost::shared_ptr shared_ptr; + + /** + * Constructor + * @param rankTol tolerance used to check if point triangulation is degenerate + * @param linThreshold threshold on relative pose changes used to decide whether to relinearize (selective relinearization) + * @param manageDegeneracy is true, in presence of degenerate triangulation, the factor is converted to a rotation-only constraint, + * otherwise the factor is simply neglected (this functionality is deprecated) + * @param enableEPI if set to true linear triangulation is refined with embedded LM iterations + * @param body_P_sensor is the transform from sensor to body frame (default identity) + */ + SmartProjectionPoseFactor(const double rankTol = 1, + const double linThreshold = -1, const DegeneracyMode manageDegeneracy = IGNORE_DEGENERACY, + const bool enableEPI = false, boost::optional body_P_sensor = boost::none, + LinearizationMode linearizeTo = HESSIAN, double landmarkDistanceThreshold = 1e10, + double dynamicOutlierRejectionThreshold = -1) : + Base(linearizeTo, rankTol, manageDegeneracy, enableEPI, + landmarkDistanceThreshold, dynamicOutlierRejectionThreshold), body_P_sensor_(body_P_sensor) {} + + /** Virtual destructor */ + virtual ~SmartProjectionPoseFactor() {} + + /** + * print + * @param s optional string naming the factor + * @param keyFormatter optional formatter useful for printing Symbols + */ + void print(const std::string& s = "", const KeyFormatter& keyFormatter = + DefaultKeyFormatter) const { + std::cout << s << "SmartProjectionPoseFactor, z = \n "; + if(body_P_sensor_) + body_P_sensor_->print("body_P_sensor_:\n"); + Base::print("", keyFormatter); + } + + /// equals + virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const { + const This *e = dynamic_cast(&p); + return e && Base::equals(p, tol); + } + + /** + * Linearize to Gaussian Factor + * @param values Values structure which must contain camera poses for this factor + * @return + */ + virtual boost::shared_ptr linearize( + const Values& values) const { + // depending on flag set on construction we may linearize to different linear factors +// switch(linearizeTo_){ +// case JACOBIAN_SVD : +// return this->createJacobianSVDFactor(Base::cameras(values), 0.0); +// break; +// case JACOBIAN_Q : +// return this->createJacobianQFactor(Base::cameras(values), 0.0); +// break; +// default: + return this->createHessianFactor(Base::cameras(values)); +// break; +// } + } + + /** + * error calculates the error of the factor. + */ + virtual double error(const Values& values) const { + if (this->active(values)) { + return this->totalReprojectionError(Base::cameras(values)); + } else { // else of active flag + return 0.0; + } + } + + /** return calibration shared pointers */ + inline const std::vector > calibration() const { + return sharedKs_; + } + + Pose3 body_P_sensor() const{ + if(body_P_sensor_) + return *body_P_sensor_; + else + return Pose3(); // if unspecified, the transformation is the identity + } + +private: + + /// Serialization function + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar & BOOST_SERIALIZATION_NVP(sharedKs_); + } + +}; // end of class declaration + +/// traits +template +struct traits > : public Testable< + SmartProjectionPoseFactor > { +}; + +} // \ namespace gtsam diff --git a/gtsam/slam/tests/smartFactorScenarios.h b/gtsam/slam/tests/smartFactorScenarios.h index c3598e4c1..e821f9e40 100644 --- a/gtsam/slam/tests/smartFactorScenarios.h +++ b/gtsam/slam/tests/smartFactorScenarios.h @@ -17,6 +17,7 @@ */ #pragma once +#include #include #include #include @@ -66,7 +67,7 @@ typedef GeneralSFMFactor SFMFactor; // default Cal3_S2 poses namespace vanillaPose { typedef PinholePose Camera; -typedef SmartProjectionFactor SmartFactor; +typedef SmartProjectionPoseFactor SmartFactor; static Cal3_S2::shared_ptr sharedK(new Cal3_S2(fov, w, h)); Camera level_camera(level_pose, sharedK); Camera level_camera_right(pose_right, sharedK); @@ -108,7 +109,7 @@ typedef GeneralSFMFactor SFMFactor; // Cal3Bundler poses namespace bundlerPose { typedef PinholePose Camera; -typedef SmartProjectionFactor SmartFactor; +typedef SmartProjectionPoseFactor SmartFactor; static boost::shared_ptr sharedBundlerK( new Cal3Bundler(500, 1e-3, 1e-3, 1000, 2000)); Camera level_camera(level_pose, sharedBundlerK); diff --git a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp index 1e21a3afd..790e85dfb 100644 --- a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp @@ -59,7 +59,7 @@ TEST( SmartProjectionPoseFactor, Constructor) { /* ************************************************************************* */ TEST( SmartProjectionPoseFactor, Constructor2) { using namespace vanillaPose; - SmartFactor factor1(SmartFactor::HESSIAN, rankTol); + SmartFactor factor1(gtsam::HESSIAN, rankTol); } /* ************************************************************************* */ @@ -72,7 +72,7 @@ TEST( SmartProjectionPoseFactor, Constructor3) { /* ************************************************************************* */ TEST( SmartProjectionPoseFactor, Constructor4) { using namespace vanillaPose; - SmartFactor factor1(SmartFactor::HESSIAN, rankTol); + SmartFactor factor1(gtsam::HESSIAN, rankTol); factor1.add(measurement1, x1, model); } @@ -495,15 +495,15 @@ TEST( SmartProjectionPoseFactor, jacobianSVD ) { projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); SmartFactor::shared_ptr smartFactor1( - new SmartFactor(SmartFactor::JACOBIAN_SVD)); + new SmartFactor(gtsam::JACOBIAN_SVD)); smartFactor1->add(measurements_cam1, views, model); SmartFactor::shared_ptr smartFactor2( - new SmartFactor(SmartFactor::JACOBIAN_SVD)); + new SmartFactor(gtsam::JACOBIAN_SVD)); smartFactor2->add(measurements_cam2, views, model); SmartFactor::shared_ptr smartFactor3( - new SmartFactor(SmartFactor::JACOBIAN_SVD)); + new SmartFactor(gtsam::JACOBIAN_SVD)); smartFactor3->add(measurements_cam3, views, model); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -549,21 +549,18 @@ TEST( SmartProjectionPoseFactor, landmarkDistance ) { projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); SmartFactor::shared_ptr smartFactor1( - new SmartFactor(SmartFactor::JACOBIAN_SVD, 1, - SmartFactor::IGNORE_DEGENERACY, false, - excludeLandmarksFutherThanDist)); + new SmartFactor(1, -1, gtsam::IGNORE_DEGENERACY, false, Pose3(), + gtsam::JACOBIAN_SVD, excludeLandmarksFutherThanDist)); smartFactor1->add(measurements_cam1, views, model); SmartFactor::shared_ptr smartFactor2( - new SmartFactor(SmartFactor::JACOBIAN_SVD, 1, - SmartFactor::IGNORE_DEGENERACY, false, - excludeLandmarksFutherThanDist)); + new SmartFactor(1, -1, gtsam::IGNORE_DEGENERACY, false, Pose3(), + gtsam::JACOBIAN_SVD, excludeLandmarksFutherThanDist)); smartFactor2->add(measurements_cam2, views, model); SmartFactor::shared_ptr smartFactor3( - new SmartFactor(SmartFactor::JACOBIAN_SVD, 1, - SmartFactor::IGNORE_DEGENERACY, false, - excludeLandmarksFutherThanDist)); + new SmartFactor(1, -1, gtsam::IGNORE_DEGENERACY, false, Pose3(), + gtsam::JACOBIAN_SVD, excludeLandmarksFutherThanDist)); smartFactor3->add(measurements_cam3, views, model); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -617,27 +614,23 @@ TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ) { measurements_cam4.at(0) = measurements_cam4.at(0) + Point2(10, 10); // add outlier SmartFactor::shared_ptr smartFactor1( - new SmartFactor(SmartFactor::JACOBIAN_SVD, 1, - SmartFactor::IGNORE_DEGENERACY, false, excludeLandmarksFutherThanDist, - dynamicOutlierRejectionThreshold)); + new SmartFactor(1, -1, gtsam::IGNORE_DEGENERACY, false, Pose3(), + gtsam::JACOBIAN_SVD, excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); smartFactor1->add(measurements_cam1, views, model); SmartFactor::shared_ptr smartFactor2( - new SmartFactor(SmartFactor::JACOBIAN_SVD, 1, - SmartFactor::IGNORE_DEGENERACY, false, excludeLandmarksFutherThanDist, - dynamicOutlierRejectionThreshold)); + new SmartFactor(1, -1, gtsam::IGNORE_DEGENERACY, false, Pose3(), + gtsam::JACOBIAN_SVD, excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); smartFactor2->add(measurements_cam2, views, model); SmartFactor::shared_ptr smartFactor3( - new SmartFactor(SmartFactor::JACOBIAN_SVD, 1, - SmartFactor::IGNORE_DEGENERACY, false, excludeLandmarksFutherThanDist, - dynamicOutlierRejectionThreshold)); + new SmartFactor(1, -1, gtsam::IGNORE_DEGENERACY, false, Pose3(), + gtsam::JACOBIAN_SVD, excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); smartFactor3->add(measurements_cam3, views, model); SmartFactor::shared_ptr smartFactor4( - new SmartFactor(SmartFactor::JACOBIAN_SVD, 1, - SmartFactor::IGNORE_DEGENERACY, false, excludeLandmarksFutherThanDist, - dynamicOutlierRejectionThreshold)); + new SmartFactor(1, -1, gtsam::IGNORE_DEGENERACY, false, Pose3(), + gtsam::JACOBIAN_SVD, excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); smartFactor4->add(measurements_cam4, views, model); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -680,15 +673,15 @@ TEST( SmartProjectionPoseFactor, jacobianQ ) { projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); SmartFactor::shared_ptr smartFactor1( - new SmartFactor(SmartFactor::JACOBIAN_Q)); + new SmartFactor(gtsam::JACOBIAN_Q)); smartFactor1->add(measurements_cam1, views, model); SmartFactor::shared_ptr smartFactor2( - new SmartFactor(SmartFactor::JACOBIAN_Q)); + new SmartFactor(gtsam::JACOBIAN_Q)); smartFactor2->add(measurements_cam2, views, model); SmartFactor::shared_ptr smartFactor3( - new SmartFactor(SmartFactor::JACOBIAN_Q)); + new SmartFactor(gtsam::JACOBIAN_Q)); smartFactor3->add(measurements_cam3, views, model); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -799,15 +792,15 @@ TEST( SmartProjectionPoseFactor, CheckHessian) { double rankTol = 10; SmartFactor::shared_ptr smartFactor1( - new SmartFactor(SmartFactor::HESSIAN, rankTol)); + new SmartFactor(gtsam::HESSIAN, rankTol)); smartFactor1->add(measurements_cam1, views, model); SmartFactor::shared_ptr smartFactor2( - new SmartFactor(SmartFactor::HESSIAN, rankTol)); + new SmartFactor(gtsam::HESSIAN, rankTol)); smartFactor2->add(measurements_cam2, views, model); SmartFactor::shared_ptr smartFactor3( - new SmartFactor(SmartFactor::HESSIAN, rankTol)); + new SmartFactor(gtsam::HESSIAN, rankTol)); smartFactor3->add(measurements_cam3, views, model); NonlinearFactorGraph graph; @@ -880,13 +873,13 @@ TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_fac double rankTol = 50; SmartFactor::shared_ptr smartFactor1( - new SmartFactor(SmartFactor::HESSIAN, rankTol, - SmartFactor::HANDLE_INFINITY)); + new SmartFactor(gtsam::HESSIAN, rankTol, + gtsam::HANDLE_INFINITY)); smartFactor1->add(measurements_cam1, views, model); SmartFactor::shared_ptr smartFactor2( - new SmartFactor(SmartFactor::HESSIAN, rankTol, - SmartFactor::HANDLE_INFINITY)); + new SmartFactor(gtsam::HESSIAN, rankTol, + gtsam::HANDLE_INFINITY)); smartFactor2->add(measurements_cam2, views, model); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -961,18 +954,18 @@ TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ) double rankTol = 10; SmartFactor::shared_ptr smartFactor1( - new SmartFactor(SmartFactor::HESSIAN, rankTol, - SmartFactor::ZERO_ON_DEGENERACY)); + new SmartFactor(gtsam::HESSIAN, rankTol, + gtsam::ZERO_ON_DEGENERACY)); smartFactor1->add(measurements_cam1, views, model); SmartFactor::shared_ptr smartFactor2( - new SmartFactor(SmartFactor::HESSIAN, rankTol, - SmartFactor::ZERO_ON_DEGENERACY)); + new SmartFactor(gtsam::HESSIAN, rankTol, + gtsam::ZERO_ON_DEGENERACY)); smartFactor2->add(measurements_cam2, views, model); SmartFactor::shared_ptr smartFactor3( - new SmartFactor(SmartFactor::HESSIAN, rankTol, - SmartFactor::ZERO_ON_DEGENERACY)); + new SmartFactor(gtsam::HESSIAN, rankTol, + gtsam::ZERO_ON_DEGENERACY)); smartFactor3->add(measurements_cam3, views, model); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -1168,7 +1161,7 @@ TEST( SmartProjectionPoseFactor, HessianWithRotationDegenerate ) { /* ************************************************************************* */ TEST( SmartProjectionPoseFactor, ConstructorWithCal3Bundler) { using namespace bundlerPose; - SmartFactor factor(SmartFactor::HESSIAN, rankTol); + SmartFactor factor(gtsam::HESSIAN, rankTol); factor.add(measurement1, x1, model); } @@ -1262,18 +1255,18 @@ TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) { double rankTol = 10; SmartFactor::shared_ptr smartFactor1( - new SmartFactor(SmartFactor::HESSIAN, rankTol, - SmartFactor::ZERO_ON_DEGENERACY)); + new SmartFactor(rankTol, -1.0, gtsam::ZERO_ON_DEGENERACY, + false, Pose3(), gtsam::HESSIAN)); smartFactor1->add(measurements_cam1, views, model); SmartFactor::shared_ptr smartFactor2( - new SmartFactor(SmartFactor::HESSIAN, rankTol, - SmartFactor::ZERO_ON_DEGENERACY)); + new SmartFactor(rankTol, -1, gtsam::ZERO_ON_DEGENERACY, + false, Pose3(), gtsam::HESSIAN)); smartFactor2->add(measurements_cam2, views, model); SmartFactor::shared_ptr smartFactor3( - new SmartFactor(SmartFactor::HESSIAN, rankTol, - SmartFactor::ZERO_ON_DEGENERACY)); + new SmartFactor(rankTol, -1, gtsam::ZERO_ON_DEGENERACY, + false, Pose3(), gtsam::HESSIAN)); smartFactor3->add(measurements_cam3, views, model); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);