Created stereo Smart factor classes, test class for SmartStereoProjectionPoseFactor
Stephen Camp
parent
917c9c46c8
commit
7a2af9999a
|
|
@ -0,0 +1,710 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file SmartStereoProjectionFactor.h
|
||||
* @brief Base class to create smart factors on poses or cameras
|
||||
* @author Luca Carlone
|
||||
* @author Zsolt Kira
|
||||
* @author Frank Dellaert
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "SmartFactorBase.h"
|
||||
|
||||
#include <gtsam/geometry/triangulation.h>
|
||||
#include <gtsam/geometry/Pose3.h>
|
||||
#include <gtsam/inference/Symbol.h>
|
||||
#include <gtsam/slam/dataset.h>
|
||||
|
||||
#include <boost/optional.hpp>
|
||||
#include <boost/make_shared.hpp>
|
||||
#include <vector>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* Structure for storing some state memory, used to speed up optimization
|
||||
* @addtogroup SLAM
|
||||
*/
|
||||
class SmartStereoProjectionFactorState {
|
||||
|
||||
protected:
|
||||
|
||||
public:
|
||||
|
||||
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
|
||||
* TODO: why LANDMARK parameter?
|
||||
*/
|
||||
template<class POSE, class LANDMARK, class CALIBRATION, size_t D>
|
||||
class SmartStereoProjectionFactor: public SmartFactorBase<POSE, CALIBRATION, D> {
|
||||
protected:
|
||||
|
||||
// Some triangulation parameters
|
||||
const double rankTolerance_; ///< threshold to decide whether triangulation is degenerate_
|
||||
const double retriangulationThreshold_; ///< threshold to decide whether to re-triangulate
|
||||
mutable std::vector<Pose3> cameraPosesTriangulation_; ///< current triangulation poses
|
||||
|
||||
const bool manageDegeneracy_; ///< if set to true will use the rotation-only version for degenerate cases
|
||||
|
||||
const bool enableEPI_; ///< if set to true, will refine triangulation using LM
|
||||
|
||||
const double linearizationThreshold_; ///< threshold to decide whether to re-linearize
|
||||
mutable std::vector<Pose3> cameraPosesLinearization_; ///< current linearization poses
|
||||
|
||||
mutable Point3 point_; ///< Current estimate of the 3D point
|
||||
|
||||
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<POSE, CALIBRATION, D> Base;
|
||||
|
||||
double landmarkDistanceThreshold_; // if the landmark is triangulated at a
|
||||
// distance larger than that the factor is considered degenerate
|
||||
|
||||
double dynamicOutlierRejectionThreshold_; // if this is nonnegative the factor will check if the
|
||||
// average reprojection error is smaller than this threshold after triangulation,
|
||||
// and the factor is disregarded if the error is large
|
||||
|
||||
/// shorthand for this class
|
||||
typedef SmartStereoProjectionFactor<POSE, LANDMARK, CALIBRATION, D> This;
|
||||
|
||||
public:
|
||||
|
||||
/// shorthand for a smart pointer to a factor
|
||||
typedef boost::shared_ptr<This> shared_ptr;
|
||||
|
||||
/// shorthand for a pinhole camera
|
||||
typedef PinholeCamera<CALIBRATION> Camera;
|
||||
typedef std::vector<Camera> Cameras;
|
||||
|
||||
/**
|
||||
* Constructor
|
||||
* @param rankTol tolerance used to check if point triangulation is degenerate
|
||||
* @param linThreshold threshold on relative pose changes used to decide whether to relinearize (selective relinearization)
|
||||
* @param manageDegeneracy is true, in presence of degenerate triangulation, the factor is converted to a rotation-only constraint,
|
||||
* otherwise the factor is simply neglected
|
||||
* @param enableEPI if set to true linear triangulation is refined with embedded LM iterations
|
||||
* @param body_P_sensor is the transform from body to sensor frame (default identity)
|
||||
*/
|
||||
SmartStereoProjectionFactor(const double rankTol, const double linThreshold,
|
||||
const bool manageDegeneracy, const bool enableEPI,
|
||||
boost::optional<POSE> body_P_sensor = boost::none,
|
||||
double landmarkDistanceThreshold = 1e10,
|
||||
double dynamicOutlierRejectionThreshold = -1,
|
||||
SmartFactorStatePtr state = SmartFactorStatePtr(new SmartStereoProjectionFactorState())) :
|
||||
Base(body_P_sensor), rankTolerance_(rankTol), retriangulationThreshold_(
|
||||
1e-5), manageDegeneracy_(manageDegeneracy), enableEPI_(enableEPI), linearizationThreshold_(
|
||||
linThreshold), degenerate_(false), cheiralityException_(false), throwCheirality_(
|
||||
false), verboseCheirality_(false), state_(state),
|
||||
landmarkDistanceThreshold_(landmarkDistanceThreshold),
|
||||
dynamicOutlierRejectionThreshold_(dynamicOutlierRejectionThreshold) {
|
||||
}
|
||||
|
||||
/** Virtual destructor */
|
||||
virtual ~SmartStereoProjectionFactor() {
|
||||
}
|
||||
|
||||
/**
|
||||
* print
|
||||
* @param s optional string naming the factor
|
||||
* @param keyFormatter optional formatter useful for printing Symbols
|
||||
*/
|
||||
void print(const std::string& s = "", const KeyFormatter& keyFormatter =
|
||||
DefaultKeyFormatter) const {
|
||||
std::cout << s << "SmartStereoProjectionFactor, z = \n";
|
||||
std::cout << "rankTolerance_ = " << rankTolerance_ << std::endl;
|
||||
std::cout << "degenerate_ = " << degenerate_ << std::endl;
|
||||
std::cout << "cheiralityException_ = " << cheiralityException_ << std::endl;
|
||||
Base::print("", keyFormatter);
|
||||
}
|
||||
|
||||
/// Check if the new linearization point_ is the same as the one used for previous triangulation
|
||||
bool decideIfTriangulate(const Cameras& cameras) const {
|
||||
// several calls to linearize will be done from the same linearization point_, hence it is not needed to re-triangulate
|
||||
// Note that this is not yet "selecting linearization", that will come later, and we only check if the
|
||||
// current linearization is the "same" (up to tolerance) w.r.t. the last time we triangulated the point_
|
||||
|
||||
size_t m = cameras.size();
|
||||
|
||||
bool retriangulate = false;
|
||||
|
||||
// if we do not have a previous linearization point_ or the new linearization point_ includes more poses
|
||||
if (cameraPosesTriangulation_.empty()
|
||||
|| cameras.size() != cameraPosesTriangulation_.size())
|
||||
retriangulate = true;
|
||||
|
||||
if (!retriangulate) {
|
||||
for (size_t i = 0; i < cameras.size(); i++) {
|
||||
if (!cameras[i].pose().equals(cameraPosesTriangulation_[i],
|
||||
retriangulationThreshold_)) {
|
||||
retriangulate = true; // at least two poses are different, hence we retriangulate
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (retriangulate) { // we store the current poses used for triangulation
|
||||
cameraPosesTriangulation_.clear();
|
||||
cameraPosesTriangulation_.reserve(m);
|
||||
for (size_t i = 0; i < m; i++)
|
||||
// cameraPosesTriangulation_[i] = cameras[i].pose();
|
||||
cameraPosesTriangulation_.push_back(cameras[i].pose());
|
||||
}
|
||||
|
||||
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));
|
||||
}
|
||||
|
||||
/// triangulateSafe
|
||||
size_t triangulateSafe(const Cameras& cameras) const {
|
||||
|
||||
size_t m = cameras.size();
|
||||
if (m < 2) { // if we have a single pose the corresponding factor is uninformative
|
||||
degenerate_ = true;
|
||||
return m;
|
||||
}
|
||||
bool retriangulate = decideIfTriangulate(cameras);
|
||||
|
||||
if (retriangulate) {
|
||||
// We triangulate the 3D position of the landmark
|
||||
try {
|
||||
// std::cout << "triangulatePoint3 i \n" << rankTolerance << std::endl;
|
||||
point_ = triangulatePoint3<CALIBRATION>(cameras, this->measured_,
|
||||
rankTolerance_, enableEPI_);
|
||||
degenerate_ = false;
|
||||
cheiralityException_ = false;
|
||||
|
||||
// Check landmark distance and reprojection errors to avoid outliers
|
||||
double totalReprojError = 0.0;
|
||||
size_t i=0;
|
||||
BOOST_FOREACH(const Camera& camera, cameras) {
|
||||
Point3 cameraTranslation = camera.pose().translation();
|
||||
// we discard smart factors corresponding to points that are far away
|
||||
if(cameraTranslation.distance(point_) > landmarkDistanceThreshold_){
|
||||
degenerate_ = true;
|
||||
break;
|
||||
}
|
||||
const Point2& zi = this->measured_.at(i);
|
||||
try {
|
||||
Point2 reprojectionError(camera.project(point_) - zi);
|
||||
totalReprojError += reprojectionError.vector().norm();
|
||||
} catch (CheiralityException) {
|
||||
cheiralityException_ = true;
|
||||
}
|
||||
i += 1;
|
||||
}
|
||||
// we discard smart factors that have large reprojection error
|
||||
if(dynamicOutlierRejectionThreshold_ > 0 &&
|
||||
totalReprojError/m > dynamicOutlierRejectionThreshold_)
|
||||
degenerate_ = true;
|
||||
|
||||
} catch (TriangulationUnderconstrainedException&) {
|
||||
// if TriangulationUnderconstrainedException can be
|
||||
// 1) There is a single pose for triangulation - this should not happen because we checked the number of poses before
|
||||
// 2) The rank of the matrix used for triangulation is < 3: rotation-only, parallel cameras (or motion towards the landmark)
|
||||
// in the second case we want to use a rotation-only smart factor
|
||||
degenerate_ = true;
|
||||
cheiralityException_ = false;
|
||||
} catch (TriangulationCheiralityException&) {
|
||||
// point is behind one of the cameras: can be the case of close-to-parallel cameras or may depend on outliers
|
||||
// we manage this case by either discarding the smart factor, or imposing a rotation-only constraint
|
||||
cheiralityException_ = true;
|
||||
}
|
||||
}
|
||||
return m;
|
||||
}
|
||||
|
||||
/// triangulate
|
||||
bool triangulateForLinearize(const Cameras& cameras) const {
|
||||
|
||||
bool isDebug = false;
|
||||
size_t nrCameras = this->triangulateSafe(cameras);
|
||||
|
||||
if (nrCameras < 2
|
||||
|| (!this->manageDegeneracy_
|
||||
&& (this->cheiralityException_ || this->degenerate_))) {
|
||||
if (isDebug) {
|
||||
std::cout << "createImplicitSchurFactor: degenerate configuration"
|
||||
<< std::endl;
|
||||
}
|
||||
return false;
|
||||
} else {
|
||||
|
||||
// instead, if we want to manage the exception..
|
||||
if (this->cheiralityException_ || this->degenerate_) { // if we want to manage the exceptions with rotation-only factors
|
||||
this->degenerate_ = true;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
/// linearize returns a Hessianfactor that is an approximation of error(p)
|
||||
boost::shared_ptr<RegularHessianFactor<D> > createHessianFactor(
|
||||
const Cameras& cameras, const double lambda = 0.0) const {
|
||||
|
||||
bool isDebug = false;
|
||||
size_t numKeys = this->keys_.size();
|
||||
// Create structures for Hessian Factors
|
||||
std::vector < Key > js;
|
||||
std::vector < Matrix > Gs(numKeys * (numKeys + 1) / 2);
|
||||
std::vector < Vector > gs(numKeys);
|
||||
|
||||
if (this->measured_.size() != cameras.size()) {
|
||||
std::cout
|
||||
<< "SmartProjectionHessianFactor: this->measured_.size() inconsistent with input"
|
||||
<< std::endl;
|
||||
exit(1);
|
||||
}
|
||||
|
||||
this->triangulateSafe(cameras);
|
||||
|
||||
if (numKeys < 2
|
||||
|| (!this->manageDegeneracy_
|
||||
&& (this->cheiralityException_ || this->degenerate_))) {
|
||||
// std::cout << "In linearize: exception" << std::endl;
|
||||
BOOST_FOREACH(gtsam::Matrix& m, Gs)
|
||||
m = zeros(D, D);
|
||||
BOOST_FOREACH(Vector& v, gs)
|
||||
v = zero(D);
|
||||
return boost::make_shared<RegularHessianFactor<D> >(this->keys_, Gs, gs,
|
||||
0.0);
|
||||
}
|
||||
|
||||
// instead, if we want to manage the exception..
|
||||
if (this->cheiralityException_ || this->degenerate_) { // if we want to manage the exceptions with rotation-only factors
|
||||
this->degenerate_ = true;
|
||||
}
|
||||
|
||||
bool doLinearize = this->decideIfLinearize(cameras);
|
||||
|
||||
if (this->linearizationThreshold_ >= 0 && doLinearize) // 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<D> >(this->keys_,
|
||||
this->state_->Gs, this->state_->gs, this->state_->f);
|
||||
}
|
||||
|
||||
// ==================================================================
|
||||
Matrix F, E;
|
||||
Matrix3 PointCov;
|
||||
Vector b;
|
||||
double f = computeJacobians(F, E, PointCov, b, cameras, lambda);
|
||||
|
||||
// Schur complement trick
|
||||
// Frank says: should be possible to do this more efficiently?
|
||||
// And we care, as in grouped factors this is called repeatedly
|
||||
Matrix H(D * numKeys, D * numKeys);
|
||||
Vector gs_vector;
|
||||
|
||||
H.noalias() = F.transpose() * (F - (E * (PointCov * (E.transpose() * F))));
|
||||
gs_vector.noalias() = F.transpose()
|
||||
* (b - (E * (PointCov * (E.transpose() * b))));
|
||||
if (isDebug)
|
||||
std::cout << "gs_vector size " << gs_vector.size() << std::endl;
|
||||
|
||||
// Populate Gs and gs
|
||||
int GsCount2 = 0;
|
||||
for (DenseIndex i1 = 0; i1 < (DenseIndex)numKeys; i1++) { // for each camera
|
||||
DenseIndex i1D = i1 * D;
|
||||
gs.at(i1) = gs_vector.segment < D > (i1D);
|
||||
for (DenseIndex i2 = 0; i2 < (DenseIndex)numKeys; i2++) {
|
||||
if (i2 >= i1) {
|
||||
Gs.at(GsCount2) = H.block < D, D > (i1D, i2 * D);
|
||||
GsCount2++;
|
||||
}
|
||||
}
|
||||
}
|
||||
// ==================================================================
|
||||
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<D> >(this->keys_, Gs, gs, f);
|
||||
}
|
||||
|
||||
// create factor
|
||||
boost::shared_ptr<ImplicitSchurFactor<D> > createImplicitSchurFactor(
|
||||
const Cameras& cameras, double lambda) const {
|
||||
if (triangulateForLinearize(cameras))
|
||||
return Base::createImplicitSchurFactor(cameras, point_, lambda);
|
||||
else
|
||||
return boost::shared_ptr<ImplicitSchurFactor<D> >();
|
||||
}
|
||||
|
||||
/// create factor
|
||||
boost::shared_ptr<JacobianFactorQ<D> > createJacobianQFactor(
|
||||
const Cameras& cameras, double lambda) const {
|
||||
if (triangulateForLinearize(cameras))
|
||||
return Base::createJacobianQFactor(cameras, point_, lambda);
|
||||
else
|
||||
return boost::make_shared< JacobianFactorQ<D> >(this->keys_);
|
||||
}
|
||||
|
||||
/// Create a factor, takes values
|
||||
boost::shared_ptr<JacobianFactorQ<D> > createJacobianQFactor(
|
||||
const Values& values, double lambda) const {
|
||||
Cameras myCameras;
|
||||
// TODO triangulate twice ??
|
||||
bool nonDegenerate = computeCamerasAndTriangulate(values, myCameras);
|
||||
if (nonDegenerate)
|
||||
return createJacobianQFactor(myCameras, lambda);
|
||||
else
|
||||
return boost::make_shared< JacobianFactorQ<D> >(this->keys_);
|
||||
}
|
||||
|
||||
/// different (faster) way to compute Jacobian factor
|
||||
boost::shared_ptr< JacobianFactor > createJacobianSVDFactor(const Cameras& cameras,
|
||||
double lambda) const {
|
||||
if (triangulateForLinearize(cameras))
|
||||
return Base::createJacobianSVDFactor(cameras, point_, lambda);
|
||||
else
|
||||
return boost::make_shared< JacobianFactorSVD<D> >(this->keys_);
|
||||
}
|
||||
|
||||
/// Returns true if nonDegenerate
|
||||
bool computeCamerasAndTriangulate(const Values& values,
|
||||
Cameras& myCameras) const {
|
||||
Values valuesFactor;
|
||||
|
||||
// Select only the cameras
|
||||
BOOST_FOREACH(const Key key, this->keys_)
|
||||
valuesFactor.insert(key, values.at(key));
|
||||
|
||||
myCameras = this->cameras(valuesFactor);
|
||||
size_t nrCameras = this->triangulateSafe(myCameras);
|
||||
|
||||
if (nrCameras < 2
|
||||
|| (!this->manageDegeneracy_
|
||||
&& (this->cheiralityException_ || this->degenerate_)))
|
||||
return false;
|
||||
|
||||
// instead, if we want to manage the exception..
|
||||
if (this->cheiralityException_ || this->degenerate_) // if we want to manage the exceptions with rotation-only factors
|
||||
this->degenerate_ = true;
|
||||
|
||||
if (this->degenerate_) {
|
||||
std::cout << "SmartStereoProjectionFactor: this is not ready" << std::endl;
|
||||
std::cout << "this->cheiralityException_ " << this->cheiralityException_
|
||||
<< std::endl;
|
||||
std::cout << "this->degenerate_ " << this->degenerate_ << std::endl;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
/// Takes values
|
||||
bool computeEP(Matrix& E, Matrix& PointCov, const Values& values) const {
|
||||
Cameras myCameras;
|
||||
bool nonDegenerate = computeCamerasAndTriangulate(values, myCameras);
|
||||
if (nonDegenerate)
|
||||
computeEP(E, PointCov, myCameras);
|
||||
return nonDegenerate;
|
||||
}
|
||||
|
||||
/// Assumes non-degenerate !
|
||||
void computeEP(Matrix& E, Matrix& PointCov, const Cameras& cameras) const {
|
||||
return Base::computeEP(E, PointCov, cameras, point_);
|
||||
}
|
||||
|
||||
/// Version that takes values, and creates the point
|
||||
bool computeJacobians(std::vector<typename Base::KeyMatrix2D>& Fblocks,
|
||||
Matrix& E, Matrix& PointCov, Vector& b, const Values& values) const {
|
||||
Cameras myCameras;
|
||||
bool nonDegenerate = computeCamerasAndTriangulate(values, myCameras);
|
||||
if (nonDegenerate)
|
||||
computeJacobians(Fblocks, E, PointCov, b, myCameras, 0.0);
|
||||
return nonDegenerate;
|
||||
}
|
||||
|
||||
/// Compute F, E only (called below in both vanilla and SVD versions)
|
||||
/// Assumes the point has been computed
|
||||
/// Note E can be 2m*3 or 2m*2, in case point is degenerate
|
||||
double computeJacobians(std::vector<typename Base::KeyMatrix2D>& Fblocks,
|
||||
Matrix& E, Vector& b, const Cameras& cameras) const {
|
||||
|
||||
if (this->degenerate_) {
|
||||
std::cout << "manage degeneracy " << manageDegeneracy_ << std::endl;
|
||||
std::cout << "point " << point_ << std::endl;
|
||||
std::cout
|
||||
<< "SmartStereoProjectionFactor: Management of degeneracy is disabled - not ready to be used"
|
||||
<< std::endl;
|
||||
if (D > 6) {
|
||||
std::cout
|
||||
<< "Management of degeneracy is not yet ready when one also optimizes for the calibration "
|
||||
<< std::endl;
|
||||
}
|
||||
|
||||
int numKeys = this->keys_.size();
|
||||
E = zeros(2 * numKeys, 2);
|
||||
b = zero(2 * numKeys);
|
||||
double f = 0;
|
||||
for (size_t i = 0; i < this->measured_.size(); i++) {
|
||||
if (i == 0) { // first pose
|
||||
this->point_ = cameras[i].backprojectPointAtInfinity(
|
||||
this->measured_.at(i));
|
||||
// 3D parametrization of point at infinity: [px py 1]
|
||||
}
|
||||
Matrix Fi, Ei;
|
||||
Vector bi = -(cameras[i].projectPointAtInfinity(this->point_, Fi, Ei)
|
||||
- this->measured_.at(i)).vector();
|
||||
|
||||
this->noise_.at(i)->WhitenSystem(Fi, Ei, bi);
|
||||
f += bi.squaredNorm();
|
||||
Fblocks.push_back(typename Base::KeyMatrix2D(this->keys_[i], Fi));
|
||||
E.block < 2, 2 > (2 * i, 0) = Ei;
|
||||
subInsert(b, bi, 2 * i);
|
||||
}
|
||||
return f;
|
||||
} else {
|
||||
// nondegenerate: just return Base version
|
||||
return Base::computeJacobians(Fblocks, E, b, cameras, point_);
|
||||
} // end else
|
||||
}
|
||||
|
||||
/// Version that computes PointCov, with optional lambda parameter
|
||||
double computeJacobians(std::vector<typename Base::KeyMatrix2D>& Fblocks,
|
||||
Matrix& E, Matrix& PointCov, Vector& b, const Cameras& cameras,
|
||||
const double lambda = 0.0) const {
|
||||
|
||||
double f = computeJacobians(Fblocks, E, b, cameras);
|
||||
|
||||
// Point covariance inv(E'*E)
|
||||
PointCov.noalias() = (E.transpose() * E + lambda * eye(E.cols())).inverse();
|
||||
|
||||
return f;
|
||||
}
|
||||
|
||||
/// takes values
|
||||
bool computeJacobiansSVD(std::vector<typename Base::KeyMatrix2D>& Fblocks,
|
||||
Matrix& Enull, Vector& b, const Values& values) const {
|
||||
typename Base::Cameras myCameras;
|
||||
double good = computeCamerasAndTriangulate(values, myCameras);
|
||||
if (good)
|
||||
computeJacobiansSVD(Fblocks, Enull, b, myCameras);
|
||||
return true;
|
||||
}
|
||||
|
||||
/// SVD version
|
||||
double computeJacobiansSVD(std::vector<typename Base::KeyMatrix2D>& Fblocks,
|
||||
Matrix& Enull, Vector& b, const Cameras& cameras) const {
|
||||
return Base::computeJacobiansSVD(Fblocks, Enull, b, cameras, point_);
|
||||
}
|
||||
|
||||
/// Returns Matrix, TODO: maybe should not exist -> not sparse !
|
||||
// TODO should there be a lambda?
|
||||
double computeJacobiansSVD(Matrix& F, Matrix& Enull, Vector& b,
|
||||
const Cameras& cameras) const {
|
||||
return Base::computeJacobiansSVD(F, Enull, b, cameras, point_);
|
||||
}
|
||||
|
||||
/// Returns Matrix, TODO: maybe should not exist -> not sparse !
|
||||
double computeJacobians(Matrix& F, Matrix& E, Matrix3& PointCov, Vector& b,
|
||||
const Cameras& cameras, const double lambda) const {
|
||||
return Base::computeJacobians(F, E, PointCov, b, cameras, point_, lambda);
|
||||
}
|
||||
|
||||
/// Calculate vector of re-projection errors, before applying noise model
|
||||
/// Assumes triangulation was done and degeneracy handled
|
||||
Vector reprojectionError(const Cameras& cameras) const {
|
||||
return Base::reprojectionError(cameras, point_);
|
||||
}
|
||||
|
||||
/// Calculate vector of re-projection errors, before applying noise model
|
||||
Vector reprojectionError(const Values& values) const {
|
||||
Cameras myCameras;
|
||||
bool nonDegenerate = computeCamerasAndTriangulate(values, myCameras);
|
||||
if (nonDegenerate)
|
||||
return reprojectionError(myCameras);
|
||||
else
|
||||
return zero(myCameras.size() * 2);
|
||||
}
|
||||
|
||||
/**
|
||||
* Calculate the error of the factor.
|
||||
* This is the log-likelihood, e.g. \f$ 0.5(h(x)-z)^2/\sigma^2 \f$ in case of Gaussian.
|
||||
* In this class, we take the raw prediction error \f$ h(x)-z \f$, ask the noise model
|
||||
* to transform it to \f$ (h(x)-z)^2/\sigma^2 \f$, and then multiply by 0.5.
|
||||
*/
|
||||
double totalReprojectionError(const Cameras& cameras,
|
||||
boost::optional<Point3> externalPoint = boost::none) const {
|
||||
|
||||
size_t nrCameras;
|
||||
if (externalPoint) {
|
||||
nrCameras = this->keys_.size();
|
||||
point_ = *externalPoint;
|
||||
degenerate_ = false;
|
||||
cheiralityException_ = false;
|
||||
} else {
|
||||
nrCameras = this->triangulateSafe(cameras);
|
||||
}
|
||||
|
||||
if (nrCameras < 2
|
||||
|| (!this->manageDegeneracy_
|
||||
&& (this->cheiralityException_ || this->degenerate_))) {
|
||||
// if we don't want to manage the exceptions we discard the factor
|
||||
// std::cout << "In error evaluation: exception" << std::endl;
|
||||
return 0.0;
|
||||
}
|
||||
|
||||
if (this->cheiralityException_) { // if we want to manage the exceptions with rotation-only factors
|
||||
std::cout
|
||||
<< "SmartProjectionHessianFactor: cheirality exception (this should not happen if CheiralityException is disabled)!"
|
||||
<< std::endl;
|
||||
this->degenerate_ = true;
|
||||
}
|
||||
|
||||
if (this->degenerate_) {
|
||||
// return 0.0; // TODO: this maybe should be zero?
|
||||
std::cout
|
||||
<< "SmartProjectionHessianFactor: trying to manage degeneracy (this should not happen is manageDegeneracy is disabled)!"
|
||||
<< std::endl;
|
||||
size_t i = 0;
|
||||
double overallError = 0;
|
||||
BOOST_FOREACH(const Camera& camera, cameras) {
|
||||
const Point2& zi = this->measured_.at(i);
|
||||
if (i == 0) // first pose
|
||||
this->point_ = camera.backprojectPointAtInfinity(zi); // 3D parametrization of point at infinity
|
||||
Point2 reprojectionError(
|
||||
camera.projectPointAtInfinity(this->point_) - zi);
|
||||
overallError += 0.5
|
||||
* this->noise_.at(i)->distance(reprojectionError.vector());
|
||||
i += 1;
|
||||
}
|
||||
return overallError;
|
||||
} else {
|
||||
// Just use version in base class
|
||||
return Base::totalReprojectionError(cameras, point_);
|
||||
}
|
||||
}
|
||||
|
||||
/// Cameras are computed in derived class
|
||||
virtual Cameras cameras(const Values& values) const = 0;
|
||||
|
||||
/** return the landmark */
|
||||
boost::optional<Point3> point() const {
|
||||
return point_;
|
||||
}
|
||||
|
||||
/** COMPUTE the landmark */
|
||||
boost::optional<Point3> point(const Values& values) const {
|
||||
triangulateSafe(values);
|
||||
return point_;
|
||||
}
|
||||
|
||||
/** return the degenerate state */
|
||||
inline bool isDegenerate() const {
|
||||
return (cheiralityException_ || degenerate_);
|
||||
}
|
||||
|
||||
/** return the cheirality status flag */
|
||||
inline bool isPointBehindCamera() const {
|
||||
return cheiralityException_;
|
||||
}
|
||||
/** return chirality verbosity */
|
||||
inline bool verboseCheirality() const {
|
||||
return verboseCheirality_;
|
||||
}
|
||||
|
||||
/** return flag for throwing cheirality exceptions */
|
||||
inline bool throwCheirality() const {
|
||||
return throwCheirality_;
|
||||
}
|
||||
|
||||
private:
|
||||
|
||||
/// Serialization function
|
||||
friend class boost::serialization::access;
|
||||
template<class ARCHIVE>
|
||||
void serialize(ARCHIVE & ar, const unsigned int version) {
|
||||
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
|
||||
ar & BOOST_SERIALIZATION_NVP(throwCheirality_);
|
||||
ar & BOOST_SERIALIZATION_NVP(verboseCheirality_);
|
||||
}
|
||||
};
|
||||
|
||||
} // \ namespace gtsam
|
||||
|
|
@ -0,0 +1,215 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file SmartStereoProjectionPoseFactor.h
|
||||
* @brief Produces an Hessian factors on POSES from monocular measurements of a single landmark
|
||||
* @author Luca Carlone
|
||||
* @author Chris Beall
|
||||
* @author Zsolt Kira
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "SmartStereoProjectionFactor.h"
|
||||
|
||||
namespace gtsam {
|
||||
/**
|
||||
*
|
||||
* @addtogroup SLAM
|
||||
*
|
||||
* If you are using the factor, please cite:
|
||||
* L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating conditionally
|
||||
* independent sets in factor graphs: a unifying perspective based on smart factors,
|
||||
* Int. Conf. on Robotics and Automation (ICRA), 2014.
|
||||
*
|
||||
*/
|
||||
|
||||
/**
|
||||
* The calibration is known here. The factor only constraints poses (variable dimension is 6)
|
||||
* @addtogroup SLAM
|
||||
*/
|
||||
template<class POSE, class LANDMARK, class CALIBRATION>
|
||||
class SmartStereoProjectionPoseFactor: public SmartStereoProjectionFactor<POSE, LANDMARK, CALIBRATION, 6> {
|
||||
protected:
|
||||
|
||||
LinearizationMode linearizeTo_; ///< How to linearize the factor (HESSIAN, JACOBIAN_SVD, JACOBIAN_Q)
|
||||
|
||||
std::vector<boost::shared_ptr<CALIBRATION> > K_all_; ///< shared pointer to calibration object (one for each camera)
|
||||
|
||||
public:
|
||||
|
||||
/// shorthand for base class type
|
||||
typedef SmartStereoProjectionFactor<POSE, LANDMARK, CALIBRATION, 6> Base;
|
||||
|
||||
/// shorthand for this class
|
||||
typedef SmartStereoProjectionPoseFactor<POSE, LANDMARK, CALIBRATION> This;
|
||||
|
||||
/// shorthand for a smart pointer to a factor
|
||||
typedef boost::shared_ptr<This> shared_ptr;
|
||||
|
||||
/**
|
||||
* Constructor
|
||||
* @param rankTol tolerance used to check if point triangulation is degenerate
|
||||
* @param linThreshold threshold on relative pose changes used to decide whether to relinearize (selective relinearization)
|
||||
* @param manageDegeneracy is true, in presence of degenerate triangulation, the factor is converted to a rotation-only constraint,
|
||||
* otherwise the factor is simply neglected
|
||||
* @param enableEPI if set to true linear triangulation is refined with embedded LM iterations
|
||||
* @param body_P_sensor is the transform from body to sensor frame (default identity)
|
||||
*/
|
||||
SmartStereoProjectionPoseFactor(const double rankTol = 1,
|
||||
const double linThreshold = -1, const bool manageDegeneracy = false,
|
||||
const bool enableEPI = false, boost::optional<POSE> body_P_sensor = boost::none,
|
||||
LinearizationMode linearizeTo = HESSIAN, double landmarkDistanceThreshold = 1e10,
|
||||
double dynamicOutlierRejectionThreshold = -1) :
|
||||
Base(rankTol, linThreshold, manageDegeneracy, enableEPI, body_P_sensor,
|
||||
landmarkDistanceThreshold, dynamicOutlierRejectionThreshold), linearizeTo_(linearizeTo) {}
|
||||
|
||||
/** Virtual destructor */
|
||||
virtual ~SmartStereoProjectionPoseFactor() {}
|
||||
|
||||
/**
|
||||
* add a new measurement and pose key
|
||||
* @param measured is the 2m dimensional location of the projection of a single landmark in the m view (the measurement)
|
||||
* @param poseKey is key corresponding to the camera observing the same landmark
|
||||
* @param noise_i is the measurement noise
|
||||
* @param K_i is the (known) camera calibration
|
||||
*/
|
||||
void add(const Point2 measured_i, const Key poseKey_i,
|
||||
const SharedNoiseModel noise_i,
|
||||
const boost::shared_ptr<CALIBRATION> K_i) {
|
||||
Base::add(measured_i, poseKey_i, noise_i);
|
||||
K_all_.push_back(K_i);
|
||||
}
|
||||
|
||||
/**
|
||||
* Variant of the previous one in which we include a set of measurements
|
||||
* @param measurements vector of the 2m dimensional location of the projection of a single landmark in the m view (the measurement)
|
||||
* @param poseKeys vector of keys corresponding to the camera observing the same landmark
|
||||
* @param noises vector of measurement noises
|
||||
* @param Ks vector of calibration objects
|
||||
*/
|
||||
void add(std::vector<Point2> measurements, std::vector<Key> poseKeys,
|
||||
std::vector<SharedNoiseModel> noises,
|
||||
std::vector<boost::shared_ptr<CALIBRATION> > Ks) {
|
||||
Base::add(measurements, poseKeys, noises);
|
||||
for (size_t i = 0; i < measurements.size(); i++) {
|
||||
K_all_.push_back(Ks.at(i));
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Variant of the previous one in which we include a set of measurements with the same noise and calibration
|
||||
* @param mmeasurements vector of the 2m dimensional location of the projection of a single landmark in the m view (the measurement)
|
||||
* @param poseKeys vector of keys corresponding to the camera observing the same landmark
|
||||
* @param noise measurement noise (same for all measurements)
|
||||
* @param K the (known) camera calibration (same for all measurements)
|
||||
*/
|
||||
void add(std::vector<Point2> measurements, std::vector<Key> poseKeys,
|
||||
const SharedNoiseModel noise, const boost::shared_ptr<CALIBRATION> K) {
|
||||
for (size_t i = 0; i < measurements.size(); i++) {
|
||||
Base::add(measurements.at(i), poseKeys.at(i), noise);
|
||||
K_all_.push_back(K);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* print
|
||||
* @param s optional string naming the factor
|
||||
* @param keyFormatter optional formatter useful for printing Symbols
|
||||
*/
|
||||
void print(const std::string& s = "", const KeyFormatter& keyFormatter =
|
||||
DefaultKeyFormatter) const {
|
||||
std::cout << s << "SmartStereoProjectionPoseFactor, z = \n ";
|
||||
BOOST_FOREACH(const boost::shared_ptr<CALIBRATION>& K, K_all_)
|
||||
K->print("calibration = ");
|
||||
Base::print("", keyFormatter);
|
||||
}
|
||||
|
||||
/// equals
|
||||
virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const {
|
||||
const This *e = dynamic_cast<const This*>(&p);
|
||||
|
||||
return e && Base::equals(p, tol);
|
||||
}
|
||||
|
||||
/// get the dimension of the factor
|
||||
virtual size_t dim() const {
|
||||
return 6 * this->keys_.size();
|
||||
}
|
||||
|
||||
/**
|
||||
* Collect all cameras involved in this factor
|
||||
* @param values Values structure which must contain camera poses corresponding
|
||||
* to keys involved in this factor
|
||||
* @return vector of Values
|
||||
*/
|
||||
typename Base::Cameras cameras(const Values& values) const {
|
||||
typename Base::Cameras cameras;
|
||||
size_t i=0;
|
||||
BOOST_FOREACH(const Key& k, this->keys_) {
|
||||
Pose3 pose = values.at<Pose3>(k);
|
||||
typename Base::Camera camera(pose, *K_all_[i++]);
|
||||
cameras.push_back(camera);
|
||||
}
|
||||
return cameras;
|
||||
}
|
||||
|
||||
/**
|
||||
* Linearize to Gaussian Factor
|
||||
* @param values Values structure which must contain camera poses for this factor
|
||||
* @return
|
||||
*/
|
||||
virtual boost::shared_ptr<GaussianFactor> linearize(
|
||||
const Values& values) const {
|
||||
// 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;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* error calculates the error of the factor.
|
||||
*/
|
||||
virtual double error(const Values& values) const {
|
||||
if (this->active(values)) {
|
||||
return this->totalReprojectionError(cameras(values));
|
||||
} else { // else of active flag
|
||||
return 0.0;
|
||||
}
|
||||
}
|
||||
|
||||
/** return the calibration object */
|
||||
inline const std::vector<boost::shared_ptr<CALIBRATION> > calibration() const {
|
||||
return K_all_;
|
||||
}
|
||||
|
||||
private:
|
||||
|
||||
/// Serialization function
|
||||
friend class boost::serialization::access;
|
||||
template<class ARCHIVE>
|
||||
void serialize(ARCHIVE & ar, const unsigned int version) {
|
||||
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
|
||||
ar & BOOST_SERIALIZATION_NVP(K_all_);
|
||||
}
|
||||
|
||||
}; // end of class declaration
|
||||
|
||||
} // \ namespace gtsam
|
||||
File diff suppressed because it is too large
Load Diff
Loading…
Reference in New Issue