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)

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),
-	lastImuReceivedStamp_(0.0)
+	imuProcessed_(false)
 {
 
 }
@@ -497,34 +496,38 @@ void OdometryROS::callbackIMU(const sensor_msgs::ImuConstPtr& msg)
 		}
 		else
 		{
-			SensorData data(imu, 0, stamp);
-			this->processData(data, msg->header.stamp);
-			imuProcessed_ = true;
-			lastImuReceivedStamp_ = stamp;
-
-			if(bufferedData_.isValid() && stamp >= bufferedData_.stamp())
+			imus_.insert(std::make_pair(stamp, imu));
+			
+			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(!data.imageRaw().empty() || !data.laserScanRaw().isEmpty())
+	if(odometry_->canProcessIMU())
 	{
-		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 "