rtabmap: supporting RGB+camera_info only with in RGBD/Enabled=true and Mem/IncrementalMemory=false (used to localize using only RGB against a prebuilt map)

2026-10-04 08:47:45 +08:00 · 2017-07-10 12:17:17 -04:00
parent 1a1919e5e0
commit e944786ee5
4 changed files with 178 additions and 54 deletions
@@ -105,6 +105,8 @@ CoreWrapper::CoreWrapper() :
 		mapToOdom_(rtabmap::Transform::getIdentity()),
 		transformThread_(0),
 		tfThreadRunning_(false),
+		SYNC_INIT(rgb),
+		SYNC_INIT(rgbOdom),
 		stereoToDepth_(false),
 		odomSensorSync_(false),
 		rate_(Parameters::defaultRtabmapDetectionRate()),
@@ -478,21 +480,84 @@ void CoreWrapper::onInit()
 	if(!this->isDataSubscribed())
 	{
 		bool isRGBD = uStr2Bool(parameters_.at(Parameters::kRGBDEnabled()).c_str());
-		if(isRGBD)
+		bool incremental = uStr2Bool(parameters_.at(Parameters::kMemIncrementalMemory()).c_str());
+		if(isRGBD && !incremental)
 		{
-			NODELET_WARN("ROS param subscribe_depth, subscribe_stereo and subscribe_rgbd are false, but RTAB-Map "
-					  "parameter \"%s\" is true! Please set subscribe_depth, subscribe_stereo or subscribe_rgbd "
-					  "to true to use rtabmap node for RGB-D SLAM, or set \"%s\" to false for loop closure "
-					  "detection on images-only.", Parameters::kRGBDEnabled().c_str(), Parameters::kRGBDEnabled().c_str());
+			NODELET_INFO("\"%s\" is true and \"%s\" is false, subscribing to RGB + camera info...",
+					Parameters::kRGBDEnabled().c_str(),
+					Parameters::kMemIncrementalMemory().c_str());
+			ros::NodeHandle rgb_nh(nh, "rgb");
+			ros::NodeHandle rgb_pnh(pnh, "rgb");
+			image_transport::ImageTransport rgb_it(rgb_nh);
+			image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh);
+			rgbSub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb);
+			rgbCameraInfoSub_.subscribe(rgb_nh, "camera_info", 1);
+
+			std::string odomFrameId;
+			pnh.getParam("odom_frame_id", odomFrameId);
+			if(!odomFrameId.empty())
+			{
+				// odom from TF
+				if(isApproxSync())
+				{
+					rgbApproximateSync_ = new message_filters::Synchronizer<rgbApproximateSyncPolicy>(
+							rgbApproximateSyncPolicy(getQueueSize()), rgbSub_, rgbCameraInfoSub_);
+					rgbApproximateSync_->registerCallback(boost::bind(&CoreWrapper::rgbCallback, this, _1, _2));
+				}
+				else
+				{
+					rgbExactSync_ = new message_filters::Synchronizer<rgbExactSyncPolicy>(
+							rgbExactSyncPolicy(getQueueSize()), rgbSub_, rgbCameraInfoSub_);
+					rgbExactSync_->registerCallback(boost::bind(&CoreWrapper::rgbCallback, this, _1, _2));
+				}
+				NODELET_INFO("\n%s subscribed to:\n   %s\n   %s",
+						getName().c_str(),
+						rgbSub_.getTopic().c_str(),
+						rgbCameraInfoSub_.getTopic().c_str());
+			}
+			else
+			{
+				rgbOdomSub_.subscribe(nh, "odom", 1);
+				if(isApproxSync())
+				{
+					rgbOdomApproximateSync_ = new message_filters::Synchronizer<rgbOdomApproximateSyncPolicy>(
+							rgbOdomApproximateSyncPolicy(getQueueSize()), rgbSub_, rgbCameraInfoSub_, rgbOdomSub_);
+					rgbOdomApproximateSync_->registerCallback(boost::bind(&CoreWrapper::rgbOdomCallback, this, _1, _2, _3));
+				}
+				else
+				{
+					rgbOdomExactSync_ = new message_filters::Synchronizer<rgbOdomExactSyncPolicy>(
+							rgbOdomExactSyncPolicy(getQueueSize()), rgbSub_, rgbCameraInfoSub_, rgbOdomSub_);
+					rgbOdomExactSync_->registerCallback(boost::bind(&CoreWrapper::rgbOdomCallback, this, _1, _2, _3));
+				}
+				NODELET_INFO("\n%s subscribed to:\n   %s\n   %s\n   %s",
+						getName().c_str(),
+						rgbSub_.getTopic().c_str(),
+						rgbCameraInfoSub_.getTopic().c_str(),
+						rgbOdomSub_.getTopic().c_str());
+			}
 		}
+		else
+		{
+			if(isRGBD)
+			{
+				NODELET_WARN("ROS param subscribe_depth, subscribe_stereo and subscribe_rgbd are false, but RTAB-Map "
+						  "parameter \"%s\" and \"%s\" are true! Please set subscribe_depth, subscribe_stereo or subscribe_rgbd "
+						  "to true to use rtabmap node for RGB-D SLAM, set \"%s\" to false for loop closure "
+						  "detection on images-only or set \"%s\" to false to localize a single RGB camera against pre-built 3D map.",
+						  Parameters::kRGBDEnabled().c_str(),
+						  Parameters::kMemIncrementalMemory().c_str(),
+						  Parameters::kRGBDEnabled().c_str(),
+						  Parameters::kMemIncrementalMemory().c_str());
+			}
+			ros::NodeHandle rgb_nh(nh, "rgb");
+			ros::NodeHandle rgb_pnh(pnh, "rgb");
+			image_transport::ImageTransport rgb_it(rgb_nh);
+			image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh);
+			defaultSub_ = rgb_it.subscribe("image", 1, &CoreWrapper::defaultCallback, this);

-		ros::NodeHandle rgb_nh(nh, "rgb");
-		ros::NodeHandle rgb_pnh(pnh, "rgb");
-		image_transport::ImageTransport rgb_it(rgb_nh);
-		image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh);
-		defaultSub_ = rgb_it.subscribe("image", 1, &CoreWrapper::defaultCallback, this);
-
-		NODELET_INFO("\n%s subscribed to:\n   %s", getName().c_str(), defaultSub_.getTopic().c_str());
+			NODELET_INFO("\n%s subscribed to:\n   %s", getName().c_str(), defaultSub_.getTopic().c_str());
+		}
 	}

 	userDataAsyncSub_ = nh.subscribe("user_data_async", 1, &CoreWrapper::userDataAsyncCallback, this);
@@ -508,6 +573,9 @@ CoreWrapper::~CoreWrapper()
 		delete transformThread_;
 	}

+	SYNC_DEL(rgb);
+	SYNC_DEL(rgbOdom);
+
 	this->saveParameters(configPath_);

 	ros::NodeHandle nh;
@@ -636,6 +704,33 @@ void CoreWrapper::defaultCallback(const sensor_msgs::ImageConstPtr & imageMsg)
 	}
 }

+
+void CoreWrapper::rgbCallback(
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg)
+{
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	nav_msgs::OdometryConstPtr odomMsg; // Null
+	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	cv_bridge::CvImageConstPtr depthMsg;// Null
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+}
+
+void CoreWrapper::rgbOdomCallback(
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+		const nav_msgs::OdometryConstPtr & odomMsg)
+{
+	rtabmap_ros::UserDataConstPtr userDataMsg; // Null
+	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	sensor_msgs::PointCloud2ConstPtr scan3dMsg; // null
+	rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
+	cv_bridge::CvImageConstPtr depthMsg;// Null
+	commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
+}
+
 bool CoreWrapper::odomUpdate(const nav_msgs::OdometryConstPtr & odomMsg)
 {
 	if(!paused_)
@@ -847,7 +942,7 @@ void CoreWrapper::commonDepthCallbackImpl(
 	}

 	UASSERT(uContains(parameters_, rtabmap::Parameters::kMemSaveDepth16Format()));
-	if(depth.type() == CV_32FC1 && uStr2Bool(parameters_.at(Parameters::kMemSaveDepth16Format())))
+	if(!depth.empty() && depth.type() == CV_32FC1 && uStr2Bool(parameters_.at(Parameters::kMemSaveDepth16Format())))
 	{
 		depth = rtabmap::util2d::cvtDepthFromFloat(depth);
 		static bool shown = false;
@@ -865,7 +960,7 @@ void CoreWrapper::commonDepthCallbackImpl(
 	Transform scanLocalTransform = Transform::getIdentity();
 	pcl::PointCloud<pcl::PointXYZ> scanCloud2d;
 	bool genMaxScanPts = 0;
-	if(scan2dMsg.get() == 0 && scan3dMsg.get() == 0 && genScan_)
+	if(scan2dMsg.get() == 0 && scan3dMsg.get() == 0 && !depth.empty() && genScan_)
 	{
 		scanCloud2d = util3d::laserScanFromDepthImages(
 				depth,
@@ -1106,18 +1106,21 @@ bool convertRGBDMsgs(
 		double waitForTransform)
 {
 	UASSERT(imageMsgs.size()>0 &&
-			imageMsgs.size() == depthMsgs.size() &&
+			(imageMsgs.size() == depthMsgs.size() || depthMsgs.empty()) &&
 			imageMsgs.size() == cameraInfoMsgs.size());

 	int imageWidth = imageMsgs[0]->image.cols;
 	int imageHeight = imageMsgs[0]->image.rows;
-	int depthWidth = depthMsgs[0]->image.cols;
-	int depthHeight = depthMsgs[0]->image.rows;
+	int depthWidth = depthMsgs.size()?depthMsgs[0]->image.cols:0;
+	int depthHeight = depthMsgs.size()?depthMsgs[0]->image.rows:0;

-	UASSERT_MSG(
-			imageWidth % depthWidth == 0 && imageHeight % depthHeight == 0 &&
-			imageWidth/depthWidth == imageHeight/depthHeight,
-			uFormat("rgb=%dx%d depth=%dx%d", imageWidth, imageHeight, depthWidth, depthHeight).c_str());
+	if(depthMsgs.size())
+	{
+		UASSERT_MSG(
+				imageWidth % depthWidth == 0 && imageHeight % depthHeight == 0 &&
+				imageWidth/depthWidth == imageHeight/depthHeight,
+				uFormat("rgb=%dx%d depth=%dx%d", imageWidth, imageHeight, depthWidth, depthHeight).c_str());
+	}

 	int cameraCount = imageMsgs.size();
 	for(unsigned int i=0; i<imageMsgs.size(); ++i)
@@ -1129,14 +1132,19 @@ bool convertRGBDMsgs(
 			 imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0 ||
 			 imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 0 ||
 			 imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0 ||
-			 imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::BAYER_GRBG8) == 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))
+			 imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::BAYER_GRBG8) == 0))
 		{
-			ROS_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8,bgra8,rgba8 and "
-					"image_depth=32FC1,16UC1,mono16. Current rgb=%s and depth=%s",
-					imageMsgs[i]->encoding.c_str(),
+
+			ROS_ERROR("Input rgb type must be image=mono8,mono16,rgb8,bgr8,bgra8,rgba8. Current rgb=%s",
+					imageMsgs[i]->encoding.c_str());
+			return false;
+		}
+		 if(depthMsgs.size() &&
+			 !(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 depth type must be image_depth=32FC1,16UC1,mono16. Current depth=%s",
 					depthMsgs[i]->encoding.c_str());
 			return false;
 		}
@@ -1147,38 +1155,47 @@ bool convertRGBDMsgs(
 						imageMsgs[i]->image.cols,
 						imageHeight,
 						imageMsgs[i]->image.rows).c_str());
-		UASSERT_MSG(depthMsgs[i]->image.cols == depthWidth && depthMsgs[i]->image.rows == depthHeight,
-				uFormat("depthWidth=%d vs %d imageHeight=%d vs %d",
-						depthWidth,
-						depthMsgs[i]->image.cols,
-						depthHeight,
-						depthMsgs[i]->image.rows).c_str());
+		ros::Time stamp;
+		if(depthMsgs.size())
+		{
+			UASSERT_MSG(depthMsgs[i]->image.cols == depthWidth && depthMsgs[i]->image.rows == depthHeight,
+					uFormat("depthWidth=%d vs %d imageHeight=%d vs %d",
+							depthWidth,
+							depthMsgs[i]->image.cols,
+							depthHeight,
+							depthMsgs[i]->image.rows).c_str());
+			stamp = depthMsgs[i]->header.stamp;
+		}
+		else
+		{
+			stamp = imageMsgs[i]->header.stamp;
+		}

 		// use depth's stamp so that geometry is sync to odom, use rgb frame as we assume depth is registered (normally depth msg should have same frame than rgb)
-		rtabmap::Transform localTransform = rtabmap_ros::getTransform(frameId, imageMsgs[i]->header.frame_id, depthMsgs[i]->header.stamp, listener, waitForTransform);
+		rtabmap::Transform localTransform = rtabmap_ros::getTransform(frameId, imageMsgs[i]->header.frame_id, 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());
+			ROS_ERROR("TF of received image %d at time %fs is not set!", i, stamp.toSec());
 			return false;
 		}
 		// sync with odometry stamp
-		if(!odomFrameId.empty() && odomStamp != depthMsgs[i]->header.stamp)
+		if(!odomFrameId.empty() && odomStamp != stamp)
 		{
 			rtabmap::Transform sensorT = getTransform(
 					frameId,
 					odomFrameId,
 					odomStamp,
-					depthMsgs[i]->header.stamp,
+					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());
+						"stamp is %fs. The depth image pose will not be synchronized with odometry.", stamp.toSec(), odomStamp.toSec());
 			}
 			else
 			{
-				//ROS_WARN("RGBD correction = %s (time diff=%fs)", sensorT.prettyPrint().c_str(), fabs(depthMsgs[i]->header.stamp.toSec()-odomStamp.toSec()));
+				//ROS_WARN("RGBD correction = %s (time diff=%fs)", sensorT.prettyPrint().c_str(), fabs(stamp.toSec()-odomStamp.toSec()));
 				localTransform = sensorT * localTransform;
 			}
 		}
@@ -1198,19 +1215,12 @@ bool convertRGBDMsgs(
 		{
 			ptrImage = cv_bridge::cvtColor(imageMsgs[i], "bgr8");
 		}
-		cv_bridge::CvImageConstPtr ptrDepth = 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(depthHeight, depthWidth*cameraCount, subDepth.type());
-		}
-
 		if(ptrImage->image.type() == rgb.type())
 		{
 			ptrImage->image.copyTo(cv::Mat(rgb, cv::Rect(i*imageWidth, 0, imageWidth, imageHeight)));
@@ -1221,14 +1231,25 @@ bool convertRGBDMsgs(
 			return false;
 		}

-		if(subDepth.type() == depth.type())
+		if(depthMsgs.size())
 		{
-			subDepth.copyTo(cv::Mat(depth, cv::Rect(i*depthWidth, 0, depthWidth, depthHeight)));
-		}
-		else
-		{
-			ROS_ERROR("Some Depth images are not the same type!");
-			return false;
+			cv_bridge::CvImageConstPtr ptrDepth = depthMsgs[i];
+			cv::Mat subDepth = ptrDepth->image;
+
+			if(depth.empty())
+			{
+				depth = cv::Mat(depthHeight, depthWidth*cameraCount, subDepth.type());
+			}
+
+			if(subDepth.type() == depth.type())
+			{
+				subDepth.copyTo(cv::Mat(depth, cv::Rect(i*depthWidth, 0, depthWidth, depthHeight)));
+			}
+			else
+			{
+				ROS_ERROR("Some Depth images are not the same type!");
+				return false;
+			}
 		}

 		cameraModels.push_back(rtabmap_ros::cameraModelFromROS(cameraInfoMsgs[i], localTransform));