i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-09-16 00:00:20 +08:00
Code Issues Packages Projects Releases Wiki Activity

ros-pkg:

Browse Source 
-visual_odometry node is now called rgbd_odometry
-added stereo_odometry node with test launch files
-updated launch files accordingly to modified parameter names, default values or new parameters

git-svn-id: http://rtabmap.googlecode.com/svn/trunk/ros-pkg@1850 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2014-10-13 19:14:30 +00:00
parent 1df6c43d28
commit c0b0219e9e
28 changed files with 1721 additions and 1109 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.cproject
+1 -1
View File
@@ -20,7 +20,7 @@
<folderInfo id="0.250647335." name="/" resourcePath="">
<toolChain id="org.eclipse.cdt.build.core.prefbase.toolchain.1185920931" name="No ToolChain" resourceTypeBasedDiscovery="false" superClass="org.eclipse.cdt.build.core.prefbase.toolchain">
<targetPlatform id="org.eclipse.cdt.build.core.prefbase.toolchain.1185920931.1819283928" name=""/>
<builder arguments="VERBOSE=true" buildPath="${ProjDirPath}/../../build/rtabmap" command="make" id="org.eclipse.cdt.build.core.settings.default.builder.425794023" keepEnvironmentInBuildfile="false" managedBuildOn="false" name="Gnu Make Builder" superClass="org.eclipse.cdt.build.core.settings.default.builder"/>
<builder arguments="VERBOSE=true -j4" buildPath="${ProjDirPath}/../../build/rtabmap" command="make" id="org.eclipse.cdt.build.core.settings.default.builder.425794023" keepEnvironmentInBuildfile="false" managedBuildOn="false" name="Gnu Make Builder" superClass="org.eclipse.cdt.build.core.settings.default.builder"/>
<tool id="org.eclipse.cdt.build.core.settings.holder.libs.1445630059" name="holder for library settings" superClass="org.eclipse.cdt.build.core.settings.holder.libs"/>
<tool id="org.eclipse.cdt.build.core.settings.holder.1764610277" name="Assembly" superClass="org.eclipse.cdt.build.core.settings.holder">
<inputType id="org.eclipse.cdt.build.core.settings.holder.inType.1671812416" languageId="org.eclipse.cdt.core.assembly" languageName="Assembly" sourceContentType="org.eclipse.cdt.core.asmSource" superClass="org.eclipse.cdt.build.core.settings.holder.inType"/>
.project
+1 -1
View File
@@ -23,7 +23,7 @@
</dictionary>
<dictionary>
<key>org.eclipse.cdt.make.core.buildArguments</key>
<value>VERBOSE=true</value>
<value>VERBOSE=true -j4</value>
</dictionary>
<dictionary>
<key>org.eclipse.cdt.make.core.buildCommand</key>
CMakeLists.txt
+7 -2
View File
@@ -122,6 +122,7 @@ add_library(rtabmap_ros
src/nodelets/point_cloud_xyzrgb.cpp
src/nodelets/disparity_to_depth.cpp
src/MsgConversion.cpp
src/OdometryROS.cpp
src/rviz/MapCloudDisplay.cpp
src/rviz/MapGraphDisplay.cpp
src/rviz/InfoDisplay.cpp
@@ -139,8 +140,12 @@ add_executable(rtabmap src/CoreNode.cpp src/CoreWrapper.cpp)
add_dependencies(rtabmap rtabmap_generate_messages_cpp)
target_link_libraries(rtabmap rtabmap_ros ${Libraries})
add_executable(visual_odometry src/VisualOdometryNode.cpp)
target_link_libraries(visual_odometry rtabmap_ros ${Libraries})
add_executable(rgbd_odometry src/RGBDOdometryNode.cpp)
target_link_libraries(rgbd_odometry rtabmap_ros ${Libraries})
#deprecated node
add_executable(visual_odometry src/VisualOdometryDEPRECATED.cpp)
target_link_libraries(visual_odometry ${Libraries})
add_executable(stereo_odometry src/StereoOdometryNode.cpp)
target_link_libraries(stereo_odometry rtabmap_ros ${Libraries})
launch/demo/demo_multi-session_mapping.launch
+1
View File
@@ -29,6 +29,7 @@
<param name="RGBD/ScanMatchingSize" type="string" value="0"/>
<param name="RGBD/LocalLoopDetectionSpace" type="string" value="false"/>
<param name="RGBD/LocalLoopDetectionTime" type="string" value="false"/>
<param name="RGBD/OptimizeFromGraphEnd" type="string" value="true"/>
<param name="Mem/BadSignaturesIgnored" type="string" value="false"/>
<param name="LccIcp/Type" type="string" value="2"/>
<param name="LccIcp2/Iterations" type="string" value="100"/>
launch/demo/demo_robot_localization.launch
+1
View File
@@ -34,6 +34,7 @@
<param name="Mem/STMSize" type="string" value="1"/> <!-- 1 location in short-term memory -->
<param name="Mem/IncrementalMemory" type="string" value="false"/> <!-- false = Localization mode-->
<param name="RGBD/ScanMatchingSize" type="string" value="1"/> <!-- Do odometry correction with consecutive laser scans -->
<param name="RGBD/OptimizeFromGraphEnd" type="string" value="false"/>
<param name="LccIcp/Type" type="string" value="2"/> <!-- 0=No ICP, 1=ICP 3D, 2=ICP 2D -->
<param name="LccBow/MaxDepth" type="string" value="0.0"/> <!-- 3D visual words maximum depth 0=infinity -->
<param name="LccBow/InlierDistance" type="string" value="0.1"/> <!-- 3D visual words correspondence distance -->
launch/demo/demo_robot_mapping.launch
+1
View File
@@ -31,6 +31,7 @@
<param name="RGBD/ScanMatchingSize" type="string" value="1"/> <!-- Do odometry correction with consecutive laser scans -->
<param name="RGBD/LocalLoopDetectionSpace" type="string" value="true"/> <!-- Local loop closure detection (using estimated position) with locations in WM -->
<param name="RGBD/LocalLoopDetectionTime" type="string" value="true"/> <!-- Local loop closure detection with locations in STM -->
<param name="RGBD/OptimizeFromGraphEnd" type="string" value="true"/>
<param name="LccIcp/Type" type="string" value="2"/> <!-- 0=No ICP, 1=ICP 3D, 2=ICP 2D -->
<param name="LccBow/MaxDepth" type="string" value="0.0"/> <!-- 3D visual words maximum depth 0=infinity -->
<param name="LccBow/InlierDistance" type="string" value="0.1"/> <!-- 3D visual words correspondence distance -->
launch/demo/demo_robot_mapping_rviz.launch
+1
View File
@@ -32,6 +32,7 @@
<param name="RGBD/ScanMatchingSize" type="string" value="1"/> <!-- Do odometry correction with consecutive laser scans -->
<param name="RGBD/LocalLoopDetectionSpace" type="string" value="true"/> <!-- Local loop closure detection (using estimated position) with locations in WM -->
<param name="RGBD/LocalLoopDetectionTime" type="string" value="true"/> <!-- Local loop closure detection with locations in STM -->
<param name="RGBD/OptimizeFromGraphEnd" type="string" value="true"/>
<param name="LccIcp/Type" type="string" value="2"/> <!-- 0=No ICP, 1=ICP 3D, 2=ICP 2D -->
<param name="LccBow/MaxDepth" type="string" value="0.0"/> <!-- 3D visual words maximum depth 0=infinity -->
<param name="LccBow/InlierDistance" type="string" value="0.1"/> <!-- 3D visual words correspondence distance -->
launch/rgbd_localization.launch
+12 -8
View File
@@ -3,16 +3,18 @@
<!-- RGB-D LOCALIZATION VERSION -->
<!-- ODOMETRY ARGUMENTS: "type", "nn" and "local_map":
<!-- ODOMETRY ARGUMENTS: "strategy", "feature", "nn" and "local_map":
-Strategy: 0=BOW (bag-of-words) 1=Optical Flow
-Feature type: 0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK
-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
-Local map size: number of unique features to keep track
-->
<arg name="type" default="0" />
<arg name="nn" default="1" />
<arg name="local_map" default="0" />
<arg name="strategy" default="0" />
<arg name="feature" default="6" />
<arg name="nn" default="3" />
<arg name="local_map" default="1000" />
<!-- TF FRAMES -->
<node pkg="tf" type="static_transform_publisher" name="base_to_camera_tf"
@@ -21,16 +23,17 @@
<group ns="rtabmap">
<!-- Odometry -->
<node pkg="rtabmap" type="visual_odometry" name="visual_odometry" output="screen">
<node pkg="rtabmap" type="rgbd_odometry" name="visual_odometry" output="screen">
<remap from="rgb/image" to="/camera/rgb/image_rect_color"/>
<remap from="depth/image" to="/camera/depth_registered/image_raw"/>
<remap from="rgb/camera_info" to="/camera/depth_registered/camera_info"/>
<param name="frame_id" type="string" value="base_link"/>
<param name="Odom/Type" type="string" value="$(arg type)"/>
<param name="Odom/NearestNeighbor" type="string" value="$(arg nn)"/>
<param name="Odom/LocalHistory" type="string" value="$(arg local_map)"/>
<param name="Odom/Strategy" type="string" value="$(arg strategy)"/>
<param name="Odom/FeatureType" type="string" value="$(arg feature)"/>
<param name="OdomBow/NNType" type="string" value="$(arg nn)"/>
<param name="OdomBow/LocalHistorySize" type="string" value="$(arg local_map)"/>
</node>
<!-- Visual SLAM (robot side) -->
@@ -52,6 +55,7 @@
<param name="Mem/STMSize" type="string" value="1"/> <!-- 1 location in short-term memory -->
<param name="Mem/IncrementalMemory" type="string" value="false"/> <!-- false = Localization mode-->
<param name="LccBow/MinInliers" type="string" value="10"/> <!-- Minimum inliers to accept a loop closure -->
<param name="RGBD/OptimizeFromGraphEnd" type="string" value="false"/>
</node>
<!-- Visualisation (client side) -->
launch/rgbd_localization_rviz.launch
+90
View File
@@ -0,0 +1,90 @@
<launch>
<!-- RGB-D LOCALIZATION VERSION -->
<!-- ODOMETRY ARGUMENTS: "strategy", "feature", "nn" and "local_map":
-Strategy: 0=BOW (bag-of-words) 1=Optical Flow
-Feature type: 0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK
-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
-Local map size: number of unique features to keep track
-->
<arg name="strategy" default="0" />
<arg name="feature" default="6" />
<arg name="nn" default="3" />
<arg name="local_map" default="1000" />
<!-- TF FRAMES -->
<node pkg="tf" type="static_transform_publisher" name="base_to_camera_tf"
args="0.0 0.0 0.0 0.0 0.0 0.0 /base_link /camera_link 100" />
<group ns="rtabmap">
<!-- Odometry -->
<node pkg="rtabmap" type="rgbd_odometry" name="visual_odometry" output="screen">
<remap from="rgb/image" to="/camera/rgb/image_rect_color"/>
<remap from="depth/image" to="/camera/depth_registered/image_raw"/>
<remap from="rgb/camera_info" to="/camera/depth_registered/camera_info"/>
<param name="frame_id" type="string" value="base_link"/>
<param name="Odom/Strategy" type="string" value="$(arg strategy)"/>
<param name="Odom/FeatureType" type="string" value="$(arg feature)"/>
<param name="OdomBow/NNType" type="string" value="$(arg nn)"/>
<param name="OdomBow/LocalHistorySize" type="string" value="$(arg local_map)"/>
</node>
<!-- Visual SLAM (robot side) -->
<!-- args: "delete_db_on_start" and "udebug" -->
<node name="rtabmap" pkg="rtabmap" type="rtabmap" output="screen" args="">
<param name="subscribe_depth" type="bool" value="true"/>
<param name="subscribe_laserScan" type="bool" value="false"/>
<remap from="rgb/image" to="/camera/rgb/image_rect_color"/>
<remap from="depth/image" to="/camera/depth_registered/image_raw"/>
<remap from="rgb/camera_info" to="/camera/depth_registered/camera_info"/>
<remap from="odom" to="odom"/>
<param name="queue_size" type="int" value="30"/>
<!-- RTAB-Map's parameters: do "rosrun rtabmap rtabmap (double-dash)params" to see the list of available parameters. -->
<param name="Rtabmap/DatabasePath" type="string" value="~/.ros/rtabmap.db"/> <!-- Database used for localization -->
<param name="Rtabmap/DetectionRate" type="string" value="1"/> <!-- Don't need to do relocation very often! Though better results if the same rate as when mapping. -->
<param name="Mem/STMSize" type="string" value="1"/> <!-- 1 location in short-term memory -->
<param name="Mem/IncrementalMemory" type="string" value="false"/> <!-- false = Localization mode-->
<param name="LccBow/MinInliers" type="string" value="10"/> <!-- Minimum inliers to accept a loop closure -->
<param name="RGBD/OptimizeFromGraphEnd" type="string" value="false"/>
</node>
</group>
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap)/launch/config/rgbd.rviz"/>
<!-- sync cloud with odometry and voxelize the point cloud (for fast visualization in rviz) -->
<node pkg="nodelet" type="nodelet" name="standalone_nodelet" args="manager" output="screen"/>
<node pkg="nodelet" type="nodelet" name="data_odom_sync" args="load rtabmap/data_odom_sync standalone_nodelet">
<remap from="rgb/image_in" to="camera/rgb/image_rect_color"/>
<remap from="depth/image_in" to="camera/depth_registered/image_raw"/>
<remap from="rgb/camera_info_in" to="camera/depth_registered/camera_info"/>
<remap from="odom_in" to="rtabmap/odom"/>
<remap from="rgb/image_out" to="data_odom_sync/image"/>
<remap from="depth/image_out" to="data_odom_sync/depth"/>
<remap from="rgb/camera_info_out" to="data_odom_sync/camera_info"/>
<remap from="odom_out" to="odom_sync"/>
<param name="queue_size" type="int" value="30"/>
</node>
<node pkg="nodelet" type="nodelet" name="points_xyzrgb" args="load rtabmap/point_cloud_xyzrgb standalone_nodelet">
<remap from="rgb/image" to="data_odom_sync/image"/>
<remap from="depth/image" to="data_odom_sync/depth"/>
<remap from="rgb/camera_info" to="data_odom_sync/camera_info"/>
<remap from="cloud" to="voxel_cloud" />
<param name="queue_size" type="int" value="10"/>
<param name="voxel_size" type="double" value="0.01"/>
</node>
</launch>
launch/rgbd_mapping.launch
+10 -7
View File
@@ -5,16 +5,18 @@
<!-- WARNING : Database is automatically deleted on each startup -->
<!-- See "delete_db_on_start" option below... -->
<!-- ODOMETRY ARGUMENTS: "type", "nn" and "local_map":
<!-- ODOMETRY ARGUMENTS: "strategy", "feature", "nn" and "local_map":
-Strategy: 0=BOW (bag-of-words) 1=Optical Flow
-Feature type: 0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK
-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
-Local map size: number of unique features to keep track
-->
<arg name="type" default="6" />
<arg name="strategy" default="0" />
<arg name="feature" default="6" />
<arg name="nn" default="3" />
<arg name="local_map" default="2000" />
<arg name="local_map" default="1000" />
<!-- TF FRAMES -->
<node pkg="tf" type="static_transform_publisher" name="base_to_camera_tf"
@@ -23,14 +25,15 @@
<group ns="rtabmap">
<!-- Odometry -->
<node pkg="rtabmap" type="visual_odometry" name="visual_odometry" output="screen">
<node pkg="rtabmap" type="rgbd_odometry" name="visual_odometry" output="screen">
<remap from="rgb/image" to="/camera/rgb/image_rect_color"/>
<remap from="depth/image" to="/camera/depth_registered/image_raw"/>
<remap from="rgb/camera_info" to="/camera/depth_registered/camera_info"/>
<param name="Odom/Type" type="string" value="$(arg type)"/>
<param name="Odom/NearestNeighbor" type="string" value="$(arg nn)"/>
<param name="Odom/LocalHistory" type="string" value="$(arg local_map)"/>
<param name="Odom/Strategy" type="string" value="$(arg strategy)"/>
<param name="Odom/FeatureType" type="string" value="$(arg feature)"/>
<param name="OdomBow/NNType" type="string" value="$(arg nn)"/>
<param name="OdomBow/LocalHistorySize" type="string" value="$(arg local_map)"/>
<param name="frame_id" type="string" value="base_link"/>
</node>
launch/rgbd_mapping_rviz.launch
+10 -7
View File
@@ -5,16 +5,18 @@
<!-- WARNING : Database is automatically deleted on each startup -->
<!-- See "delete_db_on_start" option below... -->
<!-- ODOMETRY ARGUMENTS: "type", "nn" and "local_map":
<!-- ODOMETRY ARGUMENTS: "strategy", "feature", "nn" and "local_map":
-Strategy: 0=BOW (bag-of-words) 1=Optical Flow
-Feature type: 0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK
-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
-Local map size: number of unique features to keep track
-->
<arg name="type" default="6" />
<arg name="strategy" default="0" />
<arg name="feature" default="6" />
<arg name="nn" default="3" />
<arg name="local_map" default="2000" />
<arg name="local_map" default="1000" />
<!-- TF FRAMES -->
@@ -24,14 +26,15 @@
<group ns="rtabmap">
<!-- Odometry -->
<node pkg="rtabmap" type="visual_odometry" name="visual_odometry" output="screen">
<node pkg="rtabmap" type="rgbd_odometry" name="visual_odometry" output="screen">
<remap from="rgb/image" to="/camera/rgb/image_rect_color"/>
<remap from="depth/image" to="/camera/depth_registered/image_raw"/>
<remap from="rgb/camera_info" to="/camera/depth_registered/camera_info"/>
<param name="Odom/Type" type="string" value="$(arg type)"/>
<param name="Odom/NearestNeighbor" type="string" value="$(arg nn)"/>
<param name="Odom/LocalHistory" type="string" value="$(arg local_map)"/>
<param name="Odom/Strategy" type="string" value="$(arg strategy)"/>
<param name="Odom/FeatureType" type="string" value="$(arg feature)"/>
<param name="OdomBow/NNType" type="string" value="$(arg nn)"/>
<param name="OdomBow/LocalHistorySize" type="string" value="$(arg local_map)"/>
<param name="frame_id" type="string" value="base_link"/>
</node>
launch/tests/bumblebee.launch
+26
View File
@@ -0,0 +1,26 @@
<launch>
<node pkg="camera1394stereo" type="camera1394stereo_node" name="camera1394stereo_node" output="screen" >
<param name="video_mode" value="format7_mode3" />
<param name="format7_color_coding" value="raw16" />
<param name="bayer_pattern" value="bggr" />
<param name="bayer_method" value="" />
<param name="stereo_method" value="Interlaced" />
<param name="camera_info_url_left" value="" />
<param name="camera_info_url_right" value="" />
</node>
<arg name="pi/2" value="1.5707963267948966" />
<arg name="optical_rotate" value="0 0 0 -$(arg pi/2) 0 -$(arg pi/2)" />
<node pkg="tf" type="static_transform_publisher" name="camera_base_link"
args="$(arg optical_rotate) base_link stereo_camera 100" />
<!-- Run the ROS package stereo_image_proc -->
<group ns="/stereo_camera" >
<node pkg="stereo_image_proc" type="stereo_image_proc" name="stereo_image_proc"/>
<node pkg="nodelet" type="nodelet" name="disparity2depth" args="standalone rtabmap/disparity_to_depth"/>
</group>
</launch>
launch/tests/test_stereo_data_recorder.launch
+86
View File
@@ -0,0 +1,86 @@
<launch>
<!-- from bag: rosbag record /stereo_camera/left/image_rect/compressed /stereo_camera/right/image_rect/compressed /stereo_camera/left/camera_info /stereo_camera/right/camera_info /stereo_camera/depth/compressedDepth -->
<param name="use_sim_time" type="bool" value="True"/>
<arg name="pi/2" value="1.5707963267948966" />
<arg name="optical_rotate" value="0 0 0 -$(arg pi/2) 0 -$(arg pi/2)" />
<node pkg="tf" type="static_transform_publisher" name="camera_base_link"
args="$(arg optical_rotate) base_link stereo_camera 100" />
<!-- Odometry: Choose between viso2_ros, fovis_ros and homemade approach (default) -->
<!--
<node pkg="viso2_ros" type="stereo_odometer" name="stereo_odometer" output="screen">
<remap from="stereo" to="/stereo_camera"/>
<remap from="image" to="image_rect"/>
<param name="base_link_frame_id" value="base_link"/>
<param name="odom_frame_id" value="/odom"/>
<param name="ref_frame_change_method" value="1"/>
<param name="image_transport" type="string" value="compressed"/>
</node>
-->
<node pkg="rtabmap" type="stereo_odometry" name="stereo_odometer" output="screen">
<remap from="left/image_rect" to="/stereo_camera/left/image_rect"/>
<remap from="right/image_rect" to="/stereo_camera/right/image_rect"/>
<remap from="left/camera_info" to="/stereo_camera/left/camera_info"/>
<remap from="right/camera_info" to="/stereo_camera/right/camera_info"/>
<remap from="odom" to="/stereo_odometer/odometry"/>
<remap from="odom_depth" to="/stereo_camera/depth2"/>
<param name="left/image_transport" type="string" value="compressed"/>
<param name="right/image_transport" type="string" value="compressed"/>
<param name="frame_id" type="string" value="base_link"/>
<param name="approx_sync" type="bool" value="false"/>
<param name="frame_id" type="string" value="base_link"/>
<param name="approx_sync" type="bool" value="false"/>
<param name="Odom/Strategy" type="string" value="1"/>
<param name="Odom/RoiRatios" type="string" value="0.03 0.03 0.04 0.04"/>
<param name="Odom/MaxDepth" type="string" value="10"/>
<param name="generate_depth" type="bool" value="false"/>
<!-- Parameters below only used when "generate_depth"=true -->
<param name="depth_patch_size" type="int" value="1"/>
<param name="subpix_win_size" type="int" value="3"/>
<param name="subpix_iterations" type="int" value="20"/>
<param name="subpix_epsilon" type="double" value="0.02"/>
<param name="flow_win_size" type="int" value="9"/>
<param name="flow_max_level" type="int" value="4"/>
<param name="flow_iterations" type="int" value="20"/>
<param name="flow_epsilon" type="double" value="0.02"/>
</node>
<node name="data_recorder" pkg="rtabmap" type="data_recorder" output="screen">
<param name="output_file_name" value="output.db" type="string"/>
<param name="frame_id" type="string" value="base_link"/>
<param name="subscribe_odometry" type="bool" value="true"/>
<param name="subscribe_depth" type="bool" value="false"/>
<param name="subscribe_stereo" type="bool" value="true"/>
<param name="queue_size" type="int" value="20"/>
<remap from="left/image_rect" to="/stereo_camera/left/image_rect"/>
<remap from="right/image_rect" to="/stereo_camera/right/image_rect"/>
<remap from="left/camera_info" to="/stereo_camera/left/camera_info"/>
<remap from="right/camera_info" to="/stereo_camera/right/camera_info"/>
<remap from="rgb/image" to="/stereo_camera/left/image_rect"/>
<remap from="depth/image" to="/stereo_camera/depth2"/>
<remap from="rgb/camera_info" to="/stereo_camera/left/camera_info"/>
<remap from="odom" to="/stereo_odometer/odometry"/>
<param name="rgb/image_transport" type="string" value="compressed"/>
<param name="left/image_transport" type="string" value="compressed"/>
<param name="right/image_transport" type="string" value="compressed"/>
<param name="queue_size" type="int" value="10"/>
</node>
</launch>
launch/tests/test_stereo_mapping.launch
+37 -26
View File
@@ -28,15 +28,14 @@
</group>
<!-- Odometry: Choose between viso2_ros, fovis_ros and homemade approach (default) -->
<!--
<node pkg="viso2_ros" type="stereo_odometer" name="stereo_odometer" output="screen">
<remap from="stereo" to="/stereo_camera"/>
<remap from="image" to="image_rect"/>
<param name="base_link_frame_id" value="/base_link"/>
<param name="base_link_frame_id" value="base_link"/>
<param name="odom_frame_id" value="/odom"/>
<param name="ref_frame_change_method" value="1"/>
</node>
-->
<!--
<node pkg="fovis_ros" type="fovis_stereo_odometer" name="stereo_odometer" >
<remap from="/stereo/left/image" to="/stereo_camera/left/image_rect" />
@@ -46,23 +45,34 @@
<remap from="odometry" to="stereo_odometer/odometry" />
</node>
-->
<!--
<node pkg="rtabmap" type="stereo_odometry" name="stereo_odometer" output="screen">
<remap from="left/image_rect" to="/stereo_camera/left/image_rect_color"/>
<remap from="right/image_rect" to="/stereo_camera/right/image_rect_color"/>
<remap from="left/image_rect" to="/stereo_camera/left/image_rect"/>
<remap from="right/image_rect" to="/stereo_camera/right/image_rect"/>
<remap from="left/camera_info" to="/stereo_camera/left/camera_info"/>
<remap from="right/camera_info" to="/stereo_camera/right/camera_info"/>
<remap from="odom" to="/stereo_odometer/odometry"/>
<remap from="odom_depth" to="/stereo_camera/depth2"/>
<param name="frame_id" type="string" value="base_link"/>
<param name="approx_sync" type="bool" value="false"/>
<param name="Odom/Type" type="string" value="6"/>
<param name="Odom/NearestNeighbor" type="string" value="3"/>
<param name="Odom/LocalHistory" type="string" value="500"/>
<param name="Odom/MaxDepth" type="string" value="3"/>
<param name="Odom/NNDR" type="string" value="0.6"/>
<param name="GFTT/MaxCorners" type="string" value="1000"/>
<param name="BRIEF/Bytes" type="string" value="16"/>
<param name="Odom/Strategy" type="string" value="1"/>
<param name="Odom/RoiRatios" type="string" value="0.03 0.03 0.04 0.04"/>
<param name="Odom/MaxDepth" type="string" value="10"/>
<param name="generate_depth" type="bool" value="false"/>
<!-- Parameters below only used when "generate_depth"=true -->
<param name="depth_patch_size" type="int" value="1"/>
<param name="subpix_win_size" type="int" value="3"/>
<param name="subpix_iterations" type="int" value="20"/>
<param name="subpix_epsilon" type="double" value="0.02"/>
<param name="flow_win_size" type="int" value="9"/>
<param name="flow_max_level" type="int" value="4"/>
<param name="flow_iterations" type="int" value="20"/>
<param name="flow_epsilon" type="double" value="0.02"/>
</node>
-->
<group ns="rtabmap">
<!-- Visual SLAM (robot side) -->
@@ -78,14 +88,14 @@
<remap from="odom" to="/stereo_odometer/odometry"/>
<param name="frame_id" type="string" value="/base_link"/>
<param name="queue_size" type="int" value="50"/>
<param name="frame_id" type="string" value="base_link"/>
<param name="queue_size" type="int" value="30"/>
<param name="Rtabmap/TimeThr" type="string" value="700"/>
<param name="Rtabmap/DetectionRate" type="string" value="1"/>
<param name="SURF/HessianThreshold" type="string" value="600"/>
<param name="RGBD/LocalLoopDetectionSpace" type="string" value="false"/>
<param name="RGBD/LocalLoopDetectionTime" type="string" value="false"/>
<param name="Rtabmap/DetectorStrategy" type="string" value="6"/>
<param name="NN/NNStrategy" type="string" value="3"/>
<param name="GFTT/MaxCorners" type="string" value="200"/>
<param name="LccBow/MinInliers" type="string" value="10"/>
<param name="LccBow/InlierDistance" type="string" value="0.02"/>
@@ -99,6 +109,7 @@
</node>
<!-- Visualisation (client side) -->
<!--
<node pkg="rtabmap" type="rtabmapviz" name="rtabmapviz" args="-d $(find rtabmap)/launch/config/rgbd_gui.ini" output="screen">
<param name="subscribe_depth" type="bool" value="true"/>
<param name="subscribe_laserScan" type="bool" value="false"/>
@@ -106,28 +117,29 @@
<remap from="rgb/image" to="/stereo_camera/left/image_rect_color"/>
<remap from="rgb/camera_info" to="/stereo_camera/left/camera_info"/>
<remap from="depth/image" to="/stereo_camera/depth"/>
<remap from="depth/image" to="/odom_depth"/>
<remap from="odom" to="/stereo_odometer/odometry"/>
</node>
-->
</group>
<!-- RVIZ -->
<!--
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap)/launch/config/rgbd.rviz"/>
<node pkg="nodelet" type="nodelet" name="standalone_nodelet" args="manager" output="screen"/>
<node pkg="nodelet" type="nodelet" name="data_odom_sync" args="load rtabmap/data_odom_sync standalone_nodelet">
<remap from="rgb/image_in" to="camera/rgb/image_rect_color"/>
<remap from="depth/image_in" to="camera/depth_registered/image_raw"/>
<remap from="rgb/camera_info_in" to="camera/depth_registered/camera_info"/>
<remap from="odom_in" to="rtabmap/odom"/>
<remap from="rgb/image_in" to="/stereo_camera/left/image_rect_color"/>
<remap from="depth/image_in" to="/odom_depth"/>
<remap from="rgb/camera_info_in" to="/stereo_camera/left/camera_info"/>
<remap from="odom_in" to="/stereo_odometer/odom"/>
<remap from="rgb/image_out" to="data_odom_sync/image"/>
<remap from="depth/image_out" to="data_odom_sync/depth"/>
<remap from="rgb/camera_info_out" to="data_odom_sync/camera_info"/>
<remap from="odom_out" to="odom_sync"/>
<param name="queue_size" type="int" value="50"/>
<param name="queue_size" type="int" value="30"/>
</node>
<node pkg="nodelet" type="nodelet" name="points_xyzrgb" args="load rtabmap/point_cloud_xyzrgb standalone_nodelet">
<remap from="rgb/image" to="data_odom_sync/image"/>
@@ -136,7 +148,6 @@
<remap from="cloud" to="voxel_cloud" />
<param name="queue_size" type="int" value="10"/>
<param name="voxel_size" type="double" value="0.01"/>
</node>
-->
</launch>
launch/tests/test_stereo_mapping_bag.launch
+146
View File
@@ -0,0 +1,146 @@
<launch>
<!-- from bag: rosbag record /stereo_camera/left/image_rect/compressed /stereo_camera/right/image_rect/compressed /stereo_camera/left/camera_info /stereo_camera/right/camera_info /stereo_camera/depth/compressedDepth -->
<param name="use_sim_time" type="bool" value="True"/>
<arg name="pi/2" value="1.5707963267948966" />
<arg name="optical_rotate" value="0 0 0 -$(arg pi/2) 0 -$(arg pi/2)" />
<node pkg="tf" type="static_transform_publisher" name="camera_base_link"
args="$(arg optical_rotate) base_link stereo_camera 100" />
<!-- Odometry: Choose between viso2_ros, fovis_ros and homemade approach (default) -->
<node pkg="viso2_ros" type="stereo_odometer" name="stereo_odometer" output="screen">
<remap from="stereo" to="/stereo_camera"/>
<remap from="image" to="image_rect"/>
<param name="base_link_frame_id" value="base_link"/>
<param name="odom_frame_id" value="/odom"/>
<param name="ref_frame_change_method" value="1"/>
<param name="image_transport" type="string" value="compressed"/>
</node>
<!--
<node pkg="fovis_ros" type="fovis_stereo_odometer" name="stereo_odometer" >
<remap from="/stereo/left/image" to="/stereo_camera/left/image_rect" />
<remap from="/stereo/right/image" to="/stereo_camera/right/image_rect" />
<remap from="/stereo/left/camera_info" to="/stereo_camera/left/camera_info" />
<remap from="/stereo/right/camera_info" to="/stereo_camera/right/camera_info" />
<remap from="odometry" to="stereo_odometer/odometry" />
</node>
-->
<!--
<node pkg="rtabmap" type="stereo_odometry" name="stereo_odometer" output="screen">
<remap from="left/image_rect" to="/stereo_camera/left/image_rect"/>
<remap from="right/image_rect" to="/stereo_camera/right/image_rect"/>
<remap from="left/camera_info" to="/stereo_camera/left/camera_info"/>
<remap from="right/camera_info" to="/stereo_camera/right/camera_info"/>
<remap from="odom" to="/stereo_odometer/odometry"/>
<remap from="odom_depth" to="/stereo_camera/depth2"/>
<param name="left/image_transport" type="string" value="compressed"/>
<param name="right/image_transport" type="string" value="compressed"/>
<param name="frame_id" type="string" value="base_link"/>
<param name="approx_sync" type="bool" value="false"/>
<param name="Odom/Strategy" type="string" value="1"/>
<param name="Odom/RoiRatios" type="string" value="0.03 0.03 0.04 0.04"/>
<param name="Odom/MaxDepth" type="string" value="10"/>
<param name="generate_depth" type="bool" value="false"/>
<!-- Parameters below only used when "generate_depth"=true -->
<param name="depth_patch_size" type="int" value="1"/>
<param name="subpix_win_size" type="int" value="3"/>
<param name="subpix_iterations" type="int" value="20"/>
<param name="subpix_epsilon" type="double" value="0.02"/>
<param name="flow_win_size" type="int" value="9"/>
<param name="flow_max_level" type="int" value="4"/>
<param name="flow_iterations" type="int" value="20"/>
<param name="flow_epsilon" type="double" value="0.02"/>
</node>
-->
<group ns="rtabmap">
<!-- Visual SLAM (robot side) -->
<!-- args: "delete_db_on_start" and "udebug" -->
<node name="rtabmap" pkg="rtabmap" type="rtabmap" output="screen" args="--delete_db_on_start">
<param name="subscribe_depth" type="bool" value="true"/>
<param name="subscribe_laserScan" type="bool" value="false"/>
<remap from="rgb/image" to="/stereo_camera/left/image_rect"/>
<remap from="rgb/camera_info" to="/stereo_camera/left/camera_info"/>
<remap from="depth/image" to="/stereo_camera/depth"/>
<remap from="odom" to="/stereo_odometer/odometry"/>
<param name="rgb/image_transport" type="string" value="compressed"/>
<param name="depth/image_transport" type="string" value="compressedDepth"/>
<param name="frame_id" type="string" value="base_link"/>
<param name="queue_size" type="int" value="30"/>
<param name="Rtabmap/TimeThr" type="string" value="700"/>
<param name="Rtabmap/DetectionRate" type="string" value="1"/>
<param name="Rtabmap/DetectorStrategy" type="string" value="6"/>
<param name="NN/NNStrategy" type="string" value="3"/>
<param name="GFTT/MaxCorners" type="string" value="200"/>
<param name="LccBow/MinInliers" type="string" value="10"/>
<param name="LccBow/InlierDistance" type="string" value="0.02"/>
<param name="LccBow/MaxDepth" type="string" value="3"/>
<param name="LccReextract/FeatureType" type="string" value="4"/>
<param name="LccReextract/LoopClosureFeatures" type="string" value="true"/>
<param name="LccReextract/MaxDepth" type="string" value="3"/>
<param name="LccReextract/NNDR" type="string" value="0.8"/>
<param name="LccReextract/NNType" type="string" value="3"/>
</node>
<!-- Visualisation (client side) -->
<!--
<node pkg="rtabmap" type="rtabmapviz" name="rtabmapviz" args="-d $(find rtabmap)/launch/config/rgbd_gui.ini" output="screen">
<param name="subscribe_depth" type="bool" value="true"/>
<param name="subscribe_laserScan" type="bool" value="false"/>
<param name="queue_size" type="int" value="30"/>
<remap from="rgb/image" to="/stereo_camera/left/image_rect"/>
<remap from="rgb/camera_info" to="/stereo_camera/left/camera_info"/>
<remap from="depth/image" to="/stereo_camera/depth"/>
<remap from="odom" to="/stereo_odometer/odometry"/>
<param name="rgb/image_transport" type="string" value="compressed"/>
<param name="depth/image_transport" type="string" value="compressedDepth"/>
<remap from="odom" to="/stereo_odometer/odometry"/>
</node>
-->
</group>
<!-- RVIZ -->
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap)/launch/config/rgbd.rviz"/>
<node pkg="nodelet" type="nodelet" name="standalone_nodelet" args="manager" output="screen"/>
<node pkg="nodelet" type="nodelet" name="data_odom_sync" args="load rtabmap/data_odom_sync standalone_nodelet">
<remap from="rgb/image_in" to="/stereo_camera/left/image_rect_color"/>
<remap from="depth/image_in" to="/odom_depth"/>
<remap from="rgb/camera_info_in" to="/stereo_camera/left/camera_info"/>
<remap from="odom_in" to="/stereo_odometer/odom"/>
<remap from="rgb/image_out" to="data_odom_sync/image"/>
<remap from="depth/image_out" to="data_odom_sync/depth"/>
<remap from="rgb/camera_info_out" to="data_odom_sync/camera_info"/>
<remap from="odom_out" to="odom_sync"/>
<param name="queue_size" type="int" value="30"/>
</node>
<node pkg="nodelet" type="nodelet" name="points_xyzrgb" args="load rtabmap/point_cloud_xyzrgb standalone_nodelet">
<remap from="rgb/image" to="data_odom_sync/image"/>
<remap from="depth/image" to="data_odom_sync/depth"/>
<remap from="rgb/camera_info" to="data_odom_sync/camera_info"/>
<remap from="cloud" to="voxel_cloud" />
<param name="queue_size" type="int" value="10"/>
</node>
</launch>
launch/tests/test_stereo_odometry.launch
+17 -25
View File
@@ -1,16 +1,5 @@
<launch>
<!-- ODOMETRY ARGUMENTS: "type", "nn" and "local_map":
-Feature type: 0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK
-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
-Local map size: number of unique features to keep track
-->
<arg name="type" default="6" />
<arg name="nn" default="3" />
<arg name="local_map" default="5000" />
<node pkg="camera1394stereo" type="camera1394stereo_node" name="camera1394stereo_node" output="screen" >
<param name="video_mode" value="format7_mode3" />
<param name="format7_color_coding" value="raw16" />
@@ -32,8 +21,8 @@
<!-- Odometry -->
<node pkg="rtabmap" type="stereo_odometry" name="stereo_odometry" output="screen">
<remap from="left/image_rect" to="left/image_rect_color"/>
<remap from="right/image_rect" to="right/image_rect_color"/>
<remap from="left/image_rect" to="left/image_rect"/>
<remap from="right/image_rect" to="right/image_rect"/>
<remap from="left/camera_info" to="left/camera_info"/>
<remap from="right/camera_info" to="right/camera_info"/>
<remap from="odom" to="odom_stereo"/>
@@ -43,20 +32,23 @@
<param name="frame_id" type="string" value="base_link"/>
<param name="odom_frame_id" type="string" value="odom"/>
<param name="approx_sync" type="bool" value="false"/>
<param name="queue_size" type="int" value="5"/>
<param name="min_disparity" type="int" value="0"/>
<param name="max_disparity" type="int" value="256"/>
<param name="k" type="int" value="10"/>
<param name="window_size" type="int" value="5"/>
<param name="Odom/Strategy" type="string" value="1"/>
<param name="Odom/RoiRatios" type="string" value="0.03 0.03 0.04 0.04"/>
<param name="Odom/MaxDepth" type="string" value="10"/>
<param name="Odom/Type" type="string" value="$(arg type)"/>
<param name="Odom/NearestNeighbor" type="string" value="$(arg nn)"/>
<param name="Odom/LocalHistory" type="string" value="$(arg local_map)"/>
<param name="Odom/MaxDepth" type="string" value="3"/>
<param name="Odom/NNDR" type="string" value="0.6"/>
<param name="GFTT/MaxCorners" type="string" value="1000"/>
<param name="BRIEF/Bytes" type="string" value="16"/>
<param name="generate_depth" type="bool" value="false"/>
<!-- Parameters below only used when "generate_depth"=true -->
<param name="depth_patch_size" type="int" value="1"/>
<param name="subpix_win_size" type="int" value="3"/>
<param name="subpix_iterations" type="int" value="20"/>
<param name="subpix_epsilon" type="double" value="0.02"/>
<param name="flow_win_size" type="int" value="9"/>
<param name="flow_max_level" type="int" value="4"/>
<param name="flow_iterations" type="int" value="20"/>
<param name="flow_epsilon" type="double" value="0.02"/>
</node>
</group>
src/CoreNode.cpp
+1 -2
View File
@@ -69,8 +69,7 @@ int main(int argc, char** argv)
// hide specific parameters
for(rtabmap::ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end();)
{
if(uSplit(iter->first, '/').front().compare("Odom") == 0 ||
uSplit(iter->first, '/').front().compare("OdomICP") == 0)
if(iter->first.find("Odom") == 0)
{
parameters.erase(iter++);
}
src/CoreWrapper.cpp
+28 -24
View File
@@ -394,20 +394,22 @@ void CoreWrapper::depthCallback(
}
cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depthMsg);
float depthFx = cameraInfoMsg->K[0];
float depthFy = cameraInfoMsg->K[4];
float depthCx = cameraInfoMsg->K[2];
float depthCy = cameraInfoMsg->K[5];
image_geometry::PinholeCameraModel model;
model.fromCameraInfo(*cameraInfoMsg);
float fx = model.fx();
float fy = model.fy();
float cx = model.cx();
float cy = model.cy();
process(ptrImage->header.seq,
ptrImage->image,
odom,
odomMsg->header.frame_id,
ptrDepth->image,
depthFx,
depthFy,
depthCx,
depthCy,
fx,
fy,
cx,
cy,
localTransform,
cv::Mat());
}
@@ -479,20 +481,22 @@ void CoreWrapper::depthScanCallback(
}
cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depthMsg);
float depthFx = cameraInfoMsg->K[0];
float depthFy = cameraInfoMsg->K[4];
float depthCx = cameraInfoMsg->K[2];
float depthCy = cameraInfoMsg->K[5];
image_geometry::PinholeCameraModel model;
model.fromCameraInfo(*cameraInfoMsg);
float fx = model.fx();
float fy = model.fy();
float cx = model.cx();
float cy = model.cy();
process(ptrImage->header.seq,
ptrImage->image,
odom,
odomMsg->header.frame_id,
ptrDepth->image,
depthFx,
depthFy,
depthCx,
depthCy,
fx,
fy,
cx,
cy,
localTransform,
scan);
}
@@ -504,10 +508,10 @@ void CoreWrapper::process(
const Transform & odom,
const std::string & odomFrameId,
const cv::Mat & depth,
float depthFx,
float depthFy,
float depthCx,
float depthCy,
float fx,
float fy,
float cx,
float cy,
const Transform & localTransform,
const cv::Mat & scan)
{
@@ -543,10 +547,10 @@ void CoreWrapper::process(
SensorData data(image.clone(),
depth16,
scan,
depthFx,
depthFy,
depthCx,
depthCy,
fx,
fy,
cx,
cy,
odom,
localTransform,
id);
src/CoreWrapper.h
+4 -4
View File
@@ -86,10 +86,10 @@ private:
const rtabmap::Transform & odom = rtabmap::Transform(),
const std::string & odomFrameId = "",
const cv::Mat & depth = cv::Mat(),
float depthFx = 0.0f,
float depthFy = 0.0f,
float depthCx = 0.0f,
float depthCy = 0.0f,
float fx = 0.0f,
float fy = 0.0f,
float cx = 0.0f,
float cy = 0.0f,
const rtabmap::Transform & localTransform = rtabmap::Transform(),
const cv::Mat & scan = cv::Mat());
src/DataRecorderNode.cpp
+193 -38
View File
@@ -42,10 +42,13 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/sync_policies/exact_time.h>
#include <image_transport/image_transport.h>
#include <image_transport/subscriber_filter.h>
#include <image_geometry/stereo_camera_model.h>
#include "rtabmap/MsgConversion.h"
#include <pcl_ros/transforms.h>
@@ -77,15 +80,18 @@ public:
bool subscribeOdometry = false;
bool subscribeLaserScan = false;
bool subscribeDepth = false;
bool subscribeStereo = false;
int queueSize = 10;
bool showGUI = true;
pnh.param("subscribe_odometry", subscribeOdometry, subscribeOdometry);
pnh.param("subscribe_depth", subscribeDepth, subscribeDepth);
pnh.param("subscribe_stereo", subscribeStereo, subscribeStereo);
pnh.param("subscribe_laserScan", subscribeLaserScan, subscribeLaserScan);
pnh.param("queue_size", queueSize, queueSize);
pnh.param("output_file_name", fileName_, fileName_);
pnh.param("frame_id", frameId_, frameId_);
setupCallbacks(subscribeOdometry, subscribeDepth, subscribeLaserScan, queueSize);
setupCallbacks(subscribeOdometry, subscribeDepth, subscribeStereo, subscribeLaserScan, queueSize);
}
bool init()
{
@@ -108,8 +114,42 @@ private:
void setupCallbacks(
bool subscribeOdom,
bool subscribeDepth,
bool subscribeStereo,
bool subscribeLaserScan,
int queueSize)
{
if(subscribeStereo)
{
ros::NodeHandle nh; // public
ros::NodeHandle pnh("~"); // private
ros::NodeHandle left_nh(nh, "left");
ros::NodeHandle right_nh(nh, "right");
ros::NodeHandle left_pnh(pnh, "left");
ros::NodeHandle right_pnh(pnh, "right");
image_transport::ImageTransport left_it(left_nh);
image_transport::ImageTransport right_it(right_nh);
image_transport::TransportHints hintsLeft("raw", ros::TransportHints(), left_pnh);
image_transport::TransportHints hintsRight("raw", ros::TransportHints(), right_pnh);
imageSub_.subscribe(left_it, left_nh.resolveName("image_rect"), 1, hintsLeft);
imageRightSub_.subscribe(right_it, right_nh.resolveName("image_rect"), 1, hintsRight);
cameraInfoSub_.subscribe(left_nh, "camera_info", 1);
cameraInfoRightSub_.subscribe(right_nh, "camera_info", 1);
if(subscribeOdom)
{
ROS_INFO("Registering Stero+Odom callback...");
odomSub_.subscribe(nh, "odom", 1);
stereoOdomSync_ = new message_filters::Synchronizer<MyStereoOdomSyncPolicy>(MyStereoOdomSyncPolicy(queueSize), imageSub_, imageRightSub_, cameraInfoSub_, cameraInfoRightSub_, odomSub_);
stereoOdomSync_->registerCallback(boost::bind(&DataRecorderWrapper::stereoOdomCallback, this, _1, _2, _3, _4, _5));
}
else
{
ROS_INFO("Registering Stero callback...");
stereoSync_ = new message_filters::Synchronizer<MyStereoSyncPolicy>(MyStereoSyncPolicy(queueSize), imageSub_, imageRightSub_, cameraInfoSub_, cameraInfoRightSub_);
stereoSync_->registerCallback(boost::bind(&DataRecorderWrapper::stereoCallback, this, _1, _2, _3, _4));
}
}
else
{
ros::NodeHandle nh; // public
ros::NodeHandle pnh("~"); // private
@@ -167,11 +207,12 @@ private:
defaultSub_ = rgb_it.subscribe("image", 1, &DataRecorderWrapper::defaultCallback, this);
}
}
}
void defaultCallback(const sensor_msgs::ImageConstPtr & imageMsg)
{
cv_bridge::CvImageConstPtr ptrImage = cv_bridge::toCvShare(imageMsg, "bgr8");
rtabmap::SensorData image(
rtabmap::SensorData data(
ptrImage->image.clone(),
cv::Mat(),
cv::Mat(),
@@ -181,7 +222,7 @@ private:
0.0f,
Transform(),
Transform());
recorder_.addData(image);
recorder_.addData(data);
}
void depthImageCallback(
@@ -206,10 +247,12 @@ private:
cv_bridge::CvImageConstPtr ptrImage = cv_bridge::toCvShare(imageMsg, "bgr8");
cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depthMsg);
float depthFx = cameraInfoMsg->K[0];
float depthFy = cameraInfoMsg->K[4];
float depthCx = cameraInfoMsg->K[2];
float depthCy = cameraInfoMsg->K[5];
image_geometry::PinholeCameraModel model;
model.fromCameraInfo(*cameraInfoMsg);
float fx = model.fx();
float fy = model.fy();
float cx = model.cx();
float cy = model.cy();
cv::Mat depth16;
if(ptrDepth->image.type() != CV_16UC1)
@@ -234,20 +277,20 @@ private:
}
else
{
depth16 = ptrDepth->image;
depth16 = ptrDepth->image.clone();
}
rtabmap::SensorData image(
rtabmap::SensorData data(
ptrImage->image.clone(),
depth16,
cv::Mat(),
depthFx,
depthFy,
depthCx,
depthCy,
fx,
fy,
cx,
cy,
Transform(),
localTransform);
recorder_.addData(image);
recorder_.addData(data);
}
void depthCallback(
@@ -275,10 +318,12 @@ private:
cv_bridge::CvImageConstPtr ptrImage = cv_bridge::toCvShare(imageMsg, "bgr8");
cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depthMsg);
float depthFx = cameraInfoMsg->K[0];
float depthFy = cameraInfoMsg->K[4];
float depthCx = cameraInfoMsg->K[2];
float depthCy = cameraInfoMsg->K[5];
image_geometry::PinholeCameraModel model;
model.fromCameraInfo(*cameraInfoMsg);
float fx = model.fx();
float fy = model.fy();
float cx = model.cx();
float cy = model.cy();
cv::Mat depth16;
if(ptrDepth->image.type() != CV_16UC1)
@@ -303,20 +348,20 @@ private:
}
else
{
depth16 = ptrDepth->image;
depth16 = ptrDepth->image.clone();
}
rtabmap::SensorData image(
rtabmap::SensorData data(
ptrImage->image.clone(),
depth16,
cv::Mat(),
depthFx,
depthFy,
depthCx,
depthCy,
fx,
fy,
cx,
cy,
odom,
localTransform);
recorder_.addData(image);
recorder_.addData(data);
}
void scanCallback(
@@ -348,7 +393,7 @@ private:
cv_bridge::CvImageConstPtr ptrImage = cv_bridge::toCvShare(imageMsg, "bgr8");
rtabmap::SensorData image(
rtabmap::SensorData data(
ptrImage->image.clone(),
cv::Mat(),
scan,
@@ -358,7 +403,7 @@ private:
0.0f,
odom,
Transform());
recorder_.addData(image);
recorder_.addData(data);
}
@@ -397,10 +442,12 @@ private:
cv_bridge::CvImageConstPtr ptrImage = cv_bridge::toCvShare(imageMsg, "bgr8");
cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depthMsg);
float depthFx = cameraInfoMsg->K[0];
float depthFy = cameraInfoMsg->K[4];
float depthCx = cameraInfoMsg->K[2];
float depthCy = cameraInfoMsg->K[5];
image_geometry::PinholeCameraModel model;
model.fromCameraInfo(*cameraInfoMsg);
float fx = model.fx();
float fy = model.fy();
float cx = model.cx();
float cy = model.cy();
cv::Mat depth16;
if(ptrDepth->image.type() != CV_16UC1)
@@ -425,20 +472,109 @@ private:
}
else
{
depth16 = ptrDepth->image;
depth16 = ptrDepth->image.clone();
}
rtabmap::SensorData image(
rtabmap::SensorData data(
ptrImage->image.clone(),
depth16,
scan,
depthFx,
depthFy,
depthCx,
depthCy,
fx,
fy,
cx,
cy,
odom,
localTransform);
recorder_.addData(image);
recorder_.addData(data);
}
void stereoOdomCallback(
const sensor_msgs::ImageConstPtr& leftImageMsg,
const sensor_msgs::ImageConstPtr& rightImageMsg,
const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg,
const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg,
const nav_msgs::OdometryConstPtr & odomMsg)
{
// TF ready?
Transform localTransform;
try
{
tf::StampedTransform tmp;
tfListener_.lookupTransform(frameId_, leftImageMsg->header.frame_id, leftImageMsg->header.stamp, tmp);
localTransform = transformFromTF(tmp);
}
catch(tf::TransformException & ex)
{
ROS_WARN("%s",ex.what());
return;
}
Transform odom = transformFromPoseMsg(odomMsg->pose.pose);
cv_bridge::CvImageConstPtr ptrLeftImage = cv_bridge::toCvShare(leftImageMsg, "bgr8");
cv_bridge::CvImageConstPtr ptrRightImage = cv_bridge::toCvShare(rightImageMsg, "mono8");
image_geometry::StereoCameraModel model;
model.fromCameraInfo(*leftCameraInfoMsg, *rightCameraInfoMsg);
float fx = model.right().fx();
float baseline = model.baseline();
float cx = model.right().cx();
float cy = model.right().cy();
rtabmap::SensorData data(
ptrLeftImage->image.clone(),
ptrRightImage->image.clone(),
cv::Mat(),
fx,
baseline,
cx,
cy,
odom,
localTransform);
recorder_.addData(data);
}
void stereoCallback(
const sensor_msgs::ImageConstPtr& leftImageMsg,
const sensor_msgs::ImageConstPtr& rightImageMsg,
const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg,
const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg)
{
// TF ready?
Transform localTransform;
try
{
tf::StampedTransform tmp;
tfListener_.lookupTransform(frameId_, leftImageMsg->header.frame_id, leftImageMsg->header.stamp, tmp);
localTransform = transformFromTF(tmp);
}
catch(tf::TransformException & ex)
{
ROS_WARN("%s",ex.what());
return;
}
cv_bridge::CvImageConstPtr ptrLeftImage = cv_bridge::toCvShare(leftImageMsg, "bgr8");
cv_bridge::CvImageConstPtr ptrRightImage = cv_bridge::toCvShare(rightImageMsg, "mono8");
image_geometry::StereoCameraModel model;
model.fromCameraInfo(*leftCameraInfoMsg, *rightCameraInfoMsg);
float fx = model.right().fx();
float baseline = model.baseline();
float cx = model.right().cx();
float cy = model.right().cy();
rtabmap::SensorData data(
ptrLeftImage->image.clone(),
ptrRightImage->image.clone(),
cv::Mat(),
fx,
baseline,
cx,
cy,
Transform(),
localTransform);
recorder_.addData(data);
}
private:
@@ -449,7 +585,9 @@ private:
image_transport::Subscriber defaultSub_;
image_transport::SubscriberFilter imageSub_;
image_transport::SubscriberFilter imageDepthSub_;
image_transport::SubscriberFilter imageRightSub_;
message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoSub_;
message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoRightSub_;
message_filters::Subscriber<nav_msgs::Odometry> odomSub_;
message_filters::Subscriber<sensor_msgs::LaserScan> scanSub_;
@@ -481,6 +619,23 @@ private:
sensor_msgs::CameraInfo> MyDepthImageSyncPolicy;
message_filters::Synchronizer<MyDepthImageSyncPolicy> * depthImageSync_;
//stereo with odometry
typedef message_filters::sync_policies::ApproximateTime<
sensor_msgs::Image,
sensor_msgs::Image,
sensor_msgs::CameraInfo,
sensor_msgs::CameraInfo,
nav_msgs::Odometry> MyStereoOdomSyncPolicy;
message_filters::Synchronizer<MyStereoOdomSyncPolicy> * stereoOdomSync_;
//stereo without odometry
typedef message_filters::sync_policies::ExactTime<
sensor_msgs::Image,
sensor_msgs::Image,
sensor_msgs::CameraInfo,
sensor_msgs::CameraInfo> MyStereoSyncPolicy;
message_filters::Synchronizer<MyStereoSyncPolicy> * stereoSync_;
tf::TransformListener tfListener_;
};
src/GuiWrapper.cpp
+22 -16
View File
@@ -38,6 +38,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <opencv2/highgui/highgui.hpp>
#include <image_geometry/pinhole_camera_model.h>
#include <rtabmap/gui/MainWindow.h>
#include <rtabmap/core/RtabmapEvent.h>
#include <rtabmap/core/Parameters.h>
@@ -581,18 +583,20 @@ void GuiWrapper::depthCallback(
cv_bridge::CvImageConstPtr ptrImage = cv_bridge::toCvShare(imageMsg, "bgr8");
cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depthMsg);
float depthFx = cameraInfoMsg->K[0];
float depthFy = cameraInfoMsg->K[4];
float depthCx = cameraInfoMsg->K[2];
float depthCy = cameraInfoMsg->K[5];
image_geometry::PinholeCameraModel model;
model.fromCameraInfo(*cameraInfoMsg);
float fx = model.fx();
float fy = model.fy();
float cx = model.cx();
float cy = model.cy();
rtabmap::SensorData image(
ptrImage->image.clone(),
ptrDepth->image.clone(),
depthFx,
depthFy,
depthCx,
depthCy,
fx,
fy,
cx,
cy,
odom,
localTransform);
this->post(new OdometryEvent(image));
@@ -675,19 +679,21 @@ void GuiWrapper::depthScanCallback(
cv_bridge::CvImageConstPtr ptrImage = cv_bridge::toCvShare(imageMsg, "bgr8");
cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depthMsg);
float depthFx = cameraInfoMsg->K[0];
float depthFy = cameraInfoMsg->K[4];
float depthCx = cameraInfoMsg->K[2];
float depthCy = cameraInfoMsg->K[5];
image_geometry::PinholeCameraModel model;
model.fromCameraInfo(*cameraInfoMsg);
float fx = model.fx();
float fy = model.fy();
float cx = model.cx();
float cy = model.cy();
rtabmap::SensorData image(
ptrImage->image.clone(),
ptrDepth->image.clone(),
scan,
depthFx,
depthFy,
depthCx,
depthCy,
fx,
fy,
cx,
cy,
odom,
localTransform);
this->post(new OdometryEvent(image));
src/OdometryROS.cpp
+412
View File
@@ -0,0 +1,412 @@
/*
Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include "OdometryROS.h"
#include <sensor_msgs/Image.h>
#include <sensor_msgs/image_encodings.h>
#include <sensor_msgs/PointCloud2.h>
#include <nav_msgs/Odometry.h>
#include <pcl_conversions/pcl_conversions.h>
#include <cv_bridge/cv_bridge.h>
#include <rtabmap/core/Odometry.h>
#include <rtabmap/core/util3d.h>
#include <rtabmap/core/Memory.h>
#include <rtabmap/core/Signature.h>
#include "rtabmap/MsgConversion.h"
#include "rtabmap/utilite/UConversion.h"
#include "rtabmap/utilite/ULogger.h"
#include "rtabmap/utilite/UStl.h"
namespace rtabmap {
OdometryROS::OdometryROS(int argc, char * argv[]) :
odometry_(0),
frameId_("base_link"),
odomFrameId_("odom"),
publishTf_(true),
paused_(false)
{
this->processArguments(argc, argv);
ros::NodeHandle nh;
odomPub_ = nh.advertise<nav_msgs::Odometry>("odom", 1);
odomLocalMap_ = nh.advertise<sensor_msgs::PointCloud2>("odom_local_map", 1);
odomLastFrame_ = nh.advertise<sensor_msgs::PointCloud2>("odom_last_frame", 1);
//odomMatches_ = nh.advertise<sensor_msgs::Image>("odom_matches", 1);
ros::NodeHandle pnh("~");
pnh.param("frame_id", frameId_, frameId_);
pnh.param("odom_frame_id", odomFrameId_, odomFrameId_);
pnh.param("publish_tf", publishTf_, publishTf_);
//parameters
rtabmap::ParametersMap parameters = rtabmap::Parameters::getDefaultParameters();
for(rtabmap::ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
{
std::string group = uSplit(iter->first, '/').front();
if(uStrContains(group, "Odom") ||
group.compare("SURF") == 0 ||
group.compare("SIFT") == 0 ||
group.compare("ORB") == 0 ||
group.compare("FAST") == 0 ||
group.compare("FREAK") == 0 ||
group.compare("BRIEF") == 0 ||
group.compare("GFTT") == 0 ||
group.compare("BRISK") == 0)
{
parameters_.insert(*iter);
}
}
for(rtabmap::ParametersMap::iterator iter=parameters_.begin(); iter!=parameters_.end(); ++iter)
{
std::string vStr;
bool vBool;
int vInt;
double vDouble;
if(pnh.getParam(iter->first, vStr))
{
ROS_INFO("Setting odometry parameter \"%s\"=\"%s\"", iter->first.c_str(), vStr.c_str());
iter->second = vStr;
}
else if(pnh.getParam(iter->first, vBool))
{
ROS_INFO("Setting odometry parameter \"%s\"=\"%s\"", iter->first.c_str(), uBool2Str(vBool).c_str());
iter->second = uBool2Str(vBool);
}
else if(pnh.getParam(iter->first, vInt))
{
ROS_INFO("Setting odometry parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vInt).c_str());
iter->second = uNumber2Str(vInt);
if(iter->first.compare(Parameters::kOdomMinInliers()) == 0 && vInt < 8)
{
ROS_WARN("Parameter min_inliers must be >= 8, setting to 8...");
iter->second = uNumber2Str(8);
}
}
else if(pnh.getParam(iter->first, vDouble))
{
ROS_INFO("Setting odometry parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vDouble).c_str());
iter->second = uNumber2Str(vDouble);
}
}
// Backward compatibility
std::list<std::string> oldParameterNames;
oldParameterNames.push_back("Odom/Type");
oldParameterNames.push_back("Odom/MaxWords");
oldParameterNames.push_back("Odom/WordsRatio");
oldParameterNames.push_back("Odom/LocalHistory");
oldParameterNames.push_back("Odom/NearestNeighbor");
oldParameterNames.push_back("Odom/NNDR");
for(std::list<std::string>::iterator iter=oldParameterNames.begin(); iter!=oldParameterNames.end(); ++iter)
{
std::string vStr;
if(pnh.getParam(*iter, vStr))
{
if(iter->compare("Odom/Type") == 0)
{
ROS_WARN("Parameter name changed: Odom/Type -> %s. Please update your launch file accordingly.",
Parameters::kOdomFeatureType().c_str());
parameters_.at(Parameters::kOdomFeatureType())= vStr;
}
else if(iter->compare("Odom/MaxWords") == 0)
{
ROS_WARN("Parameter name changed: Odom/MaxWords -> %s. Please update your launch file accordingly.",
Parameters::kOdomMaxFeatures().c_str());
parameters_.at(Parameters::kOdomMaxFeatures())= vStr;
}
else if(iter->compare("Odom/LocalHistory") == 0)
{
ROS_WARN("Parameter name changed: Odom/LocalHistory -> %s. Please update your launch file accordingly.",
Parameters::kOdomBowLocalHistorySize().c_str());
parameters_.at(Parameters::kOdomBowLocalHistorySize())= vStr;
}
else if(iter->compare("Odom/NearestNeighbor") == 0)
{
ROS_WARN("Parameter name changed: Odom/NearestNeighbor -> %s. Please update your launch file accordingly.",
Parameters::kOdomBowNNType().c_str());
parameters_.at(Parameters::kOdomBowNNType())= vStr;
}
else if(iter->compare("Odom/WordsRatio") == 0)
{
ROS_WARN("Parameter name changed: Odom/WordsRatio -> %s. Please update your launch file accordingly.",
Parameters::kOdomFeaturesRatio().c_str());
parameters_.at(Parameters::kOdomFeaturesRatio())= vStr;
}
else if(iter->compare("Odom/NNDR") == 0)
{
ROS_WARN("Parameter name changed: Odom/NNDR -> %s. Please update your launch file accordingly.",
Parameters::kOdomBowNNDR().c_str());
parameters_.at(Parameters::kOdomBowNNDR())= vStr;
}
}
}
int odomStrategy = 0; // BOW
Parameters::parse(parameters_, Parameters::kOdomStrategy(), odomStrategy);
if(odomStrategy == 1)
{
ROS_INFO("Using OdometryOpticalFlow");
odometry_ = new rtabmap::OdometryOpticalFlow(parameters_);
}
else
{
ROS_INFO("Using OdometryBOW");
odometry_ = new rtabmap::OdometryBOW(parameters_);
}
resetSrv_ = nh.advertiseService("reset_odom", &OdometryROS::reset, this);
pauseSrv_ = nh.advertiseService("pause_odom", &OdometryROS::pause, this);
resumeSrv_ = nh.advertiseService("resume_odom", &OdometryROS::resume, this);
}
OdometryROS::~OdometryROS()
{
ros::NodeHandle pnh("~");
for(ParametersMap::iterator iter=parameters_.begin(); iter!=parameters_.end(); ++iter)
{
pnh.deleteParam(iter->first);
}
delete odometry_;
}
void OdometryROS::processArguments(int argc, char * argv[])
{
for(int i=1;i<argc;++i)
{
if(strcmp(argv[i], "--params") == 0)
{
rtabmap::ParametersMap parameters = rtabmap::Parameters::getDefaultParameters();
rtabmap::ParametersMap parametersOdom;
if(strcmp(argv[i], "--params") == 0)
{
// show specific parameters
for(rtabmap::ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
{
if(iter->first.find("Odom") == 0 ||
uSplit(iter->first, '/').front().compare("SURF") == 0 ||
uSplit(iter->first, '/').front().compare("SIFT") == 0 ||
uSplit(iter->first, '/').front().compare("ORB") == 0 ||
uSplit(iter->first, '/').front().compare("FAST") == 0 ||
uSplit(iter->first, '/').front().compare("FREAK") == 0 ||
uSplit(iter->first, '/').front().compare("BRIEF") == 0 ||
uSplit(iter->first, '/').front().compare("GFTT") == 0 ||
uSplit(iter->first, '/').front().compare("BRISK") == 0)
{
parametersOdom.insert(*iter);
}
}
}
for(rtabmap::ParametersMap::iterator iter=parametersOdom.begin(); iter!=parametersOdom.end(); ++iter)
{
std::string str = "Param: " + iter->first + " = \"" + iter->second + "\"";
std::cout <<
str <<
std::setw(60 - str.size()) <<
" [" <<
rtabmap::Parameters::getDescription(iter->first).c_str() <<
"]" <<
std::endl;
}
ROS_WARN("Node will now exit after showing default odometry parameters because "
"argument \"--params\" is detected!");
exit(0);
}
}
}
Transform OdometryROS::processData(SensorData & data, const std_msgs::Header & header, int & quality)
{
rtabmap::Transform pose = odometry_->process(data, &quality);
if(!pose.isNull())
{
//*********************
// Update odometry
//*********************
tf::Transform poseTF;
rtabmap::transformToTF(pose, poseTF);
if(publishTf_)
{
tfBroadcaster_.sendTransform( tf::StampedTransform (poseTF, header.stamp, odomFrameId_, frameId_));
}
if(odomPub_.getNumSubscribers())
{
//next, we'll publish the odometry message over ROS
nav_msgs::Odometry odom;
odom.header.stamp = header.stamp; // use corresponding time stamp to image
odom.header.frame_id = odomFrameId_;
odom.child_frame_id = frameId_;
//set the position
odom.pose.pose.position.x = poseTF.getOrigin().x();
odom.pose.pose.position.y = poseTF.getOrigin().y();
odom.pose.pose.position.z = poseTF.getOrigin().z();
tf::quaternionTFToMsg(poseTF.getRotation().normalized(), odom.pose.pose.orientation);
//publish the message
odomPub_.publish(odom);
}
if(odomLocalMap_.getNumSubscribers() && dynamic_cast<OdometryBOW*>(odometry_))
{
const std::multimap<int, pcl::PointXYZ> & map = ((OdometryBOW*)odometry_)->getLocalMap();
pcl::PointCloud<pcl::PointXYZ> cloud;
for(std::multimap<int, pcl::PointXYZ>::const_iterator iter=map.begin(); iter!=map.end(); ++iter)
{
cloud.push_back(iter->second);
}
sensor_msgs::PointCloud2 cloudMsg;
pcl::toROSMsg(cloud, cloudMsg);
cloudMsg.header.stamp = header.stamp; // use corresponding time stamp to image
cloudMsg.header.frame_id = odomFrameId_;
odomLocalMap_.publish(cloudMsg);
}
if(odomLastFrame_.getNumSubscribers())
{
if(dynamic_cast<OdometryBOW*>(odometry_))
{
const rtabmap::Signature * s = ((OdometryBOW*)odometry_)->getMemory()->getLastWorkingSignature();
if(s)
{
const std::multimap<int, pcl::PointXYZ> & words3 = s->getWords3();
pcl::PointCloud<pcl::PointXYZ> cloud;
rtabmap::Transform t = data.localTransform();
for(std::multimap<int, pcl::PointXYZ>::const_iterator iter=words3.begin(); iter!=words3.end(); ++iter)
{
// transform to odom frame
pcl::PointXYZ pt = util3d::transformPoint(iter->second, pose);
cloud.push_back(pt);
}
sensor_msgs::PointCloud2 cloudMsg;
pcl::toROSMsg(cloud, cloudMsg);
cloudMsg.header.stamp = header.stamp; // use corresponding time stamp to image
cloudMsg.header.frame_id = odomFrameId_;
odomLastFrame_.publish(cloudMsg);
}
}
else
{
//Optical flow
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud = ((OdometryOpticalFlow*)odometry_)->getLastCorners3D();
if(cloud->size())
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudTransformed;
cloudTransformed = util3d::transformPointCloud(cloud, pose);
sensor_msgs::PointCloud2 cloudMsg;
pcl::toROSMsg(*cloudTransformed, cloudMsg);
cloudMsg.header.stamp = header.stamp; // use corresponding time stamp to image
cloudMsg.header.frame_id = odomFrameId_;
odomLastFrame_.publish(cloudMsg);
}
}
}
}
else
{
//ROS_WARN("Odometry lost!");
//send null pose to notify that odometry is lost
nav_msgs::Odometry odom;
odom.header.stamp = header.stamp; // use corresponding time stamp to image
odom.header.frame_id = odomFrameId_;
odom.child_frame_id = frameId_;
//publish the message
odomPub_.publish(odom);
}
if(odomMatches_.getNumSubscribers())
{
if(dynamic_cast<OdometryOpticalFlow*>(odometry_))
{
cv_bridge::CvImage img;
img.encoding = sensor_msgs::image_encodings::BGR8;
img.image = ((OdometryOpticalFlow*)odometry_)->imgMatches_;
sensor_msgs::ImagePtr rosMsg = img.toImageMsg();
rosMsg->header= header;
odomMatches_.publish(rosMsg);
}
}
return pose;
}
bool OdometryROS::isOdometryBOW() const
{
return dynamic_cast<OdometryBOW*>(odometry_) != 0;
}
bool OdometryROS::reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
{
ROS_INFO("visual_odometry: reset odom!");
odometry_->reset();
return true;
}
bool OdometryROS::pause(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
{
if(paused_)
{
ROS_WARN("visual_odometry: Already paused!");
}
else
{
paused_ = true;
ROS_INFO("visual_odometry: paused!");
}
return true;
}
bool OdometryROS::resume(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
{
if(!paused_)
{
ROS_WARN("visual_odometry: Already running!");
}
else
{
paused_ = false;
ROS_INFO("visual_odometry: resumed!");
}
return true;
}
}
src/OdometryROS.h
+90
View File
@@ -0,0 +1,90 @@
/*
Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef ODOMETRYROS_H_
#define ODOMETRYROS_H_
#include <ros/ros.h>
#include <tf/tf.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_listener.h>
#include <std_srvs/Empty.h>
#include <std_msgs/Header.h>
#include <rtabmap/core/SensorData.h>
#include <rtabmap/core/Parameters.h>
namespace rtabmap {
class Odometry;
class OdometryROS
{
public:
OdometryROS(int argc, char * argv[]);
~OdometryROS();
void processArguments(int argc, char * argv[]);
Transform processData(SensorData & data, const std_msgs::Header & header, int & quality);
bool reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
bool pause(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
bool resume(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
const std::string & frameId() const {return frameId_;}
const std::string & odomFrameId() const {return odomFrameId_;}
const ParametersMap & parameters() const {return parameters_;}
const tf::TransformListener & tfListener() const {return tfListener_;}
bool isPaused() const {return paused_;}
bool isOdometryBOW() const;
private:
rtabmap::Odometry * odometry_;
// parameters
std::string frameId_;
std::string odomFrameId_;
bool publishTf_;
ParametersMap parameters_;
ros::Publisher odomPub_;
ros::Publisher odomLocalMap_;
ros::Publisher odomLastFrame_;
ros::Publisher odomMatches_;
ros::ServiceServer resetSrv_;
ros::ServiceServer pauseSrv_;
ros::ServiceServer resumeSrv_;
tf::TransformBroadcaster tfBroadcaster_;
tf::TransformListener tfListener_;
bool paused_;
};
}
#endif
src/RGBDOdometryNode.cpp
+169
View File
@@ -0,0 +1,169 @@
/*
Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include "OdometryROS.h"
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <image_transport/image_transport.h>
#include <image_transport/subscriber_filter.h>
#include <image_geometry/stereo_camera_model.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/image_encodings.h>
#include <cv_bridge/cv_bridge.h>
#include "rtabmap/MsgConversion.h"
#include <rtabmap/utilite/ULogger.h>
using namespace rtabmap;
class RGBDOdometry : public OdometryROS
{
public:
RGBDOdometry(int argc, char * argv[]) :
OdometryROS(argc, argv),
sync_(0)
{
ros::NodeHandle nh;
ros::NodeHandle pnh("~");
int queueSize = 5;
pnh.param("queue_size", queueSize, queueSize);
ros::NodeHandle rgb_nh(nh, "rgb");
ros::NodeHandle depth_nh(nh, "depth");
ros::NodeHandle rgb_pnh(pnh, "rgb");
ros::NodeHandle depth_pnh(pnh, "depth");
image_transport::ImageTransport rgb_it(rgb_nh);
image_transport::ImageTransport depth_it(depth_nh);
image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh);
image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh);
image_mono_sub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb);
image_depth_sub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth);
info_sub_.subscribe(rgb_nh, "camera_info", 1);
sync_ = new message_filters::Synchronizer<MySyncPolicy>(MySyncPolicy(queueSize), image_mono_sub_, image_depth_sub_, info_sub_);
sync_->registerCallback(boost::bind(&RGBDOdometry::callback, this, _1, _2, _3));
}
~RGBDOdometry()
{
delete sync_;
}
void callback(const sensor_msgs::ImageConstPtr& image,
const sensor_msgs::ImageConstPtr& depth,
const sensor_msgs::CameraInfoConstPtr& cameraInfo)
{
if(!this->isPaused())
{
if(!(image->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
image->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
image->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
image->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) ||
!(depth->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1)==0 ||
depth->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)==0))
{
ROS_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 (mono8 recommended) and image_depth=16UC1");
return;
}
else if(depth->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)==0)
{
static bool warned = false;
if(!warned)
{
ROS_WARN("Input depth type is 32FC1, please use type 16UC1 for depth. The depth images "
"will be processed anyway but with a conversion. This warning is only be printed once...");
warned = true;
}
}
tf::StampedTransform localTransform;
try
{
this->tfListener().lookupTransform(this->frameId(), image->header.frame_id, image->header.stamp, localTransform);
}
catch(tf::TransformException & ex)
{
ROS_WARN("%s",ex.what());
return;
}
ros::WallTime time = ros::WallTime::now();
int quality = -1;
if(image->data.size() && depth->data.size() && cameraInfo->K[4] != 0)
{
image_geometry::PinholeCameraModel model;
model.fromCameraInfo(*cameraInfo);
float fx = model.fx();
float fy = model.fy();
float cx = model.cx();
float cy = model.cy();
cv_bridge::CvImageConstPtr ptrImage = cv_bridge::toCvShare(image, "mono8");
cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depth);
rtabmap::SensorData data(ptrImage->image,
ptrDepth->image,
fx,
fy,
cx,
cy,
rtabmap::Transform(),
rtabmap::transformFromTF(localTransform));
quality=0;
this->processData(data, image->header, quality);
}
ROS_INFO("Odom: quality=%d, update time=%fs", quality, (ros::WallTime::now()-time).toSec());
}
}
private:
image_transport::SubscriberFilter image_mono_sub_;
image_transport::SubscriberFilter image_depth_sub_;
message_filters::Subscriber<sensor_msgs::CameraInfo> info_sub_;
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MySyncPolicy;
message_filters::Synchronizer<MySyncPolicy> * sync_;
};
int main(int argc, char *argv[])
{
ULogger::setType(ULogger::kTypeConsole);
ULogger::setLevel(ULogger::kWarning);
ros::init(argc, argv, "rgbd_odometry");
RGBDOdometry odom(argc, argv);
ros::spin();
return 0;
}
src/StereoOdometryNode.cpp
+232 -432
View File
@@ -25,13 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include <ros/ros.h>
#include <tf/tf.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_listener.h>
#include <std_srvs/Empty.h>
#include "OdometryROS.h"
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
@@ -42,153 +36,83 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <sensor_msgs/Image.h>
#include <sensor_msgs/image_encodings.h>
#include <sensor_msgs/PointCloud2.h>
#include <nav_msgs/Odometry.h>
#include <image_geometry/stereo_camera_model.h>
#include <pcl_conversions/pcl_conversions.h>
#include <cv_bridge/cv_bridge.h>
#include <rtabmap/core/Odometry.h>
#include <rtabmap/core/util3d.h>
#include <rtabmap/core/Memory.h>
#include <rtabmap/core/Signature.h>
#include <opencv2/video/tracking.hpp>
#include "rtabmap/MsgConversion.h"
#include "rtabmap/utilite/UConversion.h"
#include "rtabmap/utilite/ULogger.h"
#include "rtabmap/utilite/UStl.h"
#include "rtabmap/utilite/UTimer.h"
#include <rtabmap/core/Features2d.h>
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UStl.h>
#include <rtabmap/utilite/UTimer.h>
#include <rtabmap/utilite/UMath.h>
using namespace rtabmap;
class StereoOdometry
class StereoOdometry : public OdometryROS
{
public:
StereoOdometry() :
odometry_(0),
StereoOdometry(int argc, char * argv[]) :
OdometryROS(argc, argv),
feature2D_(0),
frameId_("base_link"),
odomFrameId_("odom"),
publishTf_(true),
minDisparity_(0.0),
maxDisparity_(128.0),
k_(10),
winSize_(5),
sync_(0),
paused_(false)
depthPatchSize_(1),
generateDepth_(false),
stereoFlowWinSize_(21),
stereoFlowIterations_(30),
stereoFlowEpsilon_(0.01),
stereoFlowMaxLevel_(3),
stereoSubPixWinSize_(5),
stereoSubPixIterations_(20),
stereoSubPixEps_(0.03),
approxSync_(0),
exactSync_(0)
{
ros::NodeHandle nh;
odomPub_ = nh.advertise<nav_msgs::Odometry>("odom", 1);
odomLocalMapPub_ = nh.advertise<sensor_msgs::PointCloud2>("odom_local_map", 1);
odomLastFrame_ = nh.advertise<sensor_msgs::PointCloud2>("odom_last_frame", 1);
odomDepth_ = nh.advertise<sensor_msgs::Image>("odom_depth", 1);
ros::NodeHandle pnh("~");
bool approxSync = false;
int queueSize = 5;
pnh.param("frame_id", frameId_, frameId_);
pnh.param("odom_frame_id", odomFrameId_, odomFrameId_);
pnh.param("publish_tf", publishTf_, publishTf_);
pnh.param("approx_sync", approxSync, approxSync);
pnh.param("queue_size", queueSize, queueSize);
pnh.param("min_disparity", minDisparity_, minDisparity_);
pnh.param("max_disparity", maxDisparity_, maxDisparity_);
pnh.param("k", k_, k_);
pnh.param("window_size", winSize_, winSize_);
ROS_INFO("Approximate time sync = %s", approxSync?"true":"false");
//parameters
rtabmap::ParametersMap parameters = rtabmap::Parameters::getDefaultParameters();
rtabmap::ParametersMap parametersOdom;
for(rtabmap::ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
{
std::string group = uSplit(iter->first, '/').front();
if(group.compare("Odom") == 0 ||
group.compare("SURF") == 0 ||
group.compare("SIFT") == 0 ||
group.compare("ORB") == 0 ||
group.compare("FAST") == 0 ||
group.compare("FREAK") == 0 ||
group.compare("BRIEF") == 0 ||
group.compare("GFTT") == 0 ||
group.compare("BRISK") == 0)
{
parametersOdom.insert(*iter);
}
}
pnh.param("generate_depth", generateDepth_, generateDepth_);
pnh.param("depth_patch_size", depthPatchSize_, depthPatchSize_);
ROS_INFO("Generate depth = %s", generateDepth_?"true":"false");
for(rtabmap::ParametersMap::iterator iter=parametersOdom.begin(); iter!=parametersOdom.end(); ++iter)
{
std::string vStr;
bool vBool;
int vInt;
double vDouble;
if(pnh.getParam(iter->first, vStr))
{
ROS_INFO("Setting odometry parameter \"%s\"=\"%s\"", iter->first.c_str(), vStr.c_str());
iter->second = vStr;
}
else if(pnh.getParam(iter->first, vBool))
{
ROS_INFO("Setting odometry parameter \"%s\"=\"%s\"", iter->first.c_str(), uBool2Str(vBool).c_str());
iter->second = uBool2Str(vBool);
}
else if(pnh.getParam(iter->first, vInt))
{
ROS_INFO("Setting odometry parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vInt).c_str());
iter->second = uNumber2Str(vInt);
pnh.param("flow_win_size", stereoFlowWinSize_, stereoFlowWinSize_);
pnh.param("flow_iterations", stereoFlowIterations_, stereoFlowIterations_);
pnh.param("flow_epsilon", stereoFlowEpsilon_, stereoFlowEpsilon_);
pnh.param("flow_max_level", stereoFlowMaxLevel_, stereoFlowMaxLevel_);
if(iter->first.compare(Parameters::kOdomMinInliers()) == 0 && vInt < 8)
{
ROS_WARN("Parameter min_inliers must be >= 8, setting to 8...");
iter->second = uNumber2Str(8);
}
}
else if(pnh.getParam(iter->first, vDouble))
{
ROS_INFO("Setting odometry parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vDouble).c_str());
iter->second = uNumber2Str(vDouble);
}
}
pnh.param("subpix_win_size", stereoSubPixWinSize_, stereoSubPixWinSize_);
pnh.param("subpix_iterations", stereoSubPixIterations_, stereoSubPixIterations_);
pnh.param("subpix_eps", stereoSubPixEps_, stereoSubPixEps_);
odometry_ = new rtabmap::OdometryBOW(parametersOdom);
UASSERT_MSG(!this->isOdometryBOW() || (this->isOdometryBOW() && generateDepth_),
"Odom/Strategy=0 (OdometryBOW) requires depth generation (generate_depth=true).");
UASSERT(depthPatchSize_ >= 0);
//Keypoint detector
ParametersMap::const_iterator iter;
Feature2D::Type detectorStrategy = Feature2D::kFeatureUndef;
if((iter=parametersOdom.find(Parameters::kOdomType())) != parametersOdom.end())
if((iter=this->parameters().find(Parameters::kOdomFeatureType())) != this->parameters().end())
{
detectorStrategy = (Feature2D::Type)std::atoi((*iter).second.c_str());
}
switch(detectorStrategy)
{
case Feature2D::kFeatureSift:
feature2D_ = new SIFT(parametersOdom);
break;
case Feature2D::kFeatureFastBrief:
feature2D_ = new FAST_BRIEF(parametersOdom);
break;
case Feature2D::kFeatureFastFreak:
feature2D_ = new FAST_FREAK(parametersOdom);
break;
case Feature2D::kFeatureOrb:
feature2D_ = new ORB(parametersOdom);
break;
case Feature2D::kFeatureGfttFreak:
feature2D_ = new GFTT_FREAK(parametersOdom);
break;
case Feature2D::kFeatureGfttBrief:
feature2D_ = new GFTT_BRIEF(parametersOdom);
break;
case Feature2D::kFeatureBrisk:
feature2D_ = new BRISK(parametersOdom);
break;
case Feature2D::kFeatureSurf:
default:
feature2D_ = new SURF(parametersOdom);
break;
}
feature2D_ = Feature2D::create(detectorStrategy, this->parameters());
roiRatios_ = Parameters::defaultOdomRoiRatios();
Parameters::parse(this->parameters(), Parameters::kOdomRoiRatios(), roiRatios_);
ros::NodeHandle left_nh(nh, "left");
ros::NodeHandle right_nh(nh, "right");
@@ -204,25 +128,28 @@ public:
cameraInfoLeft_.subscribe(left_nh, "camera_info", 1);
cameraInfoRight_.subscribe(right_nh, "camera_info", 1);
sync_ = new message_filters::Synchronizer<MySyncPolicy>(MySyncPolicy(queueSize), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
sync_->registerCallback(boost::bind(&StereoOdometry::callback, this, _1, _2, _3, _4));
resetSrv_ = nh.advertiseService("reset_odom", &StereoOdometry::reset, this);
pauseSrv_ = nh.advertiseService("pause_odom", &StereoOdometry::pause, this);
resumeSrv_ = nh.advertiseService("resume_odom", &StereoOdometry::resume, this);
if(approxSync)
{
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
approxSync_->registerCallback(boost::bind(&StereoOdometry::callback, this, _1, _2, _3, _4));
}
else
{
exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
exactSync_->registerCallback(boost::bind(&StereoOdometry::callback, this, _1, _2, _3, _4));
}
}
~StereoOdometry()
{
ros::NodeHandle pnh("~");
ParametersMap parameters = Parameters::getDefaultParameters();
for(ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
if(approxSync_)
{
pnh.deleteParam(iter->first);
delete approxSync_;
}
if(exactSync_)
{
delete exactSync_;
}
delete sync_;
delete odometry_;
delete feature2D_;
}
@@ -232,7 +159,7 @@ public:
const sensor_msgs::CameraInfoConstPtr& cameraInfoLeft,
const sensor_msgs::CameraInfoConstPtr& cameraInfoRight)
{
if(!paused_)
if(!this->isPaused())
{
if(!(imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
@@ -250,7 +177,8 @@ public:
tf::StampedTransform localTransform;
try
{
tfListener_.lookupTransform(frameId_, imageRectLeft->header.frame_id, imageRectLeft->header.stamp, localTransform);
UDEBUG("");
this->tfListener().lookupTransform(this->frameId(), imageRectLeft->header.frame_id, imageRectLeft->header.stamp, localTransform);
}
catch(tf::TransformException & ex)
{
@@ -262,230 +190,183 @@ public:
int quality = -1;
if(imageRectLeft->data.size() && imageRectRight->data.size())
{
float depthFx = cameraInfoLeft->K[0];
float depthFy = cameraInfoLeft->K[4];
float depthCx = cameraInfoLeft->K[2];
float depthCy = cameraInfoLeft->K[5];
cv_bridge::CvImageConstPtr ptrImageLeft = cv_bridge::toCvShare(imageRectLeft, "mono8");
cv_bridge::CvImageConstPtr ptrImageRight = cv_bridge::toCvShare(imageRectRight, "mono8");
//generate depth
cv::Mat depth = cv::Mat::zeros(ptrImageLeft->image.rows, ptrImageLeft->image.cols, CV_32FC1);
std::vector<cv::KeyPoint> kptsLeft, kptsRight;
std::vector<cv::Point2f> cornersLeft, cornersRight;
cv::Mat descLeft, descRight;
kptsLeft = feature2D_->generateKeypoints(ptrImageLeft->image);
if(kptsLeft.size())
{
descLeft = feature2D_->generateDescriptors(ptrImageLeft->image, kptsLeft);
kptsRight = feature2D_->generateKeypoints(ptrImageRight->image);
if(kptsRight.size())
{
descRight = feature2D_->generateDescriptors(ptrImageRight->image, kptsRight);
if(kptsLeft.size() && kptsRight.size())
{
cornersLeft.resize(kptsLeft.size());
cornersRight.resize(kptsRight.size());
for(unsigned int i=0; i<kptsLeft.size() || i<kptsRight.size(); ++i)
{
if(i<kptsLeft.size())
{
cornersLeft[i] = kptsLeft[i].pt;
}
if(i<kptsRight.size())
{
cornersRight[i] = kptsRight[i].pt;
}
}
UTimer time;
cv::cornerSubPix( ptrImageLeft->image, cornersLeft, cv::Size( winSize_, winSize_ ), cv::Size( -1, -1 ),
cv::TermCriteria( CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, 0.03 ) );
cv::cornerSubPix( ptrImageRight->image, cornersRight, cv::Size( winSize_, winSize_ ), cv::Size( -1, -1 ),
cv::TermCriteria( CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, 0.03 ) );
UDEBUG("time subpix = %fs", time.ticks());
}
}
}
if(kptsLeft.size() >= odometry_->getMinInliers() && kptsRight.size() >= odometry_->getMinInliers())
{
std::vector<std::vector<cv::DMatch> > matches;
cv::BFMatcher matcher(descLeft.depth()==CV_8U?cv::NORM_HAMMING:cv::NORM_L2);
int k = std::min((int)kptsLeft.size(), k_);
k = std::min((int)kptsRight.size(), k);
matcher.knnMatch(descLeft, descRight, matches, k);
int added = 0;
if(matches.size())
{
image_geometry::StereoCameraModel model;
model.fromCameraInfo(*cameraInfoLeft, *cameraInfoRight);
int addedFirst = 0;
for(int i=0; i< matches.size(); ++i)
{
// add only those on same Y
for(unsigned int j=0; j<matches[i].size(); ++j)
{
float disparity = cornersLeft[matches[i].at(j).queryIdx].x - cornersRight[matches[i].at(j).trainIdx].x;
float fx = model.left().fx();
float fy = model.left().fy();
float cx = model.left().cx();
float cy = model.left().cy();
float baseline = model.baseline();
cv_bridge::CvImageConstPtr ptrImageLeft = cv_bridge::toCvShare(imageRectLeft, "mono8");
cv_bridge::CvImageConstPtr ptrImageRight = cv_bridge::toCvShare(imageRectRight, "mono8");
if((int)disparity >= minDisparity_ && (int)disparity <= maxDisparity_)
cv::Mat depthOrRightImage;
std::vector<cv::KeyPoint> kptsLeft, kptsRight;
cv::Mat descLeft, descRight;
UTimer stepTimer;
if(!generateDepth_)
{
float d = model.getZ(disparity);
if( d>0 &&
cornersLeft[matches[i].at(j).queryIdx].y >= cornersRight[matches[i].at(j).trainIdx].y - 3.0f &&
cornersLeft[matches[i].at(j).queryIdx].y <= cornersRight[matches[i].at(j).trainIdx].y + 3.0f)
{
kptsLeft[matches[i].at(j).queryIdx].pt = cornersLeft[matches[i].at(j).queryIdx];
depth.at<float>(int(kptsLeft[matches[i].at(j).queryIdx].pt.y+0.5f), int(kptsLeft[matches[i].at(j).queryIdx].pt.x+0.5f)) = d;
/*ROS_INFO("Add%d Left(%d, %d) Right(%d, %d) distance %d = %f disp=%f, depth=%f",
j,
int(kptsLeft[matches[i].at(j).queryIdx].pt.x+0.5f),
int(kptsLeft[matches[i].at(j).queryIdx].pt.y+0.5f),
int(kptsRight[matches[i].at(j).trainIdx].pt.x+0.5f),
int(kptsRight[matches[i].at(j).trainIdx].pt.y+0.5f),
i, matches[i].at(j).distance, disparity, d);*/
if(j == 0)
{
++addedFirst;
}
++added;
break;
// copy right image in depth
depthOrRightImage = ptrImageRight->image;
}
else
{
/*ROS_INFO("--- Left(%d, %d) Right(%d, %d) distance %d = %f disp=%f depth=%f",
int(kptsLeft[matches[i].queryIdx].pt.x+0.5f),
int(kptsLeft[matches[i].queryIdx].pt.y+0.5f),
int(kptsRight[matches[i].trainIdx].pt.x+0.5f),
int(kptsRight[matches[i].trainIdx].pt.y+0.5f),
i, matches[i].distance, disparity, d);*/
//generate depth
depthOrRightImage = cv::Mat::zeros(ptrImageLeft->image.rows, ptrImageLeft->image.cols, CV_32FC1);
std::vector<cv::Point2f> cornersLeft, cornersRight;
cv::Rect roi = Feature2D::computeRoi(ptrImageLeft->image, roiRatios_);
kptsLeft = feature2D_->generateKeypoints(ptrImageLeft->image, 0, roi);
UDEBUG("time generate left kpts=%fs", stepTimer.ticks());
if(!kptsLeft.size())
{
ROS_WARN("No left keypoints extracted!");
return;
}
int stereoFeaturesAdded = 0;
int stereoFeaturesMatched = 0;
int stereoFeaturesExtracted = 0;
cv::KeyPoint::convert(kptsLeft, cornersLeft);
if(stereoSubPixWinSize_ > 0 && stereoSubPixIterations_ > 0)
{
cv::cornerSubPix( ptrImageLeft->image, cornersLeft,
cv::Size( stereoSubPixWinSize_, stereoSubPixWinSize_ ),
cv::Size( -1, -1 ),
cv::TermCriteria( CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, stereoSubPixIterations_, stereoSubPixEps_ ) );
UDEBUG("time subpix left kpts=%fs", stepTimer.ticks());
}
std::vector<unsigned char> status;
std::vector<float> err;
UDEBUG("cv::calcOpticalFlowPyrLK() begin");
cv::calcOpticalFlowPyrLK(
ptrImageLeft->image,
ptrImageRight->image,
cornersLeft,
cornersRight,
status,
err,
cv::Size(stereoFlowWinSize_, stereoFlowWinSize_), stereoFlowMaxLevel_,
cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, stereoFlowIterations_, stereoFlowEpsilon_),
cv::OPTFLOW_LK_GET_MIN_EIGENVALS, 1e-4);
UDEBUG("cv::calcOpticalFlowPyrLK() end");
UDEBUG("time optical flow=%fs", stepTimer.ticks());
std::vector<cv::KeyPoint> kptsLeftFiltered(kptsLeft.size());
int oi = 0;
for(int i=0; i<status.size(); ++i)
{
if(status[i] &&
uIsInBounds(cornersLeft[i].x, 0.0f, float(depthOrRightImage.cols)-1.0f) &&
uIsInBounds(cornersLeft[i].y, 0.0f, float(depthOrRightImage.rows)-1.0f) &&
uIsInBounds(cornersRight[i].x, 0.0f, float(depthOrRightImage.cols)-1.0f) &&
uIsInBounds(cornersRight[i].y, 0.0f, float(depthOrRightImage.rows)-1.0f))
{
float disparity = cornersLeft[i].x - cornersRight[i].x;
if(disparity >= 0)
{
float d = model.getZ(disparity);
if(d>0)
{
bool depthAdded = false;
int u = int(cornersLeft[i].x+0.5f);
int v = int(cornersLeft[i].y+0.5f);
for(int j=-depthPatchSize_; j<=depthPatchSize_; ++j)
{
for(int k=-depthPatchSize_; k<=depthPatchSize_; ++k)
{
if(uIsInBounds(u+j, 0, depthOrRightImage.cols-1) &&
uIsInBounds(v+k, 0, depthOrRightImage.rows-1))
{
depthOrRightImage.at<float>(v+j, u+k) = d;
depthAdded = true;
}
}
}
if(depthAdded)
{
kptsLeftFiltered[oi] = kptsLeft[i];
kptsLeftFiltered[oi].pt = cornersLeft[i];
++oi;
}
}
}
++stereoFeaturesMatched;
}
}
stereoFeaturesAdded = oi;
stereoFeaturesExtracted = kptsLeft.size();
UDEBUG("stereoFeaturesExtracted=%d", stereoFeaturesExtracted);
UDEBUG("stereoFeaturesMatched=%d", stereoFeaturesMatched);
UDEBUG("stereoFeaturesAdded=%d", stereoFeaturesAdded);
kptsLeftFiltered.resize(oi);
kptsLeft = kptsLeftFiltered;
if(!kptsLeft.size())
{
ROS_WARN("No left keypoints extracted!");
return;
}
// For OdometryBOW, we must generate descriptors
int odomStrategy = Parameters::defaultOdomStrategy();
Parameters::parse(this->parameters(), Parameters::kOdomStrategy(), odomStrategy);
if(odomStrategy == 0)
{
descLeft = feature2D_->generateDescriptors(ptrImageLeft->image, kptsLeft);
UDEBUG("time generate left descriptors=%fs, remaining kpts=%d", stepTimer.ticks(), (int)kptsLeft.size());
if(!kptsLeft.size())
{
ROS_WARN("No left descriptors extracted!");
return;
}
}
}
UDEBUG("addedFirst = %d/%d", addedFirst, added);
//
UDEBUG("localTransform = %s", rtabmap::transformFromTF(localTransform).prettyPrint().c_str());
UDEBUG("kptsLeft=%d descLeft=%d", (int)kptsLeft.size(), descLeft.rows);
rtabmap::SensorData data(ptrImageLeft->image,
depth,
depthFx,
depthFy,
depthCx,
depthCy,
depthOrRightImage,
fx,
generateDepth_?fy:baseline,
cx,
cy,
rtabmap::Transform(),
rtabmap::transformFromTF(localTransform));
data.setFeatures(kptsLeft, descLeft);
quality=0;
rtabmap::Transform pose = odometry_->process(data, &quality);
if(!pose.isNull())
{
//*********************
// Update odometry
//*********************
tf::Transform poseTF;
rtabmap::transformToTF(pose, poseTF);
if(publishTf_)
{
tfBroadcaster_.sendTransform( tf::StampedTransform (poseTF, imageRectLeft->header.stamp, odomFrameId_, frameId_));
}
this->processData(data, imageRectLeft->header, quality);
UDEBUG("time odometry->process()=%fs", stepTimer.ticks());
if(odomPub_.getNumSubscribers())
{
//next, we'll publish the odometry message over ROS
nav_msgs::Odometry odom;
odom.header.stamp = imageRectLeft->header.stamp; // use corresponding time stamp to image
odom.header.frame_id = odomFrameId_;
odom.child_frame_id = frameId_;
//set the position
odom.pose.pose.position.x = poseTF.getOrigin().x();
odom.pose.pose.position.y = poseTF.getOrigin().y();
odom.pose.pose.position.z = poseTF.getOrigin().z();
tf::quaternionTFToMsg(poseTF.getRotation().normalized(), odom.pose.pose.orientation);
//publish the message
odomPub_.publish(odom);
}
if(odomLocalMapPub_.getNumSubscribers())
{
const std::multimap<int, pcl::PointXYZ> & map = odometry_->getLocalMap();
pcl::PointCloud<pcl::PointXYZ> cloud;
for(std::multimap<int, pcl::PointXYZ>::const_iterator iter=map.begin(); iter!=map.end(); ++iter)
{
cloud.push_back(iter->second);
}
sensor_msgs::PointCloud2 cloudMsg;
pcl::toROSMsg(cloud, cloudMsg);
cloudMsg.header.stamp = imageRectLeft->header.stamp; // use corresponding time stamp to image
cloudMsg.header.frame_id = odomFrameId_;
odomLocalMapPub_.publish(cloudMsg);
}
if(odomLastFrame_.getNumSubscribers())
{
const rtabmap::Signature * s = odometry_->getMemory()->getLastWorkingSignature();
if(s)
{
const std::multimap<int, pcl::PointXYZ> & words3 = s->getWords3();
pcl::PointCloud<pcl::PointXYZ> cloud;
rtabmap::Transform t = rtabmap::transformFromTF(localTransform);
for(std::multimap<int, pcl::PointXYZ>::const_iterator iter=words3.begin(); iter!=words3.end(); ++iter)
{
// transform to odom frame
pcl::PointXYZ pt = util3d::transformPoint(iter->second, pose);
cloud.push_back(pt);
}
sensor_msgs::PointCloud2 cloudMsg;
pcl::toROSMsg(cloud, cloudMsg);
cloudMsg.header.stamp = imageRectLeft->header.stamp; // use corresponding time stamp to image
cloudMsg.header.frame_id = odomFrameId_;
odomLastFrame_.publish(cloudMsg);
}
}
if(odomDepth_.getNumSubscribers())
if(generateDepth_ && odomDepth_.getNumSubscribers())
{
cv_bridge::CvImage img;
img.encoding = sensor_msgs::image_encodings::TYPE_32FC1;
img.image = depth;
img.image = depthOrRightImage;
sensor_msgs::ImagePtr rosMsg = img.toImageMsg();
rosMsg->header= imageRectLeft->header;
odomDepth_.publish(rosMsg);
}
}
else
{
//ROS_WARN("Odometry lost!");
//send null pose to notify that odometry is lost
nav_msgs::Odometry odom;
odom.header.stamp = imageRectLeft->header.stamp; // use corresponding time stamp to image
odom.header.frame_id = odomFrameId_;
odom.child_frame_id = frameId_;
//publish the message
odomPub_.publish(odom);
}
}
ROS_INFO("Odom: quality=%d, update time=%fs, stereo matches: added %d/%d",
quality, (ros::WallTime::now()-time).toSec(),
added, (int)matches.size());
}
else
{
ROS_WARN("Odom: no keypoints extracted!");
}
//ROS_INFO("Odom: quality=%d, update time=%fs, stereo matches: added/matched/extracted %d/%d/%d",
// quality, (ros::WallTime::now()-time).toSec(),
// stereoFeaturesAdded, stereoFeaturesMatched, stereoFeaturesExtracted);
ROS_INFO("Odom: quality=%d, update time=%fs",
quality, (ros::WallTime::now()-time).toSec());
}
else
{
@@ -494,72 +375,34 @@ public:
}
}
bool reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
{
ROS_INFO("visual_odometry: reset odom!");
odometry_->reset();
return true;
}
bool pause(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
{
if(paused_)
{
ROS_WARN("visual_odometry: Already paused!");
}
else
{
paused_ = true;
ROS_INFO("visual_odometry: paused!");
}
return true;
}
bool resume(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
{
if(!paused_)
{
ROS_WARN("visual_odometry: Already running!");
}
else
{
paused_ = false;
ROS_INFO("visual_odometry: resumed!");
}
return true;
}
private:
rtabmap::OdometryBOW * odometry_;
rtabmap::Feature2D * feature2D_;
Feature2D * feature2D_;
std::string roiRatios_;
// parameters
std::string frameId_;
std::string odomFrameId_;
bool publishTf_;
int minDisparity_;
int maxDisparity_;
int k_;
int winSize_;
// ROS parameters
int depthPatchSize_;
bool generateDepth_;
int stereoFlowWinSize_;
int stereoFlowIterations_;
double stereoFlowEpsilon_;
int stereoFlowMaxLevel_;
int stereoSubPixWinSize_;
int stereoSubPixIterations_;
double stereoSubPixEps_;
ros::Publisher odomPub_;
ros::Publisher odomLocalMapPub_;
ros::Publisher odomLastFrame_;
ros::Publisher odomDepth_;
ros::ServiceServer resetSrv_;
ros::ServiceServer pauseSrv_;
ros::ServiceServer resumeSrv_;
tf::TransformBroadcaster tfBroadcaster_;
tf::TransformListener tfListener_;
image_transport::SubscriberFilter imageRectLeft_;
image_transport::SubscriberFilter imageRectRight_;
message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoLeft_;
message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoRight_;
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo> MySyncPolicy;
message_filters::Synchronizer<MySyncPolicy> * sync_;
bool paused_;
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo> MyApproxSyncPolicy;
message_filters::Synchronizer<MyApproxSyncPolicy> * approxSync_;
typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo> MyExactSyncPolicy;
message_filters::Synchronizer<MyExactSyncPolicy> * exactSync_;
};
int main(int argc, char *argv[])
@@ -567,52 +410,9 @@ int main(int argc, char *argv[])
ULogger::setType(ULogger::kTypeConsole);
ULogger::setLevel(ULogger::kWarning);
//pcl::console::setVerbosityLevel(pcl::console::L_DEBUG);
ros::init(argc, argv, "visual_odometry");
ros::init(argc, argv, "stereo_odometry");
for(int i=1;i<argc;++i)
{
if(strcmp(argv[i], "--params") == 0)
{
rtabmap::ParametersMap parameters = rtabmap::Parameters::getDefaultParameters();
rtabmap::ParametersMap parametersOdom;
if(strcmp(argv[i], "--params") == 0)
{
// show specific parameters
for(rtabmap::ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
{
if(uSplit(iter->first, '/').front().compare("Odom") == 0 ||
uSplit(iter->first, '/').front().compare("SURF") == 0 ||
uSplit(iter->first, '/').front().compare("SIFT") == 0 ||
uSplit(iter->first, '/').front().compare("ORB") == 0 ||
uSplit(iter->first, '/').front().compare("FAST") == 0 ||
uSplit(iter->first, '/').front().compare("FREAK") == 0 ||
uSplit(iter->first, '/').front().compare("BRIEF") == 0 ||
uSplit(iter->first, '/').front().compare("GFTT") == 0 ||
uSplit(iter->first, '/').front().compare("BRISK") == 0)
{
parametersOdom.insert(*iter);
}
}
}
for(rtabmap::ParametersMap::iterator iter=parametersOdom.begin(); iter!=parametersOdom.end(); ++iter)
{
std::string str = "Param: " + iter->first + " = \"" + iter->second + "\"";
std::cout <<
str <<
std::setw(60 - str.size()) <<
" [" <<
rtabmap::Parameters::getDescription(iter->first).c_str() <<
"]" <<
std::endl;
}
ROS_WARN("Node will now exit after showing default odometry parameters because "
"argument \"--params\" is detected!");
exit(0);
}
}
StereoOdometry vOdom;
StereoOdometry odom(argc, argv);
ros::spin();
return 0;
}
src/VisualOdometryDEPRECATED.cpp
+37
View File
@@ -0,0 +1,37 @@
/*
Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include <rtabmap/utilite/ULogger.h>
int main(int argc, char *argv[])
{
ULogger::setType(ULogger::kTypeConsole);
UFATAL("visual_odometry node is deprecated, use rgbd_odometry instead!");
return 0;
}
src/VisualOdometryNode.cpp
-439
View File
@@ -1,439 +0,0 @@
/*
Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include <ros/ros.h>
#include <tf/tf.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_listener.h>
#include <std_srvs/Empty.h>
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <image_transport/image_transport.h>
#include <image_transport/subscriber_filter.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/image_encodings.h>
#include <sensor_msgs/PointCloud2.h>
#include <nav_msgs/Odometry.h>
#include <pcl_conversions/pcl_conversions.h>
#include <cv_bridge/cv_bridge.h>
#include <rtabmap/core/Odometry.h>
#include <rtabmap/core/util3d.h>
#include <rtabmap/core/Memory.h>
#include <rtabmap/core/Signature.h>
#include "rtabmap/MsgConversion.h"
#include "rtabmap/utilite/UConversion.h"
#include "rtabmap/utilite/ULogger.h"
#include "rtabmap/utilite/UStl.h"
using namespace rtabmap;
class VisualOdometry
{
public:
VisualOdometry() :
odometry_(0),
frameId_("base_link"),
odomFrameId_("odom"),
publishTf_(true),
sync_(0),
paused_(false)
{
ros::NodeHandle nh;
odomPub_ = nh.advertise<nav_msgs::Odometry>("odom", 1);
odomLocalMapPub_ = nh.advertise<sensor_msgs::PointCloud2>("odom_local_map", 1);
odomLastFrame_ = nh.advertise<sensor_msgs::PointCloud2>("odom_last_frame", 1);
ros::NodeHandle pnh("~");
int queueSize = 5;
pnh.param("frame_id", frameId_, frameId_);
pnh.param("odom_frame_id", odomFrameId_, odomFrameId_);
pnh.param("publish_tf", publishTf_, publishTf_);
pnh.param("queue_size", queueSize, queueSize);
//parameters
rtabmap::ParametersMap parameters = rtabmap::Parameters::getDefaultParameters();
rtabmap::ParametersMap parametersOdom;
for(rtabmap::ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
{
std::string group = uSplit(iter->first, '/').front();
if(group.compare("Odom") == 0 ||
group.compare("SURF") == 0 ||
group.compare("SIFT") == 0 ||
group.compare("ORB") == 0 ||
group.compare("FAST") == 0 ||
group.compare("FREAK") == 0 ||
group.compare("BRIEF") == 0 ||
group.compare("GFTT") == 0 ||
group.compare("BRISK") == 0)
{
parametersOdom.insert(*iter);
}
}
for(rtabmap::ParametersMap::iterator iter=parametersOdom.begin(); iter!=parametersOdom.end(); ++iter)
{
std::string vStr;
bool vBool;
int vInt;
double vDouble;
if(pnh.getParam(iter->first, vStr))
{
ROS_INFO("Setting odometry parameter \"%s\"=\"%s\"", iter->first.c_str(), vStr.c_str());
iter->second = vStr;
}
else if(pnh.getParam(iter->first, vBool))
{
ROS_INFO("Setting odometry parameter \"%s\"=\"%s\"", iter->first.c_str(), uBool2Str(vBool).c_str());
iter->second = uBool2Str(vBool);
}
else if(pnh.getParam(iter->first, vInt))
{
ROS_INFO("Setting odometry parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vInt).c_str());
iter->second = uNumber2Str(vInt);
if(iter->first.compare(Parameters::kOdomMinInliers()) == 0 && vInt < 8)
{
ROS_WARN("Parameter min_inliers must be >= 8, setting to 8...");
iter->second = uNumber2Str(8);
}
}
else if(pnh.getParam(iter->first, vDouble))
{
ROS_INFO("Setting odometry parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vDouble).c_str());
iter->second = uNumber2Str(vDouble);
}
}
odometry_ = new rtabmap::OdometryBOW(parametersOdom);
ros::NodeHandle rgb_nh(nh, "rgb");
ros::NodeHandle depth_nh(nh, "depth");
ros::NodeHandle rgb_pnh(pnh, "rgb");
ros::NodeHandle depth_pnh(pnh, "depth");
image_transport::ImageTransport rgb_it(rgb_nh);
image_transport::ImageTransport depth_it(depth_nh);
image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh);
image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh);
image_mono_sub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb);
image_depth_sub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth);
info_sub_.subscribe(rgb_nh, "camera_info", 1);
sync_ = new message_filters::Synchronizer<MySyncPolicy>(MySyncPolicy(queueSize), image_mono_sub_, image_depth_sub_, info_sub_);
sync_->registerCallback(boost::bind(&VisualOdometry::callback, this, _1, _2, _3));
resetSrv_ = nh.advertiseService("reset_odom", &VisualOdometry::reset, this);
pauseSrv_ = nh.advertiseService("pause_odom", &VisualOdometry::pause, this);
resumeSrv_ = nh.advertiseService("resume_odom", &VisualOdometry::resume, this);
}
~VisualOdometry()
{
ros::NodeHandle pnh("~");
ParametersMap parameters = Parameters::getDefaultParameters();
for(ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
{
pnh.deleteParam(iter->first);
}
delete sync_;
delete odometry_;
}
void callback(const sensor_msgs::ImageConstPtr& image,
const sensor_msgs::ImageConstPtr& depth,
const sensor_msgs::CameraInfoConstPtr& cameraInfo)
{
if(!paused_)
{
if(!(image->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
image->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
image->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
image->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) ||
!(depth->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1)==0 ||
depth->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)==0))
{
ROS_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 (mono8 recommended) and image_depth=16UC1");
return;
}
else if(depth->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)==0)
{
static bool warned = false;
if(!warned)
{
ROS_WARN("Input depth type is 32FC1, please use type 16UC1 for depth. The depth images "
"will be processed anyway but with a conversion. This warning is only be printed once...");
warned = true;
}
}
tf::StampedTransform localTransform;
try
{
tfListener_.lookupTransform(frameId_, image->header.frame_id, image->header.stamp, localTransform);
}
catch(tf::TransformException & ex)
{
ROS_WARN("%s",ex.what());
return;
}
ros::WallTime time = ros::WallTime::now();
int quality = -1;
if(image->data.size() && depth->data.size() && cameraInfo->K[4] != 0)
{
float depthFx = cameraInfo->K[0];
float depthFy = cameraInfo->K[4];
float depthCx = cameraInfo->K[2];
float depthCy = cameraInfo->K[5];
cv_bridge::CvImageConstPtr ptrImage = cv_bridge::toCvShare(image, "mono8");
cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depth);
rtabmap::SensorData data(ptrImage->image,
ptrDepth->image.type() == CV_32FC1?util3d::cvtDepthFromFloat(ptrDepth->image):ptrDepth->image,
depthFx,
depthFy,
depthCx,
depthCy,
rtabmap::Transform(),
rtabmap::transformFromTF(localTransform));
quality=0;
rtabmap::Transform pose = odometry_->process(data, &quality);
if(!pose.isNull())
{
//*********************
// Update odometry
//*********************
tf::Transform poseTF;
rtabmap::transformToTF(pose, poseTF);
if(publishTf_)
{
tfBroadcaster_.sendTransform( tf::StampedTransform (poseTF, image->header.stamp, odomFrameId_, frameId_));
}
if(odomPub_.getNumSubscribers())
{
//next, we'll publish the odometry message over ROS
nav_msgs::Odometry odom;
odom.header.stamp = image->header.stamp; // use corresponding time stamp to image
odom.header.frame_id = odomFrameId_;
odom.child_frame_id = frameId_;
//set the position
odom.pose.pose.position.x = poseTF.getOrigin().x();
odom.pose.pose.position.y = poseTF.getOrigin().y();
odom.pose.pose.position.z = poseTF.getOrigin().z();
tf::quaternionTFToMsg(poseTF.getRotation().normalized(), odom.pose.pose.orientation);
//publish the message
odomPub_.publish(odom);
}
if(odomLocalMapPub_.getNumSubscribers())
{
const std::multimap<int, pcl::PointXYZ> & map = odometry_->getLocalMap();
pcl::PointCloud<pcl::PointXYZ> cloud;
for(std::multimap<int, pcl::PointXYZ>::const_iterator iter=map.begin(); iter!=map.end(); ++iter)
{
cloud.push_back(iter->second);
}
sensor_msgs::PointCloud2 cloudMsg;
pcl::toROSMsg(cloud, cloudMsg);
cloudMsg.header.stamp = image->header.stamp; // use corresponding time stamp to image
cloudMsg.header.frame_id = odomFrameId_;
odomLocalMapPub_.publish(cloudMsg);
}
if(odomLastFrame_.getNumSubscribers())
{
const rtabmap::Signature * s = odometry_->getMemory()->getLastWorkingSignature();
if(s)
{
const std::multimap<int, pcl::PointXYZ> & words3 = s->getWords3();
pcl::PointCloud<pcl::PointXYZ> cloud;
rtabmap::Transform t = rtabmap::transformFromTF(localTransform);
for(std::multimap<int, pcl::PointXYZ>::const_iterator iter=words3.begin(); iter!=words3.end(); ++iter)
{
// transform to odom frame
pcl::PointXYZ pt = util3d::transformPoint(iter->second, pose);
cloud.push_back(pt);
}
sensor_msgs::PointCloud2 cloudMsg;
pcl::toROSMsg(cloud, cloudMsg);
cloudMsg.header.stamp = image->header.stamp; // use corresponding time stamp to image
cloudMsg.header.frame_id = odomFrameId_;
odomLastFrame_.publish(cloudMsg);
}
}
}
else
{
//ROS_WARN("Odometry lost!");
//send null pose to notify that odometry is lost
nav_msgs::Odometry odom;
odom.header.stamp = image->header.stamp; // use corresponding time stamp to image
odom.header.frame_id = odomFrameId_;
odom.child_frame_id = frameId_;
//publish the message
odomPub_.publish(odom);
}
}
ROS_INFO("Odom: quality=%d, update time=%fs", quality, (ros::WallTime::now()-time).toSec());
}
}
bool reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
{
ROS_INFO("visual_odometry: reset odom!");
odometry_->reset();
return true;
}
bool pause(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
{
if(paused_)
{
ROS_WARN("visual_odometry: Already paused!");
}
else
{
paused_ = true;
ROS_INFO("visual_odometry: paused!");
}
return true;
}
bool resume(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
{
if(!paused_)
{
ROS_WARN("visual_odometry: Already running!");
}
else
{
paused_ = false;
ROS_INFO("visual_odometry: resumed!");
}
return true;
}
private:
rtabmap::OdometryBOW * odometry_;
// parameters
std::string frameId_;
std::string odomFrameId_;
bool publishTf_;
ros::Publisher odomPub_;
ros::Publisher odomLocalMapPub_;
ros::Publisher odomLastFrame_;
ros::ServiceServer resetSrv_;
ros::ServiceServer pauseSrv_;
ros::ServiceServer resumeSrv_;
tf::TransformBroadcaster tfBroadcaster_;
tf::TransformListener tfListener_;
image_transport::SubscriberFilter image_mono_sub_;
image_transport::SubscriberFilter image_depth_sub_;
message_filters::Subscriber<sensor_msgs::CameraInfo> info_sub_;
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MySyncPolicy;
message_filters::Synchronizer<MySyncPolicy> * sync_;
bool paused_;
};
int main(int argc, char *argv[])
{
ULogger::setType(ULogger::kTypeConsole);
ULogger::setLevel(ULogger::kWarning);
ros::init(argc, argv, "visual_odometry");
for(int i=1;i<argc;++i)
{
if(strcmp(argv[i], "--params") == 0)
{
rtabmap::ParametersMap parameters = rtabmap::Parameters::getDefaultParameters();
rtabmap::ParametersMap parametersOdom;
if(strcmp(argv[i], "--params") == 0)
{
// show specific parameters
for(rtabmap::ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
{
if(uSplit(iter->first, '/').front().compare("Odom") == 0 ||
uSplit(iter->first, '/').front().compare("SURF") == 0 ||
uSplit(iter->first, '/').front().compare("SIFT") == 0 ||
uSplit(iter->first, '/').front().compare("ORB") == 0 ||
uSplit(iter->first, '/').front().compare("FAST") == 0 ||
uSplit(iter->first, '/').front().compare("FREAK") == 0 ||
uSplit(iter->first, '/').front().compare("BRIEF") == 0 ||
uSplit(iter->first, '/').front().compare("GFTT") == 0 ||
uSplit(iter->first, '/').front().compare("BRISK") == 0)
{
parametersOdom.insert(*iter);
}
}
}
for(rtabmap::ParametersMap::iterator iter=parametersOdom.begin(); iter!=parametersOdom.end(); ++iter)
{
std::string str = "Param: " + iter->first + " = \"" + iter->second + "\"";
std::cout <<
str <<
std::setw(60 - str.size()) <<
" [" <<
rtabmap::Parameters::getDescription(iter->first).c_str() <<
"]" <<
std::endl;
}
ROS_WARN("Node will now exit after showing default odometry parameters because "
"argument \"--params\" is detected!");
exit(0);
}
}
VisualOdometry vOdom;
ros::spin();
return 0;
}
src/nodelets/point_cloud_xyzrgb.cpp
+13 -4
View File
@@ -41,6 +41,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <image_transport/image_transport.h>
#include <image_transport/subscriber_filter.h>
#include <image_geometry/pinhole_camera_model.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/subscriber.h>
@@ -114,14 +116,21 @@ private:
cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(image, "bgr8");
cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(imageDepth);
image_geometry::PinholeCameraModel model;
model.fromCameraInfo(*cameraInfo);
float fx = model.fx();
float fy = model.fy();
float cx = model.cx();
float cy = model.cy();
pcl::PointCloud<pcl::PointXYZRGB>::Ptr pclCloud;
pclCloud = rtabmap::util3d::cloudFromDepthRGB(
imagePtr->image,
imageDepthPtr->image,
cameraInfo->K[2],
cameraInfo->K[5],
cameraInfo->K[0],
cameraInfo->K[4],
cx,
cy,
fx,
fy,
decimation_);
if(voxelSize_ > 0.0)