argument 4: <class 'TypeError'>: expected LP__CameraSpacePoint instance instead of LP__CameraSpacePoint

[sean195114]

this is my code , and it always has this problem. Can someone help me, many thx.

import comtypes
import ctypes
import _ctypes
from ctypes import sizeof, alignment
from ctypes import POINTER, c_ushort
import pygame
import numpy
import sys
import cv2
import numpy as np
import math

from pykinect2 import PyKinectV2
from pykinect2.PyKinectV2 import *
from pykinect2 import PyKinectRuntime
import mapper
import time
time.clock = time.time

Initialize Kinect

kinect = PyKinectRuntime.PyKinectRuntime(PyKinectV2.FrameSourceTypes_Color | PyKinectV2.FrameSourceTypes_Depth)

class _CameraSpacePoint(ctypes.Structure):
fields = [
('x', ctypes.c_float),
('y', ctypes.c_float),
('z', ctypes.c_float),
]
def depth_space_2_world_depth(depth_map, x, y):
"""
Return depth of object given the depth map coordinates
:param depth_map: kinect.get_last_depth_frame
:param x: depth pixel x
:param y: depth pixel y
:return: depth z of object
"""
if int(y) * 512 + int(x) <= 512 * 424:
return float(depth_map[int(y) * 512 + int(x)]) # mm
else:
# If it exceeds return the last value to catch overflow
return float(depth_map[512*424])

def color_2_world(kinect, depth_frame_data, camera_space_point, as_array=False):
"""
Map Color Frame to World Space
:param kinect: Class for main file
:param depth_frame_data: kinect._depth_frame_data
:param camera_space_point: _CameraSpacePoint structure from PyKinectV2
:param as_array: returns frame as numpy array
:return: returns mapped color frame to camera space
COMMETHOD([], HRESULT, 'MapColorFrameToCameraSpace',
( [], c_uint, 'depthDataPointCount' ),
( ['in'], POINTER(c_ushort), 'depthFrameData' ),
( [], c_uint, 'cameraPointCount' ),
( [], POINTER(_CameraSpacePoint), 'cameraSpacePoints' )),
"""
color2world_points_type = _CameraSpacePoint * np.int(1920 * 1080)
color2world_points = ctypes.cast(color2world_points_type(), ctypes.POINTER(_CameraSpacePoint))
kinect._mapper.MapColorFrameToCameraSpace(ctypes.c_uint(512 * 424), kinect._depth_frame_data, ctypes.c_uint(1920 * 1080), color2world_points)
pf_csps = ctypes.cast(color2world_points, ctypes.POINTER(ctypes.c_float))
data = np.ctypeslib.as_array(pf_csps, shape=(1080, 1920, 3))
if not as_array:
return color2world_points
else:
return data

camera_space_point = _CameraSpacePoint()

while True:
# Capture color image and depth information
if kinect.has_new_color_frame() and kinect.has_new_depth_frame() :
# Get color image
frame = kinect.get_last_color_frame()
frame = frame.reshape((kinect.color_frame_desc.Height, kinect.color_frame_desc.Width, 4)).astype(np.uint8)

    # Get depth value of object from depth information
    depth_frame = kinect.get_last_depth_frame()
    depth_image = depth_frame.reshape((kinect.depth_frame_desc.Height, kinect.depth_frame_desc.Width, 1)).astype(np.uint8)

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

    # Define the HSV range for blue color
    lower_blue = np.array([100, 100, 100])
    upper_blue = np.array([140, 255, 255])

    # Build a mask to mark the blue parts
    mask = cv2.inRange(hsv, lower_blue, upper_blue)

    # Find contours of the blue object
    contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)

    # If contours are found, display them on the image
    if contours:
        # Draw each contour
        for cnt in contours:
            cv2.drawContours(frame, [cnt], 0, (0, 255, 0), 2)
            M = cv2.moments(cnt)
            if M["m00"] != 0:
                cx = int(M["m10"] / M["m00"])
                cy = int(M["m01"] / M["m00"])

                depth_point = depth_space_2_world_depth(kinect.get_last_depth_frame(), kinect.depth_frame_desc.Width // 2 , kinect.depth_frame_desc.Height // 2)
                
                # Set the values of _CameraSpacePoint structure
                camera_space_point.x = cx
                camera_space_point.y = cy
                camera_space_point.z = depth_point
                
                # Pass the pointer to color_2_world function
                camera_color_point = color_2_world(kinect, depth_frame, camera_space_point, as_array=False)
                

                # Get the corresponding world coordinates from the mapped result
                x_world = camera_color_point.contents.x
                y_world = camera_color_point.contents.y
                z_world = camera_color_point.contents.z

                # Ensure the visibility and clarity of text position and color
                cv2.putText(frame, f'X: {x_world:.2f} Y: {y_world:.2f} Z: {z_world:.2f}', (10, 50), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 0), 2)

    # Display the color and depth images
    cv2.imshow('Color Frame', frame)
    depth_colormap = cv2.applyColorMap(cv2.convertScaleAbs(depth_image, alpha=0.03), cv2.COLORMAP_JET)
    cv2.imshow('Depth Frame', depth_colormap)
    
    # Press 'q' to exit the loop
    if cv2.waitKey(1) & 0xFF == ord('q'):
        break

Release the Kinect and close the windows

kinect.close()
cv2.destroyAllWindows()

ArgumentError Traceback (most recent call last)
in
111
112 # Pass the pointer to color_2_world function
--> 113 camera_color_point = color_2_world(kinect, depth_frame, camera_space_point, as_array=False)
114
115

in color_2_world(kinect, depth_frame_data, camera_space_point, as_array)
57 color2world_points_type = _CameraSpacePoint * np.int(1920 * 1080)
58 color2world_points = ctypes.cast(color2world_points_type(), ctypes.POINTER(_CameraSpacePoint))
---> 59 kinect._mapper.MapColorFrameToCameraSpace(ctypes.c_uint(512 * 424), kinect._depth_frame_data, ctypes.c_uint(1920 * 1080), color2world_points)
60 pf_csps = ctypes.cast(color2world_points, ctypes.POINTER(ctypes.c_float))
61 data = np.ctypeslib.as_array(pf_csps, shape=(1080, 1920, 3))

ArgumentError: argument 4: <class 'TypeError'>: expected LP__CameraSpacePoint instance instead of LP__CameraSpacePoint

def color_2_world(kinect, depth_frame_data, camera_space_point, as_array=False): I think the error happens due to here!