SmartFactorBase.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 
22 #pragma once
23 
27 
31 
32 #include <optional>
33 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
34 #include <boost/serialization/optional.hpp>
35 #endif
36 #include <vector>
37 
38 namespace gtsam {
39 
50 template<class CAMERA>
52 
53 private:
56  typedef typename CAMERA::Measurement Z;
57  typedef typename CAMERA::MeasurementVector ZVector;
58 
59 public:
60 
61  static const int Dim = traits<CAMERA>::dimension;
62  static const int ZDim = traits<Z>::dimension;
63  typedef Eigen::Matrix<double, ZDim, Dim> MatrixZD; // F blocks (derivatives wrpt camera)
64  typedef std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> > FBlocks; // vector of F blocks
65 
66 protected:
74 
81 
82  std::optional<Pose3>
84 
85  // Cache for Fblocks, to avoid a malloc ever time we re-linearize
86  mutable FBlocks Fs;
87 
88  public:
90 
92  typedef std::shared_ptr<This> shared_ptr;
93 
96 
99 
101  SmartFactorBase(const SharedNoiseModel& sharedNoiseModel,
102  std::optional<Pose3> body_P_sensor = {},
103  size_t expectedNumberCameras = 10)
104  : body_P_sensor_(body_P_sensor), Fs(expectedNumberCameras) {
105 
106  if (!sharedNoiseModel)
107  throw std::runtime_error("SmartFactorBase: sharedNoiseModel is required");
108 
109  SharedIsotropic sharedIsotropic = std::dynamic_pointer_cast<
110  noiseModel::Isotropic>(sharedNoiseModel);
111 
112  if (!sharedIsotropic)
113  throw std::runtime_error("SmartFactorBase: needs isotropic");
114 
115  noiseModel_ = sharedIsotropic;
116  }
117 
119  ~SmartFactorBase() override {
120  }
121 
127  void add(const Z& measured, const Key& key) {
128  if(std::find(keys_.begin(), keys_.end(), key) != keys_.end()) {
129  throw std::invalid_argument(
130  "SmartFactorBase::add: adding duplicate measurement for key.");
131  }
132  this->measured_.push_back(measured);
133  this->keys_.push_back(key);
134  }
135 
137  void add(const ZVector& measurements, const KeyVector& cameraKeys) {
138  assert(measurements.size() == cameraKeys.size());
139  for (size_t i = 0; i < measurements.size(); i++) {
140  this->add(measurements[i], cameraKeys[i]);
141  }
142  }
143 
148  template<class SFM_TRACK>
149  void add(const SFM_TRACK& trackToAdd) {
150  for (size_t k = 0; k < trackToAdd.numberMeasurements(); k++) {
151  this->measured_.push_back(trackToAdd.measurements[k].second);
152  this->keys_.push_back(trackToAdd.measurements[k].first);
153  }
154  }
155 
157  size_t dim() const override { return ZDim * this->measured_.size(); }
158 
160  const ZVector& measured() const { return measured_; }
161 
163  virtual Cameras cameras(const Values& values) const {
165  for(const Key& k: this->keys_) {
166  cameras.push_back(values.at<CAMERA>(k));
167  }
168  return cameras;
169  }
170 
176  void print(const std::string& s = "", const KeyFormatter& keyFormatter =
177  DefaultKeyFormatter) const override {
178  std::cout << s << "SmartFactorBase, z = \n";
179  for (size_t k = 0; k < measured_.size(); ++k) {
180  std::cout << "measurement " << k<<", px = \n" << measured_[k] << "\n";
181  noiseModel_->print("noise model = ");
182  }
183  if(body_P_sensor_)
184  body_P_sensor_->print("body_P_sensor_:\n");
185  Base::print("", keyFormatter);
186  }
187 
189  bool equals(const NonlinearFactor& p, double tol = 1e-9) const override {
190  if (const This* e = dynamic_cast<const This*>(&p)) {
191  // Check that all measurements are the same.
192  for (size_t i = 0; i < measured_.size(); i++) {
193  if (!traits<Z>::Equals(this->measured_.at(i), e->measured_.at(i), tol))
194  return false;
195  }
196  // If so, check base class.
197  return Base::equals(p, tol);
198  } else {
199  return false;
200  }
201  }
202 
208  template <class POINT>
210  const Cameras& cameras, const POINT& point,
211  typename Cameras::FBlocks* Fs = nullptr, //
212  Matrix* E = nullptr) const {
213  // Reproject, with optional derivatives.
215 
216  // Apply chain rule if body_P_sensor_ is given.
217  if (body_P_sensor_ && Fs) {
218  const Pose3 sensor_P_body = body_P_sensor_->inverse();
219  constexpr int camera_dim = traits<CAMERA>::dimension;
220  constexpr int pose_dim = traits<Pose3>::dimension;
221 
222  for (size_t i = 0; i < Fs->size(); i++) {
223  const Pose3 world_P_body = cameras[i].pose() * sensor_P_body;
225  J.setZero();
227  // Call compose to compute Jacobian for camera extrinsics
228  world_P_body.compose(*body_P_sensor_, H);
229  // Assign extrinsics part of the Jacobian
230  J.template block<pose_dim, pose_dim>(0, 0) = H;
231  Fs->at(i) = Fs->at(i) * J;
232  }
233  }
234 
235  // Correct the Jacobians in case some measurements are missing.
237 
238  return error;
239  }
240 
247  template<class POINT, class ...OptArgs, typename = std::enable_if_t<sizeof...(OptArgs)!=0>>
249  const Cameras& cameras, const POINT& point,
250  OptArgs&&... optArgs) const {
251  return unwhitenedError(cameras, point, (&optArgs)...);
252  }
253 
260  const Cameras& cameras, Vector& ue,
261  typename Cameras::FBlocks* Fs = nullptr,
262  Matrix* E = nullptr) const {}
263 
270  template<class ...OptArgs>
272  const Cameras& cameras, Vector& ue,
273  OptArgs&&... optArgs) const {
274  correctForMissingMeasurements(cameras, ue, (&optArgs)...);
275  }
276 
281  template<class POINT>
282  Vector whitenedError(const Cameras& cameras, const POINT& point) const {
284  if (noiseModel_)
285  noiseModel_->whitenInPlace(error);
286  return error;
287  }
288 
297  template<class POINT>
299  const POINT& point) const {
301  return 0.5 * error.dot(error);
302  }
303 
305  static Matrix PointCov(const Matrix& E) {
306  return (E.transpose() * E).inverse();
307  }
308 
315  template<class POINT>
317  const Cameras& cameras, const POINT& point) const {
318  // Project into Camera set and calculate derivatives
319  // As in expressionFactor, RHS vector b = - (h(x_bar) - z) = z-h(x_bar)
320  // Indeed, nonlinear error |h(x_bar+dx)-z| ~ |h(x_bar) + A*dx - z|
321  // = |A*dx - (z-h(x_bar))|
322  b = -unwhitenedError(cameras, point, &Fs, &E);
323  }
324 
330  template<class POINT>
332  Vector& b, const Cameras& cameras, const POINT& point) const {
333 
334  Matrix E;
336 
337  static const int N = FixedDimension<POINT>::value; // 2 (Unit3) or 3 (Point3)
338 
339  // Do SVD on A.
341  size_t m = this->keys_.size();
342  Enull = svd.matrixU().block(0, N, ZDim * m, ZDim * m - N); // last ZDim*m-N columns
343  }
344 
346  // TODO(dellaert): Not used/tested anywhere and not properly whitened.
347  std::shared_ptr<RegularHessianFactor<Dim> > createHessianFactor(
348  const Cameras& cameras, const Point3& point, const double lambda = 0.0,
349  bool diagonalDamping = false) const {
350 
351  Matrix E;
352  Vector b;
354 
355  // build augmented hessian
356  SymmetricBlockMatrix augmentedHessian = Cameras::SchurComplement(Fs, E, b);
357 
358  return std::make_shared<RegularHessianFactor<Dim> >(keys_,
359  augmentedHessian);
360  }
361 
368  const double lambda, bool diagonalDamping,
369  SymmetricBlockMatrix& augmentedHessian,
370  const KeyVector allKeys) const {
371  Matrix E;
372  Vector b;
374  Cameras::UpdateSchurComplement(Fs, E, b, allKeys, keys_, augmentedHessian);
375  }
376 
378  void whitenJacobians(FBlocks& F, Matrix& E, Vector& b) const {
379  noiseModel_->WhitenSystem(E, b);
380  // TODO make WhitenInPlace work with any dense matrix type
381  for (size_t i = 0; i < F.size(); i++)
382  F[i] = noiseModel_->Whiten(F[i]);
383  }
384 
386  std::shared_ptr<RegularImplicitSchurFactor<CAMERA> > //
388  double lambda = 0.0, bool diagonalDamping = false) const {
389  Matrix E;
390  Vector b;
391  FBlocks F;
393  whitenJacobians(F, E, b);
394  Matrix P = Cameras::PointCov(E, lambda, diagonalDamping);
395  return std::make_shared<RegularImplicitSchurFactor<CAMERA> >(keys_, F, E,
396  P, b);
397  }
398 
400  std::shared_ptr<JacobianFactorQ<Dim, ZDim> > createJacobianQFactor(
401  const Cameras& cameras, const Point3& point, double lambda = 0.0,
402  bool diagonalDamping = false) const {
403  Matrix E;
404  Vector b;
405  FBlocks F;
407  const size_t M = b.size();
408  Matrix P = Cameras::PointCov(E, lambda, diagonalDamping);
410  return std::make_shared<JacobianFactorQ<Dim, ZDim> >(keys_, F, E, P, b, n);
411  }
412 
417  std::shared_ptr<JacobianFactor> createJacobianSVDFactor(
418  const Cameras& cameras, const Point3& point, double lambda = 0.0) const {
419  size_t m = this->keys_.size();
420  FBlocks F;
421  Vector b;
422  const size_t M = ZDim * m;
423  Matrix E0(M, M - 3);
426  noiseModel_->sigma());
427  return std::make_shared<JacobianFactorSVD<Dim, ZDim> >(keys_, F, E0, b, n);
428  }
429 
431  static void FillDiagonalF(const FBlocks& Fs, Matrix& F) {
432  size_t m = Fs.size();
433  F.resize(ZDim * m, Dim * m);
434  F.setZero();
435  for (size_t i = 0; i < m; ++i)
436  F.block<ZDim, Dim>(ZDim * i, Dim * i) = Fs.at(i);
437  }
438 
439  // Return sensor pose.
441  if(body_P_sensor_)
442  return *body_P_sensor_;
443  else
444  return Pose3(); // if unspecified, the transformation is the identity
445  }
446 
447 private:
448 
449 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
450  friend class boost::serialization::access;
452  template<class ARCHIVE>
453  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
454  ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
455  ar & BOOST_SERIALIZATION_NVP(noiseModel_);
456  ar & BOOST_SERIALIZATION_NVP(measured_);
457  ar & BOOST_SERIALIZATION_NVP(body_P_sensor_);
458  }
459 #endif
460 };
461 // end class SmartFactorBase
462 
463 // Definitions need to avoid link errors (above are only declarations)
464 template<class CAMERA> const int SmartFactorBase<CAMERA>::Dim;
465 template<class CAMERA> const int SmartFactorBase<CAMERA>::ZDim;
466 
467 } // \ 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::SmartFactorBase::unwhitenedError
Vector unwhitenedError(const Cameras &cameras, const POINT &point, typename Cameras::FBlocks *Fs=nullptr, Matrix *E=nullptr) const
Definition: SmartFactorBase.h:209
gtsam::SmartFactorBase::body_P_sensor_
std::optional< Pose3 > body_P_sensor_
Pose of the camera in the body frame.
Definition: SmartFactorBase.h:83
CameraSet.h
Base class to create smart factors on poses or cameras.
gtsam::SmartFactorBase::body_P_sensor
Pose3 body_P_sensor() const
Definition: SmartFactorBase.h:440
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
s
RealScalar s
Definition: level1_cplx_impl.h:126
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
gtsam::CameraSet::UpdateSchurComplement
static void UpdateSchurComplement(const FBlocks &Fs, const Matrix &E, const Eigen::Matrix< double, N, N > &P, const Vector &b, const KeyVector &allKeys, const KeyVector &keys, SymmetricBlockMatrix &augmentedHessian)
Definition: CameraSet.h:397
gtsam::SmartFactorBase::SmartFactorBase
SmartFactorBase(const SharedNoiseModel &sharedNoiseModel, std::optional< Pose3 > body_P_sensor={}, size_t expectedNumberCameras=10)
Construct with given noise model and optional arguments.
Definition: SmartFactorBase.h:101
gtsam::LieGroup::compose
Class compose(const Class &g) const
Definition: Lie.h:48
gtsam::NonlinearFactor::equals
virtual bool equals(const NonlinearFactor &f, double tol=1e-9) const
Definition: NonlinearFactor.cpp:45
gtsam::SmartFactorBase::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Definition: SmartFactorBase.h:176
gtsam::SmartFactorBase::measured
const ZVector & measured() const
Return the 2D measurements (ZDim, in general).
Definition: SmartFactorBase.h:160
Eigen::ComputeFullU
@ ComputeFullU
Definition: Constants.h:393
gtsam::SmartFactorBase::This
SmartFactorBase< CAMERA > This
Definition: SmartFactorBase.h:55
gtsam::SmartFactorBase::createHessianFactor
std::shared_ptr< RegularHessianFactor< Dim > > createHessianFactor(const Cameras &cameras, const Point3 &point, const double lambda=0.0, bool diagonalDamping=false) const
Linearize to a Hessianfactor.
Definition: SmartFactorBase.h:347
gtsam::SmartFactorBase::Fs
FBlocks Fs
Definition: SmartFactorBase.h:86
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
different_sigmas::values
HybridValues values
Definition: testHybridBayesNet.cpp:245
gtsam::Factor
Definition: Factor.h:70
gtsam::SmartFactorBase::Dim
static const int Dim
Camera dimension.
Definition: SmartFactorBase.h:61
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::CameraSet::reprojectionError
Vector reprojectionError(const POINT &point, const ZVector &measured, FBlocks *Fs=nullptr, Matrix *E=nullptr) const
Calculate vector [project2(point)-z] of re-projection errors.
Definition: CameraSet.h:149
gtsam::CameraSet
A set of cameras, all with their own calibration.
Definition: CameraSet.h:36
gtsam::SmartFactorBase::ZVector
CAMERA::MeasurementVector ZVector
Definition: SmartFactorBase.h:57
gtsam::SmartFactorBase::MatrixZD
Eigen::Matrix< double, ZDim, Dim > MatrixZD
Definition: SmartFactorBase.h:63
gtsam::SmartFactorBase::unwhitenedError
Vector unwhitenedError(const Cameras &cameras, const POINT &point, OptArgs &&... optArgs) const
Definition: SmartFactorBase.h:248
gtsam::DefaultKeyFormatter
KeyFormatter DefaultKeyFormatter
Assign default key formatter.
Definition: Key.cpp:30
RegularHessianFactor.h
HessianFactor class with constant sized blocks.
gtsam::SmartFactorBase::SmartFactorBase
SmartFactorBase()
Default Constructor, for serialization.
Definition: SmartFactorBase.h:98
n
int n
Definition: BiCGSTAB_simple.cpp:1
gtsam::SmartFactorBase::~SmartFactorBase
~SmartFactorBase() override
Virtual destructor, subclasses from NonlinearFactor.
Definition: SmartFactorBase.h:119
simulated2D::Measurement
GenericMeasurement< Point2, Point2 > Measurement
Definition: simulated2D.h:278
gtsam_unstable.tests.test_ProjectionFactorRollingShutter.point
point
Definition: test_ProjectionFactorRollingShutter.py:25
J
JacobiRotation< float > J
Definition: Jacobi_makeJacobi.cpp:3
gtsam::SmartFactorBase::createRegularImplicitSchurFactor
std::shared_ptr< RegularImplicitSchurFactor< CAMERA > > createRegularImplicitSchurFactor(const Cameras &cameras, const Point3 &point, double lambda=0.0, bool diagonalDamping=false) const
Return Jacobians as RegularImplicitSchurFactor with raw access.
Definition: SmartFactorBase.h:387
gtsam::SmartFactorBase::FillDiagonalF
static void FillDiagonalF(const FBlocks &Fs, Matrix &F)
Create BIG block-diagonal matrix F from Fblocks.
Definition: SmartFactorBase.h:431
gtsam::SmartFactorBase::whitenJacobians
void whitenJacobians(FBlocks &F, Matrix &E, Vector &b) const
Whiten the Jacobians computed by computeJacobians using noiseModel_.
Definition: SmartFactorBase.h:378
gtsam::noiseModel::Isotropic
Definition: NoiseModel.h:541
gtsam::CameraSet::SchurComplement
static SymmetricBlockMatrix SchurComplement(const std::vector< Eigen::Matrix< double, ZDim, ND >, Eigen::aligned_allocator< Eigen::Matrix< double, ZDim, ND >>> &Fs, const Matrix &E, const Eigen::Matrix< double, N, N > &P, const Vector &b)
Definition: CameraSet.h:174
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::SmartFactorBase::createJacobianQFactor
std::shared_ptr< JacobianFactorQ< Dim, ZDim > > createJacobianQFactor(const Cameras &cameras, const Point3 &point, double lambda=0.0, bool diagonalDamping=false) const
Return Jacobians as JacobianFactorQ.
Definition: SmartFactorBase.h:400
gtsam::SmartFactorBase::Cameras
CameraSet< CAMERA > Cameras
The CameraSet data structure is used to refer to a set of cameras.
Definition: SmartFactorBase.h:95
gtsam::SmartFactorBase::Base
NonlinearFactor Base
Definition: SmartFactorBase.h:54
m
Matrix3f m
Definition: AngleAxis_mimic_euler.cpp:1
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:762
gtsam::SmartFactorBase::add
void add(const ZVector &measurements, const KeyVector &cameraKeys)
Add a bunch of measurements, together with the camera keys.
Definition: SmartFactorBase.h:137
JacobianFactorSVD.h
gtsam::SmartFactorBase::createJacobianSVDFactor
std::shared_ptr< JacobianFactor > createJacobianSVDFactor(const Cameras &cameras, const Point3 &point, double lambda=0.0) const
Definition: SmartFactorBase.h:417
gtsam::NonlinearFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Definition: NonlinearFactor.cpp:35
gtsam::symbol_shorthand::F
Key F(std::uint64_t j)
Definition: inference/Symbol.h:153
gtsam::SmartFactorBase::cameras
virtual Cameras cameras(const Values &values) const
Collect all cameras: important that in key order.
Definition: SmartFactorBase.h:163
lambda
static double lambda[]
Definition: jv.c:524
gtsam::SmartFactorBase::PointCov
static Matrix PointCov(const Matrix &E)
Computes Point Covariance P from the "point Jacobian" E.
Definition: SmartFactorBase.h:305
gtsam::SmartFactorBase::shared_ptr
GTSAM_MAKE_ALIGNED_OPERATOR_NEW typedef std::shared_ptr< This > shared_ptr
shorthand for a smart pointer to a factor.
Definition: SmartFactorBase.h:92
gtsam::svd
void svd(const Matrix &A, Matrix &U, Vector &S, Matrix &V)
Definition: Matrix.cpp:558
key
const gtsam::Symbol key('X', 0)
E
DiscreteKey E(5, 2)
gtsam::SmartFactorBase::FBlocks
std::vector< MatrixZD, Eigen::aligned_allocator< MatrixZD > > FBlocks
Definition: SmartFactorBase.h:64
NonlinearFactor.h
Non-linear factor base classes.
gtsam::SmartFactorBase
Base class for smart factors. This base class has no internal point, but it has a measurement,...
Definition: SmartFactorBase.h:51
gtsam::b
const G & b
Definition: Group.h:79
Eigen::JacobiSVD
Two-sided Jacobi SVD decomposition of a rectangular matrix.
Definition: ForwardDeclarations.h:278
gtsam
traits
Definition: SFMdata.h:40
gtsam::Factor::keys_
KeyVector keys_
The keys involved in this factor.
Definition: Factor.h:88
gtsam::traits
Definition: Group.h:36
gtsam::SmartFactorBase::ZDim
static const int ZDim
Measurement dimension.
Definition: SmartFactorBase.h:62
estimation_fixture::measurements
std::vector< double > measurements
Definition: testHybridEstimation.cpp:51
gtsam::Values
Definition: Values.h:65
gtsam::NonlinearFactor
Definition: NonlinearFactor.h:68
gtsam::SmartFactorBase::whitenedError
Vector whitenedError(const Cameras &cameras, const POINT &point) const
Definition: SmartFactorBase.h:282
p
float * p
Definition: Tutorial_Map_using.cpp:9
gtsam::SmartFactorBase::correctForMissingMeasurements
void correctForMissingMeasurements(const Cameras &cameras, Vector &ue, OptArgs &&... optArgs) const
Definition: SmartFactorBase.h:271
gtsam::SmartFactorBase::totalReprojectionError
double totalReprojectionError(const Cameras &cameras, const POINT &point) const
Definition: SmartFactorBase.h:298
P
static double P[]
Definition: ellpe.c:68
gtsam::tol
const G double tol
Definition: Group.h:79
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
gtsam::SharedIsotropic
noiseModel::Isotropic::shared_ptr SharedIsotropic
Definition: NoiseModel.h:766
Eigen::Matrix
The matrix class, also used for vectors and row-vectors.
Definition: 3rdparty/Eigen/Eigen/src/Core/Matrix.h:178
gtsam::noiseModel::Isotropic::Sigma
static shared_ptr Sigma(size_t dim, double sigma, bool smart=true)
Definition: NoiseModel.cpp:624
RegularImplicitSchurFactor.h
A subclass of GaussianFactor specialized to structureless SFM.
gtsam::SmartFactorBase::correctForMissingMeasurements
virtual void correctForMissingMeasurements(const Cameras &cameras, Vector &ue, typename Cameras::FBlocks *Fs=nullptr, Matrix *E=nullptr) const
Definition: SmartFactorBase.h:259
N
#define N
Definition: igam.h:9
gtsam::FixedDimension
Give fixed size dimension of a type, fails at compile time if dynamic.
Definition: Manifold.h:161
gtsam::SmartFactorBase::updateAugmentedHessian
void updateAugmentedHessian(const Cameras &cameras, const Point3 &point, const double lambda, bool diagonalDamping, SymmetricBlockMatrix &augmentedHessian, const KeyVector allKeys) const
Definition: SmartFactorBase.h:367
gtsam::SmartFactorBase::add
void add(const Z &measured, const Key &key)
Definition: SmartFactorBase.h:127
Base
Definition: test_virtual_functions.cpp:156
gtsam::SmartFactorBase::Z
CAMERA::Measurement Z
Definition: SmartFactorBase.h:56
gtsam::SmartFactorBase::equals
bool equals(const NonlinearFactor &p, double tol=1e-9) const override
equals
Definition: SmartFactorBase.h:189
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
gtsam::SmartFactorBase::add
void add(const SFM_TRACK &trackToAdd)
Definition: SmartFactorBase.h:149
gtsam::SymmetricBlockMatrix
Definition: SymmetricBlockMatrix.h:53
enable_if_t
typename std::enable_if< B, T >::type enable_if_t
from cpp_future import (convenient aliases from C++14/17)
Definition: wrap/pybind11/include/pybind11/detail/common.h:654
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
Definition: types.h:279
i
int i
Definition: BiCGSTAB_step_by_step.cpp:9
gtsam::SmartFactorBase::computeJacobiansSVD
void computeJacobiansSVD(FBlocks &Fs, Matrix &Enull, Vector &b, const Cameras &cameras, const POINT &point) const
Definition: SmartFactorBase.h:331
gtsam::CameraSet::FBlocks
std::vector< MatrixZD, Eigen::aligned_allocator< MatrixZD > > FBlocks
Definition: CameraSet.h:78
gtsam::SmartFactorBase::dim
size_t dim() const override
Return the dimension (number of rows!) of the factor.
Definition: SmartFactorBase.h:157
gtsam::SmartFactorBase::measured_
ZVector measured_
Definition: SmartFactorBase.h:80
gtsam::SmartFactorBase::computeJacobians
void computeJacobians(FBlocks &Fs, Matrix &E, Vector &b, const Cameras &cameras, const POINT &point) const
Definition: SmartFactorBase.h:316
gtsam::NonlinearFactor::error
virtual double error(const Values &c) const
Definition: NonlinearFactor.cpp:25
JacobianFactorQ.h
M
Matrix< RealScalar, Dynamic, Dynamic > M
Definition: bench_gemm.cpp:51


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