D435i IMU problem

[TanmayPathak]

---

Required Info
Camera Model	D435i
Operating System & Version	Win 10
Language	python

Issue Description

As I have taken IMU data by using following code :

accX = frames[0].as_motion_frame().get_motion_data().x
accY = frames[0].as_motion_frame().get_motion_data().y
accZ = frames[0].as_motion_frame().get_motion_data().z

gyroX = frames[1].as_motion_frame().get_motion_data().x
gyroY = frames[1].as_motion_frame().get_motion_data().y
gyroZ = frames[1].as_motion_frame().get_motion_data().z

The Data which I am getting is highly fluctuating and from that I also not be able to derive accurate YAW , PITCH , ROLL etc values .
Is there any method available in python to derive these data ( YAW, PITCH , ROLL ) directly ?

[ev-mp]

@TanmayPathak hello, IMU raw data inherently exhibits noisy characteristics.
Please take a look at rs-motion demo that continuously integrates IMU data to produce orientation angles/matrix.
Duplicate of #4356 .

[malith1992]

@ev-mp From the rs-motion example, we can get angles of two axis only. Isn't it ?

[ev-mp]

@malith1992, that is correct. In an inertial system affected by G-Force only you can extract the actual Pitch and Roll angles. You also track the left-right rotation, but the initial Heading angle is arbitrary defined.

[malith1992]

I wrote a code to calculate the YAW, PITCH and ROLL in python by referring rs-motion example and a IMU data gathering example . I used the same logic. Please tell me if there is any mistake in my code @ev-mp

import pyrealsense2 as rs
import math

def initialize_camera():
    # start the frames pipe
    p = rs.pipeline()
    conf = rs.config()
    conf.enable_stream(rs.stream.accel)
    conf.enable_stream(rs.stream.gyro)
    prof = p.start(conf)
    return p

p = initialize_camera()

first = True
alpha = 0.98

try:
    while True:
        f = p.wait_for_frames()

        #gather IMU data
        accel = f[0].as_motion_frame().get_motion_data()
        gyro = f[1].as_motion_frame().get_motion_data()

        ts = f.get_timestamp()

        #calculation for the first frame
        if (first):
            first = False
            last_ts_gyro = ts

            # accelerometer calculation
            accel_angle_z = math.degrees(math.atan2(accel.y, accel.z))
            accel_angle_x = math.degrees(math.atan2(accel.x, math.sqrt(accel.y * accel.y + accel.z * accel.z)))
            accel_angle_y = math.degrees(math.pi)

            continue

        #calculation for the second frame onwards

        # gyrometer calculations
        dt_gyro = (ts - last_ts_gyro) / 1000
        last_ts_gyro = ts

        gyro_angle_x = gyro.x * dt_gyro
        gyro_angle_y = gyro.y * dt_gyro
        gyro_angle_z = gyro.z * dt_gyro

        dangleX = gyro_angle_x * 57.2958
        dangleY = gyro_angle_y * 57.2958
        dangleZ = gyro_angle_z * 57.2958

        totalgyroangleX = accel_angle_x + dangleX
        totalgyroangleY = accel_angle_y + dangleY
        totalgyroangleZ = accel_angle_z + dangleZ

        #accelerometer calculation
        accel_angle_z = math.degrees(math.atan2(accel.y, accel.z))
        accel_angle_x = math.degrees(math.atan2(accel.x, math.sqrt(accel.y * accel.y + accel.z * accel.z)))
        accel_angle_y = math.degrees(math.pi)


        #combining gyrometer and accelerometer angles
        combinedangleX = totalgyroangleX * alpha + accel_angle_x * (1-alpha)
        combinedangleZ = totalgyroangleZ * alpha + accel_angle_z * (1-alpha)
        combinedangleY = totalgyroangleY


        print("Angle -  X: " + str(round(combinedangleX,2)) + "   Y: " + str(round(combinedangleY,2)) + "   Z: " + str(round(combinedangleZ,2)))

finally:
    p.stop()

You can get an idea about the sensor origin and coordinates of D435i camera from this document

[AhnHyeongHyun]

@malith1992
I try to your source test. but why result Angle(x, y, z) data different?

[malith1992]

@AhnHyeongHyun What kind of different you mean ?

[ev-mp]

@malith1992, in the above code you use hard-coded indexes for accel and gyro frames, which is not the correct approach:

#gather IMU data
accel = f[0].as_motion_frame().get_motion_data()
gyro = f[1].as_motion_frame().get_motion_data()

You need to iterate over the frames in the frameset to establish their affinity.
Also the orientation calculation is wrong - the acceleration values refer to the linear acceleration, i.e. they affect the device's movement in 3D space (x,y,z) but they do not affect the camera orientation. The orientation is tracked exclusively by integrating gyro velocities.

totalgyroangleX = accel_angle_x + dangleX
#accelerometer calculation
accel_angle_z = math.degrees(math.atan2(accel.y, accel.z))
accel_angle_x = math.degrees(math.atan2(accel.x, math.sqrt(accel.y * accel.y + accel.z * accel.z)))
accel_angle_y = math.degrees(math.pi)

In certain situations you can extract orientation roll and pitch from the acceleration values and use them to compensate for the gyro drifts. But this is only possible if (a) you have the IMU unit perfectly calibrated, and (b) the sum of forces applied to the camera equals G (e.g the device's is either completely static or moves with a constant linear velocity)

[ev-mp]

@TanmayPathak , do you have follow-up questions on this topic ?

[magnusbarata]

Hello, I'm new to sensors like IMU, and I've got some questions about using D435i IMU and converting it to angles.

1. I tried the rs-motion, and here's my result.
   
   As you can see, roll and pitch works fine, but yaw is not, which should be fine quoting this.

   @malith1992, that is correct. In an inertial system affected by G-Force only you can extract the actual Pitch and Roll angles. You also track the left-right rotation, but the initial Heading angle is arbitrary defined.

   My question is, why the gif shown at the rs-motion sample can track left-right (yaw) rotation?

2. Referencing from the imutest, rs-motion, and this issue source code, I tried to implement my own rs-motion in python and it looks similar to @malith1992's implementation. I notice that my Y value (yaw) is only fluctuating a bit from the initial value of PI or not changing at all (tried the above source code I am referencing above and got the same result). Is there something wrong with my device or do I need to implement another function to track the left-right rotation? If my device is normal, I'd be happy if someone could explain to me how to track the left-right rotation (it's okay even if the initial heading angle is arbitrary defined, as long as I know the degree of rotation from the initial position).

[ev-mp]

@magnusbarata , it seem that the gyro is not working properly.
With gyro streaming normally there should be no issue of tracking left/rigth. Actually gyro has no notion of "left/right" it just tracks attitude in 3D inertial frame.

Did you try to run Reaselse-Viewer and check the IMU framerates and data ?
As for the demo - please verify that this is not a host issue ( Release build/ Opengl drivers/ Remove Desktop) by running the same demo on a different PC

[yjlin0223]

@malith1992, in the above code you use hard-coded indexes for accel and gyro frames, which is not the correct approach:

#gather IMU data
accel = f[0].as_motion_frame().get_motion_data()
gyro = f[1].as_motion_frame().get_motion_data()

You need to iterate over the frames in the frameset to establish their affinity.
Also the orientation calculation is wrong - the acceleration values refer to the linear acceleration, i.e. they affect the device's movement in 3D space (x,y,z) but they do not affect the camera orientation. The orientation is tracked exclusively by integrating gyro velocities.

totalgyroangleX = accel_angle_x + dangleX
#accelerometer calculation
accel_angle_z = math.degrees(math.atan2(accel.y, accel.z))
accel_angle_x = math.degrees(math.atan2(accel.x, math.sqrt(accel.y * accel.y + accel.z * accel.z)))
accel_angle_y = math.degrees(math.pi)

In certain situations you can extract orientation roll and pitch from the acceleration values and use them to compensate for the gyro drifts. But this is only possible if (a) you have the IMU unit perfectly calibrated, and (b) the sum of forces applied to the camera equals G (e.g the device's is either completely static or moves with a constant linear velocity)

can you correct the mistakes in the codeabove @ev-mp , or provide a clear way to calculate the following values？

[roobooot]

Hey, thanks for the code. It is working very well, however, except the angle_Y. I made a few change so that angle_Y could be estimated by integrating gyro_Y (tested, working well ignoring the shift after long-time running).

    alpha = 0.98
    totalgyroangleY = 0
    try:
        while True:```

        ...
        ...

        totalgyroangleX = accel_angle_x + dangleX
        # totalgyroangleY = accel_angle_y + dangleY
        totalgyroangleY = accel_angle_y + dangleY + totalgyroangleY
        totalgyroangleZ = accel_angle_z + dangleZ

        #accelerometer calculation
        accel_angle_z = math.degrees(math.atan2(accel.y, accel.z))
        accel_angle_x = math.degrees(math.atan2(accel.x, math.sqrt(accel.y * accel.y + accel.z * accel.z)))
        # accel_angle_y = math.degrees(math.pi)
        accel_angle_y = 0

        #combining gyrometer and accelerometer angles
        combinedangleX = totalgyroangleX * alpha + accel_angle_x * (1-alpha)
        combinedangleZ = totalgyroangleZ * alpha + accel_angle_z * (1-alpha)
        combinedangleY = totalgyroangleY

Please look at the commented line and the amended line below it if you wanna get the angle_Y

[MartyG-RealSense]

Thanks so much @roobooot for sharing your solution for calculating Y :)

[roobooot]

Thanks to @malith1992 and his brilliant solution.