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

Remove goto, update docs, formatting

release/4.3a0
Varun Agrawal 2020-11-09 10:39:04 -05:00
parent c99cb14b49
commit 5762ba5ac8
2 changed files with 31 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
gtsam/slam/FrobeniusFactor.cpp
View File

@ -26,41 +26,44 @@ namespace gtsam {
SharedNoiseModel SharedNoiseModel
ConvertNoiseModel(const SharedNoiseModel &model, size_t d, bool defaultToUnit) { ConvertNoiseModel(const SharedNoiseModel &model, size_t d, bool defaultToUnit) {
double sigma = 1.0; double sigma = 1.0;
bool exit = false;
if (model != nullptr) { if (model != nullptr) {
const auto &robust = boost::dynamic_pointer_cast<noiseModel::Robust>(model); const auto &robust = boost::dynamic_pointer_cast<noiseModel::Robust>(model);
Vector sigmas; Vector sigmas;
if(robust){ if (robust) {
sigmas = robust->noise()->sigmas(); sigmas = robust->noise()->sigmas();
} else{ } else {
sigmas = model->sigmas(); sigmas = model->sigmas();
} }
size_t n = sigmas.size(); size_t n = sigmas.size();
if (n == 1) { if (n == 1) {
sigma = sigmas(0); // Rot2 sigma = sigmas(0); // Rot2
goto exit; exit = true;
} }
if (n == 3 || n == 6) { else if (n == 3 || n == 6) {
sigma = sigmas(2); // Pose2, Rot3, or Pose3 sigma = sigmas(2); // Pose2, Rot3, or Pose3
if (sigmas(0) != sigma || sigmas(1) != sigma) { if (sigmas(0) != sigma || sigmas(1) != sigma) {
if (!defaultToUnit) { if (!defaultToUnit) {
throw std::runtime_error("Can only convert isotropic rotation noise"); throw std::runtime_error("Can only convert isotropic rotation noise");
} }
} }
goto exit; exit = true;
} }
if (!defaultToUnit) { if (!defaultToUnit && !exit) {
throw std::runtime_error("Can only convert Pose2/Pose3 noise models"); throw std::runtime_error("Can only convert Pose2/Pose3 noise models");
} }
} }
exit:
auto isoModel = noiseModel::Isotropic::Sigma(d, sigma); auto isoModel = noiseModel::Isotropic::Sigma(d, sigma);
const auto &robust = boost::dynamic_pointer_cast<noiseModel::Robust>(model); const auto &robust = boost::dynamic_pointer_cast<noiseModel::Robust>(model);
if(robust) if (robust) {
return noiseModel::Robust::Create( return noiseModel::Robust::Create(
noiseModel::mEstimator::Huber::Create(1.345), isoModel); noiseModel::mEstimator::Huber::Create(1.345), isoModel);
else } else {
return isoModel; return isoModel;
}
} }
//****************************************************************************** //******************************************************************************

17
gtsam/slam/FrobeniusFactor.h
View File

@ -26,13 +26,18 @@
namespace gtsam { namespace gtsam {
/** /**
* When creating (any) FrobeniusFactor we can convert a Rot/Pose * When creating (any) FrobeniusFactor we can convert a Rot/Pose BetweenFactor
* BetweenFactor noise model into a n-dimensional isotropic noise * noise model into a n-dimensional isotropic noise
* model used to weight the Frobenius norm. If the noise model passed is * model used to weight the Frobenius norm.
* null we return a n-dimensional isotropic noise model with sigma=1.0. If * If the noise model passed is null we return a n-dimensional isotropic noise
* not, we we check if the d-dimensional noise model on rotations is * model with sigma=1.0.
* If not, we we check if the d-dimensional noise model on rotations is
* isotropic. If it is, we extend to 'n' dimensions, otherwise we throw an * isotropic. If it is, we extend to 'n' dimensions, otherwise we throw an
* error. If defaultToUnit == false throws an exception on unexepcted input. * error.
* If the noise model is a robust error model, we use the sigmas of the
* underlying noise model.
*
* If defaultToUnit == false throws an exception on unexepcted input.
*/ */
GTSAM_EXPORT SharedNoiseModel GTSAM_EXPORT SharedNoiseModel
ConvertNoiseModel(const SharedNoiseModel &model, size_t n, ConvertNoiseModel(const SharedNoiseModel &model, size_t n,