chayatat ratanasawanya min he july 21, 2010. the goal experimental setup posit algorithm ...
TRANSCRIPT
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