UAV pose estimationusing POSIT algorithm

Chayatat RatanasawanyaMin He

July 21, 2010

The goal Experimental setup POSIT algorithm Homogeneous transformation Optitrack system Result calculation Results Conclusion Questions/comments

Overview

To be able to know the pose of the UAV from images taken by the on-board camera. Use POSIT algorithm in a part of the

process Calculated pose is compared to the

reading from the Optitrack system.

Experimental goal

Test setup

Move the Q-ball around the test area in 17 different locations

Q-ball pose is different in each location For each location, take a picture using on-

board camera and record Optitrack pose reading

Process the pictures offline using POSIT algorithm. Calculate Q-ball pose using homogeneous transformation and inverse kinematics.

Results are compared to Optitrack readings.

Test setup

Developers: Daniel DeMenthon & Philip David

The algorithm determines the pose of an object relative to the camera from a set of 2D image points

POSIT algorithm

Reference: http://www.cfar.umd.edu/~daniel/SoftPOSIT.txt

POSIT

Image coordinates of min. 4 non-coplanar

feature points3D world

coordinates of the same points

Camera intrinsic

parameters

Rotation matrix of object wrt.

cameraTranslation of

object wrt. camera

Homogeneous transformation is a matrix which shows how one coordinate frame is related to another.

It is used to convert the location of a point between two frames.

Homogeneous transformation

1000zzzz

yyyy

xxxx

AC

daon

daon

daon

T

y

x

z

Frame C

y x

z

Frame A

(dx, dy, dz)

pTp AA

CC

Multiplication:

Inverse:

Homogeneous transformation

y

x

z

Frame C

y x

z

Frame A

AB

BC

AC TTT

y

x

z

Frame B

CTB

BTA

1 CA

AC TT

The process of deriving the transformation matrix from a known transformation (rotation and translation) between two frames

Forward kinematics

cs

sc

cs

sc

cs

sc

RAC

0

0

001

0

010

0

100

0

0

y

x

z

Frame C

y x

z

Frame A

(dx, dy, dz)

y

x

z

Frame A

θ

ψy x

z

Frame A

1000zzzz

yyyy

xxxx

AC

daon

daon

daon

T

The process of deriving the transformation (rotation and translation) between two frames from a known transformation matrix

Inverse kinematics

1000zzzz

yyyy

xxxx

AC

daon

daon

daon

TTranslation

Inverse kinematics formula

Rotation angles

A motion capture system It tracks the movement of IR reflectors

attached to an object in the workspace using six IR cameras

Origin of the workspace (world) coordinates has to be set up during system calibration

Point cloud mode: gives x,y,z-coordinates of individual reflector in a group

Trackable mode: gives pose of an object defined by a group of reflectors

Optitrack system

Q-ball trackable object

Result calculation

y

x

z

World frame, W

y

x

z

Box frame, B

yx

z

Q-ball frame, Q

z x

yCam frame, C

CTB

1000zzzz

yyyy

xxxx

BC

daon

daon

daon

T

POSIT

y

x

zBox frame, B

Result calculation

y

x

z

World frame, W

y

x

z

Box frame, B

yx

z

Q-ball frame, Q

z x

yCam frame, C

WTB

cmTBW

1000

6727.171100

1261.59010

6038.18001

Result calculation

y

x

z

World frame, W

y

x

z

Box frame, B

yx

z

Q-ball frame, Q

z x

yCam frame, C

QTC

cmTCQ

1000

385.32100

1275.4010

0001

Result calculation

y

x

z

World frame, W

y

x

z

Box frame, B

yx

z

Q-ball frame, Q

z x

yCam frame, C

QTCWTB

CTB

11

CQ

BC

BW

QW

BC

CQ

QW

BW

TTTT

TTTT

Translation

Inverse kinematics formula

Rotation angles

Experiment Optitrack measurements of Q-ball pose Calculation of Q-ball pose using POSIT algorithmx (cm) y (cm) z (cm) roll (deg) yaw (deg) pitch (deg) x (cm) y (cm) z (cm) roll (deg) yaw (deg) pitch (deg)

1 -55.92 78.27 -33.51 -6.432 -11.43 -8.805 -56.9388 88.6125 -31.7526 -2.1859 -11.8127 -12.19352 -46.26 63.91 -32.31 -1 -9.369 -0.6592 -46.1600 71.5917 -31.5440 0.8890 -8.4284 -3.59443 -66.03 66.32 17.57 -10.48 -20.46 0.028 -63.2006 51.1865 15.7156 -9.7964 -18.8214 7.82114 3.569 103.7 -40.82 -5.253 0.423 -11.02 18.7217 109.0871 -47.2341 -6.7481 5.8695 -14.70755 32.92 103.2 -54.8 -1.326 20.29 -11.55 29.4440 109.7598 -55.9636 -5.4079 17.9145 -15.65766 61.71 103.1 -65.01 12.11 31.45 -12.75 62.8314 109.1026 -70.7933 3.4818 34.5913 -12.19207 82.7 102.2 -70.74 13.1 41.74 -13.25 85.1300 104.3310 -78.3997 5.1808 47.0439 -6.51388 23.1 103.5 -40.13 9.587 16.73 -12.26 34.8409 108.5184 -48.2129 4.2641 23.8152 -12.61859 -53.57 92.41 -52.75 -5.341 -16.65 -12.19 -49.9633 98.3089 -54.4165 -0.7202 -15.7037 -14.3379

10 -46.95 92.62 -72.83 -1.814 -25.09 -13.02 -45.4508 86.6950 -71.7959 3.3547 -23.8781 -10.278511 -41.74 92.17 -65.05 1.685 -10.55 -10.03 -38.9600 98.1515 -65.5449 3.8103 -8.2466 -13.245512 -62.84 103.6 -9.212 1.721 -10.9 -11.78 -68.0221 105.1375 -11.2639 5.5375 -11.9621 -13.338813 -59.93 100.9 -2.383 -0.6124 -5.947 0.5845 -66.7789 103.5136 5.9820 -1.1543 -7.3512 0.647814 -54.06 66.81 -5.23 6.3 -9.559 11.55 -53.8803 83.6447 -2.1709 3.7949 -9.1901 5.518815 -60.56 76.76 -18.05 0.1576 -24.6 1.029 -60.5642 76.0576 -18.4691 -0.3815 -24.2316 1.376316 -53.01 76.78 -26.84 -0.9526 -23.04 0.681 -56.0662 83.8747 -28.5519 1.6016 -23.9035 -2.340217 -43.46 79.25 -40.21 -8.653 -10.15 -9.22 -46.0110 87.4031 -41.7292 -4.6202 -12.5088 -11.3917

Results

Results: Translational DOF

1 3 5 7 9 11 13 15 17

-100

-80

-60

-40

-20

0

20

40

Q-ball pose: z-position (cm)

Measured valueCalculated value

1 3 5 7 9 11 13 15 170

20

40

60

80

100

120

Q-ball pose: y-position (cm)

Measured value

1 3 5 7 9 11 13 15 17

-80-60-40-20

020406080

100

Q-ball pose: x-position (cm)

Measured valueCalculated value

Results: Rotational DOF

1 3 5 7 9 11 13 15 17

-15

-10

-5

0

5

10

15

Q-ball pose: roll angle (deg)

Measured valueCalculated value

1 3 5 7 9 11 13 15 17

-30-20-10

0102030405060

Q-ball pose: yaw angle (deg)

Measured valueCalculated value

1 3 5 7 9 11 13 15 17

-20

-15

-10

-5

0

5

10

15

Q-ball pose: pitch angle (deg)

Measured valueCalculated value

Sources of error: Optitrack measurement accuracy of 4 cm Imaginary c.g. of Q-ball trackable object does not

correspond exactly to c.g. of Q-ball used to define QTC

Results: Error

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 170.0000

2.0000

4.0000

6.0000

8.0000

10.0000

12.0000

14.0000

16.0000

18.0000

Error

x (cm)y (cm)z (cm)roll (deg)yaw (deg)pitch (deg)

Max. Error x = 15 cmMax. Error y = 17 cmMax. Error z = 8 cmMax. Error roll = 8.5 degMax. Error yaw = 7 degMax. Error pitch = 8 deg

POSIT algorithm can be used to estimate the pose of the UAV offline

A 3D object of known dimension must be in the scene

At least 4 non-coplanar feature points must be seen in image.

Position of the object in the world frame must be known (frame WTB)

Conclusion

Experimental goal and setup POSIT algorithm Results of POSIT & homogeneous

transformation Optitrack system How to calculate the result Test results

Summary

Thank you