Merge branch 'master' of github.com:introlab/rtabmap_ros

CMakeLists.txt

@@ -227,6 +227,7 @@ install(FILES
    launch/rgbd_mapping.launch
    launch/rgbd_localization.launch
    launch/data_recorder.launch
+   launch/rgbd_mapping_kinect2.launch
    DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch
 )
 install(DIRECTORY 

launch/calibration/kinect2/506816242542/calib_color.yaml

@@ -0,0 +1,21 @@
+%YAML:1.0
+cameraMatrix: !!opencv-matrix
+   rows: 3
+   cols: 3
+   dt: d
+   data: [ 1035.409658, 0., 953.629091, 0., 1036.265896, 550.972045, 0., 0., 1. ]
+distortionCoefficients: !!opencv-matrix
+   rows: 1
+   cols: 5
+   dt: d
+   data: [ 0.046857, -0.054802, 0.002446, -0.001013, 0. ]
+rotation: !!opencv-matrix
+   rows: 3
+   cols: 3
+   dt: d
+   data: [ 1., 0., 0., 0., 1., 0., 0., 0., 1. ]
+projection: !!opencv-matrix
+   rows: 4
+   cols: 4
+   dt: d
+   data: [ 1034.458862, 0., 949.608678, 0., 0., 1046.304565, 553.032416, 0., 0., 0., 1., 0., 0., 0., 0., 1. ]

launch/calibration/kinect2/506816242542/calib_ir.yaml

@@ -0,0 +1,26 @@
+%YAML:1.0
+cameraMatrix: !!opencv-matrix
+   rows: 3
+   cols: 3
+   dt: d
+   data: [ 3.6979200177991152e+02, 0., 2.5040545556144602e+02, 0.,
+       3.6934874236487565e+02, 2.0812949708935565e+02, 0., 0., 1. ]
+distortionCoefficients: !!opencv-matrix
+   rows: 1
+   cols: 5
+   dt: d
+   data: [ 1.0599430434497996e-01, -2.7091691685678082e-01,
+       4.8231954365675019e-04, -3.1761820716379199e-05,
+       7.7492684133604814e-02 ]
+rotation: !!opencv-matrix
+   rows: 3
+   cols: 3
+   dt: d
+   data: [ 1., 0., 0., 0., 1., 0., 0., 0., 1. ]
+projection: !!opencv-matrix
+   rows: 4
+   cols: 4
+   dt: d
+   data: [ 3.6979200177991152e+02, 0., 2.5040545556144602e+02, 0., 0.,
+       3.6934874236487565e+02, 2.0812949708935565e+02, 0., 0., 0., 1.,
+       0., 0., 0., 0., 1. ]

launch/calibration/kinect2/506816242542/calib_pose.yaml

@@ -0,0 +1,33 @@
+%YAML:1.0
+rotation: !!opencv-matrix
+   rows: 3
+   cols: 3
+   dt: d
+   data: [ 9.9998557008106792e-01, 1.2305561691822327e-03,
+       -5.2292792195576393e-03, -1.3388135514396061e-03,
+       9.9978381286062756e-01, -2.0749340233854399e-02,
+       5.2026154880109553e-03, 2.0756041852440336e-02,
+       9.9977103354653341e-01 ]
+translation: !!opencv-matrix
+   rows: 3
+   cols: 1
+   dt: d
+   data: [ -5.1351463516082281e-02, 2.3299760970435590e-03,
+       -1.6640325723173980e-02 ]
+essential: !!opencv-matrix
+   rows: 3
+   cols: 3
+   dt: d
+   data: [ -1.0156323849380249e-05, 1.6685089380143084e-02,
+       1.9841668306476608e-03, -1.6372923685201993e-02,
+       1.0453762704480134e-03, 5.1426722663111546e-02,
+       -2.2611924404357772e-03, -5.1343229156542415e-02,
+       1.0776930835878881e-03 ]
+fundamental: !!opencv-matrix
+   rows: 3
+   cols: 3
+   dt: d
+   data: [ -4.8742459614193642e-09, 8.0171558549327292e-06,
+       -1.3152531820940182e-03, -7.8512380180953911e-06,
+       5.0188640603529770e-07, 1.0980767538413172e-02,
+       3.2068378477732255e-03, -3.3465816946061419e-02, 1. ]

launch/calibration/kinect2/README.txt

@@ -0,0 +1,12 @@
+Kinect v2 calibration:
+  Copy "506816242542" folder in "~/catkin_ws/src/iai_kinect2/kinect2_bridge/data"
+
+The number is the Kinect serial shown when launching kinect2_brige:
+  ...
+  [Freenect2Impl] enumerating devices...
+  [Freenect2Impl] 12 usb devices connected
+  [Freenect2Impl] found valid Kinect v2 @4:3 with serial 506816242542
+  [Freenect2Impl] found 1 devices
+  Kinect2 devices found: 
+    0: 506816242542 (selected)
+  ...

launch/rgbd_mapping_kinect2.launch

@@ -39,6 +39,8 @@
    <arg name="min_inliers" default="20" />
    <arg name="inlier_distance" default="0.02" />
    <arg name="local_map" default="1000" />
+   <arg name="gftt_max_corners" default="500" />
+   <arg name="gftt_min_distance" default="7" />
    
   <!-- TF FRAMES -->
   <node pkg="tf" type="static_transform_publisher" name="base_to_camera_tf" 
@@ -52,7 +54,7 @@
       <remap from="depth/image" to="/kinect2/$(arg depth_prefix)/image"/>
       <remap from="rgb/camera_info" to="/kinect2/$(arg rgb_prefix)/camera_info"/>
 
-      <param name="approx_sync" type="bool" value="false"/>
+      <param name="approx_sync" type="bool" value="true"/>
 	  
       <param name="Odom/Strategy" type="string" value="$(arg strategy)"/> 
       <param name="Odom/FeatureType" type="string" value="$(arg feature)"/>  
@@ -60,8 +62,10 @@
       <param name="Odom/MaxDepth" type="string" value="$(arg max_depth)"/>  
       <param name="Odom/MinInliers" type="string" value="$(arg min_inliers)"/> 
       <param name="Odom/InlierDistance" type="string" value="$(arg inlier_distance)"/>       
-      <param name="OdomBow/LocalHistorySize" type="string" value="$(arg local_map)"/> 
-      <param name="Odom/FillInfoData" type="string" value="$(arg rtabmapviz)"/>   
+      <param name="OdomBow/LocalHistorySize" type="string" value="$(arg local_map)"/>
+      <param name="Odom/FillInfoData" type="string" value="$(arg rtabmapviz)"/>
+      <param name="GFTT/MaxCorners" type="string" value="$(arg gftt_max_corners)"/>
+      <param name="GFTT/MinDistance" type="string" value="$(arg gftt_min_distance)"/>
     </node>
   
     <!-- Visual SLAM (robot side) -->
@@ -73,7 +77,7 @@
       <remap from="depth/image" to="/kinect2/$(arg depth_prefix)/image"/>
       <remap from="rgb/camera_info" to="/kinect2/$(arg rgb_prefix)/camera_info"/>
 
-      <param name="approx_sync" type="bool" value="false"/>
+      <param name="approx_sync" type="bool" value="true"/>
 
       <param name="LccBow/MinInliers" type="string" value="10"/>
       <param name="LccBow/InlierDistance" type="string" value="0.02"/>
@@ -83,7 +87,7 @@
     <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="true"/>
       <param name="subscribe_odom_info" type="bool" value="true"/>
-      <param name="approx_sync" type="bool" value="false"/>
+      <param name="approx_sync" type="bool" value="true"/>
     
       <remap from="rgb/image" to="/kinect2/$(arg rgb_prefix)/image"/>
       <remap from="depth/image" to="/kinect2/$(arg depth_prefix)/image"/>
@@ -103,7 +107,7 @@
 
     <remap from="odom_in"             to="rtabmap/odom"/>
 
-    <param name="approx_sync" type="bool" value="false"/>
+    <param name="approx_sync" type="bool" value="true"/>
     
     <remap from="rgb/image_out"       to="data_odom_sync/image"/>
     <remap from="depth/image_out"     to="data_odom_sync/depth"/>