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

Auto graph repairing (#1691)

Browse Source 
* Auto graph repairing: detect and remove bad loop closures accepted in the past that block new good loop closures to be accepted

* Added parameter RGBD/OptimizeMaxErrorRepairRadius: added approach to repair the graph (remove bad loop closures added in the past). Refactored max optimization error function, split repairGraph to its own function.

* Handling optimization from graph end in repair

* MaxGraphErrors: changed link's pointers to copy directly to avoid seg fault down the road if not used properly

* Dont publish optimization statistics if it was not computed.
This commit is contained in:
matlabbe
2026-05-04 16:02:55 -07:00
committed by GitHub
parent 0cf2e45c81
commit 37f8fa63c5
11 changed files with 746 additions and 554 deletions
Download Patch File Download Diff File Expand all files Collapse all files

50
corelib/src/Graph.cpp
View File

@@ -927,21 +927,12 @@ Transform calcRMSE (
return t;
}
void computeMaxGraphErrors(
MaxGraphErrors computeMaxGraphErrors(
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & links,
float & maxLinearErrorRatio,
float & maxAngularErrorRatio,
float & maxLinearError,
float & maxAngularError,
const Link ** maxLinearErrorLink,
const Link ** maxAngularErrorLink,
bool force3DoF)
{
maxLinearErrorRatio = -1;
maxAngularErrorRatio = -1;
maxLinearError = -1;
maxAngularError = -1;
MaxGraphErrors maxError;
UDEBUG("poses=%d links=%d", (int)poses.size(), (int)links.size());
for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
@@ -963,19 +954,7 @@ void computeMaxGraphErrors(
iter->second.to(),
t2.prettyPrint().c_str());
if(maxLinearErrorLink)
{
*maxLinearErrorLink = 0;
}
if(maxAngularErrorLink)
{
*maxAngularErrorLink = 0;
}
maxLinearErrorRatio = -1;
maxAngularErrorRatio = -1;
maxLinearError = -1;
maxAngularError = -1;
return;
return MaxGraphErrors();
}
Transform t;
@@ -999,14 +978,11 @@ void computeMaxGraphErrors(
UASSERT(iter->second.transVariance(false)>0.0);
float stddevLinear = sqrt(iter->second.transVariance(false));
float linearErrorRatio = linearError/stddevLinear;
if(linearErrorRatio > maxLinearErrorRatio)
if(linearErrorRatio > maxError.linearRatio)
{
maxLinearError = linearError;
maxLinearErrorRatio = linearErrorRatio;
if(maxLinearErrorLink)
{
*maxLinearErrorLink = &iter->second;
}
maxError.linear = linearError;
maxError.linearRatio = linearErrorRatio;
maxError.linearLink = iter->second;
}
// For landmark links, don't compute angular error if it doesn't estimate orientation
@@ -1031,18 +1007,16 @@ void computeMaxGraphErrors(
UASSERT(iter->second.rotVariance(false)>0.0);
float stddevAngular = sqrt(iter->second.rotVariance(false));
float angularErrorRatio = angularError/stddevAngular;
if(angularErrorRatio > maxAngularErrorRatio)
if(angularErrorRatio > maxError.angularRatio)
{
maxAngularError = angularError;
maxAngularErrorRatio = angularErrorRatio;
if(maxAngularErrorLink)
{
*maxAngularErrorLink = &iter->second;
}
maxError.angular = angularError;
maxError.angularRatio = angularErrorRatio;
maxError.angularLink = iter->second;
}
}
}
}
return maxError;
}
std::vector<double> getMaxOdomInf(const std::multimap<int, Link> & links)

821
corelib/src/Rtabmap.cpp
View File

File diff suppressed because it is too large Load Diff

4
corelib/src/global_map/OccupancyGrid.cpp
View File

@@ -222,14 +222,14 @@ void OccupancyGrid::assemble(const std::list<std::pair<int, Transform> > & newPo
if(!cache().empty())
{
UDEBUG("Updating from cache");
UDEBUG("Updating %ld poses from cache", newPoses.size());
for(std::list<std::pair<int, Transform> >::const_iterator iter = newPoses.begin(); iter!=newPoses.end(); ++iter)
{
if(uContains(cache(), iter->first))
{
const LocalGrid & localGrid = cache().at(iter->first);
UDEBUG("Adding grid %d: ground=%d obstacles=%d empty=%d", iter->first, localGrid.groundCells.cols, localGrid.obstacleCells.cols, localGrid.emptyCells.cols);
//UDEBUG("Adding grid %d: ground=%d obstacles=%d empty=%d", iter->first, localGrid.groundCells.cols, localGrid.obstacleCells.cols, localGrid.emptyCells.cols);
//ground
cv::Mat ground;