![Page 1: Chayatat Ratanasawanya Min He July 21, 2010. The goal Experimental setup POSIT algorithm Homogeneous transformation Optitrack system Result](https://reader038.vdocuments.us/reader038/viewer/2022103005/56649cf85503460f949c9190/html5/thumbnails/1.jpg)
UAV pose estimationusing POSIT algorithm
Chayatat RatanasawanyaMin He
July 21, 2010
![Page 2: Chayatat Ratanasawanya Min He July 21, 2010. The goal Experimental setup POSIT algorithm Homogeneous transformation Optitrack system Result](https://reader038.vdocuments.us/reader038/viewer/2022103005/56649cf85503460f949c9190/html5/thumbnails/2.jpg)
The goal Experimental setup POSIT algorithm Homogeneous transformation Optitrack system Result calculation Results Conclusion Questions/comments
Overview
![Page 3: Chayatat Ratanasawanya Min He July 21, 2010. The goal Experimental setup POSIT algorithm Homogeneous transformation Optitrack system Result](https://reader038.vdocuments.us/reader038/viewer/2022103005/56649cf85503460f949c9190/html5/thumbnails/3.jpg)
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
![Page 4: Chayatat Ratanasawanya Min He July 21, 2010. The goal Experimental setup POSIT algorithm Homogeneous transformation Optitrack system Result](https://reader038.vdocuments.us/reader038/viewer/2022103005/56649cf85503460f949c9190/html5/thumbnails/4.jpg)
Test setup
![Page 5: Chayatat Ratanasawanya Min He July 21, 2010. The goal Experimental setup POSIT algorithm Homogeneous transformation Optitrack system Result](https://reader038.vdocuments.us/reader038/viewer/2022103005/56649cf85503460f949c9190/html5/thumbnails/5.jpg)
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
![Page 6: Chayatat Ratanasawanya Min He July 21, 2010. The goal Experimental setup POSIT algorithm Homogeneous transformation Optitrack system Result](https://reader038.vdocuments.us/reader038/viewer/2022103005/56649cf85503460f949c9190/html5/thumbnails/6.jpg)
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
![Page 7: Chayatat Ratanasawanya Min He July 21, 2010. The goal Experimental setup POSIT algorithm Homogeneous transformation Optitrack system Result](https://reader038.vdocuments.us/reader038/viewer/2022103005/56649cf85503460f949c9190/html5/thumbnails/7.jpg)
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
![Page 8: Chayatat Ratanasawanya Min He July 21, 2010. The goal Experimental setup POSIT algorithm Homogeneous transformation Optitrack system Result](https://reader038.vdocuments.us/reader038/viewer/2022103005/56649cf85503460f949c9190/html5/thumbnails/8.jpg)
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
![Page 9: Chayatat Ratanasawanya Min He July 21, 2010. The goal Experimental setup POSIT algorithm Homogeneous transformation Optitrack system Result](https://reader038.vdocuments.us/reader038/viewer/2022103005/56649cf85503460f949c9190/html5/thumbnails/9.jpg)
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
![Page 10: Chayatat Ratanasawanya Min He July 21, 2010. The goal Experimental setup POSIT algorithm Homogeneous transformation Optitrack system Result](https://reader038.vdocuments.us/reader038/viewer/2022103005/56649cf85503460f949c9190/html5/thumbnails/10.jpg)
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
![Page 11: Chayatat Ratanasawanya Min He July 21, 2010. The goal Experimental setup POSIT algorithm Homogeneous transformation Optitrack system Result](https://reader038.vdocuments.us/reader038/viewer/2022103005/56649cf85503460f949c9190/html5/thumbnails/11.jpg)
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
![Page 12: Chayatat Ratanasawanya Min He July 21, 2010. The goal Experimental setup POSIT algorithm Homogeneous transformation Optitrack system Result](https://reader038.vdocuments.us/reader038/viewer/2022103005/56649cf85503460f949c9190/html5/thumbnails/12.jpg)
Q-ball trackable object
![Page 13: Chayatat Ratanasawanya Min He July 21, 2010. The goal Experimental setup POSIT algorithm Homogeneous transformation Optitrack system Result](https://reader038.vdocuments.us/reader038/viewer/2022103005/56649cf85503460f949c9190/html5/thumbnails/13.jpg)
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
![Page 14: Chayatat Ratanasawanya Min He July 21, 2010. The goal Experimental setup POSIT algorithm Homogeneous transformation Optitrack system Result](https://reader038.vdocuments.us/reader038/viewer/2022103005/56649cf85503460f949c9190/html5/thumbnails/14.jpg)
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
![Page 15: Chayatat Ratanasawanya Min He July 21, 2010. The goal Experimental setup POSIT algorithm Homogeneous transformation Optitrack system Result](https://reader038.vdocuments.us/reader038/viewer/2022103005/56649cf85503460f949c9190/html5/thumbnails/15.jpg)
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
![Page 16: Chayatat Ratanasawanya Min He July 21, 2010. The goal Experimental setup POSIT algorithm Homogeneous transformation Optitrack system Result](https://reader038.vdocuments.us/reader038/viewer/2022103005/56649cf85503460f949c9190/html5/thumbnails/16.jpg)
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
![Page 17: Chayatat Ratanasawanya Min He July 21, 2010. The goal Experimental setup POSIT algorithm Homogeneous transformation Optitrack system Result](https://reader038.vdocuments.us/reader038/viewer/2022103005/56649cf85503460f949c9190/html5/thumbnails/17.jpg)
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
![Page 18: Chayatat Ratanasawanya Min He July 21, 2010. The goal Experimental setup POSIT algorithm Homogeneous transformation Optitrack system Result](https://reader038.vdocuments.us/reader038/viewer/2022103005/56649cf85503460f949c9190/html5/thumbnails/18.jpg)
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
![Page 19: Chayatat Ratanasawanya Min He July 21, 2010. The goal Experimental setup POSIT algorithm Homogeneous transformation Optitrack system Result](https://reader038.vdocuments.us/reader038/viewer/2022103005/56649cf85503460f949c9190/html5/thumbnails/19.jpg)
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
![Page 20: Chayatat Ratanasawanya Min He July 21, 2010. The goal Experimental setup POSIT algorithm Homogeneous transformation Optitrack system Result](https://reader038.vdocuments.us/reader038/viewer/2022103005/56649cf85503460f949c9190/html5/thumbnails/20.jpg)
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
![Page 21: Chayatat Ratanasawanya Min He July 21, 2010. The goal Experimental setup POSIT algorithm Homogeneous transformation Optitrack system Result](https://reader038.vdocuments.us/reader038/viewer/2022103005/56649cf85503460f949c9190/html5/thumbnails/21.jpg)
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
![Page 22: Chayatat Ratanasawanya Min He July 21, 2010. The goal Experimental setup POSIT algorithm Homogeneous transformation Optitrack system Result](https://reader038.vdocuments.us/reader038/viewer/2022103005/56649cf85503460f949c9190/html5/thumbnails/22.jpg)
Experimental goal and setup POSIT algorithm Results of POSIT & homogeneous
transformation Optitrack system How to calculate the result Test results
Summary
![Page 23: Chayatat Ratanasawanya Min He July 21, 2010. The goal Experimental setup POSIT algorithm Homogeneous transformation Optitrack system Result](https://reader038.vdocuments.us/reader038/viewer/2022103005/56649cf85503460f949c9190/html5/thumbnails/23.jpg)
Thank you