CS549-Homework 12 Solved

Stereo Motion (7 pts): In stereo imaging we can compute a point's world coordinates from left and right images as
𝐵𝐵⃗2

𝑋𝑋⃗𝑊𝑊 = 𝑋𝑋⃗𝐴𝐴𝐴𝐴𝐴𝐴

𝐵𝐵⃗ ∙ ∆⃗

Show that if the world point is in motion, we can compute its velocity as
𝐵𝐵⃗2                 𝐵𝐵⃗

𝑉𝑉⃗𝑊𝑊 = 𝑉𝑉⃗𝐴𝐴𝐴𝐴𝐴𝐴  − 𝑋𝑋⃗𝑊𝑊  

𝐵𝐵⃗ ∙ ∆⃗       𝐵𝐵⃗ ∙ ∆⃗

 

Suppose that a moving world point is imaged as
𝑐𝑐               −𝑐𝑐

𝑋𝑋⃗𝐿𝐿𝐼𝐼 = 20𝑑𝑑 , 𝑋𝑋⃗𝑅𝑅𝐼𝐼 = 20𝑑𝑑

𝑓𝑓               𝑓𝑓

With the usual imaging geometry of 𝐵𝐵⃗ = [𝑏𝑏  0  0]𝑇𝑇, 𝐹𝐹⃗ = [0  0 𝑓𝑓]𝑇𝑇, what is 𝑉𝑉⃗𝑊𝑊?

Express 𝑉𝑉⃗𝑊𝑊 in the simplest terms.

 

 

Shading (6 pts): Consider the two surfaces
𝑧𝑧1 = (𝑥𝑥2 + 𝑦𝑦2) and 𝑧𝑧2 = 2𝑥𝑥𝑦𝑦

Find 𝑝𝑝(𝑥𝑥, 𝑦𝑦) and 𝑞𝑞(𝑥𝑥, 𝑦𝑦) for both surfaces.
Show that 𝑧𝑧1 and 𝑧𝑧2 give rise to the same shading when a rotationally symmetric reflectance map applies, that is, when 𝑅𝑅(𝑝𝑝, 𝑞𝑞) = 𝑅𝑅(𝑝𝑝2 + 𝑞𝑞2).
 

SLAM: Following Bailey & Durrant-Whyte Part II, at time k the vehicle pose 𝐱𝐱𝑣𝑣𝑘𝑘and landmark locations m can be combined into a single state 𝐱𝐱𝑘𝑘.
𝐱𝐱𝑣𝑣𝑘𝑘

𝐱𝐱𝑘𝑘 =  𝐦𝐦

The covariance matrix of 𝐱𝐱𝑘𝑘, denoted by 𝐏𝐏𝑘𝑘|𝑘𝑘, is partitioned into

𝐏𝐏𝑘𝑘|𝑘𝑘 = 𝐏𝐏𝐏𝐏𝑣𝑣𝑣𝑣𝑇𝑇𝑣𝑣𝑣𝑣 𝐏𝐏𝐏𝐏𝑣𝑣𝑣𝑣𝑣𝑣𝑣𝑣 𝑘𝑘|𝑘𝑘

Control input 𝐮𝐮𝑘𝑘 only affects the vehicle pose, not the landmark locations, via 𝐱𝐱𝑣𝑣𝑘𝑘 =

𝐟𝐟𝒗𝒗(𝐱𝐱𝑣𝑣𝑘𝑘−1, 𝐮𝐮𝑘𝑘). Thus, the update equation for state 𝐱𝐱𝑘𝑘 is

 

𝐱𝐱𝑘𝑘 = 𝐟𝐟(𝐱𝐱𝑘𝑘−1, 𝐮𝐮𝑘𝑘) = 𝐟𝐟𝒗𝒗(𝐱𝐱𝑣𝑣𝑘𝑘−1, 𝐮𝐮𝑘𝑘)

𝐦𝐦

When control 𝐮𝐮𝑘𝑘 is applied, but before any landmark measurement updates are made, the covariance prediction 𝐏𝐏𝑘𝑘|𝑘𝑘−1 is given by

𝐏𝐏𝑘𝑘|𝑘𝑘−1 = ∇𝐟𝐟𝐱𝐱𝐏𝐏𝑘𝑘−1|𝑘𝑘−1∇𝐟𝐟𝐱𝐱𝑇𝑇 + ∇𝐟𝐟𝐮𝐮𝐔𝐔𝑘𝑘∇𝐟𝐟𝐮𝐮𝑇𝑇

𝝏𝝏𝐟𝐟                       𝝏𝝏𝐟𝐟 where ∇𝐟𝐟𝐱𝐱 = , ∇𝐟𝐟𝐮𝐮 = , and 𝐔𝐔𝑘𝑘 is the control convariance. Show that the covariance

 

𝝏𝝏𝐱𝐱𝑘𝑘−1               𝝏𝝏𝐮𝐮𝑘𝑘

∇𝐟𝐟𝒗𝒗𝐱𝐱𝐏𝐏𝑣𝑣𝑣𝑣,𝑘𝑘−1|𝑘𝑘−1∇𝐟𝐟𝒗𝒗𝑇𝑇𝐱𝐱 + ∇𝐟𝐟𝒗𝒗𝐮𝐮𝐔𝐔𝑘𝑘∇𝐟𝐟𝒗𝒗𝑇𝑇𝐮𝐮

𝐏𝐏

𝐏𝐏𝑘𝑘|𝑘𝑘−1 =        𝑣𝑣𝑣𝑣𝑇𝑇 ,𝑘𝑘−1|𝑘𝑘−1∇𝐟𝐟𝒗𝒗𝑇𝑇𝐱𝐱

𝝏𝝏𝐟𝐟

where ∇𝐟𝐟𝒗𝒗𝐱𝐱 = 𝝏𝝏𝐱𝐱𝑣𝑣𝒗𝒗 and ∇𝐟𝐟𝒗𝒗𝐮𝐮 = 𝝏𝝏𝐮𝐮𝝏𝝏𝐟𝐟𝒗𝒗𝑘𝑘.
∇𝐟𝐟𝒗𝒗𝐱𝐱𝐏𝐏𝑣𝑣𝑣𝑣,𝑘𝑘−1|𝑘𝑘−1

 

𝐏𝐏𝑣𝑣𝑣𝑣,𝑘𝑘−1|𝑘𝑘−1
prediction simplifies (!) to

𝑘𝑘−1

 

Hint: Expand  ∇𝐟𝐟𝐱𝐱 and ∇𝐟𝐟𝐮𝐮. This problem is not as hard as it looks once you understand what is being asked.