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

Whiten now class-specific, access to sigmas

release/4.3a0
Frank Dellaert 2010-01-19 04:38:58 +00:00
parent ebbdfc79aa
commit 81aaceb890
2 changed files with 47 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
cpp/NoiseModel.cpp
View File

@ -28,7 +28,7 @@ namespace gtsam {
bool Gaussian::equals(const Base& m, double tol) const {
const Gaussian* p = dynamic_cast<const Gaussian*> (&m);
if (p == NULL) return false;
return equal_with_abs_tol(sqrt_information_, p->sqrt_information_, tol);
return equal_with_abs_tol(sqrt_information_, p->sqrt_information_, sqrt(tol));
}
Vector Gaussian::whiten(const Vector& v) const {
@ -45,27 +45,11 @@ namespace gtsam {
return inner_prod(w, w);
}
// functional
Matrix Gaussian::Whiten(const Matrix& H) const {
// size_t m = H.size1(), n = H.size2();
// Matrix W(m, n);
// for (int j = 0; j < n; j++) {
// Vector wj = whiten(column(H, j));
// for (int i = 0; i < m; i++)
// W(i, j) = wj(i);
// }
// return W;
return sqrt_information_ * H;
}
// in place
void Gaussian::WhitenInPlace(Matrix& H) const {
// size_t m = H.size1(), n = H.size2();
// for (int j = 0; j < n; j++) {
// Vector wj = whiten(column(H, j));
// for (int i = 0; i < m; i++)
// H(i, j) = wj(i);
// }
H = sqrt_information_ * H;
}
@ -88,6 +72,14 @@ namespace gtsam {
return emul(v, sigmas_);
}
Matrix Diagonal::Whiten(const Matrix& H) const {
return vector_scale(invsigmas_, H);
}
void Diagonal::WhitenInPlace(Matrix& H) const {
H = vector_scale(invsigmas_, H);
}
/* ************************************************************************* */
void Constrained::print(const std::string& name) const {
@ -126,6 +118,14 @@ namespace gtsam {
return v * sigma_;
}
Matrix Isotropic::Whiten(const Matrix& H) const {
return invsigma_ * H;
}
void Isotropic::WhitenInPlace(Matrix& H) const {
H *= invsigma_;
}
/* ************************************************************************* */
void Unit::print(const std::string& name) const {
cout << "Unit " << name << endl;

34
cpp/NoiseModel.h
View File

@ -51,8 +51,7 @@ namespace gtsam { namespace noiseModel {
virtual void unwhitenInPlace(Vector& v) const {
v = unwhiten(v);
}
};
};
/**
* Gaussian implements the mathematical model
@ -123,6 +122,14 @@ namespace gtsam { namespace noiseModel {
*/
virtual void WhitenInPlace(Matrix& H) const;
/**
* Whiten a system, in place as well
*/
inline void WhitenSystem(Matrix& A, Vector& b) const {
WhitenInPlace(A);
whitenInPlace(b);
}
/**
* Return R itself, but note that Whiten(H) is cheaper than R*H
*/
@ -180,7 +187,16 @@ namespace gtsam { namespace noiseModel {
virtual void print(const std::string& name) const;
virtual Vector whiten(const Vector& v) const;
virtual Vector unwhiten(const Vector& v) const;
};
virtual Matrix Whiten(const Matrix& H) const;
virtual void WhitenInPlace(Matrix& H) const;
/**
* Return standard deviations (sqrt of diagonal)
*/
inline const Vector& sigmas() const { return sigmas_; }
inline double sigma(size_t i) const { return sigmas_(i); }
}; // Diagonal
/**
* A Constrained constrained model is a specialization of Diagonal which allows
@ -223,7 +239,8 @@ namespace gtsam { namespace noiseModel {
virtual Matrix Whiten(const Matrix& H) const;
virtual void WhitenInPlace(Matrix& H) const;
};
}; // Constrained
/**
* An isotropic noise model corresponds to a scaled diagonal covariance
@ -266,6 +283,13 @@ namespace gtsam { namespace noiseModel {
virtual double Mahalanobis(const Vector& v) const;
virtual Vector whiten(const Vector& v) const;
virtual Vector unwhiten(const Vector& v) const;
virtual Matrix Whiten(const Matrix& H) const;
virtual void WhitenInPlace(Matrix& H) const;
/**
* Return standard deviation
*/
inline double sigma() const { return sigma_; }
};
/**
@ -316,5 +340,7 @@ namespace gtsam { namespace noiseModel {
#endif
};
typedef Diagonal::shared_ptr sharedDiagonal;
} // namespace gtsam