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
ConvertNoiseModel(const SharedNoiseModel &model, size_t d, bool defaultToUnit) {
double sigma = 1.0;
bool exit = false;
if (model != nullptr) {
const auto &robust = boost::dynamic_pointer_cast<noiseModel::Robust>(model);
Vector sigmas;
if(robust){
if (robust) {
sigmas = robust->noise()->sigmas();
} else{
} else {
sigmas = model->sigmas();
}
size_t n = sigmas.size();
if (n == 1) {
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
if (sigmas(0) != sigma || sigmas(1) != sigma) {
if (!defaultToUnit) {
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");
}
}
exit:
auto isoModel = noiseModel::Isotropic::Sigma(d, sigma);
const auto &robust = boost::dynamic_pointer_cast<noiseModel::Robust>(model);
if(robust)
if (robust) {
return noiseModel::Robust::Create(
noiseModel::mEstimator::Huber::Create(1.345), isoModel);
else
} else {
return isoModel;
}
}
//******************************************************************************

17
gtsam/slam/FrobeniusFactor.h
View File

@ -26,13 +26,18 @@
namespace gtsam {
/**
* When creating (any) FrobeniusFactor we can convert a Rot/Pose
* BetweenFactor noise model into a n-dimensional isotropic noise
* model used to weight the Frobenius norm. If the noise model passed is
* null we return a n-dimensional isotropic noise model with sigma=1.0. If
* not, we we check if the d-dimensional noise model on rotations is
* When creating (any) FrobeniusFactor we can convert a Rot/Pose BetweenFactor
* noise model into a n-dimensional isotropic noise
* model used to weight the Frobenius norm.
* If the noise model passed is null we return a n-dimensional isotropic noise
* 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
* 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
ConvertNoiseModel(const SharedNoiseModel &model, size_t n,