two kinect2 one map

[yalan2017]

hi dear
 I am following this tutorial with two kinect2 which Kinects are placed at 90 degrees, clockwise . but once i am moving the camera i have mismatching like this:








my launch filese:

two_cameras.launch
 kinect2A.launch
kinect2B.launch
  also once I run the launch file this error only one time  shown but  then disappears :
 

[ INFO] [1537967620.786887328]:
/rtabmap/rgbd_odometry subscribed to (approx sync):
   /camera1/rgbd_image,
   /camera2/rgbd_image
[ERROR] (2018-09-26 21:13:41.087) OdometryF2M.cpp:197::computeTransform() Odometry bundle adjustment doesn't work with multi-cameras. It is disabled.
[ INFO] [1537967621.313737606]: Odom: quality=0, std dev=99.995000m|99.995000rad, update time=0.226210s

I am not sure I should change the bundle adjustment value or not?

I am using ubuntu 16
regards

[matlabbe]

Can you share the resulting database? (~/.ros/rtabmap.db)

[yalan2017]

Hi
thank you for your reply.

i have tried with two different "transform_publisher":
transform A:


   
  <node pkg="tf" type="static_transform_publisher" name="base_to_camera1_tf"
      args="0.0 0.0 0.0 0.0 0.0 0.0 /base_link /camera1_link 100" />
  <node pkg="tf" type="static_transform_publisher" name="base_to_camera2_tf"
      args="0.1375 -0.03 -0.1825 0.0 1.570796327 0.0 /base_link /camera2_link 100" />

transform B:

   
  <node pkg="tf" type="static_transform_publisher" name="base_to_camera1_tf"
      args="0.0 0.0 0.0 0.0 0.0 0.0 /base_link /camera1_link 100" />
  <node pkg="tf" type="static_transform_publisher" name="base_to_camera2_tf"
      args="-0.18 -0.1375 0 -1.570796327 0.0 0.0 /base_link /camera2_link 100" />

The result of the transform A  is dbA and  the result of the transform B is db.B:
the result of transform A is much better than transform B
 
my dbA      and  db.B    



and this the photo of  two kinects:


King regards

matlabbe wrote

Can you share the resulting database? (~/.ros/rtabmap.db)

[matlabbe]

Note sure why, but I got those errors when opening your databases:

Error: database disk image is malformed

Make sure to close rtabmap_ros before copying the database.

[yalan2017]

i have created another db file:

dba.
db.b




do you have any idea about blue lines on the map?
and
also dor this error? bundle adjustment should be changed?

[ERROR] (2018-09-26 21:13:41.087) OdometryF2M.cpp:197::computeTransform() Odometry bundle adjustment doesn't work with multi-cameras. It is disabled.

Regards

[matlabbe]

Hi,

To suppress the bundle adjustment error, set OdomF2M/BundleAdjustment to 0 and Vis/BundleAdjustment to 0.

The static transforms between the camera on 2_b.db are wrong:


The static transforms between the cameras for 2_a.db seem ok, but there is a missing rotation to put the cameras from image coordinates to ROS coordinates (it is like the first camera is looking up):


Looking at the demo_two_kinects.launch, the difference is that with kinect for xbox 360, the driver publishes the optical rotation between camera_link and rgb optical frame. For the kinect v2, the camera_link to rgb optical frame doesn't have the optical rotation. To copy the approach of demo_two_kinects.launch with kinect v2, we should do this:

  <!-- Frames: Rotate cameras in ROS coordinates -->
  <node pkg="tf" type="static_transform_publisher" name="camera_base_to_camera1_tf"
      args="0.0 0.0 0.0  -1.5707963267948966 0 -1.5707963267948966 /camera1_base_link /camera1_link 100" />
  <node pkg="tf" type="static_transform_publisher" name="camera_base_to_camera2_tf"
      args="0.0 0.0 0.0  -1.5707963267948966 0 -1.5707963267948966 /camera2_base_link /camera2_link 100" />

  <!-- Frames: Kinects are placed at 90 degrees, clockwise -->
  <node pkg="tf" type="static_transform_publisher" name="base_to_camera1_tf"
      args="0.0 0.0 0.0 0.0 0.0 0.0 /base_link /camera1_base_link 100" />
  <node pkg="tf" type="static_transform_publisher" name="base_to_camera2_tf"
      args="-0.18 -0.1375 0  -1.570796327 0.0 0.0 /base_link /camera2_base_link 100" />

or

  <!-- Frames: Rotate cameras in ROS coordinates -->
  <node pkg="tf" type="static_transform_publisher" name="camera_base_to_camera1_tf"
      args="0.0 0.0 0.0  -1.5707963267948966 0 -1.5707963267948966 /base_link /base_camera_link 100" />

  <node pkg="tf" type="static_transform_publisher" name="base_to_camera1_tf" 
      args="0.0 0.0 0.0 0.0 0.0 0.0 /base_camera_link /camera1_link 100" />
  <node pkg="tf" type="static_transform_publisher" name="base_to_camera2_tf" 
      args="0.1375 -0.03 -0.1825 0.0 1.570796327 0.0 /base_camera_link /camera2_link 100" />

The blue lines are the laser scans created from the camera point clouds ("gen_scan" parameter is true). Because the camera clouds are not in ROS coordinate frames, the scan was wrongly transformed.

Afterwards, quality of the map will be also greatly dependent on the accuracy of the static transform set between the RGB optical cameras.

cheers,
Mathieu

[yalan2017]

Hi
Thank you very much, my problem solved but I have another problem with the created map, the floor in the real world is flat but my map shows the floor is like a curve! after loop closure, it improved but still there is a problem with the map, do you have any solution for that?




 

king regards

[matlabbe]

Hi,

This curve is mainly caused by the camera calibration. A fast solution, if you are moving in 2D (on a plane), you can add parameter Reg/Force3DoF to true for rtabmap and rgbd_odometry nodes.

cheers,
Mathieu

[yalan2017]

Thank you for your reply I got the better result, sorry for my multiple questions.
I have a problem with my RosBag file, the recorded rosbag file during running with two cameras I face this warning and rtabmap doesn't work :

[ WARN] [1540822249.021635274, 1539068344.035866149]: /rtabmap/rtabmapviz: Did not receive data since 5 seconds! Make sure the input topics are published ("$ rostopic hz my_topic") and the timestamps in their header are set. If topics are coming from different computers, make sure the clocks of the computers are synchronized ("ntpdate"). If topics are not published at the same rate, you could increase "queue_size" parameter (current=10).
/rtabmap/rtabmapviz subscribed to (approx sync):
   /rtabmap/odom,
   /camera1/rgbd_image,
   /camera2/rgbd_image,
   /rtabmap/odom_info


when I using this 2kinect_map.launch file in real time it's working but once I recorded in a ros bag and run it later I face this problem.

recorded topics:
/camera1/qhd/camera_info /camera1/qhd/image_color_rect /camera1/qhd/image_depth_rect /camera2/qhd/camera_info /camera2/qhd/image_color_rect /camera2/qhd/image_depth_rect /tf





King regards

[matlabbe]

The link doesn't seem to work. You can copy/paste the inside of the file here between raw xml tags (or click More->Raw text). When replaying a bag with TF, make sure you started a roscore with use_sim_time set to true and play the bag with --clock:

$ roscore
$ rosparam set use_sim_time true
$ rosbag play --clock my.bag

Your bag seems to have /map and /odom frames recorded. If you just want to record camera images, don't launch rtabmap and rgbd_odometry at the same time. Otherwise, you may remove /map and /odom frames using a script similar to this one.

You can share a small rosbag to reproduce the problem.

cheers,
Mathieu

[yalan2017]

Hi
I have tried without rtabmap launch file but still, I have the same problem.

this is my rosbag file which I only run two cameras.
this is my launch file:

<!--     -->
<launch>
   
  <!-- Multi-cameras demo with 2 Kinects -->
  <!-- Cameras 
  <include file="$(find freenect_launch)/launch/freenect.launch">
    <arg name="depth_registration" value="True" />
    <arg name="camera" value="camera1" />
    <arg name="device_id" value="#1" />
  </include>
  <include file="$(find freenect_launch)/launch/freenect.launch">
    <arg name="depth_registration" value="True" />
    <arg name="camera" value="camera2" />
    <arg name="device_id" value="#2" />
  </include>
-->

   
    <param name="use_sim_time" type="bool" value="True"/>
     
    <param name="Reg/Force3DoF" type="string" value="true"/>
 
 
 
<!-- Frames: Rotate cameras in ROS coordinates -->
  <node pkg="tf" type="static_transform_publisher" name="camera_base_to_camera1_tf"
      args="0.0 0.0 0.0  -1.5707963267948966 0 -1.5707963267948966 /camera1_base_link /camera1_link 100" />
  <node pkg="tf" type="static_transform_publisher" name="camera_base_to_camera2_tf"
      args="0.0 0.0 0.0  -1.5707963267948966 0.0 -1.5707963267948966 /camera2_base_link /camera2_link 100" />
  


  <!-- Frames: Kinects are placed at 90 degrees, clockwise -0.1375 -0.233 0  -1.570796327 0.0 0.0   -->
  <node pkg="tf" type="static_transform_publisher" name="base_to_camera1_tf"
      args="0.0 0.0 0.0 0.0 0.0 0.0 /base_link /camera1_base_link 100" />
  <node pkg="tf" type="static_transform_publisher" name="base_to_camera2_tf"
      args="-0.1375 -0.223 0  -1.570796327 0.0  0.0  /base_link /camera2_base_link 100" />


  <param name ="/use_sim_time" value="true"/>
 
  <arg name="approx_sync"                 default="true"/>
      
    <param name="Reg/Force3DoF"             type="string" value="true"/>
   <!-- Choose visualization -->
   <arg name="rviz"       default="false" />
   <arg name="rtabmapviz" default="true" /> 
   
   <!-- ODOMETRY MAIN ARGUMENTS: 
        -"strategy"        : Strategy: 0=Frame-to-Map 1=Frame-to-Frame
        -"feature"         : Feature type: 0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK
        -"nn"              : Nearest neighbor strategy : 0=Linear, 1=FLANN_KDTREE, 2=FLANN_LSH, 3=BRUTEFORCE 
                             Set to 1 for float descriptor like SIFT/SURF                  
                             Set to 3 for binary descriptor like ORB/FREAK/BRIEF/BRISK  
        -"max_depth"       : Maximum features depth (m)  
        -"min_inliers"     : Minimum visual correspondences to accept a transformation (m)  
        -"inlier_distance" : RANSAC maximum inliers distance (m)  
        -"local_map"       : Local map size: number of unique features to keep track 
        -"odom_info_data"  : Fill odometry info messages with inliers/outliers data. 0.02
    -->
   <arg name="strategy"        default="0" />
   <arg name="feature"         default="6" />
   <arg name="nn"              default="3" />
   <arg name="max_depth"       default="15.0" />
   <arg name="min_inliers"     default="20" />
   <arg name="inlier_distance" default="0.1" />
   <arg name="local_map"       default="1000" />
   <arg name="odom_info_data"  default="true" />
  <arg name="wait_for_transform"  default="false" />
  <arg name="gftt_max_corners" default="1000" />
  <arg name="gftt_min_distance" default="7" />
   
   <!-- sync rgb/depth images per camera -->
  
    <node pkg="nodelet" type="nodelet" name="rgbd_sync" args="load rtabmap_ros/rgbd_sync camera1">
      <remap from="rgb/image"       to="/camera1/qhd/image_color"/>
      <remap from="depth/image"     to="/camera1/qhd/image_depth_rect"/>
      <remap from="rgb/camera_info" to="/camera1/qhd/camera_info"/>
      <remap from="rgbd_image"      to="/camera1/rgbd_image"/>
    </node>


    <node pkg="nodelet" type="nodelet" name="rgbd_sync2" args="load rtabmap_ros/rgbd_sync camera2">
      <remap from="rgb/image"       to="/camera2/qhd/image_color"/>
      <remap from="depth/image"     to="/camera2/qhd/image_depth_rect"/>
      <remap from="rgb/camera_info" to="/camera2/qhd/camera_info"/>
      <remap from="rgbd_image"      to="/camera2/rgbd_image"/>
    </node>

        
  <group ns="rtabmap">
  
    <!-- Odometry -->
    <node pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry" output="screen">
      <remap from="rgbd_image0"       to="/camera1/rgbd_image"/>
      <remap from="rgbd_image1"       to="/camera2/rgbd_image"/>
	  
	  <arg name="approx_sync"                 default="true"/>
	
	  <param name="subscribe_rgbd"           type="bool"   value="true"/>
	  <param name="frame_id"                 type="string" value="base_link"/>
	  <param name="rgbd_cameras"             type="int"    value="2"/>
	  <param name="wait_for_transform"       type="bool"   value="$(arg wait_for_transform)"/>
       <param name="Vis/CorGuessWinSize"      type="string" value="0"/>  
        <param name="Vis/BundleAdjustment"      type="string" value="0"/> 
        <param name=" OdomF2M/BundleAdjustment"      type="string" value="0"/> 
	  <param name="Odom/Strategy"            type="string" value="$(arg strategy)"/> 
	  <param name="Vis/EstimationType"      type="string" value="0"/> 
	  <param name="Vis/FeatureType"         type="string" value="$(arg feature)"/>  
	  <param name="Vis/CorNNType"           type="string" value="$(arg nn)"/>
	  <param name="Vis/MaxDepth"            type="string" value="$(arg max_depth)"/>  
 <param name="Vis/MinDepth"            type="string" value="0.2"/>  
	  <param name="Vis/MinInliers"          type="string" value="$(arg min_inliers)"/> 
	  <param name="Vis/InlierDistance"      type="string" value="$(arg inlier_distance)"/>   
 <param name="Vis/MaxFeatures" type="string" value="$(arg gftt_max_corners)"/>
 <param name="GFTT/MinDistance" type="string" value="$(arg gftt_min_distance)"/>    
      <param name="OdomF2M/MaxSize" type="string" value="$(arg local_map)"/> 
      <param name="Odom/FillInfoData"        type="string" value="$(arg odom_info_data)"/>   
    </node>
  
    <!-- Visual SLAM (robot side) -->

    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
      <param name="subscribe_depth"  type="bool"   value="false"/>
      <param name="subscribe_rgbd"   type="bool"   value="true"/>
      <param name="rgbd_cameras"    type="int"    value="2"/>
	  <param name="frame_id"         type="string" value="base_link"/>
	  <param name="gen_scan"         type="bool"   value="false"/>
	  <!--<param name="wait_for_transform" type="bool"   value="$(arg wait_for_transform)"/>-->
	  <param name="map_negative_poses_ignored" type="bool"   value="false"/>        
	  <param name="map_negative_scan_empty_ray_tracing" type="bool" value="false"/> 
	
      <remap from="rgbd_image0"       to="/camera1/rgbd_image"/>
      <remap from="rgbd_image1"       to="/camera2/rgbd_image"/>

      <param name="Grid/FromDepth"     type="string" value="false"/>
      <param name="Vis/EstimationType" type="string" value="0"/> 
      <param name="Vis/MinInliers"     type="string" value="10"/>
      <param name="Vis/InlierDistance" type="string" value="$(arg inlier_distance)"/>
    </node>
  
    <!-- Visualisation RTAB-Map -->
    <node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" output="screen">
      <param name="subscribe_depth"  type="bool"   value="false"/>
      <param name="subscribe_rgbd"   type="bool"   value="true"/>
      <param name="subscribe_odom_info" type="bool"   value="$(arg odom_info_data)"/>
      <param name="frame_id"            type="string" value="base_link"/>
      <param name="rgbd_cameras"       type="int"    value="2"/>
      <!--<param name="wait_for_transform"  type="bool"   value="$(arg wait_for_transform)"/>-->

 

      <remap from="rgbd_image0"       to="/camera1/rgbd_image"/>
      <remap from="rgbd_image1"       to="/camera2/rgbd_image"/>
    </node>
  
  </group>

  <node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/config/rgbd.rviz"/>

</launch>

king regards

[matlabbe]

Hi,

I cleanup the launch files to keep minimal stuff to run the example:

<launch>

    <param name="use_sim_time" type="bool" value="True"/>
      
   <!-- Choose visualization -->
   <arg name="rviz"       default="false" />
   <arg name="rtabmapviz" default="true" /> 

  <!-- Frames: Rotate cameras in ROS coordinates -->
  <node pkg="tf" type="static_transform_publisher" name="camera_base_to_camera1_tf"
      args="0.0 0.0 0.0  -1.5707963267948966 0 -1.5707963267948966 /camera1_base_link /camera1_link 100" />
  <node pkg="tf" type="static_transform_publisher" name="camera_base_to_camera2_tf"
      args="0.0 0.0 0.0  -1.5707963267948966 0.0 -1.5707963267948966 /camera2_base_link /camera2_link 100" />
  


  <!-- Frames: Kinects are placed at 90 degrees, clockwise -0.1375 -0.233 0  -1.570796327 0.0 0.0   -->
  <node pkg="tf" type="static_transform_publisher" name="base_to_camera1_tf"
      args="0.0 0.0 0.0 0.0 0.0 0.0 /base_link /camera1_base_link 100" />
  <node pkg="tf" type="static_transform_publisher" name="base_to_camera2_tf"
      args="-0.1375 -0.223 0  -1.570796327 0.0  0.0  /base_link /camera2_base_link 100" />
   
   <!-- sync rgb/depth images per camera -->
    <node pkg="nodelet" type="nodelet" name="nodelet_manager" args="manager"/>
    <node pkg="nodelet" type="nodelet" name="rgbd_sync" args="load rtabmap_ros/rgbd_sync nodelet_manager">
      <remap from="rgb/image"       to="/camera1/qhd/image_color_rect"/>
      <remap from="depth/image"     to="/camera1/qhd/image_depth_rect"/>
      <remap from="rgb/camera_info" to="/camera1/qhd/camera_info"/>
      <remap from="rgbd_image"      to="/camera1/rgbd_image"/>
      <param name="approx_sync"     value="false"/>
    </node>


    <node pkg="nodelet" type="nodelet" name="rgbd_sync2" args="load rtabmap_ros/rgbd_sync nodelet_manager">
      <remap from="rgb/image"       to="/camera2/qhd/image_color_rect"/>
      <remap from="depth/image"     to="/camera2/qhd/image_depth_rect"/>
      <remap from="rgb/camera_info" to="/camera2/qhd/camera_info"/>
      <remap from="rgbd_image"      to="/camera2/rgbd_image"/>
      <param name="approx_sync"     value="false"/>
    </node>

        
  <group ns="rtabmap">
  
    <!-- Odometry -->
    <node pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry" output="screen">
      <remap from="rgbd_image0"       to="/camera1/rgbd_image"/>
      <remap from="rgbd_image1"       to="/camera2/rgbd_image"/>
	
	  <param name="subscribe_rgbd"           type="bool"   value="true"/>
	  <param name="frame_id"                 type="string" value="base_link"/>
	  <param name="rgbd_cameras"             type="int"    value="2"/>
	  <param name="Vis/CorGuessWinSize"      type="string" value="0"/>  
	  <param name="Vis/BundleAdjustment"      type="string" value="0"/> 
	  <param name=" OdomF2M/BundleAdjustment"  type="string" value="0"/> 
	  <param name="Vis/EstimationType"      type="string" value="0"/> 
    </node>
  
    <!-- Visual SLAM (robot side) -->

    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
      <param name="subscribe_depth"  type="bool"   value="false"/>
      <param name="subscribe_rgbd"   type="bool"   value="true"/>
      <param name="rgbd_cameras"     type="int"    value="2"/>
      <param name="frame_id"         type="string" value="base_link"/>
	
      <remap from="rgbd_image0"       to="/camera1/rgbd_image"/>
      <remap from="rgbd_image1"       to="/camera2/rgbd_image"/>

      <param name="Grid/FromDepth"     type="string" value="false"/>
      <param name="Vis/EstimationType" type="string" value="0"/> 
    </node>
  
    <!-- Visualisation RTAB-Map -->
    <node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" output="screen">
      <param name="subscribe_depth"  type="bool"   value="false"/>
      <param name="subscribe_rgbd"   type="bool"   value="true"/>
      <param name="subscribe_odom_info" type="bool"   value="true"/>
      <param name="frame_id"            type="string" value="base_link"/>
      <param name="rgbd_cameras"       type="int"    value="2"/>


      <remap from="rgbd_image0"       to="/camera1/rgbd_image"/>
      <remap from="rgbd_image1"       to="/camera2/rgbd_image"/>
    </node>
  
  </group>

  <node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/config/rgbd.rviz"/>

</launch>

One problem was that running without the camera drivers, there were no nodelet managers. I added one for rgbd_sync nodelets. As Kinect v2 topics are used, I set also approx_sync to false for rgbd_sync nodelets.

Run the bag:

roslaunch test.launch
rosbag play --clock test1.bag

The cameras seem tilted towards the ceiling though, adjust TF or make sure that the cameras are physically looking straight forward.

cheers,
Mathieu