PointCloud generation with a masked image

[Seemon]

Hey,

I'm using the ROS version of rtabmap and I'm struggling to localize where the pointcloud is being generated.

The problem I'm trying to solve is that I want to keep the original camera rgb images for localization purpose but I want to generate the point cloud using a different (masked) image. Could you point me in a right direction on where the PointCloud generation happens.

I already tried the point_cloud_xyzrgb nodelet but this is not really what I need as it simply generates the current pointcloud.

Kind regards,
Szymon

[matlabbe]

Hi Szymon,

If you are referring to cloud_map topic (>=Kinetic version), the cloud is generated from local occupancy grids saved in graph's nodes. The grids are generated from here. The cloud is actually created here. To get the cloud of the node masked, you can filter the depth image prior to send it to rtabmap node.

To have more control of the 3D reconstruction from RGB/D data. You may subscribe to MapData topic and reconstruct the point cloud on your side. This will be similar to MapCloudDisplay plugin in RVIZ or the point cloud shown in rtabmapviz. You may look at the MapCloudDisplay code to see how the map is regenerated from raw RGB-D data.

cheers,
Mathieu

[Seemon]

Hey Mathieu,

Thank you for your response.

Before I read your message I tried to edit the map_assembler (MapAssemblerNode.cpp) node and at first it seemed pretty straight forward to mask the pointcloud there.

In mapDataRecievedCallback I simply replace the Image in in the node that is inserted to nodes_ (by uInsert). Unfortunately the pointcloud I get is still unmasked. Do you have any idea what am I missing here?

void mapDataReceivedCallback(const rtabmap_ros::MapDataConstPtr & msg)
	{
		[...]

		for(unsigned int i=0; i<msg->nodes.size(); ++i)
		{
			if(msg->nodes[i].image.size() ||
			   msg->nodes[i].depth.size() ||
			   msg->nodes[i].laserScan.size())
			{
				/// Replace the image in msg->nodes[i] - create replaced_node; 
				uInsert(nodes_, std::make_pair(msg->nodes[i].id, rtabmap_ros::nodeDataFromROS(replaced_node)));
			}
		}
		
		[...]
	}

Kind regards, Szymon

[matlabbe]

Hi Szymon,

Yeah you could replace the depth image there. MapAssembler uses MapsManager class to generate the maps. If the node data has local occupancy grid set, it will generate the cloud from that. If not set, it will regenerate the cloud from the RGB and Depth images (using "Grid/" parameters). Either clear the grid info in the node data (set grid_cell_size to 0 to force MapsManager to regenerate the local grids from current RGB-D data) or set Mem/CreateOccupancyGrid RGBD/CreateOccupancyGrid to false in rtabmap parameters so that occupancy grids are not created at the first place.

cheers,/
Mathieu