search

IntelRealSense / realsense-ros

ROS Wrapper for Intel(R) RealSense(TM) Cameras
http://wiki.ros.org/RealSense
Apache License 2.0
2.54k stars 1.75k forks source link

D435i Question: Is there any way to get the gravity direction using the realsense-ros wrapper? #2521

Closed martinakos closed 1 year ago

martinakos commented 1 year ago

Ideally the gravity direction wrt. camera frame, published in some topic.

If this is not possible with realsense-ros, is it possible to get this direction vector using librealsense sdk?

MartyG-RealSense commented 1 year ago

Hi @martinakos On RealSense cameras equipped with an IMU (D435i, D455, D457) vertical gravity is represented by the Y value of Accel. You can publish the Accel topic on its own without gyro data by enabling the topic in the roslaunch instruction with enable_accel:=true

This should cause the topics below to be published.

/camera/accel/imu_info
/camera/accel/sample

The Accel values should be in the /camera/accel/sample topic.

Do not use the unite_imu_method instruction if you only want the Accel values as this instruction combines the separate gyro and accel topics into a single /imu topic.

More information about the D435i's IMU directions can be found at the link below.

https://github.com/IntelRealSense/librealsense/blob/master/doc/d435i.md#sensor-origin-and-coordinate-system

martinakos commented 1 year ago

I'm using unite_imu_method at the moment as I need the /imu topic for a VIO. Is there any way to get the /camera/accel/sample and /imu topics simultaneously?

MartyG-RealSense commented 1 year ago

My understanding is that if you are publishing /imu then then the accel values can be obtained from that topic as linear_acceleration, as shown at https://github.com/IntelRealSense/realsense-ros/issues/1286#issue-656550753

image

MartyG-RealSense commented 1 year ago

Hi @martinakos Do you require further assistance with this case, please? Thanks!

martinakos commented 1 year ago

Sorry, I was away these last days. I'll check what you suggest and come back with the results.

MartyG-RealSense commented 1 year ago

No problem at all, @martinakos - thanks very much for the update. I look forward to your next report.

martinakos commented 1 year ago

If I plot the linear_acceleration vector I don't get a constant up vector in the fix frame, it seems to turn with the camera. On the other hand, I can plot the up vector successfully using https://github.com/CCNYRoboticsLab/imu_tools So I checked their code and see they multiply the linear_acceleration by the orientation https://github.com/CCNYRoboticsLab/imu_tools/blob/noetic/rviz_imu_plugin/src/imu_acc_visual.cpp#L103 However, my orientation is always zeros.

I think I may need to do a sensor fusion using a imu_filter_madgwick to get the orientation. I'll have to explore how that works. Can I double check, before exploring how to use imu_filter_madgwick, that the realsense-ros driver doesn't do that sensor fusion so that I can get an up direction vector?

MartyG-RealSense commented 1 year ago

In Intel's SLAM navigation guide for ROS and IMU-equipped RealSense cameras such as D435i at the link below, imu_filter_madgwick is used in combination with the RealSense ROS wrapper's ROS1 launch file opensource_tracking.launch

https://github.com/IntelRealSense/realsense-ros/wiki/SLAM-with-D435i

MartyG-RealSense commented 1 year ago

Hi @martinakos Do you require further assistance with this case, please? Thanks!

martinakos commented 1 year ago

I got this to work using an imu_complementary_filter to fuse the angular velocity and linear acceleration into an orientation and then fill the empty orientation field in the Imu message . From that I get the up direction.

MartyG-RealSense commented 1 year ago

It's excellent news that you achieved a solution. Thanks very much for the update!