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

add some comments for Constrained model

release/4.3a0
Duy-Nguyen Ta 2013-08-02 13:29:54 +00:00
parent a48864452b
commit ef99e1bf1a
1 changed files with 6 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
gtsam/linear/NoiseModel.cpp
View File

@ -253,6 +253,9 @@ void Constrained::print(const std::string& name) const {
/* ************************************************************************* */ /* ************************************************************************* */
Vector Constrained::whiten(const Vector& v) const { Vector Constrained::whiten(const Vector& v) const {
// If sigmas[i] is not 0 then divide v[i] by sigmas[i], as usually done in
// other normal Gaussian noise model. Otherwise, sigmas[i] = 0 indicating
// a hard constraint, we don't do anything.
const Vector& a = v; const Vector& a = v;
const Vector& b = sigmas_; const Vector& b = sigmas_;
size_t n = a.size(); size_t n = a.size();
@ -287,6 +290,9 @@ Matrix Constrained::Whiten(const Matrix& H) const {
/* ************************************************************************* */ /* ************************************************************************* */
void Constrained::WhitenInPlace(Matrix& H) const { void Constrained::WhitenInPlace(Matrix& H) const {
// selective scaling // selective scaling
// Scale row i of H by sigmas[i], basically multiplying H with diag(sigmas)
// Set inf_mask flag is true so that if invsigmas[i] is inf, i.e. sigmas[i] = 0,
// indicating a hard constraint, we leave H's row i in place.
vector_scale_inplace(invsigmas(), H, true); vector_scale_inplace(invsigmas(), H, true);
} }