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

Formatting only

release/4.3a0
dellaert 2014-05-31 22:10:51 -04:00
parent f32968cc03
commit 8f493d6ee5
1 changed files with 18 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
gtsam/linear/NoiseModel.cpp
View File

@ -74,22 +74,27 @@ boost::optional<Vector> checkIfDiagonal(const Matrix M) {
/* ************************************************************************* */ /* ************************************************************************* */
Gaussian::shared_ptr Gaussian::SqrtInformation(const Matrix& R, bool smart) { Gaussian::shared_ptr Gaussian::SqrtInformation(const Matrix& R, bool smart) {
size_t m = R.rows(), n = R.cols(); size_t m = R.rows(), n = R.cols();
if (m != n) throw invalid_argument("Gaussian::SqrtInformation: R not square"); if (m != n)
throw invalid_argument("Gaussian::SqrtInformation: R not square");
boost::optional<Vector> diagonal = boost::none; boost::optional<Vector> diagonal = boost::none;
if (smart) if (smart)
diagonal = checkIfDiagonal(R); diagonal = checkIfDiagonal(R);
if (diagonal) return Diagonal::Sigmas(reciprocal(*diagonal),true); if (diagonal)
else return shared_ptr(new Gaussian(R.rows(),R)); return Diagonal::Sigmas(reciprocal(*diagonal), true);
else
return shared_ptr(new Gaussian(R.rows(), R));
} }
/* ************************************************************************* */ /* ************************************************************************* */
Gaussian::shared_ptr Gaussian::Information(const Matrix& M, bool smart) { Gaussian::shared_ptr Gaussian::Information(const Matrix& M, bool smart) {
size_t m = M.rows(), n = M.cols(); size_t m = M.rows(), n = M.cols();
if (m != n) throw invalid_argument("Gaussian::Information: R not square"); if (m != n)
throw invalid_argument("Gaussian::Information: R not square");
boost::optional<Vector> diagonal = boost::none; boost::optional<Vector> diagonal = boost::none;
if (smart) if (smart)
diagonal = checkIfDiagonal(M); diagonal = checkIfDiagonal(M);
if (diagonal) return Diagonal::Precisions(*diagonal, true); if (diagonal)
return Diagonal::Precisions(*diagonal, true);
else { else {
Matrix R = RtR(M); Matrix R = RtR(M);
return shared_ptr(new Gaussian(R.rows(), R)); return shared_ptr(new Gaussian(R.rows(), R));
@ -97,14 +102,18 @@ Gaussian::shared_ptr Gaussian::Information(const Matrix& M, bool smart) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
Gaussian::shared_ptr Gaussian::Covariance(const Matrix& covariance, bool smart) { Gaussian::shared_ptr Gaussian::Covariance(const Matrix& covariance,
bool smart) {
size_t m = covariance.rows(), n = covariance.cols(); size_t m = covariance.rows(), n = covariance.cols();
if (m != n) throw invalid_argument("Gaussian::Covariance: covariance not square"); if (m != n)
throw invalid_argument("Gaussian::Covariance: covariance not square");
boost::optional<Vector> variances = boost::none; boost::optional<Vector> variances = boost::none;
if (smart) if (smart)
variances = checkIfDiagonal(covariance); variances = checkIfDiagonal(covariance);
if (variances) return Diagonal::Variances(*variances,true); if (variances)
else return shared_ptr(new Gaussian(n, inverse_square_root(covariance))); return Diagonal::Variances(*variances, true);
else
return shared_ptr(new Gaussian(n, inverse_square_root(covariance)));
} }
/* ************************************************************************* */ /* ************************************************************************* */