search

IntelRealSense / librealsense

Intel® RealSense™ SDK
https://www.intelrealsense.com/
Apache License 2.0
7.43k stars 4.8k forks source link

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

Closed MyOtherNamesWereTaken closed 2 years ago

MyOtherNamesWereTaken commented 2 years ago
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 commented 2 years ago

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 https://github.com/IntelRealSense/librealsense/issues/3917#issuecomment-512336371 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 https://github.com/IntelRealSense/librealsense/issues/8692#issuecomment-809287786

MartyG-RealSense commented 2 years ago

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

MyOtherNamesWereTaken commented 2 years ago

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 commented 2 years ago

As you started another case with a new approach in https://github.com/IntelRealSense/librealsense/issues/10092 do you still require assistance with this case here, please?

MyOtherNamesWereTaken commented 2 years ago

U can close this case, thanks

MartyG-RealSense commented 2 years ago

Thanks very much @MyOtherNamesWereTaken

eya8sahli commented 2 months ago

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()