Kevyn Young
December 3, 2014





Introduction
Previous Work
Implementation/Algorithms
Testing
Results
Image from http://modelaircraft.com/1758/first-test-flight-blade-350qx-quadcopter/

Use Structure from Motion to find AboveGround Level Altitude
◦ Use for terrain-following, station-keeping, landing


OpenCV 2.4.9 (C++)
Use Shi-Tomasi to find features in images


Use Optical Flow to track
features across images
RANSAC to find ground plane
points and camera homography
𝐻 (Discrete Space)


1
𝐻𝐶 = 𝜔 + 𝑣𝑁 𝑇
𝑑
1
+ 𝑇𝑁 𝑇
𝑑
𝑋2 = 𝐻𝐷 𝑋1 → 𝐻𝐷 = 𝑅
Four Possible Solutions:
Solution 1:
𝑅1 = 𝑊1 𝑈1𝑇
𝑁1 = 𝑣2 × 𝑢1
Solution 3:
𝑅3 = −𝑅1
𝑁3 = −𝑁1
1
1
𝑇3 = − 𝑇1
𝑑
𝑑
𝑅2 = 𝑊2 𝑈2𝑇
𝑁2 = 𝑣2 × 𝑢2
Solution 4:
𝑅4 = −𝑅2
𝑁4 = −𝑁2
1
1
𝑇4 = − 𝑇2
𝑑
𝑑
1
𝑇 = 𝐻𝐷 − 𝑅1 𝑁1
𝑑 1
Solution 2:
1
𝑇 = 𝐻𝐷 − 𝑅2 𝑁2
𝑑 2



Use single value decomposition to find 𝑢1 , 𝑢2 , 𝑣1 , 𝑣2
◦ 𝑣1 and 𝑣2 are not velocities!
𝑈1 = 𝑣2 , 𝑢1 , 𝑣2 𝑢1 ,
𝑊1 = 𝐻𝑣2 , 𝐻𝑢1 , 𝐻𝑣2 𝐻𝑢1
𝑈2 = 𝑣2 , 𝑢2 , 𝑣2 𝑢2 ,
𝑊2 = [𝐻𝑣2 , 𝐻𝑢2 , 𝐻𝑣2 𝐻𝑢2 ]
Only 2 real solutions → 𝑁𝑧 < 0 so that d is positive
Use most vertical normal (largest |𝑁𝑧 |) as solution
𝑅
Δ𝑡

Find angular velocity: 𝜔 =

Use continuous motion constraint:
→ Δ𝑡 =
1
𝐹𝑃𝑆
𝑋 𝑡 =𝜔 𝑡 𝑋 𝑡 +𝑣 𝑡

In image space: Continuous Homography
Constraint:
𝑥 𝐻𝐶 𝑥 = 𝑥 𝑢

Remove angular velocity from optical flow
𝑢𝑥′
𝑢𝑦′

−𝑥𝑥 𝑥𝑦
𝑢𝑥
= 𝑢 −
𝑦
− 1 + 𝑥𝑦
2
1 + 𝑥𝑥2
−𝑥𝑦
𝑥𝑥 𝑥𝑦
𝑥𝑥
With angular velocity removed:
1
𝐻𝐶 = 𝑣𝑁 𝑇
𝑑
𝜔

4-point algorithm (use RANSAC inliers):
𝑎𝑇 𝐻 𝑆 = 𝑥 𝑢 → 𝐴𝐻 𝑆 = 𝐵,
𝐴 = 𝑎1 , … , 𝑎𝑛 𝑇 ∈ ℝ3𝑛×9 ,
𝐵 = 𝑥1 𝑢1 , … , 𝑥𝑛 𝑢𝑛 𝑇 ∈ ℝ3𝑛 ,
𝑎 =𝑥⊗𝑥
𝐻 𝑆 = 𝐻11 , 𝐻21 , … , 𝐻33 𝑇 ∈ ℝ9



Reconstruct 𝐻𝐶 from 𝐻 𝑆 (normalize 𝐻𝐶 )
Known: 𝐻𝐶 , 𝑣, 𝑁
Solve for 𝑑 ∈ ℝ
𝑑 = 𝑁 𝑇 𝐻 −1 𝑣 → [𝑚𝑚]

Synthetic Data
◦ No Rotation
◦ Translation in y-axis only (y and z
in camera frame)

Known Test Data
◦ Collected Using SCORbot in W220
◦ Data collected using Raspberry Pi
Camera Module

Poor results, couldn’t prove
implementation due to
inadequate data
◦ No instantaneous velocity data
◦ No focal length information
◦ Camera wasn’t fixed with fixed
zoom

Low number of features
◦ Shi-Tomasi settings were
incorrect

Estimated, constant velocity
was inaccurate

Correct Homography
found (Discrete Space)
◦ Confirmed by comparing
results to
planeDectection
program provided by
Professor Hoff (uses
SURF for feature
matching)

Data Collected from SCORbot
◦ Tagged with velocity data
◦ Collected corresponding positional data
 Can confirm if altitude detected is correct