Creating an OctoMap and a 2D Occupancy Grid from RGBD in C++

[camasmie]

I am implementing rtabmap in our robot. I am able to run the standalone c++ "rgbd_mapping" example. (we don't use ROS).

However, the example saves a .pcl or .ply pointcloud file. Instead we need an OctoMap and a 2D occupancy grid so that we can run a global planner on it. I could not find a way to import .pcl or .ply into Octomap.

I looked in your GUI implemenation and in your libraries and noticed the following functions in "rtabmap/core/OctoMap.h":

"writeBinary" that saves the OctoMap

"createProjection" that creates the 2D grid

Are these the correct functions? Is so, do you have a recommendation of how to implement them in "rgbd_mapping" example, or is there a better way to get the OctoMap and 2D occupancy grid?

Thank you for your work!

[matlabbe]

Yes, those classes/functions could work. Follow those options in rtabmap-reprocess:

rtabmap/tools/Reprocess/main.cpp

Lines 84 to 85 in 860c14e

	" -o2 Assemble OctoMap 2D projection and save it to \"[output]_octomap.pgm\". Use with -db to save in database.\n"
	" -o3 Assemble OctoMap 3D cloud and save it to \"[output]_octomap.pcd\".\n"

This will give you an example how to generate a 3D OctoMap in C++.

OctoMap octomap;

// in the main loop after adding a new node (after `rtabmap.process()`):
const rtabmap::Statistics & stats = rtabmap.getStatistics();
const cv::Point3f & viewpoint = stats.getLastSignatureData().sensorData().gridViewPoint();
cv::Mat ground, obstacles, empty;
stats.getLastSignatureData().sensorData().uncompressDataConst(0, 0, 0, 0, &ground, &obstacles, &empty);
octomap.addToCache(id, ground, obstacles, empty, viewpoint);
octomap.update(stats.poses());

// after the main loop:
float xMin,yMin,cellSize;
cv::Mat map = octomap.createProjectionMap(xMin, yMin, cellSize);

octomap.writeBinary(...)