Closed willdzeng closed 8 years ago
Hi Di,
The warning is coming from the points_xyzrgb
plugin. To avoid this warning, you can set this instead in that nodelet (disabling voxel filtering):
<param name="decimation" type="double" value="4"/>
<param name="voxel_size" type="double" value="0.0"/>
The following command gives the same results as your modified launch file:
$ roslaunch rtabmap_ros stereo_mapping.launch stereo_namespace:=/camera right_image_topic:=/camera/right/image_rect_color frame_id:=camera_link rtabmap_args:="--Vis/CorFlowMaxLevel 5 --Stereo/MaxDisparity 200 rviz:=true"
However, even if the camera doesn't move, sometime the odometry gets lost (inliers drop from 300 to under 20):
[ INFO] [1466090354.718997697]: Odom: quality=315, std dev=0.010495m, update time=0.073979s
[ INFO] [1466090354.796440017]: Odom: quality=319, std dev=0.013374m, update time=0.073643s
[ WARN] (2016-06-16 11:19:14.866) OdometryF2M.cpp:227::computeTransform() Registration failed: "Not enough inliers 13/20 (matches=339) between -1 and 0"
[ INFO] [1466090354.867368030]: Odom: quality=13, std dev=0.000000m, update time=0.068204s
[ WARN] (2016-06-16 11:19:14.934) OdometryF2M.cpp:227::computeTransform() Registration failed: "Not enough inliers 10/20 (matches=342) between -1 and 0"
[ INFO] [1466090354.935537317]: Odom: quality=10, std dev=0.000000m, update time=0.065454s
[ INFO] [1466090355.003787475]: Odom: quality=306, std dev=0.010724m, update time=0.065558s
This problem comes from cv::solvePnPRansac, maybe because of a not so good calibration or that RANSAC failed to find good correspondences for transformation estimation. You can try with --Vis/EstimationType 0
to use 3D->3D motion estimation instead of PnP.
As you are using the Zed camera, they provide already a depth image that can be used (so no need for rtabmap to re-compute stereo correspondences). So like in the RGB-D tutorial, we can do:
$ roslaunch rtabmap_ros rgbd_mapping.launch depth_registered_topic:=/camera/depth/image_rect_color rviz:=true
The odometry is far more stable with more inliers.
cheers, Mathieu
Hi Mathieu,
Thanks.
I found out I increase the nodelet decimation from 2 to 4 it works without change the voxel_size.
<param name="decimation" type="double" value="4"/>
This fixed the issue.
Is this because the desktop I am using has more computation power so that it needs more image to process at a time? because I never had this problem on my laptop.
The warning seems related to a large point cloud to voxelize: https://github.com/PointCloudLibrary/pcl/issues/585
I think setting max_depth
would also fix that problem (the point cloud area will be bounded). With stereo, sometimes disparity for a pixel can be very small (~0), thus generating very long distance (~inf).
<param name="max_depth" type="double" value="15.0"/>
Hi Mathieu,
This is Di from Transcend Robotics.
I am successfully able to launch your code and run the hand held mapping with a ZED camera. But It keep giving me this warning, is this a problem of PCL libary? should I update the PCL from source?
My system is Ubuntu 14.04. ROS Jade is installed. Build from source. Here is my launch file.