Hi
Im using two point Grey Chameleon3 mono camera set up as Master Salve and synchronized so can work as stereo camera. I have a installed ROS driver and able to publish the camera topics . Im using this ROS driver https://github.com/KumarRobotics/flea3. And with roslaunch flea3 stereo_node.launch left:=18081067 right:=17496474 camera:=stereo_camera
can launch the driver. Im hand holding the cameras so for that case using this tutorial http://wiki.ros.org/rtabmap_ros/Tutorials/StereoHandHeldMapping about hand-held stereo mapping.
This is my launch file that Im launching with rtabmap$ roslaunch rtabmap_ros stereo_mapping.launch rtabmap_args:="--delete_db_on_start --Vis/CorFlowMaxLevel 9000 --Stereo/MaxDisparity 80000 --Odom/Strategy 1 --Odom/GuessMotion true --Vis/EstimationType 1 --Vis/CorType 1 --Odom/ResetCountdown 1" left_image_topic:=/stereo_camera/left/image_rect_color queue_size:=40 stereo:=true rviz:=true rtabmapviz:=false
<launch>
<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_rtabmap"
args="$(arg optical_rotate) base_link stereo_camera/left 100" />
<group ns="/stereo_camera" >
<node pkg="stereo_image_proc" type="stereo_image_proc" name="stereo_image_proc"/>
!-- Odometry -->
</group>
<!-- Choose between RGB-D and stereo -->
<arg name="stereo" default="true"/>
<!-- Choose visualization -->
<arg name="rtabmapviz" default="true" />
<arg name="rviz" default="false" />
<!-- Localization-only mode -->
<arg name="localization" default="false"/>
<!-- sim time for convenience, if playing a rosbag -->
<arg name="use_sim_time" default="false"/>
<param if="$(arg use_sim_time)" name="use_sim_time" value="true"/>
<!-- Corresponding config files -->
<arg name="cfg" default="" /> <!-- To change RTAB-Map's parameters, set the path of config file (*.ini) generated by the standalone app -->
<arg name="gui_cfg" default="~/.ros/rtabmap_gui.ini" />
<arg name="rviz_cfg" default="$(find rtabmap_ros)/launch/config/rgbd.rviz" />
<arg name="frame_id" default="base_link"/> <!-- Fixed frame id, you may set "base_link" or "base_footprint" if they are published -->
<arg name="odom_frame_id" default=""/> <!-- If set, TF is used to get odometry instead of the topic -->
<arg name="map_frame_id" default="map"/>
<arg name="ground_truth_frame_id" default=""/> <!-- e.g., "world" -->
<arg name="ground_truth_base_frame_id" default=""/> <!-- e.g., "tracker", a fake frame matching the frame "frame_id" (but on different TF tree) -->
<arg name="namespace" default="rtabmap"/>
<arg name="database_path" default="~/.ros/rtabmap.db"/>
<arg name="queue_size" default="10"/>
<arg name="wait_for_transform" default="0.2"/>
<arg name="args" default=""/> <!-- delete_db_on_start, udebug -->
<arg name="rtabmap_args" default="$(arg args)"/> <!-- deprecated, use "args" argument -->
<arg name="launch_prefix" default=""/> <!-- for debugging purpose, it fills launch-prefix tag of the nodes -->
<arg name="output" default="screen"/> <!-- Control node output (screen or log) -->
<!-- if timestamps of the input topics are synchronized using approximate or exact time policy-->
<arg if="$(arg stereo)" name="approx_sync" default="false"/>
<arg unless="$(arg stereo)" name="approx_sync" default="true"/>
<!-- RGB-D related topics -->
<arg name="rgb_topic" default="/camera/rgb/image_rect_color" />
<arg name="depth_topic" default="/camera/depth_registered/image_raw" />
<arg name="camera_info_topic" default="/camera/rgb/camera_info" />
<arg name="depth_camera_info_topic" default="$(arg camera_info_topic)" />
<!-- stereo related topics -->
<arg name="stereo_namespace" default="/stereo_camera"/>
<arg name="left_image_topic" default="$(arg stereo_namespace)/left/image_rect" />
<arg name="right_image_topic" default="$(arg stereo_namespace)/right/image_rect" /> <!-- using grayscale image for efficiency -->
<arg name="left_camera_info_topic" default="$(arg stereo_namespace)/left/camera_info" />
<arg name="right_camera_info_topic" default="$(arg stereo_namespace)/right/camera_info" />
<!-- Already synchronized RGB-D related topic, with rtabmap_ros/rgbd_sync nodelet -->
<arg name="rgbd_sync" default="false"/> <!-- pre-sync rgb_topic, depth_topic, camera_info_topic -->
<arg name="approx_rgbd_sync" default="true"/> <!-- false=exact synchronization -->
<arg name="subscribe_rgbd" default="$(arg rgbd_sync)"/>
<arg name="rgbd_topic" default="rgbd_image" />
<arg name="depth_scale" default="1.0" />
<arg name="compressed" default="false"/> <!-- If you want to subscribe to compressed image topics -->
<arg name="rgb_image_transport" default="compressed"/> <!-- Common types: compressed, theora (see "rosrun image_transport list_transports") -->
<arg name="depth_image_transport" default="compressedDepth"/> <!-- Depth compatible types: compressedDepth (see "rosrun image_transport list_transports") -->
<arg name="subscribe_scan" default="false"/>
<arg name="scan_topic" default="/scan"/>
<arg name="subscribe_scan_cloud" default="false"/>
<arg name="scan_cloud_topic" default="/scan_cloud"/>
<arg name="scan_normal_k" default="0"/>
<arg name="visual_odometry" default="true"/> <!-- Launch rtabmap visual odometry node -->
<arg name="icp_odometry" default="false"/> <!-- Launch rtabmap icp odometry node -->
<arg name="odom_topic" default="odom"/> <!-- Odometry topic name -->
<arg name="vo_frame_id" default="$(arg odom_topic)"/> <!-- Visual/Icp odometry frame ID for TF -->
<arg name="odom_tf_angular_variance" default="1"/> <!-- If TF is used to get odometry, this is the default angular variance -->
<arg name="odom_tf_linear_variance" default="1"/> <!-- If TF is used to get odometry, this is the default linear variance -->
<arg name="odom_args" default=""/> <!-- More arguments for odometry (overwrite same parameters in rtabmap_args) -->
<arg name="odom_sensor_sync" default="false"/>
<arg name="odom_guess_frame_id" default=""/>
<arg name="odom_guess_min_translation" default="0"/>
<arg name="odom_guess_min_rotation" default="0"/>
<arg name="subscribe_user_data" default="false"/> <!-- user data synchronized subscription -->
<arg name="user_data_topic" default="/user_data"/>
<arg name="user_data_async_topic" default="/user_data_async" /> <!-- user data async subscription (rate should be lower than map update rate) -->
<!-- These arguments should not be modified directly, see referred topics without "_relay" suffix above -->
<arg if="$(arg compressed)" name="rgb_topic_relay" default="$(arg rgb_topic)_relay"/>
<arg unless="$(arg compressed)" name="rgb_topic_relay" default="$(arg rgb_topic)"/>
<arg if="$(arg compressed)" name="depth_topic_relay" default="$(arg depth_topic)_relay"/>
<arg unless="$(arg compressed)" name="depth_topic_relay" default="$(arg depth_topic)"/>
<arg if="$(arg compressed)" name="left_image_topic_relay" default="$(arg left_image_topic)_relay"/>
<arg unless="$(arg compressed)" name="left_image_topic_relay" default="$(arg left_image_topic)"/>
<arg if="$(arg compressed)" name="right_image_topic_relay" default="$(arg right_image_topic)_relay"/>
<arg unless="$(arg compressed)" name="right_image_topic_relay" default="$(arg right_image_topic)"/>
<!-- Nodes -->
<group ns="$(arg namespace)">
<!-- Visual odometry -->
<!-- Stereo Odometry -->
<node if="$(arg stereo)" pkg="rtabmap_ros" type="stereo_odometry" name="stereo_odometry" output="$(arg output)" args="$(arg rtabmap_args) $(arg odom_args)" launch-prefix="$(arg launch_prefix)">
<remap from="left/image_rect" to="$(arg left_image_topic_relay)"/>
<remap from="right/image_rect" to="$(arg right_image_topic_relay)"/>
<remap from="left/camera_info" to="$(arg left_camera_info_topic)"/>
<remap from="right/camera_info" to="$(arg right_camera_info_topic)"/>
<remap from="odom" to="$(arg odom_topic)"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="odom_frame_id" type="string" value="$(arg vo_frame_id)"/>
<param name="ground_truth_frame_id" type="string" value="$(arg ground_truth_frame_id)"/>
<param name="ground_truth_base_frame_id" type="string" value="$(arg ground_truth_base_frame_id)"/>
<param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
<param name="approx_sync" type="bool" value="$(arg approx_sync)"/>
<param name="config_path" type="string" value="$(arg cfg)"/>
<param name="queue_size" type="int" value="$(arg queue_size)"/>
<param name="subscribe_rgbd" type="bool" value="$(arg subscribe_rgbd)"/>
<param name="guess_frame_id" type="string" value="$(arg odom_guess_frame_id)"/>
<param name="guess_min_translation" type="double" value="$(arg odom_guess_min_translation)"/>
<param name="guess_min_rotation" type="double" value="$(arg odom_guess_min_rotation)"/>
</node>
<!-- Visual SLAM (robot side) -->
<!-- args: "delete_db_on_start" and "udebug" -->
<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="$(arg output)" args="$(arg rtabmap_args)" launch-prefix="$(arg launch_prefix)">
<param if="$(arg stereo)" name="subscribe_depth" type="bool" value="false"/>
<param unless="$(arg stereo)" name="subscribe_depth" type="bool" value="true"/>
<param name="subscribe_rgbd" type="bool" value="$(arg subscribe_rgbd)"/>
<param name="subscribe_stereo" type="bool" value="$(arg stereo)"/>
<param name="subscribe_scan" type="bool" value="$(arg subscribe_scan)"/>
<param name="subscribe_scan_cloud" type="bool" value="$(arg subscribe_scan_cloud)"/>
<param name="subscribe_user_data" type="bool" value="$(arg subscribe_user_data)"/>
<param if="$(arg visual_odometry)" name="subscribe_odom_info" type="bool" value="true"/>
<param if="$(arg icp_odometry)" name="subscribe_odom_info" type="bool" value="true"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="map_frame_id" type="string" value="$(arg map_frame_id)"/>
<param name="odom_frame_id" type="string" value="$(arg odom_frame_id)"/>
<param name="ground_truth_frame_id" type="string" value="$(arg ground_truth_frame_id)"/>
<param name="ground_truth_base_frame_id" type="string" value="$(arg ground_truth_base_frame_id)"/>
<param name="odom_tf_angular_variance" type="double" value="$(arg odom_tf_angular_variance)"/>
<param name="odom_tf_linear_variance" type="double" value="$(arg odom_tf_linear_variance)"/>
<param name="odom_sensor_sync" type="bool" value="$(arg odom_sensor_sync)"/>
<param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
<param name="database_path" type="string" value="$(arg database_path)"/>
<param name="approx_sync" type="bool" value="$(arg approx_sync)"/>
<param name="config_path" type="string" value="$(arg cfg)"/>
<param name="queue_size" type="int" value="$(arg queue_size)"/>
<param name="scan_normal_k" type="int" value="$(arg scan_normal_k)"/>
<remap from="depth/image" to="$(arg depth_topic_relay)"/>
<remap from="left/image_rect" to="$(arg left_image_topic_relay)"/>
<remap from="right/image_rect" to="$(arg right_image_topic_relay)"/>
<remap from="left/camera_info" to="$(arg left_camera_info_topic)"/>
<remap from="right/camera_info" to="$(arg right_camera_info_topic)"/>
<remap from="user_data" to="$(arg user_data_topic)"/>
<remap from="user_data_async" to="$(arg user_data_async_topic)"/>
<remap from="odom" to="$(arg odom_topic)"/>
<!-- localization mode -->
<param if="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="false"/>
<param unless="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="true"/>
<param name="Mem/InitWMWithAllNodes" type="string" value="$(arg localization)"/>
</node>
<!-- Visualisation RTAB-Map -->
<node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(arg gui_cfg)" output="$(arg output)" launch-prefix="$(arg launch_prefix)">
<param if="$(arg stereo)" name="subscribe_depth" type="bool" value="false"/>
<param unless="$(arg stereo)" name="subscribe_depth" type="bool" value="true"/>
<param name="subscribe_rgbd" type="bool" value="$(arg subscribe_rgbd)"/>
<param name="subscribe_stereo" type="bool" value="$(arg stereo)"/>
<param name="subscribe_scan" type="bool" value="$(arg subscribe_scan)"/>
<param name="subscribe_scan_cloud" type="bool" value="$(arg subscribe_scan_cloud)"/>
<param if="$(arg visual_odometry)" name="subscribe_odom_info" type="bool" value="true"/>
<param if="$(arg icp_odometry)" name="subscribe_odom_info" type="bool" value="true"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="odom_frame_id" type="string" value="$(arg odom_frame_id)"/>
<param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
<param name="queue_size" type="int" value="$(arg queue_size)"/>
<param name="approx_sync" type="bool" value="$(arg approx_sync)"/>
</node>
</group>
<!-- Visualization RVIZ -->
<node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(arg rviz_cfg)"/>
<node if="$(arg rviz)" pkg="nodelet" type="nodelet" name="points_xyzrgb" args="standalone rtabmap_ros/point_cloud_xyzrgb" output="$(arg output)">
<remap from="cloud" to="voxel_cloud" />
<param name="decimation" type="double" value="4"/>
<param name="voxel_size" type="double" value="5.0"/>
<param name="approx_sync" type="bool" value="$(arg approx_sync)"/>
</node>
</launch>
this is my TF tree from
rosrun tf view_frames
then rectified left right images and disparity.
Im not sure if the density is good enough but my point clouds in rtabmapviz looks ok. This is the rtabmapviz view
The odometry quality is ok I think as it is around 900 as the output shows.
[ INFO] [1534836551.209910547]: Odom: quality=922, std dev=0.046187m|0.029729rad, update time=0.042618s
[ INFO] [1534836551.255021098]: Odom: quality=930, std dev=0.042296m|0.025758rad, update time=0.037571s
[ INFO] [1534836551.316518224]: Odom: quality=916, std dev=0.051433m|0.024391rad, update time=0.048281s
[ INFO] [1534836551.348123310]: Odom: quality=927, std dev=0.048367m|0.024039rad, update time=0.030788s
And the Left camera_info:
header:
seq: 8450
stamp:
secs: 1534835474
nsecs: 986650929
frame_id: "/stereo_camera/left"
height: 384
width: 512
distortion_model: "plumb_bob"
D: [-0.2055041051204516, 0.1089634021571866, -0.003090968625828524, 0.005018586734219781, 0.0]
K: [436.2518603710264, 0.0, 272.4668548867118, 0.0, 441.0091008894988, 174.4470981361869, 0.0, 0.0, 1.0]
R: [0.9980451574484078, 0.02122380979567181, 0.05878276610997515, -0.02121268522220995, 0.9997746548558092, -0.0008133225748451598, -0.05878678150270122, -0.0004352076567140128, 0.9982704668149054]
P: [445.2952930001379, 0.0, 240.8624572753906, 0.0, 0.0, 445.2952930001379, 179.0239391326904, 0.0, 0.0, 0.0, 1.0, 0.0]
binning_x: 0
binning_y: 0
roi:
x_offset: 0
y_offset: 0
height: 0
width: 0
do_rectify: False
Im able to get the octomap but is not so good. Any help how to improve the map?
This is my Rviz. Fixed frame is base_link. Im not sure what should be the reference frame, as I dont have robot just hand hold the camera.
Here the map in rviz
Now the problem is the null timestamp Left Camera
seq: 1280
stamp:
secs: 0
nsecs: 0
frame_id: "/stereo_camera/left"
height: 384
width: 512
distortion_model: "plumb_bob"
D: [-0.2086468681317675, 0.1309844488937598, 0.001823015908195491, 0.003865352272151913, 0.0]
K: [451.5433695164733, 0.0, 262.2015316836384, 0.0, 454.8382333187173, 182.9314120544186, 0.0, 0.0, 1.0]
R: [0.9989286928378713, 0.01431409869945952, 0.04400651319570897, -0.0143430318904704, 0.99989707501352, 0.0003417829667208515, -0.04399709151081382, -0.0009726036343351752, 0.999031185689796]
P: [457.894691467317, 0.0, 240.6130847930908, 0.0, 0.0, 457.894691467317, 189.4381809234619, 0.0, 0.0, 0.0, 1.0, 0.0]
binning_x: 0
binning_y: 0
roi:
x_offset: 0
y_offset: 0
height: 0
width: 0
do_rectify: False
Right Camera
header:
seq: 2738
stamp:
secs: 0
nsecs: 0
frame_id: "/stereo_camera/right"
height: 384
width: 512
distortion_model: "plumb_bob"
D: [-0.2228200499957892, 0.1973123886940819, 0.002679274149156493, 0.001686405084842895, 0.0]
K: [444.8007479437985, 0.0, 265.8307844199959, 0.0, 447.6530059966123, 193.6930325932133, 0.0, 0.0, 1.0]
R: [0.9988184466381711, 0.01405957605271069, 0.04651923232953362, -0.01402898365686818, 0.999901104726368, -0.000984065315183817, -0.04652846733846289, 0.0003302850394213167, 0.9989169097772479]
P: [457.894691467317, 0.0, 240.6130847930908, -58.04791693861652, 0.0, 457.894691467317, 189.4381809234619, 0.0, 0.0, 0.0, 1.0, 0.0]
binning_x: 0
binning_y: 0
roi:
x_offset: 0
y_offset: 0
height: 0
width: 0
do_rectify: False
Thanks
Originally posted by Astronaut on ROS Answers with karma: 330 on 2018-08-21
Post score: 1