gtsam 4.1.1
gtsam
StereoFactor.h
1/* ----------------------------------------------------------------------------
2
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4 * Atlanta, Georgia 30332-0415
5 * All Rights Reserved
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7
8 * See LICENSE for the license information
9
10 * -------------------------------------------------------------------------- */
11
19#pragma once
20
23
24namespace gtsam {
25
30template<class POSE, class LANDMARK>
31class GenericStereoFactor: public NoiseModelFactor2<POSE, LANDMARK> {
32private:
33
34 // Keep a copy of measurement and calibration for I/O
35 StereoPoint2 measured_;
36 Cal3_S2Stereo::shared_ptr K_;
37 boost::optional<POSE> body_P_sensor_;
38
39 // verbosity handling for Cheirality Exceptions
40 bool throwCheirality_;
41 bool verboseCheirality_;
42
43public:
44
45 // shorthand for base class type
48 typedef boost::shared_ptr<GenericStereoFactor> shared_ptr;
49 typedef POSE CamPose;
50
54 GenericStereoFactor() : K_(new Cal3_S2Stereo(444, 555, 666, 777, 888, 1.0)),
55 throwCheirality_(false), verboseCheirality_(false) {}
56
67 Key poseKey, Key landmarkKey, const Cal3_S2Stereo::shared_ptr& K,
68 boost::optional<POSE> body_P_sensor = boost::none) :
69 Base(model, poseKey, landmarkKey), measured_(measured), K_(K), body_P_sensor_(body_P_sensor),
70 throwCheirality_(false), verboseCheirality_(false) {}
71
84 Key poseKey, Key landmarkKey, const Cal3_S2Stereo::shared_ptr& K,
86 boost::optional<POSE> body_P_sensor = boost::none) :
87 Base(model, poseKey, landmarkKey), measured_(measured), K_(K), body_P_sensor_(body_P_sensor),
88 throwCheirality_(throwCheirality), verboseCheirality_(verboseCheirality) {}
89
92
94 gtsam::NonlinearFactor::shared_ptr clone() const override {
95 return boost::static_pointer_cast<gtsam::NonlinearFactor>(
96 gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
97
103 void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
104 Base::print(s, keyFormatter);
105 measured_.print(s + ".z");
106 if(this->body_P_sensor_)
107 this->body_P_sensor_->print(" sensor pose in body frame: ");
108 }
109
113 bool equals(const NonlinearFactor& f, double tol = 1e-9) const override {
114 const GenericStereoFactor* e = dynamic_cast<const GenericStereoFactor*> (&f);
115 return e
116 && Base::equals(f)
117 && measured_.equals(e->measured_, tol)
118 && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_)));
119 }
120
122 Vector evaluateError(const Pose3& pose, const Point3& point,
123 boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) const override {
124 try {
125 if(body_P_sensor_) {
126 if(H1) {
127 gtsam::Matrix H0;
128 StereoCamera stereoCam(pose.compose(*body_P_sensor_, H0), K_);
129 StereoPoint2 reprojectionError(stereoCam.project(point, H1, H2) - measured_);
130 *H1 = *H1 * H0;
131 return reprojectionError.vector();
132 } else {
133 StereoCamera stereoCam(pose.compose(*body_P_sensor_), K_);
134 return (stereoCam.project(point, H1, H2) - measured_).vector();
135 }
136 } else {
137 StereoCamera stereoCam(pose, K_);
138 return (stereoCam.project(point, H1, H2) - measured_).vector();
139 }
140 } catch(StereoCheiralityException& e) {
141 if (H1) *H1 = Matrix::Zero(3,6);
142 if (H2) *H2 = Z_3x3;
143 if (verboseCheirality_)
144 std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) <<
145 " moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl;
146 if (throwCheirality_)
147 throw StereoCheiralityException(this->key2());
148 }
149 return Vector3::Constant(2.0 * K_->fx());
150 }
151
153 const StereoPoint2& measured() const {
154 return measured_;
155 }
156
158 inline const Cal3_S2Stereo::shared_ptr calibration() const {
159 return K_;
160 }
161
163 inline bool verboseCheirality() const { return verboseCheirality_; }
164
166 inline bool throwCheirality() const { return throwCheirality_; }
167
168private:
171 template<class Archive>
172 void serialize(Archive & ar, const unsigned int /*version*/) {
173 ar & boost::serialization::make_nvp("NoiseModelFactor2",
174 boost::serialization::base_object<Base>(*this));
175 ar & BOOST_SERIALIZATION_NVP(measured_);
176 ar & BOOST_SERIALIZATION_NVP(K_);
177 ar & BOOST_SERIALIZATION_NVP(body_P_sensor_);
178 ar & BOOST_SERIALIZATION_NVP(throwCheirality_);
179 ar & BOOST_SERIALIZATION_NVP(verboseCheirality_);
180 }
181};
182
184template<class T1, class T2>
185struct traits<GenericStereoFactor<T1, T2> > : public Testable<GenericStereoFactor<T1, T2> > {};
186
187} // \ namespace gtsam
A Stereo Camera based on two Simple Cameras.
Non-linear factor base classes.
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
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:35
noiseModel::Base::shared_ptr SharedNoiseModel
Note, deliberately not in noiseModel namespace.
Definition: NoiseModel.h:736
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:69
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
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition: concepts.h:30
A helper that implements the traits interface for GTSAM types.
Definition: Testable.h:151
Definition: Cal3_S2Stereo.h:30
Definition: Pose3.h:37
Definition: StereoCamera.h:26
Definition: StereoCamera.h:47
StereoPoint2 project(const Point3 &point) const
Project 3D point to StereoPoint2 (uL,uR,v)
Definition: StereoCamera.cpp:32
Definition: StereoPoint2.h:32
Vector3 vector() const
convert to vector
Definition: StereoPoint2.h:115
bool equals(const StereoPoint2 &q, double tol=1e-9) const
equals
Definition: StereoPoint2.h:64
void print(const std::string &s="") const
print
Definition: StereoPoint2.cpp:26
This is the base class for all factor types.
Definition: Factor.h:56
Nonlinear factor base class.
Definition: NonlinearFactor.h:43
bool equals(const NonlinearFactor &f, double tol=1e-9) const override
Check if two factors are equal.
Definition: NonlinearFactor.cpp:71
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Print.
Definition: NonlinearFactor.cpp:63
A convenient base class for creating your own NoiseModelFactor with 2 variables.
Definition: NonlinearFactor.h:369
Key key1() const
methods to retrieve both keys
Definition: NonlinearFactor.h:401
Definition: StereoFactor.h:31
bool throwCheirality() const
return flag for throwing cheirality exceptions
Definition: StereoFactor.h:166
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
bool equals(const NonlinearFactor &f, double tol=1e-9) const override
equals
Definition: StereoFactor.h:113
const Cal3_S2Stereo::shared_ptr calibration() const
return the calibration object
Definition: StereoFactor.h:158
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
GenericStereoFactor< POSE, LANDMARK > This
typedef for this class (with templates)
Definition: StereoFactor.h:47
~GenericStereoFactor() override
Virtual destructor.
Definition: StereoFactor.h:91
bool verboseCheirality() const
return verbosity
Definition: StereoFactor.h:163
boost::shared_ptr< GenericStereoFactor > shared_ptr
typedef for shared pointer to this object
Definition: StereoFactor.h:48
const StereoPoint2 & measured() const
return the measured
Definition: StereoFactor.h:153
GenericStereoFactor()
Default constructor.
Definition: StereoFactor.h:54
friend class boost::serialization::access
Serialization function.
Definition: StereoFactor.h:170
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
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition: StereoFactor.h:103
POSE CamPose
typedef for Pose Lie Value type
Definition: StereoFactor.h:49
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition: StereoFactor.h:94
NoiseModelFactor2< POSE, LANDMARK > Base
typedef for base class
Definition: StereoFactor.h:46