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

Merge pull request #1842 from borglab/better-noisemodel-checking 

Non-loop versions for checking sigma values
release/4.3a0
Varun Agrawal 2024-09-25 16:38:02 -04:00 committed by GitHub
parent e4ec8d3b9c 9243655e3b
commit 8f474600e0
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
1 changed files with 17 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

37
gtsam/linear/NoiseModel.cpp
View File

@ -269,10 +269,7 @@ double Gaussian::negLogConstant() const {
/* ************************************************************************* */
// Diagonal
/* ************************************************************************* */
Diagonal::Diagonal() :
Gaussian(1) // TODO: Frank asks: really sure about this?
{
}
Diagonal::Diagonal() : Gaussian() {}
/* ************************************************************************* */
Diagonal::Diagonal(const Vector& sigmas)
@ -284,31 +281,30 @@ Diagonal::Diagonal(const Vector& sigmas)
/* ************************************************************************* */
Diagonal::shared_ptr Diagonal::Variances(const Vector& variances, bool smart) {
if (smart) {
// check whether all the same entry
size_t n = variances.size();
for (size_t j = 1; j < n; j++)
if (variances(j) != variances(0)) goto full;
return Isotropic::Variance(n, variances(0), true);
}
full: return shared_ptr(new Diagonal(variances.cwiseSqrt()));
// check whether all the same entry
return (smart && (variances.array() == variances(0)).all())
? Isotropic::Variance(variances.size(), variances(0), true)
: shared_ptr(new Diagonal(variances.cwiseSqrt()));
}
/* ************************************************************************* */
Diagonal::shared_ptr Diagonal::Sigmas(const Vector& sigmas, bool smart) {
if (smart) {
size_t n = sigmas.size();
if (n==0) goto full;
if (n == 0) goto full;
// look for zeros to make a constraint
for (size_t j=0; j< n; ++j)
if (sigmas(j)<1e-8)
return Constrained::MixedSigmas(sigmas);
if ((sigmas.array() < 1e-8).any()) {
return Constrained::MixedSigmas(sigmas);
}
// check whether all the same entry
for (size_t j = 1; j < n; j++)
if (sigmas(j) != sigmas(0)) goto full;
return Isotropic::Sigma(n, sigmas(0), true);
if ((sigmas.array() == sigmas(0)).all()) {
return Isotropic::Sigma(n, sigmas(0), true);
}
}
full: return Diagonal::shared_ptr(new Diagonal(sigmas));
full:
return Diagonal::shared_ptr(new Diagonal(sigmas));
}
/* ************************************************************************* */
@ -316,6 +312,7 @@ Diagonal::shared_ptr Diagonal::Precisions(const Vector& precisions,
bool smart) {
return Variances(precisions.array().inverse(), smart);
}
/* ************************************************************************* */
void Diagonal::print(const string& name) const {
gtsam::print(sigmas_, name + "diagonal sigmas ");