Determine camera movement from observing image features in 2D
Assume that two features (A, B) are visible on a surface
A camera moves in a plane over the surface (at the same height) pointing directly towards the floor
Determine that camera movement from the depicted features in two concecutive images (0,1)
Let Pn be the location of top left corner of the image on the floor in image n
The position of the depicted image move from P0 to P1 between the two consecutive images
Let Rn be the angle of the x-axis of the image on the floor in image n
The angle of the depicted image change from R0 to R1 between the two consecutive images
Let the position of each feature A and B relative (top corner of the image) be defined as:
[
Pa0 = (Xa0, Ya0) = position of feature a in image 0
]
[
Pb0 = (Xb0, Yb0) = position of feature a in image 0
]
[
Pa1 = (Xa1, Ya1) = position of feature a in image 1
]
[
Pb1 = (Xb1, Yb1) = position of feature a in image 1
]
Take the angle between x-axis and A-B vector:
[
A0 = atan (Yb0-Ya0) / (Xb0-Xa0) = angle of vector AB in image 0
]
[
A1 = atan (Yb1-Ya1) / (Xb1-Xa1) = angle of vector AB in image 1
]
The difference in angle will be as follows.
[
Phi = A1 - A0
]
Translation of feature A is calculated as follows
[
T = ( Xa1 - Xa0, Ya1 - Ya0 )
]
So the task is to determine how the transformation (P0, R0) to (P1, R1) is to match with tranformation of depicted features in image 0 and 1.
Rotation is easy because rotation of vector AB will match rotation of camera.
[
R1 = R0 - Phi
]
Translation is assumed to be done before rotation. And translation is also assumed to be around feature A in image 1.
Rotation is done by moltiplication of a rotation matrix.
[
\begin{array}{cc}
cos(a) & sin(a) \
-sin(a) & cos(a) \
\end{array}
]
other
[
Ra = ( cos a sin a )
( -sin a cos a )
]
Rotation in negative direction is written as R(-a)
So determine P1 we first need to translate camera with (-T) and rotate (-phi) around point A in image 1.
[
P1 = (((P0 - T) - Pa1) * R(-phi) ) + Pa1
]
Now we have movement in pixels. To determine world movement we been a scaling factor between pixels and meters.
#!/usr/bin/ruby
# return angle of vector in radians
def v_angle(x,y)
ans = Math.atan(y/x)
if (ans > 0 && y > 0) || (ans < 0 && y < 0)
ans
else
ans+Math::PI
end
end
def rad_to_degrees(rad)
180 * (rad / Math::PI)
end
points = [[10.0,1.0], [-10.0,10.0] , [10.0,-1.0], [-10.0,1.0], [-10.0,-1.0]]
points.each { |s|
puts s.inspect + " -> " + rad_to_degrees(v_angle(s[0], s[1])).to_s
}
# ./angle.rb
# [10.0, 1.0] -> 5.710593137499643
# [-10.0, 10.0] -> 135.0
# [10.0, -1.0] -> -5.710593137499643
# [-10.0, 1.0] -> 174.28940686250036
# [-10.0, -1.0] -> 185.71059313749964