gtsam  4.1.0
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 
24 namespace gtsam {
25 
30 template<class POSE, class LANDMARK>
31 class GenericStereoFactor: public NoiseModelFactor2<POSE, LANDMARK> {
32 private:
33 
34  // Keep a copy of measurement and calibration for I/O
35  StereoPoint2 measured_;
37  boost::optional<POSE> body_P_sensor_;
38 
39  // verbosity handling for Cheirality Exceptions
40  bool throwCheirality_;
41  bool verboseCheirality_;
42 
43 public:
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 
91  virtual ~GenericStereoFactor() {}
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 
168 private:
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 
184 template<class T1, class T2>
185 struct traits<GenericStereoFactor<T1, T2> > : public Testable<GenericStereoFactor<T1, T2> > {};
186 
187 } // \ namespace gtsam
gtsam::GenericStereoFactor
Definition: StereoFactor.h:31
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::~GenericStereoFactor
virtual ~GenericStereoFactor()
Virtual destructor.
Definition: StereoFactor.h:91
gtsam::GenericStereoFactor::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition: StereoFactor.h:94
gtsam::StereoCheiralityException
Definition: StereoCamera.h:26
gtsam::NoiseModelFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Print.
Definition: NonlinearFactor.cpp:63
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Note, deliberately not in noiseModel namespace.
Definition: NoiseModel.h:734
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::StereoPoint2
Definition: StereoPoint2.h:32
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
gtsam::GenericStereoFactor::GenericStereoFactor
GenericStereoFactor()
Default constructor.
Definition: StereoFactor.h:54
gtsam::NoiseModelFactor2< POSE, LANDMARK >::key1
Key key1() const
methods to retrieve both keys
Definition: NonlinearFactor.h:387
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::throwCheirality
bool throwCheirality() const
return flag for throwing cheirality exceptions
Definition: StereoFactor.h:166
gtsam::GenericStereoFactor::This
GenericStereoFactor< POSE, LANDMARK > This
typedef for this class (with templates)
Definition: StereoFactor.h:47
gtsam::NoiseModelFactor::equals
bool equals(const NonlinearFactor &f, double tol=1e-9) const override
Check if two factors are equal.
Definition: NonlinearFactor.cpp:71
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::GenericStereoFactor::CamPose
POSE CamPose
typedef for Pose Lie Value type
Definition: StereoFactor.h:49
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition: Testable.h:150
gtsam::GenericStereoFactor::equals
bool equals(const NonlinearFactor &f, double tol=1e-9) const override
equals
Definition: StereoFactor.h:113
gtsam::NonlinearFactor
Nonlinear factor base class.
Definition: NonlinearFactor.h:43
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
NonlinearFactor.h
Non-linear factor base classes.
gtsam::GenericStereoFactor::calibration
const Cal3_S2Stereo::shared_ptr calibration() const
return the calibration object
Definition: StereoFactor.h:158
gtsam::GenericStereoFactor::Base
NoiseModelFactor2< POSE, LANDMARK > Base
typedef for base class
Definition: StereoFactor.h:46
gtsam::StereoPoint2::vector
Vector3 vector() const
convert to vector
Definition: StereoPoint2.h:115
gtsam::StereoPoint2::print
void print(const std::string &s="") const
print
Definition: StereoPoint2.cpp:26
gtsam::Pose3
Definition: Pose3.h:37
gtsam::StereoCamera
Definition: StereoCamera.h:47
gtsam::KeyFormatter
boost::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::Factor
This is the base class for all factor types.
Definition: Factor.h:55
gtsam::NoiseModelFactor2
A convenient base class for creating your own NoiseModelFactor with 2 variables.
Definition: NonlinearFactor.h:355
gtsam::StereoCamera::project
StereoPoint2 project(const Point3 &point) const
Project 3D point to StereoPoint2 (uL,uR,v)
Definition: StereoCamera.cpp:32
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::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:35
gtsam::GenericStereoFactor::measured
const StereoPoint2 & measured() const
return the measured
Definition: StereoFactor.h:153
gtsam::Cal3_S2Stereo::shared_ptr
boost::shared_ptr< Cal3_S2Stereo > shared_ptr
shared pointer to stereo calibration object
Definition: Cal3_S2Stereo.h:39
gtsam::Cal3_S2Stereo
Definition: Cal3_S2Stereo.h:30
gtsam::GenericStereoFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition: StereoFactor.h:103
gtsam::StereoPoint2::equals
bool equals(const StereoPoint2 &q, double tol=1e-9) const
equals
Definition: StereoPoint2.h:64
gtsam::GenericStereoFactor::access
friend class boost::serialization::access
Serialization function.
Definition: StereoFactor.h:170
StereoCamera.h
A Stereo Camera based on two Simple Cameras.