Go to the documentation of this file.
33 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
34 #include <boost/serialization/optional.hpp>
50 template<
class CAMERA>
57 typedef typename CAMERA::MeasurementVector
ZVector;
64 typedef std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> >
FBlocks;
103 size_t expectedNumberCameras = 10)
106 if (!sharedNoiseModel)
107 throw std::runtime_error(
"SmartFactorBase: sharedNoiseModel is required");
112 if (!sharedIsotropic)
113 throw std::runtime_error(
"SmartFactorBase: needs isotropic");
129 throw std::invalid_argument(
130 "SmartFactorBase::add: adding duplicate measurement for key.");
132 this->measured_.push_back(
measured);
133 this->
keys_.push_back(key);
140 this->
add(measurements[
i], cameraKeys[
i]);
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);
157 size_t dim()
const override {
return ZDim * this->measured_.size(); }
166 cameras.push_back(
values.at<CAMERA>(k));
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";
190 if (
const This*
e =
dynamic_cast<const This*
>(&
p)) {
208 template <
class POINT>
222 for (
size_t i = 0;
i <
Fs->size();
i++) {
223 const Pose3 world_P_body =
cameras[
i].pose() * sensor_P_body;
230 J.template block<pose_dim, pose_dim>(0, 0) =
H;
247 template<
class POINT,
class ...OptArgs,
typename =
std::enable_if_t<
sizeof...(OptArgs)!=0>>
250 OptArgs&&... optArgs)
const {
270 template<
class ...OptArgs>
273 OptArgs&&... optArgs)
const {
281 template<
class POINT>
297 template<
class POINT>
299 const POINT&
point)
const {
306 return (
E.transpose() *
E).inverse();
315 template<
class POINT>
330 template<
class POINT>
341 size_t m = this->
keys_.size();
349 bool diagonalDamping =
false)
const {
358 return std::make_shared<RegularHessianFactor<Dim> >(
keys_,
368 const double lambda,
bool diagonalDamping,
381 for (
size_t i = 0;
i <
F.size();
i++)
386 std::shared_ptr<RegularImplicitSchurFactor<CAMERA> >
388 double lambda = 0.0,
bool diagonalDamping =
false)
const {
395 return std::make_shared<RegularImplicitSchurFactor<CAMERA> >(
keys_,
F,
E,
402 bool diagonalDamping =
false)
const {
407 const size_t M =
b.size();
410 return std::make_shared<JacobianFactorQ<Dim, ZDim> >(
keys_,
F,
E,
P,
b,
n);
419 size_t m = this->
keys_.size();
422 const size_t M =
ZDim *
m;
427 return std::make_shared<JacobianFactorSVD<Dim, ZDim> >(
keys_,
F, E0,
b,
n);
432 size_t m =
Fs.size();
435 for (
size_t i = 0;
i <
m; ++
i)
449 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
450 friend class boost::serialization::access;
452 template<
class ARCHIVE>
453 void serialize(ARCHIVE & ar,
const unsigned int ) {
454 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(
Base);
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
Vector unwhitenedError(const Cameras &cameras, const POINT &point, typename Cameras::FBlocks *Fs=nullptr, Matrix *E=nullptr) const
std::optional< Pose3 > body_P_sensor_
Pose of the camera in the body frame.
Base class to create smart factors on poses or cameras.
Pose3 body_P_sensor() const
static Matrix PointCov(const Matrix &E, const double lambda=0.0, bool diagonalDamping=false)
Computes Point Covariance P, with lambda parameter, dynamic version.
Array< double, 1, 3 > e(1./3., 0.5, 2.)
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)
SmartFactorBase(const SharedNoiseModel &sharedNoiseModel, std::optional< Pose3 > body_P_sensor={}, size_t expectedNumberCameras=10)
Construct with given noise model and optional arguments.
Class compose(const Class &g) const
virtual bool equals(const NonlinearFactor &f, double tol=1e-9) const
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
const ZVector & measured() const
Return the 2D measurements (ZDim, in general).
SmartFactorBase< CAMERA > This
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.
static const int Dim
Camera dimension.
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Vector reprojectionError(const POINT &point, const ZVector &measured, FBlocks *Fs=nullptr, Matrix *E=nullptr) const
Calculate vector [project2(point)-z] of re-projection errors.
A set of cameras, all with their own calibration.
CAMERA::MeasurementVector ZVector
Eigen::Matrix< double, ZDim, Dim > MatrixZD
Vector unwhitenedError(const Cameras &cameras, const POINT &point, OptArgs &&... optArgs) const
KeyFormatter DefaultKeyFormatter
Assign default key formatter.
HessianFactor class with constant sized blocks.
SmartFactorBase()
Default Constructor, for serialization.
~SmartFactorBase() override
Virtual destructor, subclasses from NonlinearFactor.
GenericMeasurement< Point2, Point2 > Measurement
JacobiRotation< float > J
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.
static void FillDiagonalF(const FBlocks &Fs, Matrix &F)
Create BIG block-diagonal matrix F from Fblocks.
void whitenJacobians(FBlocks &F, Matrix &E, Vector &b) const
Whiten the Jacobians computed by computeJacobians using noiseModel_.
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)
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
SharedIsotropic noiseModel_
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.
CameraSet< CAMERA > Cameras
The CameraSet data structure is used to refer to a set of cameras.
noiseModel::Base::shared_ptr SharedNoiseModel
void add(const ZVector &measurements, const KeyVector &cameraKeys)
Add a bunch of measurements, together with the camera keys.
std::shared_ptr< JacobianFactor > createJacobianSVDFactor(const Cameras &cameras, const Point3 &point, double lambda=0.0) const
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
virtual Cameras cameras(const Values &values) const
Collect all cameras: important that in key order.
static Matrix PointCov(const Matrix &E)
Computes Point Covariance P from the "point Jacobian" E.
GTSAM_MAKE_ALIGNED_OPERATOR_NEW typedef std::shared_ptr< This > shared_ptr
shorthand for a smart pointer to a factor.
void svd(const Matrix &A, Matrix &U, Vector &S, Matrix &V)
const gtsam::Symbol key('X', 0)
std::vector< MatrixZD, Eigen::aligned_allocator< MatrixZD > > FBlocks
Non-linear factor base classes.
Base class for smart factors. This base class has no internal point, but it has a measurement,...
Two-sided Jacobi SVD decomposition of a rectangular matrix.
KeyVector keys_
The keys involved in this factor.
static const int ZDim
Measurement dimension.
std::vector< double > measurements
Vector whitenedError(const Cameras &cameras, const POINT &point) const
void correctForMissingMeasurements(const Cameras &cameras, Vector &ue, OptArgs &&... optArgs) const
double totalReprojectionError(const Cameras &cameras, const POINT &point) const
noiseModel::Isotropic::shared_ptr SharedIsotropic
The matrix class, also used for vectors and row-vectors.
static shared_ptr Sigma(size_t dim, double sigma, bool smart=true)
A subclass of GaussianFactor specialized to structureless SFM.
virtual void correctForMissingMeasurements(const Cameras &cameras, Vector &ue, typename Cameras::FBlocks *Fs=nullptr, Matrix *E=nullptr) const
Give fixed size dimension of a type, fails at compile time if dynamic.
void updateAugmentedHessian(const Cameras &cameras, const Point3 &point, const double lambda, bool diagonalDamping, SymmetricBlockMatrix &augmentedHessian, const KeyVector allKeys) const
void add(const Z &measured, const Key &key)
bool equals(const NonlinearFactor &p, double tol=1e-9) const override
equals
std::uint64_t Key
Integer nonlinear key type.
void add(const SFM_TRACK &trackToAdd)
typename std::enable_if< B, T >::type enable_if_t
from cpp_future import (convenient aliases from C++14/17)
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
void computeJacobiansSVD(FBlocks &Fs, Matrix &Enull, Vector &b, const Cameras &cameras, const POINT &point) const
std::vector< MatrixZD, Eigen::aligned_allocator< MatrixZD > > FBlocks
size_t dim() const override
Return the dimension (number of rows!) of the factor.
void computeJacobians(FBlocks &Fs, Matrix &E, Vector &b, const Cameras &cameras, const POINT &point) const
virtual double error(const Values &c) const
Matrix< RealScalar, Dynamic, Dynamic > M
gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:35:28