0.17.4 (added optional imu input topic to stereo odometry when a vio approach is selected, see Odom/Strategy parameter)

CMakeLists.txt

@@ -1,6 +1,17 @@
 cmake_minimum_required(VERSION 2.8.3)
 project(rtabmap_ros)
 
+# Policy CMP0043 introduced in cmake version 3.0 IGNORES the use of COMPILE_DEFINITIONS suffixed variables, e.g. COMPILE_DEFINITIONS_DEBUG
+# Set to OLD behavior until minimum cmake version >= 2.8.10 (version that COMPILE_DEFINITIONS can be set by generator expressions instead)
+if (POLICY CMP0043)
+	cmake_policy(SET CMP0043 OLD)
+endif (POLICY CMP0043)
+# Policy CMP0042 introduced in cmake version 3.0 enables the use of @rpath in an install name via MACOSX_RPATH by default
+# Set to OLD behavior so that all versions use the same behavior, or until minimum cmake version >= 2.8.12 (version where @rpath is available)
+if (POLICY CMP0042)
+	cmake_policy(SET CMP0042 OLD)
+endif (POLICY CMP0042)
+
 ## Find catkin macros and libraries
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
 ## is used, also find other catkin packages
@@ -19,7 +30,7 @@ find_package(find_object_2d)
 
 ## System dependencies are found with CMake's conventions
 # find_package(Boost REQUIRED COMPONENTS system)
-find_package(RTABMap 0.17.2 REQUIRED)
+find_package(RTABMap 0.17.4 REQUIRED)
 
 find_package(OpenCV REQUIRED)
 

launch/rtabmap.launch

@@ -91,7 +91,8 @@
   <arg name="odom_sensor_sync"         default="false"/>
   <arg name="odom_guess_frame_id"        default=""/>
   <arg name="odom_guess_min_translation" default="0"/>
-  <arg name="odom_guess_min_rotation"    default="0"/>  
+  <arg name="odom_guess_min_rotation"    default="0"/> 
+  <arg name="imu_topic"                default="/imu"/>          <!-- only used with VIO approaches -->
   
   <arg name="subscribe_user_data"      default="false"/>             <!-- user data synchronized subscription -->
   <arg name="user_data_topic"          default="/user_data"/>
@@ -190,6 +191,7 @@
           <remap     if="$(arg rgbd_sync)" from="rgbd_image" to="$(arg rgbd_topic)"/>
           <remap unless="$(arg rgbd_sync)" from="rgbd_image" to="$(arg rgbd_topic)_relay"/>
           <remap from="odom"                   to="$(arg odom_topic)"/>
+          <remap from="imu"                    to="$(arg imu_topic)"/>
       
           <param name="frame_id"                    type="string" value="$(arg frame_id)"/>
           <param name="odom_frame_id"               type="string" value="$(arg vo_frame_id)"/>
@@ -227,7 +229,7 @@
   
     <!-- Visual SLAM (robot side) -->
     <!-- args: "delete_db_on_start" and "udebug" -->
-    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="$(arg output)" args="$(arg rtabmap_args)" launch-prefix="$(arg launch_prefix)">
+    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="$(arg output)" args="$(arg rtabmap_args)" launch-prefix="">
       <param     if="$(arg stereo)" name="subscribe_depth"  type="bool" value="false"/>
       <param unless="$(arg stereo)" name="subscribe_depth"  type="bool" value="true"/>
       <param name="subscribe_rgbd"       type="bool"   value="$(arg subscribe_rgbd)"/>

launch/tests/euroc_datasets.launch

@@ -2,6 +2,12 @@
 
 <param name="use_sim_time" value="true"/>
 
+<arg name="cfg" default=""/>
+<arg name="MH_seq" default="false"/> <!-- For MH sequences, the ground truth is coming from a different topic -->
+<arg name="rtabmapviz" default="true"/>
+<arg name="rviz" default="false"/>
+
+<!-- Image rectification and publishing synchronized camera_info-->
 <group ns="stereo_camera">
 
   <node pkg="rtabmap_ros" type="yaml_to_camera_info.py" name="yaml_to_camera_info_left">
@@ -22,21 +28,36 @@
 
 </group>
 
-<node pkg="tf" type="static_transform_publisher" name="camera_base_link" args="0.009811 0.064677 -0.021640 -1.596576 -0.014866 -1.566656 base_link cam0 100"/>  
-<node pkg="tf" type="static_transform_publisher" name="leica_base_link" args="0.120209 -0.0184772 -0.0748903 0 0 0 leica base_link_gt 100"/>  
+<!-- TF frames -->
+<node pkg="tf" type="static_transform_publisher" name="imu_base_link" args="0 0 0 3.1415926 -1.570796 0 base_link imu4 5"/>
+<node pkg="tf" type="static_transform_publisher" name="camera_imu_link" args="-0.021640 -0.064677 0.009811 1.555925 0.025777 0.003757 imu4 cam0 50"/>  
+<node     if="$(arg MH_seq)" pkg="tf" type="static_transform_publisher" name="leica_base_link" args="0.120209 -0.0184772 -0.0748903 0 0 0 leica base_link_gt 100"/> 
+<node unless="$(arg MH_seq)" pkg="tf" type="static_transform_publisher" name="vicon_base_link" args="0.120209 -0.0184772 -0.0748903 0 0 0 vicon/firefly_sbx/firefly_sbx base_link_gt 100"/>
 
-<node pkg="rtabmap_ros" type="point_to_tf.py" name="point_to_tf">
+<node     if="$(arg MH_seq)" pkg="rtabmap_ros" type="point_to_tf.py" name="point_to_tf">
   <remap from="point" to="/leica/position"/>
   <param name="frame_id" value="leica"/>
   <param name="fixed_frame_id" value="world"/>
 </node>
+<node unless="$(arg MH_seq)" pkg="rtabmap_ros" type="transform_to_tf.py" name="transform_to_tf">
+  <remap from="transform" to="/vicon/firefly_sbx/firefly_sbx"/>
+  <param name="frame_id" value="world"/>
+  <param name="child_frame_id" value="vicon/firefly_sbx/firefly_sbx"/>
+</node>
+<node pkg="tf" type="static_transform_publisher" name="world_to_map" args="0.0 0.0 0.0 0.0 0.0 0.0 /world /map 100" />
 
+<!-- RTAB-Map -->
 <include file="$(find rtabmap_ros)/launch/rtabmap.launch">
    <arg name="rtabmap_args" value="--delete_db_on_start --RGBD/CreateOccupancyGrid false --Rtabmap/DetectionRate 1"/>
    <arg name="stereo" value="true"/>
    <arg name="frame_id" value="base_link"/>
+   <arg name="wait_for_transform" value="0.01"/>
    <arg name="ground_truth_frame_id" value="world"/>
    <arg name="ground_truth_base_frame_id" value="base_link_gt"/>
+   <arg name="cfg" value="$(arg cfg)"/>
+   <arg name="imu_topic" value="/imu0"/>
+   <arg name="rtabmapviz" value="$(arg rtabmapviz)"/>
+   <arg name="rviz" value="$(arg rviz)"/>
 </include>
 
 </launch>

package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package>
   <name>rtabmap_ros</name>
-  <version>0.17.2</version>
+  <version>0.17.4</version>
   <description>RTAB-Map's ros-pkg. RTAB-Map is a RGB-D SLAM approach with real-time constraints.</description>
   <maintainer email="matlabbe@gmail.com">Mathieu Labbe</maintainer>
   <author>Mathieu Labbe</author>

scripts/point_to_tf.py

@@ -18,7 +18,7 @@ def callback(point):
 
 if __name__ == "__main__":
 
-    rospy.init_node("yaml_to_camera_info", anonymous=True)
+    rospy.init_node("point_to_tf", anonymous=True)
 
     frame_id = rospy.get_param('~frame_id', 'point')
     fixed_frame_id = rospy.get_param('~fixed_frame_id', 'world')

scripts/transform_to_tf.py

@@ -0,0 +1,32 @@
+#!/usr/bin/env python
+import rospy
+import tf
+from geometry_msgs.msg import TransformStamped
+
+def callback(transform):
+    global br
+    global frame_id
+    global child_frame_id
+    local_frame_id = transform.header.frame_id
+    local_child_frame_id = transform.child_frame_id
+    if not local_frame_id:
+        local_frame_id = frame_id
+    if not local_child_frame_id:
+        local_child_frame_id = child_frame_id
+    br.sendTransform(
+        (transform.transform.translation.x, transform.transform.translation.y, transform.transform.translation.z),
+        (transform.transform.rotation.x, transform.transform.rotation.y, transform.transform.rotation.z, transform.transform.rotation.w),
+        transform.header.stamp,
+        child_frame_id,
+        frame_id)
+
+if __name__ == "__main__":
+
+    rospy.init_node("transform_to_tf", anonymous=True)
+
+    frame_id = rospy.get_param('~frame_id', 'world')
+    child_frame_id = rospy.get_param('~child_frame_id', 'transform')
+
+    br = tf.TransformBroadcaster()
+    rospy.Subscriber("transform", TransformStamped, callback, queue_size=1)
+    rospy.spin()

src/OdometryROS.cpp

@@ -192,6 +192,7 @@ void OdometryROS::onInit()
 
 	//parameters
 	parameters_ = Parameters::getDefaultOdometryParameters(stereoParams_, visParams_, icpParams_);
+	parameters_.insert(*Parameters::getDefaultParameters().find(Parameters::kRtabmapImagesAlreadyRectified()));
 	if(!configPath.empty())
 	{
 		if(UFile::exists(configPath.c_str()))
@@ -377,24 +378,27 @@ Transform OdometryROS::getTransform(const std::string & fromFrameId, const std::
 
 void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
 {
-	if(odometry_->getPose().isIdentity() &&
-	   !groundTruthFrameId_.empty())
+	if(!data.imageRaw().empty())
 	{
-		// sync with the first value of the ground truth
-		Transform initialPose = getTransform(groundTruthFrameId_, groundTruthBaseFrameId_, stamp);
-		if(initialPose.isNull())
+		if(odometry_->getPose().isIdentity() &&
+		   !groundTruthFrameId_.empty())
 		{
-			NODELET_WARN("Ground truth frames \"%s\" -> \"%s\" are set but failed to "
-					"get them, odometry won't be synchronized with ground truth.",
-					groundTruthFrameId_.c_str(), groundTruthBaseFrameId_.c_str());
-		}
-		else
-		{
-			NODELET_INFO( "Initializing odometry pose to %s (from \"%s\" -> \"%s\")",
-					initialPose.prettyPrint().c_str(),
-					groundTruthFrameId_.c_str(),
-					groundTruthBaseFrameId_.c_str());
-			odometry_->reset(initialPose);
+			// sync with the first value of the ground truth
+			Transform initialPose = getTransform(groundTruthFrameId_, groundTruthBaseFrameId_, stamp);
+			if(initialPose.isNull())
+			{
+				NODELET_WARN("Ground truth frames \"%s\" -> \"%s\" are set but failed to "
+						"get them, odometry won't be synchronized with ground truth.",
+						groundTruthFrameId_.c_str(), groundTruthBaseFrameId_.c_str());
+			}
+			else
+			{
+				NODELET_INFO( "Initializing odometry pose to %s (from \"%s\" -> \"%s\")",
+						initialPose.prettyPrint().c_str(),
+						groundTruthFrameId_.c_str(),
+						groundTruthBaseFrameId_.c_str());
+				odometry_->reset(initialPose);
+			}
 		}
 	}
 
@@ -617,6 +621,10 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
 			odomLocalScanMap_.publish(cloudMsg);
 		}
 	}
+	else if(data.imageRaw().empty() && !data.imu().empty())
+	{
+		return;
+	}
 	else if(publishNullWhenLost_)
 	{
 		//NODELET_WARN( "Odometry lost!");
@@ -676,22 +684,24 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
 		odomInfoPub_.publish(infoMsg);
 	}
 
-	if(visParams_)
+	if(!data.imageRaw().empty())
 	{
-		if(icpParams_)
+		if(visParams_)
 		{
-			NODELET_INFO( "Odom: quality=%d, ratio=%f, std dev=%fm|%frad, update time=%fs", info.reg.inliers, info.reg.icpInliersRatio, pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at<double>(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at<double>(5,5)), (ros::WallTime::now()-time).toSec());
+			if(icpParams_)
+			{
+				NODELET_INFO( "Odom: quality=%d, ratio=%f, std dev=%fm|%frad, update time=%fs", info.reg.inliers, info.reg.icpInliersRatio, pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at<double>(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at<double>(5,5)), (ros::WallTime::now()-time).toSec());
+			}
+			else
+			{
+				NODELET_INFO( "Odom: quality=%d, std dev=%fm|%frad, update time=%fs", info.reg.inliers, pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at<double>(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at<double>(5,5)), (ros::WallTime::now()-time).toSec());
+			}
 		}
 		else
 		{
-			NODELET_INFO( "Odom: quality=%d, std dev=%fm|%frad, update time=%fs", info.reg.inliers, pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at<double>(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at<double>(5,5)), (ros::WallTime::now()-time).toSec());
+			NODELET_INFO( "Odom: ratio=%f, std dev=%fm|%frad, update time=%fs", info.reg.icpInliersRatio, pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at<double>(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at<double>(5,5)), (ros::WallTime::now()-time).toSec());
 		}
 	}
-	else
-	{
-		NODELET_INFO( "Odom: ratio=%f, std dev=%fm|%frad, update time=%fs", info.reg.icpInliersRatio, pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at<double>(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at<double>(5,5)), (ros::WallTime::now()-time).toSec());
-	}
-
 }
 
 bool OdometryROS::reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&)

src/nodelets/stereo_odometry.cpp

@@ -38,6 +38,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <sensor_msgs/Image.h>
 #include <sensor_msgs/image_encodings.h>
+#include <sensor_msgs/Imu.h>
 
 #include <image_geometry/stereo_camera_model.h>
 
@@ -49,6 +50,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UTimer.h>
 #include <rtabmap/utilite/UStl.h>
 #include <rtabmap/utilite/UConversion.h>
+#include <rtabmap/core/Odometry.h>
 
 using namespace rtabmap;
 
@@ -140,6 +142,15 @@ private:
 					cameraInfoLeft_.getTopic().c_str(),
 					cameraInfoRight_.getTopic().c_str());
 		}
+
+		int odomStrategy = 0;
+		Parameters::parse(this->parameters(), Parameters::kOdomStrategy(), odomStrategy);
+		if(odomStrategy == Odometry::kTypeOkvis || odomStrategy == Odometry::kTypeMSCKF)
+		{
+			imuSub_ = nh.subscribe("imu", queueSize_*5, &StereoOdometry::callbackIMU, this);
+			NODELET_INFO("VIO approach selected, subscribing to IMU topic %s", imuSub_.getTopic().c_str());
+		}
+
 		this->startWarningThread(subscribedTopicsMsg, approxSync);
 	}
 
@@ -185,8 +196,6 @@ private:
 				return;
 			}
 
-			ros::WallTime time = ros::WallTime::now();
-
 			int quality = -1;
 			if(imageRectLeft->data.size() && imageRectRight->data.size())
 			{
@@ -312,6 +321,36 @@ private:
 		}
 	}
 
+	void callbackIMU(
+			const sensor_msgs::ImuConstPtr& msg)
+	{
+		if(!this->isPaused())
+		{
+			double stamp = msg->header.stamp.toSec();
+			rtabmap::Transform localTransform = rtabmap::Transform::getIdentity();
+			if(this->frameId().compare(msg->header.frame_id) != 0)
+			{
+				localTransform = getTransform(this->frameId(), msg->header.frame_id, msg->header.stamp);
+			}
+			if(localTransform.isNull())
+			{
+				ROS_ERROR("Could not transform IMU msg from frame \"%s\" to frame \"%s\", TF not available at time %f",
+						msg->header.frame_id.c_str(), this->frameId().c_str(), stamp);
+				return;
+			}
+
+			IMU imu(
+					cv::Vec3d(msg->angular_velocity.x, msg->angular_velocity.y, msg->angular_velocity.z),
+					cv::Mat(3,3,CV_64FC1,(void*)msg->angular_velocity_covariance.data()).clone(),
+					cv::Vec3d(msg->linear_acceleration.x, msg->linear_acceleration.y, msg->linear_acceleration.z),
+					cv::Mat(3,3,CV_64FC1,(void*)msg->linear_acceleration_covariance.data()).clone(),
+					localTransform);
+
+			SensorData data(imu, 0, stamp);
+			this->processData(data, msg->header.stamp);
+		}
+	}
+
 protected:
 	virtual void flushCallbacks()
 	{
@@ -340,6 +379,7 @@ private:
 	typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo> MyExactSyncPolicy;
 	message_filters::Synchronizer<MyExactSyncPolicy> * exactSync_;
 	ros::Subscriber rgbdSub_;
+	ros::Subscriber imuSub_;
 	int queueSize_;
 };