How rtabmap completes visual odometry and imu fusion ？
Locked
How rtabmap completes visual odometry and imu fusion ？
 
|
This post was updated on .
Dear people:
I want to use two D435i + a 2d lidar + imu to achieve slam, but the visual odometry and imu become very bad after using ekf_localization fusion, how should this problem be solved. thank you my launch:
<launch>
<!-- Multi-cameras demo with 2 D400 cameras -->
<!-- Choose visualization -->
<arg name="rviz" default="true" />
<arg name="rtabmapviz" default="true" />
<arg name="imu_ignore_acc" default="true" />
<arg name="imu_remove_gravitational_acceleration" default="true" />
<!-- Cameras -->
<include file="$(find realsense2_camera)/launch/rs_rgbd.launch">
<arg name="camera" value="camera1" />
<arg name="serial_no" value="046222071909" />
<arg name="align_depth" value="true" />
</include>
<include file="$(find realsense2_camera)/launch/rs_rgbd.launch">
<arg name="camera" value="camera2" />
<arg name="serial_no" value="036227071493" />
<arg name="align_depth" value="true" />
</include>
<!--laser-->
<include file="$(find rplidar_ros)/launch/rplidar_s1.launch">
</include>
<!-- Frames: Cameras are placed at 90 degrees, clockwise -->
<node pkg="tf" type="static_transform_publisher" name="base_to_camera1_tf"
args="0.0 0.0 0.0 0.0 0.0 0.0 /base_link /camera1_link 100" />
<node pkg="tf" type="static_transform_publisher" name="base_to_camera2_tf"
args="-0.025 -0.125 0.0 -0.877 0.10472 0.0 /base_link /camera2_link 100" />
<node pkg="tf" type="static_transform_publisher" name="base_to_laser"
args="-0.05 -0.063 0.0 2.702 0.0 0.0 /base_link /laser 100" respawn="true"/>
<node pkg="tf" type="static_transform_publisher" name="base_to_imu"
args="-0.12 0.0 0.0 0.0 0.0 0.0 /base_link /imu_link 100" />
<!-- sync rgb/depth images per camera -->
<group ns="camera1">
<node pkg="nodelet" type="nodelet" name="nodelet_manager1" args="manager"/>
<node pkg="nodelet" type="nodelet" name="rgbd_sync1" args="load rtabmap_ros/rgbd_sync nodelet_manager1">
<remap from="rgb/image" to="color/image_rect_color"/>
<remap from="depth/image" to="aligned_depth_to_color/image_raw"/>
<remap from="rgb/camera_info" to="color/camera_info"/>
<param name="approx" value="false"/>
</node>
</group>
<group ns="camera2">
<node pkg="nodelet" type="nodelet" name="nodelet_manager2" args="manager"/>
<node pkg="nodelet" type="nodelet" name="rgbd_sync2" args="load rtabmap_ros/rgbd_sync nodelet_manager2">
<remap from="rgb/image" to="color/image_rect_color"/>
<remap from="depth/image" to="aligned_depth_to_color/image_raw"/>
<remap from="rgb/camera_info" to="color/camera_info"/>
<param name="approx" value="false"/>
</node>
</group>
<group ns="rtabmap">
<!-- approx sync cameras together -->
<node pkg="rtabmap_ros" type="rgbdx_sync" name="rgbdx_sync">
<remap from="rgbd_image0" to="/camera1/rgbd_image"/>
<remap from="rgbd_image1" to="/camera2/rgbd_image"/>
<param name="rgbd_cameras" type="int" value="2"/>
<param name="approx_sync" type="bool" value="true"/>
</node>
<!-- Odometry -->
<node pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry" output="screen">
<remap from="rgbd_image0" to="/camera1/rgbd_image"/>
<remap from="rgbd_image1" to="/camera2/rgbd_image"/>
<param name="subscribe_rgbd" type="bool" value="true"/>
<param name="frame_id" type="string" value="base_link"/>
<param name="rgbd_cameras" type="int" value="2"/>
<param name="Vis/EstimationType" type="string" value="1"/> <!-- should be 0 for multi-cameras -->
<param name="Vis/CorGuessWinSize" type="string" value="0"/>
</node>
<!-- Visual SLAM (robot side) -->
<!-- args: "delete_db_on_start" and "udebug" -->
<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
<param name="subscribe_depth" type="bool" value="false"/>
<param name="subscribe_rgbd" type="bool" value="true"/>
<param name="rgbd_cameras" type="int" value="2"/>
<param name="frame_id" type="string" value="base_link"/>
<param name="gen_scan" type="bool" value="true"/>
<param name="map_always_update" type="bool" value="false"/> <!-- refresh grid map even if we are not moving-->
<param name="map_empty_ray_tracing" type="bool" value="false"/> <!-- don't fill empty space between the generated scans-->
<param name="subscribe_scan" type="bool" value="true"/>
<remap from="scan" to="/scan"/>
<remap from="rgbd_image0" to="/camera1/rgbd_image"/>
<remap from="rgbd_image1" to="/camera2/rgbd_image"/>
<param name="Grid/Sensor" type="string" value="false"/>
<param name="Vis/EstimationType" type="string" value="0"/> <!-- should be 0 for multi-cameras -->
<param name="Vis/CorGuessWinSize" type="string" value="0"/>
<param name="OdomF2M/BundleAdjustment" type="string" value="0"/>
</node>
<!-- Localization-only mode -->
<arg name="localization" default="false"/>
<arg if="$(arg localization)" name="rtabmap_args" default=""/>
<arg unless="$(arg localization)" name="rtabmap_args" default="--delete_db_on_start"/>
<!-- 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="35"/>
<param name="sensor_timeout" value="0.1"/>
<param name="two_d_mode" value="false"/>
<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="/rtabmap/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, true,
false, false, false,
true, true, true,
false, false, false,
false, false, false]</rosparam>
<rosparam if="$(arg imu_ignore_acc)" param="imu0_config">[
false, false, false,
true, true, true,
false, false, false,
true, true, true,
false, false, false] </rosparam>
<rosparam unless="$(arg imu_ignore_acc)" param="imu0_config">[
false, false, false,
true, true, true,
false, false, false,
true, true, true,
true, true, true] </rosparam>
<param name="odom0_differential" value="false"/>
<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="$(arg imu_remove_gravitational_acceleration)"/>
<param name="print_diagnostics" value="true"/>
<!-- ======== ADVANCED PARAMETERS ======== -->
<param name="odom0_queue_size" value="5"/>
<param name="imu0_queue_size" value="50"/>
<!-- 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>
<!-- Visualisation RTAB-Map -->
<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_depth" type="bool" value="false"/>
<param name="subscribe_rgbd" type="bool" value="false"/>
<param name="subscribe_odom_info" type="bool" value="true"/>
<param name="frame_id" type="string" value="base_link"/>
<param name="rgbd_cameras" type="int" value="2"/>
<!--remap from="odom" to="/rtabmap/odometry/filtered"/-->
<remap from="rgbd_image0" to="/camera1/rgbd_image"/>
<remap from="rgbd_image1" to="/camera2/rgbd_image"/>
</node>
</group>
<!-- Visualization RVIZ -->
<node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/config/rgbd.rviz"/>
</launch>
3d image:
 |
Re: How rtabmap completes visual odometry and imu fusion ？
Administrator
|
| Free forum by Nabble | Edit this page |