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

31
gtsam/linear/NoiseModel.cpp
View File

@ -270,10 +270,7 @@ double Gaussian::logNormalizationConstant() const {
/* ************************************************************************* */
// Diagonal
/* ************************************************************************* */
Diagonal::Diagonal() :
Gaussian(1) // TODO: Frank asks: really sure about this?
{
}
Diagonal::Diagonal() : Gaussian() {}
/* ************************************************************************* */
Diagonal::Diagonal(const Vector& sigmas)
@ -287,29 +284,32 @@ 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);
if ((variances.array() == variances(0)).all()) {
return Isotropic::Variance(variances.size(), variances(0), true);
} else {
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) {
if (smart) {
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
for (size_t j=0; j< n; ++j)
if (sigmas(j)<1e-8)
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;
if ((sigmas.array() == sigmas(0)).all()) {
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) {
return Variances(precisions.array().inverse(), smart);
}
/* ************************************************************************* */
void Diagonal::print(const string& name) const {
gtsam::print(sigmas_, name + "diagonal sigmas ");