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

Localization with Reg/Force3DoF=true, fixed how graph optimization errors are computed (use only x,y,yaw).

Browse Source
This commit is contained in:
matlabbe
2022-01-12 16:59:03 -05:00
parent fcec98105d
commit 12c2dd707c
4 changed files with 12 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
corelib/include/rtabmap/core/Graph.h
View File

@@ -125,7 +125,8 @@ void RTABMAP_EXP computeMaxGraphErrors(
float & maxLinearError, float & maxLinearError,
float & maxAngularError, float & maxAngularError,
const Link ** maxLinearErrorLink = 0, const Link ** maxLinearErrorLink = 0,
const Link ** maxAngularErrorLink = 0); const Link ** maxAngularErrorLink = 0,
bool for3DoF = false);
std::vector<double> RTABMAP_EXP getMaxOdomInf(const std::multimap<int, Link> & links); std::vector<double> RTABMAP_EXP getMaxOdomInf(const std::multimap<int, Link> & links);

9
corelib/src/Graph.cpp
View File

@@ -898,7 +898,8 @@ void computeMaxGraphErrors(
float & maxLinearError, float & maxLinearError,
float & maxAngularError, float & maxAngularError,
const Link ** maxLinearErrorLink, const Link ** maxLinearErrorLink,
const Link ** maxAngularErrorLink) const Link ** maxAngularErrorLink,
bool for3DoF)
{ {
maxLinearErrorRatio = -1; maxLinearErrorRatio = -1;
maxAngularErrorRatio = -1; maxAngularErrorRatio = -1;
@@ -918,7 +919,7 @@ void computeMaxGraphErrors(
float linearError = uMax3( float linearError = uMax3(
fabs(iter->second.transform().x() - t.x()), fabs(iter->second.transform().x() - t.x()),
fabs(iter->second.transform().y() - t.y()), fabs(iter->second.transform().y() - t.y()),
fabs(iter->second.transform().z() - t.z())); for3DoF?0:fabs(iter->second.transform().z() - t.z()));
UASSERT(iter->second.transVariance(false)>0.0); UASSERT(iter->second.transVariance(false)>0.0);
float stddevLinear = sqrt(iter->second.transVariance(false)); float stddevLinear = sqrt(iter->second.transVariance(false));
float linearErrorRatio = linearError/stddevLinear; float linearErrorRatio = linearError/stddevLinear;
@@ -937,8 +938,8 @@ void computeMaxGraphErrors(
t.getEulerAngles(opt_roll, opt_pitch, opt_yaw); t.getEulerAngles(opt_roll, opt_pitch, opt_yaw);
iter->second.transform().getEulerAngles(link_roll, link_pitch, link_yaw); iter->second.transform().getEulerAngles(link_roll, link_pitch, link_yaw);
float angularError = uMax3( float angularError = uMax3(
fabs(opt_roll - link_roll), for3DoF?0:fabs(opt_roll - link_roll),
fabs(opt_pitch - link_pitch), for3DoF?0:fabs(opt_pitch - link_pitch),
fabs(opt_yaw - link_yaw)); fabs(opt_yaw - link_yaw));
angularError = angularError>M_PI?2*M_PI-angularError:angularError; angularError = angularError>M_PI?2*M_PI-angularError:angularError;
UASSERT(iter->second.rotVariance(false)>0.0); UASSERT(iter->second.rotVariance(false)>0.0);

1
corelib/src/RegistrationIcp.cpp
View File

@@ -329,6 +329,7 @@ Transform RegistrationIcp::computeTransformationImpl(
UDEBUG("Max translation=%f", _maxTranslation); UDEBUG("Max translation=%f", _maxTranslation);
UDEBUG("Max rotation=%f", _maxRotation); UDEBUG("Max rotation=%f", _maxRotation);
UDEBUG("Downsampling step=%d", _downsamplingStep); UDEBUG("Downsampling step=%d", _downsamplingStep);
UDEBUG("Force 3DoF=%s", this->force3DoF()?"true":"false");
UDEBUG("Force 4DoF=%s", _force4DoF?"true":"false"); UDEBUG("Force 4DoF=%s", _force4DoF?"true":"false");
UDEBUG("Min Complexity=%f", _pointToPlaneMinComplexity); UDEBUG("Min Complexity=%f", _pointToPlaneMinComplexity);
UDEBUG("libpointmatcher (knn=%d, outlier ratio=%f)", _libpointmatcherKnn, _outlierRatio); UDEBUG("libpointmatcher (knn=%d, outlier ratio=%f)", _libpointmatcherKnn, _outlierRatio);

6
corelib/src/Rtabmap.cpp
View File

@@ -2957,7 +2957,8 @@ bool Rtabmap::process(
maxLinearError, maxLinearError,
maxAngularError, maxAngularError,
&maxLinearLink, &maxLinearLink,
&maxAngularLink); &maxAngularLink,
_graphOptimizer->isSlam2d());
if(maxLinearLink == 0 && maxAngularLink==0 && _maxOdomCacheSize>0) if(maxLinearLink == 0 && maxAngularLink==0 && _maxOdomCacheSize>0)
{ {
UWARN("Could not compute graph errors! Wrong loop closures could be accepted!"); UWARN("Could not compute graph errors! Wrong loop closures could be accepted!");
@@ -5568,7 +5569,8 @@ bool Rtabmap::addLink(const Link & link)
maxLinearError, maxLinearError,
maxAngularError, maxAngularError,
&maxLinearLink, &maxLinearLink,
&maxAngularLink); &maxAngularLink,
_graphOptimizer->isSlam2d());
if(maxLinearLink == 0 && maxAngularLink==0) if(maxLinearLink == 0 && maxAngularLink==0)
{ {
UWARN("Could not compute graph errors! Wrong loop closures could be accepted!"); UWARN("Could not compute graph errors! Wrong loop closures could be accepted!");