def solve_for_r1(pts, K, lane_width):
""" Solve for first column of the rotation matrix, utilizing the
fact that we know the lane width. We require two pairs of points
(p1,p2), (p3,p4), such that p1 is directly across from p2 (same
for p3, p4).
Input:
tuple pts: ((p1, p2), (p3, p4))
where each point is a pixel coord: (float x, float y)
nparray K
The 3x3 camera intrinsic matrix.
float lane_width
The width of the lane (e.g., 3.66 meters).
Output:
nparray r1
A 3x1 column vector consisting of the first column of R.
"""
(fx, fy, (cx, cy)) = util_camera.get_intrinsics(K)
# Construct data matrix A
Araw = np.zeros([len(pts) * 2, 4])
i = 0 # Actual index into A
for ii, (pi, pj) in enumerate(pts):
xi, yi = pi
xj, yj = pj
Araw[i, :] = (0, -fy, (-cy + yj), (yi - yj))
Araw[i + 1, :] = (fx, 0, (cx - xj), (-xj + xi))
#Araw[i+2, :] = (-yj*fx, xj*fy, -yj*cx + xj*cy, yj*xi - xj*yi)
i += 2
rnk = numpy.linalg.matrix_rank(Araw)
print " Rank(A):", rnk
if rnk == 3:
A = Araw # Perfect! Just the rank we want.
elif rnk < 3:
raise Exception(
"Matrix A needs to have rank either 4 or 3! Rank was: {0}".format(
rnk))
else:
# A is full rank - perform fixed-rank approx. -> rank 3
print "(solve_for_r1) A is full rank, performing fixed rank approx..."
U, S, V = numpy.linalg.svd(Araw)
if (np.linalg.det(V) < 0):
# We require U,V to have positive determinant
print " U,V had negative determinant, correcting."
U = -U
V = -V
S_part = np.diag([S[0], S[1], S[2], 0]) # Kill last singular value
S_new = np.zeros([Araw.shape[0], 4])
S_new[0:4, :] = S_part
A = np.dot(U, np.dot(S_new, V))
print " new rank:", np.linalg.matrix_rank(A)
if np.linalg.matrix_rank(A) != 3:
raise Exception("(solve_for_r1) What?! Fixed-rank approx. failed!")
U, S, V = numpy.linalg.svd(A)
if (np.linalg.det(V) < 0):
# We require U,V to have positive determinant
print " U,V had negative determinant, correcting."
U = -U
V = -V
v = V[-1, :]
residual = numpy.linalg.norm(np.dot(A, v.T))
print "(solve_for_r1) Residual: {0}".format(residual)
gamma = v[-1]
v_norm = v / gamma
r11, r21, r31, _ = v_norm
return np.array([r11, r21, r31])
def solve_for_r1(pts, K, lane_width):
""" Solve for first column of the rotation matrix, utilizing the
fact that we know the lane width. We require two pairs of points
(p1,p2), (p3,p4), such that p1 is directly across from p2 (same
for p3, p4).
Input:
tuple pts: ((p1, p2), (p3, p4))
where each point is a pixel coord: (float x, float y)
nparray K
The 3x3 camera intrinsic matrix.
float lane_width
The width of the lane (e.g., 3.66 meters).
Output:
nparray r1
A 3x1 column vector consisting of the first column of R.
"""
(fx, fy, (cx, cy)) = util_camera.get_intrinsics(K)
# Construct data matrix A
Araw = np.zeros([len(pts) * 2, 4])
i = 0 # Actual index into A
for ii, (pi, pj) in enumerate(pts):
xi, yi = pi
xj, yj = pj
Araw[i, :] = (0, -fy, (-cy + yj), (yi - yj))
Araw[i+1, :] = (fx, 0, (cx - xj), (-xj + xi))
#Araw[i+2, :] = (-yj*fx, xj*fy, -yj*cx + xj*cy, yj*xi - xj*yi)
i += 2
rnk = numpy.linalg.matrix_rank(Araw)
print " Rank(A):", rnk
if rnk == 3:
A = Araw # Perfect! Just the rank we want.
elif rnk < 3:
raise Exception("Matrix A needs to have rank either 4 or 3! Rank was: {0}".format(rnk))
else:
# A is full rank - perform fixed-rank approx. -> rank 3
print "(solve_for_r1) A is full rank, performing fixed rank approx..."
U, S, V = numpy.linalg.svd(Araw)
if (np.linalg.det(V) < 0):
# We require U,V to have positive determinant
print " U,V had negative determinant, correcting."
U = -U
V = -V
S_part = np.diag([S[0], S[1], S[2], 0]) # Kill last singular value
S_new = np.zeros([Araw.shape[0], 4])
S_new[0:4, :] = S_part
A = np.dot(U, np.dot(S_new, V))
print " new rank:", np.linalg.matrix_rank(A)
if np.linalg.matrix_rank(A) != 3:
raise Exception("(solve_for_r1) What?! Fixed-rank approx. failed!")
U, S, V = numpy.linalg.svd(A)
if (np.linalg.det(V) < 0):
# We require U,V to have positive determinant
print " U,V had negative determinant, correcting."
U = -U
V = -V
v = V[-1, :]
residual = numpy.linalg.norm(np.dot(A, v.T))
print "(solve_for_r1) Residual: {0}".format(residual)
gamma = v[-1]
v_norm = v / gamma
r11, r21, r31, _ = v_norm
return np.array([r11, r21, r31])
def solve_for_t(pts, K, r1, r3, lane_width):
""" Recover the translation vector T, using the computed r1, r3.
The input points pairs must be directly across from the lanes.
Input:
tuple pts: ((pt1, pt2), ...)
Each point pair (pt_i, pt_j) must be directly across the
lanes i.e. (X_j - X_i) = 3.66 meters, and:
X_i = -1.83 meters
X_j = +1.83 meters
nparray K
3x3 camera intrinsic matrix.
nparray r1, r3
The 3x1 column vectors comprising the first/third columns
of the rotation matrix R.
float lane_width
Width of the lane (in meters).
Output:
nparray T
The translation vector T as a 3x1 column vector.
"""
(fx, fy, (cx, cy)) = util_camera.get_intrinsics(K)
Kinv = numpy.linalg.inv(K)
r11, r21, r31 = r1
r13, r23, r33 = r3
ww = lane_width / 2
# Construct data matrix A
Araw = np.zeros([len(pts) * 6, 5])
i = 0 # Actual index into A
for ii, (pi, pj) in enumerate(pts):
xi, yi = pi
xj, yj = pj
bi = np.array([(xi - cx) / fx, (yi - cy) / fy, 1]).T
bj = np.array([(xj - cx) / fx, (yj - cy) / fy, 1]).T
Araw[i, :] = [r13, 1, 0, 0, -ww * r11 - bi[0]]
Araw[i + 1, :] = [r23, 0, 1, 0, -ww * r21 - bi[1]]
Araw[i + 2, :] = [r33, 0, 0, 1, -ww * r31 - bi[2]]
Araw[i + 3, :] = [r13, 1, 0, 0, -ww * r11 - bj[0]]
Araw[i + 4, :] = [r23, 0, 1, 0, -ww * r21 - bj[1]]
Araw[i + 5, :] = [r33, 0, 0, 1, -ww * r31 - bj[2]]
i += 6
rnk = np.linalg.matrix_rank(Araw)
if rnk == 4:
A = Araw # Perfect! Just the rank we want.
elif rnk < 4:
raise Exception(
"(solve_for_r3) Matrix rank needs to be either 5 or 4 (was: {0})".
format(rnk))
else:
# Perform fixed-rank approx on Araw (want rank 4)
print "(solve_for_t): Araw has full rank, performing fixed_rank approx..."
U, S, V = np.linalg.svd(Araw)
if np.linalg.svd(V) < 0:
U = -U
V = -V
S_new = np.zeros([U.shape[0], 5])
for i in xrange(4):
S_new[i, i] = S[i]
A = np.dot(U, np.dot(S_new, V))
print np.allclose(Araw, A)
print Araw[0, :]
print '=='
print A[0, :]
print "(solve_for_t): Rank(A):", np.linalg.matrix_rank(A)
U, S, V = numpy.linalg.svd(A)
if np.linalg.det(V) < 0:
U = -U
V = -V
v = V[-1, :]
print " residual: {0}".format(np.linalg.norm(np.dot(A, v)))
gamma = v[-1]
v_norm = v / gamma
Z, tx, ty, tz, _ = v_norm
return np.array([tx, ty, tz]).T
def solve_for_t(pts, K, r1, r3, lane_width):
""" Recover the translation vector T, using the computed r1, r3.
The input points pairs must be directly across from the lanes.
Input:
tuple pts: ((pt1, pt2), ...)
Each point pair (pt_i, pt_j) must be directly across the
lanes i.e. (X_j - X_i) = 3.66 meters, and:
X_i = -1.83 meters
X_j = +1.83 meters
nparray K
3x3 camera intrinsic matrix.
nparray r1, r3
The 3x1 column vectors comprising the first/third columns
of the rotation matrix R.
float lane_width
Width of the lane (in meters).
Output:
nparray T
The translation vector T as a 3x1 column vector.
"""
(fx, fy, (cx, cy)) = util_camera.get_intrinsics(K)
Kinv = numpy.linalg.inv(K)
r11, r21, r31 = r1
r13, r23, r33 = r3
ww = lane_width / 2
# Construct data matrix A
Araw = np.zeros([len(pts) * 6, 5])
i = 0 # Actual index into A
for ii, (pi, pj) in enumerate(pts):
xi, yi = pi
xj, yj = pj
bi = np.array([(xi - cx) / fx,
(yi - cy) / fy,
1]).T
bj = np.array([(xj - cx) / fx,
(yj - cy) / fy,
1]).T
Araw[i, :] = [r13, 1, 0, 0, -ww*r11 - bi[0]]
Araw[i+1, :] = [r23, 0, 1, 0, -ww*r21 - bi[1]]
Araw[i+2, :] = [r33, 0, 0, 1, -ww*r31 - bi[2]]
Araw[i+3, :] = [r13, 1, 0, 0, -ww*r11 - bj[0]]
Araw[i+4, :] = [r23, 0, 1, 0, -ww*r21 - bj[1]]
Araw[i+5, :] = [r33, 0, 0, 1, -ww*r31 - bj[2]]
i += 6
rnk = np.linalg.matrix_rank(Araw)
if rnk == 4:
A = Araw # Perfect! Just the rank we want.
elif rnk < 4:
raise Exception("(solve_for_r3) Matrix rank needs to be either 5 or 4 (was: {0})".format(rnk))
else:
# Perform fixed-rank approx on Araw (want rank 4)
print "(solve_for_t): Araw has full rank, performing fixed_rank approx..."
U, S, V = np.linalg.svd(Araw)
if np.linalg.svd(V) < 0:
U = -U
V = -V
S_new = np.zeros([U.shape[0], 5])
for i in xrange(4):
S_new[i,i] = S[i]
A = np.dot(U, np.dot(S_new, V))
print np.allclose(Araw, A)
print Araw[0,:]
print '=='
print A[0,:]
print "(solve_for_t): Rank(A):", np.linalg.matrix_rank(A)
U, S, V = numpy.linalg.svd(A)
if np.linalg.det(V) < 0:
U = -U
V = -V
v = V[-1, :]
print " residual: {0}".format(np.linalg.norm(np.dot(A, v)))
gamma = v[-1]
v_norm = v / gamma
Z, tx, ty, tz, _ = v_norm
return np.array([tx, ty, tz]).T