SmartProjectionPoseFactorRollingShutter.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 
19 #pragma once
20 
23 #include <gtsam_unstable/dllexport.h>
24 
25 namespace gtsam {
44 template <class CAMERA>
46  : public SmartProjectionFactor<CAMERA> {
47  private:
50  typedef typename CAMERA::CalibrationType CALIBRATION;
51  typedef typename CAMERA::Measurement MEASUREMENT;
52  typedef typename CAMERA::MeasurementVector MEASUREMENTS;
53 
54  protected:
57  std::vector<std::pair<Key, Key>> world_P_body_key_pairs_;
58 
61  std::vector<double> alphas_;
62 
65  std::shared_ptr<typename Base::Cameras> cameraRig_;
66 
70 
71  public:
73 
74  static const int DimBlock =
75  12;
76  static const int DimPose = 6;
78  static const int ZDim = 2;
80  MatrixZD; // F blocks (derivatives wrt block of 2 poses)
81  typedef std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD>>
82  FBlocks; // vector of F blocks
83 
84  typedef CAMERA Camera;
86 
88  typedef std::shared_ptr<This> shared_ptr;
89 
92 
101  const SharedNoiseModel& sharedNoiseModel,
102  const std::shared_ptr<Cameras>& cameraRig,
104  : Base(sharedNoiseModel, params), cameraRig_(cameraRig) {
105  // throw exception if configuration is not supported by this factor
106  if (Base::params_.degeneracyMode != gtsam::ZERO_ON_DEGENERACY)
107  throw std::runtime_error(
108  "SmartProjectionRigFactor: "
109  "degeneracyMode must be set to ZERO_ON_DEGENERACY");
110  if (Base::params_.linearizationMode != gtsam::HESSIAN)
111  throw std::runtime_error(
112  "SmartProjectionRigFactor: "
113  "linearizationMode must be set to HESSIAN");
114  }
115 
128  void add(const MEASUREMENT& measured, const Key& world_P_body_key1,
129  const Key& world_P_body_key2, const double& alpha,
130  const size_t& cameraId = 0) {
131  // store measurements in base class
132  this->measured_.push_back(measured);
133 
134  // store the pair of keys for each measurement, in the same order
135  world_P_body_key_pairs_.push_back(
136  std::make_pair(world_P_body_key1, world_P_body_key2));
137 
138  // also store keys in the keys_ vector: these keys are assumed to be
139  // unique, so we avoid duplicates here
140  if (std::find(this->keys_.begin(), this->keys_.end(), world_P_body_key1) ==
141  this->keys_.end())
142  this->keys_.push_back(world_P_body_key1); // add only unique keys
143  if (std::find(this->keys_.begin(), this->keys_.end(), world_P_body_key2) ==
144  this->keys_.end())
145  this->keys_.push_back(world_P_body_key2); // add only unique keys
146 
147  // store interpolation factor
148  alphas_.push_back(alpha);
149 
150  // store id of the camera taking the measurement
151  cameraIds_.push_back(cameraId);
152  }
153 
168  const std::vector<std::pair<Key, Key>>& world_P_body_key_pairs,
169  const std::vector<double>& alphas,
171  if (world_P_body_key_pairs.size() != measurements.size() ||
172  world_P_body_key_pairs.size() != alphas.size() ||
173  (world_P_body_key_pairs.size() != cameraIds.size() &&
174  cameraIds.size() != 0)) { // cameraIds.size()=0 is default
175  throw std::runtime_error(
176  "SmartProjectionPoseFactorRollingShutter: "
177  "trying to add inconsistent inputs");
178  }
179  if (cameraIds.size() == 0 && cameraRig_->size() > 1) {
180  throw std::runtime_error(
181  "SmartProjectionPoseFactorRollingShutter: "
182  "camera rig includes multiple camera "
183  "but add did not input cameraIds");
184  }
185  for (size_t i = 0; i < measurements.size(); i++) {
187  world_P_body_key_pairs[i].second, alphas[i],
188  cameraIds.size() == 0 ? 0
189  : cameraIds[i]); // use 0 as default if
190  // cameraIds was not specified
191  }
192  }
193 
196  const std::vector<std::pair<Key, Key>>& world_P_body_key_pairs() const {
198  }
199 
201  const std::vector<double>& alphas() const { return alphas_; }
202 
204  const std::shared_ptr<Cameras>& cameraRig() const { return cameraRig_; }
205 
207  const FastVector<size_t>& cameraIds() const { return cameraIds_; }
208 
214  void print(
215  const std::string& s = "",
216  const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
217  std::cout << s << "SmartProjectionPoseFactorRollingShutter: \n ";
218  for (size_t i = 0; i < cameraIds_.size(); i++) {
219  std::cout << "-- Measurement nr " << i << std::endl;
220  std::cout << " pose1 key: "
221  << keyFormatter(world_P_body_key_pairs_[i].first) << std::endl;
222  std::cout << " pose2 key: "
223  << keyFormatter(world_P_body_key_pairs_[i].second) << std::endl;
224  std::cout << " alpha: " << alphas_[i] << std::endl;
225  std::cout << "cameraId: " << cameraIds_[i] << std::endl;
226  (*cameraRig_)[cameraIds_[i]].print("camera in rig:\n");
227  }
228  Base::print("", keyFormatter);
229  }
230 
232  bool equals(const NonlinearFactor& p, double tol = 1e-9) const override {
235  &p);
236 
237  double keyPairsEqual = true;
238  if (this->world_P_body_key_pairs_.size() ==
239  e->world_P_body_key_pairs().size()) {
240  for (size_t k = 0; k < this->world_P_body_key_pairs_.size(); k++) {
241  const Key key1own = world_P_body_key_pairs_[k].first;
242  const Key key1e = e->world_P_body_key_pairs()[k].first;
243  const Key key2own = world_P_body_key_pairs_[k].second;
244  const Key key2e = e->world_P_body_key_pairs()[k].second;
245  if (!(key1own == key1e) || !(key2own == key2e)) {
246  keyPairsEqual = false;
247  break;
248  }
249  }
250  } else {
251  keyPairsEqual = false;
252  }
253 
254  return e && Base::equals(p, tol) && alphas_ == e->alphas() &&
255  keyPairsEqual && cameraRig_->equals(*(e->cameraRig())) &&
256  std::equal(cameraIds_.begin(), cameraIds_.end(),
257  e->cameraIds().begin());
258  }
259 
266  typename Base::Cameras cameras(const Values& values) const override {
267  typename Base::Cameras cameras;
268  for (size_t i = 0; i < this->measured_.size();
269  i++) { // for each measurement
270  const Pose3& w_P_body1 =
272  const Pose3& w_P_body2 =
273  values.at<Pose3>(world_P_body_key_pairs_[i].second);
274  double interpolationFactor = alphas_[i];
275  const Pose3& w_P_body =
276  interpolate<Pose3>(w_P_body1, w_P_body2, interpolationFactor);
277  const typename Base::Camera& camera_i = (*cameraRig_)[cameraIds_[i]];
278  const Pose3& body_P_cam = camera_i.pose();
279  const Pose3& w_P_cam = w_P_body.compose(body_P_cam);
280  cameras.emplace_back(w_P_cam,
281  make_shared<typename CAMERA::CalibrationType>(
282  camera_i.calibration()));
283  }
284  return cameras;
285  }
286 
290  double error(const Values& values) const override {
291  if (this->active(values)) {
292  return this->totalReprojectionError(this->cameras(values));
293  } else { // else of active flag
294  return 0.0;
295  }
296  }
297 
308  const Values& values) const {
309  if (!this->result_) {
310  throw("computeJacobiansWithTriangulatedPoint");
311  } else { // valid result: compute jacobians
312  size_t numViews = this->measured_.size();
313  E = Matrix::Zero(2 * numViews,
314  3); // a Point2 for each view (point jacobian)
315  b = Vector::Zero(2 * numViews); // a Point2 for each view
316  // intermediate Jacobians
317  Eigen::Matrix<double, ZDim, DimPose> dProject_dPoseCam;
318  Eigen::Matrix<double, DimPose, DimPose> dInterpPose_dPoseBody1,
319  dInterpPose_dPoseBody2, dPoseCam_dInterpPose;
321 
322  for (size_t i = 0; i < numViews; i++) { // for each camera/measurement
323  auto w_P_body1 = values.at<Pose3>(world_P_body_key_pairs_[i].first);
324  auto w_P_body2 = values.at<Pose3>(world_P_body_key_pairs_[i].second);
325  double interpolationFactor = alphas_[i];
326  // get interpolated pose:
327  auto w_P_body =
328  interpolate<Pose3>(w_P_body1, w_P_body2, interpolationFactor,
329  dInterpPose_dPoseBody1, dInterpPose_dPoseBody2);
330  const typename Base::Camera& camera_i = (*cameraRig_)[cameraIds_[i]];
331  auto body_P_cam = camera_i.pose();
332  auto w_P_cam = w_P_body.compose(body_P_cam, dPoseCam_dInterpPose);
333  typename Base::Camera camera(
334  w_P_cam, make_shared<typename CAMERA::CalibrationType>(
335  camera_i.calibration()));
336 
337  // get jacobians and error vector for current measurement
338  Point2 reprojectionError_i = camera.reprojectionError(
339  *this->result_, this->measured_.at(i), dProject_dPoseCam, Ei);
341  J.block(0, 0, ZDim, 6) =
342  dProject_dPoseCam * dPoseCam_dInterpPose *
343  dInterpPose_dPoseBody1; // (2x6) * (6x6) * (6x6)
344  J.block(0, 6, ZDim, 6) =
345  dProject_dPoseCam * dPoseCam_dInterpPose *
346  dInterpPose_dPoseBody2; // (2x6) * (6x6) * (6x6)
347 
348  // fit into the output structures
349  Fs.push_back(J);
350  size_t row = 2 * i;
351  b.segment<ZDim>(row) = -reprojectionError_i;
352  E.block<ZDim, 3>(row, 0) = Ei;
353  }
354  }
355  }
356 
358  std::shared_ptr<RegularHessianFactor<DimPose>> createHessianFactor(
359  const Values& values, const double& lambda = 0.0,
360  bool diagonalDamping = false) const {
361  // we may have multiple observation sharing the same keys (due to the
362  // rolling shutter interpolation), hence the number of unique keys may be
363  // smaller than 2 * nrMeasurements
364  size_t nrUniqueKeys =
365  this->keys_
366  .size(); // note: by construction, keys_ only contains unique keys
367 
368  typename Base::Cameras cameras = this->cameras(values);
369 
370  // Create structures for Hessian Factors
371  KeyVector js;
372  std::vector<Matrix> Gs(nrUniqueKeys * (nrUniqueKeys + 1) / 2);
373  std::vector<Vector> gs(nrUniqueKeys);
374 
375  if (this->measured_.size() !=
376  cameras.size()) // 1 observation per interpolated camera
377  throw std::runtime_error(
378  "SmartProjectionPoseFactorRollingShutter: "
379  "measured_.size() inconsistent with input");
380 
381  // triangulate 3D point at given linearization point
382  this->triangulateSafe(cameras);
383 
384  if (!this->result_) { // failed: return "empty/zero" Hessian
386  for (Matrix& m : Gs) m = Matrix::Zero(DimPose, DimPose);
387  for (Vector& v : gs) v = Vector::Zero(DimPose);
388  return std::make_shared<RegularHessianFactor<DimPose>>(this->keys_,
389  Gs, gs, 0.0);
390  } else {
391  throw std::runtime_error(
392  "SmartProjectionPoseFactorRollingShutter: "
393  "only supported degeneracy mode is ZERO_ON_DEGENERACY");
394  }
395  }
396  // compute Jacobian given triangulated 3D Point
397  FBlocks Fs;
398  Matrix E;
399  Vector b;
401 
402  // Whiten using noise model
403  this->noiseModel_->WhitenSystem(E, b);
404  for (size_t i = 0; i < Fs.size(); i++)
405  Fs[i] = this->noiseModel_->Whiten(Fs[i]);
406 
407  Matrix3 P = Cameras::PointCov(E, lambda, diagonalDamping);
408 
409  // Collect all the key pairs: these are the keys that correspond to the
410  // blocks in Fs (on which we apply the Schur Complement)
411  KeyVector nonuniqueKeys;
412  for (size_t i = 0; i < world_P_body_key_pairs_.size(); i++) {
413  nonuniqueKeys.push_back(world_P_body_key_pairs_.at(i).first);
414  nonuniqueKeys.push_back(world_P_body_key_pairs_.at(i).second);
415  }
416 
417  // Build augmented Hessian (with last row/column being the information
418  // vector) Note: we need to get the augumented hessian wrt the unique keys
419  // in key_
420  SymmetricBlockMatrix augmentedHessianUniqueKeys =
421  Base::Cameras::template SchurComplementAndRearrangeBlocks<3, 12, 6>(
422  Fs, E, P, b, nonuniqueKeys, this->keys_);
423 
424  return std::make_shared<RegularHessianFactor<DimPose>>(
425  this->keys_, augmentedHessianUniqueKeys);
426  }
427 
435  std::shared_ptr<GaussianFactor> linearizeDamped(
436  const Values& values, const double& lambda = 0.0) const {
437  // depending on flag set on construction we may linearize to different
438  // linear factors
439  switch (this->params_.linearizationMode) {
440  case HESSIAN:
441  return this->createHessianFactor(values, lambda);
442  default:
443  throw std::runtime_error(
444  "SmartProjectionPoseFactorRollingShutter: "
445  "unknown linearization mode");
446  }
447  }
448 
450  std::shared_ptr<GaussianFactor> linearize(
451  const Values& values) const override {
452  return this->linearizeDamped(values);
453  }
454 
455  private:
456 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
457  friend class boost::serialization::access;
459  template <class ARCHIVE>
460  void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
461  ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
462  }
463 #endif
464 };
465 // end of class declaration
466 
468 template <class CAMERA>
470  : public Testable<SmartProjectionPoseFactorRollingShutter<CAMERA>> {};
471 
472 } // namespace gtsam
gtsam::SmartProjectionPoseFactorRollingShutter::Cameras
CameraSet< CAMERA > Cameras
Definition: SmartProjectionPoseFactorRollingShutter.h:85
gtsam::SmartProjectionPoseFactorRollingShutter::cameras
Base::Cameras cameras(const Values &values) const override
Definition: SmartProjectionPoseFactorRollingShutter.h:266
gtsam::SmartProjectionPoseFactorRollingShutter::cameraRig
const std::shared_ptr< Cameras > & cameraRig() const
return the calibration object
Definition: SmartProjectionPoseFactorRollingShutter.h:204
gtsam::SmartProjectionPoseFactorRollingShutter::MEASUREMENTS
CAMERA::MeasurementVector MEASUREMENTS
Definition: SmartProjectionPoseFactorRollingShutter.h:52
CameraSet.h
Base class to create smart factors on poses or cameras.
gtsam::CameraSet::PointCov
static Matrix PointCov(const Matrix &E, const double lambda=0.0, bool diagonalDamping=false)
Computes Point Covariance P, with lambda parameter, dynamic version.
Definition: CameraSet.h:359
alpha
RealScalar alpha
Definition: level1_cplx_impl.h:147
gtsam::SmartProjectionPoseFactorRollingShutter::Camera
CAMERA Camera
Definition: SmartProjectionPoseFactorRollingShutter.h:84
s
RealScalar s
Definition: level1_cplx_impl.h:126
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
gtsam::SmartProjectionPoseFactorRollingShutter::world_P_body_key_pairs_
std::vector< std::pair< Key, Key > > world_P_body_key_pairs_
Definition: SmartProjectionPoseFactorRollingShutter.h:57
gtsam::HESSIAN
@ HESSIAN
Definition: SmartFactorParams.h:31
gtsam::SmartProjectionPoseFactorRollingShutter::cameraIds
const FastVector< size_t > & cameraIds() const
return the calibration object
Definition: SmartProjectionPoseFactorRollingShutter.h:207
gtsam::SmartProjectionPoseFactorRollingShutter::error
double error(const Values &values) const override
Definition: SmartProjectionPoseFactorRollingShutter.h:290
gtsam::SmartProjectionParams
Definition: SmartFactorParams.h:42
gtsam::SmartProjectionPoseFactorRollingShutter::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Definition: SmartProjectionPoseFactorRollingShutter.h:214
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::SmartProjectionPoseFactorRollingShutter::This
SmartProjectionPoseFactorRollingShutter< CAMERA > This
Definition: SmartProjectionPoseFactorRollingShutter.h:49
gtsam::SmartProjectionPoseFactorRollingShutter::MatrixZD
Eigen::Matrix< double, ZDim, DimBlock > MatrixZD
Definition: SmartProjectionPoseFactorRollingShutter.h:80
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::SmartProjectionPoseFactorRollingShutter::SmartProjectionPoseFactorRollingShutter
SmartProjectionPoseFactorRollingShutter()
Default constructor, only for serialization.
Definition: SmartProjectionPoseFactorRollingShutter.h:91
gtsam::Factor
Definition: Factor.h:70
gtsam::SmartProjectionPoseFactorRollingShutter::cameraRig_
std::shared_ptr< typename Base::Cameras > cameraRig_
Definition: SmartProjectionPoseFactorRollingShutter.h:65
gtsam::SmartProjectionPoseFactorRollingShutter::ZDim
static const int ZDim
Measurement dimension (Point2)
Definition: SmartProjectionPoseFactorRollingShutter.h:78
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::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::CameraSet
A set of cameras, all with their own calibration.
Definition: CameraSet.h:36
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::SmartProjectionPoseFactorRollingShutter::MEASUREMENT
CAMERA::Measurement MEASUREMENT
Definition: SmartProjectionPoseFactorRollingShutter.h:51
gtsam::SmartProjectionParams::linearizationMode
LinearizationMode linearizationMode
How to linearize the factor.
Definition: SmartFactorParams.h:44
J
JacobiRotation< float > J
Definition: Jacobi_makeJacobi.cpp:3
gtsam::SmartProjectionFactor::result_
TriangulationResult result_
result from triangulateSafe
Definition: SmartProjectionFactor.h:62
gtsam::SmartProjectionPoseFactorRollingShutter::Base
SmartProjectionFactor< CAMERA > Base
Definition: SmartProjectionPoseFactorRollingShutter.h:48
gtsam::row
const MATRIX::ConstRowXpr row(const MATRIX &A, size_t j)
Definition: base/Matrix.h:221
gtsam::SmartProjectionPoseFactorRollingShutter::world_P_body_key_pairs
const std::vector< std::pair< Key, Key > > & world_P_body_key_pairs() const
Definition: SmartProjectionPoseFactorRollingShutter.h:196
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::SmartProjectionPoseFactorRollingShutter::linearizeDamped
std::shared_ptr< GaussianFactor > linearizeDamped(const Values &values, const double &lambda=0.0) const
Definition: SmartProjectionPoseFactorRollingShutter.h:435
gtsam::SmartProjectionPoseFactorRollingShutter
Definition: SmartProjectionPoseFactorRollingShutter.h:45
gtsam::SmartProjectionPoseFactorRollingShutter::SmartProjectionPoseFactorRollingShutter
SmartProjectionPoseFactorRollingShutter(const SharedNoiseModel &sharedNoiseModel, const std::shared_ptr< Cameras > &cameraRig, const SmartProjectionParams &params=SmartProjectionParams())
Definition: SmartProjectionPoseFactorRollingShutter.h:100
gtsam::SmartProjectionPoseFactorRollingShutter::alphas
const std::vector< double > & alphas() const
return the interpolation factors alphas
Definition: SmartProjectionPoseFactorRollingShutter.h:201
m
Matrix3f m
Definition: AngleAxis_mimic_euler.cpp:1
gtsam::Point2
Vector2 Point2
Definition: Point2.h:32
gtsam::SmartProjectionPoseFactorRollingShutter::add
void add(const MEASUREMENTS &measurements, const std::vector< std::pair< Key, Key >> &world_P_body_key_pairs, const std::vector< double > &alphas, const FastVector< size_t > &cameraIds=FastVector< size_t >())
Definition: SmartProjectionPoseFactorRollingShutter.h:167
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::SmartProjectionPoseFactorRollingShutter::equals
bool equals(const NonlinearFactor &p, double tol=1e-9) const override
equals
Definition: SmartProjectionPoseFactorRollingShutter.h:232
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::SmartProjectionPoseFactorRollingShutter::computeJacobiansWithTriangulatedPoint
void computeJacobiansWithTriangulatedPoint(FBlocks &Fs, Matrix &E, Vector &b, const Values &values) const
Definition: SmartProjectionPoseFactorRollingShutter.h:307
gtsam::b
const G & b
Definition: Group.h:79
gtsam::SmartProjectionFactor
Definition: SmartProjectionFactor.h:44
gtsam
traits
Definition: SFMdata.h:40
gtsam::Testable
Definition: Testable.h:152
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::SmartProjectionPoseFactorRollingShutter::CALIBRATION
CAMERA::CalibrationType CALIBRATION
Definition: SmartProjectionPoseFactorRollingShutter.h:50
gtsam::Values
Definition: Values.h:65
gtsam::SmartProjectionPoseFactorRollingShutter::shared_ptr
std::shared_ptr< This > shared_ptr
shorthand for a smart pointer to a factor
Definition: SmartProjectionPoseFactorRollingShutter.h:88
gtsam::NonlinearFactor
Definition: NonlinearFactor.h:68
gtsam::SmartProjectionPoseFactorRollingShutter::DimPose
static const int DimPose
Pose3 dimension.
Definition: SmartProjectionPoseFactorRollingShutter.h:77
p
float * p
Definition: Tutorial_Map_using.cpp:9
v
Array< int, Dynamic, 1 > v
Definition: Array_initializer_list_vector_cxx11.cpp:1
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::SmartProjectionPoseFactorRollingShutter::DimBlock
static const EIGEN_MAKE_ALIGNED_OPERATOR_NEW int DimBlock
Definition: SmartProjectionPoseFactorRollingShutter.h:74
gtsam::SmartProjectionPoseFactorRollingShutter::alphas_
std::vector< double > alphas_
Definition: SmartProjectionPoseFactorRollingShutter.h:61
gtsam::SmartProjectionPoseFactorRollingShutter::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: SmartProjectionPoseFactorRollingShutter.h:358
Eigen::Matrix
The matrix class, also used for vectors and row-vectors.
Definition: 3rdparty/Eigen/Eigen/src/Core/Matrix.h:178
gtsam::SmartProjectionPoseFactorRollingShutter::cameraIds_
FastVector< size_t > cameraIds_
Definition: SmartProjectionPoseFactorRollingShutter.h:69
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::SymmetricBlockMatrix
Definition: SymmetricBlockMatrix.h:53
gtsam::SmartProjectionPoseFactorRollingShutter::linearize
std::shared_ptr< GaussianFactor > linearize(const Values &values) const override
linearize
Definition: SmartProjectionPoseFactorRollingShutter.h:450
gtsam::SmartProjectionPoseFactorRollingShutter::add
void add(const MEASUREMENT &measured, const Key &world_P_body_key1, const Key &world_P_body_key2, const double &alpha, const size_t &cameraId=0)
Definition: SmartProjectionPoseFactorRollingShutter.h:128
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::SmartProjectionPoseFactorRollingShutter::FBlocks
std::vector< MatrixZD, Eigen::aligned_allocator< MatrixZD > > FBlocks
Definition: SmartProjectionPoseFactorRollingShutter.h:82
gtsam::SmartFactorBase::measured_
ZVector measured_
Definition: SmartFactorBase.h:80


gtsam
Author(s):
autogenerated on Sat Nov 16 2024 04:04:22