stereo_odometry and optical flow crash?
Locked
 
|
Hello,
I'm attempting to test out the various options for visual odometry, to see if I can improve the existing wheel odometry / IMU localization. I'll paste my full launch file below, but the error is as follows: I can run using F2M and Feature Matching, but when I try to use Optical Flow I get the following crash: terminate called after throwing an instance of 'UException' what(): [FATAL] (2018-02-13 18:33:52.039) OdometryF2M.cpp:763::computeTransform() Condition (lastFrame_->getWordsDescriptors().size() == lastFrame_->getWords3().size()) not met! [0 vs 910] It's always [0 vs NNN], although the getWords3().size() can vary a bit. My launch file is below. Apologies about size, but I didn't want to leave out any useful info. There's a single stereo_odometry section which should contain the goods.
<?xml version="1.0"?>
<launch>
<param name="use_sim_time" type="bool" value="True"/>
<arg name="camera_namespace_name" default="bumblebee_xb3" />
<arg name="stereo_pair_name" default="stereo_camera_LC" />
<arg name="rtabmap_namespace_name" default="rtabmap" />
<arg name="odometry_topic_name" default="/odometry/filtered" />
<arg name="rtabmapviz" default="1" />
<arg name="stereo_odometry" default="false" />
<node name="image_proc" ns="$(arg camera_namespace_name)/$(arg stereo_pair_name)/left" pkg="image_proc" type="image_proc" output="screen" />
<node name="image_proc" ns="$(arg camera_namespace_name)/$(arg stereo_pair_name)/right" pkg="image_proc" type="image_proc" output="screen" />
<node if="$(arg stereo_odometry)" pkg="rtabmap_ros" type="stereo_odometry" name="stereo_odometry" >
<remap from="left/image_rect" to="/$(arg camera_namespace_name)/$(arg stereo_pair_name)/left/image_rect_color" />
<remap from="right/image_rect" to="/$(arg camera_namespace_name)/$(arg stereo_pair_name)/right/image_rect" />
<remap from="left/camera_info" to="/$(arg camera_namespace_name)/$(arg stereo_pair_name)/left/camera_info" />
<remap from="right/camera_info" to="/$(arg camera_namespace_name)/$(arg stereo_pair_name)/right/camera_info" />
<remap from="odom" to="/odometry/stereo" />
<remap from="odom_info" to="/odometry/stereo_odom_info" />
<param name="frame_id" type="string" value="base_footprint"/>
<param name="odom_frame_id" type="string" value="odom_stereo"/>
<param name="approx_sync" type="bool" value="false"/>
<param name="publish_tf" type="bool" value="true"/>
<param name="wait_for_transform" type="string" value="0.2" />
<!-- If the robot is holonomic (can strafe). If not, Y will be estimated from X and tan(yaw) -->
<param name="Odom/Holonomic" type="bool" value="false" />
<!-- Odometry Strategies: 0=Frame-to-Map (F2M) 1=Frame-to-Frame (F2F) 2=Fovis 3=viso2 4=DVO-SLAM 5=ORB_SLAM2 -->
<param name="Odom/Strategy" type="string" value="0" />
<!-- Max local map size, when doing F2M -->
<param name="OdomF2M/MaxSize" type="string" value="1000" />
<!-- Visual Odometry Correspondence Approach: 0=Feature Matching, 1=Optical Flow -->
<param name="Vis/CorType" type="string" value="1" />
<!-- Visual Odometry Features: 0=SURF, 1=SIFT, 2=ORB, 3=FAST/FREAK, 4=FAST/BRIEF, 5=GFTT/FREAK, 6=GFTT/BRIEF, 7=BRISK, 8=GFTT/ORB, 9=KAZE -->
<param name="Vis/FeatureType" type="string" value="6"/>
<!-- Fill info with data (inliers/outliers features) -->
<param name="Odom/FillInfoData" type="string" value="true"/>
<!-- Reset Odometry after X consecutive images on which odometry cannot be computed -->
<param name="Odom/ResetCountdown" type="string" value="0"/>
</node>
<group ns="$(arg rtabmap_namespace_name)" >
<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete-db-on-start" >
<param name="frame_id" type="string" value="base_footprint" />
<param name="subscribe_stereo" type="bool" value="true" />
<param name="subscribe_depth" type="bool" value="false" />
<param name="approx_sync" type="bool" value="false" />
<remap from="left/image_rect" to="/$(arg camera_namespace_name)/$(arg stereo_pair_name)/left/image_rect_color" />
<remap from="right/image_rect" to="/$(arg camera_namespace_name)/$(arg stereo_pair_name)/right/image_rect" />
<remap from="left/camera_info" to="/$(arg camera_namespace_name)/$(arg stereo_pair_name)/left/camera_info" />
<remap from="right/camera_info" to="/$(arg camera_namespace_name)/$(arg stereo_pair_name)/right/camera_info" />
<!-- Since approx_sync is false (we have perfectly synchronized images), we have to use
tf for the odom transform. This is because it also assumes that the nav_msgs/Odometry
timestamp perfectly matches, which it does not. You need to correclty set the
variance of the odometry as well. -->
<param name="odom_frame_id" value="odom" />
<param name="odom_tf_linear_variance" value="1" />
<param name="odom_tf_angular_variance" value="1" />
<!-- Not using the nav_msgs/Odometry topic due to sync issues (see above) -->
<!-- <remap from="odom" to="$(arg odometry_topic_name)" /> -->
<param name="queue_size" type="int" value="30" />
<!-- RTAB-Map's parameters -->
<param name="Rtabmap/TimeThr" type="string" value="700" />
<param name="Rtabmap/DetectionRate" type="string" value="1" />
<param name="Vis/MinInliers" type="string" value="10" />
<param name="Vis/InlierDistance" type="string" value="0.1" />
<!-- Feature Detection Strategy -->
<!-- Detector Stragety Options: 0=SURF, 1=SIFT, 2=ORB, 3=FAST/FREAK, 4=FAST/BRIEF, 5=GFTT/FREAK, 6=GFTT/BRIEF, 7=BRISK, 8=GFTT/ORB -->
<param name="Kp/DetectorStrategy" type="string" value="2" />
<!-- Nearest-Neighbor Options: 0=kNNFlannNaive, 1=kNNFlannKdTree, 2=kNNFlannLSH, 3=kNNBruteForce, 4=kNNBruteForceGPU -->
<param name="Kp/NNStrategy" type="string" value="1" />
<!-- Number of features per image -->
<param name="Kp/MaxFeatures" type="string" value="400" />
<!-- Graph Optimization Options -->
<!-- Robust: When true, use Vertigo when using g2o/GTSAM -->
<!-- RGBD/OptimizeMaxError: MUST BE 0 IF USING ROBUST. Reject loop closure if optimization error gt this value (0 disables). -->
<!-- Strategy Options: 0=TORO, 1=G2O, 2=GTSAM -->
<param name="Optimizer/Robust" type="bool" value="true" />
<param name="RGBD/OptimizeMaxError" type="string" value="0" />
<param name="Optimizer/Strategy" type="string" value="2" />
<!-- Dense Stereo parameters -->
<param name="StereoBM/MinDisparities" type="string" value="0" />
<param name="StereoBM/NumDisparities" type="string" value="128" />
<param name="StereoBM/SpeckleWindowSize" type="string" value="500" />
<!-- Mapping Configurations -->
<!-- General Configuration -->
<param name="Grid/3D" value="true" />
<param name="Grid/FromDepth" value="true" />
<!-- Ground/Obstacle Height -->
<param name="Grid/MaxObstacleHeight" value="2.0"/> <!-- Set so that we filter out overpasses / ceiling -->
<param name="Grid/MaxGroundHeight" value="0"/>
<param name="Grid/MaxGroundAngle" value="45"/>
<!-- Vehicle Mask -->
<param name="Grid/FootprintHeight" value = "1.1" />
<param name="Grid/FootprintLength" value = "0.99" />
<param name="Grid/FootprintWidth" value = "0.67" />
<!-- Min/Max Obstacle Distance -->
<param name="Grid/DepthMin" value="0.0" />
<param name="Grid/DepthMax" value="16.0" />
</node>
<node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" output="screen" >
<param name="subscribe_stereo" type="bool" value="true" />
<param name="subscribe_odom_info" type="bool" value="false" />
<param name="approx_sync" type="bool" value="false" />
<!-- Since approx_sync is false (we have perfectly synchronized images), we have to use
tf for the odom transform. This is because it also assumes that the nav_msgs/Odometry
timestamp perfectly matches, which it does not. You need to set correctly the
variance of the odometry as well. -->
<param name="odom_frame_id" value="odom" />
<param name="odom_tf_linear_variance" value="1" />
<param name="odom_tf_angular_variance" value="1" />
<param name="queue_size" type="int" value="10" />
<param name="frame_id" type="string" value="base_footprint" />
<remap from="left/image_rect" to="/$(arg camera_namespace_name)/$(arg stereo_pair_name)/left/image_rect_color" />
<remap from="right/image_rect" to="/$(arg camera_namespace_name)/$(arg stereo_pair_name)/right/image_rect" />
<remap from="left/camera_info" to="/$(arg camera_namespace_name)/$(arg stereo_pair_name)/left/camera_info" />
<remap from="right/camera_info" to="/$(arg camera_namespace_name)/$(arg stereo_pair_name)/right/camera_info" />
<!-- Not using the nav_msgs/Odometry topic due to sync issues (see above) -->
<!-- <remap from="odom" to="$(arg odometry_topic_name)" /> -->
<remap from="mapData" to="mapData" />
</node>
</group>
</launch>
|
Administrator
|
Hi,
Vis/CorType=1 (Optical Flow correspondences) can be used only with Odom/Strategy=1 (F2F). Note also that Vis/CorType=1 doesn't work anymore for rtabmap node, only for stereo_odometry or rgbd_odometry nodes. In recent code, there are warnings if Vis/CorType=1 is not supported by the approach. To improve odometry, take a look at the GFTT parameters, in particular MinDistance and QualityLevel. cheers, Mathieu |
|
|
Great, thank you for the quick reply!
|
|
|
Now that I've given this a run and no crash, I have two other stupid questions:
* Optical flow seems to have a lock for a little bit, then immediately drops out (i.e. after about 5s of data). I'll probably go back to using feature matching instead, but wondered if you have any insight into this behavior * On rtabmapviz, with my configuration listed above, I don't see any info about odometry. What have I configured incorrectly that it doesn't show up in rtabmapviz? Thank you! |
|
Administrator
|
Hi,
not sure what do you mean by locking a little bit then drops out. Maybe a video would be helpful. For rtabmapviz, you should set subscribe_odom_info to true, as it is not receiving any odometry info. cheers, Mathieu |
|
|
Hi,
For odom info in rtabmapviz, I'm still having an issue. Here's the lauch file for it (below). Note that I"m still using odometry from the tf in actual operation here (I've set odom_frame_id), because I still want to use the filtered platform odometry. I'd just like to also view the output of stereo_odometry so that I can check in on it while fusing it. I'm also posting the rosnode info for stereo_odometry and rtabmapviz, and the rostopic and rosparam stuff that's relevent. Launch
<node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" output="screen" >
<param name="subscribe_stereo" type="bool" value="false" />
<param if="$(arg stereo_odometry)" name="subscribe_odom_info" type="bool" value="true" />
<remap if="$(arg stereo_odometry)" from="odom_info" to="/odometry/stereo_odom_info" />
<param unless="$(arg stereo_odometry)" name="subscribe_odom_info" type="bool" value="false" />
<!-- If requested, add in point cloud data -->
<param if="$(arg use_pointcloud)" name="subscribe_scan_cloud" value="true"/>
<param unless="$(arg use_pointcloud)" name="subscribe_scan_cloud" value="false"/>
<!-- If requested, add in scan data -->
<param if="$(arg use_scan)" name="subscribe_scan" value="true"/>
<param unless="$(arg use_scan)" name="subscribe_scan" value="false"/>
<!-- For some reason, scan_cloud_topic param isn't working, so I'll just remap instead. -->
<remap from="scan_cloud" to="/velodyne_points" />
<remap from="scan" to="/scan" />
<param name="approx_sync" type="bool" value="false" />
<!-- Since approx_sync is false (we have perfectly synchronized images), we have to use
tf for the odom transform. This is because it also assumes that the nav_msgs/Odometry
timestamp perfectly matches, which it does not. You need to set correctly the
variance of the odometry as well. -->
<param name="odom_frame_id" value="odom" />
<param name="odom_tf_linear_variance" value="1" />
<param name="odom_tf_angular_variance" value="1" />
<param name="queue_size" type="int" value="10" />
<param name="frame_id" type="string" value="base_footprint" />
<remap from="left/image_rect" to="/$(arg camera_namespace_name)/$(arg stereo_pair_name)/left/image_rect_color" />
<remap from="right/image_rect" to="/$(arg camera_namespace_name)/$(arg stereo_pair_name)/right/image_rect" />
<remap from="left/camera_info" to="/$(arg camera_namespace_name)/$(arg stereo_pair_name)/left/camera_info" />
<remap from="right/camera_info" to="/$(arg camera_namespace_name)/$(arg stereo_pair_name)/right/camera_info" />
<!-- Not using the nav_msgs/Odometry topic due to sync issues (see above) -->
<!-- <remap from="odom" to="$(arg odometry_topic_name)" /> -->
<remap from="mapData" to="mapData" />
</node>
Screen Output
started roslaunch server http://MM198306-PC:38006/
SUMMARY
========
PARAMETERS
* /rosdistro: kinetic
* /rosversion: 1.12.12
* /rtabmap/rtabmap/Grid/3D: True
* /rtabmap/rtabmap/Grid/DepthMax: 8.0
* /rtabmap/rtabmap/Grid/DepthMin: 0.0
* /rtabmap/rtabmap/Grid/FootprintHeight: 1.1
* /rtabmap/rtabmap/Grid/FootprintLength: 0.99
* /rtabmap/rtabmap/Grid/FootprintWidth: 0.67
* /rtabmap/rtabmap/Grid/FromDepth: True
* /rtabmap/rtabmap/Grid/MaxGroundAngle: 45
* /rtabmap/rtabmap/Grid/MaxGroundHeight: 0
* /rtabmap/rtabmap/Grid/MaxObstacleHeight: 2.0
* /rtabmap/rtabmap/Kp/DetectorStrategy: 2
* /rtabmap/rtabmap/Kp/MaxFeatures: 400
* /rtabmap/rtabmap/Kp/NNStrategy: 1
* /rtabmap/rtabmap/Optimizer/Robust: True
* /rtabmap/rtabmap/Optimizer/Strategy: 2
* /rtabmap/rtabmap/RGBD/OptimizeMaxError: 0
* /rtabmap/rtabmap/Rtabmap/DetectionRate: 1
* /rtabmap/rtabmap/Rtabmap/TimeThr: 700
* /rtabmap/rtabmap/SURF/HessianThreshold: 1000
* /rtabmap/rtabmap/StereoBM/MinDisparities: 0
* /rtabmap/rtabmap/StereoBM/NumDisparities: 128
* /rtabmap/rtabmap/StereoBM/SpeckleWindowSize: 500
* /rtabmap/rtabmap/Vis/InlierDistance: 0.1
* /rtabmap/rtabmap/Vis/MinInliers: 10
* /rtabmap/rtabmap/approx_sync: False
* /rtabmap/rtabmap/frame_id: base_footprint
* /rtabmap/rtabmap/map_frame_id: rtabmap
* /rtabmap/rtabmap/odom_frame_id: odom
* /rtabmap/rtabmap/odom_tf_angular_variance: 1
* /rtabmap/rtabmap/odom_tf_linear_variance: 1
* /rtabmap/rtabmap/publish_tf: True
* /rtabmap/rtabmap/queue_size: 30
* /rtabmap/rtabmap/subscribe_depth: False
* /rtabmap/rtabmap/subscribe_stereo: True
* /rtabmap/rtabmap/tf_delay: 0.05
* /rtabmap/rtabmapviz/approx_sync: False
* /rtabmap/rtabmapviz/frame_id: base_footprint
* /rtabmap/rtabmapviz/odom_frame_id: odom
* /rtabmap/rtabmapviz/odom_tf_angular_variance: 1
* /rtabmap/rtabmapviz/odom_tf_linear_variance: 1
* /rtabmap/rtabmapviz/queue_size: 10
* /rtabmap/rtabmapviz/subscribe_odom_info: True
* /rtabmap/rtabmapviz/subscribe_scan: False
* /rtabmap/rtabmapviz/subscribe_scan_cloud: False
* /rtabmap/rtabmapviz/subscribe_stereo: False
* /stereo_odometry/GFTT/MinDistance: 5
* /stereo_odometry/GFTT/QualityLevel: 0.01
* /stereo_odometry/Odom/FillInfoData: true
* /stereo_odometry/Odom/Holonomic: False
* /stereo_odometry/Odom/ResetCountdown: 0
* /stereo_odometry/Odom/Strategy: 1
* /stereo_odometry/OdomF2M/MaxSize: 1000
* /stereo_odometry/Vis/CorType: 0
* /stereo_odometry/Vis/FeatureType: 6
* /stereo_odometry/approx_sync: False
* /stereo_odometry/frame_id: base_footprint
* /stereo_odometry/odom_frame_id: odom_stereo
* /stereo_odometry/publish_tf: True
* /stereo_odometry/wait_for_transform: 0.2
* /use_sim_time: True
NODES
/bumblebee_xb3/stereo_camera_LC/right/
image_proc (image_proc/image_proc)
/rtabmap/
rtabmap (rtabmap_ros/rtabmap)
rtabmapviz (rtabmap_ros/rtabmapviz)
/
stereo_odometry (rtabmap_ros/stereo_odometry)
/bumblebee_xb3/stereo_camera_LC/left/
image_proc (image_proc/image_proc)
auto-starting new master
process[master]: started with pid [26283]
ROS_MASTER_URI=http://localhost:11311
setting /run_id to 722a7690-1279-11e8-be55-247703e32908
process[rosout-1]: started with pid [26296]
started core service [/rosout]
process[bumblebee_xb3/stereo_camera_LC/left/image_proc-2]: started with pid [26320]
process[bumblebee_xb3/stereo_camera_LC/right/image_proc-3]: started with pid [26321]
process[stereo_odometry-4]: started with pid [26322]
process[rtabmap/rtabmap-5]: started with pid [26335]
process[rtabmap/rtabmapviz-6]: started with pid [26350]
[ERROR] [1518717345.880394414]: Skipped loading plugin with error: XML Document '/opt/ros/kinetic/share/gmapping/nodelet_plugins.xml' has no Root Element. This likely means the XML is malformed or missing..
[ERROR] [1518717345.881458293]: Skipped loading plugin with error: XML Document '/opt/ros/kinetic/share/gmapping/nodelet_plugins.xml' has no Root Element. This likely means the XML is malformed or missing..
[ INFO] [1518717346.020541423]: Starting node...
[ERROR] [1518717346.073554401]: Skipped loading plugin with error: XML Document '/opt/ros/kinetic/share/gmapping/nodelet_plugins.xml' has no Root Element. This likely means the XML is malformed or missing..
[ERROR] [1518717346.078835380]: Skipped loading plugin with error: XML Document '/opt/ros/kinetic/share/gmapping/nodelet_plugins.xml' has no Root Element. This likely means the XML is malformed or missing..
[ INFO] [1518717346.125567458]: Initializing nodelet with 8 worker threads.
[ WARN] [1518717346.198613463]: The input topic '/bumblebee_xb3/stereo_camera_LC/right/image_raw' is not yet advertised
[ WARN] [1518717346.198671032]: The input topic '/bumblebee_xb3/stereo_camera_LC/right/camera_info' is not yet advertised
[ WARN] [1518717346.201634858]: The input topic '/bumblebee_xb3/stereo_camera_LC/left/image_raw' is not yet advertised
[ WARN] [1518717346.201690168]: The input topic '/bumblebee_xb3/stereo_camera_LC/left/camera_info' is not yet advertised
[ INFO] [1518717346.247782883]: Starting node...
[ INFO] [1518717346.425551891]: rtabmapviz: Using configuration from "/home/tve_new/catkin_ws/src/rtabmap_ros/launch/config/rgbd_gui.ini"
[ INFO] [1518717347.713903880]: Reading parameters from the ROS server...
[ INFO] [1518717347.843159597]: Parameters read = 0
[ INFO] [1518717348.073571816]: /rtabmap/rtabmapviz: queue_size = 10
[ INFO] [1518717348.073608534]: /rtabmap/rtabmapviz: rgbd_cameras = 1
[ INFO] [1518717348.073621713]: /rtabmap/rtabmapviz: approx_sync = false
[ INFO] [1518717348.077619398]:
/rtabmap/rtabmapviz subscribed to:
/rtabmap/odom
[ INFO] [1518717348.077719065]: rtabmapviz started.
[ INFO] [1518717353.687136537, 1518211229.625009887]: /rtabmap/rtabmap(maps): map_filter_radius = 0.000000
[ INFO] [1518717353.687224394, 1518211229.625009887]: /rtabmap/rtabmap(maps): map_filter_angle = 30.000000
[ INFO] [1518717353.687262892, 1518211229.625009887]: /rtabmap/rtabmap(maps): map_cleanup = true
[ INFO] [1518717353.687295879, 1518211229.625009887]: /rtabmap/rtabmap(maps): map_negative_poses_ignored = false
[ INFO] [1518717353.687329691, 1518211229.625009887]: /rtabmap/rtabmap(maps): map_negative_scan_ray_tracing = true
[ INFO] [1518717353.687363033, 1518211229.625009887]: /rtabmap/rtabmap(maps): cloud_output_voxelized = true
[ INFO] [1518717353.687394150, 1518211229.625009887]: /rtabmap/rtabmap(maps): cloud_subtract_filtering = false
[ INFO] [1518717353.687448265, 1518211229.625009887]: /rtabmap/rtabmap(maps): cloud_subtract_filtering_min_neighbors = 2
[ INFO] [1518717353.690463444, 1518211229.625009887]: /rtabmap/rtabmap(maps): octomap_tree_depth = 16
[ INFO] [1518717353.690525902, 1518211229.625009887]: /rtabmap/rtabmap(maps): octomap_occupancy_thr = 0.500000
[ INFO] [1518717353.731000154, 1518211229.625009887]: rtabmap: frame_id = base_footprint
[ INFO] [1518717353.731059071, 1518211229.625009887]: rtabmap: odom_frame_id = odom
[ INFO] [1518717353.731094450, 1518211229.625009887]: rtabmap: map_frame_id = rtabmap
[ INFO] [1518717353.731160735, 1518211229.625009887]: rtabmap: tf_delay = 0.050000
[ INFO] [1518717353.731215850, 1518211229.625009887]: rtabmap: tf_tolerance = 0.100000
[ INFO] [1518717353.731253823, 1518211229.625009887]: rtabmap: odom_sensor_sync = false
[ INFO] [1518717353.732734674, 1518211229.625009887]: rtabmap: stereo_to_depth = false
[ INFO] [1518717353.828180266, 1518211229.718088475]: Setting RTAB-Map parameter "Grid/3D"="true"
[ INFO] [1518717353.835625626, 1518211229.728146848]: Setting RTAB-Map parameter "Grid/DepthMax"="8"
[ INFO] [1518717353.838703884, 1518211229.728146848]: Setting RTAB-Map parameter "Grid/DepthMin"="0"
[ INFO] [1518717353.848322575, 1518211229.738210453]: Setting RTAB-Map parameter "Grid/FootprintHeight"="1.1"
[ INFO] [1518717353.850751556, 1518211229.738210453]: Setting RTAB-Map parameter "Grid/FootprintLength"="0.99"
[ INFO] [1518717353.861387523, 1518211229.748279062]: Setting RTAB-Map parameter "Grid/FootprintWidth"="0.67"
[ INFO] [1518717353.863543272, 1518211229.748279062]: Setting RTAB-Map parameter "Grid/FromDepth"="true"
[ INFO] [1518717353.868576472, 1518211229.758339684]: Setting RTAB-Map parameter "Grid/MaxGroundAngle"="45"
[ INFO] [1518717353.869808009, 1518211229.758339684]: Setting RTAB-Map parameter "Grid/MaxGroundHeight"="0"
[ INFO] [1518717353.870949433, 1518211229.758339684]: Setting RTAB-Map parameter "Grid/MaxObstacleHeight"="2"
[ INFO] [1518717353.937625812, 1518211229.828767982]: Setting RTAB-Map parameter "Kp/DetectorStrategy"="2"
[ INFO] [1518717353.944503642, 1518211229.828767982]: Setting RTAB-Map parameter "Kp/MaxFeatures"="400"
[ INFO] [1518717353.946648397, 1518211229.838822775]: Setting RTAB-Map parameter "Kp/NNStrategy"="1"
[ INFO] [1518717354.162805918, 1518211230.051626138]: Setting RTAB-Map parameter "Optimizer/Robust"="true"
[ INFO] [1518717354.164171574, 1518211230.051626138]: Setting RTAB-Map parameter "Optimizer/Strategy"="2"
[ INFO] [1518717354.192917118, 1518211230.081794070]: Setting RTAB-Map parameter "RGBD/OptimizeMaxError"="0"
[ INFO] [1518717354.230563679, 1518211230.122748717]: Setting RTAB-Map parameter "Rtabmap/DetectionRate"="1"
[ INFO] [1518717354.251897365, 1518211230.142863558]: Setting RTAB-Map parameter "Rtabmap/TimeThr"="700"
[ INFO] [1518717354.269024158, 1518211230.152919852]: Setting RTAB-Map parameter "SURF/HessianThreshold"="1000"
[ INFO] [1518717354.300179179, 1518211230.184091080]: Setting RTAB-Map parameter "StereoBM/NumDisparities"="128"
[ INFO] [1518717354.305416377, 1518211230.194147544]: Setting RTAB-Map parameter "StereoBM/SpeckleWindowSize"="500"
[ INFO] [1518717354.335412196, 1518211230.224311748]: Setting RTAB-Map parameter "Vis/InlierDistance"="0.1"
[ INFO] [1518717354.346859793, 1518211230.234410225]: Setting RTAB-Map parameter "Vis/MinInliers"="10"
[ INFO] [1518717354.663009613, 1518211230.548481501]: RTAB-Map detection rate = 1.000000 Hz
[ INFO] [1518717354.663309396, 1518211230.548481501]: rtabmap: Using database from "/home/tve_new/.ros/rtabmap.db" (0 MB).
[ INFO] [1518717355.131809796, 1518211231.020375523]: rtabmap: Database version = "0.15.0".
[ INFO] [1518717355.168619489, 1518211231.060995605]: /rtabmap/rtabmap: queue_size = 30
[ INFO] [1518717355.168666771, 1518211231.060995605]: /rtabmap/rtabmap: rgbd_cameras = 1
[ INFO] [1518717355.168692926, 1518211231.060995605]: /rtabmap/rtabmap: approx_sync = false
[ INFO] [1518717355.168787391, 1518211231.060995605]: Setup stereo callback
[ INFO] [1518717355.196848253, 1518211231.081112092]:
/rtabmap/rtabmap subscribed to (exact sync):
/bumblebee_xb3/stereo_camera_LC/left/image_rect_color,
/bumblebee_xb3/stereo_camera_LC/right/image_rect,
/bumblebee_xb3/stereo_camera_LC/left/camera_info,
/bumblebee_xb3/stereo_camera_LC/right/camera_info
[ INFO] [1518717355.204491631, 1518211231.091173573]: rtabmap 0.15.0 started...
[ INFO] [1518717355.662435891, 1518211231.546616933]: rtabmap (1): Rate=1.00s, Limit=0.700s, RTAB-Map=0.2230s, Maps update=0.0128s pub=0.0011s (local map=1, WM=1)
[ INFO] [1518717356.690791400, 1518211232.581388914]: rtabmap (2): Rate=1.00s, Limit=0.700s, RTAB-Map=0.2042s, Maps update=0.0024s pub=0.0001s (local map=1, WM=1)
[ INFO] [1518717357.764750164, 1518211233.654475075]: rtabmap (3): Rate=1.00s, Limit=0.700s, RTAB-Map=0.2193s, Maps update=0.0017s pub=0.0001s (local map=1, WM=1)
[ INFO] [1518717358.833155087, 1518211234.726006266]: rtabmap (4): Rate=1.00s, Limit=0.700s, RTAB-Map=0.2264s, Maps update=0.0019s pub=0.0001s (local map=1, WM=1)
[ INFO] [1518717359.898090531, 1518211235.787980768]: rtabmap (5): Rate=1.00s, Limit=0.700s, RTAB-Map=0.2236s, Maps update=0.0022s pub=0.0001s (local map=1, WM=1)
[ INFO] [1518717360.942895260, 1518211236.831347409]: rtabmap (6): Rate=1.00s, Limit=0.700s, RTAB-Map=0.2069s, Maps update=0.0017s pub=0.0001s (local map=1, WM=1)
rosnode outputNode [/rtabmap/rtabmapviz] Publications: * /rosout [rosgraph_msgs/Log] Subscriptions: * /clock [rosgraph_msgs/Clock] * /rtabmap/global_path [nav_msgs/Path] * /rtabmap/goal_node [unknown type] * /rtabmap/goal_reached [std_msgs/Bool] * /rtabmap/info [rtabmap_ros/Info] * /rtabmap/mapData [rtabmap_ros/MapData] * /rtabmap/odom [unknown type] * /tf [tf2_msgs/TFMessage] * /tf_static [unknown type] Services: * /rtabmap/rtabmapviz/get_loggers * /rtabmap/rtabmapviz/set_logger_level rostopic for odometry$ rostopic list /odometry /odometry/filtered /odometry/stereo /odometry/stereo_odom_info $ rostopic hz /odometry/stereo_odom_info WARNING: may be using simulated time average rate: 9.680 min: 0.083s max: 0.149s std dev: 0.02020s window: 9 average rate: 9.942 min: 0.083s max: 0.149s std dev: 0.01677s window: 15 |
|
Administrator
|
Hi,
Is this working?
<node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz_odom" args="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" output="screen" >
<param name="subscribe_stereo" type="bool" value="true" />
<param name="subscribe_odom_info" type="bool" value="true" />
<param name="approx_sync" type="bool" value="false" />
<param name="frame_id" type="string" value="base_footprint" />
<remap from="odom_info" to="/odometry/stereo_odom_info" />
<remap from="odom" to="/odometry/stereo" /> <!-- Use odometry topic published by stereo odometry, not the one after sensor fusion -->
<remap from="mapData" to="/not_used_map_data" />
<remap from="left/image_rect" to="/$(arg camera_namespace_name)/$(arg stereo_pair_name)/left/image_rect_color" />
<remap from="right/image_rect" to="/$(arg camera_namespace_name)/$(arg stereo_pair_name)/right/image_rect" />
<remap from="left/camera_info" to="/$(arg camera_namespace_name)/$(arg stereo_pair_name)/left/camera_info" />
<remap from="right/camera_info" to="/$(arg camera_namespace_name)/$(arg stereo_pair_name)/right/camera_info" />
</node>
You may use this configuration just for the Odometry view (not the 3D view or loop closure view). I removed mapData because it doesn't make sense to subscribe to it as odometry frame is not the same used for mapping. "subscribe_stereo" should be enabled otherwise "subscribe_odom_info" is ignored. To visualize the map, you would have to create another rtabmapviz subscribing to mapData:
<node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz_map" args="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" output="screen" >
<param name="frame_id" type="string" value="base_footprint" />
<param name="odom_frame_id" type="string" value="odom_filtered" />
<remap from="mapData" to="/rtabmap/mapData" />
</node>
Note that some info point cloud topics published by stereo odometry node can be also visualized in RVIZ. cheers, Mathieu |
«
Return to Official RTAB-Map Forum
|
1 view|%1 views
| Free forum by Nabble | Edit this page |