Store full PC2 in Rtabmap Database

[Nosille]

While I understand that there are significant computational and storage efficiencies associated with the built-in pcl datatypes (e.g., pcl::PointCloudpcl::PointXYZRGBNormal), I find these datatypes very limiting. I am a ROS user, and I frequently find myself wishing RTABMAP would simply store the full original PointCloud2 object in the database. As a result, I have been working on a modified version of RTABMAP that adds three new database entries, pc2_info, pc2_fields, and pc2_data. These entries are simply a breakdown of a PCLPointCloud2 objects into blobs that can be stored in a sql database. I am close to having a usable product and would love the official RTABMAP team's advice and feedback moving forward. My hope is that this approach will provide a lot of flexibility for data storage. It many cases, I believe it will provide a very straight forward path for capturing user generated data.

https://github.com/Nosille/rtabmap/tree/AddPc2ToDatabase
https://github.com/Nosille/rtabmap_ros/tree/AddPc2ToDatabase

I am currently creating the scan and scan_info blobs consistent with the main branch and simultaneously populating the pc2_info, pc2_fields, and pc2_data blobs with the full original PCLPointCloud2 object. While this means that basic pointcloud information is stored twice right now, I believe the efficiency of pcl::PointCloud objects for common operations justifies keeping the scan/scan_info blobs, and I wasn't ready to couple the scan and pc2 database entries in a way that would allow the pc2 data to exist without the x, y, z fields. Hopefully, it is something that can be improved in the future.

[matlabbe]

You may have succeeded to do something similar by extending formats supported by LaserScan object (for the format missing of your PointCloud2):

rtabmap/corelib/include/rtabmap/core/LaserScan.h

Lines 40 to 51 in f8b1d50

	enum Format{kUnknown=0,
	kXY=1,
	kXYI=2,
	kXYNormal=3,
	kXYINormal=4,
	kXYZ=5,
	kXYZI=6,
	kXYZRGB=7,
	kXYZNormal=8,
	kXYZINormal=9,
	kXYZRGBNormal=10,
	kXYZIT=11};

then create a new global map object similar to CloudMap (assembling LocalGrid) that could be named like LaserScanCloudMap using LaserScan member of each SensorData, in order to assemble the output cloud on ROS side (in same format than your input PointCloud2 format).

For PointXYZRGBNormal format, are you referring to ExportCloudsDialog? I agree that we cannot export the laser scans in exact format than the one in LaserScan object (even if we add for compatible formats), that class would need to be refactored. Maybe by reusing the new LaserScanCloudMap class. ExportCloudsDialog supports many options (with/without normals, with/without colored RGB points or intensity, ...), which make difficult to use PCLPointCloud2. Note that in case of lidar, PointXYZRGBNormal is converted to PointXYZINormal before saving, so we can recover Intensity in the output file.

rtabmap/guilib/src/ExportCloudsDialog.cpp

Lines 4298 to 4303 in f8b1d50

	int * intensity = (int *)&cloudIWithNormals->points[i].intensity;
	*intensity =
	int(transformedCloud->points[i].r) |
	int(transformedCloud->points[i].g) << 8 |
	int(transformedCloud->points[i].b) << 16 |
	int(transformedCloud->points[i].a) << 24;

Another thing is that over the years, LaserScan looks more and more to a PCLPointCloud2::Ptr object (which was not the case at the beginning where only 2D scans were supported). Changing cv::Mat format used inside LaserScan to PCLPointCloud2::Ptr format could be a more sustainable solution, though a large refactoring task.