Coloring scan (camera projection on point cloud) (#693)

* ExportClouds: Added camera projection options * ExportClouds: fixed ceiling/floor filtering options not saved in config * ExportClouds: fixed colorless scan points still exported when option is unchecked. * Export tool: added --bin, --poses, --images, --las and --cam_projection options; export with intensity with --scan option. PDALWriter: added binary option (only used for PLY an PCD formats). Rtabmap: Do graph optimization if neighbor link refined and Mem/UseOdomGravity is used.
introlab · Mar 1, 2021 · d119487 · d119487
1 parent 967c57d
commit d119487
Show file tree

Hide file tree

Showing 9 changed files with 1,333 additions and 191 deletions.
diff --git a/corelib/include/rtabmap/core/PDALWriter.h b/corelib/include/rtabmap/core/PDALWriter.h
@@ -35,11 +35,11 @@ namespace rtabmap {
 
 std::string getPDALSupportedWriters();
 
-int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointXYZ> & cloud);
-int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointXYZRGB> & cloud);
-int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud);
-int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointXYZI> & cloud);
-int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointXYZINormal> & cloud);
+int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointXYZ> & cloud,          const std::vector<int> & cameraIds = std::vector<int>(), bool binary = false);
+int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointXYZRGB> & cloud,       const std::vector<int> & cameraIds = std::vector<int>(), bool binary = false);
+int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud, const std::vector<int> & cameraIds = std::vector<int>(), bool binary = false);
+int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointXYZI> & cloud,         const std::vector<int> & cameraIds = std::vector<int>(), bool binary = false);
+int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointXYZINormal> & cloud,   const std::vector<int> & cameraIds = std::vector<int>(), bool binary = false);
 
 }
 

diff --git a/corelib/include/rtabmap/core/util3d.h b/corelib/include/rtabmap/core/util3d.h
@@ -38,6 +38,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/SensorData.h>
 #include <rtabmap/core/Parameters.h>
 #include <opencv2/core/core.hpp>
+#include <rtabmap/core/ProgressState.h>
 #include <map>
 #include <list>
 
@@ -300,6 +301,33 @@ void RTABMAP_EXP fillProjectedCloudHoles(
 		bool verticalDirection,
 		bool fillToBorder);
 
+/**
+ * For each point, return pixel of the best camera (NodeID->CameraIndex)
+ * looking at it based on the policy and parameters
+ */
+std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > RTABMAP_EXP projectCloudToCameras (
+		const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud,
+		const std::map<int, Transform> & cameraPoses,
+		const std::map<int, std::vector<CameraModel> > & cameraModels,
+		float maxDistance = 0.0f,
+		float maxAngle = 0.0f,
+		const std::vector<float> & roiRatios = std::vector<float>(),
+		bool distanceToCamPolicy = false,
+		const ProgressState * state = 0);
+/**
+ * For each point, return pixel of the best camera (NodeID->CameraIndex)
+ * looking at it based on the policy and parameters
+ */
+std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > RTABMAP_EXP projectCloudToCameras (
+		const pcl::PointCloud<pcl::PointXYZINormal> & cloud,
+		const std::map<int, Transform> & cameraPoses,
+		const std::map<int, std::vector<CameraModel> > & cameraModels,
+		float maxDistance = 0.0f,
+		float maxAngle = 0.0f,
+		const std::vector<float> & roiRatios = std::vector<float>(),
+		bool distanceToCamPolicy = false,
+		const ProgressState * state = 0);
+
 bool RTABMAP_EXP isFinite(const cv::Point3f & pt);
 
 pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP concatenateClouds(