OdometryROS: refactored imu sync (#331). Using tf to compute baseline if Rtabmap/ImagesAlreadyRectified is true (for convenience with realsense rectified IR images but right camera info Tx not set)

include/rtabmap_ros/CoreWrapper.h

@@ -358,6 +358,7 @@ private:
 	float rate_;
 	bool createIntermediateNodes_;
 	int maxMappingNodes_;
+	bool alreadyRectifiedImages_;
 	ros::Time previousStamp_;
 };
 

include/rtabmap_ros/MsgConversion.h

@@ -227,7 +227,8 @@ bool convertStereoMsg(
 		cv::Mat & right,
 		rtabmap::StereoCameraModel & stereoModel,
 		tf::TransformListener & listener,
-		double waitForTransform);
+		double waitForTransform,
+		bool alreadyRectified);
 
 bool convertScanMsg(
 		const sensor_msgs::LaserScan & scan2dMsg,

include/rtabmap_ros/OdometryROS.h

@@ -141,7 +141,7 @@ private:
 	int odomStrategy_;
 	bool waitIMUToinit_;
 	bool imuProcessed_;
-	double lastImuReceivedStamp_;
+	std::map<double, rtabmap::IMU> imus_;
 	rtabmap::SensorData bufferedData_;
 };
 

launch/tests/test_d435i_vio.launch

@@ -5,10 +5,18 @@
 <arg name="rtabmapviz" default="true"/>
 <arg name="rviz"       default="false"/>
 <arg name="depth_mode" default="true"/>
+<arg name="unite_imu_method" default="copy"/> <!-- "copy" or "linear_interpolation" -->
 
 <include file="$(find realsense2_camera)/launch/rs_camera.launch">
-   <arg name="align_depth" value="true"/>
+   <arg name="align_depth"   value="$(arg depth_mode)"/>
-   <arg name="unite_imu_method" value="linear_interpolation"/>
+   <arg name="unite_imu_method" value="$(arg unite_imu_method)"/> 
+   <arg name="enable_gyro"   value="true"/>
+   <arg name="enable_accel"  value="true"/>
+   <arg name="enable_infra1" value="true"/>
+   <arg name="enable_infra2" value="true"/>
+   <arg name="gyro_fps"      value="200"/>
+   <arg name="accel_fps"     value="250"/>
+   <arg name="enable_sync"   value="true"/>
 </include>
 
 <node pkg="imu_filter_madgwick" type="imu_filter_node" name="imu_filter_node">
@@ -19,17 +27,15 @@
    <remap from="/imu/data"     to="/rtabmap/imu"/>
 </node>
 
-<node pkg="topic_tools" type="throttle" name="imu_throttle" args="messages /rtabmap/imu 100.0"/>
-
 <!-- 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 --Rtabmap/ImagesAlreadyRectified false" output="screen">
+<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">
    <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"/>
    <remap from="right/camera_info" to="/camera/infra2/camera_info"/>
-   <remap from="imu"               to="/rtabmap/imu_throttle"/>
+   <remap from="imu"               to="/rtabmap/imu"/>
    <param name="frame_id"          value="camera_link"/>
    <param name="wait_imu_to_init"  value="true"/>
 </node>
@@ -40,8 +46,9 @@
    <arg name="depth_topic"       value="/camera/aligned_depth_to_color/image_raw"/>
    <arg name="camera_info_topic" value="/camera/color/camera_info"/>
    <arg name="visual_odometry"   value="false"/>
+   <arg name="approx_sync"       value="false"/>
    <arg name="frame_id"          value="camera_link"/>
-   <arg name="imu_topic"         value="/rtabmap/imu_throttle"/>
+   <arg name="imu_topic"         value="/rtabmap/imu"/>
    <arg name="rtabmapviz"        value="$(arg rtabmapviz)"/>
    <arg name="rviz"              value="$(arg rviz)"/>
 </include>
@@ -49,14 +56,13 @@
 <!-- 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="odom_args"               value="--Rtabmap/ImagesAlreadyRectified false"/>
    <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"/>
    <arg name="right_camera_info_topic" value="/camera/infra2/camera_info"/>
    <arg name="stereo"                  value="true"/>
    <arg name="frame_id"                value="camera_link"/>
-   <arg name="imu_topic"               value="/rtabmap/imu_throttle"/>
+   <arg name="imu_topic"               value="/rtabmap/imu"/>
    <arg name="wait_imu_to_init"        value="true"/>
    <arg name="rtabmapviz"              value="$(arg rtabmapviz)"/>
    <arg name="rviz"                    value="$(arg rviz)"/>

python/rtabmap_ros/.gitignore

@@ -0,0 +1 @@
+*.pyc

src/CoreWrapper.cpp

@@ -115,6 +115,7 @@ CoreWrapper::CoreWrapper() :
 		rate_(Parameters::defaultRtabmapDetectionRate()),
 		createIntermediateNodes_(Parameters::defaultRtabmapCreateIntermediateNodes()),
 		maxMappingNodes_(Parameters::defaultGridGlobalMaxNodes()),
+		alreadyRectifiedImages_(Parameters::defaultRtabmapImagesAlreadyRectified()),
 		previousStamp_(0),
 		mbClient_(0)
 {
@@ -544,6 +545,10 @@ void CoreWrapper::onInit()
 			NODELET_INFO("Max mapping nodes = %d", maxMappingNodes_);
 		}
 	}
+	if(parameters_.find(Parameters::kRtabmapImagesAlreadyRectified()) != parameters_.end())
+	{
+		Parameters::parse(parameters_, Parameters::kRtabmapImagesAlreadyRectified(), alreadyRectifiedImages_);
+	}
 
 	if(paused_)
 	{
@@ -1334,7 +1339,8 @@ void CoreWrapper::commonStereoCallback(
 			right,
 			stereoModel,
 			tfListener_,
-			waitForTransform_?waitForTransformDuration_:0.0))
+			waitForTransform_?waitForTransformDuration_:0.0,
+			alreadyRectifiedImages_))
 	{
 		NODELET_ERROR("Could not convert stereo msgs! Aborting rtabmap update...");
 		return;

src/GuiWrapper.cpp

@@ -693,7 +693,8 @@ void GuiWrapper::commonStereoCallback(
 				right,
 				stereoModel,
 				tfListener_,
-				waitForTransform_?waitForTransformDuration_:0.0))
+				waitForTransform_?waitForTransformDuration_:0.0,
+				true))
 		{
 			ROS_ERROR("Could not convert stereo msgs! Aborting rtabmapviz update...");
 			return;

src/MsgConversion.cpp

@@ -1767,7 +1767,8 @@ bool convertStereoMsg(
 		cv::Mat & right,
 		rtabmap::StereoCameraModel & stereoModel,
 		tf::TransformListener & listener,
-		double waitForTransform)
+		double waitForTransform,
+		bool alreadyRectified)
 {
 	UASSERT(leftImageMsg.get() && rightImageMsg.get());
 
@@ -1843,6 +1844,41 @@ bool convertStereoMsg(
 			shown = true;
 		}
 	}
+	else if(stereoModel.baseline() == 0 && alreadyRectified)
+	{
+		rtabmap::Transform stereoTransform = getTransform(
+				leftCamInfoMsg.header.frame_id,
+				rightCamInfoMsg.header.frame_id,
+				leftCamInfoMsg.header.stamp,
+				listener,
+				waitForTransform);
+		if(stereoTransform.isNull() || stereoTransform.x()<=0)
+		{
+			ROS_WARN("We cannot estimated the baseline of the rectified images with tf! (%s->%s = %s)",
+					rightCamInfoMsg.header.frame_id.c_str(), leftCamInfoMsg.header.frame_id.c_str(), stereoTransform.prettyPrint().c_str());
+		}
+		else
+		{
+			static bool warned = false;
+			if(!warned)
+			{
+				ROS_WARN("Right camera info doesn't have Tx set but we are assuming that stereo images are already rectified (see %s parameter). While not "
+						"recommended, we used TF to get the baseline (%s->%s = %fm) for convenience (e.g., D400 ir stereo issue). It is preferred to feed "
+						"a valid right camera info if stereo images are already rectified. This message is only printed once...",
+						rtabmap::Parameters::kRtabmapImagesAlreadyRectified().c_str(),
+						rightCamInfoMsg.header.frame_id.c_str(), leftCamInfoMsg.header.frame_id.c_str(), stereoTransform.x());
+				warned = true;
+			}
+			stereoModel = rtabmap::StereoCameraModel(
+					stereoModel.left().fx(),
+					stereoModel.left().fy(),
+					stereoModel.left().cx(),
+					stereoModel.left().cy(),
+					stereoTransform.x(),
+					stereoModel.localTransform(),
+					stereoModel.left().imageSize());
+		}
+	}
 	return true;
 }
 

src/OdometryROS.cpp

@@ -83,8 +83,7 @@ OdometryROS::OdometryROS(bool stereoParams, bool visParams, bool icpParams) :
 	maxUpdateRate_(0.0),
 	odomStrategy_(Parameters::defaultOdomStrategy()),
 	waitIMUToinit_(false),
-	imuProcessed_(false),
+	imuProcessed_(false)
-	lastImuReceivedStamp_(0.0)
 {
 
 }
@@ -497,34 +496,38 @@ void OdometryROS::callbackIMU(const sensor_msgs::ImuConstPtr& msg)
 		}
 		else
 		{
-			SensorData data(imu, 0, stamp);
+			imus_.insert(std::make_pair(stamp, imu));
-			this->processData(data, msg->header.stamp);
+			
-			imuProcessed_ = true;
+			if(bufferedData_.isValid() && stamp > bufferedData_.stamp())
-			lastImuReceivedStamp_ = stamp;
-
-			if(bufferedData_.isValid() && stamp >= bufferedData_.stamp())
 			{
-				processData(bufferedData_, ros::Time(bufferedData_.stamp()));
+				SensorData data = bufferedData_;
+				bufferedData_ = SensorData();
+				processData(data, ros::Time(data.stamp()));
+			}
+			
+			if(imus_.size() > 1000)
+			{
+				imus_.erase(imus_.begin());
 			}
-			bufferedData_ = SensorData();
 		}
 	}
 }
 
 void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
 {
-	if((waitIMUToinit_ && !imuProcessed_) && odometry_->framesProcessed() == 0 && odometry_->getPose().isIdentity() && data.imu().empty())
+	if((waitIMUToinit_ && !imuProcessed_) && odometry_->framesProcessed() == 0 && odometry_->getPose().isIdentity() && imus_.empty())
 	{
 		NODELET_WARN("odometry: waiting imu to initialize orientation (wait_imu_to_init=true)");
 		return;
 	}
 
-	Transform groundTruth;
+	if(odometry_->canProcessIMU())
-	if(!data.imageRaw().empty() || !data.laserScanRaw().isEmpty())
 	{
-		if(odometry_->canProcessIMU() && data.imu().empty() && lastImuReceivedStamp_>0.0 && data.stamp() > lastImuReceivedStamp_)
+		if(waitIMUToinit_ && (imus_.empty() || imus_.rbegin()->first<stamp.toSec()))
 		{
-			//NODELET_WARN("Data received is more recent than last imu received, waiting for imu update to process it.");
+			//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())
 			{
 				NODELET_ERROR("Overwriting previous data! Make sure IMU is published faster than data rate.");
@@ -532,7 +535,26 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
 			bufferedData_ = data;
 			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)
+		std::map<double, rtabmap::IMU>::iterator iterEnd = imus_.lower_bound(stamp.toSec());
+		if(iterEnd!= imus_.end())
+		{
+			++iterEnd;
+		}
+		for(std::map<double, rtabmap::IMU>::iterator iter=imus_.begin(); iter!=iterEnd;)
+		{
+			//NODELET_WARN("img callback: process imu   %f", iter->first);
+			SensorData dataIMU(iter->second, 0, iter->first);
+			odometry_->process(dataIMU);
+			imus_.erase(iter++);
+			imuProcessed_ = true;
+		}
+	}
+	//NODELET_WARN("img callback: process image %f", stamp.toSec());
 
+	Transform groundTruth;
+	if(!data.imageRaw().empty() || !data.laserScanRaw().isEmpty())
+	{
 		if(previousStamp_>0.0 && previousStamp_ >= stamp.toSec())
 		{
 			NODELET_WARN("Odometry: Detected not valid consecutive stamps (previous=%fs new=%fs). New stamp should be always greater than previous stamp. This new data is ignored. This message will appear only once.",
@@ -917,7 +939,7 @@ void OdometryROS::reset(const Transform & pose)
 	resetCurrentCount_ = resetCountdown_;
 	imuProcessed_ = false;
 	bufferedData_= SensorData();
-	lastImuReceivedStamp_=0.0;
+	imus_.clear();
 	this->flushCallbacks();
 }
 

src/nodelets/stereo_odometry.cpp

@@ -212,6 +212,36 @@ private:
 
 				rtabmap::StereoCameraModel stereoModel = rtabmap_ros::stereoCameraModelFromROS(*cameraInfoLeft, *cameraInfoRight, localTransform, stereoTransform);
 
+				if(stereoModel.baseline() == 0 && alreadyRectified)
+				{
+					stereoTransform = getTransform(
+							cameraInfoLeft->header.frame_id,
+							cameraInfoRight->header.frame_id,
+							cameraInfoLeft->header.stamp);
+
+					if(!stereoTransform.isNull() && stereoTransform.x()>0)
+					{
+						static bool warned = false;
+						if(!warned)
+						{
+							ROS_WARN("Right camera info doesn't have Tx set but we are assuming that stereo images are already rectified (see %s parameter). While not "
+									"recommended, we used TF to get the baseline (%s->%s = %fm) for convenience (e.g., D400 ir stereo issue). It is preferred to feed "
+									"a valid right camera info if stereo images are already rectified. This message is only printed once...",
+									rtabmap::Parameters::kRtabmapImagesAlreadyRectified().c_str(),
+									cameraInfoRight->header.frame_id.c_str(), cameraInfoLeft->header.frame_id.c_str(), stereoTransform.x());
+							warned = true;
+						}
+						stereoModel = rtabmap::StereoCameraModel(
+								stereoModel.left().fx(),
+								stereoModel.left().fy(),
+								stereoModel.left().cx(),
+								stereoModel.left().cy(),
+								stereoTransform.x(),
+								stereoModel.localTransform(),
+								stereoModel.left().imageSize());
+					}
+				}
+
 				if(alreadyRectified && stereoModel.baseline() <= 0)
 				{
 					NODELET_ERROR("The stereo baseline (%f) should be positive (baseline=-Tx/fx). We assume a horizontal left/right stereo "