Loading...
Searching...
No Matches
ProjectionFactor.h
Go to the documentation of this file.
/* ----------------------------------------------------------------------------
* 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/PinholeCamera.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <boost/optional.hpp>
namespace gtsam {
template <class POSE = Pose3, class LANDMARK = Point3,
class CALIBRATION = Cal3_S2>
class GenericProjectionFactor: public NoiseModelFactorN<POSE, LANDMARK> {
protected:
// Keep a copy of measurement and calibration for I/O
Point2 measured_;
boost::shared_ptr<CALIBRATION> K_;
boost::optional<POSE> body_P_sensor_;
// verbosity handling for Cheirality Exceptions
bool throwCheirality_;
bool verboseCheirality_;
public:
typedef NoiseModelFactorN<POSE, LANDMARK> Base;
typedef GenericProjectionFactor<POSE, LANDMARK, CALIBRATION> This;
typedef boost::shared_ptr<This> shared_ptr;
GenericProjectionFactor() :
measured_(0, 0), throwCheirality_(false), verboseCheirality_(false) {
}
GenericProjectionFactor(const Point2& measured, const SharedNoiseModel& model,
Key poseKey, Key pointKey, const boost::shared_ptr<CALIBRATION>& K,
boost::optional<POSE> body_P_sensor = boost::none) :
Base(model, poseKey, pointKey), measured_(measured), K_(K), body_P_sensor_(body_P_sensor),
throwCheirality_(false), verboseCheirality_(false) {}
GenericProjectionFactor(const Point2& measured, const SharedNoiseModel& model,
Key poseKey, Key pointKey, const boost::shared_ptr<CALIBRATION>& K,
bool throwCheirality, bool verboseCheirality,
boost::optional<POSE> body_P_sensor = boost::none) :
Base(model, poseKey, pointKey), measured_(measured), K_(K), body_P_sensor_(body_P_sensor),
throwCheirality_(throwCheirality), verboseCheirality_(verboseCheirality) {}
~GenericProjectionFactor() 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 << "GenericProjectionFactor, z = ";
traits<Point2>::Print(measured_);
if(this->body_P_sensor_)
this->body_P_sensor_->print(" sensor pose in body frame: ");
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)
&& traits<Point2>::Equals(this->measured_, e->measured_, tol)
&& this->K_->equals(*e->K_, tol)
&& ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_)));
}
Vector evaluateError(const Pose3& pose, const Point3& point,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) const override {
try {
if(body_P_sensor_) {
if(H1) {
gtsam::Matrix H0;
PinholeCamera<CALIBRATION> camera(pose.compose(*body_P_sensor_, H0), *K_);
Point2 reprojectionError(camera.project(point, H1, H2, boost::none) - measured_);
*H1 = *H1 * H0;
return reprojectionError;
} else {
PinholeCamera<CALIBRATION> camera(pose.compose(*body_P_sensor_), *K_);
return camera.project(point, H1, H2, boost::none) - measured_;
}
} else {
PinholeCamera<CALIBRATION> camera(pose, *K_);
return camera.project(point, H1, H2, boost::none) - measured_;
}
} catch( CheiralityException& e) {
if (H1) *H1 = Matrix::Zero(2,6);
if (H2) *H2 = Matrix::Zero(2,3);
if (verboseCheirality_)
std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) <<
" moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl;
if (throwCheirality_)
throw CheiralityException(this->key2());
}
return Vector2::Constant(2.0 * K_->fx());
}
const Point2& measured() const {
return measured_;
}
const boost::shared_ptr<CALIBRATION> calibration() const {
return K_;
}
const boost::optional<POSE>& body_P_sensor() const {
return body_P_sensor_;
}
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(measured_);
ar & BOOST_SERIALIZATION_NVP(K_);
ar & BOOST_SERIALIZATION_NVP(body_P_sensor_);
ar & BOOST_SERIALIZATION_NVP(throwCheirality_);
ar & BOOST_SERIALIZATION_NVP(verboseCheirality_);
}
public:
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
};
template<class POSE, class LANDMARK, class CALIBRATION>
struct traits<GenericProjectionFactor<POSE, LANDMARK, CALIBRATION> > :
public Testable<GenericProjectionFactor<POSE, LANDMARK, CALIBRATION> > {
};
} // \ namespace gtsam
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
This marks a GTSAM object to require alignment.
Definition types.h:308
Point3.h
3D Point
Cal3_S2.h
The most common 5DOF 3D->2D calibration.
PinholeCamera.h
Base class for all pinhole cameras.
Pose3.h
3D Pose
NonlinearFactor.h
Non-linear factor base classes.
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
gtsam::Point2
Vector2 Point2
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point2 to Vector2...
Definition Point2.h:27
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::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
gtsam::Cal3_S2
The most common 5DOF 3D->2D calibration.
Definition Cal3_S2.h:34
gtsam::CheiralityException
Definition CalibratedCamera.h:32
gtsam::PinholeCamera
A pinhole camera class that has a Pose3 and a Calibration.
Definition PinholeCamera.h:33
gtsam::PinholeBaseK::project
Point2 project(const Point3 &pw, OptionalJacobian< 2, 6 > Dpose=boost::none, OptionalJacobian< 2, 3 > Dpoint=boost::none, OptionalJacobian< 2, DimK > Dcal=boost::none) const
project a 3D point from world coordinates into the image
Definition PinholePose.h:118
gtsam::Pose3
A 3D pose (R,t) : (Rot3,Point3).
Definition Pose3.h:37
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::NoiseModelFactorN< Pose3, Point3 >::NoiseModelFactorN
NoiseModelFactorN()
Definition NonlinearFactor.h:469
gtsam::GenericProjectionFactor
Non-linear factor for a constraint derived from a 2D measurement.
Definition ProjectionFactor.h:40
gtsam::GenericProjectionFactor< POSE, LANDMARK, CALIBRATION >::verboseCheirality
bool verboseCheirality() const
Definition ProjectionFactor.h:181
gtsam::GenericProjectionFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition ProjectionFactor.h:116
gtsam::GenericProjectionFactor::equals
bool equals(const NonlinearFactor &p, double tol=1e-9) const override
equals
Definition ProjectionFactor.h:125
gtsam::GenericProjectionFactor< POSE, LANDMARK, CALIBRATION >::measured_
Point2 measured_
Definition ProjectionFactor.h:44
gtsam::GenericProjectionFactor::This
GenericProjectionFactor< POSE, LANDMARK, CALIBRATION > This
shorthand for this class
Definition ProjectionFactor.h:58
gtsam::GenericProjectionFactor< POSE, LANDMARK, CALIBRATION >::measured
const Point2 & measured() const
Definition ProjectionFactor.h:166
gtsam::GenericProjectionFactor< POSE, LANDMARK, CALIBRATION >::body_P_sensor_
boost::optional< POSE > body_P_sensor_
Definition ProjectionFactor.h:46
gtsam::GenericProjectionFactor< POSE, LANDMARK, CALIBRATION >::K_
boost::shared_ptr< CALIBRATION > K_
Definition ProjectionFactor.h:45
gtsam::GenericProjectionFactor::~GenericProjectionFactor
~GenericProjectionFactor() override
Virtual destructor.
Definition ProjectionFactor.h:104
gtsam::GenericProjectionFactor::evaluateError
Vector evaluateError(const Pose3 &pose, const Point3 &point, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none) const override
Evaluate error h(x)-z and optionally derivatives.
Definition ProjectionFactor.h:135
gtsam::GenericProjectionFactor::GenericProjectionFactor
GenericProjectionFactor()
Default constructor.
Definition ProjectionFactor.h:64
gtsam::GenericProjectionFactor< POSE, LANDMARK, CALIBRATION >::throwCheirality_
bool throwCheirality_
Definition ProjectionFactor.h:49
gtsam::GenericProjectionFactor::Base
NoiseModelFactorN< POSE, LANDMARK > Base
shorthand for base class type
Definition ProjectionFactor.h:55
gtsam::GenericProjectionFactor< POSE, LANDMARK, CALIBRATION >::body_P_sensor
const boost::optional< POSE > & body_P_sensor() const
Definition ProjectionFactor.h:176
gtsam::GenericProjectionFactor< POSE, LANDMARK, CALIBRATION >::throwCheirality
bool throwCheirality() const
Definition ProjectionFactor.h:184
gtsam::GenericProjectionFactor::GenericProjectionFactor
GenericProjectionFactor(const Point2 &measured, const SharedNoiseModel &model, Key poseKey, Key pointKey, const boost::shared_ptr< CALIBRATION > &K, bool throwCheirality, bool verboseCheirality, boost::optional< POSE > body_P_sensor=boost::none)
Constructor with exception-handling flags TODO: Mark argument order standard (keys,...
Definition ProjectionFactor.h:96
gtsam::GenericProjectionFactor::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition ProjectionFactor.h:107
gtsam::GenericProjectionFactor< POSE, LANDMARK, CALIBRATION >::shared_ptr
boost::shared_ptr< This > shared_ptr
Definition ProjectionFactor.h:61
gtsam::GenericProjectionFactor< POSE, LANDMARK, CALIBRATION >::verboseCheirality_
bool verboseCheirality_
Definition ProjectionFactor.h:50
gtsam::GenericProjectionFactor::GenericProjectionFactor
GenericProjectionFactor(const Point2 &measured, const SharedNoiseModel &model, Key poseKey, Key pointKey, const boost::shared_ptr< CALIBRATION > &K, boost::optional< POSE > body_P_sensor=boost::none)
Constructor TODO: Mark argument order standard (keys, measurement, parameters).
Definition ProjectionFactor.h:78
gtsam::GenericProjectionFactor::calibration
const boost::shared_ptr< CALIBRATION > calibration() const
return the calibration object
Definition ProjectionFactor.h:171