MultiProjectionFactor.h
Go to the documentation of this file.
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 "gtsam/geometry/Cal3_S2.h"
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
40  std::shared_ptr<CALIBRATION> K_;
41  std::optional<POSE> body_P_sensor_;
42 
43 
44  // verbosity handling for Cheirality Exceptions
47 
48  public:
49 
52 
55 
57  typedef std::shared_ptr<This> shared_ptr;
58 
61 
73  KeySet poseKeys, Key pointKey, const std::shared_ptr<CALIBRATION>& K,
74  std::optional<POSE> body_P_sensor = {}) :
76  throwCheirality_(false), verboseCheirality_(false) {
77  keys_.assign(poseKeys.begin(), poseKeys.end());
78  keys_.push_back(pointKey);
79  }
80 
94  KeySet poseKeys, Key pointKey, const std::shared_ptr<CALIBRATION>& K,
96  std::optional<POSE> body_P_sensor = {}) :
99 
102 
105  return std::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, OptionalMatrixVecType H = nullptr) 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 
167  OptionalJacobian<2, 6> H1 = {}, OptionalJacobian<2,3> H2 = {}) 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->keys_.at(1)) <<
191  " moved behind camera " << DefaultKeyFormatter(this->keys_.at(0)) << 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 std::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 
216 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
217  friend class boost::serialization::access;
219  template<class ARCHIVE>
220  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
221  ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
222  ar & BOOST_SERIALIZATION_NVP(measured_);
223  ar & BOOST_SERIALIZATION_NVP(K_);
224  ar & BOOST_SERIALIZATION_NVP(body_P_sensor_);
225  ar & BOOST_SERIALIZATION_NVP(throwCheirality_);
226  ar & BOOST_SERIALIZATION_NVP(verboseCheirality_);
227  }
228 #endif
229  };
230 } // \ namespace gtsam
H
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy y set format x g set format y g set format x2 g set format y2 g set format z g set angles radians set nogrid set key title set key left top Right noreverse box linetype linewidth samplen spacing width set nolabel set noarrow set nologscale set logscale x set set pointsize set encoding default set nopolar set noparametric set set set set surface set nocontour set clabel set mapping cartesian set nohidden3d set cntrparam order set cntrparam linear set cntrparam levels auto set cntrparam points set size set set xzeroaxis lt lw set x2zeroaxis lt lw set yzeroaxis lt lw set y2zeroaxis lt lw set tics in set ticslevel set tics set mxtics default set mytics default set mx2tics default set my2tics default set xtics border mirror norotate autofreq set ytics border mirror norotate autofreq set ztics border nomirror norotate autofreq set nox2tics set noy2tics set timestamp bottom norotate set rrange[*:*] noreverse nowriteback set trange[*:*] noreverse nowriteback set urange[*:*] noreverse nowriteback set vrange[*:*] noreverse nowriteback set xlabel matrix size set x2label set timefmt d m y n H
Definition: gnuplot_common_settings.hh:74
gtsam::NonlinearFactor::shared_ptr
std::shared_ptr< This > shared_ptr
Definition: NonlinearFactor.h:79
gtsam::MultiProjectionFactor::This
MultiProjectionFactor< POSE, LANDMARK, CALIBRATION > This
shorthand for this class
Definition: MultiProjectionFactor.h:54
gtsam::MultiProjectionFactor::Base
NoiseModelFactor Base
shorthand for base class type
Definition: MultiProjectionFactor.h:51
pose
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5))
pointKey
const gtsam::Key pointKey
Definition: testRelativeElevationFactor.cpp:25
s
RealScalar s
Definition: level1_cplx_impl.h:126
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
Cal3_S2.h
The most common 5DOF 3D->2D calibration.
gtsam::MultiProjectionFactor::verboseCheirality
bool verboseCheirality() const
Definition: MultiProjectionFactor.h:209
gtsam::MultiProjectionFactor::MultiProjectionFactor
MultiProjectionFactor()
Default constructor.
Definition: MultiProjectionFactor.h:60
x
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy x
Definition: gnuplot_common_settings.hh:12
gtsam::MultiProjectionFactor::shared_ptr
std::shared_ptr< This > shared_ptr
shorthand for a smart pointer to a factor
Definition: MultiProjectionFactor.h:57
gtsam::MultiProjectionFactor::evaluateError
Vector evaluateError(const Pose3 &pose, const Point3 &point, OptionalJacobian< 2, 6 > H1={}, OptionalJacobian< 2, 3 > H2={}) const
Definition: MultiProjectionFactor.h:166
gtsam::MultiProjectionFactor::throwCheirality_
bool throwCheirality_
If true, rethrows Cheirality exceptions (default: false)
Definition: MultiProjectionFactor.h:45
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
gtsam::MultiProjectionFactor::throwCheirality
bool throwCheirality() const
Definition: MultiProjectionFactor.h:212
gtsam::Factor
Definition: Factor.h:70
gtsam::FastSet< Key >
gtsam::MultiProjectionFactor::measured_
Vector measured_
2D measurement for each of the n views
Definition: MultiProjectionFactor.h:39
gtsam::MultiProjectionFactor::body_P_sensor_
std::optional< POSE > body_P_sensor_
The pose of the sensor in the body frame.
Definition: MultiProjectionFactor.h:41
body_P_sensor
static Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0))
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:39
gtsam::MultiProjectionFactor::unwhitenedError
Vector unwhitenedError(const Values &x, OptionalMatrixVecType H=nullptr) const override
Evaluate error h(x)-z and optionally derivatives.
Definition: MultiProjectionFactor.h:132
gtsam::DefaultKeyFormatter
KeyFormatter DefaultKeyFormatter
Assign default key formatter.
Definition: Key.cpp:30
gtsam::MultiProjectionFactor::calibration
const std::shared_ptr< CALIBRATION > calibration() const
Definition: MultiProjectionFactor.h:204
gtsam_unstable.tests.test_ProjectionFactorRollingShutter.point
point
Definition: test_ProjectionFactorRollingShutter.py:25
gtsam::MultiProjectionFactor::clone
NonlinearFactor::shared_ptr clone() const override
Definition: MultiProjectionFactor.h:104
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::Pose3
Definition: Pose3.h:37
gtsam::MultiProjectionFactor::MultiProjectionFactor
MultiProjectionFactor(const Vector &measured, const SharedNoiseModel &model, KeySet poseKeys, Key pointKey, const std::shared_ptr< CALIBRATION > &K, bool throwCheirality, bool verboseCheirality, std::optional< POSE > body_P_sensor={})
Definition: MultiProjectionFactor.h:93
gtsam::NoiseModelFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Definition: NonlinearFactor.cpp:74
gtsam::MultiProjectionFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Definition: MultiProjectionFactor.h:113
PinholeCamera.h
Base class for all pinhole cameras.
gtsam::MultiProjectionFactor
Definition: MultiProjectionFactor.h:35
gtsam::MultiProjectionFactor::verboseCheirality_
bool verboseCheirality_
If true, prints text for Cheirality exceptions (default: false)
Definition: MultiProjectionFactor.h:46
gtsam::NoiseModelFactor::equals
bool equals(const NonlinearFactor &f, double tol=1e-9) const override
Definition: NonlinearFactor.cpp:82
gtsam::Point2
Vector2 Point2
Definition: Point2.h:32
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:762
model
noiseModel::Diagonal::shared_ptr model
Definition: doc/Code/Pose2SLAMExample.cpp:7
NonlinearFactor.h
Non-linear factor base classes.
gtsam::OptionalMatrixVecType
std::vector< Matrix > * OptionalMatrixVecType
Definition: NonlinearFactor.h:62
a
ArrayXXi a
Definition: Array_initializer_list_23_cxx11.cpp:1
gtsam
traits
Definition: SFMdata.h:40
gtsam::Factor::keys_
KeyVector keys_
The keys involved in this factor.
Definition: Factor.h:88
K
#define K
Definition: igam.h:8
gtsam::OptionalJacobian
Definition: OptionalJacobian.h:38
gtsam::NoiseModelFactor
Definition: NonlinearFactor.h:198
gtsam::Values
Definition: Values.h:65
gtsam::NonlinearFactor
Definition: NonlinearFactor.h:69
p
float * p
Definition: Tutorial_Map_using.cpp:9
gtsam::tol
const G double tol
Definition: Group.h:79
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
gtsam::MultiProjectionFactor::K_
std::shared_ptr< CALIBRATION > K_
shared pointer to calibration object
Definition: MultiProjectionFactor.h:40
gtsam::MultiProjectionFactor::equals
bool equals(const NonlinearFactor &p, double tol=1e-9) const override
equals
Definition: MultiProjectionFactor.h:122
camera
static const CalibratedCamera camera(kDefaultPose)
Base
Definition: test_virtual_functions.cpp:156
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
gtsam::MultiProjectionFactor::MultiProjectionFactor
MultiProjectionFactor(const Vector &measured, const SharedNoiseModel &model, KeySet poseKeys, Key pointKey, const std::shared_ptr< CALIBRATION > &K, std::optional< POSE > body_P_sensor={})
Definition: MultiProjectionFactor.h:72
gtsam::MultiProjectionFactor::measured
const Vector & measured() const
Definition: MultiProjectionFactor.h:199
gtsam::MultiProjectionFactor::~MultiProjectionFactor
~MultiProjectionFactor() override
Definition: MultiProjectionFactor.h:101


gtsam
Author(s):
autogenerated on Sun Dec 22 2024 04:12:22