Loading...
Searching...
No Matches
TriangulationFactor.h
/* ----------------------------------------------------------------------------
* 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
* -------------------------------------------------------------------------- */
#pragma once
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/geometry/CalibratedCamera.h>
#include <boost/make_shared.hpp>
#include <boost/lexical_cast.hpp>
namespace gtsam {
template<class CAMERA>
class TriangulationFactor: public NoiseModelFactorN<Point3> {
public:
using Camera = CAMERA;
protected:
using Base = NoiseModelFactorN<Point3>;
using This = TriangulationFactor<CAMERA>;
using Measurement = typename CAMERA::Measurement;
// Keep a copy of measurement and calibration for I/O
const CAMERA camera_;
const Measurement measured_;
// verbosity handling for Cheirality Exceptions
const bool throwCheirality_;
const bool verboseCheirality_;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
using shared_ptr = boost::shared_ptr<This>;
TriangulationFactor() :
throwCheirality_(false), verboseCheirality_(false) {
}
TriangulationFactor(const CAMERA& camera, const Measurement& measured,
const SharedNoiseModel& model, Key pointKey, bool throwCheirality = false,
bool verboseCheirality = false) :
Base(model, pointKey), camera_(camera), measured_(measured), throwCheirality_(
throwCheirality), verboseCheirality_(verboseCheirality) {
if (model && model->dim() != traits<Measurement>::dimension)
throw std::invalid_argument(
"TriangulationFactor must be created with "
+ boost::lexical_cast<std::string>((int) traits<Measurement>::dimension)
+ "-dimensional noise model.");
}
~TriangulationFactor() override {
}
gtsam::NonlinearFactor::shared_ptr clone() const override {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
}
void print(const std::string& s = "", const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const override {
std::cout << s << "TriangulationFactor,";
camera_.print("camera");
traits<Measurement>::Print(measured_, "z");
Base::print("", keyFormatter);
}
bool equals(const NonlinearFactor& p, double tol = 1e-9) const override {
const This *e = dynamic_cast<const This*>(&p);
return e && Base::equals(p, tol) && this->camera_.equals(e->camera_, tol)
&& traits<Measurement>::Equals(this->measured_, e->measured_, tol);
}
Vector evaluateError(const Point3& point, boost::optional<Matrix&> H2 =
boost::none) const override {
try {
return traits<Measurement>::Local(measured_, camera_.project2(point, boost::none, H2));
} catch (CheiralityException& e) {
if (H2)
*H2 = Matrix::Zero(traits<Measurement>::dimension, 3);
if (verboseCheirality_)
std::cout << e.what() << ": Landmark "
<< DefaultKeyFormatter(this->key()) << " moved behind camera"
<< std::endl;
if (throwCheirality_)
throw e;
return camera_.defaultErrorWhenTriangulatingBehindCamera();
}
}
mutable VerticalBlockMatrix Ab;
mutable Matrix A;
mutable Vector b;
boost::shared_ptr<GaussianFactor> linearize(const Values& x) const override {
// Only linearize if the factor is active
if (!this->active(x))
return boost::shared_ptr<JacobianFactor>();
// Allocate memory for Jacobian factor, do only once
if (Ab.rows() == 0) {
std::vector<size_t> dimensions(1, 3);
Ab = VerticalBlockMatrix(dimensions, traits<Measurement>::dimension, true);
A.resize(traits<Measurement>::dimension,3);
b.resize(traits<Measurement>::dimension);
}
// Would be even better if we could pass blocks to project
const Point3& point = x.at<Point3>(key());
b = traits<Measurement>::Local(camera_.project2(point, boost::none, A), measured_);
if (noiseModel_)
this->noiseModel_->WhitenSystem(A, b);
Ab(0) = A;
Ab(1) = b;
return boost::make_shared<JacobianFactor>(this->keys_, Ab);
}
const Measurement& measured() const {
return measured_;
}
inline bool verboseCheirality() const {
return verboseCheirality_;
}
inline bool throwCheirality() const {
return throwCheirality_;
}
private:
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(camera_);
ar & BOOST_SERIALIZATION_NVP(measured_);
ar & BOOST_SERIALIZATION_NVP(throwCheirality_);
ar & BOOST_SERIALIZATION_NVP(verboseCheirality_);
}
};
} // \ namespace gtsam
CalibratedCamera.h
Calibrated camera for which only pose is unknown.
NonlinearFactor.h
Non-linear factor base classes.
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
gtsam::Point3
Vector3 Point3
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point3 to Vector3...
Definition Point3.h:36
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition NoiseModel.h:724
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
gtsam::VerticalBlockMatrix
This class stores a dense matrix and allows it to be accessed as a collection of vertical blocks.
Definition VerticalBlockMatrix.h:43
gtsam::CheiralityException
Definition CalibratedCamera.h:32
gtsam::Factor::keys_
KeyVector keys_
The keys involved in this factor.
Definition Factor.h:85
gtsam::Factor::print
virtual void print(const std::string &s="Factor", const KeyFormatter &formatter=DefaultKeyFormatter) const
print
Definition Factor.cpp:29
gtsam::Factor::equals
bool equals(const This &other, double tol=1e-9) const
check equality
Definition Factor.cpp:42
gtsam::NonlinearFactor
Nonlinear factor base class.
Definition NonlinearFactor.h:42
gtsam::NonlinearFactor::active
virtual bool active(const Values &) const
Checks whether a factor should be used based on a set of values.
Definition NonlinearFactor.h:118
gtsam::NoiseModelFactorN< Point3 >::NoiseModelFactorN
NoiseModelFactorN()
Definition NonlinearFactor.h:469
gtsam::NoiseModelFactorN< Point3 >::key
Key key() const
Definition NonlinearFactor.h:518
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition Values.h:65
gtsam::Values::at
const ValueType at(Key j) const
Retrieve a variable by key j.
Definition Values-inl.h:361
gtsam::TriangulationFactor::Ab
VerticalBlockMatrix Ab
thread-safe (?) scratch memory for linearize
Definition TriangulationFactor.h:140
gtsam::TriangulationFactor::throwCheirality_
const bool throwCheirality_
If true, rethrows Cheirality exceptions (default: false).
Definition TriangulationFactor.h:56
gtsam::TriangulationFactor::throwCheirality
bool throwCheirality() const
return flag for throwing cheirality exceptions
Definition TriangulationFactor.h:185
gtsam::TriangulationFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition TriangulationFactor.h:106
gtsam::TriangulationFactor::evaluateError
Vector evaluateError(const Point3 &point, boost::optional< Matrix & > H2=boost::none) const override
Evaluate error h(x)-z and optionally derivatives.
Definition TriangulationFactor.h:122
gtsam::TriangulationFactor::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition TriangulationFactor.h:96
gtsam::TriangulationFactor::linearize
boost::shared_ptr< GaussianFactor > linearize(const Values &x) const override
Linearize to a JacobianFactor, does not support constrained noise model !
Definition TriangulationFactor.h:149
gtsam::TriangulationFactor::TriangulationFactor
TriangulationFactor(const CAMERA &camera, const Measurement &measured, const SharedNoiseModel &model, Key pointKey, bool throwCheirality=false, bool verboseCheirality=false)
Constructor with exception-handling flags.
Definition TriangulationFactor.h:79
gtsam::TriangulationFactor::Measurement
typename CAMERA::Measurement Measurement
shorthand for measurement type, e.g. Point2 or StereoPoint2
Definition TriangulationFactor.h:49
gtsam::TriangulationFactor::measured
const Measurement & measured() const
return the measurement
Definition TriangulationFactor.h:175
gtsam::TriangulationFactor::verboseCheirality
bool verboseCheirality() const
return verbosity
Definition TriangulationFactor.h:180
gtsam::TriangulationFactor::Camera
CAMERA Camera
CAMERA type.
Definition TriangulationFactor.h:38
gtsam::TriangulationFactor::equals
bool equals(const NonlinearFactor &p, double tol=1e-9) const override
equals
Definition TriangulationFactor.h:115
gtsam::TriangulationFactor::~TriangulationFactor
~TriangulationFactor() override
Virtual destructor.
Definition TriangulationFactor.h:92
gtsam::TriangulationFactor::Base
NoiseModelFactorN< Point3 > Base
shorthand for base class type
Definition TriangulationFactor.h:43
gtsam::TriangulationFactor::verboseCheirality_
const bool verboseCheirality_
If true, prints text for Cheirality exceptions (default: false).
Definition TriangulationFactor.h:57
gtsam::TriangulationFactor::access
friend class boost::serialization::access
Serialization function.
Definition TriangulationFactor.h:192
gtsam::TriangulationFactor::TriangulationFactor
TriangulationFactor()
Default constructor.
Definition TriangulationFactor.h:66
gtsam::TriangulationFactor::This
TriangulationFactor< CAMERA > This
shorthand for this class
Definition TriangulationFactor.h:46
gtsam::TriangulationFactor::measured_
const Measurement measured_
2D measurement
Definition TriangulationFactor.h:53
gtsam::TriangulationFactor::shared_ptr
boost::shared_ptr< This > shared_ptr
shorthand for a smart pointer to a factor
Definition TriangulationFactor.h:63
gtsam::TriangulationFactor::camera_
const CAMERA camera_
CAMERA in which this landmark was seen.
Definition TriangulationFactor.h:52