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_;
187 virtual double logDetR()
const;
195 const std::optional<Matrix>& sqrt_information = {})
196 :
Base(dim), sqrt_information_(sqrt_information) {}
205 static shared_ptr SqrtInformation(
const Matrix&
R,
bool smart =
true);
212 static shared_ptr Information(
const Matrix&
M,
bool smart =
true);
219 static shared_ptr Covariance(
const Matrix& covariance,
bool smart =
true);
221 void print(
const std::string&
name)
const override;
236 virtual void WhitenInPlace(
Matrix&
H)
const;
246 void WhitenSystem(std::vector<Matrix>&
A,
Vector&
b)
const override;
260 virtual std::shared_ptr<Diagonal> QR(
Matrix&
Ab)
const;
266 virtual Matrix information()
const;
269 virtual Matrix covariance()
const;
280 double negLogConstant()
const;
283 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
285 friend class boost::serialization::access;
286 template<
class ARCHIVE>
287 void serialize(ARCHIVE & ar,
const unsigned int ) {
288 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(
Base);
289 ar & BOOST_SERIALIZATION_NVP(sqrt_information_);
315 virtual double logDetR()
const override;
329 static shared_ptr Sigmas(
const Vector&
sigmas,
bool smart =
true);
337 static shared_ptr Variances(
const Vector& variances,
bool smart =
true);
343 static shared_ptr Precisions(
const Vector& precisions,
bool smart =
true);
345 void print(
const std::string&
name)
const override;
350 void WhitenInPlace(
Matrix&
H)
const override;
356 inline double sigma(
size_t i)
const {
return sigmas_(
i); }
362 inline double invsigma(
size_t i)
const {
return invsigmas_(
i);}
374 return invsigmas().asDiagonal();
378 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
380 friend class boost::serialization::access;
381 template<
class ARCHIVE>
382 void serialize(ARCHIVE & ar,
const unsigned int ) {
383 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(
Gaussian);
384 ar & BOOST_SERIALIZATION_NVP(sigmas_);
385 ar & BOOST_SERIALIZATION_NVP(invsigmas_);
435 bool constrained(
size_t i)
const;
456 static shared_ptr MixedSigmas(
double m,
const Vector&
sigmas);
462 static shared_ptr MixedVariances(
const Vector&
mu,
const Vector& variances);
463 static shared_ptr MixedVariances(
const Vector& variances);
469 static shared_ptr MixedPrecisions(
const Vector&
mu,
const Vector& precisions);
470 static shared_ptr MixedPrecisions(
const Vector& precisions);
477 double squaredMahalanobisDistance(
const Vector&
v)
const override;
494 void print(
const std::string&
name)
const override;
502 void WhitenInPlace(
Matrix&
H)
const override;
520 shared_ptr unit()
const;
523 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
525 friend class boost::serialization::access;
526 template<
class ARCHIVE>
527 void serialize(ARCHIVE & ar,
const unsigned int ) {
528 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(
Diagonal);
529 ar & BOOST_SERIALIZATION_NVP(mu_);
550 virtual double logDetR()
const override;
572 static shared_ptr Variance(
size_t dim,
double variance,
bool smart =
true);
578 return Variance(dim, 1.0/
precision, smart);
581 void print(
const std::string&
name)
const override;
582 double squaredMahalanobisDistance(
const Vector&
v)
const override;
586 void WhitenInPlace(
Matrix&
H)
const override;
587 void whitenInPlace(
Vector&
v)
const override;
593 inline double sigma()
const {
return sigma_; }
596 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
598 friend class boost::serialization::access;
599 template<
class ARCHIVE>
600 void serialize(ARCHIVE & ar,
const unsigned int ) {
601 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(
Diagonal);
602 ar & BOOST_SERIALIZATION_NVP(sigma_);
603 ar & BOOST_SERIALIZATION_NVP(invsigma_);
617 virtual double logDetR()
const override;
636 bool isUnit()
const override {
return true; }
638 void print(
const std::string&
name)
const override;
639 double squaredMahalanobisDistance(
const Vector&
v)
const override;
651 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
653 friend class boost::serialization::access;
654 template<
class ARCHIVE>
655 void serialize(ARCHIVE & ar,
const unsigned int ) {
656 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(
Isotropic);
696 :
Base(noise->dim()), robust_(robust), noise_(noise) {}
701 void print(
const std::string&
name)
const override;
712 {
Vector r =
v; this->WhitenSystem(r);
return r; }
716 {
throw std::invalid_argument(
"unwhiten is not currently supported for robust noise models."); }
718 double loss(
const double squared_distance)
const override {
719 return robust_->loss(
std::sqrt(squared_distance));
725 return noise_->squaredMahalanobisDistance(
v);
729 virtual void WhitenSystem(
Vector&
b)
const;
730 void WhitenSystem(std::vector<Matrix>&
A,
Vector&
b)
const override;
736 double weight(
const Vector&
v)
const override;
738 static shared_ptr Create(
739 const RobustModel::shared_ptr &robust,
const NoiseModel::shared_ptr noise);
742 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
744 friend class boost::serialization::access;
745 template<
class ARCHIVE>
746 void serialize(ARCHIVE & ar,
const unsigned int ) {
747 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(
Base);
748 ar & boost::serialization::make_nvp(
"robust_",
const_cast<RobustModel::shared_ptr&
>(robust_));
749 ar & boost::serialization::make_nvp(
"noise_",
const_cast<NoiseModel::shared_ptr&
>(noise_));
771 template<>
struct traits<noiseModel::Constrained> :
public Testable<noiseModel::Constrained> {};
772 template<>
struct traits<noiseModel::Isotropic> :
public Testable<noiseModel::Isotropic> {};
773 template<>
struct traits<noiseModel::Unit> :
public Testable<noiseModel::Unit> {};
double Gaussian(double mu, double sigma, double z)
Gaussian density function.
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
double logDeterminant(const typename BAYESTREE::sharedClique &clique)
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 Fri Nov 1 2024 03:33:50