Odom: don't update previousStamp on IMU updates, updated euroc_datasets.launch with okvis example

launch/tests/euroc_datasets.launch

@@ -18,9 +18,17 @@ Examples:
       $ roslaunch rtabmap_ros euroc_datasets.launch args:="-d RGBD/CreateOccupancyGrid false Odom/Strategy 8" MH_seq:=true
       $ rosbag play -.-clock -s 24 MH_01_easy.bag
 
+   OKVIS (VIO):
+      $ roslaunch rtabmap_ros euroc_datasets.launch args:="-d RGBD/CreateOccupancyGrid false 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:="-d RGBD/CreateOccupancyGrid false 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:="-d RGBD/CreateOccupancyGrid false 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
 -->
 
 <param name="use_sim_time" value="true"/>
@@ -29,7 +37,7 @@ Examples:
 <arg name="cfg" default=""/>
 <arg name="MH_seq" default="false"/> <!-- For MH sequences, the ground truth is coming from a different topic -->
 <arg name="raw_images_for_odom" default="false"/>
-<arg name="record_ground_truth" default="true"/>
+<arg name="record_ground_truth" default="false"/>
 <arg name="rtabmapviz" default="true"/>
 <arg name="rviz" default="false"/>
 

launch/tests/test_velodyne.launch

@@ -5,15 +5,14 @@
     Hand-held 3D lidar mapping example using only a Velodyne PUCK (no camera).
     Prerequisities: rtabmap should be built with libpointmatcher
     Example:
-     $ roslaunch velodyne_pointcloud VLP16_points.launch rpm:=600
      $ roslaunch rtabmap_ros test_velodyne.launch
      $ rosrun rviz rviz -f map
      $ Show TF and /rtabmap/cloud_map topics
     -->
 
     <arg name="rtabmapviz"    default="true"/>
-    <arg name="use_imu"       default="false"/> <!-- Assuming IMU fixed to lidar and the top of the lidar always points to the ceiling -->
+    <arg name="use_imu"       default="false"/> <!-- Assuming IMU fixed to lidar with /velodyne -> /imu_link TF -->
-    <arg name="imu_topic"     default="/data/imu"/>
+    <arg name="imu_topic"     default="/imu/data"/>
     <arg name="scan_20_hz"    default="false"/> <!-- If we launch the velodyne with "rpm:=1200" argument -->
     <arg name="use_sim_time"  default="false"/>
     <param if="$(arg use_sim_time)" name="use_sim_time" value="true"/>
@@ -27,7 +26,7 @@
     
     <!-- IMU orientation estimation and publish tf accordingly to os1_sensor frame -->
     <node if="$(arg use_imu)" pkg="rtabmap_ros" type="imu_to_tf" name="imu_to_tf">
-      <remap from="imu/data" to="/imu/data"/>
+      <remap from="imu/data" to="$(arg imu_topic)"/>
       <param name="fixed_frame_id" value="$(arg frame_id)_stabilized"/>
       <param name="base_frame_id" value="$(arg frame_id)"/>
     </node> 
@@ -40,7 +39,7 @@
         <param     if="$(arg scan_20_hz)" name="expected_update_rate" type="double" value="25"/>
         <param unless="$(arg scan_20_hz)" name="expected_update_rate" type="double" value="15"/>
 
-        <remap if="$(arg use_imu)" from="imu" to="/imu/data"/>
+        <remap if="$(arg use_imu)" from="imu" to="$(arg imu_topic)"/>
         <param if="$(arg use_imu)" name="guess_frame_id"   type="string" value="$(arg frame_id)_stabilized"/>
         <param if="$(arg use_imu)" name="wait_imu_to_init" type="bool" value="true"/>
      

src/OdometryROS.cpp

@@ -497,13 +497,8 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
 	{
 		if(previousStamp_>0.0 && previousStamp_ >= stamp.toSec())
 		{
-			static bool warned = false;
+			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.",
-			if(!warned)
+					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.",
-						previousStamp_, stamp.toSec());
-				warned = true;
-			}
 			return;
 		}
 		else if(expectedUpdateRate_ > 0 &&
@@ -848,8 +843,8 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
 		{
 			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());
 		}
+		previousStamp_ = stamp.toSec();
 	}
-	previousStamp_ = stamp.toSec();
 }
 
 bool OdometryROS::reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&)

src/nodelets/stereo_odometry.cpp

@@ -200,8 +200,8 @@ private:
 				if(!alreadyRectified)
 				{
 					stereoTransform = getTransform(
-							cameraInfoLeft->header.frame_id,
 							cameraInfoRight->header.frame_id,
+							cameraInfoLeft->header.frame_id,
 							cameraInfoLeft->header.stamp);
 					if(stereoTransform.isNull())
 					{