included possibility to linearize to Jacobian for smart Pose factors
parent
b088945641
commit
4f91f94d1e
|
@ -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
|
||||
|
|
|
@ -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 {
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
Loading…
Reference in New Issue