diff --git a/gtsam_unstable/slam/SmartFactorBase.h b/gtsam_unstable/slam/SmartFactorBase.h index d24a878bb..265df5ecd 100644 --- a/gtsam_unstable/slam/SmartFactorBase.h +++ b/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 > (Fblocks, E, PointCov, b); } + // **************************************************************************************************** + boost::shared_ptr 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 diff --git a/gtsam_unstable/slam/SmartProjectionFactor.h b/gtsam_unstable/slam/SmartProjectionFactor.h index 59a841813..786c578fd 100644 --- a/gtsam_unstable/slam/SmartProjectionFactor.h +++ b/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 >(); } + /// different (faster) way to compute Jacobian factor + boost::shared_ptr createJacobianSVDFactor(const Cameras& cameras, + double lambda) const { + if (triangulateForLinearize(cameras)) + return Base::createJacobianQFactor(cameras, point_, lambda); + else + return boost::shared_ptr >(); + } + /// Returns true if nonDegenerate bool computeCamerasAndTriangulate(const Values& values, Cameras& myCameras) const { diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactor.h b/gtsam_unstable/slam/SmartProjectionPoseFactor.h index 879e5ab80..4a565d7dd 100644 --- a/gtsam_unstable/slam/SmartProjectionPoseFactor.h +++ b/gtsam_unstable/slam/SmartProjectionPoseFactor.h @@ -31,6 +31,8 @@ template class SmartProjectionPoseFactor: public SmartProjectionFactor { protected: + linearizationType linearizeTo_; + // Known calibration std::vector > 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 body_P_sensor = boost::none) : - Base(rankTol, linThreshold, manageDegeneracy, enableEPI, body_P_sensor) {} + const bool enableEPI = false, boost::optional body_P_sensor = boost::none, + 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 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; + } } /**