How do i get position and angles data from my d435i?

[MyOtherNamesWereTaken]

• Before opening a new issue, we wanted to provide you with some useful suggestions (Click "Preview" above for a better view):

  • Consider checking out SDK examples.
  • Have you looked in our documentations?
  • Is you question a frequently asked one?
  • Try searching our GitHub Issues (open and closed) for a similar issue.

• All users are welcomed to report bugs, ask questions, suggest or request enhancements and generally feel free to open new issue, even if they haven't followed any of the suggestions above :)

---

Required Info
Camera Model	{ D435i }
Firmware Version	(05.13.00.50)
Operating System & Version	{ Linux (Ubuntu 14/16/17)
Kernel Version (Linux Only)	(5.11.0 -38-generic )
Platform	PC
Language	{python}
Segment	{Robot/Smartphone/VR/AR/others }

Issue Description

Hello.

I am trying to get following data from my d435i
-xyz angles (to itself and the object)
-xyz position (of itself and the object)

[MartyG-RealSense]

Hi @MyOtherNamesWereTaken Obtaining both the angle and position of something is known as its pose, and therefore the process of calculating these values is pose estimation.

This is a difficult problem for D435i, as unlike the T265 Tracking Camera model it does not have built-in support for pose stream data. It is possible to obtain pose with a D435i though by using ROS or OpenCV.

If you wish to avoid using ROS as part of your solution then you may prefer to obtain the pose of an observed object using an OpenCV SolvePNP algorithm. The official OpenCV documentation has a Python tutorial for this at the link below.

https://docs.opencv.org/4.x/d7/d53/tutorial_py_pose.html

If you are able to use ROS to obtain the pose of an object then there are a range of available solutions, such as Deep Object Pose Estimation (DOPE) that uses a technique called inference.

https://github.com/NVlabs/Deep_Object_Pose

---

In regard to obtaining the pose of the D435i camera itself, this could be more complicated. It is straightforward to obtain the angle of the camera from its IMU using Python code such as the example in #3917 (comment) or without using the IMU by performing a plane-fit algorithm (see the link below for more information).

https://support.intelrealsense.com/hc/en-us/community/posts/360050894154/comments/360013322694

For obtaining the relative position of the camera on a real-world map, you could consider doing so using ROS and SLAM, as described at #8692 (comment)

[MartyG-RealSense]

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

[MyOtherNamesWereTaken]

I just need to figure out the cameras rotation (to itself) via the gyro/accel. As in i need the Roll, Yaw and Pitch. I want the z axis to be on the cameras length axis and the x axis where the camera lenses "look out of". Im basically holding the camera vertically and i can ignore the rotation on the said x-axis. How do i program this?

def initialize_camera():
# start the frames pipe
context = rs.context()
pipeline = rs.pipeline(context)
config = rs.config()
config.enable_stream(rs.stream.gyro, rs.format.motion_xyz32f, 400)
config.enable_stream(rs.stream.accel, rs.format.motion_xyz32f, 250)
profile = pipeline.start(config)
return pipeline

pipeline = initialize_camera()

first = True
alpha = 0.98

try:
while True:
f = pipeline.wait_for_frames()
# xyz depth
# xcamera =
# ycamera =
# zcamera =

    # 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

    # gyro-meter calculations
    dt_gyro = (ts - last_ts_gyro) / 1000
    last_ts_gyro = ts

    # change in angle
    gyro_angle_x = gyro.x * dt_gyro  # Pitch
    gyro_angle_y = gyro.y * dt_gyro  # Yaw
    gyro_angle_z = gyro.z * dt_gyro  # Roll

    # angle in degree
    dangleX = gyro_angle_x * 57.2958
    dangleY = gyro_angle_y * 57.2958
    dangleZ = gyro_angle_z * 57.2958

    total_gyro_angleX = accel_angle_x + dangleX
    total_gyro_angleY = accel_angle_y + dangleY
    total_gyro_angleZ = 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 gyro-meter and accelerometer angles
    combined_angleX = total_gyro_angleX
    combined_angleY = total_gyro_angleY * alpha + accel_angle_x * (1 - alpha)
    combined_angleZ = total_gyro_angleZ * alpha + accel_angle_z * (1 - alpha)

    print(
        "Angle -  X: " + str(round(combined_angleX, 2)) + "   Y: " + str(
            round(combined_angleY, 2)) + "   Z: " + str(
            round(combined_angleZ, 2)))

finally:
pipeline.stop()

This is my code so far but:

1. I don't know if this is fundamentally wrong
2. The x, y, z rotation still needs to be swapped with each other i think

[MartyG-RealSense]

As you started another case with a new approach in #10092 do you still require assistance with this case here, please?

[MyOtherNamesWereTaken]

U can close this case, thanks

[MartyG-RealSense]

Thanks very much @MyOtherNamesWereTaken

[eya8sahli]

I have a problem in my code. I want to know if there is a rotation in wrist and neck landmarks.
Can anyone help me, please?
import numpy as np
import csv
from datetime import datetime
import pyrealsense2 as rs
import mediapipe as mp
import cv2

Function to calculate angle

def calculate_angle(a, b, c):
a = np.array(a)
b = np.array(b)
c = np.array(c)

radians = np.arctan2(c[1]-b[1], c[0]-b[0]) - np.arctan2(a[1]-b[1], a[0]-b[0])
angle = np.abs(radians*180.0/np.pi)

if angle > 180.0:
    angle = 360 - angle

return int(angle)

mp_drawing = mp.solutions.drawing_utils

Setup MediaPipe instance

mp_pose = mp.solutions.pose

Configure RealSense pipeline

pipeline = rs.pipeline()
config = rs.config()
config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)
config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)

Start streaming

pipeline.start(config)

Create CSV file to write data

with open('angles1.csv', mode='w', newline='') as file:
writer = csv.writer(file)
writer.writerow(['current_time','left_elbow', 'left_shoulder', 'right_elbow', 'right_shoulder', 'left_wrist', 'right_wrist', 'mid_hip', 'neck_angle', 'right_knee', 'left_knee'])

# Initialize filter parameters
prev_angle = None
alpha = 0.2

# Setup Mediapipe Pose detection
with mp_pose.Pose(min_detection_confidence=0.5, min_tracking_confidence=0.5) as pose:
    while True:
        # Wait for a coherent pair of frames: depth and color
        frames = pipeline.wait_for_frames()
        depth_frame = frames.get_depth_frame()
        color_frame = frames.get_color_frame()

        if not depth_frame or not color_frame:
            continue

        # Convert images to numpy arrays
        depth_image = np.asanyarray(depth_frame.get_data())
        color_image = np.asanyarray(color_frame.get_data())
       
        # Recolor image to RGB
        image = cv2.cvtColor(color_image, cv2.COLOR_BGR2RGB)
        image.flags.writeable = False
        

        # Apply Gaussian blur to reduce noise
        blurred_image = cv2.GaussianBlur(image, (5, 5), 0)

        # Make detection
        results = pose.process(blurred_image)

        # Recolor back to BGR
        blurred_image.flags.writeable = True
        blurred_image = cv2.cvtColor(blurred_image, cv2.COLOR_RGB2BGR)
        
        # Extract landmarks
        try:
            landmarks = results.pose_landmarks.landmark
                
            
           #get coordinates
            leftshoulder = [landmarks[mp_pose.PoseLandmark.LEFT_SHOULDER.value].x,landmarks[mp_pose.PoseLandmark.LEFT_SHOULDER.value].y]
            leftelbow = [landmarks[mp_pose.PoseLandmark.LEFT_ELBOW.value].x,landmarks[mp_pose.PoseLandmark.LEFT_ELBOW.value].y]
            leftwrist = [landmarks[mp_pose.PoseLandmark.LEFT_WRIST.value].x,landmarks[mp_pose.PoseLandmark.LEFT_WRIST.value].y]
            lefthip = [landmarks[mp_pose.PoseLandmark.LEFT_HIP.value].x,landmarks[mp_pose.PoseLandmark.LEFT_HIP.value].y]
            print(lefthip)
            rightshoulder = [landmarks[mp_pose.PoseLandmark.RIGHT_SHOULDER.value].x,landmarks[mp_pose.PoseLandmark.RIGHT_SHOULDER.value].y]
            rightelbow = [landmarks[mp_pose.PoseLandmark.RIGHT_ELBOW.value].x,landmarks[mp_pose.PoseLandmark.RIGHT_ELBOW.value].y]
            rightwrist = [landmarks[mp_pose.PoseLandmark.RIGHT_WRIST.value].x,landmarks[mp_pose.PoseLandmark.RIGHT_WRIST.value].y]
            righthip = [landmarks[mp_pose.PoseLandmark.RIGHT_HIP.value].x,landmarks[mp_pose.PoseLandmark.RIGHT_HIP.value].y]
            leftankle = [landmarks[mp_pose.PoseLandmark.LEFT_ANKLE.value].x,landmarks[mp_pose.PoseLandmark.LEFT_ANKLE.value].y]
            leftknee = [landmarks[mp_pose.PoseLandmark.LEFT_KNEE.value].x,landmarks[mp_pose.PoseLandmark.LEFT_KNEE.value].y]
            rightknee = [landmarks[mp_pose.PoseLandmark.RIGHT_KNEE.value].x,landmarks[mp_pose.PoseLandmark.RIGHT_KNEE.value].y]
            rightankle = [landmarks[mp_pose.PoseLandmark.RIGHT_ANKLE.value].x,landmarks[mp_pose.PoseLandmark.RIGHT_ANKLE.value].y]
            leftindex = [landmarks[mp_pose.PoseLandmark.LEFT_INDEX.value].x,landmarks[mp_pose.PoseLandmark.LEFT_WRIST.value].y]
            rightindex = [landmarks[mp_pose.PoseLandmark.RIGHT_INDEX.value].x,landmarks[mp_pose.PoseLandmark.RIGHT_WRIST.value].y]
            rightfootindex = [landmarks[mp_pose.PoseLandmark.RIGHT_FOOT_INDEX.value].x,landmarks[mp_pose.PoseLandmark.RIGHT_FOOT_INDEX.value].y]
            leftfootindex = [landmarks[mp_pose.PoseLandmark.LEFT_FOOT_INDEX.value].x,landmarks[mp_pose.PoseLandmark.LEFT_FOOT_INDEX.value].y]
            rightpinky= [landmarks[mp_pose.PoseLandmark.RIGHT_PINKY.value].x,landmarks[mp_pose.PoseLandmark.RIGHT_PINKY.value].y]
            leftpinky= [landmarks[mp_pose.PoseLandmark.LEFT_PINKY.value].x,landmarks[mp_pose.PoseLandmark.LEFT_PINKY.value].y]
            leftthumb= [landmarks[mp_pose.PoseLandmark.LEFT_THUMB.value].x,landmarks[mp_pose.PoseLandmark.LEFT_THUMB.value].y]
            rightthumb= [landmarks[mp_pose.PoseLandmark.RIGHT_THUMB.value].x,landmarks[mp_pose.PoseLandmark.RIGHT_THUMB.value].y]

            #calculate angle
            left_elbow = calculate_angle(leftshoulder,leftelbow,leftwrist)
            left_shoulder = calculate_angle(lefthip,leftshoulder,leftelbow)
            right_elbow = calculate_angle(rightshoulder,rightelbow,rightwrist)
            right_shoulder = calculate_angle(righthip,rightshoulder,rightelbow)
            left_wrist = calculate_angle(leftpinky,leftwrist,leftthumb)
            right_wrist = calculate_angle(rightpinky,rightwrist,rightthumb)
            
            right_knee = calculate_angle(lefthip,leftknee,leftankle)
            left_knee = calculate_angle(righthip,rightknee,rightankle)
            nose = [landmarks[mp_pose.PoseLandmark.NOSE.value].x,landmarks[mp_pose.PoseLandmark.NOSE.value].y]
            leftear = [landmarks[mp_pose.PoseLandmark.LEFT_EAR.value].x,landmarks[mp_pose.PoseLandmark.LEFT_EAR.value].y]
                            # les coordonnées du neck sont la moyenne des coordonnées des deux épaules
            neck_x = (leftshoulder[0] + rightshoulder[0]) / 2
            neck_y = (leftshoulder[1] + rightshoulder[1]) / 2
            neck_position = [neck_x, neck_y]
            hip_x = (lefthip[0] + righthip[0]) / 2
            hip_y = (lefthip[1] + righthip[1]) / 2
            mid_hip = [hip_x, hip_y]
            # calcul de l'angle pour le neck
            neck_angle = calculate_angle(nose, neck_position, leftear)
            mid_hip_angle = calculate_angle(neck_position, mid_hip, leftshoulder)

            #visualize
            cv2.putText(blurred_image, str(left_elbow), 
                        tuple(np.multiply(leftelbow, [640, 480]).astype(int)), 
                        cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 2, cv2.LINE_AA)
            cv2.putText(blurred_image, str(left_shoulder), 
                        tuple(np.multiply(leftshoulder, [640, 480]).astype(int)), 
                        cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 2, cv2.LINE_AA)
            cv2.putText(blurred_image, str(right_elbow), 
                        tuple(np.multiply(rightelbow, [640, 480]).astype(int)), 
                        cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 2, cv2.LINE_AA)
            cv2.putText(blurred_image, str(right_shoulder), 
                        tuple(np.multiply(rightshoulder, [640, 480]).astype(int)), 
                        cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 2, cv2.LINE_AA)
            cv2.putText(blurred_image, str(left_wrist), 
                        tuple(np.multiply(leftwrist, [640, 480]).astype(int)), 
                        cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 2, cv2.LINE_AA)
            cv2.putText(blurred_image, str(right_wrist), 
                        tuple(np.multiply(rightwrist, [640, 480]).astype(int)), 
                        cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 2, cv2.LINE_AA)
            
            cv2.putText(blurred_image, str(left_knee), 
                        tuple(np.multiply(leftknee, [640, 480]).astype(int)), 
                        cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 2, cv2.LINE_AA)
            cv2.putText(blurred_image, str(right_knee), 
                        tuple(np.multiply(rightknee, [640, 480]).astype(int)), 
                        cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 2, cv2.LINE_AA)
            cv2.putText(blurred_image, str(neck_angle), 
                        tuple(np.multiply(neck_position, [640, 480]).astype(int)), 
                        cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 2, cv2.LINE_AA)
            cv2.putText(blurred_image, str(mid_hip_angle), 
                        tuple(np.multiply(mid_hip, [640, 480]).astype(int)), 
                        cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 2, cv2.LINE_AA)
            print(landmarks)
            
            # Get current time
            current_time = datetime.now().strftime("%Y-%m-%d %H:%M:%S")

            # Write data to CSV
            writer.writerow([current_time, left_elbow, left_shoulder, right_elbow, right_shoulder, left_wrist, right_wrist, mid_hip, neck_angle, right_knee, left_knee])

        except Exception as e:
            print(f"Error: {e}")
            pass

            # Apply low-pass filter to smooth the angle
            if prev_angle is not None:
                angle_filtered = alpha * angle + (1 - alpha) * prev_angle
            else:
                angle_filtered = angle

            prev_angle = angle_filtered

            # Get current time
            current_time = datetime.now().strftime("%Y-%m-%d %H:%M:%S")

            # Write data to CSV
            writer.writerow([current_time, angle_filtered, ...])  # Écrivez les autres données nécessaires

        except Exception as e:
            print(f"Error: {e}")
            pass

        # Render detection
        if results.pose_landmarks:
            mp_drawing.draw_landmarks(blurred_image, results.pose_landmarks, mp_pose.POSE_CONNECTIONS,
                                      mp_drawing.DrawingSpec(color=(245, 117, 66), thickness=2, circle_radius=2),
                                      mp_drawing.DrawingSpec(color=(245, 66, 230), thickness=2, circle_radius=2)
                                      )

        cv2.imshow('Mediapipe Feed', blurred_image)

        if cv2.waitKey(1) & 0xFF == ord('q'):
            break

pipeline.stop()
cv2.destroyAllWindows()