Go to the documentation of this file.
31 namespace noiseModel {
39 template<
class MATRIX>
41 size_t n =
Ab.cols()-1;
42 Ab.middleCols(
j+1,
n-
j) -=
a * rd.segment(
j+1,
n-
j).transpose();
48 size_t m =
M.rows(),
n =
M.cols();
53 for (
i = 0;
i <
m;
i++)
55 for (
j =
i + 1;
j <
n;
j++)
64 for (
j = 0;
j <
n;
j++)
72 throw(
"Base::sigmas: sigmas() not implemented for this noise model");
76 double Base::squaredMahalanobisDistance(
const Vector&
v)
const {
84 size_t m =
R.rows(),
n =
R.cols();
86 throw invalid_argument(
"Gaussian::SqrtInformation: R not square");
90 return Diagonal::Sigmas(
diagonal->array().inverse(),
true);
93 return std::make_shared<Gaussian>(
R.rows(),
R);
98 size_t m = information.rows(),
n = information.cols();
100 throw invalid_argument(
"Gaussian::Information: R not square");
101 std::optional<Vector>
diagonal = {};
105 return Diagonal::Precisions(*
diagonal,
true);
109 return std::make_shared<Gaussian>(
n,
R);
116 size_t m = covariance.rows(),
n = covariance.cols();
118 throw invalid_argument(
"Gaussian::Covariance: covariance not square");
119 std::optional<Vector> variances = {};
123 return Diagonal::Variances(*variances,
true);
129 return Information(covariance.inverse(),
false);
141 if (
p ==
nullptr)
return false;
142 if (
typeid(*
this) !=
typeid(*p))
return false;
150 Matrix I = Matrix::Identity(
R.rows(),
R.cols());
154 return Rinv * Rinv.transpose();
178 void Gaussian::WhitenInPlace(
Matrix&
H)
const {
191 gttic(Gaussian_noise_model_QR);
193 static const bool debug =
false;
197 size_t m =
Ab.rows(),
n =
Ab.cols()-1;
198 size_t maxRank =
min(
m,
n);
213 return noiseModel::Unit::Create(maxRank);
216 void Gaussian::WhitenSystem(vector<Matrix>& A,
Vector&
b)
const {
217 for(
Matrix& Aj:
A) { WhitenInPlace(Aj); }
239 Matrix Gaussian::information()
const {
return R().transpose() *
R(); }
242 double Gaussian::logDetR()
const {
244 R().diagonal().unaryExpr([](
double x) {
return log(
x); }).sum();
254 return -2.0 * logDetR();
258 double Gaussian::negLogConstant()
const {
264 constexpr
double log2pi = 1.8378770664093454835606594728112;
266 return 0.5 *
n * log2pi - logDetR();
285 return (smart && (variances.array() == variances(0)).
all())
294 if (
n == 0)
goto full;
297 if ((
sigmas.array() < 1
e-8).any()) {
401 assert (
b.size()==
a.size());
403 for(
size_t i = 0;
i <
n;
i++ ) {
404 const double& ai =
a(
i), bi =
b(
i);
405 c(
i) = (bi==0.0) ? ai : ai/bi;
421 const Vector& variances) {
429 const Vector& precisions) {
471 for (
size_t i=0;
i<
dim(); ++
i)
484 template <
typename VECTOR>
486 std::optional<size_t> constraint_row;
489 double max_element = 1
e-9;
490 for (
size_t i = 0;
i <
m;
i++) {
494 if (abs_ai > max_element) {
495 max_element = abs_ai;
499 return constraint_row;
503 static const double kInfinity = std::numeric_limits<double>::infinity();
506 size_t m =
Ab.rows();
507 const size_t n =
Ab.cols() - 1;
508 const size_t maxRank =
min(
m,
n);
511 typedef std::tuple<size_t, Matrix, double> Triple;
522 for (
size_t j = 0;
j <
n; ++
j) {
529 if (constraint_row) {
534 rd =
Ab.row(*constraint_row);
540 if (
Rd.size() >= maxRank)
546 if (*constraint_row !=
m) {
547 Ab.row(*constraint_row) =
Ab.row(
m);
548 weights(*constraint_row) = weights(
m);
554 a_reduced *= (1.0/rd(0,
j));
557 Ab.block(0,
j + 1,
m,
n -
j).noalias() -= a_reduced * rd.middleCols(
j + 1,
n -
j);
566 for (
size_t i = 0;
i <
m;
i++) {
569 pseudo[
i] = weights[
i] * ai;
580 rd.block(0,
j + 1, 1,
n -
j) = pseudo.transpose() *
Ab.block(0,
j + 1,
m,
n -
j);
592 if (
Rd.size() >= maxRank)
596 Ab.block(0,
j + 1,
m,
n -
j).noalias() -=
a * rd.middleCols(
j + 1,
n -
j);
607 for (
const Triple&
t:
Rd) {
608 const size_t&
j = std::get<0>(
t);
609 const Matrix& rd = std::get<1>(
t);
613 Ab.block(
i,
j, 1,
n + 1 -
j) = rd.block(0,
j, 1,
n + 1 -
j);
637 cout <<
"isotropic dim=" <<
dim() <<
" sigma=" <<
sigma_ << endl;
682 cout <<
name <<
"unit (" <<
dim_ <<
") " << endl;
704 if (
p ==
nullptr)
return false;
734 return noise_->unweightedWhiten(
v);
static shared_ptr MixedVariances(const Vector &mu, const Vector &variances)
Vector unwhiten(const Vector &v) const override
Unwhiten an error vector.
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
static void fix(const Vector &sigmas, Vector &precisions, Vector &invsigmas)
Vector whiten(const Vector &v) const override
Whiten an error vector.
EIGEN_STRONG_INLINE Packet4f print(const Packet4f &a)
Annotation for function names.
EIGEN_DONT_INLINE void llt(const Mat &A, const Mat &B, Mat &C)
Expression of a fixed-size or dynamic-size block.
const EIGEN_DEVICE_FUNC InverseReturnType inverse() const
const RobustModel::shared_ptr & robust() const
Return the contained robust error function.
Matrix Whiten(const Matrix &H) const override
Array< double, 1, 3 > e(1./3., 0.5, 2.)
static shared_ptr MixedSigmas(const Vector &mu, const Vector &sigmas)
std::shared_ptr< Base > shared_ptr
Vector unweightedWhiten(const Vector &v) const override
static shared_ptr Precisions(const Vector &precisions, bool smart=true)
std::optional< size_t > check_if_constraint(VECTOR a, const Vector &invsigmas, size_t m)
static shared_ptr Create(const RobustModel::shared_ptr &robust, const NoiseModel::shared_ptr noise)
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 x
void print(const std::string &name) const override
const Vector & invsigmas() const
bool equal_with_abs_tol(const Eigen::DenseBase< MATRIX > &A, const Eigen::DenseBase< MATRIX > &B, double tol=1e-9)
void print(const std::string &name) const override
void print(const std::string &name) const override
static shared_ptr Variance(size_t dim, double variance, bool smart=true)
const EIGEN_DEVICE_FUNC LogReturnType log() const
static shared_ptr Sigmas(const Vector &sigmas, bool smart=true)
void updateAb(MATRIX &Ab, int j, const Vector &a, const Vector &rd)
void WhitenInPlace(Matrix &H) const override
size_t dim() const
Dimensionality.
Constrained(const Vector &mu, const Vector &sigmas)
virtual void WhitenSystem(Vector &b) const
std::shared_ptr< Gaussian > shared_ptr
Diagonal::shared_ptr QR(Matrix &Ab) const override
void diagonal(const MatrixType &m)
virtual double logDetR() const override
Compute the log of |R|. Used for computing log(|Σ|)
void whitenInPlace(Vector &v) const override
void print(const std::string &name) const override
static const double sigma
Matrix< SCALARA, Dynamic, Dynamic, opt_A > A
double squaredMahalanobisDistance(const Vector &v) const override
Squared Mahalanobis distance v'*R'*R*v = <R*v,R*v>
Vector sigmas() const override
Calculate standard deviations.
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
void vector_scale_inplace(const Vector &v, Matrix &A, bool inf_mask)
double precision(size_t i) const
const EIGEN_DEVICE_FUNC SquareReturnType square() const
static const Eigen::internal::all_t all
double logDeterminant(const typename BAYESTREE::sharedClique &clique)
static constexpr bool debug
static shared_ptr Create(size_t dim)
noiseModel::Diagonal::shared_ptr SharedDiagonal
virtual double logDetR() const override
Compute the log of |R|. Used for computing log(|Σ|)
double squaredMahalanobisDistance(const Vector &v) const override
Squared Mahalanobis distance v'*R'*R*v = <R*v,R*v>
Matrix Whiten(const Matrix &H) const override
Vector whiten(const Vector &v) const override
Calculates error vector with weights applied.
const RobustModel::shared_ptr robust_
robust error function used
double squaredMahalanobisDistance(const Vector &v) const override
double weight(const Vector &v) const override
std::optional< Vector > checkIfDiagonal(const Matrix &M)
const Vector & precisions() const
const NoiseModel::shared_ptr noise_
noise model used
Standard Cholesky decomposition (LL^T) of a matrix and associated features.
void inplace_QR(Matrix &A)
Vector whiten(const Vector &v) const override
Whiten an error vector.
Vector backSubstituteUpper(const Matrix &U, const Vector &b, bool unit)
void WhitenInPlace(Matrix &H) const override
Matrix vector_scale(const Vector &v, const Matrix &A, bool inf_mask)
virtual double logDetR() const override
Compute the log of |R|. Used for computing log(|Σ|)
ptrdiff_t DenseIndex
The index type for Eigen objects.
std::shared_ptr< Diagonal > shared_ptr
static noiseModel::Diagonal::shared_ptr Diagonal(const Matrix &covariance)
Matrix Whiten(const Matrix &H) const override
std::shared_ptr< Robust > shared_ptr
static shared_ptr MixedPrecisions(const Vector &mu, const Vector &precisions)
void print(const std::string &name) const override
Array< int, Dynamic, 1 > v
bool constrained(size_t i) const
Return true if a particular dimension is free or constrained.
static shared_ptr Sigma(size_t dim, double sigma, bool smart=true)
std::shared_ptr< Isotropic > shared_ptr
const NoiseModel::shared_ptr & noise() const
Return the contained noise model.
void WhitenInPlace(Matrix &H) const override
std::shared_ptr< Constrained > shared_ptr
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
Jet< T, N > sqrt(const Jet< T, N > &f)
bool equals(const Base &expected, double tol=1e-9) const override
Rot2 R(Rot2::fromAngle(0.1))
Vector unwhiten(const Vector &v) const override
Unwhiten an error vector.
static shared_ptr Variances(const Vector &variances, bool smart=true)
EIGEN_DEFAULT_DENSE_INDEX_TYPE Index
The Index type as used for the API.
Matrix< RealScalar, Dynamic, Dynamic > M
gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:33:50