DegeneracyMode

gtsam/slam/SmartProjectionFactor.h

@@ -44,13 +44,19 @@ public:
     HESSIAN, IMPLICIT_SCHUR, JACOBIAN_Q, JACOBIAN_SVD
   };
 
+  /// How to manage degeneracy
+  enum DegeneracyMode {
+    IGNORE_DEGENERACY, ZERO_ON_DEGENERACY, HANDLE_INFINITY
+  };
+
 private:
   typedef SmartFactorBase<CAMERA> Base;
   typedef SmartProjectionFactor<CAMERA> This;
 
 protected:
 
-  LinearizationMode linearizeTo_; ///< How to linearize the factor
+  LinearizationMode linearizationMode_; ///< How to linearize the factor
+  DegeneracyMode degeneracyMode_; ///< How to linearize the factor
 
   /// @name Caching triangulation
   /// @{
@@ -63,7 +69,6 @@ protected:
 
   /// @name Parameters governing how triangulation result is treated
   /// @{
-  const bool manageDegeneracy_; ///< if set to true will use the rotation-only version for degenerate cases
   const bool throwCheirality_; ///< If true, rethrows Cheirality exceptions (default: false)
   const bool verboseCheirality_; ///< If true, prints text for Cheirality exceptions (default: false)
   /// @}
@@ -85,13 +90,16 @@ public:
    * @param enableEPI if set to true linear triangulation is refined with embedded LM iterations
    */
   SmartProjectionFactor(LinearizationMode linearizationMode = HESSIAN,
-      double rankTolerance = 1, bool manageDegeneracy = false, bool enableEPI =
+      double rankTolerance = 1, DegeneracyMode degeneracyMode =
-          false, double landmarkDistanceThreshold = 1e10,
+          IGNORE_DEGENERACY, bool enableEPI = false,
+      double landmarkDistanceThreshold = 1e10,
       double dynamicOutlierRejectionThreshold = -1) :
-      linearizeTo_(linearizationMode), parameters_(rankTolerance, enableEPI,
+      linearizationMode_(linearizationMode), //
-          landmarkDistanceThreshold, dynamicOutlierRejectionThreshold), //
+      degeneracyMode_(degeneracyMode), //
+      parameters_(rankTolerance, enableEPI, landmarkDistanceThreshold,
+          dynamicOutlierRejectionThreshold), //
       result_(TriangulationResult::Degenerate()), //
-      retriangulationThreshold_(1e-5), manageDegeneracy_(manageDegeneracy), //
+      retriangulationThreshold_(1e-5), //
       throwCheirality_(false), verboseCheirality_(false) {
   }
 
@@ -107,7 +115,7 @@ public:
   void print(const std::string& s = "", const KeyFormatter& keyFormatter =
       DefaultKeyFormatter) const {
     std::cout << s << "SmartProjectionFactor\n";
-    std::cout << "linearizationMode:\n" << linearizeTo_ << std::endl;
+    std::cout << "linearizationMode:\n" << linearizationMode_ << std::endl;
     std::cout << "triangulationParameters:\n" << parameters_ << std::endl;
     std::cout << "result:\n" << result_ << std::endl;
     Base::print("", keyFormatter);
@@ -116,7 +124,8 @@ public:
   /// equals
   virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const {
     const This *e = dynamic_cast<const This*>(&p);
-    return e && linearizeTo_ == e->linearizeTo_ && Base::equals(p, tol);
+    return e && linearizationMode_ == e->linearizationMode_
+        && Base::equals(p, tol);
   }
 
   /// Check if the new linearization point is the same as the one used for previous triangulation
@@ -194,7 +203,7 @@ public:
 
     triangulateSafe(cameras);
 
-    if (!manageDegeneracy_ && !result_) {
+    if (degeneracyMode_ == ZERO_ON_DEGENERACY && !result_) {
       // put in "empty" Hessian
       BOOST_FOREACH(Matrix& m, Gs)
         m = zeros(Base::Dim, Base::Dim);
@@ -281,7 +290,7 @@ public:
       const double lambda = 0.0) const {
     // depending on flag set on construction we may linearize to different linear factors
     Cameras cameras = this->cameras(values);
-    switch (linearizeTo_) {
+    switch (linearizationMode_) {
     case HESSIAN:
       return createHessianFactor(cameras, lambda);
     case IMPLICIT_SCHUR:
@@ -381,7 +390,7 @@ public:
     if (result_)
       // All good, just use version in base class
       return Base::totalReprojectionError(cameras, *result_);
-    else if (manageDegeneracy_) {
+    else if (degeneracyMode_ == HANDLE_INFINITY) {
       // Otherwise, manage the exceptions with rotation-only factors
       const Point2& z0 = this->measured_.at(0);
       Unit3 backprojected = cameras.front().backprojectPointAtInfinity(z0);

gtsam/slam/tests/testSmartProjectionCameraFactor.cpp

@@ -31,7 +31,6 @@ using namespace boost::assign;
 static bool isDebugTest = false;
 
 static double rankTol = 1.0;
-static double linThreshold = -1.0;
 
 // Convenience for named keys
 using symbol_shorthand::X;
@@ -79,7 +78,7 @@ TEST( SmartProjectionCameraFactor, Constructor) {
 /* ************************************************************************* */
 TEST( SmartProjectionCameraFactor, Constructor2) {
   using namespace vanilla;
-  SmartFactor factor1(SmartFactor::HESSIAN, rankTol, linThreshold);
+  SmartFactor factor1(SmartFactor::HESSIAN, rankTol);
 }
 
 /* ************************************************************************* */
@@ -92,7 +91,7 @@ TEST( SmartProjectionCameraFactor, Constructor3) {
 /* ************************************************************************* */
 TEST( SmartProjectionCameraFactor, Constructor4) {
   using namespace vanilla;
-  SmartFactor factor1(SmartFactor::HESSIAN, rankTol, linThreshold);
+  SmartFactor factor1(SmartFactor::HESSIAN, rankTol);
   factor1.add(measurement1, x1, unit2);
 }
 
@@ -108,7 +107,6 @@ TEST( SmartProjectionCameraFactor, Equals ) {
 
 /* *************************************************************************/
 TEST( SmartProjectionCameraFactor, noiseless ) {
-  // cout << " ************************ SmartProjectionCameraFactor: noisy ****************************" << endl;
   using namespace vanilla;
 
   Values values;
@@ -153,7 +151,8 @@ TEST( SmartProjectionCameraFactor, noisy ) {
 
   // Check triangulated point
   CHECK(factor1->point());
-  EXPECT(assert_equal(Point3(13.7587, 1.43851, -1.14274),*factor1->point(), 1e-4));
+  EXPECT(
+      assert_equal(Point3(13.7587, 1.43851, -1.14274), *factor1->point(), 1e-4));
 
   // Check whitened errors
   Vector expected(4);
@@ -236,16 +235,23 @@ TEST( SmartProjectionCameraFactor, perturbPoseAndOptimize ) {
   CHECK(smartFactor2->point());
   CHECK(smartFactor3->point());
 
-  EXPECT(assert_equal(Point3(2.57696, -0.182566, 1.04085),*smartFactor1->point(), 1e-4));
+  EXPECT(
-  EXPECT(assert_equal(Point3(2.80114, -0.702153, 1.06594),*smartFactor2->point(), 1e-4));
+      assert_equal(Point3(2.57696, -0.182566, 1.04085), *smartFactor1->point(),
-  EXPECT(assert_equal(Point3(1.82593, -0.289569, 2.13438),*smartFactor3->point(), 1e-4));
+          1e-4));
+  EXPECT(
+      assert_equal(Point3(2.80114, -0.702153, 1.06594), *smartFactor2->point(),
+          1e-4));
+  EXPECT(
+      assert_equal(Point3(1.82593, -0.289569, 2.13438), *smartFactor3->point(),
+          1e-4));
 
   // Check whitened errors
   Vector expected(6);
   expected << 256, 29, -26, 29, -206, -202;
   Point3 point1 = *smartFactor1->point();
   SmartFactor::Cameras cameras1 = smartFactor1->cameras(initial);
-  Vector reprojectionError = cameras1.reprojectionError(point1, measurements_cam1);
+  Vector reprojectionError = cameras1.reprojectionError(point1,
+      measurements_cam1);
   EXPECT(assert_equal(expected, reprojectionError, 1));
   Vector actual = smartFactor1->whitenedError(cameras1, point1);
   EXPECT(assert_equal(expected, actual, 1));
@@ -796,15 +802,12 @@ TEST( SmartProjectionCameraFactor, implicitJacobianFactor ) {
   values.insert(c1, level_camera);
   values.insert(c2, level_camera_right);
   double rankTol = 1;
-  double linThreshold = -1;
-  bool manageDegeneracy = false;
   bool useEPI = false;
-  bool isImplicit = false;
 
   // Hessian version
   SmartFactor::shared_ptr explicitFactor(
-      new SmartFactor(SmartFactor::HESSIAN, rankTol, linThreshold, manageDegeneracy, useEPI,
+      new SmartFactor(SmartFactor::HESSIAN, rankTol,
-          isImplicit));
+          SmartFactor::IGNORE_DEGENERACY, useEPI));
   explicitFactor->add(level_uv, c1, unit2);
   explicitFactor->add(level_uv_right, c2, unit2);
 
@@ -814,10 +817,9 @@ TEST( SmartProjectionCameraFactor, implicitJacobianFactor ) {
       dynamic_cast<HessianFactor&>(*gaussianHessianFactor);
 
   // Implicit Schur version
-  isImplicit = true;
   SmartFactor::shared_ptr implicitFactor(
-      new SmartFactor(SmartFactor::IMPLICIT_SCHUR, rankTol, linThreshold, manageDegeneracy, useEPI,
+      new SmartFactor(SmartFactor::IMPLICIT_SCHUR, rankTol,
-          isImplicit));
+          SmartFactor::IGNORE_DEGENERACY, useEPI));
   implicitFactor->add(level_uv, c1, unit2);
   implicitFactor->add(level_uv_right, c2, unit2);
   GaussianFactor::shared_ptr gaussianImplicitSchurFactor =

gtsam/slam/tests/testSmartProjectionPoseFactor.cpp

@@ -31,8 +31,6 @@
 using namespace boost::assign;
 
 static const double rankTol = 1.0;
-static const bool manageDegeneracy = true;
-
 // Create a noise model for the pixel error
 static const double sigma = 0.1;
 static SharedIsotropic model(noiseModel::Isotropic::Sigma(2, sigma));
@@ -501,15 +499,15 @@ TEST( SmartProjectionPoseFactor, jacobianSVD ) {
   projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
 
   SmartFactor::shared_ptr smartFactor1(
-      new SmartFactor(SmartFactor::JACOBIAN_SVD, 1, false, false));
+      new SmartFactor(SmartFactor::JACOBIAN_SVD));
   smartFactor1->add(measurements_cam1, views, model);
 
   SmartFactor::shared_ptr smartFactor2(
-      new SmartFactor(SmartFactor::JACOBIAN_SVD, 1, false, false));
+      new SmartFactor(SmartFactor::JACOBIAN_SVD));
   smartFactor2->add(measurements_cam2, views, model);
 
   SmartFactor::shared_ptr smartFactor3(
-      new SmartFactor(SmartFactor::JACOBIAN_SVD, 1, false, false));
+      new SmartFactor(SmartFactor::JACOBIAN_SVD));
   smartFactor3->add(measurements_cam3, views, model);
 
   const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@@ -530,7 +528,6 @@ TEST( SmartProjectionPoseFactor, jacobianSVD ) {
   values.insert(x3, Camera(pose_above * noise_pose, sharedK));
 
   Values result;
-  params.verbosityLM = LevenbergMarquardtParams::SUMMARY;
   LevenbergMarquardtOptimizer optimizer(graph, values, params);
   result = optimizer.optimize();
   EXPECT(assert_equal(pose_above, result.at<Camera>(x3).pose(), 1e-8));
@@ -556,17 +553,20 @@ TEST( SmartProjectionPoseFactor, landmarkDistance ) {
   projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
 
   SmartFactor::shared_ptr smartFactor1(
-      new SmartFactor(SmartFactor::JACOBIAN_SVD, 1, -1, false, false,
+      new SmartFactor(SmartFactor::JACOBIAN_SVD, 1,
+          SmartFactor::IGNORE_DEGENERACY, false,
           excludeLandmarksFutherThanDist));
   smartFactor1->add(measurements_cam1, views, model);
 
   SmartFactor::shared_ptr smartFactor2(
-      new SmartFactor(SmartFactor::JACOBIAN_SVD, 1, -1, false, false,
+      new SmartFactor(SmartFactor::JACOBIAN_SVD, 1,
+          SmartFactor::IGNORE_DEGENERACY, false,
           excludeLandmarksFutherThanDist));
   smartFactor2->add(measurements_cam2, views, model);
 
   SmartFactor::shared_ptr smartFactor3(
-      new SmartFactor(SmartFactor::JACOBIAN_SVD, 1, -1, false, false,
+      new SmartFactor(SmartFactor::JACOBIAN_SVD, 1,
+          SmartFactor::IGNORE_DEGENERACY, false,
           excludeLandmarksFutherThanDist));
   smartFactor3->add(measurements_cam3, views, model);
 
@@ -621,23 +621,27 @@ 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, false, false,
+      new SmartFactor(SmartFactor::JACOBIAN_SVD, 1,
-          excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold));
+          SmartFactor::IGNORE_DEGENERACY, false, excludeLandmarksFutherThanDist,
+          dynamicOutlierRejectionThreshold));
   smartFactor1->add(measurements_cam1, views, model);
 
   SmartFactor::shared_ptr smartFactor2(
-      new SmartFactor(SmartFactor::JACOBIAN_SVD, 1, false, false,
+      new SmartFactor(SmartFactor::JACOBIAN_SVD, 1,
-          excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold));
+          SmartFactor::IGNORE_DEGENERACY, false, excludeLandmarksFutherThanDist,
+          dynamicOutlierRejectionThreshold));
   smartFactor2->add(measurements_cam2, views, model);
 
   SmartFactor::shared_ptr smartFactor3(
-      new SmartFactor(SmartFactor::JACOBIAN_SVD, 1, false, false,
+      new SmartFactor(SmartFactor::JACOBIAN_SVD, 1,
-          excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold));
+          SmartFactor::IGNORE_DEGENERACY, false, excludeLandmarksFutherThanDist,
+          dynamicOutlierRejectionThreshold));
   smartFactor3->add(measurements_cam3, views, model);
 
   SmartFactor::shared_ptr smartFactor4(
-      new SmartFactor(SmartFactor::JACOBIAN_SVD, 1, false, false,
+      new SmartFactor(SmartFactor::JACOBIAN_SVD, 1,
-          excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold));
+          SmartFactor::IGNORE_DEGENERACY, false, excludeLandmarksFutherThanDist,
+          dynamicOutlierRejectionThreshold));
   smartFactor4->add(measurements_cam4, views, model);
 
   const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@@ -680,15 +684,15 @@ TEST( SmartProjectionPoseFactor, jacobianQ ) {
   projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
 
   SmartFactor::shared_ptr smartFactor1(
-      new SmartFactor(SmartFactor::JACOBIAN_Q, 1, false, false));
+      new SmartFactor(SmartFactor::JACOBIAN_Q));
   smartFactor1->add(measurements_cam1, views, model);
 
   SmartFactor::shared_ptr smartFactor2(
-      new SmartFactor(SmartFactor::JACOBIAN_Q, 1, false, false));
+      new SmartFactor(SmartFactor::JACOBIAN_Q));
   smartFactor2->add(measurements_cam2, views, model);
 
   SmartFactor::shared_ptr smartFactor3(
-      new SmartFactor(SmartFactor::JACOBIAN_Q, 1, false, false));
+      new SmartFactor(SmartFactor::JACOBIAN_Q));
   smartFactor3->add(measurements_cam3, views, model);
 
   const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@@ -885,11 +889,13 @@ TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_fac
 
   double rankTol = 50;
   SmartFactor::shared_ptr smartFactor1(
-      new SmartFactor(SmartFactor::HESSIAN, rankTol, manageDegeneracy));
+      new SmartFactor(SmartFactor::HESSIAN, rankTol,
+          SmartFactor::ZERO_ON_DEGENERACY));
   smartFactor1->add(measurements_cam1, views, model);
 
   SmartFactor::shared_ptr smartFactor2(
-      new SmartFactor(SmartFactor::HESSIAN, rankTol, manageDegeneracy));
+      new SmartFactor(SmartFactor::HESSIAN, rankTol,
+          SmartFactor::ZERO_ON_DEGENERACY));
   smartFactor2->add(measurements_cam2, views, model);
 
   const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@@ -923,6 +929,7 @@ TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_fac
           values.at<Camera>(x3).pose()));
 
   Values result;
+  params.verbosityLM = LevenbergMarquardtParams::SUMMARY;
   LevenbergMarquardtOptimizer optimizer(graph, values, params);
   result = optimizer.optimize();
 
@@ -964,15 +971,18 @@ TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor )
   double rankTol = 10;
 
   SmartFactor::shared_ptr smartFactor1(
-      new SmartFactor(SmartFactor::HESSIAN, rankTol, manageDegeneracy));
+      new SmartFactor(SmartFactor::HESSIAN, rankTol,
+          SmartFactor::ZERO_ON_DEGENERACY));
   smartFactor1->add(measurements_cam1, views, model);
 
   SmartFactor::shared_ptr smartFactor2(
-      new SmartFactor(SmartFactor::HESSIAN, rankTol, manageDegeneracy));
+      new SmartFactor(SmartFactor::HESSIAN, rankTol,
+          SmartFactor::ZERO_ON_DEGENERACY));
   smartFactor2->add(measurements_cam2, views, model);
 
   SmartFactor::shared_ptr smartFactor3(
-      new SmartFactor(SmartFactor::HESSIAN, rankTol, manageDegeneracy));
+      new SmartFactor(SmartFactor::HESSIAN, rankTol,
+          SmartFactor::ZERO_ON_DEGENERACY));
   smartFactor3->add(measurements_cam3, views, model);
 
   const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@@ -1265,15 +1275,18 @@ TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) {
   double rankTol = 10;
 
   SmartFactor::shared_ptr smartFactor1(
-      new SmartFactor(SmartFactor::HESSIAN, rankTol, manageDegeneracy));
+      new SmartFactor(SmartFactor::HESSIAN, rankTol,
+          SmartFactor::ZERO_ON_DEGENERACY));
   smartFactor1->add(measurements_cam1, views, model);
 
   SmartFactor::shared_ptr smartFactor2(
-      new SmartFactor(SmartFactor::HESSIAN, rankTol, manageDegeneracy));
+      new SmartFactor(SmartFactor::HESSIAN, rankTol,
+          SmartFactor::ZERO_ON_DEGENERACY));
   smartFactor2->add(measurements_cam2, views, model);
 
   SmartFactor::shared_ptr smartFactor3(
-      new SmartFactor(SmartFactor::HESSIAN, rankTol, manageDegeneracy));
+      new SmartFactor(SmartFactor::HESSIAN, rankTol,
+          SmartFactor::ZERO_ON_DEGENERACY));
   smartFactor3->add(measurements_cam3, views, model);
 
   const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);