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

detectMoreLoopClosures tool: regenerate global occupancy grid for convenience if there was one before

Browse Source
This commit is contained in:
matlabbe
2022-09-30 15:48:09 -07:00
parent 30bf5895ca
commit 9afbcf2d06
1 changed files with 29 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

29
tools/DetectMoreLoopClosures/main.cpp
View File

@@ -27,6 +27,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/core/DBDriver.h>
#include <rtabmap/core/Rtabmap.h>
#include <rtabmap/core/Memory.h>
#include <rtabmap/core/OccupancyGrid.h>
#include <rtabmap/core/util3d.h>
#include <rtabmap/core/util3d_filtering.h>
#include <rtabmap/core/util3d_transforms.h>
@@ -222,6 +224,9 @@ int main(int argc, char * argv[])
uInsert(parameters, inputParams);
rtabmap.init(parameters, dbPath);
float xMin, yMin, cellSize;
bool haveOptimizedMap = !rtabmap.getMemory()->load2DMap(xMin, yMin, cellSize).empty();
PrintProgressState progress;
printf("Detecting...\n");
int detected = rtabmap.detectMoreLoopClosures(clusterRadiusMax, clusterAngle, iterations, intraSession, interSession, &progress, clusterRadiusMin);
@@ -236,6 +241,30 @@ int main(int argc, char * argv[])
printf("Loop closure detection failed!\n");
}
}
else if(detected > 0 && haveOptimizedMap)
{
printf("The database has a global occupancy grid, regenerating one with new optimized graph!\n");
OccupancyGrid grid(parameters);
std::map<int, Transform> optimizedPoses = rtabmap.getLocalOptimizedPoses();
for(std::map<int, Transform>::iterator iter=optimizedPoses.lower_bound(0); iter!=optimizedPoses.end(); ++iter)
{
cv::Mat occupancyGrid;
SensorData data = rtabmap.getMemory()->getNodeData(iter->first, false, false, false, true);
data.uncompressData();
grid.addToCache(iter->first, data.gridGroundCellsRaw(), data.gridObstacleCellsRaw(), data.gridEmptyCellsRaw());
}
grid.update(optimizedPoses);
cv::Mat map = grid.getMap(xMin, yMin);
if(map.empty())
{
printf("Could not generate the global occupancy grid!\n");
}
else
{
rtabmap.getMemory()->save2DMap(map, xMin, yMin, cellSize);
printf("Save new global occupancy grid!\n");
}
}
rtabmap.close();