SmartProjectionRigFactor.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 
25 #pragma once
26 
28 
29 namespace gtsam {
51 template <class CAMERA>
53  private:
56  typedef typename CAMERA::CalibrationType CALIBRATION;
57  typedef typename CAMERA::Measurement MEASUREMENT;
58  typedef typename CAMERA::MeasurementVector MEASUREMENTS;
59 
60  static const int DimPose = 6;
61  static const int ZDim = 2;
62 
63  protected:
66 
68  std::shared_ptr<typename Base::Cameras> cameraRig_;
69 
73 
74  public:
76 
77  typedef CAMERA Camera;
79 
81  typedef std::shared_ptr<This> shared_ptr;
82 
85 
95  const SharedNoiseModel& sharedNoiseModel,
96  const std::shared_ptr<Cameras>& cameraRig,
98  : Base(sharedNoiseModel, params), cameraRig_(cameraRig) {
99  // throw exception if configuration is not supported by this factor
100  if (Base::params_.degeneracyMode != gtsam::ZERO_ON_DEGENERACY)
101  throw std::runtime_error(
102  "SmartProjectionRigFactor: "
103  "degeneracyMode must be set to ZERO_ON_DEGENERACY");
104  if (Base::params_.linearizationMode != gtsam::HESSIAN)
105  throw std::runtime_error(
106  "SmartProjectionRigFactor: "
107  "linearizationMode must be set to HESSIAN");
108  }
109 
120  void add(const MEASUREMENT& measured, const Key& poseKey,
121  const size_t& cameraId = 0) {
122  // store measurement and key
123  this->measured_.push_back(measured);
124  this->nonUniqueKeys_.push_back(poseKey);
125 
126  // also store keys in the keys_ vector: these keys are assumed to be
127  // unique, so we avoid duplicates here
128  if (std::find(this->keys_.begin(), this->keys_.end(), poseKey) ==
129  this->keys_.end())
130  this->keys_.push_back(poseKey); // add only unique keys
131 
132  // store id of the camera taking the measurement
133  cameraIds_.push_back(cameraId);
134  }
135 
146  void add(const MEASUREMENTS& measurements, const KeyVector& poseKeys,
148  if (poseKeys.size() != measurements.size() ||
149  (poseKeys.size() != cameraIds.size() && cameraIds.size() != 0)) {
150  throw std::runtime_error(
151  "SmartProjectionRigFactor: "
152  "trying to add inconsistent inputs");
153  }
154  if (cameraIds.size() == 0 && cameraRig_->size() > 1) {
155  throw std::runtime_error(
156  "SmartProjectionRigFactor: "
157  "camera rig includes multiple camera "
158  "but add did not input cameraIds");
159  }
160  for (size_t i = 0; i < measurements.size(); i++) {
161  add(measurements[i], poseKeys[i],
162  cameraIds.size() == 0 ? 0 : cameraIds[i]);
163  }
164  }
165 
168  const KeyVector& nonUniqueKeys() const { return nonUniqueKeys_; }
169 
171  const std::shared_ptr<Cameras>& cameraRig() const { return cameraRig_; }
172 
174  const FastVector<size_t>& cameraIds() const { return cameraIds_; }
175 
181  void print(
182  const std::string& s = "",
183  const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
184  std::cout << s << "SmartProjectionRigFactor: \n ";
185  for (size_t i = 0; i < nonUniqueKeys_.size(); i++) {
186  std::cout << "-- Measurement nr " << i << std::endl;
187  std::cout << "key: " << keyFormatter(nonUniqueKeys_[i]) << std::endl;
188  std::cout << "cameraId: " << cameraIds_[i] << std::endl;
189  (*cameraRig_)[cameraIds_[i]].print("camera in rig:\n");
190  }
191  Base::print("", keyFormatter);
192  }
193 
195  bool equals(const NonlinearFactor& p, double tol = 1e-9) const override {
196  const This* e = dynamic_cast<const This*>(&p);
197  return e && Base::equals(p, tol) && nonUniqueKeys_ == e->nonUniqueKeys() &&
198  cameraRig_->equals(*(e->cameraRig())) &&
199  std::equal(cameraIds_.begin(), cameraIds_.end(),
200  e->cameraIds().begin());
201  }
202 
209  typename Base::Cameras cameras(const Values& values) const override {
210  typename Base::Cameras cameras;
211  cameras.reserve(nonUniqueKeys_.size()); // preallocate
212  for (size_t i = 0; i < nonUniqueKeys_.size(); i++) {
213  const typename Base::Camera& camera_i = (*cameraRig_)[cameraIds_[i]];
214  const Pose3 world_P_sensor_i =
215  values.at<Pose3>(nonUniqueKeys_[i]) // = world_P_body
216  * camera_i.pose(); // = body_P_cam_i
217  cameras.emplace_back(world_P_sensor_i,
218  make_shared<typename CAMERA::CalibrationType>(
219  camera_i.calibration()));
220  }
221  return cameras;
222  }
223 
227  double error(const Values& values) const override {
228  if (this->active(values)) {
229  return this->totalReprojectionError(this->cameras(values));
230  } else { // else of active flag
231  return 0.0;
232  }
233  }
234 
245  Matrix& E, Vector& b,
246  const Cameras& cameras) const {
247  if (!this->result_) {
248  throw("computeJacobiansWithTriangulatedPoint");
249  } else { // valid result: compute jacobians
250  b = -cameras.reprojectionError(*this->result_, this->measured_, Fs, E);
251  for (size_t i = 0; i < Fs.size(); i++) {
252  const Pose3& body_P_sensor = (*cameraRig_)[cameraIds_[i]].pose();
253  const Pose3 world_P_body = cameras[i].pose() * body_P_sensor.inverse();
255  world_P_body.compose(body_P_sensor, H);
256  Fs.at(i) = Fs.at(i) * H;
257  }
258  }
259  }
260 
262  std::shared_ptr<RegularHessianFactor<DimPose> > createHessianFactor(
263  const Values& values, const double& lambda = 0.0,
264  bool diagonalDamping = false) const {
265  // we may have multiple observation sharing the same keys (e.g., 2 cameras
266  // measuring from the same body pose), hence the number of unique keys may
267  // be smaller than nrMeasurements
268  size_t nrUniqueKeys =
269  this->keys_
270  .size(); // note: by construction, keys_ only contains unique keys
271 
272  Cameras cameras = this->cameras(values);
273 
274  // Create structures for Hessian Factors
275  std::vector<size_t> js;
276  std::vector<Matrix> Gs(nrUniqueKeys * (nrUniqueKeys + 1) / 2);
277  std::vector<Vector> gs(nrUniqueKeys);
278 
279  if (this->measured_.size() != cameras.size()) // 1 observation per camera
280  throw std::runtime_error(
281  "SmartProjectionRigFactor: "
282  "measured_.size() inconsistent with input");
283 
284  // triangulate 3D point at given linearization point
285  this->triangulateSafe(cameras);
286 
287  if (!this->result_) { // failed: return "empty/zero" Hessian
289  for (Matrix& m : Gs) m = Matrix::Zero(DimPose, DimPose);
290  for (Vector& v : gs) v = Vector::Zero(DimPose);
291  return std::make_shared<RegularHessianFactor<DimPose> >(this->keys_,
292  Gs, gs, 0.0);
293  } else {
294  throw std::runtime_error(
295  "SmartProjectionRigFactor: "
296  "only supported degeneracy mode is ZERO_ON_DEGENERACY");
297  }
298  }
299 
300  // compute Jacobian given triangulated 3D Point
301  typename Base::FBlocks Fs;
302  Matrix E;
303  Vector b;
305 
306  // Whiten using noise model
307  this->noiseModel_->WhitenSystem(E, b);
308  for (size_t i = 0; i < Fs.size(); i++) {
309  Fs[i] = this->noiseModel_->Whiten(Fs[i]);
310  }
311 
312  const Matrix3 P = Base::Cameras::PointCov(E, lambda, diagonalDamping);
313 
314  // Build augmented Hessian (with last row/column being the information
315  // vector) Note: we need to get the augumented hessian wrt the unique keys
316  // in key_
317  SymmetricBlockMatrix augmentedHessianUniqueKeys =
318  Base::Cameras::template SchurComplementAndRearrangeBlocks<3, 6, 6>(
319  Fs, E, P, b, nonUniqueKeys_, this->keys_);
320 
321  return std::make_shared<RegularHessianFactor<DimPose> >(
322  this->keys_, augmentedHessianUniqueKeys);
323  }
324 
332  std::shared_ptr<GaussianFactor> linearizeDamped(
333  const Values& values, const double& lambda = 0.0) const {
334  // depending on flag set on construction we may linearize to different
335  // linear factors
336  switch (this->params_.linearizationMode) {
337  case HESSIAN:
338  return this->createHessianFactor(values, lambda);
339  default:
340  throw std::runtime_error(
341  "SmartProjectionRigFactor: unknown linearization mode");
342  }
343  }
344 
346  std::shared_ptr<GaussianFactor> linearize(
347  const Values& values) const override {
348  return this->linearizeDamped(values);
349  }
350 
351  private:
352 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
353  friend class boost::serialization::access;
355  template <class ARCHIVE>
356  void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
357  ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
358  // ar& BOOST_SERIALIZATION_NVP(nonUniqueKeys_);
359  // ar& BOOST_SERIALIZATION_NVP(cameraRig_);
360  // ar& BOOST_SERIALIZATION_NVP(cameraIds_);
361  }
362 #endif
363 };
364 // end of class declaration
365 
367 template <class CAMERA>
369  : public Testable<SmartProjectionRigFactor<CAMERA> > {};
370 
371 } // namespace gtsam
gtsam::SmartProjectionRigFactor::equals
bool equals(const NonlinearFactor &p, double tol=1e-9) const override
equals
Definition: SmartProjectionRigFactor.h:195
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::SmartProjectionRigFactor::linearizeDamped
std::shared_ptr< GaussianFactor > linearizeDamped(const Values &values, const double &lambda=0.0) const
Definition: SmartProjectionRigFactor.h:332
gtsam::SmartFactorBase::body_P_sensor
Pose3 body_P_sensor() const
Definition: SmartFactorBase.h:440
gtsam::SmartProjectionRigFactor::Camera
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef CAMERA Camera
Definition: SmartProjectionRigFactor.h:77
s
RealScalar s
Definition: level1_cplx_impl.h:126
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
gtsam::HESSIAN
@ HESSIAN
Definition: SmartFactorParams.h:31
gtsam::SmartProjectionParams
Definition: SmartFactorParams.h:42
gtsam::FastVector
std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > FastVector
Definition: FastVector.h:34
SmartProjectionFactor.h
Smart factor on cameras (pose + calibration)
pybind_wrapper_test_script.this
this
Definition: pybind_wrapper_test_script.py:38
gtsam::LieGroup::compose
Class compose(const Class &g) const
Definition: Lie.h:48
gtsam::SmartProjectionFactor::equals
bool equals(const NonlinearFactor &p, double tol=1e-9) const override
equals
Definition: SmartProjectionFactor.h:114
gtsam::SmartFactorBase::measured
const ZVector & measured() const
Return the 2D measurements (ZDim, in general).
Definition: SmartFactorBase.h:160
gtsam::SmartFactorBase::Fs
FBlocks Fs
Definition: SmartFactorBase.h:86
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
gtsam::SmartProjectionFactor::params_
SmartProjectionParams params_
Definition: SmartProjectionFactor.h:57
different_sigmas::values
HybridValues values
Definition: testHybridBayesNet.cpp:245
gtsam::SmartProjectionRigFactor::nonUniqueKeys
const KeyVector & nonUniqueKeys() const
Definition: SmartProjectionRigFactor.h:168
gtsam::Factor
Definition: Factor.h:70
vanilla::params
static const SmartProjectionParams params
Definition: smartFactorScenarios.h:69
gtsam::SmartProjectionFactor::totalReprojectionError
double totalReprojectionError(const Cameras &cameras, std::optional< Point3 > externalPoint={}) const
Definition: SmartProjectionFactor.h:411
gtsam::SmartProjectionRigFactor::SmartProjectionRigFactor
SmartProjectionRigFactor()
Default constructor, only for serialization.
Definition: SmartProjectionRigFactor.h:84
gtsam::NonlinearFactor::active
virtual bool active(const Values &) const
Definition: NonlinearFactor.h:141
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:92
gtsam::SmartProjectionFactor::triangulateSafe
TriangulationResult triangulateSafe(const Cameras &cameras) const
Call gtsam::triangulateSafe iff we need to re-triangulate.
Definition: SmartProjectionFactor.h:173
gtsam::SmartProjectionRigFactor::add
void add(const MEASUREMENTS &measurements, const KeyVector &poseKeys, const FastVector< size_t > &cameraIds=FastVector< size_t >())
Definition: SmartProjectionRigFactor.h:146
gtsam::CameraSet
A set of cameras, all with their own calibration.
Definition: CameraSet.h:36
gtsam::SmartProjectionRigFactor::Cameras
CameraSet< CAMERA > Cameras
Definition: SmartProjectionRigFactor.h:78
poseKey
const gtsam::Key poseKey
Definition: testPoseRotationPrior.cpp:29
gtsam::SmartProjectionRigFactor::cameraRig
const std::shared_ptr< Cameras > & cameraRig() const
return the calibration object
Definition: SmartProjectionRigFactor.h:171
gtsam::DefaultKeyFormatter
KeyFormatter DefaultKeyFormatter
Assign default key formatter.
Definition: Key.cpp:30
gtsam::SmartProjectionFactor::Cameras
CameraSet< CAMERA > Cameras
Definition: SmartProjectionFactor.h:74
gtsam::SmartProjectionParams::degeneracyMode
DegeneracyMode degeneracyMode
How to linearize the factor.
Definition: SmartFactorParams.h:45
gtsam::ZERO_ON_DEGENERACY
@ ZERO_ON_DEGENERACY
Definition: SmartFactorParams.h:36
simulated2D::Measurement
GenericMeasurement< Point2, Point2 > Measurement
Definition: simulated2D.h:278
gtsam::SmartProjectionRigFactor::shared_ptr
std::shared_ptr< This > shared_ptr
shorthand for a smart pointer to a factor
Definition: SmartProjectionRigFactor.h:81
gtsam::SmartProjectionParams::linearizationMode
LinearizationMode linearizationMode
How to linearize the factor.
Definition: SmartFactorParams.h:44
gtsam::SmartProjectionFactor::result_
TriangulationResult result_
result from triangulateSafe
Definition: SmartProjectionFactor.h:62
gtsam::SmartProjectionRigFactor::This
SmartProjectionRigFactor< CAMERA > This
Definition: SmartProjectionRigFactor.h:55
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::SmartFactorBase::noiseModel_
SharedIsotropic noiseModel_
Definition: SmartFactorBase.h:73
gtsam::Pose3::inverse
Pose3 inverse() const
inverse transformation with derivatives
Definition: Pose3.cpp:49
gtsam::SmartProjectionRigFactor::MEASUREMENTS
CAMERA::MeasurementVector MEASUREMENTS
Definition: SmartProjectionRigFactor.h:58
gtsam::SmartProjectionRigFactor::CALIBRATION
CAMERA::CalibrationType CALIBRATION
Definition: SmartProjectionRigFactor.h:56
gtsam::SmartProjectionRigFactor::cameraIds_
FastVector< size_t > cameraIds_
Definition: SmartProjectionRigFactor.h:72
m
Matrix3f m
Definition: AngleAxis_mimic_euler.cpp:1
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:762
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
#define EIGEN_MAKE_ALIGNED_OPERATOR_NEW
Definition: Memory.h:841
gtsam::SmartProjectionRigFactor::cameraIds
const FastVector< size_t > & cameraIds() const
return the calibration object
Definition: SmartProjectionRigFactor.h:174
gtsam::SmartProjectionRigFactor::Base
SmartProjectionFactor< CAMERA > Base
Definition: SmartProjectionRigFactor.h:54
lambda
static double lambda[]
Definition: jv.c:524
gtsam::SmartProjectionFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Definition: SmartProjectionFactor.h:102
E
DiscreteKey E(5, 2)
gtsam::SmartFactorBase::FBlocks
std::vector< MatrixZD, Eigen::aligned_allocator< MatrixZD > > FBlocks
Definition: SmartFactorBase.h:64
gtsam::SmartProjectionRigFactor::SmartProjectionRigFactor
SmartProjectionRigFactor(const SharedNoiseModel &sharedNoiseModel, const std::shared_ptr< Cameras > &cameraRig, const SmartProjectionParams &params=SmartProjectionParams())
Definition: SmartProjectionRigFactor.h:94
gtsam::b
const G & b
Definition: Group.h:79
gtsam::SmartProjectionFactor
Definition: SmartProjectionFactor.h:44
gtsam::SmartProjectionRigFactor::computeJacobiansWithTriangulatedPoint
void computeJacobiansWithTriangulatedPoint(typename Base::FBlocks &Fs, Matrix &E, Vector &b, const Cameras &cameras) const
Definition: SmartProjectionRigFactor.h:244
gtsam::SmartProjectionRigFactor::linearize
std::shared_ptr< GaussianFactor > linearize(const Values &values) const override
linearize
Definition: SmartProjectionRigFactor.h:346
gtsam::SmartProjectionRigFactor::MEASUREMENT
CAMERA::Measurement MEASUREMENT
Definition: SmartProjectionRigFactor.h:57
gtsam
traits
Definition: SFMdata.h:40
gtsam::Testable
Definition: Testable.h:152
gtsam::SmartProjectionRigFactor::cameraRig_
std::shared_ptr< typename Base::Cameras > cameraRig_
cameras in the rig (fixed poses wrt body and intrinsics, for each camera)
Definition: SmartProjectionRigFactor.h:68
gtsam::Factor::keys_
KeyVector keys_
The keys involved in this factor.
Definition: Factor.h:88
gtsam::traits
Definition: Group.h:36
estimation_fixture::measurements
std::vector< double > measurements
Definition: testHybridEstimation.cpp:51
gtsam::SmartProjectionRigFactor::add
void add(const MEASUREMENT &measured, const Key &poseKey, const size_t &cameraId=0)
Definition: SmartProjectionRigFactor.h:120
gtsam::Values
Definition: Values.h:65
gtsam::NonlinearFactor
Definition: NonlinearFactor.h:68
gtsam::SmartProjectionRigFactor::nonUniqueKeys_
KeyVector nonUniqueKeys_
vector of keys (one for each observation) with potentially repeated keys
Definition: SmartProjectionRigFactor.h:65
p
float * p
Definition: Tutorial_Map_using.cpp:9
v
Array< int, Dynamic, 1 > v
Definition: Array_initializer_list_vector_cxx11.cpp:1
gtsam::SmartProjectionRigFactor::createHessianFactor
std::shared_ptr< RegularHessianFactor< DimPose > > createHessianFactor(const Values &values, const double &lambda=0.0, bool diagonalDamping=false) const
linearize and return a Hessianfactor that is an approximation of error(p)
Definition: SmartProjectionRigFactor.h:262
gtsam::SmartProjectionFactor::Camera
CAMERA Camera
shorthand for a set of cameras
Definition: SmartProjectionFactor.h:73
P
static double P[]
Definition: ellpe.c:68
gtsam::tol
const G double tol
Definition: Group.h:79
gtsam::SmartProjectionRigFactor::cameras
Base::Cameras cameras(const Values &values) const override
Definition: SmartProjectionRigFactor.h:209
Eigen::Matrix
The matrix class, also used for vectors and row-vectors.
Definition: 3rdparty/Eigen/Eigen/src/Core/Matrix.h:178
gtsam::SmartProjectionRigFactor::ZDim
static const int ZDim
Measurement dimension.
Definition: SmartProjectionRigFactor.h:61
Base
Definition: test_virtual_functions.cpp:156
gtsam::SmartProjectionRigFactor::error
double error(const Values &values) const override
Definition: SmartProjectionRigFactor.h:227
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
gtsam::SymmetricBlockMatrix
Definition: SymmetricBlockMatrix.h:53
gtsam::SmartProjectionRigFactor::DimPose
static const int DimPose
Pose3 dimension.
Definition: SmartProjectionRigFactor.h:60
gtsam::SmartProjectionRigFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Definition: SmartProjectionRigFactor.h:181
gtsam::equal
bool equal(const T &obj1, const T &obj2, double tol)
Definition: Testable.h:85
i
int i
Definition: BiCGSTAB_step_by_step.cpp:9
gtsam::SmartProjectionRigFactor
Definition: SmartProjectionRigFactor.h:52
gtsam::SmartFactorBase::measured_
ZVector measured_
Definition: SmartFactorBase.h:80


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:35:31