Coordinate conversion error

Software Python
1

Hi！
I want to convert the reference frame of the point cloud from the camera coordinate system to the floor, as shown in the image below:

IMG20231015161618_page-0001
IMG20231015161618_page-00011107×736 138 KB

But my following code doesn’t seem to do that, specifically where went wrong

import sys
import pyzed.sl as sl


def main():
    zed = sl.Camera()

    # Set configuration parameters
    init = sl.InitParameters()
    init.depth_mode = sl.DEPTH_MODE.ULTRA
    init.coordinate_units = sl.UNIT.METER
    init.camera_resolution = sl.RESOLUTION.HD1080

    # If applicable, use the SVO given as parameter
    # Otherwise use ZED live stream
    if len(sys.argv) == 2:
        filepath = sys.argv[1]
        print("Reading SVO file: {0}".format(filepath))
        init.set_from_svo_file(filepath)

    # Open the camera
    status = zed.open(init)
    if status != sl.ERROR_CODE.SUCCESS:
        print(repr(status))
        exit(1)

    cloud = sl.Mat()  # current left image
    pose = sl.Pose()  # positional tracking data
    plane = sl.Plane()  # detected plane


    zed.enable_positional_tracking()
    runtime_parameters = sl.RuntimeParameters()

    retrieve_cloud_state = False
    reset_tracking_floor_frame = sl.Transform()
    
    while True:
        if zed.grab(runtime_parameters) == sl.ERROR_CODE.SUCCESS:
            tracking_state = zed.get_position(pose)

            if tracking_state == sl.POSITIONAL_TRACKING_STATE.OK:
                find_plane_status = zed.find_floor_plane(plane, reset_tracking_floor_frame)
                
                if find_plane_status == sl.ERROR_CODE.SUCCESS:
                    resetTrackingFloorFrame_inverse = sl.Transform()
                    reset_tracking_floor_frame.inverse_mat(resetTrackingFloorFrame_inverse)
                    zed.reset_positional_tracking(resetTrackingFloorFrame_inverse)
                    retrieve_cloud_state = True
                    break

    if zed.grab(runtime_parameters) == sl.ERROR_CODE.SUCCESS:
        if retrieve_cloud_state == True:
            zed.retrieve_measure(cloud, sl.MEASURE.XYZRGBA)
            cloud.write('Pointcloud.ply')
            print("successfully save Pointcloud{}.ply")

    zed.disable_positional_tracking()
    zed.close()

if __name__ == "__main__":
    main()

Thank you！

2

Can someone teach me？

3

Can someone help me with this？

4

Hi,

You are not doing a conversion here. You just reset the positional tracking while giving a initial position in the world to the camera.If you want to change reference, you need to apply a conversion matrix to your coordinates. You can find documentation about that on the internet. For example here.