included possibility to linearize to Jacobian for smart Pose factors

release/4.3a0
Luca 2014-05-22 21:27:46 -04:00
parent b088945641
commit 4f91f94d1e
3 changed files with 43 additions and 4 deletions

View File

@ -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

View File

@ -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 {

View File

@ -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) :
Base(rankTol, linThreshold, manageDegeneracy, enableEPI, body_P_sensor) {}
const bool enableEPI = false, boost::optional<POSE> 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<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;
}
}
/**