search

introlab / rtabmap_ros

RTAB-Map's ROS package.
http://wiki.ros.org/rtabmap_ros
BSD 3-Clause "New" or "Revised" License
974 stars 557 forks source link

How can I output Euler angle from imu? #323

Open Garand0o0 opened 5 years ago

Garand0o0 commented 5 years ago

My camera is realsense d435i.After running command with D435i, the following list of topics will be available:

Is there code for handling IMU to get the Euler angle in rtabmap_ros or rtabmap?I want to print the Euler angle on the terminal. What should I do?

matlabbe commented 5 years ago

You would have to use madgwick or complementary filter to get the quaternion from accelerometer and gyroscope. If you enable publish_tf parameter of those nodes, you can visualize the orientation with TF in RVIZ.

Garand0o0 commented 5 years ago

Hi, matlabbe. Thank you for your reply. I ussed complementary filter: 1.Open RTAB-Map 2.Click on window -> preferences -> source select RGBD as realsense2, select IMU filter as complementary filter 3.start There is a warning at the terminal:

[ WARN] (2019-07-20 17:19:53.201) CameraRealSense2.cpp:368::getPoseAndIMU() Could not find acc data to interpolate at time 1563614393141.365479 after waiting 30 ms (last is 10311.474490)... [ WARN] (2019-07-20 17:19:53.242) CameraRealSense2.cpp:368::getPoseAndIMU() Could not find acc data to interpolate at time 1563614393174.957520 after waiting 30 ms (last is 10359.626492)... [ WARN] (2019-07-20 17:19:53.280) CameraRealSense2.cpp:368::getPoseAndIMU() Could not find acc data to interpolate at time 1563614393226.548828 after waiting 30 ms (last is 10391.729494)... [ WARN] (2019-07-20 17:19:53.327) CameraRealSense2.cpp:368::getPoseAndIMU() Could not find acc data to interpolate at time 1563614393258.920410 after waiting 30 ms (last is 10439.882496)... [ WARN] (2019-07-20 17:19:53.365) CameraRealSense2.cpp:368::getPoseAndIMU() Could not find acc data to interpolate at time 1563614393311.875488 after waiting 30 ms (last is 10488.035498)... [ WARN] (2019-07-20 17:19:53.406) CameraRealSense2.cpp:368::getPoseAndIMU() Could not find acc data to interpolate at time 1563614393342.908447 after waiting 30 ms (last is 10520.137500)... .

There are acc and gyro in D435i. What shuold I do! Thank you!

matlabbe commented 5 years ago

Are you using latest version from master branch of rtabmap? Which librealsense2 version are you using?

I cannot reproduce the error with my D435i. Not related to your error, but to make rtabmap actually use IMU in odometry and graph optimization, parameter Preferences->Graph Optimzation->Gravity Sigma should not be zero.

cheers, Mathieu

Garand0o0 commented 5 years ago

Hi, matlabbe. Thank you for your reply. The librealsense2 version I use is realsense-2.2.3. The rtabmap I used is the latest version. I changed what you said about configuration parameters, but I still have the same warning. I want to use rtabmap-ros. I modified the following configuration parameters in the rs_camera.launch

arg name="unite_imu_method" default="copy"

Rtabmap_ros can accept imu_topic:

imu_topic:=/camera/imu.

But there is an error at the terminal:

[ERROR] (2019-07-26 16:19:35.082) CoreWrapper.cpp:2104::imuAsyncCallback() IMU received doesn't have orientation set, it is ignored. [ERROR] (2019-07-26 16:19:35.083) CoreWrapper.cpp:2104::imuAsyncCallback() IMU received doesn't have orientation set, it is ignored. [ERROR] (2019-07-26 16:19:35.094) CoreWrapper.cpp:2104::imuAsyncCallback() IMU received doesn't have orientation set, it is ignored.

What shuold I do! Thank you!

matlabbe commented 5 years ago

Does /camera/imu contain a valid quaternion? If it is just accelerometer and gyroscope, use an imu filter node (madgwick or complementary filter) to compute the quaternion, then feed rtabmap the output of the imu filter.

For the standalone, just tried also with latest librealsense2-dev binaries and it works. Maybe an USB problem?

cheers, Mathieu

Garand0o0 commented 5 years ago

Hi, matlabbe. Thank you for your reply. I added some statements to the rtabmap.launch:

<node pkg="nodelet" type="nodelet" name="imu_manager" 
    args="manager" output="screen" />

  <node pkg="imu_complementary_filter" type="complementary_filter_node"
      name="complementary_filter_gain_node" output="screen">
    <param name="do_bias_estimation" value="true"/>
    <param name="do_adaptive_gain" value="true"/>
    <param name="use_mag" value="false"/>
    <param name="gain_acc" value="0.01"/>
    <param name="gain_mag" value="0.01"/>

I used the imu filter node (complmentary). There is no error at the terminal, and it can publish topic: 

Published topics:
* /imu/data [sensor_msgs/Imu].

This topic has been subscribed:

Subscribed topics:
* /imu/data [sensor_msgs/Imu] 1 subscriber

The imu_topic is "/imu/data"/ in rtabmap.launch: <arg name="imu_topic" default="/imu/data"/> <!-- only used with VIO

But I'm not sure if IMU is added to the graph. How do I verify it?

matlabbe commented 5 years ago

Open the database in DatabaseViewer, then open Graph View, look at the bottom. The last line shows the number of links of each type. For IMU links, look for GR (Gravity) links. Below there are 109 gravity links. I just added (see this commit) also on right in the screenshot a Gravity line with the gravity vector corresponding to IMU orientation (transformed in base_link). Screenshot_2019-07-29_10-39-53

Garand0o0 commented 5 years ago

Thank you very much for your reply. These are very helpful to me, but I still have some questions that need your help.

Is the yellow line in your picture the gravity line? I changed the program as you said, but I didn't see the gravity line in my screenshot. Only a few nodes are yellow. I'm sorry I can't upload pictures here.

I tilted the sensor when I ran rtabmap_ros. Result of the graph was clear when I looked at it sideways, but it would be blurry if the rotating graph let it face me. I downloaded the demo_mapping.bag. Result of the demo is great, it is clear in any direction. Is this phenomenon caused by sensors(D435i)?

I want to model a channel 360 degrees and try to rotate it with two sensors, but there are many overlaps and faults in the graph. How can I improve this situation, or choose more sensors to work?

matlabbe commented 5 years ago

Hi,

The yellow lines are proximity detection, not gravity links. The gravity links are not shown in the graph view. For demo_mapping.bag, no imu is used, the mapping is done in 2D (Reg/Force3DoF=true), this is why the ground is straight on the xy plane. To compare with and without gravity constraints, you can change parameter Optimizer/GravitySigma (See Core Parameters view) to non-zero and zero respectively. Click on checkbox "Span to all maps" two times to force a re-optimization. You can do Edit->View point cloud to see the map in 3D to better see the influence of gravity constraints.

Not sure if I can visualize clearly the "model a channel 360 degrees and try to rotate it with two sensors", if you use multiple cameras and input them to rtabmap, make sure TF is accurate between the cameras. With D435s, I think you can also hardware synchronize the cameras, which I would recommend.

cheers, Mathieu