Selective relinearization

gtsam_unstable/examples/SmartProjectionFactorExample_kitti.cpp

@@ -264,7 +264,7 @@ int main(int argc, char** argv) {
       }
 
       if (useSmartProjectionFactor) {
-        SmartFactor::shared_ptr smartFactor(new SmartFactor(measurements, pixel_sigma, views, K));
+        SmartFactor::shared_ptr smartFactor(new SmartFactor(views, measurements, pixel_sigma, K));
         graph.push_back(smartFactor);
       }
 
@@ -290,7 +290,7 @@ int main(int argc, char** argv) {
   }
   // Add last measurements
   if (useSmartProjectionFactor) {
-    SmartFactor::shared_ptr smartFactor(new SmartFactor(measurements, pixel_sigma, views, K));
+    SmartFactor::shared_ptr smartFactor(new SmartFactor(views, measurements, pixel_sigma, K));
     graph.push_back(smartFactor);
   }
 

gtsam_unstable/examples/SmartProjectionFactorExample_kitti_nonbatch.cpp

@@ -437,7 +437,7 @@ int main(int argc, char** argv) {
 
         // This is a new landmark, create a new factor and add to mapping
         boost::shared_ptr<SmartProjectionFactorState> smartFactorState(new SmartProjectionFactorState());
-        SmartFactor::shared_ptr smartFactor(new SmartFactor(measurements, pixel_sigma, views, K));
+        SmartFactor::shared_ptr smartFactor(new SmartFactor(views, measurements, pixel_sigma,  K));
         smartFactorStates.insert( make_pair(L(l), smartFactorState) );
         smartFactors.insert( make_pair(L(l), smartFactor) );
         graph.push_back(smartFactor);

gtsam_unstable/slam/SmartProjectionFactor.h

@@ -59,28 +59,24 @@ namespace gtsam {
     double overallError;
     std::vector<Pose3> cameraPosesError;
 
-    // Hessian
+    // Hessian representation (after Schur complement)
     bool calculatedHessian;
     Matrix H;
     Vector gs_vector;
-    double f;
-
     std::vector<Matrix> Gs;
     std::vector<Vector> gs;
+    double f;
 
+    // Jacobian representation (before Schur complement)
     Matrix Hx;
     Matrix Hl;
     Vector b;
 
     // C = Hl'Hl
-
     // Cinv = inv(Hl'Hl)
     // Matrix3 Cinv;
-
     // E = Hx'Hl
-
     // w = Hl'b
-
   };
 
   int SmartProjectionFactorState::lastID = 0;
@@ -105,6 +101,7 @@ namespace gtsam {
     bool throwCheirality_; ///< If true, rethrows Cheirality exceptions (default: false)
     bool verboseCheirality_; ///< If true, prints text for Cheirality exceptions (default: false)
 
+
   public:
 
     /// shorthand for base class type
@@ -124,15 +121,16 @@ namespace gtsam {
 
     /**
      * Constructor
-     * TODO: Mark argument order standard (keys, measurement, parameters)
+     * @param poseKeys is the set of indices corresponding to the cameras observing the same landmark
      * @param measured is the 2m dimensional location of the projection of a single landmark in the m views (the measurements)
      * @param model is the standard deviation (current version assumes that the uncertainty is the same for all views)
-     * @param poseKeys is the set of indices corresponding to the cameras observing the same landmark
      * @param K shared pointer to the constant calibration
      * @param body_P_sensor is the transform from body to sensor frame (default identity)
      */
-    SmartProjectionFactor(const std::vector<Point2> measured, const SharedNoiseModel& model,
+    SmartProjectionFactor(std::vector<Key> poseKeys, // camera poses
-        std::vector<Key> poseKeys, const boost::shared_ptr<CALIBRATION>& K,
+        const std::vector<Point2> measured,          // pixel measurements
+        const SharedNoiseModel& model,               // noise model (same for all measurements)
+        const boost::shared_ptr<CALIBRATION>& K,     // calibration matrix (same for all measurements)
         boost::optional<POSE> body_P_sensor = boost::none,
         SmartFactorStatePtr state = SmartFactorStatePtr(new SmartProjectionFactorState())) :
           measured_(measured), noise_(model), K_(K), body_P_sensor_(body_P_sensor),
@@ -151,14 +149,15 @@ namespace gtsam {
      * @param verboseCheirality determines whether exceptions are printed for Cheirality
      * @param body_P_sensor is the transform from body to sensor frame  (default identity)
      */
-    SmartProjectionFactor(const std::vector<Point2> measured, const SharedNoiseModel& model,
+    SmartProjectionFactor(std::vector<Key> poseKeys,
-        std::vector<Key> poseKeys, const boost::shared_ptr<CALIBRATION>& K,
+        const std::vector<Point2> measured,
+        const SharedNoiseModel& model,
+        const boost::shared_ptr<CALIBRATION>& K,
         bool throwCheirality, bool verboseCheirality,
         boost::optional<POSE> body_P_sensor = boost::none,
         SmartFactorStatePtr state = SmartFactorStatePtr(new SmartProjectionFactorState())) :
           measured_(measured), noise_(model), K_(K), body_P_sensor_(body_P_sensor),
           state_(state), throwCheirality_(throwCheirality), verboseCheirality_(verboseCheirality) {
-
     }
 
     /**
@@ -185,6 +184,83 @@ namespace gtsam {
       keys_.push_back(poseKey);
     }
 
+    // This function decides whether a new triangulation is needed
+    inline bool decideIfTriangulate(std::vector<Pose3> cameraPoses, const Values& values) const {
+      // several calls to linearize will be done from the same linearization point, hence it is not needed to re-triangulate
+      // Note that this is not yet "selecting linearization", that will come later, and we only check if the
+      // current linearization is the "same" (up to tolerance) w.r.t. the last time we triangulated the point
+      bool retriangulate = true;
+      bool valuesEqualRetriangulation = true;
+      double retriangulationThreshold = 1e-9;
+
+      int poseCount = 0;
+      BOOST_FOREACH(const Key& k, keys_) {
+        Pose3 cameraPose;
+
+        if(body_P_sensor_)
+          cameraPose = values.at<Pose3>(k).compose(*body_P_sensor_);
+        else
+          cameraPose = values.at<Pose3>(k);
+
+        if (!state_->cameraPosesTriangulation.empty()) {
+
+          // TODO: are you sure that when using "add" the number of poses will be ok? (old linearization point will contain 1 pose less)
+          if (!cameraPose.equals(state_->cameraPosesTriangulation[poseCount], retriangulationThreshold)) {
+            valuesEqualRetriangulation = false;
+          }
+
+        } else {
+          valuesEqualRetriangulation = false;
+        }
+
+        cameraPoses.push_back(cameraPose);
+        poseCount++;
+      }
+
+      if (valuesEqualRetriangulation) {
+        retriangulate = false;
+      }
+
+      return retriangulate;
+    }
+
+    // This function decides whether a new triangulation is needed
+    //    bool decideIfLinearize(std::vector<Pose3> cameraPoses) const {
+    //      // "selecting linearization"
+    //      bool doLinearize = true;
+    //      double linearizationThreshold = 1e-2;
+    //
+    //      Pose3 firstCameraPose;
+    //      Pose3 firstCameraPoseOld;
+    //
+    //      for(size_t i = 0; i < cameraPoses.size(); i++) {
+    //        Pose3 cameraPose = cameraPoses.at(i);
+    //
+    //        if (!state_->cameraPosesLinearization.empty()) { // if we have a previous linearization point
+    //
+    //          if(i==0){ // we store the initial pose, this is useful for selective re-linearization
+    //            firstCameraPose = cameraPose;
+    //            firstCameraPoseOld = state_->cameraPosesLinearization[i];
+    //            continue;
+    //          }
+    //
+    //          // we compare the poses in the frame of the first pose
+    //          Pose3 localCameraPose = firstCameraPose.between(cameraPose);
+    //          Pose3 localCameraPoseOld = firstCameraPoseOld.between(state_->cameraPosesLinearization[i]);
+    //
+    //          if (!localCameraPose.equals(localCameraPoseOld, linearizationThreshold)) {
+    //            doLinearize = false;
+    //          }
+    //
+    //        } else {
+    //          doLinearize = false;
+    //        }
+    //      }
+    //
+    //      return doLinearize;
+    //    }
+
+
     /**
      * print
      * @param s optional string naming the factor
@@ -226,25 +302,21 @@ namespace gtsam {
     /// linearize returns a Hessianfactor that is an approximation of error(p)
     virtual boost::shared_ptr<GaussianFactor> linearize(const Values& values) const {
 
-      bool blockwise = false;
+      bool blockwise = false;                  // the full matrix version in faster
-      double retriangulationThreshold = 1e-9;
+      int dim_landmark = 3;                    // for degenerate instances this will become 2 (direction-only information)
-
-      int dim_landmark = 3;
-      bool retriangulate = true;
       
+      // Create structures for Hessian Factors
       unsigned int numKeys = keys_.size();
       std::vector<Index> js;
       std::vector<Matrix> Gs(numKeys*(numKeys+1)/2);
       std::vector<Vector> gs(numKeys);
       double f=0;
 
-      Vector changeLinPoses(numKeys*6);
-      Point3 changeLinPoint;
-
       // Collect all poses (Cameras)
-      bool valuesEqualRetriangulation = true;
       std::vector<Pose3> cameraPoses;
-      int poseCount = 0;
+
+      bool retriangulate = true; // decideIfTriangulate(cameraPoses, values);
+
       BOOST_FOREACH(const Key& k, keys_) {
         Pose3 cameraPose;
 
@@ -253,27 +325,10 @@ namespace gtsam {
         else
           cameraPose = values.at<Pose3>(k);
 
-        if (!state_->cameraPosesTriangulation.empty()) {
-          // TODO: are you sure that when using "add" the number of poses will be ok? (old linearization point will contain 1 pose less)
-          if (!cameraPose.equals(state_->cameraPosesTriangulation[poseCount], retriangulationThreshold)) {
-            valuesEqualRetriangulation = false;
-            subInsert(changeLinPoses, Vector::Zero(6), 6*poseCount);
-          }else{
-            Vector changeLinPoses_i = Pose3::Logmap(state_->cameraPosesTriangulation[poseCount].between(cameraPose));
-            subInsert(changeLinPoses, changeLinPoses_i, 6*poseCount);
-          }
-        } else {
-          valuesEqualRetriangulation = false;
-          subInsert(changeLinPoses, Vector::Zero(6), 6*poseCount);
-        }
-
         cameraPoses.push_back(cameraPose);
-        poseCount++;
       }
 
-      if (valuesEqualRetriangulation) {
+      if(retriangulate) {
-        retriangulate = false;
-      } else {
         state_->cameraPosesTriangulation = cameraPoses;
       }
 
@@ -281,7 +336,7 @@ namespace gtsam {
         // We triangulate the 3D position of the landmark
         try {
           Point3 newPoint = triangulatePoint3(cameraPoses, measured_, *K_);
-          changeLinPoint = newPoint - state_->point;  // TODO: implement this check for the degenerate case
+          // changeLinPoint = newPoint - state_->point;  // TODO: implement this check for the degenerate case
           state_->point = newPoint;
           state_->degenerate = false;
           state_->cheiralityException = false;
@@ -313,6 +368,17 @@ namespace gtsam {
         dim_landmark = 2;
       }
 
+      bool doLinearize = true; //= decideIfLinearize(cameraPoses);
+
+      if (doLinearize) {
+        state_->cameraPosesLinearization = cameraPoses;
+      }
+
+      if(!doLinearize){ // return the previous Hessian factor
+        return HessianFactor::shared_ptr(new HessianFactor(keys_, state_->Gs, state_->gs, state_->f));
+      }
+      //otherwise redo linearization
+
       if (blockwise){
         // ==========================================================================================================
         std::cout << "Deprecated use of blockwise version. This is slower and no longer supported" << std::endl;
@@ -401,40 +467,11 @@ namespace gtsam {
         }
         else{
 
-
           for(size_t i = 0; i < measured_.size(); i++) {
             Pose3 pose = cameraPoses.at(i);
             PinholeCamera<CALIBRATION> camera(pose, *K_);
             Matrix Hxi, Hli;
 
-            // Selective relinearization (check if new linearization is needed)
-            Vector repErr_i;
-            try {
-              repErr_i = - ( camera.project(state_->point) - measured_.at(i) ).vector();
-            } catch ( CheiralityException& e) {
-              std::cout << "Cheirality exception " << state_->ID << std::endl;
-              exit(EXIT_FAILURE);
-            }
-            noise_-> whitenInPlace(repErr_i);
-            f += repErr_i.squaredNorm();
-
-            Vector linRepErr;
-
-            linRepErr = state_->Hx * changeLinPoses + state_->Hl * changeLinPoint.vector() - state_->b;
-
-            double f_lin = linRepErr.squaredNorm();
-
-            // Relinearization check
-            if (state_->f - f_lin > 1e-7){
-              double rho = (state_->f - f) / (state_->f - f_lin);
-              if( fabs(rho) > 0.75 ){
-                return HessianFactor::shared_ptr(new HessianFactor(keys_, state_->Gs, state_->gs, state_->f));
-              }
-            }
-            else{
-              return HessianFactor::shared_ptr(new HessianFactor(keys_, state_->Gs, state_->gs, state_->f));
-            }
-
             Vector bi;
             try {
               bi = -( camera.project(state_->point,Hxi,Hli) - measured_.at(i) ).vector();
@@ -494,15 +531,15 @@ namespace gtsam {
      * to transform it to \f$ (h(x)-z)^2/\sigma^2 \f$, and then multiply by 0.5.
      */
     virtual double error(const Values& values) const {
-      double retriangulationThreshold = 1e-9;
       if (this->active(values)) {
         double overallError=0;
-        bool retriangulate = true;
 
         // Collect all poses (Cameras)
-        bool valuesEqualRetriangulation = true;
         std::vector<Pose3> cameraPoses;
-        int poseCount = 0;
+
+        // check if triangulation and linearization are actually needed
+        bool retriangulate = true; //decideIfTriangulate(cameraPoses, values);
+
         BOOST_FOREACH(const Key& k, keys_) {
           Pose3 cameraPose;
 
@@ -511,21 +548,10 @@ namespace gtsam {
           else
             cameraPose = values.at<Pose3>(k);
 
-          if (!state_->cameraPosesTriangulation.empty()) {
-            if (!cameraPose.equals(state_->cameraPosesTriangulation[poseCount], retriangulationThreshold)) {
-              valuesEqualRetriangulation = false;
-            }
-          } else {
-            valuesEqualRetriangulation = false;
-          }
-
           cameraPoses.push_back(cameraPose);
-          poseCount++;
         }
 
-        if (valuesEqualRetriangulation) {
+        if(retriangulate) {
-          retriangulate = false;
-        } else {
           state_->cameraPosesTriangulation = cameraPoses;
         }
 
@@ -629,3 +655,80 @@ namespace gtsam {
   };
 
 } // \ namespace gtsam
+
+/*
+// Discarded version of decideIfTriangulate and decideIfLinearize
+ *  This function decides whether a new triangulation and linearization is needed
+bool decideIfLinearize(std::vector<Pose3> cameraPoses) {
+  // Selective relinearization (check if new linearization is needed)
+        Vector repErr_i;
+        try {
+          repErr_i = - ( camera.project(state_->point) - measured_.at(i) ).vector();
+        } catch ( CheiralityException& e) {
+          std::cout << "Cheirality exception " << state_->ID << std::endl;
+          exit(EXIT_FAILURE);
+        }
+        noise_-> whitenInPlace(repErr_i);
+        f += repErr_i.squaredNorm();
+
+        Vector linRepErr;
+
+        linRepErr = state_->Hx * changeLinPoses + state_->Hl * changeLinPoint.vector() - state_->b;
+
+        double f_lin = linRepErr.squaredNorm();
+
+        // Relinearization check
+        if (state_->f - f_lin > 1e-7){
+          double rho = (state_->f - f) / (state_->f - f_lin);
+          if( fabs(rho) > 0.75 ){
+            return HessianFactor::shared_ptr(new HessianFactor(keys_, state_->Gs, state_->gs, state_->f));
+          }
+        }
+        else{
+          return HessianFactor::shared_ptr(new HessianFactor(keys_, state_->Gs, state_->gs, state_->f));
+        }
+
+
+
+bool decideIfTriangulateAndLinearize(std::vector<Pose3> cameraPoses) {
+      //      Vector changeLinPoses(numKeys*6);
+      //      Point3 changeLinPoint;
+
+      Pose3 firstCameraPose;
+      Pose3 firstCameraPoseOld;
+
+      int poseCount = 0;
+      BOOST_FOREACH(const Key& k, keys_) {
+        Pose3 cameraPose;
+
+        if(body_P_sensor_)
+          cameraPose = values.at<Pose3>(k).compose(*body_P_sensor_);
+        else
+          cameraPose = values.at<Pose3>(k);
+
+        if (!state_->cameraPosesTriangulation.empty()) {
+
+          if(poseCount==0){ // we store the initial pose, this is useful for selective re-linearization
+            firstCameraPose = cameraPose;
+            firstCameraPoseOld = state_->cameraPosesTriangulation[poseCount];
+          }
+
+          // TODO: are you sure that when using "add" the number of poses will be ok? (old linearization point will contain 1 pose less)
+          if (!cameraPose.equals(state_->cameraPosesTriangulation[poseCount], retriangulationThreshold)) {
+            valuesEqualRetriangulation = false;
+            subInsert(changeLinPoses, Vector::Zero(6), 6*poseCount);
+          }else{
+            Vector changeLinPoses_i = Pose3::Logmap(state_->cameraPosesTriangulation[poseCount].between(cameraPose));
+            subInsert(changeLinPoses, changeLinPoses_i, 6*poseCount);
+          }
+        } else {
+          valuesEqualRetriangulation = false;
+          subInsert(changeLinPoses, Vector::Zero(6), 6*poseCount);
+        }
+
+        cameraPoses.push_back(cameraPose);
+        poseCount++;
+      }
+    }
+    */
+

gtsam_unstable/slam/tests/testSmartProjectionFactor.cpp

@@ -73,7 +73,7 @@ TEST( SmartProjectionFactor, Constructor) {
   std::vector<Point2> measurements;
   measurements.push_back(Point2(323.0, 240.0));
 
-  TestSmartProjectionFactor factor(measurements, model, views, K);
+  TestSmartProjectionFactor factor(views, measurements, model, K);
 }
 
 /* ************************************************************************* */
@@ -87,7 +87,7 @@ TEST( SmartProjectionFactor, ConstructorWithTransform) {
   measurements.push_back(Point2(323.0, 240.0));
   Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
 
-  TestSmartProjectionFactor factor(measurements, model, views, K, body_P_sensor);
+  TestSmartProjectionFactor factor(views, measurements, model, K, body_P_sensor);
 }
 
 /* ************************************************************************* */
@@ -98,8 +98,8 @@ TEST( SmartProjectionFactor, Equals ) {
 
   std::vector<Key> views;
   views += X(1);
-  TestSmartProjectionFactor factor1(measurements, model, views, K);
+  TestSmartProjectionFactor factor1(views, measurements, model, K);
-  TestSmartProjectionFactor factor2(measurements, model, views, K);
+  TestSmartProjectionFactor factor2(views, measurements, model, K);
 
   CHECK(assert_equal(factor1, factor2));
 }
@@ -113,8 +113,8 @@ TEST( SmartProjectionFactor, EqualsWithTransform ) {
 
   std::vector<Key> views;
   views += X(1);
-  TestSmartProjectionFactor factor1(measurements, model, views, K, body_P_sensor);
+  TestSmartProjectionFactor factor1(views, measurements, model, K, body_P_sensor);
-  TestSmartProjectionFactor factor2(measurements, model, views, K, body_P_sensor);
+  TestSmartProjectionFactor factor2(views, measurements, model, K, body_P_sensor);
 
   CHECK(assert_equal(factor1, factor2));
 }
@@ -631,8 +631,8 @@ TEST( SmartProjectionFactor, 3poses_2land_rotation_only_smart_projection_factor
 
   typedef SmartProjectionFactor<Pose3, Point3, Cal3_S2> SmartFactor;
 
-  SmartFactor::shared_ptr smartFactor1(new SmartFactor(measurements_cam1, noiseProjection, views, K));
+  SmartFactor::shared_ptr smartFactor1(new SmartFactor(views, measurements_cam1, noiseProjection, K));
-  SmartFactor::shared_ptr smartFactor2(new SmartFactor(measurements_cam2, noiseProjection, views, K));
+  SmartFactor::shared_ptr smartFactor2(new SmartFactor(views, measurements_cam2, noiseProjection, K));
 
   const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
   const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, 0.10);