Updated for rtabmap 0.20.5. OdometryROS: Fixed odometry stamps slightly off when subscribing to IMU (causing exact sync problems on rtabmap/rtambapviz side).

CMakeLists.txt

@@ -31,7 +31,7 @@ find_package(find_object_2d)
 
 ## System dependencies are found with CMake's conventions
 # find_package(Boost REQUIRED COMPONENTS system)
-find_package(RTABMap 0.20.4 REQUIRED)
+find_package(RTABMap 0.20.5 REQUIRED)
 
 find_package(OpenCV REQUIRED)
 

include/rtabmap_ros/CommonDataSubscriber.h

@@ -72,6 +72,7 @@ public:
 	int rgbdCameras() const {return isSubscribedToRGBD()?(int)rgbdSubs_.size():0;}
 	int getQueueSize() const {return queueSize_;}
 	bool isApproxSync() const {return approxSync_;}
+	const std::string & name() const {return name_;}
 
 protected:
 	void setupCallbacks(

include/rtabmap_ros/OdometryROS.h

@@ -142,7 +142,7 @@ private:
 	bool waitIMUToinit_;
 	bool imuProcessed_;
 	std::map<double, rtabmap::IMU> imus_;
-	rtabmap::SensorData bufferedData_;
+	std::pair<rtabmap::SensorData, ros::Time> bufferedData_;
 };
 
 }

launch/tests/test_d435i_vio.launch

@@ -7,6 +7,7 @@
 <arg name="rtabmapviz" default="true"/>
 <arg name="rviz"       default="false"/>
 <arg name="depth_mode" default="true"/>
+<arg name="odom_strategy" default="9"/> <!-- default VINS -->
 <arg name="unite_imu_method" default="copy"/> <!-- "copy" or "linear_interpolation" -->
 
 <include file="$(find realsense2_camera)/launch/rs_camera.launch">
@@ -32,7 +33,7 @@
 <!-- RTAB-Map: depth mode -->
 <!-- We have to launch stereo_odometry externally from rtabmap.launch so that rtabmap can use RGB-D input -->
 <group ns="rtabmap">
-<node if="$(arg depth_mode)" pkg="rtabmap_ros" type="stereo_odometry" name="stereo_odometry" args="--Optimizer/GravitySigma 0.3 --Odom/Strategy 9 --OdomVINS/ConfigPath $(find vins)/../config/realsense_d435i/realsense_stereo_imu_config.yaml" output="screen">
+<node if="$(arg depth_mode)" pkg="rtabmap_ros" type="stereo_odometry" name="stereo_odometry" args="--Optimizer/GravitySigma 0.3 --Odom/Strategy $(arg odom_strategy) --OdomVINS/ConfigPath $(find vins)/../config/realsense_d435i/realsense_stereo_imu_config.yaml" output="screen">
    <remap from="left/image_rect"   to="/camera/infra1/image_rect_raw"/>
    <remap from="right/image_rect"  to="/camera/infra2/image_rect_raw"/>
    <remap from="left/camera_info"  to="/camera/infra1/camera_info"/>
@@ -57,7 +58,7 @@
 
 <!-- RTAB-Map: Stereo mode -->
 <include unless="$(arg depth_mode)" file="$(find rtabmap_ros)/launch/rtabmap.launch">
-   <arg name="rtabmap_args"            value="--delete_db_on_start --Optimizer/GravitySigma 0.3 --Odom/Strategy 9 --OdomVINS/ConfigPath $(find vins)/../config/realsense_d435i/realsense_stereo_imu_config.yaml"/>
+   <arg name="rtabmap_args"            value="--delete_db_on_start --Optimizer/GravitySigma 0.3 --Odom/Strategy $(arg odom_strategy) --OdomVINS/ConfigPath $(find vins)/../config/realsense_d435i/realsense_stereo_imu_config.yaml"/>
    <arg name="left_image_topic"        value="/camera/infra1/image_rect_raw"/>
    <arg name="right_image_topic"       value="/camera/infra2/image_rect_raw"/>
    <arg name="left_camera_info_topic"  value="/camera/infra1/camera_info"/>

src/MsgConversion.cpp

@@ -920,29 +920,37 @@ void mapGraphToROS(
 rtabmap::Signature nodeDataFromROS(const rtabmap_ros::NodeData & msg)
 {
 	//Features stuff...
-	std::multimap<int, cv::KeyPoint> words;
-	std::multimap<int, cv::Point3f> words3D;
-	std::multimap<int, cv::Mat> wordsDescriptors;
-	cv::Mat descriptors = rtabmap::uncompressData(msg.wordDescriptors);
+	std::multimap<int, int> words;
+	std::vector<cv::KeyPoint> wordsKpts;
+	std::vector<cv::Point3f> words3D;
+	cv::Mat wordsDescriptors = rtabmap::uncompressData(msg.wordDescriptors);
 
-	for(unsigned int i=0; i<msg.wordIds.size() && i<msg.wordKpts.size(); ++i)
+	if(!msg.wordKpts.empty() && msg.wordKpts.size() != msg.wordIds.size())
 	{
-		cv::KeyPoint pt = keypointFromROS(msg.wordKpts.at(i));
-		int wordId = msg.wordIds.at(i);
-		words.insert(std::make_pair(wordId, pt));
-		if(i< msg.wordPts.size())
-		{
-			words3D.insert(std::make_pair(wordId, point3fFromROS(msg.wordPts[i])));
-		}
-		if(i < descriptors.rows)
-		{
-			wordsDescriptors.insert(std::make_pair(wordId, descriptors.row(i).clone()));
-		}
+		ROS_ERROR("Word IDs and 2D keypoints should be the same size (%d, %d)!", (int)msg.wordIds.size(), (int)msg.wordKpts.size());
+	}
+	if(!msg.wordPts.empty() && msg.wordPts.size() != msg.wordIds.size())
+	{
+		ROS_ERROR("Word IDs and 3D points should be the same size (%d, %d)!", (int)msg.wordIds.size(), (int)msg.wordPts.size());
+	}
+	if(wordsDescriptors.rows != (int)msg.wordIds.size())
+	{
+		ROS_ERROR("Word IDs and descriptors should be the same size (%d, %d)!", (int)msg.wordIds.size(), wordsDescriptors.rows);
+		wordsDescriptors = cv::Mat();
 	}
 
-	if(words3D.size() && words3D.size() != words.size())
+	for(unsigned int i=0; i<msg.wordIds.size(); ++i)
 	{
-		ROS_ERROR("Words 2D and 3D should be the same size (%d, %d)!", (int)words.size(), (int)words3D.size());
+		words.insert(std::make_pair(msg.wordIds.at(i), words.size())); // ID to index
+		if(msg.wordIds.size() == msg.wordKpts.size())
+		{
+			cv::KeyPoint pt = keypointFromROS(msg.wordKpts.at(i));
+			wordsKpts.push_back(pt);
+		}
+		if(msg.wordIds.size() == msg.wordPts.size())
+		{
+			words3D.push_back(point3fFromROS(msg.wordPts[i]));
+		}
 	}
 
 	rtabmap::StereoCameraModel stereoModel;
@@ -1031,9 +1039,7 @@ rtabmap::Signature nodeDataFromROS(const rtabmap_ros::NodeData & msg)
 					msg.id,
 					msg.stamp,
 					compressedMatFromBytes(msg.userData)));
-	s.setWords(words);
-	s.setWords3(words3D);
-	s.setWordsDescriptors(wordsDescriptors);
+	s.setWords(words, wordsKpts, words3D, wordsDescriptors);
 	s.sensorData().setGlobalDescriptors(rtabmap_ros::globalDescriptorsFromROS(msg.globalDescriptors));
 	s.sensorData().setEnvSensors(rtabmap_ros::envSensorsFromROS(msg.env_sensors));
 	s.sensorData().setOccupancyGrid(
@@ -1110,67 +1116,56 @@ void nodeDataToROS(const rtabmap::Signature & signature, rtabmap_ros::NodeData &
 
 	//Features stuff...
 	msg.wordIds = uKeys(signature.getWords());
-	msg.wordKpts.resize(signature.getWords().size());
-	int index = 0;
-	for(std::multimap<int, cv::KeyPoint>::const_iterator jter=signature.getWords().begin();
-		jter!=signature.getWords().end();
-		++jter)
+	if(!signature.getWordsKpts().empty())
 	{
-		keypointToROS(jter->second, msg.wordKpts.at(index++));
-	}
-
-	if(signature.getWords3().size() && signature.getWords3().size() == signature.getWords().size())
-	{
-		msg.wordPts.resize(signature.getWords3().size());
-		int i=0;
-		for(std::multimap<int, cv::Point3f>::const_iterator jter=signature.getWords3().begin();
-			jter!=signature.getWords3().end();
-			++jter)
+		if(msg.wordIds.size() == signature.getWordsKpts().size())
 		{
-			point3fToROS(jter->second, msg.wordPts[i++]);
+			msg.wordKpts.resize(signature.getWordsKpts().size());
+		}
+		else
+		{
+			ROS_ERROR("Word IDs and 2D keypoints must have the same size (%d vs %d)!",
+					(int)signature.getWords().size(),
+					(int)signature.getWordsKpts().size());
 		}
 	}
-	else if(signature.getWords3().size())
-	{
-		ROS_ERROR("Words 2D and words 3D must have the same size (%d vs %d)!",
-				(int)signature.getWords().size(),
-				(int)signature.getWords3().size());
-	}
 
-	if(signature.getWordsDescriptors().size() && signature.getWordsDescriptors().size() == signature.getWords().size())
+	if(!signature.getWords3().empty())
 	{
-		cv::Mat descriptors(
-				signature.getWordsDescriptors().size(),
-				signature.getWordsDescriptors().begin()->second.cols,
-				signature.getWordsDescriptors().begin()->second.type());
-		index = 0;
-		bool valid = true;
-		for(std::multimap<int, cv::Mat>::const_iterator jter=signature.getWordsDescriptors().begin();
-			jter!=signature.getWordsDescriptors().end() && valid;
-			++jter)
+		if(msg.wordIds.size() == signature.getWords3().size())
 		{
-			if(jter->second.cols == descriptors.cols &&
-				jter->second.type() == descriptors.type())
-			{
-				jter->second.copyTo(descriptors.row(index++));
-			}
-			else
-			{
-				valid = false;
-				ROS_ERROR("Some descriptors have different type/size! Cannot copy them...");
-			}
+			msg.wordPts.resize(signature.getWords3().size());
 		}
-
-		if(valid)
+		else
 		{
-			msg.wordDescriptors = rtabmap::compressData(descriptors);
+			ROS_ERROR("Word IDs and 3D points must have the same size (%d vs %d)!",
+					(int)signature.getWords().size(),
+					(int)signature.getWords3().size());
 		}
 	}
-	else if(signature.getWordsDescriptors().size())
+	if(!msg.wordKpts.empty() || !msg.wordPts.empty())
 	{
-		ROS_ERROR("Words and descriptors must have the same size (%d vs %d)!",
-				(int)signature.getWords().size(),
-				(int)signature.getWordsDescriptors().size());
+		for(size_t i=0; i<msg.wordIds.size(); ++i)
+		{
+			if(!msg.wordKpts.empty())
+				keypointToROS(signature.getWordsKpts().at(i), msg.wordKpts.at(i));
+			if(!msg.wordPts.empty())
+				point3fToROS(signature.getWords3().at(i), msg.wordPts[i]);
+		}
+	}
+
+	if(!signature.getWordsDescriptors().empty())
+	{
+		if(signature.getWordsDescriptors().rows == (int)signature.getWords().size())
+		{
+			msg.wordDescriptors = rtabmap::compressData(signature.getWordsDescriptors());
+		}
+		else
+		{
+			ROS_ERROR("Word IDs and descriptors must have the same size (%d vs %d)!",
+					(int)signature.getWords().size(),
+					signature.getWordsDescriptors().rows);
+		}
 	}
 
 	rtabmap_ros::globalDescriptorsToROS(signature.sensorData().globalDescriptors(), msg.globalDescriptors);

src/OdometryROS.cpp

@@ -498,11 +498,11 @@ void OdometryROS::callbackIMU(const sensor_msgs::ImuConstPtr& msg)
 		{
 			imus_.insert(std::make_pair(stamp, imu));
 			
-			if(bufferedData_.isValid() && stamp > bufferedData_.stamp())
+			if(bufferedData_.first.isValid() && stamp > bufferedData_.first.stamp())
 			{
-				SensorData data = bufferedData_;
-				bufferedData_ = SensorData();
-				processData(data, ros::Time(data.stamp()));
+				SensorData data = bufferedData_.first;
+				bufferedData_.first = SensorData();
+				processData(data, bufferedData_.second);
 			}
 			
 			if(imus_.size() > 1000)
@@ -528,11 +528,15 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
 			//NODELET_WARN("No imu received with higher stamp than last image (%f)! Buffering this image until we get more imu msgs...", stamp.toSec());
 		
 			// keep in cache to process later when we will receive imu msgs
-			if(bufferedData_.isValid())
+			if(bufferedData_.first.isValid())
 			{
-				NODELET_ERROR("Overwriting previous data! Make sure IMU is published faster than data rate. (last image stamp buffered=%f and new one is %f, last imu stamp received=%f)", bufferedData_.stamp(), data.stamp(), imus_.empty()?0:imus_.rbegin()->first);
+				NODELET_ERROR("Overwriting previous data! Make sure IMU is "
+						"published faster than data rate. (last image stamp "
+						"buffered=%f and new one is %f, last imu stamp received=%f)",
+						bufferedData_.first.stamp(), data.stamp(), imus_.empty()?0:imus_.rbegin()->first);
 			}
-			bufferedData_ = data;
+			bufferedData_.first = data;
+			bufferedData_.second = stamp;
 			return;
 		}
 		// process all imu data up to current image stamp (or just after so that underlying odom approach can do interpolation of imu at image stamp)
@@ -774,14 +778,14 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
 			// check which type of Odometry is using
 			if(odometry_->getType() == Odometry::kTypeF2M) // If it's Frame to Map Odometry
 			{
-				const std::multimap<int, cv::Point3f> & words3  = ((OdometryF2M*)odometry_)->getLastFrame().getWords3();
+				const std::vector<cv::Point3f> & words3  = ((OdometryF2M*)odometry_)->getLastFrame().getWords3();
 				if(words3.size())
 				{
 					pcl::PointCloud<pcl::PointXYZ> cloud;
-					for(std::multimap<int, cv::Point3f>::const_iterator iter=words3.begin(); iter!=words3.end(); ++iter)
+					for(std::vector<cv::Point3f>::const_iterator iter=words3.begin(); iter!=words3.end(); ++iter)
 					{
 						// transform to odom frame
-						cv::Point3f pt = util3d::transformPoint(iter->second, pose);
+						cv::Point3f pt = util3d::transformPoint(*iter, pose);
 						cloud.push_back(pcl::PointXYZ(pt.x, pt.y, pt.z));
 					}
 
@@ -799,10 +803,10 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
 				if(refFrame.getWords3().size())
 				{
 					pcl::PointCloud<pcl::PointXYZ> cloud;
-					for(std::multimap<int, cv::Point3f>::const_iterator iter=refFrame.getWords3().begin(); iter!=refFrame.getWords3().end(); ++iter)
+					for(std::vector<cv::Point3f>::const_iterator iter=refFrame.getWords3().begin(); iter!=refFrame.getWords3().end(); ++iter)
 					{
 						// transform to odom frame
-						cv::Point3f pt = util3d::transformPoint(iter->second, pose);
+						cv::Point3f pt = util3d::transformPoint(*iter, pose);
 						cloud.push_back(pcl::PointXYZ(pt.x, pt.y, pt.z));
 					}
 					sensor_msgs::PointCloud2 cloudMsg;
@@ -940,7 +944,7 @@ void OdometryROS::reset(const Transform & pose)
 	previousStamp_ = 0.0;
 	resetCurrentCount_ = resetCountdown_;
 	imuProcessed_ = false;
-	bufferedData_= SensorData();
+	bufferedData_.first= SensorData();
 	imus_.clear();
 	this->flushCallbacks();
 }