Hello,
I am doing a project that is combining the data from visual odometry (camera ASUS Xtion Pro live) with IMU (SparkFun razor). I want to combine some sources of information to have the best odometry and i start with visual odometry and IMU. I used package rtabmap_ros node rgbd_odometry for visual odometry and package razor_imu_9dof for IMU data. To combine 2 sources, i used package robot_localization node ekf. I published 2 static tf (/base_link /camera_link; /base_link /base_imu_link). I intend to fuse x, y, z, roll, pitch, yaw from visual odometry and roll, pitch, yaw from IMU. My filtered odometry is worse than the odometry from visual odometry. The pose is drift very quick even in case that i don't move the camera. I saw the covariance matrix of filtered odometry data is always increased. The covariance of imu data is set as constant. The covariance of visual odometry is changed frequently but it's small values (the type of visual odometry is nav_msg/odometry but data is existed in geometry_msgs/PoseWithCovariance, geometry_msgs/TwistWithCovariance is always 0). I don't understand whether it affect the system or not. Can you give me an advice for configuring the filter or other solution to fuse 2 informations.
Another question is about the orientation of camera and IMU board. I setup the platform like this. Is the axis are correct?
PLATFORM http://s1.postimg.org/5cp7gyoj3/platform.jpg
I attached here the NodeGraph,TFTree, node robot_localization configuration and static tf publish configuration.
Thank you in advance for your attention to this matter.
ROS_GRAPH http://s27.postimg.org/xmij6av83/rosgraph.png TF_TREE http://s1.postimg.org/kt29kbzqn/frames.png
<node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization" clear_params="true">
<!-- ======== STANDARD PARAMETERS ======== -->
<param name="frequency" value="10"/>
<param name="sensor_timeout" value="0.5"/>
<param name="two_d_mode" value="false"/>
<param name="map_frame" value="map"/>
<!-- Defaults to "odom" if unspecified -->
<param name="odom_frame" value="odom"/>
<!-- Defaults to "base_link" if unspecified -->
<param name="base_link_frame" value="base_link"/>
<!-- Defaults to the value of "odom_frame" if unspecified -->
<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"/>
<rosparam param="odom0_config">[true, true, true,
true, true, true,
false, false, false,
false, false, false,
false, false, false]</rosparam>
<rosparam param="imu0_config">[false, false, false,
true, true, true,
false, false, false,
false, false, false,
false, false, false]</rosparam>
<param name="odom0_differential" value="false"/>
<param name="imu0_differential" value="false"/>
<param name="odom0_relative" value="false"/>
<param name="imu0_relative" value="false"/>
<param name="imu0_remove_gravitational_acceleration" value="true"/>
<param name="print_diagnostics" value="true"/>
<!-- ======== ADVANCED PARAMETERS ======== -->
<param name="odom0_queue_size" value="2"/>
<param name="imu0_queue_size" value="10"/>
<param name="odom0_pose_rejection_threshold" value="0.1"/>
<param name="odom0_twist_rejection_threshold" value="0"/>
<param name="imu0_pose_rejection_threshold" value="0.5"/>
<param name="imu0_twist_rejection_threshold" value="0"/>
<param name="imu0_linear_acceleration_rejection_threshold" value="0.1"/>
<param name="debug" value="false"/>
<param name="debug_out_file" value="debug_ekf_localization.txt"/>
<rosparam param="process_noise_covariance">[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.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.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.025, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0.025, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0.04, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.02, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.015]</rosparam>
<rosparam param="initial_estimate_covariance">[1e-5, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 1e-5, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 1e-5, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 1e-5, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 1e-5, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 1e-5, 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>
<node pkg="tf" type="static_transform_publisher" name="link1_broadcaster" args="0 0 0 0 0 0 base_link base_imu_link 10" />
<node pkg="tf" type="static_transform_publisher" name="link2_broadcaster" args="0 0 0 0 0 0 base_link camera_link 10" />
Originally posted by VMHOANG68 on ROS Answers with karma: 33 on 2015-11-26
Post score: 3
Original comments
Comment by Tom Moore on 2016-03-01:
Please provide sample input messages from your sensors.
Comment by zhchpan on 2018-01-20:
hello, had you been able to successfully fuse VO and IMU using robot_localization? Could you give me your config file or launch file? I want to know where I made mistakes. Thanks for your help~:)
Comment by jayess on 2018-01-20:
Please don't use an answer to ask a question. This isn't a forum. Please create a new question.