Refactoring common depth/stereo/laser msgs conversion into shared methods in MsgConversion.h between CoreWrapper.cpp and GuiWrapper.cpp (avoiding some duplicated codes)

2026-10-06 17:57:45 +08:00 · 2016-08-31 12:45:55 -04:00
parent 7323e58e8d
commit 02decf639d
5 changed files with 697 additions and 745 deletions
@@ -39,6 +39,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <tf_conversions/tf_eigen.h>
 #include <image_geometry/pinhole_camera_model.h>
 #include <image_geometry/stereo_camera_model.h>
+#include <sensor_msgs/image_encodings.h>
+#include <cv_bridge/cv_bridge.h>
+#include <laser_geometry/laser_geometry.h>
+#include <rtabmap/core/util3d_surface.h>

 namespace rtabmap_ros {

@@ -864,4 +868,440 @@ void odomInfoToROS(const rtabmap::OdometryInfo & info, rtabmap_ros::OdomInfo & m
 	msg.localScanMap = rtabmap::compressData(info.localScanMap);
 }

+rtabmap::Transform getTransform(
+		const std::string & fromFrameId,
+		const std::string & toFrameId,
+		const ros::Time & stamp,
+		tf::TransformListener & listener,
+		double waitForTransform)
+{
+	// TF ready?
+	rtabmap::Transform transform;
+	try
+	{
+		if(waitForTransform > 0.0 && !stamp.isZero())
+		{
+			//if(!tfBuffer_.canTransform(fromFrameId, toFrameId, stamp, ros::Duration(1)))
+			std::string errorMsg;
+			if(!listener.waitForTransform(fromFrameId, toFrameId, stamp, ros::Duration(waitForTransform), ros::Duration(0.01), &errorMsg))
+			{
+				ROS_WARN("Could not get transform from %s to %s after %f seconds (for stamp=%f)! Error=\"%s\".",
+						fromFrameId.c_str(), toFrameId.c_str(), waitForTransform, stamp.toSec(), errorMsg.c_str());
+				return transform;
+			}
+		}
+
+		tf::StampedTransform tmp;
+		listener.lookupTransform(fromFrameId, toFrameId, stamp, tmp);
+		transform = rtabmap_ros::transformFromTF(tmp);
+	}
+	catch(tf::TransformException & ex)
+	{
+		ROS_WARN("%s",ex.what());
+	}
+	return transform;
+}
+
+// get moving transform accordingly to a fixed frame. For example get
+// transform between moving /base_link between two stamps accordingly to /odom frame.
+rtabmap::Transform getTransform(
+		const std::string & sourceTargetFrame,
+		const std::string & fixedFrame,
+		const ros::Time & stampSource,
+		const ros::Time & stampTarget,
+		tf::TransformListener & listener,
+		double waitForTransform)
+{
+	// TF ready?
+	rtabmap::Transform transform;
+	try
+	{
+		ros::Time stamp = stampSource>stampTarget?stampSource:stampTarget;
+		if(waitForTransform > 0.0 && !stamp.isZero())
+		{
+			std::string errorMsg;
+			if(!listener.waitForTransform(sourceTargetFrame, fixedFrame, stamp, ros::Duration(waitForTransform), ros::Duration(0.01), &errorMsg))
+			{
+				ROS_WARN("Could not get transform from %s to %s accordingly to %s after %f seconds (for stamps=%f -> %f)! Error=\"%s\".",
+						sourceTargetFrame.c_str(), sourceTargetFrame.c_str(), fixedFrame.c_str(), waitForTransform, stampSource.toSec(), stampTarget.toSec(), errorMsg.c_str());
+				return transform;
+			}
+		}
+
+		tf::StampedTransform tmp;
+		listener.lookupTransform(sourceTargetFrame, stampTarget, sourceTargetFrame, stampSource, fixedFrame, tmp);
+		transform = rtabmap_ros::transformFromTF(tmp);
+	}
+	catch(tf::TransformException & ex)
+	{
+		ROS_WARN("%s",ex.what());
+	}
+	return transform;
+}
+
+bool convertRGBDMsgs(
+		const std::vector<sensor_msgs::ImageConstPtr> & imageMsgs,
+		const std::vector<sensor_msgs::ImageConstPtr> & depthMsgs,
+		const std::vector<sensor_msgs::CameraInfoConstPtr> & cameraInfoMsgs,
+		const std::string & frameId,
+		const std::string & odomFrameId,
+		const ros::Time & odomStamp,
+		cv::Mat & rgb,
+		cv::Mat & depth,
+		std::vector<rtabmap::CameraModel> & cameraModels,
+		tf::TransformListener & listener,
+		double waitForTransform)
+{
+	UASSERT(imageMsgs.size()>0 &&
+			imageMsgs.size() == depthMsgs.size() &&
+			imageMsgs.size() == cameraInfoMsgs.size());
+
+	int imageWidth = imageMsgs[0]->width;
+	int imageHeight = imageMsgs[0]->height;
+	int cameraCount = imageMsgs.size();
+	for(unsigned int i=0; i<imageMsgs.size(); ++i)
+	{
+		if(!(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) == 0 ||
+			 imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
+			 imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
+			 imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
+			 imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) ||
+			!(depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1) == 0 ||
+			 depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) == 0 ||
+			 depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0))
+		{
+			ROS_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 and image_depth=32FC1,16UC1,mono16");
+			return false;
+		}
+
+		UASSERT_MSG(imageMsgs[i]->width == imageWidth && imageMsgs[i]->height == imageHeight,
+				uFormat("imageWidth=%d vs %d imageHeight=%d vs %d",
+						imageWidth,
+						imageMsgs[i]->width,
+						imageHeight,
+						imageMsgs[i]->height).c_str());
+		UASSERT_MSG(depthMsgs[i]->width == imageWidth && depthMsgs[i]->height == imageHeight,
+				uFormat("imageWidth=%d vs %d imageHeight=%d vs %d",
+						imageWidth,
+						depthMsgs[i]->width,
+						imageHeight,
+						depthMsgs[i]->height).c_str());
+
+		rtabmap::Transform localTransform = rtabmap_ros::getTransform(frameId, depthMsgs[i]->header.frame_id, depthMsgs[i]->header.stamp, listener, waitForTransform);
+		if(localTransform.isNull())
+		{
+			ROS_ERROR("TF of received depth image %d at time %fs is not set!", i, depthMsgs[i]->header.stamp.toSec());
+			return false;
+		}
+		// sync with odometry stamp
+		if(odomStamp != depthMsgs[i]->header.stamp)
+		{
+			rtabmap::Transform sensorT = getTransform(
+					frameId,
+					odomFrameId,
+					odomStamp,
+					depthMsgs[i]->header.stamp,
+					listener,
+					waitForTransform);
+			if(sensorT.isNull())
+			{
+				ROS_WARN("Could not get odometry value for depth image stamp (%fs). Latest odometry "
+						"stamp is %fs. The depth image pose will not be synchronized with odometry.", depthMsgs[i]->header.stamp.toSec(), odomStamp.toSec());
+			}
+			else
+			{
+				localTransform = sensorT * localTransform;
+			}
+		}
+
+		cv_bridge::CvImageConstPtr ptrImage;
+		if(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0)
+		{
+			ptrImage = cv_bridge::toCvShare(imageMsgs[i]);
+		}
+		else if(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
+		   imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
+		{
+			ptrImage = cv_bridge::toCvShare(imageMsgs[i], "mono8");
+		}
+		else
+		{
+			ptrImage = cv_bridge::toCvShare(imageMsgs[i], "bgr8");
+		}
+		cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depthMsgs[i]);
+		cv::Mat subDepth = ptrDepth->image;
+
+		// initialize
+		if(rgb.empty())
+		{
+			rgb = cv::Mat(imageHeight, imageWidth*cameraCount, ptrImage->image.type());
+		}
+		if(depth.empty())
+		{
+			depth = cv::Mat(imageHeight, imageWidth*cameraCount, subDepth.type());
+		}
+
+		if(ptrImage->image.type() == rgb.type())
+		{
+			ptrImage->image.copyTo(cv::Mat(rgb, cv::Rect(i*imageWidth, 0, imageWidth, imageHeight)));
+		}
+		else
+		{
+			ROS_ERROR("Some RGB images are not the same type!");
+			return false;
+		}
+
+		if(subDepth.type() == depth.type())
+		{
+			subDepth.copyTo(cv::Mat(depth, cv::Rect(i*imageWidth, 0, imageWidth, imageHeight)));
+		}
+		else
+		{
+			ROS_ERROR("Some Depth images are not the same type!");
+			return false;
+		}
+
+		cameraModels.push_back(rtabmap_ros::cameraModelFromROS(*cameraInfoMsgs[i], localTransform));
+	}
+	return true;
+}
+
+bool convertStereoMsg(
+		const sensor_msgs::ImageConstPtr& leftImageMsg,
+		const sensor_msgs::ImageConstPtr& rightImageMsg,
+		const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg,
+		const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg,
+		const std::string & frameId,
+		const std::string & odomFrameId,
+		const ros::Time & odomStamp,
+		cv::Mat & left,
+		cv::Mat & right,
+		rtabmap::StereoCameraModel & stereoModel,
+		tf::TransformListener & listener,
+		double waitForTransform)
+{
+	UASSERT(leftImageMsg.get() && rightImageMsg.get());
+	UASSERT(leftCamInfoMsg.get() && rightCamInfoMsg.get());
+
+	if(!(leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
+		leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0 ||
+		leftImageMsg->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
+		leftImageMsg->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) ||
+		!(rightImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
+		rightImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0 ||
+		rightImageMsg->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
+		rightImageMsg->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0))
+	{
+		ROS_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8");
+		return false;
+	}
+
+	if(leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
+	   leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
+	{
+		left = cv_bridge::toCvCopy(leftImageMsg, "mono8")->image;
+	}
+	else
+	{
+		left = cv_bridge::toCvCopy(leftImageMsg, "bgr8")->image;
+	}
+	right = cv_bridge::toCvCopy(rightImageMsg, "mono8")->image;
+
+	rtabmap::Transform localTransform = getTransform(frameId, leftImageMsg->header.frame_id, leftImageMsg->header.stamp, listener, waitForTransform);
+	if(localTransform.isNull())
+	{
+		return false;
+	}
+	// sync with odometry stamp
+	if(odomStamp != leftImageMsg->header.stamp)
+	{
+		rtabmap::Transform sensorT = getTransform(
+				frameId,
+				odomFrameId,
+				odomStamp,
+				leftImageMsg->header.stamp,
+				listener,
+				waitForTransform);
+		if(sensorT.isNull())
+		{
+			ROS_WARN("Could not get odometry value for stereo msg stamp (%fs). Latest odometry "
+					"stamp is %fs. The stereo image pose will not be synchronized with odometry.", leftImageMsg->header.stamp.toSec(), odomStamp.toSec());
+		}
+		else
+		{
+			localTransform = sensorT * localTransform;
+		}
+	}
+
+	stereoModel = rtabmap_ros::stereoCameraModelFromROS(*leftCamInfoMsg, *rightCamInfoMsg, localTransform);
+
+	if(stereoModel.baseline() > 10.0)
+	{
+		static bool shown = false;
+		if(!shown)
+		{
+			ROS_WARN("Detected baseline (%f m) is quite large! Is your "
+					 "right camera_info P(0,3) correctly set? Note that "
+					 "baseline=-P(0,3)/P(0,0). You may need to calibrate your camera. "
+					 "This warning is printed only once.",
+					 stereoModel.baseline());
+			shown = true;
+		}
+	}
+	return true;
+}
+
+bool convertScanMsg(
+		const sensor_msgs::LaserScanConstPtr& scan2dMsg,
+		const std::string & frameId,
+		const std::string & odomFrameId,
+		const ros::Time & odomStamp,
+		cv::Mat & scan,
+		rtabmap::Transform & scanLocalTransform,
+		tf::TransformListener & listener,
+		double waitForTransform)
+{
+	// make sure the frame of the laser is updated too
+	rtabmap::Transform tmpT = getTransform(
+			odomFrameId,
+			scan2dMsg->header.frame_id,
+			scan2dMsg->header.stamp + ros::Duration().fromSec(scan2dMsg->ranges.size()*scan2dMsg->time_increment),
+			listener,
+			waitForTransform);
+	if(tmpT.isNull())
+	{
+		return false;
+	}
+
+	scanLocalTransform = getTransform(
+			frameId,
+			scan2dMsg->header.frame_id,
+			scan2dMsg->header.stamp,
+			listener,
+			waitForTransform);
+	if(scanLocalTransform.isNull())
+	{
+		return false;
+	}
+
+	//transform in frameId_ frame
+	sensor_msgs::PointCloud2 scanOut;
+	laser_geometry::LaserProjection projection;
+	projection.transformLaserScanToPointCloud(odomFrameId, *scan2dMsg, scanOut, listener);
+	pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
+	pcl::fromROSMsg(scanOut, *pclScan);
+
+	//transform back in laser frame
+	rtabmap::Transform laserToOdom = getTransform(
+			scan2dMsg->header.frame_id,
+			odomFrameId,
+			scan2dMsg->header.stamp,
+			listener,
+			waitForTransform);
+	if(laserToOdom.isNull())
+	{
+		return false;
+	}
+
+	// sync with odometry stamp
+	if(odomStamp != scan2dMsg->header.stamp)
+	{
+		rtabmap::Transform sensorT = getTransform(
+				frameId,
+				odomFrameId,
+				odomStamp,
+				scan2dMsg->header.stamp,
+				listener,
+				waitForTransform);
+		if(sensorT.isNull())
+		{
+			ROS_WARN("Could not get odometry value for laser scan stamp (%fs). Latest odometry "
+					"stamp is %fs. The laser scan pose will not be synchronized with odometry.", scan2dMsg->header.stamp.toSec(), odomStamp.toSec());
+		}
+		else
+		{
+			scanLocalTransform = sensorT * scanLocalTransform;
+		}
+	}
+	scan = rtabmap::util3d::laserScan2dFromPointCloud(*pclScan, laserToOdom); // put back in laser frame
+	return true;
+}
+
+bool convertScan3dMsg(
+		const sensor_msgs::PointCloud2ConstPtr & scan3dMsg,
+		const std::string & frameId,
+		const std::string & odomFrameId,
+		const ros::Time & odomStamp,
+		int scanCloudNormalK,
+		cv::Mat & scan,
+		rtabmap::Transform & scanLocalTransform,
+		tf::TransformListener & listener,
+		double waitForTransform)
+{
+	bool containNormals = false;
+	for(unsigned int i=0; i<scan3dMsg->fields.size(); ++i)
+	{
+		if(scan3dMsg->fields[i].name.compare("normal_x") == 0)
+		{
+			containNormals = true;
+			break;
+		}
+	}
+
+	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());
+		return false;
+	}
+
+	// sync with odometry stamp
+	if(odomStamp != scan3dMsg->header.stamp)
+	{
+		rtabmap::Transform sensorT = getTransform(
+				frameId,
+				odomFrameId,
+				odomStamp,
+				scan3dMsg->header.stamp,
+				listener,
+				waitForTransform);
+		if(sensorT.isNull())
+		{
+			ROS_WARN("Could not get odometry value for laser scan stamp (%fs). Latest odometry "
+					"stamp is %fs. The 3d laser scan pose will not be synchronized with odometry.", scan3dMsg->header.stamp.toSec(), odomStamp.toSec());
+		}
+		else
+		{
+			scanLocalTransform = sensorT * scanLocalTransform;
+		}
+	}
+
+	if(containNormals)
+	{
+		pcl::PointCloud<pcl::PointNormal>::Ptr pclScan(new pcl::PointCloud<pcl::PointNormal>);
+		pcl::fromROSMsg(*scan3dMsg, *pclScan);
+
+		scan = rtabmap::util3d::laserScanFromPointCloud(*pclScan);
+	}
+	else
+	{
+		pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
+		pcl::fromROSMsg(*scan3dMsg, *pclScan);
+
+		if(scanCloudNormalK > 0)
+		{
+			//compute normals
+			pcl::PointCloud<pcl::Normal>::Ptr normals = rtabmap::util3d::computeNormals(pclScan, scanCloudNormalK);
+			pcl::PointCloud<pcl::PointNormal>::Ptr pclScanNormal(new pcl::PointCloud<pcl::PointNormal>);
+			pcl::concatenateFields(*pclScan, *normals, *pclScanNormal);
+			scan = rtabmap::util3d::laserScanFromPointCloud(*pclScanNormal);
+		}
+		else
+		{
+			scan = rtabmap::util3d::laserScanFromPointCloud(*pclScan);
+		}
+	}
+	return true;
+}
+
 }