L515 : from depth frame pixel to color frame pixel coordinate issue

#####################################################
##               Read bag from file                ##
#####################################################
# -*- coding: utf-8 -*-
# First import library
import pyrealsense2 as rs
# Import Numpy for easy array manipulation
import numpy as np
# Import OpenCV for easy image rendering
import cv2
# Import argparse for command-line options
import argparse
# Import os.path for file path manipulation
import os.path
import math
from scipy.ndimage import generic_filter
from time import sleep
from ultralytics import SAM
def draw_distance_with_arrows(image, p1, p2, text, color):
    midpoint = ((p1[0] + p2[0]) // 2, (p1[1] + p2[1]) // 2)
    cv2.arrowedLine(image, midpoint, ((p1[0] + midpoint[0]) // 2, (p1[1] + midpoint[1]) // 2), color, 2, tipLength=0.3)
    cv2.putText(image, text, (midpoint[0] + 10, midpoint[1] - 10), cv2.FONT_HERSHEY_SIMPLEX, 0.6, color, 2)

def draw_angle(image, p1, p2, p3, angle, color,t):
    # Draw lines from p2 to p1 and p2 to p3
    #cv2.line(image, p2, p1, color, 1)
    #cv2.line(image, p2, p3, color, 1)
    
    # Draw the arc representing the angle inside
    radius = 60  # Adjust the radius to fit inside
    start_angle = np.degrees(np.arctan2(p1[1] - p2[1], p1[0] - p2[0]))
    end_angle = np.degrees(np.arctan2(p3[1] - p2[1], p3[0] - p2[0]))
  
    
    #Ensure the arc is drawn clockwise
    if t==True :
      #start_angle -= 360
      start_angle = end_angle  + 90
      #start_angle = -1*start_angle +360
    center = tuple(p2)
    cv2.ellipse(image, center, (radius, radius), 0, end_angle, start_angle, color, 2)
    
    # Display the angle value near the arc
    angle_text = f"{angle:.1f}°"
    text_position = (p2[0] - radius // 2, p2[1] - radius // 2)
    cv2.putText(image, angle_text, text_position, cv2.FONT_HERSHEY_SIMPLEX, 0.6, color, 2)

# Function to calculate the angle between three points (p1, p2, p3)
def calculate_angle(p1, p2, p3):
    v1 = np.array( [p1[0] - p2[0], p1[1] - p2[1], p1[2] - p2[2] ] )
    v2 = np.array([p3[0] - p2[0], p3[1] - p2[1],p3[2] - p2[2]] )
    angle_rad = np.arccos(np.dot(v1, v2) / (np.linalg.norm(v1) * np.linalg.norm(v2)))
    angle_deg = np.degrees(angle_rad)
    return angle_deg

def calculate_distance(p1, p2):
    # Euclidean distance formula
    return math.sqrt((p2[0] - p1[0]) ** 2 + (p2[1] - p1[1]) ** 2 + (p2[2] - p1[2]) ** 2 + (point2c[2] - point1c[2])**2)

# Initialize selected_points as an empty list to store selected points
selected_points = []

def reorder_rectangle_points(points):
    # Convert points to a list of tuples and sort by Y-coordinate
    points = sorted(points, key=lambda x: x[1])  # Sort by vertical position (Y)
    
    # Separate into top and bottom points
    top_points = sorted(points[:2], key=lambda x: x[0])  # Sort top points by X
    bottom_points = sorted(points[2:], key=lambda x: x[0])  # Sort bottom points by X
    
    # Return points in order: top-left, top-right, bottom-right, bottom-left
    return np.array([top_points[0], top_points[1], bottom_points[1], bottom_points[0]])

def mouse_callback(event, x, y, flags, param):
    global selected_points
    # Scale back the coordinates to match the original registered frame
    x_original = int(x )
    y_original = int(y )
    
    # Left click to add a point
    if event == cv2.EVENT_LBUTTONDOWN:
        if len(selected_points) == 2:
            selected_points.pop(0)  # Keep only the latest two points
        selected_points.append((x_original, y_original))
    # Right click to remove the last point
    elif event == cv2.EVENT_RBUTTONDOWN and selected_points:
        selected_points.pop()

# Create object for parsing command-line options
parser = argparse.ArgumentParser(description="Read recorded bag file and display depth stream in jet colormap.\
                                Remember to change the stream fps and format to match the recorded.")
# Add argument which takes path to a bag file as an input
parser.add_argument("-i", "--input", type=str, help="C:/Users/adnen/OneDrive/Documents/20250108_165104.bag")
# Parse the command line arguments to an object
args = parser.parse_args()

# Safety if no parameter have been given

try:
    # Create pipeline
    
    #align_to = rs.stream.color
   #align = rs.align(align_to)
    pipeline = rs.pipeline()

    # Create a config object
    config= rs.config()
    # Tell config that we will use a recorded device from file to be used by the pipeline through playback.
    rs.config.enable_device_from_file(config, "C:/Users/adnen/OneDrive/Documents/20250108_165104.bag")
    #config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)
    
    temporal_filter = rs.temporal_filter()
    spatial_filter = rs.spatial_filter()
    temporal_smooth_alpha = 0.63
    temporal_smooth_delta = 55
    persistency_index = 8
    temporal_filter.set_option(rs.option.filter_smooth_alpha, temporal_smooth_alpha)
    temporal_filter.set_option(rs.option.filter_smooth_delta, temporal_smooth_delta)
    temporal_filter.set_option(rs.option.holes_fill, persistency_index)
    
    
                            
    
    
    
    
    
    
    profile = config.resolve(pipeline)
   
   
    
    
    

    # Start streaming from file
    pipeline.start(config)

    # Create opencv window to render image in
    cv2.namedWindow("Depth Stream", cv2.WINDOW_AUTOSIZE)
    cv2.setMouseCallback("Depth Stream", mouse_callback)
    
    # Create colorizer object
    colorizer = rs.colorizer()
    colorizer.set_option(rs.option.min_distance,2.1)
    colorizer.set_option(rs.option.max_distance,3.3)
    colorizer.set_option(rs.option.color_scheme,4)

    # Streaming loop
    while True:
        # Get frameset of depth
        frames = pipeline.wait_for_frames()

        # Get depth frame
        depth_frame = frames.get_depth_frame()
        color_frame = frames.get_color_frame()
        #aligned_frames = align.process(frames)
        #color_frame = aligned_frames.get_color_frame()
        #depth_frame = aligned_frames.get_depth_frame()
        
        depth_intrinsics = depth_frame.profile.as_video_stream_profile().intrinsics
        # Get the color frame intrinsics
        color_intrinsics = color_frame.profile.as_video_stream_profile().intrinsics
        
        color_image = np.asanyarray(color_frame.get_data())
        depth_frame = temporal_filter.process(depth_frame)

        # Colorize depth frame to jet colormap
        depth_color_frame = colorizer.colorize(depth_frame)
        #depth_frame = hole_filling_filter.process(depth_frame)
        
        
        depth_data = np.asanyarray(depth_frame.get_data())
        # Convert depth_frame to numpy array to render image in opencv
        depth_color_image = np.asanyarray(depth_color_frame.get_data())

        
        # Convert the image to HSV color space
        hsv = cv2.cvtColor(depth_color_image, cv2.COLOR_BGR2HSV)

        # Define the range for the greenish color
        lower_green = np.array([35, 50, 50])  # Adjust these values based on the green shade
        upper_green = np.array([85, 255, 255])  # Adjust these values based on the green shade

        # Create a mask for the green color
        mask = cv2.inRange(hsv, lower_green, upper_green)

        # Apply the mask to the original image
        depth_color_image = cv2.bitwise_and(depth_color_image, depth_color_image, mask=mask)
        contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
        if contours:
            # Find the largest contour (assuming the green shape is the largest green object)
            largest_contour = max(contours, key=cv2.contourArea)

            # Approximate the contour to a polygon
            epsilon = 0.02 * cv2.arcLength(largest_contour, True)
            approx = cv2.approxPolyDP(largest_contour, epsilon, True)

            # Check if the approximated contour has 4 points (rectangle)
            if len(approx) == 4:
                
                rectangle_points = approx.reshape(4,2)
                reordered_points = reorder_rectangle_points(rectangle_points)
                rectangle_points=reordered_points
                p1 = rectangle_points[0]
                p2 = rectangle_points[1]
                p3 = rectangle_points[2]
                p4 = rectangle_points[3]
                depth1 =  depth_data[p1[1], p1[0]] * 0.00025  # (y, x) order for depth frame
                depth2 =  depth_data[p2[1], p2[0]] * 0.00025
                depth3 =  depth_data[p3[1], p3[0]] * 0.00025
                depth4 =  depth_data[p4[1], p4[0]] * 0.00025
                print(depth1)
                point1c = rs.rs2_deproject_pixel_to_point(depth_intrinsics, [p1[0], p1[1]], depth1*1000)
                point2c = rs.rs2_deproject_pixel_to_point(depth_intrinsics, [p2[0], p2[1]], depth2*1000)
                point3c = rs.rs2_deproject_pixel_to_point(depth_intrinsics, [p3[0], p3[1]], depth3*1000)
                point4c = rs.rs2_deproject_pixel_to_point(depth_intrinsics, [p4[0], p4[1]], depth4*1000)
                
                distance1 = math.sqrt((point2c[0] - point1c[0])**2 +(point2c[1] - point1c[1])**2 + (point2c[2] - point1c[2])**2)
                distance2 = math.sqrt((point3c[0] - point2c[0])**2 +(point3c[1] - point2c[1])**2 + (point3c[2] - point2c[2])**2)
                distance3 = math.sqrt((point4c[0] - point3c[0])**2 +(point4c[1] - point3c[1])**2 + (point4c[2] - point3c[2])**2)
                distance4 = math.sqrt((point4c[0] - point1c[0])**2 +(point4c[1] - point1c[1])**2 + (point4c[2] - point1c[2])**2)

                #color_image=cv2.resize(color_image,(680,420),cv2.INTER_AREA)
                
                
                p1x=rs.rs2_project_point_to_pixel(color_intrinsics, point1c)
                p2x=rs.rs2_project_point_to_pixel(color_intrinsics, point2c)
                p3x=rs.rs2_project_point_to_pixel(color_intrinsics, point3c)
                p4x=rs.rs2_project_point_to_pixel(color_intrinsics, point4c)
                print(p1x)
                print([p1[0], p1[1]])
                p1x[1]=+45
                p1x[0]=int(p1x[0])-10
                
                p2x[0]=int(p2x[0]) -10
                p2x[1]=int(p2x[1]) +36
                p3x[0]=int(p3x[0])-10
                p3x[1]=int(p3x[1])+ +36
                p4x[0]=int(p4x[0])-10
                p4x[1]=int(p4x[1])+36
                
                
                #print(p2x)
                #print(p3x)
                #print(p4x)
                # Draw arrows and distances
                depth_color_image=color_image
                draw_distance_with_arrows(depth_color_image, p1x, p2x, f"{distance1:.3f} mm", (0, 0, 255))  # Green arrow for distance1
                draw_distance_with_arrows(depth_color_image, p2x, p3x, f"{distance2:.3f} mm", (255, 0, 0))  # Blue arrow for distance2
                draw_distance_with_arrows(depth_color_image, p3x, p4x, f"{distance3:.3f} mm", (0, 255, 255))  # Yellow arrow for distance3
                draw_distance_with_arrows(depth_color_image, p4x, p1x, f"{distance4:.3f} mm", (255, 255, 0))  # Cyan arrow for distance4
                # Calculate angles at each corner
                angle1 = calculate_angle(point4c,point1c, point2c)
                angle2 = calculate_angle(point1c, point2c, point3c)
                angle3 = calculate_angle(point2c, point3c, point4c)
                angle4 = calculate_angle(point3c, point4c, point1c)

                # Draw angles on the image
                draw_angle(depth_color_image, p4x, p1x, p2x, angle1, (0, 0, 255),False)  # Red
                draw_angle(depth_color_image, p1x, p2x, p3x, angle2, (0, 255, 255),True)  # Green
                draw_angle(depth_color_image, p2x, p3x, p4x, angle3, (255, 0, 0),False)  # Blue
                draw_angle(depth_color_image, p3x, p4x, p1x, angle4, (255, 255, 0),False)  # Yellow

                # Draw the rectangle points on the original image
                for f,point in enumerate(rectangle_points):
                    if f==0 :
                     cv2.circle(depth_color_image, tuple(p1x), 10, (0, 0, 255), -1)  # red points
                    if f==1 :
                     cv2.circle(depth_color_image, tuple(p2x), 10, (255, 0, 0), -1)  # blue points
                    if f==2 :
                     cv2.circle(depth_color_image, tuple(p3x), 10, (0, 255, 255), -1)  # yellow points
                    if f==3 :
                     cv2.circle(depth_color_image, tuple(p4x), 10, (255, 255, 0), -1)  # purple points 
                 #Draw the contour
                #cv2.drawContours(depth_color_image, [approx], -1, (0, 0, 255), 2)  # Red contour
            else:
                print("The shape is not a rectangle.")
        
        
        cv2.imshow("Depth Stream", depth_color_image)
        #cv2.imshow("Depth Stream", depth_color_image)
        
        key = cv2.waitKey(1)
        # if pressed escape exit program
        if key == 27:
            cv2.destroyAllWindows()
            break

finally:
    pass