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

non-loop versions for checking sigma values

release/4.3a0
Varun Agrawal 2024-09-23 16:05:23 -04:00
parent 788b074ac0
commit 6d3bc21a18
1 changed files with 18 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

35
gtsam/linear/NoiseModel.cpp
View File

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