Added util3d::projectCloudToCamera() with PCLPointcloud2 interface, update util2d::fillDeptHoles() to support CV_32FC1

CMakeLists.txt

@@ -21,7 +21,7 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
 #######################
 SET(RTABMAP_MAJOR_VERSION 0)
 SET(RTABMAP_MINOR_VERSION 12)
-SET(RTABMAP_PATCH_VERSION 0)
+SET(RTABMAP_PATCH_VERSION 1)
 SET(RTABMAP_VERSION
   ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
   

corelib/include/rtabmap/core/util3d.h

@@ -216,18 +216,25 @@ cv::Point3f RTABMAP_EXP projectDisparityTo3D(
 		const cv::Mat & disparity,
 		const StereoCameraModel & model);
 
-// Register point cloud to camera (return registered depth image)
+// Register point cloud to camera (return registered depth image 32FC1)
 cv::Mat RTABMAP_EXP projectCloudToCamera(
 		const cv::Size & imageSize,
 		const cv::Mat & cameraMatrixK,  
 		const cv::Mat & laserScan,                   // assuming points are already in /base_link coordinate
 		const rtabmap::Transform & cameraTransform); // /base_link -> /camera_link
 
-// Register point cloud to camera (return registered depth image)
+// Register point cloud to camera (return registered depth image 32FC1)
+cv::Mat RTABMAP_EXP projectCloudToCamera(
+		const cv::Size & imageSize,
+		const cv::Mat & cameraMatrixK,
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr laserScan, // assuming points are already in /base_link coordinate
+		const rtabmap::Transform & cameraTransform);         // /base_link -> /camera_link
+
+// Register point cloud to camera (return registered depth image 32FC1)
 cv::Mat RTABMAP_EXP projectCloudToCamera(
 		const cv::Size & imageSize,
 		const cv::Mat & cameraMatrixK,                       
-		const pcl::PointCloud<pcl::PointXYZ>::Ptr laserScan, // assuming points are already in /base_link coordinate
+		const pcl::PCLPointCloud2::Ptr laserScan, 			 // assuming points are already in /base_link coordinate
 		const rtabmap::Transform & cameraTransform);         // /base_link -> /camera_link
 
 // Direction vertical (>=0), horizontal (<0)

corelib/src/util2d.cpp

@@ -1414,18 +1414,29 @@ cv::Mat registerDepth(
 	return registered;
 }
 
-cv::Mat fillDepthHoles(const cv::Mat & registeredDepth, int maximumHoleSize, float errorRatio)
+cv::Mat fillDepthHoles(const cv::Mat & depth, int maximumHoleSize, float errorRatio)
 {
-	UASSERT(registeredDepth.type() == CV_16UC1);
+	UASSERT(depth.type() == CV_16UC1 || depth.type() == CV_32FC1);
 	UASSERT(maximumHoleSize > 0);
-	cv::Mat output = registeredDepth.clone();
+	cv::Mat output = depth.clone();
-	for(int y=0; y<registeredDepth.rows-2; ++y)
+	bool isMM = depth.type() == CV_16UC1;
+	for(int y=0; y<depth.rows-2; ++y)
 	{
-		for(int x=0; x<registeredDepth.cols-2; ++x)
+		for(int x=0; x<depth.cols-2; ++x)
 		{
-			float a = registeredDepth.at<unsigned short>(y, x);
+			float a, bRight, bDown;
-			float bRight = registeredDepth.at<unsigned short>(y, x+1);
+			if(isMM)
-			float bDown = registeredDepth.at<unsigned short>(y+1, x);
+			{
+				a = depth.at<unsigned short>(y, x);
+				bRight = depth.at<unsigned short>(y, x+1);
+				bDown = depth.at<unsigned short>(y+1, x);
+			}
+			else
+			{
+				a = depth.at<float>(y, x);
+				bRight = depth.at<float>(y, x+1);
+				bDown = depth.at<float>(y+1, x);
+			}
 
 			if(a > 0.0f && (bRight == 0.0f || bDown == 0.0f))
 			{
@@ -1437,13 +1448,13 @@ cv::Mat fillDepthHoles(const cv::Mat & registeredDepth, int maximumHoleSize, flo
 					// horizontal
 					if(!horizontalSet)
 					{
-						if(x+1+h >= registeredDepth.cols)
+						if(x+1+h >= depth.cols)
 						{
 							horizontalSet = true;
 						}
 						else
 						{
-							float c = registeredDepth.at<unsigned short>(y, x+1+h);
+							float c = isMM?depth.at<unsigned short>(y, x+1+h):depth.at<float>(y, x+1+h);
 							if(c == 0)
 							{
 								// ignore this size
@@ -1456,16 +1467,36 @@ cv::Mat fillDepthHoles(const cv::Mat & registeredDepth, int maximumHoleSize, flo
 								{
 									//linear interpolation
 									float slope = (c-a)/float(h+1);
-									for(int z=x+1; z<x+1+h; ++z)
+									if(isMM)
 									{
-										if(output.at<unsigned short>(y, z) == 0)
+										for(int z=x+1; z<x+1+h; ++z)
 										{
-											output.at<unsigned short>(y, z) = (unsigned short)(a+(slope*float(z-x)));
+											unsigned short & value = output.at<unsigned short>(y, z);
+											if(value == 0)
+											{
+												value = (unsigned short)(a+(slope*float(z-x)));
+											}
+											else
+											{
+												// average with the previously set value
+												value = (value+(unsigned short)(a+(slope*float(z-x))))/2;
+											}
 										}
-										else
+									}
+									else
+									{
+										for(int z=x+1; z<x+1+h; ++z)
 										{
-											// average with the previously set value
+											float & value = output.at<float>(y, z);
-											output.at<unsigned short>(y, z) = (output.at<unsigned short>(y, z)+(unsigned short)(a+(slope*float(z-x))))/2;
+											if(value == 0)
+											{
+												value = a+(slope*float(z-x));
+											}
+											else
+											{
+												// average with the previously set value
+												value = (value+(a+(slope*float(z-x))))/2;
+											}
 										}
 									}
 								}
@@ -1478,13 +1509,13 @@ cv::Mat fillDepthHoles(const cv::Mat & registeredDepth, int maximumHoleSize, flo
 					// vertical
 					if(!verticalSet)
 					{
-						if(y+1+h >= registeredDepth.rows)
+						if(y+1+h >= depth.rows)
 						{
 							verticalSet = true;
 						}
 						else
 						{
-							float c = registeredDepth.at<unsigned short>(y+1+h, x);
+							float c = isMM?depth.at<unsigned short>(y+1+h, x):depth.at<float>(y+1+h, x);
 							if(c == 0)
 							{
 								// ignore this size
@@ -1497,16 +1528,36 @@ cv::Mat fillDepthHoles(const cv::Mat & registeredDepth, int maximumHoleSize, flo
 								{
 									//linear interpolation
 									float slope = (c-a)/float(h+1);
-									for(int z=y+1; z<y+1+h; ++z)
+									if(isMM)
 									{
-										if(output.at<unsigned short>(z, x) == 0)
+										for(int z=y+1; z<y+1+h; ++z)
 										{
-											output.at<unsigned short>(z, x) = (unsigned short)(a+(slope*float(z-y)));
+											unsigned short & value = output.at<unsigned short>(z, x);
+											if(value == 0)
+											{
+												value = (unsigned short)(a+(slope*float(z-y)));
+											}
+											else
+											{
+												// average with the previously set value
+												value = (value+(unsigned short)(a+(slope*float(z-y))))/2;
+											}
 										}
-										else
+									}
+									else
+									{
+										for(int z=y+1; z<y+1+h; ++z)
 										{
-											// average with the previously set value
+											float & value = output.at<float>(z, x);
-											output.at<unsigned short>(z, x) = (output.at<unsigned short>(z, x)+(unsigned short)(a+(slope*float(z-y))))/2;
+											if(value == 0)
+											{
+												value = (a+(slope*float(z-y)));
+											}
+											else
+											{
+												// average with the previously set value
+												value = (value+(a+(slope*float(z-y))))/2;
+											}
 										}
 									}
 								}

corelib/src/util3d.cpp

@@ -1483,6 +1483,10 @@ cv::Mat projectCloudToCamera(
 	cv::Mat registered = cv::Mat::zeros(imageSize, CV_32FC1);
 	Transform t = cameraTransform.inverse();
 
+	const cv::Vec2f* vec2Ptr = laserScan.ptr<cv::Vec2f>();
+	const cv::Vec3f* vec3Ptr = laserScan.ptr<cv::Vec3f>();
+	const cv::Vec6f* vec6Ptr = laserScan.ptr<cv::Vec6f>();
+
 	int count = 0;
 	for(int i=0; i<laserScan.cols; ++i)
 	{
@@ -1490,38 +1494,61 @@ cv::Mat projectCloudToCamera(
 		cv::Point3f ptScan;
 		if(laserScan.type() == CV_32FC2)
 		{
-			ptScan.x = laserScan.at<cv::Vec2f>(i)[0];
+			ptScan.x = vec2Ptr[i][0];
-			ptScan.y = laserScan.at<cv::Vec2f>(i)[1];
+			ptScan.y = vec2Ptr[i][1];
 			ptScan.z = 0;
 		}
 		else if(laserScan.type() == CV_32FC3)
 		{
-			ptScan.x = laserScan.at<cv::Vec3f>(i)[0];
+			ptScan.x = vec3Ptr[i][0];
-			ptScan.y = laserScan.at<cv::Vec3f>(i)[1];
+			ptScan.y = vec3Ptr[i][1];
-			ptScan.z = laserScan.at<cv::Vec3f>(i)[2];
+			ptScan.z = vec3Ptr[i][2];
 		}
 		else
 		{
-			ptScan.x = laserScan.at<cv::Vec6f>(i)[0];
+			ptScan.x = vec6Ptr[i][0];
-			ptScan.y = laserScan.at<cv::Vec6f>(i)[1];
+			ptScan.y = vec6Ptr[i][1];
-			ptScan.z = laserScan.at<cv::Vec6f>(i)[2];
+			ptScan.z = vec6Ptr[i][2];
 		}
 		ptScan = util3d::transformPoint(ptScan, t);
 
 		// re-project in camera frame
 		float z = ptScan.z;
-		float invZ = 1.0f/z;
-		int dx = (fx*ptScan.x)*invZ + cx;
-		int dy = (fy*ptScan.y)*invZ + cy;
 
-		if(z > 0.0f && uIsInBounds(dx, 0, registered.cols) && uIsInBounds(dy, 0, registered.rows))
+		bool set = false;
+		if(z > 0.0f)
 		{
-			++count;
+			float invZ = 1.0f/z;
-			float &zReg = registered.at<float>(dy, dx);
+			float dx = (fx*ptScan.x)*invZ + cx;
-			if(zReg == 0 || z < zReg)
+			float dy = (fy*ptScan.y)*invZ + cy;
+			int dx_low = dx;
+			int dy_low = dy;
+			int dx_high = dx + 0.5f;
+			int dy_high = dy + 0.5f;
+
+			if(uIsInBounds(dx_low, 0, registered.cols) && uIsInBounds(dy_low, 0, registered.rows))
 			{
-				zReg = z;
+				float &zReg = registered.at<float>(dy_low, dx_low);
+				if(zReg == 0 || z < zReg)
+				{
+					zReg = z;
+				}
+				set = true;
 			}
+			if((dx_low != dx_high || dy_low != dy_high) &&
+				uIsInBounds(dx_high, 0, registered.cols) && uIsInBounds(dy_high, 0, registered.rows))
+			{
+				float &zReg = registered.at<float>(dy_high, dx_high);
+				if(zReg == 0 || z < zReg)
+				{
+					zReg = z;
+				}
+				set = true;
+			}
+		}
+		if(set)
+		{
+			count++;
 		}
 	}
 	UDEBUG("Points in camera=%d/%d", count, laserScan.cols);
@@ -1556,26 +1583,170 @@ cv::Mat projectCloudToCamera(
 
 		// re-project in camera frame
 		float z = ptScan.z;
+		bool set = false;
 		if(z > 0.0f)
 		{
 			float invZ = 1.0f/z;
-			int dx = (fx*ptScan.x)*invZ + cx;
+			float dx = (fx*ptScan.x)*invZ + cx;
-			if(uIsInBounds(dx, 0, registered.cols))
+			float dy = (fy*ptScan.y)*invZ + cy;
+			int dx_low = dx;
+			int dy_low = dy;
+			int dx_high = dx + 0.5f;
+			int dy_high = dy + 0.5f;
+			if(uIsInBounds(dx_low, 0, registered.cols) && uIsInBounds(dy_low, 0, registered.rows))
 			{
-				int dy = (fy*ptScan.y)*invZ + cy;
+				set = true;
-				if(uIsInBounds(dy, 0, registered.rows))
+				float &zReg = registered.at<float>(dy_low, dx_low);
+				if(zReg == 0 || z < zReg)
 				{
-					++count;
+					zReg = z;
-					float &zReg = registered.at<float>(dy, dx);
+				}
-					if(zReg == 0 || z < zReg)
+			}
+			if((dx_low != dx_high || dy_low != dy_high) &&
+				uIsInBounds(dx_high, 0, registered.cols) && uIsInBounds(dy_high, 0, registered.rows))
+			{
+				set = true;
+				float &zReg = registered.at<float>(dy_high, dx_high);
+				if(zReg == 0 || z < zReg)
+				{
+					zReg = z;
+				}
+			}
+		}
+		if(set)
+		{
+			count++;
+		}
+	}
+	UDEBUG("Points in camera=%d/%d", count, (int)laserScan->size());
+
+	return registered;
+}
+
+cv::Mat projectCloudToCamera(
+		const cv::Size & imageSize,
+		const cv::Mat & cameraMatrixK,
+		const pcl::PCLPointCloud2::Ptr laserScan,  // assuming points are already in /base_link coordinate
+		const rtabmap::Transform & cameraTransform)           // /base_link -> /camera_link
+{
+	UASSERT(!cameraTransform.isNull());
+	UASSERT(!laserScan->data.empty());
+	UASSERT(cameraMatrixK.type() == CV_64FC1 && cameraMatrixK.cols == 3 && cameraMatrixK.cols == 3);
+
+	float fx = cameraMatrixK.at<double>(0,0);
+	float fy = cameraMatrixK.at<double>(1,1);
+	float cx = cameraMatrixK.at<double>(0,2);
+	float cy = cameraMatrixK.at<double>(1,2);
+
+	cv::Mat registered = cv::Mat::zeros(imageSize, CV_32FC1);
+	Transform t = cameraTransform.inverse();
+
+	pcl::MsgFieldMap field_map;
+	pcl::createMapping<pcl::PointXYZ> (laserScan->fields, field_map);
+
+	int count = 0;
+	if(field_map.size() == 1)
+	{
+		for (uint32_t row = 0; row < laserScan->height; ++row)
+		{
+			const uint8_t* row_data = &laserScan->data[row * laserScan->row_step];
+			for (uint32_t col = 0; col < laserScan->width; ++col)
+			{
+				const uint8_t* msg_data = row_data + col * laserScan->point_step;
+				pcl::PointXYZ ptScan;
+				memcpy (&ptScan, msg_data + field_map.front().serialized_offset, field_map.front().size);
+				ptScan = util3d::transformPoint(ptScan, t);
+
+				// re-project in camera frame
+				float z = ptScan.z;
+				bool set = false;
+				if(z > 0.0f)
+				{
+					float invZ = 1.0f/z;
+					float dx = (fx*ptScan.x)*invZ + cx;
+					float dy = (fy*ptScan.y)*invZ + cy;
+					int dx_low = dx;
+					int dy_low = dy;
+					int dx_high = dx + 0.5f;
+					int dy_high = dy + 0.5f;
+					if(uIsInBounds(dx_low, 0, registered.cols) && uIsInBounds(dy_low, 0, registered.rows))
 					{
-						zReg = z;
+						set = true;
+						float &zReg = registered.at<float>(dy_low, dx_low);
+						if(zReg == 0 || z < zReg)
+						{
+							zReg = z;
+						}
 					}
+					if((dx_low != dx_high || dy_low != dy_high) &&
+						uIsInBounds(dx_high, 0, registered.cols) && uIsInBounds(dy_high, 0, registered.rows))
+					{
+						set = true;
+						float &zReg = registered.at<float>(dy_high, dx_high);
+						if(zReg == 0 || z < zReg)
+						{
+							zReg = z;
+						}
+					}
+				}
+				if(set)
+				{
+					count++;
 				}
 			}
 		}
 	}
-	UDEBUG("Points in camera=%d/%d", count, (int)laserScan->size());
+	else
+	{
+		UERROR("field map pcl::pointXYZ not found!");
+	}
+/*
+	int count = 0;
+	for(int i=0; i<(int)laserScan->size(); ++i)
+	{
+		// Get 3D from laser scan
+		pcl::PointXYZ ptScan = laserScan->at(i);
+		ptScan = util3d::transformPoint(ptScan, t);
+
+		// re-project in camera frame
+		float z = ptScan.z;
+		bool set = false;
+		if(z > 0.0f)
+		{
+			float invZ = 1.0f/z;
+			float dx = (fx*ptScan.x)*invZ + cx;
+			float dy = (fy*ptScan.y)*invZ + cy;
+			int dx_low = dx;
+			int dy_low = dy;
+			int dx_high = dx + 0.5f;
+			int dy_high = dy + 0.5f;
+			if(uIsInBounds(dx_low, 0, registered.cols) && uIsInBounds(dy_low, 0, registered.rows))
+			{
+				set = true;
+				float &zReg = registered.at<float>(dy_low, dx_low);
+				if(zReg == 0 || z < zReg)
+				{
+					zReg = z;
+				}
+			}
+			if((dx_low != dx_high || dy_low != dy_high) &&
+				uIsInBounds(dx_high, 0, registered.cols) && uIsInBounds(dy_high, 0, registered.rows))
+			{
+				set = true;
+				float &zReg = registered.at<float>(dy_high, dx_high);
+				if(zReg == 0 || z < zReg)
+				{
+					zReg = z;
+				}
+			}
+		}
+		if(set)
+		{
+			count++;
+		}
+	}
+	*/
+	UDEBUG("Points in camera=%d/%d", count, (int)laserScan->data.size());
 
 	return registered;
 }