Stereo outdoor mapping on RELLIS
Locked
Stereo outdoor mapping on RELLIS
 
|
I am currently using RTABMAP stereo outdoor mapping on the RELLIS multi modal dataset. First, I had to correct the camera_info from left and right as I had been getting this error -
"The stereo baseline (0.000000) should be positive (baseline=-Tx/fx). We assume a horizontal left/right stereo setup where the Tx (or P(0,3)) is negative in the right camera info msg." So, after raising an issue with the RELLIS team, they provided a new set of stereo calibration parameters for which I wrote two nodes which would tap on the original topic but publish the stereo calibrated D,K,R,P parameters. Here are the relevant files - nerian_stereo_left.yaml nerian_stereo_right.yaml camera_left_publisher.py camera_right_publisher.py rellis_sim.launch rellis.rviz RELLIS Dataset These are a few screenshots of the issue- The misaligned map and base_link  the map frames   the base_link frames   the rqt_tf_tree for the setup tf_tree.png The stereo odometry seems fine as it moves appropriately with the robot motion but the map and base_link frame are not aligned. I am quite new to the rtabmap stereo setup but I have worked on it previously with d435i and hokuyo (4m) which ran very well. I see one issue which is the parent tf frame for the stereo, Should it be something like base_link->camera_link->nerian_left, nerian_right. I did put a static transform and followed the rtabmap stereo handheld mapping guidelines for the camera to produce image in world frame. I could however see no change. But please do let me know if i am wrong. Any help would be appreciated. Thanks, Prasheel |
Re: Stereo outdoor mapping on RELLIS
Administrator
|
This post was updated on .
Hi Prasheel,
Based on your launch file, I created a new one to test also with velodyne/ouster to compare with pure visual odometry. test.launch:
<?xml version="1.0"?>
<launch>
<arg name="icp_odom" default="false"/> <!-- stereo odom if false -->
<arg name="ouster" default="false"/> <!-- velodyne if false -->
<arg name="voxel_size" default="0.5"/> <!-- outdoor: between 0.2 and 0.5 -->
<param name="use_sim_time" type="bool" value="true"/>
<param name="nerian_stereo_left" textfile="/home/mathieu/.ros/camera_info/nerian_stereo_left.yaml" />
<param name="nerian_stereo_right" textfile="/home/mathieu/.ros/camera_info/nerian_stereo_right.yaml" />
<!-- Invert imu link: z should be up based on accelerometer data and quaternion of /imu/data -->
<node pkg="tf" type="static_transform_publisher" name="imu_link_fixed"
args="0 0 0 0 0 3.14159265359 imu_link imu_link_fixed 100" />
<node pkg="rtabmap_ros" type="odom_msg_to_tf" name="odom_msg_to_tf">
<remap from="odom" to="/vectornav/Odom"/>
<param name="odom_frame_id" value="world"/>
<param name="frame_id" value="base_link_gt"/>
</node>
<!-- Run the ROS package stereo_image_proc for image rectification -->
<group ns="/stereo_camera" >
<node pkg="stereo_image_proc" type="stereo_image_proc" name="stereo_image_proc">
<remap from="left/image_raw" to="/nerian/left/image_raw"/>
<remap from="left/camera_info" to="/nerian_left_info_new"/>
<remap from="right/image_raw" to="/nerian/right/image_raw"/>
<remap from="right/camera_info" to="/nerian_right_info_new"/>
<param name="disparity_range" value="128"/>
</node>
<node pkg="nodelet" type="nodelet" name="stereo_sync" args="standalone rtabmap_ros/stereo_sync">
<remap from="left/image_raw" to="/stereo_camera/left/image_rect"/>
<remap from="left/camera_info" to="/nerian_left_info_new"/>
<remap from="right/image_raw" to="/stereo_camera/right/image_rect"/>
<remap from="right/camera_info" to="/nerian_right_info_new"/>
</node>
</group>
<group ns="rtabmap">
<node if="$(arg icp_odom)" pkg="rtabmap_ros" type="icp_odometry" name="icp_odometry" output="screen">
<remap if="$(arg ouster)" from="scan_cloud" to="/os1_cloud_node/points"/>
<remap unless="$(arg ouster)" from="scan_cloud" to="/velodyne_points"/>
<param name="frame_id" type="string" value="base_link"/>
<param name="odom_frame_id" type="string" value="vo_odom"/>
<!-- param name="guess_frame_id" type="string" value="odom"/ --> <!-- Disabled, odom->base_link seems having yaw inverted! -->
<param name="wait_imu_to_init" type="bool" value="true"/>
<param unless="$(arg ouster)" name="scan_cloud_max_points" type="int" value="60000"/> <!-- velodyne max points per rotation -->
<param name="expected_update_rate" type="double" value="10"/>
<remap from="imu" to="/imu/data_fixed"/>
<!-- ICP parameters -->
<param name="Icp/PointToPlane" type="string" value="true"/>
<param name="Icp/Iterations" type="string" value="10"/>
<param name="Icp/VoxelSize" type="string" value="$(arg voxel_size)"/>
<param name="Icp/Epsilon" type="string" value="0.001"/>
<param name="Icp/PointToPlaneK" type="string" value="20"/>
<param name="Icp/PointToPlaneRadius" type="string" value="0"/>
<param name="Icp/MaxTranslation" type="string" value="2"/>
<param name="Icp/MaxCorrespondenceDistance" type="string" value="1"/>
<param name="Icp/PM" type="string" value="true"/>
<param name="Icp/PMOutlierRatio" type="string" value="0.1"/>
<param name="Icp/CorrespondenceRatio" type="string" value="0.01"/>
<!-- Odom parameters -->
<param name="Odom/ScanKeyFrameThr" type="string" value="0.7"/>
<param name="Odom/Strategy" type="string" value="0"/>
<param name="OdomF2M/ScanSubtractRadius" type="string" value="$(arg voxel_size)"/>
<param name="OdomF2M/ScanMaxSize" type="string" value="15000"/>
</node>
<node unless="$(arg icp_odom)" pkg="rtabmap_ros" type="stereo_odometry" name="stereo_odometry" output="screen">
<remap from="rgbd_image" to="/stereo_camera/rgbd_image"/>
<param name="subscribe_rgbd" type="bool" value="true"/>
<param name="frame_id" type="string" value="base_link"/>
<param name="odom_frame_id" type="string" value="vo_odom"/>
<!-- param name="guess_frame_id" type="string" value="odom"/ --> <!-- Disabled, odom->base_link seems having yaw inverted! -->
<param name="wait_imu_to_init" type="bool" value="true"/>
<remap from="imu" to="/imu/data_fixed"/>
<param name="GFTT/MinDistance" type="string" value="10"/>
<param name="GFTT/QualityLevel" type="string" value="0.00001"/>
</node>
<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
<param name="frame_id" type="string" value="base_link"/>
<param name="subscribe_rgbd" type="bool" value="true"/>
<param name="subscribe_scan_cloud" type="bool" value="true"/>
<param name="approx_sync" type="bool" value="true"/>
<remap from="rgbd_image" to="/stereo_camera/rgbd_image"/>
<remap from="imu" to="/imu/data_fixed"/>
<remap from="gps/fix" to="/vectornav/GPS"/>
<remap if="$(arg icp_odom)" from="scan_cloud" to="odom_filtered_input_scan"/> <!-- Get already filtered scan -->
<remap if="$(eval not icp_odom and ouster)" from="scan_cloud" to="/os1_cloud_node/points"/>
<remap if="$(eval not icp_odom and not ouster)" from="scan_cloud" to="/velodyne_points"/>
<!-- RTAB-Map's parameters -->
<param if="$(arg icp_odom)" name="RGBD/NeighborLinkRefining" type="string" value="false"/>
<param unless="$(arg icp_odom)" name="RGBD/NeighborLinkRefining" type="string" value="true"/>
<param name="RGBD/ProximityBySpace" type="string" value="true"/>
<param name="RGBD/ProximityMaxGraphDepth" type="string" value="0"/>
<param name="RGBD/ProximityPathMaxNeighbors" type="string" value="1"/>
<param name="RGBD/LocalRadius" type="string" value="2"/>
<param name="Mem/STMSize" type="string" value="30"/>
<param unless="$(arg icp_odom)" name="Mem/LaserScanVoxelSize" type="string" value="$(arg voxel_size)"/>
<param unless="$(arg icp_odom)" name="Mem/LaserScanNormalK" type="string" value="20"/>
<param unless="$(arg icp_odom)" name="Mem/LaserScanRadius" type="string" value="0"/>
<param name="Reg/Strategy" type="string" value="1"/>
<param name="Grid/CellSize" type="string" value="$(arg voxel_size)"/>
<param name="Grid/RangeMax" type="string" value="20"/>
<param name="Grid/ClusterRadius" type="string" value="1"/>
<param name="Grid/GroundIsObstacle" type="string" value="false"/>
<!-- ICP parameters -->
<param name="Icp/VoxelSize" type="string" value="0"/>
<param name="Icp/PointToPlaneK" type="string" value="20"/>
<param name="Icp/PointToPlaneRadius" type="string" value="0"/>
<param name="Icp/PointToPlane" type="string" value="true"/>
<param name="Icp/Iterations" type="string" value="10"/>
<param name="Icp/Epsilon" type="string" value="0.001"/>
<param name="Icp/MaxTranslation" type="string" value="3"/>
<param name="Icp/MaxCorrespondenceDistance" type="string" value="1"/>
<param name="Icp/PM" type="string" value="true"/>
<param name="Icp/PMOutlierRatio" type="string" value="0.7"/>
<param name="Icp/CorrespondenceRatio" type="string" value="0.2"/>
</node>
<node pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" output="screen" launch-prefix="">
<param name="subscribe_odom_info" type="bool" value="true"/>
<param if="$(arg icp_odom)" name="subscribe_scan_cloud" type="bool" value="true"/>
<param unless="$(arg icp_odom)" name="subscribe_rgbd" type="bool" value="true"/>
<param name="approx_sync" type="bool" value="false"/>
<param name="frame_id" type="string" value="base_link"/>
<remap from="rgbd_image" to="/stereo_camera/rgbd_image"/>
<remap from="scan_cloud" to="odom_filtered_input_scan"/>
</node>
</group>
</launch>
repub_imu.py:
#!/usr/bin/env python
import rospy
from sensor_msgs.msg import Imu
def callback(data):
global pub
data.header.frame_id = "imu_link_fixed"
pub.publish(data)
def listener():
global pub
rospy.init_node('repub', anonymous=True)
pub = rospy.Publisher('/imu/data_fixed', Imu, queue_size=10)
rospy.Subscriber("/imu/data", Imu, callback)
rospy.spin()
if __name__ == '__main__':
listener()
Usage: roslaunch test.launch python camera_left_publisher.py python camera_right_publisher.py python repub_imu.py rosbag play --clock 20200213_trail_2.bag We have 2 main parameters for this launch file, if icp_odom=true, icp_odometry using lidar is done. If icp_odom=false, stereo_odometry is done. If ouster=true, the ouster is used as the lidar, otherwise, the velodyne is used. With icp_odom=true, icp_odometry is done using IMU for rotation guess. With icp_odom=false, visual odometry poses are refined with the lidar, this seems to give the better accuracy (rtabmap should be built with libpointmatcher!). Issues * If rtabmapviz crashes at some point (not sure why yet, see log at the end of this post), add to its launch-prefix="xterm -e gdb -q -ex run --args". * I noticed that /imu/data has acceleration z positive, but its imu_link frame is inverted. I added a new link imu_link_fixed with z->up, then I republish the imu data by changing its frame_id to fixed one. Currently I cannot debug if a yaw correction is required for that frame. * I also noticed that /odometry/filtered has yaw inverted, so it is not reliable as guess for visual or icp odometry, so it is ignored. Ideally, odom tf would have to be removed from the bag to avoid TF unconnected tree conflicts if visualized with rviz. * I have some doubts about the accuracy of the stereo calibration / rectification, and also for the time synchronization between the cameras. With better calibration, stereo_odometry would have perform a lot better. * At the beginning, with stereo_odometry, it may get lost on start because of a lag/noise in images received. Just do Edit->Delete Memory and Detection->Reset Odometry. * Note sure why, but using ouster point cloud instead of velodyne point cloud with icp_odom=true results in worst performance. ICP may need to be tuned more for ouster point cloud. See some results below, when opening the database in rtabmap-databaseViewer, in Graph View, we can compare slam poses (blue) with GPS (green), also RMSE is shown. Visual odometry-only without lidar refining (with re-scaling factor 1.069 to account baseline accuracy):  Visual odometry with velodyne lidar refining:    rtabmapviz error log:
(gdb) bt
#0 0x00007fffe7d04ea0 in () at /lib/x86_64-linux-gnu/libopencv_calib3d.so.4.2
#1 0x00007fffe7d0518a in () at /lib/x86_64-linux-gnu/libopencv_calib3d.so.4.2
#2 0x00007ffff65dfb22 in cv::parallel_for_(cv::Range const&, cv::ParallelLoopBody const&, double) ()
at /lib/x86_64-linux-gnu/libopencv_core.so.4.2
#3 0x00007fffe7d0dc59 in () at /lib/x86_64-linux-gnu/libopencv_calib3d.so.4.2
#4 0x00007ffff58f576f in rtabmap::StereoBM::computeDisparity(cv::Mat const&, cv::Mat const&) const ()
at /home/mathieu/catkin_ws/devel/lib/librtabmap_core.so.0.20
#5 0x00007ffff5665d8a in rtabmap::util2d::disparityFromStereoImages(cv::Mat const&, cv::Mat const&, std::map<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::less<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >, std::allocator<std::pair<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > > > > const&) ()
at /home/mathieu/catkin_ws/devel/lib/librtabmap_core.so.0.20
#6 0x00007ffff567f620 in rtabmap::util3d::cloudFromStereoImages(cv::Mat const&, cv::Mat const&, rtabmap::StereoCameraModel const&, int, float, float, std::vector<int, std::allocator<int> >*, std::map<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::less<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >, std::allocator<std::pair<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > > > > const&) ()
at /home/mathieu/catkin_ws/devel/lib/librtabmap_core.so.0.20
#7 0x00007ffff568576d in rtabmap::util3d::cloudRGBFromSensorData(rtabmap::SensorData const&, int, float, float, std::vector<int, std::allocator<int> >*, std::map<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::less<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >, std::allocator<std::pair<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > > > > const&, std::vector<float, std::allocator<float> > const&) ()
at /home/mathieu/catkin_ws/devel/lib/librtabmap_core.so.0.20
#8 0x00007ffff4c2d8fe in rtabmap::MainWindow::processOdometry(rtabmap::OdometryEvent const&, bool) ()
at /home/mathieu/catkin_ws/devel/lib/librtabmap_gui.so.0.20
#9 0x00007ffff4f4238b in rtabmap::MainWindow::qt_static_metacall(QObject*, QMetaObject::Call, int, void**) ()
at /home/mathieu/catkin_ws/devel/lib/librtabmap_gui.so.0.20
#10 0x00007ffff41c1d5a in QObject::event(QEvent*) () at /lib/x86_64-linux-gnu/libQt5Core.so.5
#11 0x00007ffff460525d in QWidget::event(QEvent*) () at /lib/x86_64-linux-gnu/libQt5Widgets.so.5
#12 0x00007ffff471a148 in QMainWindow::event(QEvent*) () at /lib/x86_64-linux-gnu/libQt5Widgets.so.5
cheers, Mathieu |
| Free forum by Nabble | Edit this page |