From 547c3cbd7626b1b18aeb588224c920a27d0e8e26 Mon Sep 17 00:00:00 2001
From: matlabbe <matlabbe@gmail.com>
Date: Wed, 13 Nov 2019 16:54:06 -0500
Subject: [PATCH] rtabmap: Added inter_odom optional input topic (available
 only if Rtabmap/DetectionRate=0 and Rtabmap/CreateIntermediateNodes=true)

---
 include/rtabmap_ros/CoreWrapper.h |  3 ++
 src/CoreWrapper.cpp               | 82 +++++++++++++++++++++++++++++++
 2 files changed, 85 insertions(+)

diff --git a/include/rtabmap_ros/CoreWrapper.h b/include/rtabmap_ros/CoreWrapper.h
index b8a18a1a..5bd6681c 100644
--- a/include/rtabmap_ros/CoreWrapper.h
+++ b/include/rtabmap_ros/CoreWrapper.h
@@ -143,6 +143,7 @@ private:
 	void tagDetectionsAsyncCallback(const apriltag_ros::AprilTagDetectionArray & tagDetections);
 #endif
 	void imuAsyncCallback(const sensor_msgs::ImuConstPtr & tagDetections);
+	void interOdomCallback(const nav_msgs::OdometryConstPtr & msg);
 
 	void initialPoseCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr & msg);
 
@@ -319,6 +320,8 @@ private:
 	std::map<int, geometry_msgs::PoseWithCovarianceStamped> tags_;
 	ros::Subscriber imuSub_;
 	std::map<double, rtabmap::Transform> imus_;
+	ros::Subscriber interOdomSub_;
+	std::list<nav_msgs::Odometry> interOdoms_;
 
 	bool stereoToDepth_;
 	bool odomSensorSync_;
diff --git a/src/CoreWrapper.cpp b/src/CoreWrapper.cpp
index e812e1e9..fe922c92 100644
--- a/src/CoreWrapper.cpp
+++ b/src/CoreWrapper.cpp
@@ -516,6 +516,11 @@ void CoreWrapper::onInit()
 		if(createIntermediateNodes_)
 		{
 			NODELET_INFO("Create intermediate nodes");
+			if(rate_ == 0.0f)
+			{
+				NODELET_INFO("Subscribe to inter odom messges");
+				interOdomSub_ = nh.subscribe("inter_odom", 1, &CoreWrapper::interOdomCallback, this);
+			}
 		}
 	}
 	if(parameters_.find(Parameters::kGridGlobalMaxNodes()) != parameters_.end())
@@ -1632,6 +1637,75 @@ void CoreWrapper::process(
 	UTimer timer;
 	if(rtabmap_.isIDsGenerated() || data.id() > 0)
 	{
+		// Add intermediate nodes?
+		for(std::list<nav_msgs::Odometry>::iterator iter=interOdoms_.begin(); iter!=interOdoms_.end();)
+		{
+			if(iter->header.stamp < lastPoseStamp_)
+			{
+				Transform interOdom = rtabmap_ros::transformFromPoseMsg(iter->pose.pose);
+				if(!interOdom.isNull())
+				{
+					cv::Mat covariance;
+					double variance = iter->twist.covariance[0];
+					if(variance == BAD_COVARIANCE || variance <= 0.0f)
+					{
+						//use the one of the pose
+						covariance = cv::Mat(6,6,CV_64FC1, (void*)iter->pose.covariance.data()).clone();
+						covariance /= 2.0;
+					}
+					else
+					{
+						covariance = cv::Mat(6,6,CV_64FC1, (void*)iter->twist.covariance.data()).clone();
+					}
+					if(!uIsFinite(covariance.at<double>(0,0)) || covariance.at<double>(0,0)<=0.0f)
+					{
+						covariance = cv::Mat::eye(6,6,CV_64FC1);
+						if(odomDefaultLinVariance_ > 0.0f)
+						{
+							covariance.at<double>(0,0) = odomDefaultLinVariance_;
+							covariance.at<double>(1,1) = odomDefaultLinVariance_;
+							covariance.at<double>(2,2) = odomDefaultLinVariance_;
+						}
+						if(odomDefaultAngVariance_ > 0.0f)
+						{
+							covariance.at<double>(3,3) = odomDefaultAngVariance_;
+							covariance.at<double>(4,4) = odomDefaultAngVariance_;
+							covariance.at<double>(5,5) = odomDefaultAngVariance_;
+						}
+					}
+
+					cv::Mat rgb = cv::Mat::zeros(2,1,CV_8UC1);
+					cv::Mat depth = cv::Mat::zeros(2,1,CV_16UC1);
+					CameraModel model(
+							1,
+							1,
+							0.5,
+							1,
+							Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0),
+							0,
+							cv::Size(1,2));
+					SensorData interData(rgb, depth, model, -1, rtabmap_ros::timestampFromROS(iter->header.stamp));
+					Transform gt;
+					if(!groundTruthFrameId_.empty())
+					{
+						gt = rtabmap_ros::getTransform(groundTruthFrameId_, groundTruthBaseFrameId_, iter->header.stamp, tfListener_, waitForTransform_?waitForTransformDuration_:0.0);
+					}
+					interData.setGroundTruth(gt);
+					rtabmap_.process(interData, interOdom, covariance);
+				}
+				interOdoms_.erase(iter++);
+			}
+			else if(iter->header.stamp == lastPoseStamp_)
+			{
+				interOdoms_.erase(iter++);
+				break;
+			}
+			else
+			{
+				break;
+			}
+		}
+
 		//Add async stuff
 		Transform groundTruthPose;
 		if(!groundTruthFrameId_.empty())
@@ -2103,6 +2177,14 @@ void CoreWrapper::imuAsyncCallback(const sensor_msgs::ImuConstPtr & msg)
 	}
 }
 
+void CoreWrapper::interOdomCallback(const nav_msgs::OdometryConstPtr & msg)
+{
+	if(!paused_)
+	{
+		interOdoms_.push_back(*msg);
+	}
+}
+
 
 void CoreWrapper::initialPoseCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr & msg)
 {