Updated cloud/scan conversion to include intensity if there is

src/CoreWrapper.cpp

@@ -1071,8 +1071,7 @@ void CoreWrapper::commonDepthCallbackImpl(
 		}
 	}
 
-	cv::Mat scan;
-	Transform scanLocalTransform = Transform::getIdentity();
+	LaserScan scan;
 	bool genMaxScanPts = 0;
 	if(scan2dMsg.get() == 0 && scan3dMsg.get() == 0 && !depth.empty() && genScan_)
 	{
@@ -1083,7 +1082,7 @@ void CoreWrapper::commonDepthCallbackImpl(
 				genScanMaxDepth_,
 				genScanMinDepth_);
 		genMaxScanPts += depth.cols;
-		scan = rtabmap::util3d::laserScan2dFromPointCloud(*scanCloud2d);
+		scan = LaserScan(rtabmap::util3d::laserScan2dFromPointCloud(*scanCloud2d), 0, genScanMaxDepth_, LaserScan::kXY);
 	}
 	else if(scan2dMsg.get() != 0)
 	{
@@ -1093,26 +1092,14 @@ void CoreWrapper::commonDepthCallbackImpl(
 				odomSensorSync_?odomFrameId:"",
 				lastPoseStamp_,
 				scan,
-				scanLocalTransform,
 				tfListener_,
-				waitForTransform_?waitForTransformDuration_:0))
+				waitForTransform_?waitForTransformDuration_:0,
+				// backward compatibility, project 2D scan in /base_link frame
+				rtabmap_.getMemory() && uStrNumCmp(rtabmap_.getMemory()->getDatabaseVersion(), "0.11.10") < 0))
 		{
 			NODELET_ERROR("Could not convert laser scan msg! Aborting rtabmap update...");
 			return;
 		}
-		Transform zAxis(0,0,1,0,0,0);
-		if((scanLocalTransform.rotation()*zAxis).z() < 0)
-		{
-			cv::Mat flipScan;
-			cv::flip(scan, flipScan, 1);
-			scan = flipScan;
-		}
-		if(rtabmap_.getMemory() && uStrNumCmp(rtabmap_.getMemory()->getDatabaseVersion(), "0.11.10") < 0)
-		{
-			// backward compatibility, project 2D scan in /base_link frame
-			scan = util3d::transformLaserScan(LaserScan::backwardCompatibility(scan), scanLocalTransform).data();
-			scanLocalTransform = Transform::getIdentity();
-		}
 	}
 	else if(scan3dMsg.get() != 0)
 	{
@@ -1122,9 +1109,9 @@ void CoreWrapper::commonDepthCallbackImpl(
 				odomSensorSync_?odomFrameId:"",
 				lastPoseStamp_,
 				scan,
-				scanLocalTransform,
 				tfListener_,
-				waitForTransform_?waitForTransformDuration_:0))
+				waitForTransform_?waitForTransformDuration_:0,
+				scanCloudMaxPoints_))
 		{
 			NODELET_ERROR("Could not convert 3d laser scan msg! Aborting rtabmap update...");
 			return;
@@ -1155,18 +1142,8 @@ void CoreWrapper::commonDepthCallbackImpl(
 		userData_ = cv::Mat();
 	}
 
-	SensorData data(scan2dMsg.get() !=0?
-					LaserScan::backwardCompatibility(scan,
-							scan2dMsg->range_min,
-							scan2dMsg->range_max,
-							scan2dMsg->angle_min,
-							scan2dMsg->angle_max,
-							scan2dMsg->angle_increment,
-							scanLocalTransform):
-					LaserScan::backwardCompatibility(scan,
-							genScan_?genMaxScanPts:scan3dMsg.get() != 0?scanCloudMaxPoints_:0,
-							genScan_?genScanMaxDepth_:0.0f,
-							scanLocalTransform),
+	SensorData data(
+			scan,
 			rgb,
 			depth,
 			cameraModels,
@@ -1372,8 +1349,7 @@ void CoreWrapper::commonStereoCallback(
 		return;
 	}
 
-	cv::Mat scan;
-	Transform scanLocalTransform = Transform::getIdentity();
+	LaserScan scan;
 	if(scan2dMsg.get() != 0)
 	{
 		if(!rtabmap_ros::convertScanMsg(
@@ -1382,26 +1358,14 @@ void CoreWrapper::commonStereoCallback(
 				odomSensorSync_?odomFrameId:"",
 				lastPoseStamp_,
 				scan,
-				scanLocalTransform,
 				tfListener_,
-				waitForTransform_?waitForTransformDuration_:0))
+				waitForTransform_?waitForTransformDuration_:0,
+				// backward compatibility, project 2D scan in /base_link frame
+				rtabmap_.getMemory() && uStrNumCmp(rtabmap_.getMemory()->getDatabaseVersion(), "0.11.10") < 0))
 		{
 			NODELET_ERROR("Could not convert laser scan msg! Aborting rtabmap update...");
 			return;
 		}
-		Transform zAxis(0,0,1,0,0,0);
-		if((scanLocalTransform.rotation()*zAxis).z() < 0)
-		{
-			cv::Mat flipScan;
-			cv::flip(scan, flipScan, 1);
-			scan = flipScan;
-		}
-		if(rtabmap_.getMemory() && uStrNumCmp(rtabmap_.getMemory()->getDatabaseVersion(), "0.11.10") < 0)
-		{
-			// backward compatibility, project 2D scan in /base_link frame
-			scan = util3d::transformLaserScan(LaserScan::backwardCompatibility(scan), scanLocalTransform).data();
-			scanLocalTransform = Transform::getIdentity();
-		}
 	}
 	else if(scan3dMsg.get() != 0)
 	{
@@ -1411,9 +1375,9 @@ void CoreWrapper::commonStereoCallback(
 				odomSensorSync_?odomFrameId:"",
 				lastPoseStamp_,
 				scan,
-				scanLocalTransform,
 				tfListener_,
-				waitForTransform_?waitForTransformDuration_:0))
+				waitForTransform_?waitForTransformDuration_:0,
+				scanCloudMaxPoints_))
 		{
 			NODELET_ERROR("Could not convert 3d laser scan msg! Aborting rtabmap update...");
 			return;
@@ -1444,18 +1408,8 @@ void CoreWrapper::commonStereoCallback(
 		userData_ = cv::Mat();
 	}
 
-	SensorData data(scan2dMsg.get() != 0?
-					LaserScan::backwardCompatibility(scan,
-							scan2dMsg->range_min,
-							scan2dMsg->range_max,
-							scan2dMsg->angle_min,
-							scan2dMsg->angle_max,
-							scan2dMsg->angle_increment,
-							scanLocalTransform):
-					LaserScan::backwardCompatibility(scan,
-							scan3dMsg.get() != 0?scanCloudMaxPoints_:0,
-							0,
-							scanLocalTransform),
+	SensorData data(
+			scan,
 			left,
 			right,
 			stereoModel,
@@ -1591,8 +1545,7 @@ void CoreWrapper::commonLaserScanCallback(
 		return;
 	}
 
-	cv::Mat scan;
-	Transform scanLocalTransform = Transform::getIdentity();
+	LaserScan scan;
 	if(scan2dMsg.get() != 0)
 	{
 		if(!rtabmap_ros::convertScanMsg(
@@ -1601,26 +1554,14 @@ void CoreWrapper::commonLaserScanCallback(
 				odomSensorSync_?odomFrameId:"",
 				lastPoseStamp_,
 				scan,
-				scanLocalTransform,
 				tfListener_,
-				waitForTransform_?waitForTransformDuration_:0))
+				waitForTransform_?waitForTransformDuration_:0,
+				// backward compatibility, project 2D scan in /base_link frame
+				rtabmap_.getMemory() && uStrNumCmp(rtabmap_.getMemory()->getDatabaseVersion(), "0.11.10") < 0))
 		{
 			NODELET_ERROR("Could not convert laser scan msg! Aborting rtabmap update...");
 			return;
 		}
-		Transform zAxis(0,0,1,0,0,0);
-		if((scanLocalTransform.rotation()*zAxis).z() < 0)
-		{
-			cv::Mat flipScan;
-			cv::flip(scan, flipScan, 1);
-			scan = flipScan;
-		}
-		if(rtabmap_.getMemory() && uStrNumCmp(rtabmap_.getMemory()->getDatabaseVersion(), "0.11.10") < 0)
-		{
-			// backward compatibility, project 2D scan in /base_link frame
-			scan = util3d::transformLaserScan(LaserScan::backwardCompatibility(scan), scanLocalTransform).data();
-			scanLocalTransform = Transform::getIdentity();
-		}
 	}
 	else if(scan3dMsg.get() != 0)
 	{
@@ -1630,9 +1571,9 @@ void CoreWrapper::commonLaserScanCallback(
 				odomSensorSync_?odomFrameId:"",
 				lastPoseStamp_,
 				scan,
-				scanLocalTransform,
 				tfListener_,
-				waitForTransform_?waitForTransformDuration_:0))
+				waitForTransform_?waitForTransformDuration_:0,
+				scanCloudMaxPoints_))
 		{
 			NODELET_ERROR("Could not convert 3d laser scan msg! Aborting rtabmap update...");
 			return;
@@ -1670,22 +1611,12 @@ void CoreWrapper::commonLaserScanCallback(
 			1,
 			0.5,
 			1,
-			scanLocalTransform*Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0),
+			scan.localTransform()*Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0),
 			0,
 			cv::Size(1,2));
 
-	SensorData data(scan2dMsg.get() != 0?
-					LaserScan::backwardCompatibility(scan,
-							scan2dMsg->range_min,
-							scan2dMsg->range_max,
-							scan2dMsg->angle_min,
-							scan2dMsg->angle_max,
-							scan2dMsg->angle_increment,
-							scanLocalTransform):
-					LaserScan::backwardCompatibility(scan,
-							scan3dMsg.get() != 0?scanCloudMaxPoints_:0,
-							0,
-							scanLocalTransform),
+	SensorData data(
+			scan,
 			rgb,
 			depth,
 			model,

src/GuiWrapper.cpp

@@ -485,8 +485,7 @@ void GuiWrapper::commonDepthCallback(
 	cv::Mat rgb;
 	cv::Mat depth;
 	std::vector<CameraModel> cameraModels;
-	cv::Mat scan;
-	Transform scanLocalTransform = Transform::getIdentity();
+	LaserScan scan;
 	rtabmap::OdometryInfo info;
 	bool ignoreData = false;
 
@@ -526,7 +525,6 @@ void GuiWrapper::commonDepthCallback(
 					odomSensorSync_?odomHeader.frame_id:"",
 					odomHeader.stamp,
 					scan,
-					scanLocalTransform,
 					tfListener_,
 					waitForTransform_?waitForTransformDuration_:0))
 			{
@@ -542,7 +540,6 @@ void GuiWrapper::commonDepthCallback(
 					odomSensorSync_?odomHeader.frame_id:"",
 					odomHeader.stamp,
 					scan,
-					scanLocalTransform,
 					tfListener_,
 					waitForTransform_?waitForTransformDuration_:0))
 			{
@@ -571,10 +568,7 @@ void GuiWrapper::commonDepthCallback(
 	info.reg.covariance = covariance;
 	rtabmap::OdometryEvent odomEvent(
 		rtabmap::SensorData(
-				LaserScan::backwardCompatibility(scan,
-						scan2dMsg.get()?(int)scan2dMsg->ranges.size():0,
-						scan2dMsg.get()?(int)scan2dMsg->range_max:0,
-						scanLocalTransform),
+				scan,
 				rgb,
 				depth,
 				cameraModels,
@@ -642,8 +636,7 @@ void GuiWrapper::commonStereoCallback(
 
 	cv::Mat left;
 	cv::Mat right;
-	cv::Mat scan;
-	Transform scanLocalTransform = Transform::getIdentity();
+	LaserScan scan;
 	rtabmap::StereoCameraModel stereoModel;
 	rtabmap::OdometryInfo info;
 	bool ignoreData = false;
@@ -682,7 +675,6 @@ void GuiWrapper::commonStereoCallback(
 					odomSensorSync_?odomHeader.frame_id:"",
 					odomHeader.stamp,
 					scan,
-					scanLocalTransform,
 					tfListener_,
 					waitForTransform_?waitForTransformDuration_:0))
 			{
@@ -698,7 +690,6 @@ void GuiWrapper::commonStereoCallback(
 					odomSensorSync_?odomHeader.frame_id:"",
 					odomHeader.stamp,
 					scan,
-					scanLocalTransform,
 					tfListener_,
 					waitForTransform_?waitForTransformDuration_:0))
 			{
@@ -727,10 +718,7 @@ void GuiWrapper::commonStereoCallback(
 	info.reg.covariance = covariance;
 	rtabmap::OdometryEvent odomEvent(
 		rtabmap::SensorData(
-				LaserScan::backwardCompatibility(scan,
-						scan2dMsg.get()?(int)scan2dMsg->ranges.size():0,
-						scan2dMsg.get()?(int)scan2dMsg->range_max:0,
-						scanLocalTransform),
+				scan,
 				left,
 				right,
 				stereoModel,
@@ -794,10 +782,10 @@ void GuiWrapper::commonLaserScanCallback(
 		return;
 	}
 
-	cv::Mat scan;
-	Transform scanLocalTransform = Transform::getIdentity();
+	LaserScan scan;
 	rtabmap::OdometryInfo info;
 	bool ignoreData = false;
+	Transform fakeCameraLocalTransform = Transform::getIdentity();
 
 	// limit update rate
 	if(maxOdomUpdateRate_<=0.0 ||
@@ -815,7 +803,6 @@ void GuiWrapper::commonLaserScanCallback(
 					odomSensorSync_?odomHeader.frame_id:"",
 					odomHeader.stamp,
 					scan,
-					scanLocalTransform,
 					tfListener_,
 					waitForTransform_?waitForTransformDuration_:0))
 			{
@@ -831,7 +818,6 @@ void GuiWrapper::commonLaserScanCallback(
 					odomSensorSync_?odomHeader.frame_id:"",
 					odomHeader.stamp,
 					scan,
-					scanLocalTransform,
 					tfListener_,
 					waitForTransform_?waitForTransformDuration_:0))
 			{
@@ -851,11 +837,11 @@ void GuiWrapper::commonLaserScanCallback(
 		//just get scan local transform to adjust camera frame
 		if(scan2dMsg.get() != 0)
 		{
-			scanLocalTransform = getTransform(frameId_, scan2dMsg->header.frame_id, scan2dMsg->header.stamp, tfListener_, waitForTransform_?waitForTransformDuration_:0);
+			fakeCameraLocalTransform = getTransform(frameId_, scan2dMsg->header.frame_id, scan2dMsg->header.stamp, tfListener_, waitForTransform_?waitForTransformDuration_:0);
 		}
 		else if(scan3dMsg.get() != 0)
 		{
-			scanLocalTransform = getTransform(frameId_, scan3dMsg->header.frame_id, scan3dMsg->header.stamp, tfListener_, waitForTransform_?waitForTransformDuration_:0);
+			fakeCameraLocalTransform = getTransform(frameId_, scan3dMsg->header.frame_id, scan3dMsg->header.stamp, tfListener_, waitForTransform_?waitForTransformDuration_:0);
 		}
 
 		info = rtabmap_ros::odomInfoFromROS(*odomInfoMsg).copyWithoutData();
@@ -874,22 +860,14 @@ void GuiWrapper::commonLaserScanCallback(
 			1,
 			0.5,
 			1,
-			scanLocalTransform*Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0),
+			fakeCameraLocalTransform*Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0),
 			0,
 			cv::Size(1,2));
 
 	info.reg.covariance = covariance;
 	rtabmap::OdometryEvent odomEvent(
 		rtabmap::SensorData(
-				scan2dMsg.get() != 0?
-					LaserScan::backwardCompatibility(scan,
-							scan2dMsg->range_min,
-							scan2dMsg->range_max,
-							scan2dMsg->angle_min,
-							scan2dMsg->angle_max,
-							scan2dMsg->angle_increment,
-							scanLocalTransform):
-					LaserScan::backwardCompatibility(scan,0,0,scanLocalTransform),
+				scan,
 				rgb,
 				depth,
 				model,

src/MsgConversion.cpp

@@ -1633,10 +1633,10 @@ bool convertScanMsg(
 		const std::string & frameId,
 		const std::string & odomFrameId,
 		const ros::Time & odomStamp,
-		cv::Mat & scan,
-		rtabmap::Transform & scanLocalTransform,
+		rtabmap::LaserScan & scan,
 		tf::TransformListener & listener,
-		double waitForTransform)
+		double waitForTransform,
+		bool outputInFrameId)
 {
 	// make sure the frame of the laser is updated too
 	rtabmap::Transform tmpT = getTransform(
@@ -1650,7 +1650,7 @@ bool convertScanMsg(
 		return false;
 	}
 
-	scanLocalTransform = getTransform(
+	rtabmap::Transform scanLocalTransform = getTransform(
 			frameId,
 			scan2dMsg->header.frame_id,
 			scan2dMsg->header.stamp,
@@ -1665,9 +1665,6 @@ bool convertScanMsg(
 	sensor_msgs::PointCloud2 scanOut;
 	laser_geometry::LaserProjection projection;
 	projection.transformLaserScanToPointCloud(odomFrameId.empty()?frameId:odomFrameId, *scan2dMsg, scanOut, listener);
-	pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
-	pcl::fromROSMsg(scanOut, *pclScan);
-	pclScan->is_dense = true;
 
 	//transform back in laser frame
 	rtabmap::Transform laserToOdom = getTransform(
@@ -1703,7 +1700,56 @@ bool convertScanMsg(
 		}
 	}
 
-	scan = rtabmap::util3d::laserScan2dFromPointCloud(*pclScan, laserToOdom); // put back in laser frame
+	if(outputInFrameId)
+	{
+		laserToOdom *= scanLocalTransform;
+	}
+
+	bool containIntensity = false;
+	for(unsigned int i=0; i<scanOut.fields.size(); ++i)
+	{
+		if(scanOut.fields[i].name.compare("intensity") == 0)
+		{
+			containIntensity = true;
+		}
+	}
+
+	rtabmap::LaserScan::Format format;
+	cv::Mat data;
+	if(containIntensity)
+	{
+		pcl::PointCloud<pcl::PointXYZI>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZI>);
+		pcl::fromROSMsg(scanOut, *pclScan);
+		pclScan->is_dense = true;
+		data = rtabmap::util3d::laserScan2dFromPointCloud(*pclScan, laserToOdom); // put back in laser frame
+		format = rtabmap::LaserScan::kXYI;
+	}
+	else
+	{
+		pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
+		pcl::fromROSMsg(scanOut, *pclScan);
+		pclScan->is_dense = true;
+		data = rtabmap::util3d::laserScan2dFromPointCloud(*pclScan, laserToOdom); // put back in laser frame
+		format = rtabmap::LaserScan::kXY;
+	}
+
+	rtabmap::Transform zAxis(0,0,1,0,0,0);
+	if((scanLocalTransform.rotation()*zAxis).z() < 0)
+	{
+		cv::Mat flipScan;
+		cv::flip(data, flipScan, 1);
+		data = flipScan;
+	}
+
+	scan = rtabmap::LaserScan(
+			data,
+			format,
+			scan2dMsg->range_min,
+			scan2dMsg->range_max,
+			scan2dMsg->angle_min,
+			scan2dMsg->angle_max,
+			scan2dMsg->angle_increment,
+			outputInFrameId?rtabmap::Transform::getIdentity():scanLocalTransform);
 
 	return true;
 }
@@ -1713,13 +1759,15 @@ bool convertScan3dMsg(
 		const std::string & frameId,
 		const std::string & odomFrameId,
 		const ros::Time & odomStamp,
-		cv::Mat & scan,
-		rtabmap::Transform & scanLocalTransform,
+		rtabmap::LaserScan & scan,
 		tf::TransformListener & listener,
-		double waitForTransform)
+		double waitForTransform,
+		int maxPoints,
+		float maxRange)
 {
 	bool containNormals = false;
 	bool containColors = false;
+	bool containIntensity = false;
 	for(unsigned int i=0; i<scan3dMsg->fields.size(); ++i)
 	{
 		if(scan3dMsg->fields[i].name.compare("normal_x") == 0)
@@ -1730,9 +1778,13 @@ bool convertScan3dMsg(
 		{
 			containColors = true;
 		}
+		if(scan3dMsg->fields[i].name.compare("intensity") == 0)
+		{
+			containIntensity = true;
+		}
 	}
 
-	scanLocalTransform = getTransform(frameId, scan3dMsg->header.frame_id, scan3dMsg->header.stamp, listener, waitForTransform);
+	rtabmap::Transform scanLocalTransform = getTransform(frameId, scan3dMsg->header.frame_id, scan3dMsg->header.stamp, listener, waitForTransform);
 	if(scanLocalTransform.isNull())
 	{
 		ROS_ERROR("TF of received scan cloud at time %fs is not set, aborting rtabmap update.", scan3dMsg->header.stamp.toSec());
@@ -1770,7 +1822,17 @@ bool convertScan3dMsg(
 			{
 				pclScan = rtabmap::util3d::removeNaNNormalsFromPointCloud(pclScan);
 			}
-			scan = rtabmap::util3d::laserScanFromPointCloud(*pclScan);
+			scan = rtabmap::LaserScan(rtabmap::util3d::laserScanFromPointCloud(*pclScan), maxPoints, maxRange, rtabmap::LaserScan::kXYZRGBNormal, scanLocalTransform);
+		}
+		else if(containIntensity)
+		{
+			pcl::PointCloud<pcl::PointXYZINormal>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZINormal>);
+			pcl::fromROSMsg(*scan3dMsg, *pclScan);
+			if(!pclScan->is_dense)
+			{
+				pclScan = rtabmap::util3d::removeNaNNormalsFromPointCloud(pclScan);
+			}
+			scan = rtabmap::LaserScan(rtabmap::util3d::laserScanFromPointCloud(*pclScan), maxPoints, maxRange, rtabmap::LaserScan::kXYZINormal, scanLocalTransform);
 		}
 		else
 		{
@@ -1780,7 +1842,7 @@ bool convertScan3dMsg(
 			{
 				pclScan = rtabmap::util3d::removeNaNNormalsFromPointCloud(pclScan);
 			}
-			scan = rtabmap::util3d::laserScanFromPointCloud(*pclScan);
+			scan = rtabmap::LaserScan(rtabmap::util3d::laserScanFromPointCloud(*pclScan), maxPoints, maxRange, rtabmap::LaserScan::kXYZNormal, scanLocalTransform);
 		}
 	}
 	else
@@ -1793,8 +1855,17 @@ bool convertScan3dMsg(
 			{
 				pclScan = rtabmap::util3d::removeNaNFromPointCloud(pclScan);
 			}
-
-			scan = rtabmap::util3d::laserScanFromPointCloud(*pclScan);
+			scan = rtabmap::LaserScan(rtabmap::util3d::laserScanFromPointCloud(*pclScan), maxPoints, maxRange, rtabmap::LaserScan::kXYZRGB, scanLocalTransform);
+		}
+		else if(containIntensity)
+		{
+			pcl::PointCloud<pcl::PointXYZI>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZI>);
+			pcl::fromROSMsg(*scan3dMsg, *pclScan);
+			if(!pclScan->is_dense)
+			{
+				pclScan = rtabmap::util3d::removeNaNFromPointCloud(pclScan);
+			}
+			scan = rtabmap::LaserScan(rtabmap::util3d::laserScanFromPointCloud(*pclScan), maxPoints, maxRange, rtabmap::LaserScan::kXYZI, scanLocalTransform);
 		}
 		else
 		{
@@ -1804,8 +1875,7 @@ bool convertScan3dMsg(
 			{
 				pclScan = rtabmap::util3d::removeNaNFromPointCloud(pclScan);
 			}
-
-			scan = rtabmap::util3d::laserScanFromPointCloud(*pclScan);
+			scan = rtabmap::LaserScan(rtabmap::util3d::laserScanFromPointCloud(*pclScan), maxPoints, maxRange, rtabmap::LaserScan::kXYZ, scanLocalTransform);
 		}
 	}
 	return true;