# ZED Positional Tracking

###### tags: `視覺組`

> [name=沈尚緯([e-mail])]

The ZED API returns pose(position and orientation) information for each frame.

## Getting Pose
- position: the location of the camera in space is available as a vector [X, Y, Z]. Its norm represents the total distance traveled between the current camera position and the reference coordinate frame.
- orientation: the orientation of the camera in space is available as a quaternion [X, Y, Z, W].

## Convert Quaternion to Euler

[explanation](https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles)

roll = math.degrees(math.atan2(2*(ow*ox + oy*oz), 1-2*(ox*ox + oy*oy)))
pitch = math.degrees(math.asin(2*(ow*oy - ox*oz)))
yaw = math.degrees(math.atan2(2*(ow*oz + ox*oy), 1-2*(oy*oy + oz*oz)))

## Coordinate System

currently using `COORDINATE_SYSTEM_LEFT_HANDED_Z_UP`


## Example code

print ZED's pose and roll, pitch, yaw during 1000 frames

import math
import pyzed.sl as sl

def main():
    # Create a Camera object
    zed = sl.Camera()

    # Create a InitParameters object and set configuration parameters
    init_params = sl.InitParameters()
    init_params.camera_resolution = sl.RESOLUTION.HD720  # Use HD720 video mode (default fps: 60)
    # Use a left-handed Z-up coordinate system
    init_params.coordinate_system = sl.COORDINATE_SYSTEM.LEFT_HANDED_Z_UP
    init_params.coordinate_units = sl.UNIT.CENTIMETER  # Set units

    # Open the camera
    err = zed.open(init_params)
    if err != sl.ERROR_CODE.SUCCESS:
        exit(1)

    # Enable positional tracking with default parameters
    py_transform = sl.Transform()  # First create a Transform object for TrackingParameters object
    tracking_parameters = sl.PositionalTrackingParameters(_init_pos=py_transform)
    err = zed.enable_positional_tracking(tracking_parameters)
    if err != sl.ERROR_CODE.SUCCESS:
        exit(1)

    # Track the camera position during 1000 frames
    i = 0
    zed_pose = sl.Pose()

    runtime_parameters = sl.RuntimeParameters()
    
    while i < 1000:
        if zed.grab(runtime_parameters) == sl.ERROR_CODE.SUCCESS:
            # Get the pose of the left eye of the camera with reference to the world frame
            zed.get_position(zed_pose, sl.REFERENCE_FRAME.WORLD)

            # Display the translation and timestamp
            py_translation = sl.Translation()
            tx = round(zed_pose.get_translation(py_translation).get()[0], 3)
            ty = round(zed_pose.get_translation(py_translation).get()[1], 3)
            tz = round(zed_pose.get_translation(py_translation).get()[2], 3)
            # print("Translation: Tx: {0}, Ty: {1}, Tz {2}, Timestamp: {3}".format(tx, ty, tz, zed_pose.timestamp.get_milliseconds()))
            print("Translation: Tx: {0}, Ty: {1}, Tz {2}".format(tx, ty, tz))

            # Display the orientation quaternion
            py_orientation = sl.Orientation()
            ox = zed_pose.get_orientation(py_orientation).get()[0]
            oy = zed_pose.get_orientation(py_orientation).get()[1]
            oz = zed_pose.get_orientation(py_orientation).get()[2]
            ow = zed_pose.get_orientation(py_orientation).get()[3]
            print("Orientation: Ox: {0}, Oy: {1}, Oz {2}, Ow: {3}".format(round(ox, 3), round(oy, 3), round(oz, 3), round(ow, 3)))

            roll = math.degrees(math.atan2(2*(ow*ox + oy*oz), 1-2*(ox*ox + oy*oy)))
            pitch = math.degrees(math.asin(2*(ow*oy - ox*oz)))
            yaw = math.degrees(math.atan2(2*(ow*oz + ox*oy), 1-2*(oy*oy + oz*oz)))
            print("roll: {0}\npitch: {1}\nyaw: {2}".format(round(roll, 3), round(pitch, 3), round(yaw, 3)))
            
            i = i + 1

    # Close the camera
    zed.close()

if __name__ == "__main__":
    main()

## Publish the Pose to ROS

[GitHub Repo](https://github.com/NCTU-AUV/pose)

## Accuracy Tests

將ZED實際正轉與逆轉90°的測量值

roll
- 90.67°
- 90.52°
- 91.14°
- -90.64°
- -90.43°
- -90.13°

pitch轉到90°附近時（座標系統原點）roll和yaw角度會失準，但在90°以下都正常，不影響實際使用。

yaw 90°
- 88.54°
- 88.71°
- 89.45°
- -89.17°
- -88.91°
- -89.02°

yaw 60°
- 60.55°
- 60.00°
- 61.43°
- -58.65°
- -59.93°
- -62.91°

yaw 30°
- -29.17°
- -29.47°
- -30.77°
- 31.33°
- 31.91°
- 30.77°