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) {
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;
if (smart)
diagonal = checkIfDiagonal(R);
if (diagonal) return Diagonal::Sigmas(reciprocal(*diagonal),true);
else return shared_ptr(new Gaussian(R.rows(),R));
if (diagonal)
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) {
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;
if (smart)
diagonal = checkIfDiagonal(M);
if (diagonal) return Diagonal::Precisions(*diagonal, true);
if (diagonal)
return Diagonal::Precisions(*diagonal, true);
else {
Matrix R = RtR(M);
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();
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;
if (smart)
variances = checkIfDiagonal(covariance);
if (variances) return Diagonal::Variances(*variances,true);
else return shared_ptr(new Gaussian(n, inverse_square_root(covariance)));
if (variances)
return Diagonal::Variances(*variances, true);
else
return shared_ptr(new Gaussian(n, inverse_square_root(covariance)));
}
/* ************************************************************************* */