Go to the documentation of this file.
24 #include <gtsam/dllexport.h>
27 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
28 #include <boost/serialization/nvp.hpp>
29 #include <boost/serialization/extended_type_info.hpp>
30 #include <boost/serialization/singleton.hpp>
31 #include <boost/serialization/shared_ptr.hpp>
39 namespace noiseModel {
69 Base(
size_t dim = 1):dim_(dim) {}
76 virtual bool isUnit()
const {
return false; }
79 inline size_t dim()
const {
return dim_;}
81 virtual void print(
const std::string&
name =
"")
const = 0;
98 virtual double squaredMahalanobisDistance(
const Vector&
v)
const;
102 return std::sqrt(squaredMahalanobisDistance(
v));
106 virtual double loss(
const double squared_distance)
const {
107 return 0.5 * squared_distance;
110 virtual void WhitenSystem(std::vector<Matrix>&
A,
Vector&
b)
const = 0;
144 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
146 friend class boost::serialization::access;
147 template<
class ARCHIVE>
148 void serialize(ARCHIVE & ar,
const unsigned int ) {
149 ar & BOOST_SERIALIZATION_NVP(dim_);
182 if (!sqrt_information_)
throw std::runtime_error(
"Gaussian: has no R matrix");
183 return *sqrt_information_;
193 const std::optional<Matrix>& sqrt_information = {})
194 :
Base(dim), sqrt_information_(sqrt_information) {}
203 static shared_ptr SqrtInformation(
const Matrix&
R,
bool smart =
true);
210 static shared_ptr Information(
const Matrix&
M,
bool smart =
true);
217 static shared_ptr Covariance(
const Matrix& covariance,
bool smart =
true);
219 void print(
const std::string&
name)
const override;
234 virtual void WhitenInPlace(
Matrix&
H)
const;
244 void WhitenSystem(std::vector<Matrix>&
A,
Vector&
b)
const override;
258 virtual std::shared_ptr<Diagonal> QR(
Matrix&
Ab)
const;
264 virtual Matrix information()
const;
267 virtual Matrix covariance()
const;
270 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
272 friend class boost::serialization::access;
273 template<
class ARCHIVE>
274 void serialize(ARCHIVE & ar,
const unsigned int ) {
275 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(
Base);
276 ar & BOOST_SERIALIZATION_NVP(sqrt_information_);
315 static shared_ptr Sigmas(
const Vector&
sigmas,
bool smart =
true);
323 static shared_ptr Variances(
const Vector& variances,
bool smart =
true);
329 static shared_ptr Precisions(
const Vector& precisions,
bool smart =
true);
331 void print(
const std::string&
name)
const override;
336 void WhitenInPlace(
Matrix&
H)
const override;
342 inline double sigma(
size_t i)
const {
return sigmas_(
i); }
348 inline double invsigma(
size_t i)
const {
return invsigmas_(
i);}
360 return invsigmas().asDiagonal();
364 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
366 friend class boost::serialization::access;
367 template<
class ARCHIVE>
368 void serialize(ARCHIVE & ar,
const unsigned int ) {
369 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(
Gaussian);
370 ar & BOOST_SERIALIZATION_NVP(sigmas_);
371 ar & BOOST_SERIALIZATION_NVP(invsigmas_);
421 bool constrained(
size_t i)
const;
442 static shared_ptr MixedSigmas(
double m,
const Vector&
sigmas);
448 static shared_ptr MixedVariances(
const Vector&
mu,
const Vector& variances);
449 static shared_ptr MixedVariances(
const Vector& variances);
455 static shared_ptr MixedPrecisions(
const Vector&
mu,
const Vector& precisions);
456 static shared_ptr MixedPrecisions(
const Vector& precisions);
463 double squaredMahalanobisDistance(
const Vector&
v)
const override;
480 void print(
const std::string&
name)
const override;
488 void WhitenInPlace(
Matrix&
H)
const override;
506 shared_ptr unit()
const;
509 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
511 friend class boost::serialization::access;
512 template<
class ARCHIVE>
513 void serialize(ARCHIVE & ar,
const unsigned int ) {
514 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(
Diagonal);
515 ar & BOOST_SERIALIZATION_NVP(mu_);
555 static shared_ptr Variance(
size_t dim,
double variance,
bool smart =
true);
561 return Variance(dim, 1.0/
precision, smart);
564 void print(
const std::string&
name)
const override;
565 double squaredMahalanobisDistance(
const Vector&
v)
const override;
569 void WhitenInPlace(
Matrix&
H)
const override;
570 void whitenInPlace(
Vector&
v)
const override;
576 inline double sigma()
const {
return sigma_; }
579 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
581 friend class boost::serialization::access;
582 template<
class ARCHIVE>
583 void serialize(ARCHIVE & ar,
const unsigned int ) {
584 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(
Diagonal);
585 ar & BOOST_SERIALIZATION_NVP(sigma_);
586 ar & BOOST_SERIALIZATION_NVP(invsigma_);
615 bool isUnit()
const override {
return true; }
617 void print(
const std::string&
name)
const override;
618 double squaredMahalanobisDistance(
const Vector&
v)
const override;
630 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
632 friend class boost::serialization::access;
633 template<
class ARCHIVE>
634 void serialize(ARCHIVE & ar,
const unsigned int ) {
635 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(
Isotropic);
675 :
Base(noise->dim()), robust_(robust), noise_(noise) {}
680 void print(
const std::string&
name)
const override;
691 {
Vector r =
v; this->WhitenSystem(r);
return r; }
695 {
throw std::invalid_argument(
"unwhiten is not currently supported for robust noise models."); }
697 double loss(
const double squared_distance)
const override {
698 return robust_->loss(
std::sqrt(squared_distance));
704 return noise_->squaredMahalanobisDistance(
v);
708 virtual void WhitenSystem(
Vector&
b)
const;
709 void WhitenSystem(std::vector<Matrix>&
A,
Vector&
b)
const override;
715 double weight(
const Vector&
v)
const override;
717 static shared_ptr Create(
718 const RobustModel::shared_ptr &robust,
const NoiseModel::shared_ptr noise);
721 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
723 friend class boost::serialization::access;
724 template<
class ARCHIVE>
725 void serialize(ARCHIVE & ar,
const unsigned int ) {
726 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(
Base);
727 ar & boost::serialization::make_nvp(
"robust_",
const_cast<RobustModel::shared_ptr&
>(robust_));
728 ar & boost::serialization::make_nvp(
"noise_",
const_cast<NoiseModel::shared_ptr&
>(noise_));
748 template<>
struct traits<noiseModel::Gaussian> :
public Testable<noiseModel::Gaussian> {};
750 template<>
struct traits<noiseModel::Constrained> :
public Testable<noiseModel::Constrained> {};
751 template<>
struct traits<noiseModel::Isotropic> :
public Testable<noiseModel::Isotropic> {};
752 template<>
struct traits<noiseModel::Unit> :
public Testable<noiseModel::Unit> {};
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
virtual bool isUnit() const
true if a unit noise model, saves slow/clumsy dynamic casting
virtual double loss(const double squared_distance) const
loss function, input is Mahalanobis distance
Annotation for function names.
double invsigma(size_t i) const
Expression of a fixed-size or dynamic-size block.
const RobustModel::shared_ptr & robust() const
Return the contained robust error function.
Array< double, 1, 3 > e(1./3., 0.5, 2.)
void unwhitenInPlace(Eigen::Block< Vector > &) const override
void unwhitenInPlace(Vector &) const override
Concept check for values that can be used in unit tests.
std::shared_ptr< Base > shared_ptr
void WhitenInPlace(Matrix &) const override
typedef and functions to augment Eigen's MatrixXd
virtual double mahalanobisDistance(const Vector &v) const
Mahalanobis distance.
Gaussian(size_t dim=1, const std::optional< Matrix > &sqrt_information={})
std::optional< Matrix > sqrt_information_
std::shared_ptr< Unit > shared_ptr
const Vector & invsigmas() const
Isotropic(size_t dim, double sigma)
noiseModel::Constrained::shared_ptr SharedConstrained
static shared_ptr Precision(size_t dim, double precision, bool smart=true)
size_t dim() const
Dimensionality.
std::shared_ptr< Gaussian > shared_ptr
static const double sigma
Vector sigmas() const override
Calculate standard deviations.
virtual Vector unweightedWhiten(const Vector &v) const
virtual bool isConstrained() const
true if a constrained noise model, saves slow/clumsy dynamic casting
Vector mu_
Penalty function weight - needs to be large enough to dominate soft constraints.
const Vector & mu() const
Access mu as a vector.
void print(const Matrix &A, const string &s, ostream &stream)
Robust()
Default Constructor for serialization.
std::shared_ptr< Base > shared_ptr
virtual double weight(const Vector &v) const
~Robust() override
Destructor.
double precision(size_t i) const
virtual void unwhitenInPlace(Vector &v) const
static shared_ptr Create(size_t dim)
noiseModel::Diagonal::shared_ptr SharedDiagonal
bool isUnit() const override
true if a unit noise model, saves slow/clumsy dynamic casting
double sigma(size_t i) const
static shared_ptr All(size_t dim)
Vector whiten(const Vector &v) const override
Whiten an error vector.
noiseModel::Gaussian::shared_ptr SharedGaussian
noiseModel::Base::shared_ptr SharedNoiseModel
const RobustModel::shared_ptr robust_
robust error function used
void whitenInPlace(Eigen::Block< Vector > &) const override
Vector whiten(const Vector &v) const override
Whiten an error vector.
mEstimator::Base RobustModel
std::optional< Vector > checkIfDiagonal(const Matrix &M)
static shared_ptr All(size_t dim, const Vector &mu)
const Vector & precisions() const
const NoiseModel::shared_ptr noise_
noise model used
noiseModel::Base NoiseModel
double loss(const double squared_distance) const override
Compute loss from the m-estimator using the Mahalanobis distance.
virtual void unwhitenInPlace(Eigen::Block< Vector > &v) const
bool isConstrained() const override
true if a constrained noise mode, saves slow/clumsy dynamic casting
virtual void whitenInPlace(Vector &v) const
Base(size_t dim=1)
primary constructor
std::shared_ptr< Diagonal > shared_ptr
Vector unwhiten(const Vector &) const override
Unwhiten an error vector.
virtual Matrix R() const
Return R itself, but note that Whiten(H) is cheaper than R*H.
static noiseModel::Diagonal::shared_ptr Diagonal(const Matrix &covariance)
std::shared_ptr< Robust > shared_ptr
Array< int, Dynamic, 1 > v
Robust(const RobustModel::shared_ptr robust, const NoiseModel::shared_ptr noise)
Constructor.
Matrix Whiten(const Matrix &A) const override
Whiten a matrix.
noiseModel::Isotropic::shared_ptr SharedIsotropic
Vector unwhiten(const Vector &v) const override
Unwhiten an error vector.
The matrix class, also used for vectors and row-vectors.
std::shared_ptr< Isotropic > shared_ptr
virtual void whitenInPlace(Eigen::Block< Vector > &v) const
Matrix R() const override
const NoiseModel::shared_ptr & noise() const
Return the contained noise model.
std::shared_ptr< Constrained > shared_ptr
double squaredMahalanobisDistance(const Vector &v) const override
Squared Mahalanobis distance v'*R'*R*v = <R*v,R*v>
const Matrix & thisR() const
Jet< T, N > sqrt(const Jet< T, N > &f)
Rot2 R(Rot2::fromAngle(0.1))
void whitenInPlace(Vector &) const override
void WhitenInPlace(Eigen::Block< Matrix >) const override
static shared_ptr All(size_t dim, double mu)
Matrix Whiten(const Matrix &H) const override
Matrix< RealScalar, Dynamic, Dynamic > M
gtsam
Author(s):
autogenerated on Thu Jun 13 2024 03:03:49