how to use d435i stereo + px4(imu) to run the algorithm?

[huang-shijie]

I successfully ran the algorithm using the binocular + imu of d435i, but when using the imu data of px4 (subscribing to the sensor_combined topic in the 1.14 firmware), the algorithm drifted after running for a period of time. The message_filters (python) in ros2 are used to synchronize the data with imu one after another, and the synchronization effect is okay.As shown below. and the code of synchronization are shown below:

from message_filters import ApproximateTimeSynchronizer, Subscriber
import rclpy
from rclpy.node import Node
from rclpy.clock import Clock
from rclpy.qos import qos_profile_sensor_data,QoSProfile, ReliabilityPolicy, HistoryPolicy, DurabilityPolicy
# from sensor_msgs.msg import Image, PointCloud2
from px4_msgs.msg import SensorCombined
from sensor_msgs.msg import Image
from sensor_msgs.msg import Imu

class ExampleNode(Node):
    def __init__(self):
        super().__init__("ExampleNode")
        self.imu_pub = self.create_publisher(Imu,"imu", 100) 
        self.image1_pub = self.create_publisher(Image,"image1", 10) 
        self.image2_pub = self.create_publisher(Image,"image2", 10) 

        self.image1_sub = Subscriber(self, Image, "camera/infra1/image_rect_raw")
        self.image2_sub = Subscriber(self, Image, "camera/infra2/image_rect_raw")
        qos = QoSProfile(
            reliability=ReliabilityPolicy.BEST_EFFORT,
            durability=DurabilityPolicy.TRANSIENT_LOCAL,
            history=HistoryPolicy.KEEP_LAST,
            depth=1
        )
        self.imu_sub = Subscriber(
            self, SensorCombined, "fmu/out/sensor_combined", qos_profile=qos
        )
        queue_size = 60
        # you can use ApproximateTimeSynchronizer if msgs dont have exactly the same timestamp
        self.ts = ApproximateTimeSynchronizer(
            [self.image1_sub,self.image2_sub, self.imu_sub],
            queue_size,
            0.01,  # defines the delay (in seconds) with which messages can be synchronized
            allow_headerless=True
        )
        self.ts.registerCallback(self.callback)

    def callback(self, image1_msg, image2_msg, imu_msg):
        # do your stuff here
        imu_data = Imu()
        clk = Clock()
        imu_data.header.stamp = clk.now().to_msg()
        imu_data.angular_velocity.x = float(imu_msg.gyro_rad[0])
        imu_data.angular_velocity.y = float(imu_msg.gyro_rad[1])
        imu_data.angular_velocity.z = float(imu_msg.gyro_rad[2])

        imu_data.linear_acceleration.x = float(imu_msg.accelerometer_m_s2[0])
        imu_data.linear_acceleration.y = float(imu_msg.accelerometer_m_s2[1])
        imu_data.linear_acceleration.z = float(imu_msg.accelerometer_m_s2[2])

        self.imu_pub.publish(imu_data)
        self.image1_pub.publish(image1_msg)
        self.image2_pub.publish(image2_msg)

def main(args=None):
    rclpy.init(args=args) # 初始化rclpy
    node = ExampleNode()
    print("111")
    rclpy.spin(node)
    print("222")
    rclpy.shutdown()

How can i solve it?

[goldbattle]

A good way to test if things will work in VIO is if you were able to calibrate them successfully with Kalibr to a good accuracy. How did you do your IMU-camera calibration and what did the reprojection and other residual errors look like?

[goldbattle]

Feel free to reopen with a bag and your calibration files which you have issues running wit. I recommend running through Kalibr and posting your result PDF.

[makeecat]

@huang-shijie Hi! Can you share your realsense-ros launch configuration for d435i case? Did you set the QoS of IMU specifically to SENSOR_DATA?

I encountered similar issue you described on realsense d435i sensor.