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

CCCorelib: updated wrong output check. Updated RGBD/MaxOdomCacheSize approach to work properly with Rtabmap/CreateIntermediateNodes=true.

Browse Source
This commit is contained in:
matlabbe
2022-01-28 14:48:32 -05:00
parent 02140f92db
commit 315350dbfc
2 changed files with 3 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
corelib/src/Rtabmap.cpp
View File

@@ -1579,7 +1579,7 @@ bool Rtabmap::process(
} }
// Localization mode stuff // Localization mode stuff
_lastLocalizationPose = newPose; // keep in cache the latest corrected pose _lastLocalizationPose = newPose; // keep in cache the latest corrected pose
if(!_memory->isIncremental()) if(!_memory->isIncremental() && signature->getWeight() >= 0)
{ {
UDEBUG("Update odometry localization cache (size=%d/%d)", (int)_odomCachePoses.size(), _maxOdomCacheSize); UDEBUG("Update odometry localization cache (size=%d/%d)", (int)_odomCachePoses.size(), _maxOdomCacheSize);
if(!_odomCachePoses.empty()) if(!_odomCachePoses.empty())

5
corelib/src/icp/cccorelib.h
View File

@@ -137,15 +137,14 @@ rtabmap::Transform icpCC(
icpTransformation.setNull(); icpTransformation.setNull();
return icpTransformation; return icpTransformation;
} }
else if(finalPointCount < 50) else if(!transform.R.isValid())
{ {
std::string msg = uFormat("CCCoreLib has failed: Rejecting transform as finalPointCount %d < 50 ", finalPointCount); std::string msg = uFormat("CCCoreLib has failed: Rotation matrix is invalid");
UDEBUG(msg.c_str()); UDEBUG(msg.c_str());
if(errorMsg) if(errorMsg)
{ {
*errorMsg = msg; *errorMsg = msg;
} }
icpTransformation.setNull(); icpTransformation.setNull();
return icpTransformation; return icpTransformation;
} }