gtsam  4.1.0
gtsam
MultiProjectionFactor.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 
21 #pragma once
22 
25 #include <boost/optional.hpp>
26 
27 namespace gtsam {
28 
34  template<class POSE, class LANDMARK, class CALIBRATION = Cal3_S2>
36  protected:
37 
38  // Keep a copy of measurement and calibration for I/O
39  Vector measured_;
40  boost::shared_ptr<CALIBRATION> K_;
41  boost::optional<POSE> body_P_sensor_;
42 
43 
44  // verbosity handling for Cheirality Exceptions
47 
48  public:
49 
52 
55 
57  typedef boost::shared_ptr<This> shared_ptr;
58 
61 
72  MultiProjectionFactor(const Vector& measured, const SharedNoiseModel& model,
73  KeySet poseKeys, Key pointKey, const boost::shared_ptr<CALIBRATION>& K,
74  boost::optional<POSE> body_P_sensor = boost::none) :
75  Base(model), measured_(measured), K_(K), body_P_sensor_(body_P_sensor),
76  throwCheirality_(false), verboseCheirality_(false) {
77  keys_.assign(poseKeys.begin(), poseKeys.end());
78  keys_.push_back(pointKey);
79  }
80 
93  MultiProjectionFactor(const Vector& measured, const SharedNoiseModel& model,
94  KeySet poseKeys, Key pointKey, const boost::shared_ptr<CALIBRATION>& K,
96  boost::optional<POSE> body_P_sensor = boost::none) :
97  Base(model), measured_(measured), K_(K), body_P_sensor_(body_P_sensor),
99 
102 
104  NonlinearFactor::shared_ptr clone() const override {
105  return boost::static_pointer_cast<NonlinearFactor>(
106  NonlinearFactor::shared_ptr(new This(*this))); }
107 
113  void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
114  std::cout << s << "MultiProjectionFactor, z = ";
115  std::cout << measured_ << "measurements, z = ";
116  if(this->body_P_sensor_)
117  this->body_P_sensor_->print(" sensor pose in body frame: ");
118  Base::print("", keyFormatter);
119  }
120 
122  bool equals(const NonlinearFactor& p, double tol = 1e-9) const override {
123  const This *e = dynamic_cast<const This*>(&p);
124  return e
125  && Base::equals(p, tol)
126  //&& this->measured_.equals(e->measured_, tol)
127  && this->K_->equals(*e->K_, tol)
128  && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_)));
129  }
130 
132  Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const override {
133 
134  Vector a;
135  return a;
136 
137 // Point3 point = x.at<Point3>(*keys_.end());
138 //
139 // std::vector<KeyType>::iterator vit;
140 // for (vit = keys_.begin(); vit != keys_.end()-1; vit++) {
141 // Key key = (*vit);
142 // Pose3 pose = x.at<Pose3>(key);
143 //
144 // if(body_P_sensor_) {
145 // if(H1) {
146 // Matrix H0;
147 // PinholeCamera<CALIBRATION> camera(pose.compose(*body_P_sensor_, H0), *K_);
148 // Point2 reprojectionError(camera.project(point, H1, H2) - measured_);
149 // *H1 = *H1 * H0;
150 // return reprojectionError;
151 // } else {
152 // PinholeCamera<CALIBRATION> camera(pose.compose(*body_P_sensor_), *K_);
153 // Point2 reprojectionError(camera.project(point, H1, H2) - measured_);
154 // return reprojectionError;
155 // }
156 // } else {
157 // PinholeCamera<CALIBRATION> camera(pose, *K_);
158 // Point2 reprojectionError(camera.project(point, H1, H2) - measured_);
159 // return reprojectionError;
160 // }
161 // }
162 
163  }
164 
165 
166  Vector evaluateError(const Pose3& pose, const Point3& point,
167  boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) const {
168  try {
169  if(body_P_sensor_) {
170  if(H1) {
171  Matrix H0;
172  PinholeCamera<CALIBRATION> camera(pose.compose(*body_P_sensor_, H0), *K_);
173  Point2 reprojectionError(camera.project(point, H1, H2) - measured_);
174  *H1 = *H1 * H0;
175  return reprojectionError;
176  } else {
177  PinholeCamera<CALIBRATION> camera(pose.compose(*body_P_sensor_), *K_);
178  Point2 reprojectionError(camera.project(point, H1, H2) - measured_);
179  return reprojectionError;
180  }
181  } else {
182  PinholeCamera<CALIBRATION> camera(pose, *K_);
183  Point2 reprojectionError(camera.project(point, H1, H2) - measured_);
184  return reprojectionError;
185  }
186  } catch( CheiralityException& e) {
187  if (H1) *H1 = Matrix::Zero(2,6);
188  if (H2) *H2 = Matrix::Zero(2,3);
189  if (verboseCheirality_)
190  std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) <<
191  " moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl;
192  if (throwCheirality_)
193  throw e;
194  }
195  return Vector::Ones(2) * 2.0 * K_->fx();
196  }
197 
199  const Vector& measured() const {
200  return measured_;
201  }
202 
204  inline const boost::shared_ptr<CALIBRATION> calibration() const {
205  return K_;
206  }
207 
209  inline bool verboseCheirality() const { return verboseCheirality_; }
210 
212  inline bool throwCheirality() const { return throwCheirality_; }
213 
214  private:
215 
218  template<class ARCHIVE>
219  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
220  ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
221  ar & BOOST_SERIALIZATION_NVP(measured_);
222  ar & BOOST_SERIALIZATION_NVP(K_);
223  ar & BOOST_SERIALIZATION_NVP(body_P_sensor_);
224  ar & BOOST_SERIALIZATION_NVP(throwCheirality_);
225  ar & BOOST_SERIALIZATION_NVP(verboseCheirality_);
226  }
227  };
228 } // \ namespace gtsam
gtsam::MultiProjectionFactor::throwCheirality_
bool throwCheirality_
If true, rethrows Cheirality exceptions (default: false)
Definition: MultiProjectionFactor.h:45
gtsam::MultiProjectionFactor::clone
NonlinearFactor::shared_ptr clone() const override
Definition: MultiProjectionFactor.h:104
PinholeCamera.h
Base class for all pinhole cameras.
gtsam::MultiProjectionFactor::calibration
const boost::shared_ptr< CALIBRATION > calibration() const
return the calibration object
Definition: MultiProjectionFactor.h:204
gtsam::MultiProjectionFactor::verboseCheirality
bool verboseCheirality() const
return verbosity
Definition: MultiProjectionFactor.h:209
gtsam::MultiProjectionFactor::MultiProjectionFactor
MultiProjectionFactor()
Default constructor.
Definition: MultiProjectionFactor.h:60
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::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
gtsam::MultiProjectionFactor::verboseCheirality_
bool verboseCheirality_
If true, prints text for Cheirality exceptions (default: false)
Definition: MultiProjectionFactor.h:46
gtsam::MultiProjectionFactor::equals
bool equals(const NonlinearFactor &p, double tol=1e-9) const override
equals
Definition: MultiProjectionFactor.h:122
gtsam::Factor::keys_
KeyVector keys_
The keys involved in this factor.
Definition: Factor.h:72
gtsam::MultiProjectionFactor::MultiProjectionFactor
MultiProjectionFactor(const Vector &measured, const SharedNoiseModel &model, KeySet poseKeys, 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: MultiProjectionFactor.h:72
gtsam::NoiseModelFactor
A nonlinear sum-of-squares factor with a zero-mean noise model implementing the density Templated on...
Definition: NonlinearFactor.h:154
gtsam::MultiProjectionFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition: MultiProjectionFactor.h:113
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::MultiProjectionFactor::measured_
Vector measured_
2D measurement for each of the n views
Definition: MultiProjectionFactor.h:39
gtsam::MultiProjectionFactor::throwCheirality
bool throwCheirality() const
return flag for throwing cheirality exceptions
Definition: MultiProjectionFactor.h:212
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::MultiProjectionFactor::measured
const Vector & measured() const
return the measurements
Definition: MultiProjectionFactor.h:199
gtsam::MultiProjectionFactor::~MultiProjectionFactor
virtual ~MultiProjectionFactor()
Virtual destructor.
Definition: MultiProjectionFactor.h:101
gtsam::NonlinearFactor
Nonlinear factor base class.
Definition: NonlinearFactor.h:43
NonlinearFactor.h
Non-linear factor base classes.
gtsam::MultiProjectionFactor::MultiProjectionFactor
MultiProjectionFactor(const Vector &measured, const SharedNoiseModel &model, KeySet poseKeys, 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: MultiProjectionFactor.h:93
gtsam::MultiProjectionFactor::This
MultiProjectionFactor< POSE, LANDMARK, CALIBRATION > This
shorthand for this class
Definition: MultiProjectionFactor.h:54
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::MultiProjectionFactor::unwhitenedError
Vector unwhitenedError(const Values &x, boost::optional< std::vector< Matrix > & > H=boost::none) const override
Evaluate error h(x)-z and optionally derivatives.
Definition: MultiProjectionFactor.h:132
gtsam::FastSet< Key >
gtsam::MultiProjectionFactor::body_P_sensor_
boost::optional< POSE > body_P_sensor_
The pose of the sensor in the body frame.
Definition: MultiProjectionFactor.h:41
gtsam::PinholeCamera
Definition: PinholeCamera.h:33
gtsam::Pose3
Definition: Pose3.h:37
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::MultiProjectionFactor
Definition: MultiProjectionFactor.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:35
gtsam::MultiProjectionFactor::access
friend class boost::serialization::access
Serialization function.
Definition: MultiProjectionFactor.h:217
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition: Values.h:71
gtsam::MultiProjectionFactor::shared_ptr
boost::shared_ptr< This > shared_ptr
shorthand for a smart pointer to a factor
Definition: MultiProjectionFactor.h:57
gtsam::MultiProjectionFactor::Base
NoiseModelFactor Base
shorthand for base class type
Definition: MultiProjectionFactor.h:51
gtsam::MultiProjectionFactor::K_
boost::shared_ptr< CALIBRATION > K_
shared pointer to calibration object
Definition: MultiProjectionFactor.h:40