included possibility to linearize to Jacobian for smart Pose factors

gtsam_unstable/slam/SmartFactorBase.h

@@ -20,6 +20,7 @@
 #pragma once
 
 #include "JacobianFactorQ.h"
+#include "JacobianFactorSVD.h"
 #include "ImplicitSchurFactor.h"
 #include "RegularHessianFactor.h"
 
@@ -629,6 +630,17 @@ public:
     return boost::make_shared < JacobianFactorQ<D> > (Fblocks, E, PointCov, b);
   }
 
+  // ****************************************************************************************************
+  boost::shared_ptr<JacobianFactor> createJacobianSVDFactor(
+      const Cameras& cameras, const Point3& point, double lambda = 0.0) const {
+    size_t numKeys = this->keys_.size();
+    std::vector < KeyMatrix2D > Fblocks;
+    Vector b;
+    Matrix Enull(2*numKeys, 2*numKeys-3);
+    computeJacobiansSVD(Fblocks, Enull, b, cameras, point, lambda);
+    return boost::make_shared< JacobianFactorSVD<6> >(Fblocks, Enull, b);
+  }
+
 private:
 
   /// Serialization function

gtsam_unstable/slam/SmartProjectionFactor.h

@@ -54,6 +54,10 @@ public:
   double f;
 };
 
+enum linearizationType {
+  HESSIAN, JACOBIAN_SVD, JACOBIAN_Q
+};
+
 /**
  * SmartProjectionFactor: triangulates point
  * TODO: why LANDMARK parameter?
@@ -400,6 +404,15 @@ public:
       return boost::shared_ptr<JacobianFactorQ<D> >();
   }
 
+  /// different (faster) way to compute Jacobian factor
+  boost::shared_ptr<JacobianFactor > createJacobianSVDFactor(const Cameras& cameras,
+      double lambda) const {
+    if (triangulateForLinearize(cameras))
+      return Base::createJacobianQFactor(cameras, point_, lambda);
+    else
+      return boost::shared_ptr<JacobianFactorQ<D> >();
+  }
+
   /// Returns true if nonDegenerate
   bool computeCamerasAndTriangulate(const Values& values,
       Cameras& myCameras) const {

gtsam_unstable/slam/SmartProjectionPoseFactor.h

@@ -31,6 +31,8 @@ template<class POSE, class LANDMARK, class CALIBRATION>
 class SmartProjectionPoseFactor: public SmartProjectionFactor<POSE, LANDMARK, CALIBRATION, 6> {
 protected:
 
+  linearizationType linearizeTo_;
+
   // Known calibration
   std::vector<boost::shared_ptr<CALIBRATION> > K_all_; ///< shared pointer to calibration object (one for each camera)
 
@@ -56,8 +58,9 @@ public:
    */
   SmartProjectionPoseFactor(const double rankTol = 1,
       const double linThreshold = -1, const bool manageDegeneracy = false,
-      const bool enableEPI = false, boost::optional<POSE> body_P_sensor = boost::none) :
+      const bool enableEPI = false, boost::optional<POSE> body_P_sensor = boost::none,
-        Base(rankTol, linThreshold, manageDegeneracy, enableEPI, body_P_sensor) {}
+      linearizationType linearizeTo = HESSIAN) :
+        Base(rankTol, linThreshold, manageDegeneracy, enableEPI, body_P_sensor), linearizeTo_(linearizeTo) {}
 
   /** Virtual destructor */
   virtual ~SmartProjectionPoseFactor() {}
@@ -139,11 +142,22 @@ public:
   }
 
   /**
-   * linearize returns a Hessian factor contraining the poses
+   * linear factor on the poses
    */
   virtual boost::shared_ptr<GaussianFactor> linearize(
       const Values& values) const {
-    return this->createHessianFactor(cameras(values));
+    // depending on flag set on construction we may linearize to different linear factors
+    switch(linearizeTo_){
+    case JACOBIAN_SVD :
+      return this->createJacobianSVDFactor(cameras(values), 0.0);
+      break;
+    case JACOBIAN_Q :
+      return this->createJacobianQFactor(cameras(values), 0.0);
+      break;
+    default:
+      return this->createHessianFactor(cameras(values));
+      break;
+    }
   }
 
   /**