i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 17:40:23 +08:00
Code Issues Packages Projects Releases Wiki Activity

fixed #643

Browse Source
This commit is contained in:
matlabbe
2020-11-22 14:29:05 -05:00
parent bdc7be40b4
commit f88845e138
3 changed files with 18 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

14
corelib/src/optimizer/vertigo/gtsam/betweenFactorMaxMix.h
View File

@@ -10,6 +10,16 @@
#include <gtsam/linear/NoiseModel.h>
#include <Eigen/Eigen>
#include <gtsam/config.h>
#if GTSAM_VERSION_MAJOR > 4 || (GTSAM_VERSION_MAJOR==4 && GTSAM_VERSION_MINOR>=1)
namespace gtsam {
gtsam::Matrix inverse(const gtsam::Matrix & matrix)
{
return matrix.inverse();
}
}
#endif
namespace vertigo {
@@ -39,7 +49,11 @@ namespace vertigo {
double nu1 = 1.0/sqrt(gtsam::inverse(info1).determinant());
double l1 = nu1 * exp(-0.5*m1);
#if GTSAM_VERSION_MAJOR > 4 || (GTSAM_VERSION_MAJOR==4 && GTSAM_VERSION_MINOR>=1)
double m2 = nullHypothesisModel->squaredMahalanobisDistance(error);
#else
double m2 = nullHypothesisModel->distance(error);
#endif
gtsam::noiseModel::Gaussian::shared_ptr g2 = nullHypothesisModel;
gtsam::Matrix info2(g2->R().transpose()*g2->R());
double nu2 = 1.0/sqrt(gtsam::inverse(info2).determinant());

4
corelib/src/optimizer/vertigo/gtsam/switchVariableLinear.h
View File

@@ -78,8 +78,8 @@ namespace vertigo {
inline SwitchVariableLinear between(const SwitchVariableLinear& l2,
boost::optional<gtsam::Matrix&> H1=boost::none,
boost::optional<gtsam::Matrix&> H2=boost::none) const {
if(H1) *H1 = -gtsam::eye(1);
if(H2) *H2 = gtsam::eye(1);
if(H1) *H1 = -gtsam::Matrix::Identity(1, 1);
if(H2) *H2 = gtsam::Matrix::Identity(1, 1);
return SwitchVariableLinear(l2.value() - value());
}

4
corelib/src/optimizer/vertigo/gtsam/switchVariableSigmoid.h
View File

@@ -78,8 +78,8 @@ namespace vertigo {
inline SwitchVariableSigmoid between(const SwitchVariableSigmoid& l2,
boost::optional<gtsam::Matrix&> H1=boost::none,
boost::optional<gtsam::Matrix&> H2=boost::none) const {
if(H1) *H1 = -gtsam::eye(1);
if(H2) *H2 = gtsam::eye(1);
if(H1) *H1 = -gtsam::Matrix::Identity(1, 1);
if(H2) *H2 = gtsam::Matrix::Identity(1, 1);
return SwitchVariableSigmoid(l2.value() - value());
}