Remove selective relinearization and state

gtsam_unstable/slam/SmartStereoProjectionFactor.h

@@ -33,33 +33,6 @@
 
 namespace gtsam {
 
-/**
- * Structure for storing some state memory, used to speed up optimization
- * @addtogroup SLAM
- */
-class SmartStereoProjectionFactorState {
-
-protected:
-
-public:
-
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-
-  SmartStereoProjectionFactorState() {
-  }
-  // Hessian representation (after Schur complement)
-  bool calculatedHessian;
-  Matrix H;
-  Vector gs_vector;
-  std::vector<Matrix> Gs;
-  std::vector<Vector> gs;
-  double f;
-};
-
-enum LinearizationMode {
-  HESSIAN, JACOBIAN_SVD, JACOBIAN_Q
-};
-
 /**
  * SmartStereoProjectionFactor: triangulates point
  */
@@ -84,14 +57,6 @@ protected:
   mutable bool degenerate_;
   mutable bool cheiralityException_;
 
-  // verbosity handling for Cheirality Exceptions
-  const bool throwCheirality_; ///< If true, rethrows Cheirality exceptions (default: false)
-  const bool verboseCheirality_; ///< If true, prints text for Cheirality exceptions (default: false)
-
-  boost::shared_ptr<SmartStereoProjectionFactorState> state_;
-
-  /// shorthand for smart projection factor state variable
-  typedef boost::shared_ptr<SmartStereoProjectionFactorState> SmartFactorStatePtr;
 
   /// shorthand for base class type
   typedef SmartFactorBase<StereoCamera> Base;
@@ -110,6 +75,12 @@ protected:
     ZDim = 3
   }; ///< Dimension trait of measurement type
 
+  /// @name Parameters governing how triangulation result is treated
+  /// @{
+   const bool throwCheirality_; ///< If true, rethrows Cheirality exceptions (default: false)
+   const bool verboseCheirality_; ///< If true, prints text for Cheirality exceptions (default: false)
+   /// @}
+
 public:
 
   /// shorthand for a smart pointer to a factor
@@ -133,12 +104,11 @@ public:
   SmartStereoProjectionFactor(const double rankTol, const double linThreshold,
       const bool manageDegeneracy, const bool enableEPI,
       double landmarkDistanceThreshold = 1e10,
-      double dynamicOutlierRejectionThreshold = -1, SmartFactorStatePtr state =
+      double dynamicOutlierRejectionThreshold = -1) :
-          SmartFactorStatePtr(new SmartStereoProjectionFactorState())) :
       rankTolerance_(rankTol), retriangulationThreshold_(1e-5), manageDegeneracy_(
           manageDegeneracy), enableEPI_(enableEPI), linearizationThreshold_(
           linThreshold), degenerate_(false), cheiralityException_(false), throwCheirality_(
-          false), verboseCheirality_(false), state_(state), landmarkDistanceThreshold_(
+          false), verboseCheirality_(false), landmarkDistanceThreshold_(
           landmarkDistanceThreshold), dynamicOutlierRejectionThreshold_(
           dynamicOutlierRejectionThreshold) {
   }
@@ -199,40 +169,6 @@ public:
     return retriangulate; // if we arrive to this point_ all poses are the same and we don't need re-triangulation
   }
 
-  /// This function checks if the new linearization point_ is 'close'  to the previous one used for linearization
-  bool decideIfLinearize(const Cameras& cameras) const {
-    // "selective linearization"
-    // The function evaluates how close are the old and the new poses, transformed in the ref frame of the first pose
-    // (we only care about the "rigidity" of the poses, not about their absolute pose)
-
-    if (this->linearizationThreshold_ < 0) //by convention if linearizationThreshold is negative we always relinearize
-      return true;
-
-    // if we do not have a previous linearization point_ or the new linearization point_ includes more poses
-    if (cameraPosesLinearization_.empty()
-        || (cameras.size() != cameraPosesLinearization_.size()))
-      return true;
-
-    Pose3 firstCameraPose, firstCameraPoseOld;
-    for (size_t i = 0; i < cameras.size(); i++) {
-
-      if (i == 0) { // we store the initial pose, this is useful for selective re-linearization
-        firstCameraPose = cameras[i].pose();
-        firstCameraPoseOld = cameraPosesLinearization_[i];
-        continue;
-      }
-
-      // we compare the poses in the frame of the first pose
-      Pose3 localCameraPose = firstCameraPose.between(cameras[i].pose());
-      Pose3 localCameraPoseOld = firstCameraPoseOld.between(
-          cameraPosesLinearization_[i]);
-      if (!localCameraPose.equals(localCameraPoseOld,
-          this->linearizationThreshold_))
-        return true; // at least two "relative" poses are different, hence we re-linearize
-    }
-    return false; // if we arrive to this point_ all poses are the same and we don't need re-linearize
-  }
-
   /// triangulateSafe
   size_t triangulateSafe(const Values& values) const {
     return triangulateSafe(this->cameras(values));
@@ -364,7 +300,7 @@ public:
       exit(1);
     }
 
-    this->triangulateSafe(cameras);
+    triangulateSafe(cameras);
     if (isDebug)
       std::cout << "point_ = " << point_ << std::endl;
 
@@ -388,29 +324,10 @@ public:
         std::cout << "degenerate_ = true" << std::endl;
     }
 
-    bool doLinearize = this->decideIfLinearize(cameras);
+    if (this->linearizationThreshold_ >= 0) // if we apply selective relinearization and we need to relinearize
-
-    if (isDebug)
-      std::cout << "doLinearize = " << doLinearize << std::endl;
-
-    if (this->linearizationThreshold_ >= 0 && doLinearize) // if we apply selective relinearization and we need to relinearize
       for (size_t i = 0; i < cameras.size(); i++)
         this->cameraPosesLinearization_[i] = cameras[i].pose();
 
-    if (!doLinearize) { // return the previous Hessian factor
-      std::cout << "=============================" << std::endl;
-      std::cout << "doLinearize " << doLinearize << std::endl;
-      std::cout << "this->linearizationThreshold_ "
-          << this->linearizationThreshold_ << std::endl;
-      std::cout << "this->degenerate_ " << this->degenerate_ << std::endl;
-      std::cout
-          << "something wrong in SmartProjectionHessianFactor: selective relinearization should be disabled"
-          << std::endl;
-      exit(1);
-      return boost::make_shared<RegularHessianFactor<Base::Dim> >(this->keys_,
-          this->state_->Gs, this->state_->gs, this->state_->f);
-    }
-
     // ==================================================================
     std::vector<typename Base::MatrixZD> Fblocks;
     Matrix F, E;
@@ -447,11 +364,6 @@ public:
     }
     // ==================================================================
     double f = b.squaredNorm();
-    if (this->linearizationThreshold_ >= 0) { // if we do not use selective relinearization we don't need to store these variables
-      this->state_->Gs = Gs;
-      this->state_->gs = gs;
-      this->state_->f = f;
-    }
     return boost::make_shared<RegularHessianFactor<Base::Dim> >(this->keys_, Gs, gs, f);
   }
 

gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h

@@ -40,6 +40,13 @@ namespace gtsam {
  */
 template<class CALIBRATION>
 class SmartStereoProjectionPoseFactor: public SmartStereoProjectionFactor<CALIBRATION> {
+
+public:
+  /// Linearization mode: what factor to linearize to
+   enum LinearizationMode {
+     HESSIAN, IMPLICIT_SCHUR, JACOBIAN_Q, JACOBIAN_SVD
+   };
+
 protected:
 
   LinearizationMode linearizeTo_;  ///< How to linearize the factor (HESSIAN, JACOBIAN_SVD, JACOBIAN_Q)

gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp

@@ -64,7 +64,6 @@ static Pose3 body_P_sensor1(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2),
     Point3(0.25, -0.10, 1.0));
 
 typedef SmartStereoProjectionPoseFactor<Cal3_S2Stereo> SmartFactor;
-typedef SmartStereoProjectionPoseFactor<Cal3Bundler> SmartFactorBundler;
 
 vector<StereoPoint2> stereo_projectToMultipleCameras(const StereoCamera& cam1,
     const StereoCamera& cam2, const StereoCamera& cam3, Point3 landmark) {
@@ -327,15 +326,15 @@ TEST( SmartStereoProjectionPoseFactor, jacobianSVD ) {
       cam2, cam3, landmark3);
 
   SmartFactor::shared_ptr smartFactor1(
-      new SmartFactor(1, -1, false, false, JACOBIAN_SVD));
+      new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD));
   smartFactor1->add(measurements_cam1, views, model, K);
 
   SmartFactor::shared_ptr smartFactor2(
-      new SmartFactor(1, -1, false, false, JACOBIAN_SVD));
+      new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD));
   smartFactor2->add(measurements_cam2, views, model, K);
 
   SmartFactor::shared_ptr smartFactor3(
-      new SmartFactor(1, -1, false, false, JACOBIAN_SVD));
+      new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD));
   smartFactor3->add(measurements_cam3, views, model, K);
 
   const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@@ -395,17 +394,17 @@ TEST( SmartStereoProjectionPoseFactor, landmarkDistance ) {
       cam2, cam3, landmark3);
 
   SmartFactor::shared_ptr smartFactor1(
-      new SmartFactor(1, -1, false, false, JACOBIAN_SVD,
+      new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD,
           excludeLandmarksFutherThanDist));
   smartFactor1->add(measurements_cam1, views, model, K);
 
   SmartFactor::shared_ptr smartFactor2(
-      new SmartFactor(1, -1, false, false, JACOBIAN_SVD,
+      new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD,
           excludeLandmarksFutherThanDist));
   smartFactor2->add(measurements_cam2, views, model, K);
 
   SmartFactor::shared_ptr smartFactor3(
-      new SmartFactor(1, -1, false, false, JACOBIAN_SVD,
+      new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD,
           excludeLandmarksFutherThanDist));
   smartFactor3->add(measurements_cam3, views, model, K);
 
@@ -473,22 +472,22 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) {
   measurements_cam4.at(0) = measurements_cam4.at(0) + StereoPoint2(10, 10, 1); // add outlier
 
   SmartFactor::shared_ptr smartFactor1(
-      new SmartFactor(1, -1, false, false, JACOBIAN_SVD,
+      new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD,
           excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold));
   smartFactor1->add(measurements_cam1, views, model, K);
 
   SmartFactor::shared_ptr smartFactor2(
-      new SmartFactor(1, -1, false, false, JACOBIAN_SVD,
+      new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD,
           excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold));
   smartFactor2->add(measurements_cam2, views, model, K);
 
   SmartFactor::shared_ptr smartFactor3(
-      new SmartFactor(1, -1, false, false, JACOBIAN_SVD,
+      new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD,
           excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold));
   smartFactor3->add(measurements_cam3, views, model, K);
 
   SmartFactor::shared_ptr smartFactor4(
-      new SmartFactor(1, -1, false, false, JACOBIAN_SVD,
+      new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD,
           excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold));
   smartFactor4->add(measurements_cam4, views, model, K);