added test_d435i_vio.launch example (VINS-Fusion integration)

include/rtabmap_ros/OdometryROS.h

@@ -139,6 +139,7 @@ private:
 	double expectedUpdateRate_;
 	int odomStrategy_;
 	bool waitIMUToinit_;
+	bool imuProcessed_;
 };
 
 }

launch/tests/euroc_datasets.launch

@@ -11,23 +11,23 @@ Examples:
       $ rosbag play -.-clock MH_01_easy.bag
 
    MSCKF (VIO):
-      $ roslaunch rtabmap_ros euroc_datasets.launch args:="--Odom/Strategy 8"
+      $ roslaunch rtabmap_ros euroc_datasets.launch args:="Odom/Strategy 8"
       $ rosbag play -.-clock V1_01_easy.bag
 
       We need to ignore the first 24 seconds for correct VIO initialization (drone should not move).
-      $ roslaunch rtabmap_ros euroc_datasets.launch args:="--Odom/Strategy 8" MH_seq:=true
+      $ roslaunch rtabmap_ros euroc_datasets.launch args:="Odom/Strategy 8" MH_seq:=true
       $ rosbag play -.-clock -s 24 MH_01_easy.bag
 
    OKVIS (VIO):
-      $ roslaunch rtabmap_ros euroc_datasets.launch args:="--Odom/Strategy 6 OdomOKVIS/ConfigPath ~/okvis/config/config_fpga_p2_euroc.yaml" MH_seq:=true raw_images_for_odom:=true
+      $ roslaunch rtabmap_ros euroc_datasets.launch args:="Odom/Strategy 6 OdomOKVIS/ConfigPath ~/okvis/config/config_fpga_p2_euroc.yaml" MH_seq:=true raw_images_for_odom:=true
       $ rosbag play -.-clock MH_01_easy.bag
 
    VINS (VIO):
-      $ roslaunch rtabmap_ros euroc_datasets.launch args:="--Odom/Strategy 9 OdomVINS/ConfigPath ~/catkin_ws/src/VINS-Fusion/config/euroc/euroc_stereo_imu_config.yaml" MH_seq:=true raw_images_for_odom:=true
+      $ roslaunch rtabmap_ros euroc_datasets.launch args:="Odom/Strategy 9 OdomVINS/ConfigPath ~/catkin_ws/src/VINS-Fusion/config/euroc/euroc_stereo_imu_config.yaml" MH_seq:=true raw_images_for_odom:=true
       $ rosbag play -.-clock MH_01_easy.bag
 
    VINS (VO):
-      $ roslaunch rtabmap_ros euroc_datasets.launch args:="--Odom/Strategy 9 OdomVINS/ConfigPath ~/catkin_ws/src/VINS-Fusion/config/euroc/euroc_stereo_config.yaml" MH_seq:=true raw_images_for_odom:=true
+      $ roslaunch rtabmap_ros euroc_datasets.launch args:="Odom/Strategy 9 OdomVINS/ConfigPath ~/catkin_ws/src/VINS-Fusion/config/euroc/euroc_stereo_config.yaml" MH_seq:=true raw_images_for_odom:=true
       $ rosbag play -.-clock MH_01_easy.bag
 -->
 
@@ -99,8 +99,8 @@ Examples:
 
 <!-- RTAB-Map -->
 <include file="$(find rtabmap_ros)/launch/rtabmap.launch">
-   <arg     if="$(arg raw_images_for_odom)" name="rtabmap_args" value="$(arg args) --Rtabmap/ImagesAlreadyRectified false"/>
+   <arg     if="$(arg raw_images_for_odom)" name="rtabmap_args" value="$(arg common_args) --Rtabmap/ImagesAlreadyRectified false"/>
-   <arg unless="$(arg raw_images_for_odom)" name="rtabmap_args" value="$(arg args)"/>
+   <arg unless="$(arg raw_images_for_odom)" name="rtabmap_args" value="$(arg common_args)"/>
    <arg     if="$(arg raw_images_for_odom)" name="odom_args" value="--Rtabmap/ImagesAlreadyRectified false"/>
    <arg     if="$(arg raw_images_for_odom)" name="left_image_topic" value="/cam0/image_raw"/>
    <arg     if="$(arg raw_images_for_odom)" name="right_image_topic" value="/cam1/image_raw"/>

launch/tests/test_d435i_vio.launch

@@ -0,0 +1,62 @@
+<launch>
+
+<!-- Example usage of RTAB-Map with VINS-Fusion support for realsense D435i -->
+
+<arg name="rtabmapviz" default="true"/>
+<arg name="rviz"       default="false"/>
+<arg name="depth_mode" default="true"/>
+
+<include file="$(find realsense2_camera)/launch/rs_camera.launch">
+   <arg name="align_depth" value="true"/>
+   <arg name="unite_imu_method" value="linear_interpolation"/>
+</include>
+
+<node pkg="imu_filter_madgwick" type="imu_filter_node" name="imu_filter_node">
+   <param name="use_mag"       value="false"/>
+   <param name="publish_tf"    value="false"/>
+   <param name="world_frame"   value="enu"/>
+   <remap from="/imu/data_raw" to="/camera/imu"/>
+   <remap from="/imu/data"     to="/rtabmap/imu"/>
+</node>
+
+<!-- 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">
+   <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"/>
+   <param name="frame_id"          value="camera_link"/>
+   <param name="wait_imu_to_init"  value="true"/>
+</node>
+</group>
+<include if="$(arg depth_mode)" file="$(find rtabmap_ros)/launch/rtabmap.launch">
+   <arg name="rtabmap_args"      value="--delete_db_on_start --Optimizer/GravitySigma 0.3"/>
+   <arg name="rgb_topic"         value="/camera/color/image_raw"/>
+   <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="frame_id"          value="camera_link"/>
+   <arg name="imu_topic"         value="/rtabmap/imu"/>
+   <arg name="rtabmapviz"        value="$(arg rtabmapviz)"/>
+   <arg name="rviz"              value="$(arg rviz)"/>
+</include>
+
+<!-- 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="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"/>
+   <arg name="wait_imu_to_init"        value="true"/>
+   <arg name="rtabmapviz"              value="$(arg rtabmapviz)"/>
+   <arg name="rviz"                    value="$(arg rviz)"/>
+</include>
+
+</launch>

src/OdometryROS.cpp

@@ -81,7 +81,8 @@ OdometryROS::OdometryROS(bool stereoParams, bool visParams, bool icpParams) :
 	previousStamp_(0.0),
 	expectedUpdateRate_(0.0),
 	odomStrategy_(Parameters::defaultOdomStrategy()),
-	waitIMUToinit_(false)
+	waitIMUToinit_(false),
+	imuProcessed_(false)
 {
 
 }
@@ -487,13 +488,14 @@ void OdometryROS::callbackIMU(const sensor_msgs::ImuConstPtr& msg)
 		{
 			SensorData data(imu, 0, stamp);
 			this->processData(data, msg->header.stamp);
+			imuProcessed_ = true;
 		}
 	}
 }
 
 void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
 {
-	if(waitIMUToinit_ && odometry_->framesProcessed() == 0 && odometry_->getPose().isIdentity() && data.imu().empty())
+	if((waitIMUToinit_ && !imuProcessed_) && odometry_->framesProcessed() == 0 && odometry_->getPose().isIdentity() && data.imu().empty())
 	{
 		NODELET_WARN("odometry: waiting imu to initialize orientation (wait_imu_to_init=true)");
 		return;
@@ -876,6 +878,7 @@ void OdometryROS::reset(const Transform & pose)
 	guessPreviousPose_.setNull();
 	previousStamp_ = 0.0;
 	resetCurrentCount_ = resetCountdown_;
+	imuProcessed_ = false;
 	this->flushCallbacks();
 }
 

src/nodelets/stereo_odometry.cpp

@@ -365,7 +365,6 @@ 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_;
 };