Loading...
Searching...
No Matches
StereoFactor.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/StereoCamera.h>
namespace gtsam {
template<class POSE, class LANDMARK>
class GenericStereoFactor: public NoiseModelFactorN<POSE, LANDMARK> {
private:
// Keep a copy of measurement and calibration for I/O
StereoPoint2 measured_;
Cal3_S2Stereo::shared_ptr K_;
boost::optional<POSE> body_P_sensor_;
// verbosity handling for Cheirality Exceptions
bool throwCheirality_;
bool verboseCheirality_;
public:
// shorthand for base class type
typedef NoiseModelFactorN<POSE, LANDMARK> Base;
typedef GenericStereoFactor<POSE, LANDMARK> This;
typedef boost::shared_ptr<GenericStereoFactor> shared_ptr;
typedef POSE CamPose;
GenericStereoFactor() : K_(new Cal3_S2Stereo(444, 555, 666, 777, 888, 1.0)),
throwCheirality_(false), verboseCheirality_(false) {}
GenericStereoFactor(const StereoPoint2& measured, const SharedNoiseModel& model,
Key poseKey, Key landmarkKey, const Cal3_S2Stereo::shared_ptr& K,
boost::optional<POSE> body_P_sensor = boost::none) :
Base(model, poseKey, landmarkKey), measured_(measured), K_(K), body_P_sensor_(body_P_sensor),
throwCheirality_(false), verboseCheirality_(false) {}
GenericStereoFactor(const StereoPoint2& measured, const SharedNoiseModel& model,
Key poseKey, Key landmarkKey, const Cal3_S2Stereo::shared_ptr& K,
bool throwCheirality, bool verboseCheirality,
boost::optional<POSE> body_P_sensor = boost::none) :
Base(model, poseKey, landmarkKey), measured_(measured), K_(K), body_P_sensor_(body_P_sensor),
throwCheirality_(throwCheirality), verboseCheirality_(verboseCheirality) {}
~GenericStereoFactor() 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 {
Base::print(s, keyFormatter);
measured_.print(s + ".z");
if(this->body_P_sensor_)
this->body_P_sensor_->print(" sensor pose in body frame: ");
}
bool equals(const NonlinearFactor& f, double tol = 1e-9) const override {
const GenericStereoFactor* e = dynamic_cast<const GenericStereoFactor*> (&f);
return e
&& Base::equals(f)
&& measured_.equals(e->measured_, 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;
StereoCamera stereoCam(pose.compose(*body_P_sensor_, H0), K_);
StereoPoint2 reprojectionError(stereoCam.project(point, H1, H2) - measured_);
*H1 = *H1 * H0;
return reprojectionError.vector();
} else {
StereoCamera stereoCam(pose.compose(*body_P_sensor_), K_);
return (stereoCam.project(point, H1, H2) - measured_).vector();
}
} else {
StereoCamera stereoCam(pose, K_);
return (stereoCam.project(point, H1, H2) - measured_).vector();
}
} catch(StereoCheiralityException& e) {
if (H1) *H1 = Matrix::Zero(3,6);
if (H2) *H2 = Z_3x3;
if (verboseCheirality_)
std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) <<
" moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl;
if (throwCheirality_)
throw StereoCheiralityException(this->key2());
}
return Vector3::Constant(2.0 * K_->fx());
}
const StereoPoint2& measured() const {
return measured_;
}
inline const Cal3_S2Stereo::shared_ptr calibration() const {
return K_;
}
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*/) {
// NoiseModelFactor2 instead of NoiseModelFactorN for backward compatibility
ar & boost::serialization::make_nvp("NoiseModelFactor2",
boost::serialization::base_object<Base>(*this));
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_);
}
};
template<class T1, class T2>
struct traits<GenericStereoFactor<T1, T2> > : public Testable<GenericStereoFactor<T1, T2> > {};
} // \ namespace gtsam
StereoCamera.h
A Stereo Camera based on two Simple Cameras.
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::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
gtsam::Cal3_S2Stereo
The most common 5DOF 3D->2D calibration, stereo version.
Definition Cal3_S2Stereo.h:30
gtsam::Pose3
A 3D pose (R,t) : (Rot3,Point3).
Definition Pose3.h:37
gtsam::StereoCheiralityException
Definition StereoCamera.h:26
gtsam::StereoCamera
A stereo camera class, parameterize by left camera pose and stereo calibration.
Definition StereoCamera.h:47
gtsam::StereoCamera::project
StereoPoint2 project(const Point3 &point) const
Project 3D point to StereoPoint2 (uL,uR,v).
Definition StereoCamera.cpp:32
gtsam::StereoPoint2
A 2D stereo point, v will be same for rectified images.
Definition StereoPoint2.h:32
gtsam::StereoPoint2::vector
Vector3 vector() const
convert to vector
Definition StereoPoint2.h:115
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< POSE, LANDMARK >::NoiseModelFactorN
NoiseModelFactorN()
Definition NonlinearFactor.h:469
gtsam::GenericStereoFactor
A Generic Stereo Factor.
Definition StereoFactor.h:31
gtsam::GenericStereoFactor::throwCheirality
bool throwCheirality() const
return flag for throwing cheirality exceptions
Definition StereoFactor.h:166
gtsam::GenericStereoFactor::evaluateError
Vector evaluateError(const Pose3 &pose, const Point3 &point, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none) const override
h(x)-z
Definition StereoFactor.h:122
gtsam::GenericStereoFactor::equals
bool equals(const NonlinearFactor &f, double tol=1e-9) const override
equals
Definition StereoFactor.h:113
gtsam::GenericStereoFactor::calibration
const Cal3_S2Stereo::shared_ptr calibration() const
return the calibration object
Definition StereoFactor.h:158
gtsam::GenericStereoFactor::GenericStereoFactor
GenericStereoFactor(const StereoPoint2 &measured, const SharedNoiseModel &model, Key poseKey, Key landmarkKey, const Cal3_S2Stereo::shared_ptr &K, bool throwCheirality, bool verboseCheirality, boost::optional< POSE > body_P_sensor=boost::none)
Constructor with exception-handling flags.
Definition StereoFactor.h:83
gtsam::GenericStereoFactor::This
GenericStereoFactor< POSE, LANDMARK > This
typedef for this class (with templates)
Definition StereoFactor.h:47
gtsam::GenericStereoFactor::~GenericStereoFactor
~GenericStereoFactor() override
Virtual destructor.
Definition StereoFactor.h:91
gtsam::GenericStereoFactor::verboseCheirality
bool verboseCheirality() const
return verbosity
Definition StereoFactor.h:163
gtsam::GenericStereoFactor::shared_ptr
boost::shared_ptr< GenericStereoFactor > shared_ptr
typedef for shared pointer to this object
Definition StereoFactor.h:48
gtsam::GenericStereoFactor::measured
const StereoPoint2 & measured() const
return the measured
Definition StereoFactor.h:153
gtsam::GenericStereoFactor::GenericStereoFactor
GenericStereoFactor()
Default constructor.
Definition StereoFactor.h:54
gtsam::GenericStereoFactor::access
friend class boost::serialization::access
Serialization function.
Definition StereoFactor.h:170
gtsam::GenericStereoFactor::GenericStereoFactor
GenericStereoFactor(const StereoPoint2 &measured, const SharedNoiseModel &model, Key poseKey, Key landmarkKey, const Cal3_S2Stereo::shared_ptr &K, boost::optional< POSE > body_P_sensor=boost::none)
Constructor.
Definition StereoFactor.h:66
gtsam::GenericStereoFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition StereoFactor.h:103
gtsam::GenericStereoFactor::CamPose
POSE CamPose
typedef for Pose Lie Value type
Definition StereoFactor.h:49
gtsam::GenericStereoFactor::Base
NoiseModelFactorN< POSE, LANDMARK > Base
typedef for base class
Definition StereoFactor.h:46
gtsam::GenericStereoFactor::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition StereoFactor.h:94