Updated RGB-D mapping C++ example

examples/RGBDMapping/main.cpp

@@ -31,9 +31,12 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/CameraRGBD.h"
 #include "rtabmap/core/CameraThread.h"
 #include "rtabmap/core/OdometryThread.h"
+#include "rtabmap/core/Graph.h"
 #include "rtabmap/utilite/UEventsManager.h"
 #include <QApplication>
 #include <stdio.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl/io/ply_io.h>
 
 #include "MapBuilder.h"
 
@@ -167,5 +170,51 @@ int main(int argc, char * argv[])
 	odomThread.join(true);
 	rtabmapThread.join(true);
 
+	// Save 3D map
+	printf("Saving rtabmap_cloud.pcd...\n");
+	std::map<int, Signature> nodes;
+	std::map<int, Transform> optimizedPoses;
+	std::multimap<int, Link> links;
+	rtabmap->get3DMap(nodes, optimizedPoses, links, true, true);
+	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
+	for(std::map<int, Transform>::iterator iter=optimizedPoses.begin(); iter!=optimizedPoses.end(); ++iter)
+	{
+		Signature node = nodes.find(iter->first)->second;
+
+		// uncompress data
+		node.sensorData().uncompressData();
+
+		pcl::PointCloud<pcl::PointXYZRGB>::Ptr tmp = util3d::cloudRGBFromSensorData(
+				node.sensorData(),
+				4,           // image decimation before creating the clouds
+				4.0f,        // maximum depth of the cloud
+				0.01f);  // Voxel grid filtering
+		*cloud += *util3d::transformPointCloud(tmp, iter->second); // transform the point cloud to its pose
+	}
+	if(cloud->size())
+	{
+		printf("Voxel grid filtering of the assembled cloud (voxel=%f, %d points)\n", 0.01f, (int)cloud->size());
+		cloud = util3d::voxelize(cloud, 0.01f);
+
+		printf("Saving rtabmap_cloud.pcd... done! (%d points)\n", (int)cloud->size());
+		pcl::io::savePCDFile("rtabmap_cloud.pcd", *cloud);
+		//pcl::io::savePLYFile("rtabmap_cloud.ply", *cloud); // to save in PLY format
+	}
+	else
+	{
+		printf("Saving rtabmap_cloud.pcd... failed! The cloud is empty.\n");
+	}
+
+	// Save trajectory
+	printf("Saving rtabmap_trajectory.txt ...\n");
+	if(optimizedPoses.size() && graph::exportPoses("rtabmap_trajectory.txt", 0, optimizedPoses, links))
+	{
+		printf("Saving rtabmap_trajectory.txt... done!\n");
+	}
+	else
+	{
+		printf("Saving rtabmap_trajectory.txt... failed!\n");
+	}
+
 	return 0;
 }