12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Remove selective relinearization and state

release/4.3a0
cbeall3 2015-04-08 17:52:25 -04:00
parent 51482ea358
commit 762a7b7435
3 changed files with 27 additions and 109 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

108
gtsam_unstable/slam/SmartStereoProjectionFactor.h
View File

@ -33,33 +33,6 @@
namespace gtsam {
/**
* Structure for storing some state memory, used to speed up optimization
* @addtogroup SLAM
*/
class SmartStereoProjectionFactorState {
protected:
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
SmartStereoProjectionFactorState() {
}
// Hessian representation (after Schur complement)
bool calculatedHessian;
Matrix H;
Vector gs_vector;
std::vector<Matrix> Gs;
std::vector<Vector> gs;
double f;
};
enum LinearizationMode {
HESSIAN, JACOBIAN_SVD, JACOBIAN_Q
};
/**
* SmartStereoProjectionFactor: triangulates point
*/
@ -84,14 +57,6 @@ protected:
mutable bool degenerate_;
mutable bool cheiralityException_;
// verbosity handling for Cheirality Exceptions
const bool throwCheirality_; ///< If true, rethrows Cheirality exceptions (default: false)
const bool verboseCheirality_; ///< If true, prints text for Cheirality exceptions (default: false)
boost::shared_ptr<SmartStereoProjectionFactorState> state_;
/// shorthand for smart projection factor state variable
typedef boost::shared_ptr<SmartStereoProjectionFactorState> SmartFactorStatePtr;
/// shorthand for base class type
typedef SmartFactorBase<StereoCamera> Base;
@ -110,6 +75,12 @@ protected:
ZDim = 3
}; ///< Dimension trait of measurement type
/// @name Parameters governing how triangulation result is treated
/// @{
const bool throwCheirality_; ///< If true, rethrows Cheirality exceptions (default: false)
const bool verboseCheirality_; ///< If true, prints text for Cheirality exceptions (default: false)
/// @}
public:
/// shorthand for a smart pointer to a factor
@ -133,12 +104,11 @@ public:
SmartStereoProjectionFactor(const double rankTol, const double linThreshold,
const bool manageDegeneracy, const bool enableEPI,
double landmarkDistanceThreshold = 1e10,
double dynamicOutlierRejectionThreshold = -1, SmartFactorStatePtr state =
SmartFactorStatePtr(new SmartStereoProjectionFactorState())) :
double dynamicOutlierRejectionThreshold = -1) :
rankTolerance_(rankTol), retriangulationThreshold_(1e-5), manageDegeneracy_(
manageDegeneracy), enableEPI_(enableEPI), linearizationThreshold_(
linThreshold), degenerate_(false), cheiralityException_(false), throwCheirality_(
false), verboseCheirality_(false), state_(state), landmarkDistanceThreshold_(
false), verboseCheirality_(false), landmarkDistanceThreshold_(
landmarkDistanceThreshold), dynamicOutlierRejectionThreshold_(
dynamicOutlierRejectionThreshold) {
}
@ -199,40 +169,6 @@ public:
return retriangulate; // if we arrive to this point_ all poses are the same and we don't need re-triangulation
}
/// This function checks if the new linearization point_ is 'close' to the previous one used for linearization
bool decideIfLinearize(const Cameras& cameras) const {
// "selective linearization"
// The function evaluates how close are the old and the new poses, transformed in the ref frame of the first pose
// (we only care about the "rigidity" of the poses, not about their absolute pose)
if (this->linearizationThreshold_ < 0) //by convention if linearizationThreshold is negative we always relinearize
return true;
// if we do not have a previous linearization point_ or the new linearization point_ includes more poses
if (cameraPosesLinearization_.empty()
|| (cameras.size() != cameraPosesLinearization_.size()))
return true;
Pose3 firstCameraPose, firstCameraPoseOld;
for (size_t i = 0; i < cameras.size(); i++) {
if (i == 0) { // we store the initial pose, this is useful for selective re-linearization
firstCameraPose = cameras[i].pose();
firstCameraPoseOld = cameraPosesLinearization_[i];
continue;
}
// we compare the poses in the frame of the first pose
Pose3 localCameraPose = firstCameraPose.between(cameras[i].pose());
Pose3 localCameraPoseOld = firstCameraPoseOld.between(
cameraPosesLinearization_[i]);
if (!localCameraPose.equals(localCameraPoseOld,
this->linearizationThreshold_))
return true; // at least two "relative" poses are different, hence we re-linearize
}
return false; // if we arrive to this point_ all poses are the same and we don't need re-linearize
}
/// triangulateSafe
size_t triangulateSafe(const Values& values) const {
return triangulateSafe(this->cameras(values));
@ -364,7 +300,7 @@ public:
exit(1);
}
this->triangulateSafe(cameras);
triangulateSafe(cameras);
if (isDebug)
std::cout << "point_ = " << point_ << std::endl;
@ -388,29 +324,10 @@ public:
std::cout << "degenerate_ = true" << std::endl;
}
bool doLinearize = this->decideIfLinearize(cameras);
if (isDebug)
std::cout << "doLinearize = " << doLinearize << std::endl;
if (this->linearizationThreshold_ >= 0 && doLinearize) // if we apply selective relinearization and we need to relinearize
if (this->linearizationThreshold_ >= 0) // if we apply selective relinearization and we need to relinearize
for (size_t i = 0; i < cameras.size(); i++)
this->cameraPosesLinearization_[i] = cameras[i].pose();
if (!doLinearize) { // return the previous Hessian factor
std::cout << "=============================" << std::endl;
std::cout << "doLinearize " << doLinearize << std::endl;
std::cout << "this->linearizationThreshold_ "
<< this->linearizationThreshold_ << std::endl;
std::cout << "this->degenerate_ " << this->degenerate_ << std::endl;
std::cout
<< "something wrong in SmartProjectionHessianFactor: selective relinearization should be disabled"
<< std::endl;
exit(1);
return boost::make_shared<RegularHessianFactor<Base::Dim> >(this->keys_,
this->state_->Gs, this->state_->gs, this->state_->f);
}
// ==================================================================
std::vector<typename Base::MatrixZD> Fblocks;
Matrix F, E;
@ -447,11 +364,6 @@ public:
}
// ==================================================================
double f = b.squaredNorm();
if (this->linearizationThreshold_ >= 0) { // if we do not use selective relinearization we don't need to store these variables
this->state_->Gs = Gs;
this->state_->gs = gs;
this->state_->f = f;
}
return boost::make_shared<RegularHessianFactor<Base::Dim> >(this->keys_, Gs, gs, f);
}

7
gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h
View File

@ -40,6 +40,13 @@ namespace gtsam {
*/
template<class CALIBRATION>
class SmartStereoProjectionPoseFactor: public SmartStereoProjectionFactor<CALIBRATION> {
public:
/// Linearization mode: what factor to linearize to
enum LinearizationMode {
HESSIAN, IMPLICIT_SCHUR, JACOBIAN_Q, JACOBIAN_SVD
};
protected:
LinearizationMode linearizeTo_; ///< How to linearize the factor (HESSIAN, JACOBIAN_SVD, JACOBIAN_Q)

21
gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp
View File

@ -64,7 +64,6 @@ static Pose3 body_P_sensor1(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2),
Point3(0.25, -0.10, 1.0));
typedef SmartStereoProjectionPoseFactor<Cal3_S2Stereo> SmartFactor;
typedef SmartStereoProjectionPoseFactor<Cal3Bundler> SmartFactorBundler;
vector<StereoPoint2> stereo_projectToMultipleCameras(const StereoCamera& cam1,
const StereoCamera& cam2, const StereoCamera& cam3, Point3 landmark) {
@ -327,15 +326,15 @@ TEST( SmartStereoProjectionPoseFactor, jacobianSVD ) {
cam2, cam3, landmark3);
SmartFactor::shared_ptr smartFactor1(
new SmartFactor(1, -1, false, false, JACOBIAN_SVD));
new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD));
smartFactor1->add(measurements_cam1, views, model, K);
SmartFactor::shared_ptr smartFactor2(
new SmartFactor(1, -1, false, false, JACOBIAN_SVD));
new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD));
smartFactor2->add(measurements_cam2, views, model, K);
SmartFactor::shared_ptr smartFactor3(
new SmartFactor(1, -1, false, false, JACOBIAN_SVD));
new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD));
smartFactor3->add(measurements_cam3, views, model, K);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@ -395,17 +394,17 @@ TEST( SmartStereoProjectionPoseFactor, landmarkDistance ) {
cam2, cam3, landmark3);
SmartFactor::shared_ptr smartFactor1(
new SmartFactor(1, -1, false, false, JACOBIAN_SVD,
new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD,
excludeLandmarksFutherThanDist));
smartFactor1->add(measurements_cam1, views, model, K);
SmartFactor::shared_ptr smartFactor2(
new SmartFactor(1, -1, false, false, JACOBIAN_SVD,
new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD,
excludeLandmarksFutherThanDist));
smartFactor2->add(measurements_cam2, views, model, K);
SmartFactor::shared_ptr smartFactor3(
new SmartFactor(1, -1, false, false, JACOBIAN_SVD,
new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD,
excludeLandmarksFutherThanDist));
smartFactor3->add(measurements_cam3, views, model, K);
@ -473,22 +472,22 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) {
measurements_cam4.at(0) = measurements_cam4.at(0) + StereoPoint2(10, 10, 1); // add outlier
SmartFactor::shared_ptr smartFactor1(
new SmartFactor(1, -1, false, false, JACOBIAN_SVD,
new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD,
excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold));
smartFactor1->add(measurements_cam1, views, model, K);
SmartFactor::shared_ptr smartFactor2(
new SmartFactor(1, -1, false, false, JACOBIAN_SVD,
new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD,
excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold));
smartFactor2->add(measurements_cam2, views, model, K);
SmartFactor::shared_ptr smartFactor3(
new SmartFactor(1, -1, false, false, JACOBIAN_SVD,
new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD,
excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold));
smartFactor3->add(measurements_cam3, views, model, K);
SmartFactor::shared_ptr smartFactor4(
new SmartFactor(1, -1, false, false, JACOBIAN_SVD,
new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD,
excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold));
smartFactor4->add(measurements_cam4, views, model, K);