Open bj-neilson opened 5 years ago
matlabbe
commented
5 years ago Hi, if you disable the scan (subscribe_scan=false), is it better? If you have again a V shape, it would be more related to zed odometry.
You have written "RGBD/OptimisemaxError" instead of "RGBD/OptimizeMaxError" in the launch file. I don't really recommend to set it below 3, otherwise a lot of good loop closures may be rejected.
cheers, Mathieu
bj-neilson
commented
5 years ago Hi Mathieu, Thanks for the help...I found the spelling error. That helped a bit! I'll set that back to 3.0 and try. Will also try the 2 odometry strategies separately and try to determine why I'm getting a V shape. The weekend got in the way of some testing. Cheers, Ben
JoeyLeeNPU
commented
4 years ago I want to ask whether your lidar is working better in this launch file? I want to refer this to write my launch file.
bj-neilson
commented
4 years ago Hi @JoeyLeeNPU, My odometer is not really working better from the lidar. Maybe the RPLidarA2 is not quite providing the best solution in my environment. I'm spending more time trying to improve the camera setup for better visual odometry. Not quite solved yet.
JoeyLeeNPU
commented
4 years ago I feed hokuyo 2d laser into rtabmap, maybe also not work well. I think if I provide both laser and camera for odometry, the rtabmap will be better? Because I read paper of rtabmap and find this maybe a good solution for uav slam.
JoeyLeeNPU
commented
4 years ago @bj-neilson Could we discuss more about this? In email or other software. Need for your help.
Hi Matthieu,
I have RTABMap Ros running reasonably well. I am trying to map outdoors in an orchard of tree rows. I find when I turn at the end of a tree row the robot never sees a 180deg turn. Maybe 150deg or so. So there is a V shaped 2D map rather than a map of both sides of a tree row.
I am using the Zed Odom and ICP with the RPLidarA2. I have been trying to tune by changing parameters in .launch file. Some parameters change, some do not. I'm a bit confused. For example I was unable to change the value of "RGBD/OptimizeMaxError" is 3.000000. I tried in launch file to change to 0.1, but it still runs at 3.00000.
Kindly see below...
ubuntu@ubuntu-tx2:~/catkin_ws$ roslaunch zed_rtabmap_example mower.launch ... logging to /home/ubuntu/.ros/log/1f753e18-cf87-11e9-85fb-00044ba839fa/roslaunch-ubuntu-tx2-27728.log Checking log directory for disk usage. This may take awhile. Press Ctrl-C to interrupt Done checking log file disk usage. Usage is <1GB.
started roslaunch server http://ubuntu-tx2:39513/
SUMMARY
PARAMETERS
NODES /rtabmap/ rgbd_sync (nodelet/nodelet) rtabmap (rtabmap_ros/rtabmap) / move_base (move_base/move_base) rplidarNode (rplidar_ros/rplidarNode) /zed/ zed_node (zed_wrapper/zed_wrapper_node)
ROS_MASTER_URI=http://localhost:11311
process[zed/zed_node-1]: started with pid [27744] process[rplidarNode-2]: started with pid [27745] process[move_base-3]: started with pid [27746] [ INFO] [1567654901.718420226]: RPLIDAR running on ROS package rplidar_ros. SDK Version:1.10.0 process[rtabmap/rgbd_sync-4]: started with pid [27752] type is rtabmap_ros/rgbd_sync process[rtabmap/rtabmap-5]: started with pid [27760] [ INFO] [1567654901.814693872]: Initializing nodelet with 6 worker threads. [ INFO] [1567654901.863445226]: SDK version : 2.8.2 [ INFO] [1567654901.863539404]: PARAMETERS [ INFO] [1567654901.864414553]: Camera Resolution -> VGA [ INFO] [1567654901.865100416]: Camera Framerate -> 30 [ INFO] [1567654901.866361427]: Gpu ID -> -1 [ INFO] [1567654901.867561398]: Camera ID -> -1 [ INFO] [1567654901.871805591]: Verbose -> ENABLED [ INFO] [1567654901.875756173]: Camera Flip -> DISABLED [ INFO] [1567654901.878741860]: Self calibration -> ENABLED [ INFO] [1567654901.881984905]: Camera Model -> zed [ INFO] [1567654901.907235828]: Depth quality -> PERFORMANCE [ INFO] [1567654901.908593971]: Depth Sensing mode -> STANDARD [ INFO] [1567654901.910495956]: OpenNI mode -> DISABLED [ INFO] [1567654901.911879080]: Depth Stabilization -> ENABLED [ INFO] [1567654901.913311750]: Minimum depth -> 0.3 [ INFO] [1567654901.919893913]: Path rate -> 1 Hz [ INFO] [1567654901.922143003]: Path history size -> 1 [ INFO] [1567654901.925862973]: Odometry DB path -> [ INFO] [1567654901.928908962]: Spatial Memory -> ENABLED [ INFO] [1567654901.930046640]: IMU Fusion -> ENABLED [ INFO] [1567654901.932068201]: Floor alignment -> ENABLED [ INFO] [1567654901.933689451]: Init Odometry with first valid pose data -> ENABLED [ INFO] [1567654901.935381117]: Two D mode -> ENABLED [ INFO] [1567654901.936956675]: Fixed Z value -> 1 [ INFO] [1567654901.937836608]: Publish Pose Covariance -> ENABLED [ INFO] [1567654901.938828187]: Fixed covariance -> DISABLED [ INFO] [1567654901.940062740]: Fixed cov. value -> 1e-06 [ INFO] [1567654901.941869198]: Mapping -> ENABLED [ INFO] [1567654901.943079220]: Mapping resolution -> LOW [ INFO] [1567654901.944007671]: Fused point cloud freq: -> 1 Hz [ INFO] [1567654901.945454918]: IMU timestamp sync -> DISABLED [ INFO] [1567654901.946239583]: IMU data freq -> 0 Hz [ INFO] [1567654901.948365468]: SVO REC compression -> HEVC (H265) [ INFO] [1567654901.965077014]: world_frame -> map [ INFO] [1567654901.965173729]: map_frame -> map [ INFO] [1567654901.965234801]: odometry_frame -> odom [ INFO] [1567654901.965280911]: base_frame -> base_link [ INFO] [1567654901.965323809]: camera_frame -> zed_camera_center [ INFO] [1567654901.965364942]: imu_link -> imu_link [ INFO] [1567654901.965404629]: left_camera_frame -> zed_left_camera_frame [ INFO] [1567654901.965445890]: left_camera_optical_frame -> zed_left_camera_optical_frame [ INFO] [1567654901.965487537]: right_camera_frame -> zed_right_camera_frame [ INFO] [1567654901.965528573]: right_camera_optical_frame -> zed_right_camera_optical_frame [ INFO] [1567654901.965581265]: depth_frame -> zed_left_camera_frame [ INFO] [1567654901.965621691]: depth_optical_frame -> zed_left_camera_optical_frame [ INFO] [1567654901.965660255]: disparity_frame -> zed_left_camera_frame [ INFO] [1567654901.965695800]: disparity_optical_frame -> zed_left_camera_optical_frame [ INFO] [1567654901.965734396]: confidence_frame -> zed_left_camera_frame [ INFO] [1567654901.965787505]: confidence_optical_frame -> zed_left_camera_optical_frame [ INFO] [1567654901.967580959]: Broadcast odometry TF -> ENABLED [ INFO] [1567654901.969193347]: Broadcast map pose TF -> DISABLED [ INFO] [1567654901.969993038]: [DYN] mat_resize_factor -> 1 [ INFO] [1567654901.970994186]: [DYN] confidence -> 100 [ INFO] [1567654901.971863266]: [DYN] max_depth -> 15 [ INFO] [1567654901.972897808]: [DYN] exposure -> 100 [ INFO] [1567654901.973993615]: [DYN] gain -> 100 [ INFO] [1567654901.974801912]: [DYN] auto_exposure -> ENABLED [ INFO] [1567654901.976166798]: [DYN] point_cloud_freq -> 10 Hz [ INFO] [1567654902.006898817]: Camera coordinate system -> COORDINATE_SYSTEM_RIGHT_HANDED_Z_UP_X_FWD ZED (Init) >> Depth mode: PERFORMANCE RPLIDAR S/N: DB9D9AF0C5E29DD2B6E39DF5584E3116 [ INFO] [1567654902.229200073]: Firmware Ver: 1.24 [ INFO] [1567654902.229278970]: Hardware Rev: 5 [ INFO] [1567654902.231029561]: RPLidar health status : 0 ZED (Init) >> Video mode: VGA@30 [ INFO] [1567654902.779679175]: Starting node... [ INFO] [1567654902.900671120]: current scan mode: Boost, max_distance: 12.0 m, Point number: 8.0K , angle_compensate: 2 [ INFO] [1567654903.001170694]: Initializing nodelet with 6 worker threads. [ INFO] [1567654903.130001664]: ZED connection -> SUCCESS [ INFO] [1567654903.716128324]: /rtabmap/rgbd_sync: approx_sync = false [ INFO] [1567654903.718409793]: /rtabmap/rgbd_sync: queue_size = 10 [ INFO] [1567654903.718486739]: /rtabmap/rgbd_sync: depth_scale = 1.000000 [ INFO] [1567654903.718560509]: /rtabmap/rgbd_sync: compressed_rate = 0.000000 [ INFO] [1567654903.726575829]: /rtabmap/rtabmap(maps): map_filter_radius = 0.000000 [ INFO] [1567654903.726695164]: /rtabmap/rtabmap(maps): map_filter_angle = 30.000000 [ INFO] [1567654903.726760334]: /rtabmap/rtabmap(maps): map_cleanup = true [ INFO] [1567654903.726812380]: /rtabmap/rtabmap(maps): map_always_update = false [ INFO] [1567654903.726903413]: /rtabmap/rtabmap(maps): map_empty_ray_tracing = true [ INFO] [1567654903.726969129]: /rtabmap/rtabmap(maps): cloud_output_voxelized = true [ INFO] [1567654903.727021143]: /rtabmap/rtabmap(maps): cloud_subtract_filtering = false [ INFO] [1567654903.727083778]: /rtabmap/rtabmap(maps): cloud_subtract_filtering_min_neighbors = 2 [ INFO] [1567654903.745205480]: /rtabmap/rtabmap(maps): octomap_tree_depth = 16 [ INFO] [1567654903.786466825]: /rtabmap/rgbd_sync subscribed to (exact sync): /zed/zed_node/rgb/image_rect_color, /zed/zed_node/depth/depth_registered, /zed/zed_node/rgb/camera_info [ INFO] [1567654903.805145923]: rtabmap: frame_id = base_link [ INFO] [1567654903.805261471]: rtabmap: map_frame_id = map [ INFO] [1567654903.805338032]: rtabmap: use_action_for_goal = false [ INFO] [1567654903.805383725]: rtabmap: tf_delay = 0.050000 [ INFO] [1567654903.805462853]: rtabmap: tf_tolerance = 0.100000 [ INFO] [1567654903.805521606]: rtabmap: odom_sensor_sync = false [ INFO] [1567654903.951507138]: Setting RTAB-Map parameter "Grid/FromDepth"="false" [ INFO] [1567654904.075637014]: Setting RTAB-Map parameter "Icp/MaxCorrespondenceDistance"="0.1" [ INFO] [1567654904.121805724]: Setting RTAB-Map parameter "Icp/VoxelSize"="0.05" [ INFO] [1567654904.374274044]: Setting RTAB-Map parameter "RGBD/AngularUpdate"="0.01" [ INFO] [1567654904.379750080]: Setting RTAB-Map parameter "RGBD/Enabled"="true" [ INFO] [1567654904.390788926]: Setting RTAB-Map parameter "RGBD/LinearUpdate"="0.01" [ INFO] [1567654904.420213230]: Setting RTAB-Map parameter "RGBD/NeighborLinkRefining"="true" [ INFO] [1567654904.425196472]: Setting RTAB-Map parameter "RGBD/OptimizeFromGraphEnd"="true" [ INFO] [1567654904.446006604]: Setting RTAB-Map parameter "RGBD/ProximityBySpace"="true" [ INFO] [1567654904.463143364]: Setting RTAB-Map parameter "RGBD/ProximityPathMaxNeighbors"="15" [ INFO] [1567654904.481220354]: Setting RTAB-Map parameter "Reg/Force3DoF"="true" [ INFO] [1567654904.485784006]: Setting RTAB-Map parameter "Reg/Strategy"="1" [ INFO] [1567654905.130357703]: CAMERA MODEL -> ZED [ INFO] [1567654905.130447854]: Serial Number -> 21955 [ INFO] [1567654905.130506736]: FW Version -> 1523 [ INFO] [1567654905.212185321]: Setting "Grid/RangeMax" parameter to 0 (default 5.000000) as "subscribe_scan" or "subscribe_scan_cloud" is true. [ INFO] [1567654905.576752958]: Advertised on topic /zed/zed_node/rgb/image_rect_color [ INFO] [1567654905.576878419]: Advertised on topic /zed/zed_node/rgb/camera_info [ INFO] [1567654905.722184027]: Advertised on topic /zed/zed_node/rgb_raw/image_raw_color [ INFO] [1567654905.722334311]: Advertised on topic /zed/zed_node/rgb_raw/camera_info [ INFO] [1567654905.859024934]: Advertised on topic /zed/zed_node/left/image_rect_color [ INFO] [1567654905.859302700]: Advertised on topic /zed/zed_node/left/camera_info [ INFO] [1567654905.949738087]: RTAB-Map detection rate = 1.000000 Hz [ INFO] [1567654905.965721556]: Advertised on topic /zed/zed_node/left_raw/image_raw_color [ INFO] [1567654905.965842239]: Advertised on topic /zed/zed_node/left_raw/camera_info [ INFO] [1567654906.011855380]: rtabmap: Deleted database "/home/ubuntu/.ros/rtabmap.db" (--delete_db_on_start or -d are set). [ INFO] [1567654906.011990819]: rtabmap: Using database from "/home/ubuntu/.ros/rtabmap.db" (0 MB). [ INFO] [1567654906.040699717]: Advertised on topic /zed/zed_node/right/image_rect_color [ INFO] [1567654906.040820182]: Advertised on topic /zed/zed_node/right/camera_info [ INFO] [1567654906.109650632]: Advertised on topic /zed/zed_node/right_raw/image_raw_color [ INFO] [1567654906.109765485]: Advertised on topic /zed/zed_node/right_raw/camera_info [ INFO] [1567654906.180305141]: Advertised on topic /zed/zed_node/depth/depth_registered [ INFO] [1567654906.180426920]: Advertised on topic /zed/zed_node/depth/camera_info [ INFO] [1567654906.192792453]: rtabmap: Database version = "0.19.3". [ INFO] [1567654906.299623866]: Advertised on topic /zed/zed_node/confidence/confidence_image [ INFO] [1567654906.299715088]: Advertised on topic /zed/zed_node/confidence/camera_info [ INFO] [1567654906.302005007]: /rtabmap/rtabmap: subscribe_depth = false [ INFO] [1567654906.302086577]: /rtabmap/rtabmap: subscribe_rgb = false [ INFO] [1567654906.302128902]: /rtabmap/rtabmap: subscribe_stereo = false [ INFO] [1567654906.302199507]: /rtabmap/rtabmap: subscribe_rgbd = true (rgbd_cameras=1) [ INFO] [1567654906.302378905]: /rtabmap/rtabmap: subscribe_odom_info = false [ INFO] [1567654906.302425943]: /rtabmap/rtabmap: subscribe_user_data = false [ INFO] [1567654906.302482536]: /rtabmap/rtabmap: subscribe_scan = true [ INFO] [1567654906.302559202]: /rtabmap/rtabmap: subscribe_scan_cloud = false [ INFO] [1567654906.302601590]: /rtabmap/rtabmap: queue_size = 15 [ INFO] [1567654906.302639458]: /rtabmap/rtabmap: approx_sync = true [ INFO] [1567654906.302832099]: Setup rgbd callback [ INFO] [1567654906.351183145]: /rtabmap/rtabmap subscribed to (approx sync): /zed/zed_node/odom, /rtabmap/rgbd_image, /scan [ INFO] [1567654906.395656685]: rtabmap 0.19.3 started... [ INFO] [1567654906.402451185]: Advertised on topic /zed/zed_node/stereo/image_rect_color [ INFO] [1567654906.461172610]: Advertised on topic /zed/zed_node/stereo_raw/image_raw_color [ INFO] [1567654906.465306724]: Advertised on topic /zed/zed_node/confidence/confidence_map [ INFO] [1567654906.467755616]: Advertised on topic /zed/zed_node/disparity/disparity_image [ INFO] [1567654906.470657188]: Advertised on topic /zed/zed_node/point_cloud/cloud_registered [ INFO] [1567654906.473147250]: Advertised on topic /zed/zed_node/point_cloud/fused_cloud_registered @ 1 Hz [ INFO] [1567654906.475627821]: Advertised on topic /zed/zed_node/pose [ INFO] [1567654906.478190411]: Advertised on topic /zed/zed_node/pose_with_covariance [ INFO] [1567654906.480587103]: Advertised on topic /zed/zed_node/odom [ INFO] [1567654906.482942592]: Advertised on topic /zed/zed_node/path_odom [ INFO] [1567654906.484846876]: Advertised on topic /zed/zed_node/path_map [ INFO] [1567654906.508755369]: Starting Positional Tracking [ INFO] [1567654906.508905076]: Waiting for valid static transformations... [ INFO] [1567654906.509090663]: Static transform Sensor to Base [zed_left_camera_frame -> base_link] [ INFO] [1567654906.509169573]: Translation: {-1.150,-0.060,-0.440} [ INFO] [1567654906.509251112]: Rotation: {0.000,-0.000,0.000} [ INFO] [1567654906.509376482]: Static transform Sensor to Camera Center [zed_left_camera_frame -> zed_camera_center] [ INFO] [1567654906.509467320]: Translation: {0.000,-0.060,0.000} [ INFO] [1567654906.509535071]: Rotation: {0.000,-0.000,0.000} [ INFO] [1567654906.509613307]: Static transform Camera Center to Base [zed_camera_center -> base_link] [ INFO] [1567654906.509686477]: Translation: {-1.150,0.000,-0.440} [ INFO] [1567654906.509749323]: Rotation: {0.000,-0.000,0.000} [ INFO] [1567654906.610042396]: Initial ZED left camera pose (ZED pos. tracking): [ INFO] [1567654906.610282716]: T: [1.15,0.06,0.44] [ INFO] [1567654906.610411293]: * Q: [0,0,0,1] [ INFO] [1567654906.689016019]: Starting Spatial Mapping [ WARN] [1567654906.954939204]: Timed out waiting for transform from base_link to map to become available before running costmap, tf error: canTransform: target_frame map does not exist.. canTransform returned after 0.100905 timeout was 0.1. [ INFO] [1567654907.483445577]: * Resolution: 0.08 m [ WARN] [1567654907.947024201]: Odometry will be published as soon as the floor as been detected for the first time [ INFO] [1567654908.002416812]: Odometry aligned to last tracking pose
[ INFO] [1567654908.409882774]: Using plugin "obstacle_layer" [ INFO] [1567654908.421237249]: Subscribed to Topics: laser_scan_sensor [ INFO] [1567654908.466532870]: Using plugin "inflation_layer" [ INFO] [1567654908.603204736]: Using plugin "obstacle_layer" [ INFO] [1567654908.606719837]: Subscribed to Topics: laser_scan_sensor [ INFO] [1567654908.659021299]: Using plugin "inflation_layer" [ INFO] [1567654908.775112808]: Created local_planner base_local_planner/TrajectoryPlannerROS [ INFO] [1567654908.797625756]: Sim period is set to 0.10
[ INFO] [1567654909.775172468]: Recovery behavior will clear layer obstacles [ INFO] [1567654909.786089587]: Recovery behavior will clear layer obstacles
[ WARN] (2019-09-05 13:42:35.089) OptimizerG2O.cpp:1061::optimize() Computing marginals: vertex 46 has negative hessian index (-1). Cannot compute last pose covariance.
[ WARN] (2019-09-05 13:42:41.235) OptimizerG2O.cpp:1061::optimize() Computing marginals: vertex 52 has negative hessian index (-1). Cannot compute last pose covariance.
[ WARN] (2019-09-05 13:42:42.216) OptimizerG2O.cpp:1061::optimize() Computing marginals: vertex 53 has negative hessian index (-1). Cannot compute last pose covariance. [ WARN] (2019-09-05 13:42:42.216) Rtabmap.cpp:2888::process() Rejecting all added loop closures (1, first is 53 <-> 34) in this iteration because a wrong loop closure has been detected after graph optimization, resulting in a maximum graph error ratio of 3.093660 (edge 1->46, type=1, abs error=0.053685 m, stddev=0.017353). The maximum error ratio parameter "RGBD/OptimizeMaxError" is 3.000000 of std deviation. [ WARN] (2019-09-05 13:42:42.216) Rtabmap.cpp:2922::process() Loop closure 53->34 rejected!
[ WARN] (2019-09-05 13:42:43.246) OptimizerG2O.cpp:1061::optimize() Computing marginals: vertex 54 has negative hessian index (-1). Cannot compute last pose covariance. [ WARN] (2019-09-05 13:42:43.246) Rtabmap.cpp:2888::process() Rejecting all added loop closures (1, first is 54 <-> 35) in this iteration because a wrong loop closure has been detected after graph optimization, resulting in a maximum graph error ratio of 10.129725 (edge 35->54, type=1, abs error=0.159222 m, stddev=0.015718). The maximum error ratio parameter "RGBD/OptimizeMaxError" is 3.000000 of std deviation. [ WARN] (2019-09-05 13:42:43.246) Rtabmap.cpp:2922::process() Loop closure 54->35 rejected!
[ WARN] (2019-09-05 13:42:44.401) OptimizerG2O.cpp:1061::optimize() Computing marginals: vertex 38 has negative hessian index (-1). Cannot compute last pose covariance. [ WARN] (2019-09-05 13:42:44.464) OptimizerG2O.cpp:1061::optimize() Computing marginals: vertex 55 has negative hessian index (-1). Cannot compute last pose covariance.
Sorry for the big cut and paste. I'd appreciate your thoughts on anything from the launch file/parameters. Then I'll try again outside and record some fresh data to show.
Thank you,
Ben