Added util3d::projectCloudToCamera() method (creating a registered depth image from the laser scan), used for "Generate depth from scan" option of CameraImages

corelib/include/rtabmap/core/CameraInfo.h

@@ -55,6 +55,7 @@ public:
 	float timeMirroring;
 	float timeImageDecimation;
 	float timeScanFromDepth;
+	float timeDepthFromScan;
 };
 
 } // namespace rtabmap

corelib/include/rtabmap/core/CameraRGB.h

@@ -96,6 +96,13 @@ public:
 		}
 	}
 
+	void setDepthFromScan(bool enabled, bool fillHolesVertical = true, bool fillHolesFromBorder = false)
+	{
+		_depthFromScan = enabled;
+		_depthFromScanFillHolesVertical = fillHolesVertical;
+		_depthFromScanFillHolesFromBorder = fillHolesFromBorder;
+	}
+
 	void setGroundTruthPath(const std::string & filePath, int format = 0)
 	{
 		groundTruthPath_ = filePath;
@@ -134,6 +141,10 @@ private:
 	float _scanVoxelSize;
 	int _scanNormalsK;
 
+	bool _depthFromScan;
+	bool _depthFromScanFillHolesVertical;
+	bool _depthFromScanFillHolesFromBorder;
+
 	bool _filenamesAreTimestamps;
 	std::string timestampsPath_;
 

corelib/include/rtabmap/core/Parameters.h

@@ -397,7 +397,7 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(Icp, VoxelSize,                 float, 0.025, "Uniform sampling voxel size (0=disabled).");
 	RTABMAP_PARAM(Icp, DownsamplingStep,          int, 1, 	    "Downsampling step size (1=no sampling). This is done before uniform sampling.");
 	RTABMAP_PARAM(Icp, MaxCorrespondenceDistance, float, 0.05,  "Max distance for point correspondences.");
-	RTABMAP_PARAM(Icp, Iterations,                int, 30, 		"Max iterations.");
+	RTABMAP_PARAM(Icp, Iterations,                int, 10, 		"Max iterations.");
 	RTABMAP_PARAM(Icp, CorrespondenceRatio,       float, 0.3,   "Ratio of matching correspondences to accept the transform.");
 	RTABMAP_PARAM(Icp, PointToPlane,              bool, false, 	"Use point to plane ICP.");
 	RTABMAP_PARAM(Icp, PointToPlaneNormalNeighbors, int, 20,    "Number of neighbors to compute normals for point to plane.");

corelib/include/rtabmap/core/util2d.h

@@ -109,7 +109,7 @@ float RTABMAP_EXP getDepth(
 
 cv::Mat RTABMAP_EXP decimate(const cv::Mat & image, int d);
 
-// Registration Depth to RGB
+// Registration Depth to RGB (return registered depth image)
 cv::Mat RTABMAP_EXP registerDepth(
 		const cv::Mat & depth,
 		const cv::Mat & depthK,
@@ -117,7 +117,7 @@ cv::Mat RTABMAP_EXP registerDepth(
 		const rtabmap::Transform & transform);
 
 void RTABMAP_EXP fillRegisteredDepthHoles(
-		cv::Mat & depth,
+		cv::Mat & depthRegistered,
 		bool vertical,
 		bool horizontal,
 		bool fillDoubleHoles = false);

corelib/include/rtabmap/core/util3d.h

@@ -145,6 +145,26 @@ cv::Point3f RTABMAP_EXP projectDisparityTo3D(
 		const cv::Mat & disparity,
 		const StereoCameraModel & model);
 
+// Register point cloud to camera (return registered depth image)
+cv::Mat RTABMAP_EXP projectCloudToCamera(
+		const cv::Size & imageSize,
+		const cv::Mat & cameraMatrixK,  // /base_link -> /camera_link
+		const cv::Mat & laserScan,      // assuming points are already in /base_link coordinate
+		const rtabmap::Transform & cameraTransform);
+
+// Register point cloud to camera (return registered depth image)
+cv::Mat RTABMAP_EXP projectCloudToCamera(
+		const cv::Size & imageSize,
+		const cv::Mat & cameraMatrixK,                       // /base_link -> /camera_link
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr laserScan, // assuming points are already in /base_link coordinate
+		const rtabmap::Transform & cameraTransform);
+
+// Direction vertical (>=0), horizontal (<0)
+void RTABMAP_EXP fillProjectedCloudHoles(
+		cv::Mat & depthRegistered,
+		bool verticalDirection,
+		bool fillToBorder);
+
 bool RTABMAP_EXP isFinite(const cv::Point3f & pt);
 
 pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP concatenateClouds(
@@ -199,6 +219,12 @@ cv::Mat RTABMAP_EXP loadScan(
 		float voxelSize = 0.0f,
 		int normalsK = 0);
 
+pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP loadCloud(
+		const std::string & path,
+		const Transform & transform = Transform::getIdentity(),
+		int downsampleStep = 1,
+		float voxelSize = 0.0f);
+
 } // namespace util3d
 } // namespace rtabmap