12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Added Whiten

release/4.3a0
Frank Dellaert 2015-06-10 18:27:10 -04:00
parent 39ffe3ac32
commit 7ec26ff323
2 changed files with 10 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
gtsam/linear/NoiseModel.cpp
View File

@ -376,8 +376,11 @@ double Constrained::distance(const Vector& v) const {
/* ************************************************************************* */ /* ************************************************************************* */
Matrix Constrained::Whiten(const Matrix& H) const { Matrix Constrained::Whiten(const Matrix& H) const {
// selective scaling Matrix A = H;
return vector_scale(invsigmas(), H, true); for (DenseIndex i=0; i<(DenseIndex)dim_; ++i)
if (!constrained(i)) // if constrained, leave row of A as is
A.row(i) *= invsigmas_(i);
return A;
} }
/* ************************************************************************* */ /* ************************************************************************* */

5
gtsam/linear/NoiseModel.h
View File

@ -81,6 +81,9 @@ namespace gtsam {
/// Whiten an error vector. /// Whiten an error vector.
virtual Vector whiten(const Vector& v) const = 0; virtual Vector whiten(const Vector& v) const = 0;
/// Whiten a matrix.
virtual Matrix Whiten(const Matrix& H) const = 0;
/// Unwhiten an error vector. /// Unwhiten an error vector.
virtual Vector unwhiten(const Vector& v) const = 0; virtual Vector unwhiten(const Vector& v) const = 0;
@ -856,6 +859,8 @@ namespace gtsam {
// TODO: functions below are dummy but necessary for the noiseModel::Base // TODO: functions below are dummy but necessary for the noiseModel::Base
inline virtual Vector whiten(const Vector& v) const inline virtual Vector whiten(const Vector& v) const
{ Vector r = v; this->WhitenSystem(r); return r; } { Vector r = v; this->WhitenSystem(r); return r; }
inline virtual Matrix Whiten(const Matrix& A) const
{ Vector b; Matrix B=A; this->WhitenSystem(B,b); return B; }
inline virtual Vector unwhiten(const Vector& /*v*/) const inline virtual Vector unwhiten(const Vector& /*v*/) const
{ throw std::invalid_argument("unwhiten is not currently supported for robust noise models."); } { throw std::invalid_argument("unwhiten is not currently supported for robust noise models."); }
inline virtual double distance(const Vector& v) const inline virtual double distance(const Vector& v) const