Re: 3D-map is so bad
Posted by matlabbe on
URL: http://official-rtab-map-forum.206.s1.nabble.com/3D-map-is-so-bad-tp8867p8878.html
Hi,
thx for the bag. The main issue is coming from the IMU, so the orientation estimation from /odometry/filtered is pretty bad. Here is an example showing velodyne points against odom frame:
I removed the odom TF frame from the rosbag using this script (with odom instead of map) and regenerate the odometry filtered with this robot_localization config (note that I recompute the quaternion from the IMU msg, not sure why it was so wrong or maybe it was not in ENU coordinates):
Here is the same trajectory as above during rotation, you can see that it is a lot more accurate already even without doing SLAM:
This is why I told you "Did you test every parts separately?" before trying rtabmap, as if you are feeding bad data to rtabmap, you will get a bad map.
For the mapping part, I updated demo_husky.launch to get better ICP with those real data with velodyne. Here is the final trajectory:
Commands used (with /odom frame filtered from lab_27jan_2022-01-27-14-21-13.bag and we remap the /odometry/filtered inside the bag to something not used, as our robot_localization.launch above will republish it on the same name):
cheers,
Mathieu
URL: http://official-rtab-map-forum.206.s1.nabble.com/3D-map-is-so-bad-tp8867p8878.html
Hi,
thx for the bag. The main issue is coming from the IMU, so the orientation estimation from /odometry/filtered is pretty bad. Here is an example showing velodyne points against odom frame:
I removed the odom TF frame from the rosbag using this script (with odom instead of map) and regenerate the odometry filtered with this robot_localization config (note that I recompute the quaternion from the IMU msg, not sure why it was so wrong or maybe it was not in ENU coordinates):
<!-- robot_localization.launch -->
<launch>
<node pkg="imu_filter_madgwick" type="imu_filter_node" name="imu_filter_node">
<param name="use_mag" value="false"/>
<param name="world_frame" value="enu"/>
<param name="publish_tf" value="true"/>
<remap from="imu/data_raw" to="/gx5/imu/data"/>
</node>
<!-- Odometry fusion (EKF), refer to demo launch file in robot_localization for more info -->
<node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization" clear_params="true" output="screen">
<param name="frequency" value="20"/>
<param name="sensor_timeout" value="0.1"/>
<param name="two_d_mode" value="true"/>
<param name="odom_frame" value="odom"/>
<param name="base_link_frame" value="base_link"/>
<param name="world_frame" value="odom"/>
<param name="transform_time_offset" value="0.0"/>
<param name="odom0" value="/warthog_velocity_controller/odom"/>
<param name="imu0" value="imu/data"/>
<!-- The order of the values is x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. -->
<rosparam param="odom0_config">[true, true, false,
false, false, false,
true, false, false,
false, false, false,
false, false, false]</rosparam>
<rosparam param="imu0_config">[
false, false, false,
false, false, true,
false, false, false,
false, false, false,
false, false, false] </rosparam>
<param name="odom0_differential" value="true"/>
<param name="imu0_differential" value="false"/>
<param name="odom0_relative" value="true"/>
<param name="imu0_relative" value="true"/>
<param name="imu0_remove_gravitational_acceleration" value="true"/>
<param name="print_diagnostics" value="true"/>
<!-- ======== ADVANCED PARAMETERS ======== -->
<param name="odom0_queue_size" value="5"/>
<param name="imu0_queue_size" value="5"/>
<!-- The values are ordered as x, y, z, roll, pitch, yaw, vx, vy, vz,
vroll, vpitch, vyaw, ax, ay, az. -->
<rosparam param="process_noise_covariance">[0.005, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0.005, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0.006, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0.003, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0.003, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0.006, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0.0025, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0.0025, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0.004, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0.001, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.001, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.002, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.001, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.001, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.0015]</rosparam>
<!-- The values are ordered as x, y,
z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. -->
<rosparam param="initial_estimate_covariance">[1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9]</rosparam>
</node>
</launch>
Here is the same trajectory as above during rotation, you can see that it is a lot more accurate already even without doing SLAM:
This is why I told you "Did you test every parts separately?" before trying rtabmap, as if you are feeding bad data to rtabmap, you will get a bad map.
For the mapping part, I updated demo_husky.launch to get better ICP with those real data with velodyne. Here is the final trajectory:
Commands used (with /odom frame filtered from lab_27jan_2022-01-27-14-21-13.bag and we remap the /odometry/filtered inside the bag to something not used, as our robot_localization.launch above will republish it on the same name):
roscore rosparam set use_sim_time true roslaunch rtabmap_ros demo_husky.launch lidar3d:=true roslaunch robot_localization.launch rosbag play --clock --pause lab_27jan_2022-01-27-14-21-13.bag /odometry/filtered:=not_used
cheers,
Mathieu
| Free forum by Nabble | Edit this page |