def pose_estimate_nls(K, Twcg, Ipts, Wpts):
"""
Estimate camera pose from 2D-3D correspondences via NLS.
The function performs a nonlinear least squares optimization procedure
to determine the best estimate of the camera pose in the calibration
target frame, given 2D-3D point correspondences.
Parameters:
-----------
Twcg - 4x4 homogenous pose matrix, initial guess for camera pose.
Ipts - 2xn array of cross-junction points (with subpixel accuracy).
Wpts - 3xn array of world points (one-to-one correspondence with Ipts).
Returns:
--------
Twc - 4x4 np.array, homogenous pose matrix, camera pose in target frame.
"""
maxIters = 250 # Set maximum iterations.
tp = Ipts.shape[1] # Num points.
ps = np.reshape(Ipts, (2 * tp, 1), 'F') # Stacked vector of observations.
dY = np.zeros((2 * tp, 1)) # Residuals.
J = np.zeros((2 * tp, 6)) # Jacobian.
#--- FILL ME IN ---
E_pose = epose_from_hpose(Twcg)
for i in range(maxIters):
H_pose = hpose_from_epose(E_pose)
# Constructing A and b matrices to house Jacobian:
A = np.zeros([6, 6])
b = np.zeros([6, 1])
for j in range(Ipts.shape[1]):
J = find_jacobian(K, H_pose, Wpts[:, j].reshape([3, 1]))
# Adding J^TJ to A:
A += J.T.dot(J)
# Converting to H, finding error E, and adding J^TE to b:
H_pose_Wpt = np.linalg.inv(H_pose).dot(
np.vstack((Wpts[:, j].reshape([3, 1]), 1)))
HK = K.dot(H_pose_Wpt[:-1])
f = HK / HK[-1]
E = Ipts[:, j].reshape([2, 1]) - f[:-1]
b += J.T.dot(E)
E_pose += np.linalg.inv(A).dot(b)
#------------------
return hpose_from_epose(E_pose)
def pose_estimate_nls(K, Twcg, Ipts, Wpts):
"""
Estimate camera pose from 2D-3D correspondences via NLS.
The function performs a nonlinear least squares optimization procedure
to determine the best estimate of the camera pose in the calibration
target frame, given 2D-3D point correspondences.
Parameters:
-----------
Twcg - 4x4 homogenous pose matrix, initial guess for camera pose.
Ipts - 2xn array of cross-junction points (with subpixel accuracy).
Wpts - 3xn array of world points (one-to-one correspondence with Ipts).
Returns:
--------
Twc - 4x4 np.array, homogenous pose matrix, camera pose in target frame.
"""
maxIters = 250 # Set maximum iterations.
tp = Ipts.shape[1] # Num points.
ps = np.reshape(Ipts, (2 * tp, 1), 'F') # Stacked vector of observations.
dY = np.zeros((2 * tp, 1)) # Residuals.
J = np.zeros((2 * tp, 6)) # Jacobian.
# Run for maxIters
for iter in range(maxIters):
# Calculate residuals
for i in range(tp):
dY[2 * i:2 * i +
2, :] = img_pt_from_world_pt(K, Twcg, Wpts[:, i].reshape(
(3, 1))) - Ipts[:, i].reshape(2, 1)
# Calculate jacobian
for i in range(tp):
J[2 * i:2 * i + 2, :] = find_jacobian(K, Twcg, Wpts[:, i].reshape(
(3, 1)))
# Get update step
step = np.linalg.inv(J.T @ J) @ J.T @ dY
# Update parameters
curr_E = epose_from_hpose(Twcg)
updated_E = curr_E - step
Twcg = hpose_from_epose(updated_E)
Twc = Twcg
return Twc
def main():
# Set up test case - fixed parameters.
K = np.array([[564.9, 0, 337.3], [0, 564.3, 226.5], [0, 0, 1]])
Wpt = np.array([[0.0635, 0, 0]]).T
# Camera pose (rotation matrix, translation vector).
C_cam = np.array(
[[0.960656116714365, -0.249483426036932, 0.122056730876061],
[-0.251971275568189, -0.967721063070012, 0.005140075795822],
[0.116834505638601, -0.035692635424156, -0.992509815603182]])
t_cam = np.array(
[[0.201090356081375, 0.114474051344464, 1.193821106321156]]).T
Twc = np.hstack((C_cam, t_cam))
Twc = np.vstack((Twc, np.array([[0, 0, 0, 1]])))
J = find_jacobian(K, Twc, Wpt)
print(J)
def pose_estimate_nls(K, Twcg, Ipts, Wpts):
"""
Estimate camera pose from 2D-3D correspondences via NLS.
The function performs a nonlinear least squares optimization procedure
to determine the best estimate of the camera pose in the calibration
target frame, given 2D-3D point correspondences.
Parameters:
-----------
Twcg - 4x4 homogenous pose matrix, initial guess for camera pose.
Ipts - 2xn array of cross-junction points (with subpixel accuracy).
Wpts - 3xn array of world points (one-to-one correspondence with Ipts).
Returns:
--------
Twc - 4x4 np.array, homogenous pose matrix, camera pose in target frame.
"""
maxIters = 250 # Set maximum iterations.
tp = Ipts.shape[1] # Num points.
ps = np.reshape(Ipts, (2 * tp, 1), 'F') # Stacked vector of observations.
dY = np.zeros((2 * tp, 1)) # Residuals.
J = np.zeros((2 * tp, 6)) # Jacobian.
#--- FILL ME IN ---
#error threshold
eps = 0.0000001
#get the original parameters
X = epose_from_hpose(Twcg)
#get the H matrix from the original parameters
Twc = hpose_from_epose(X)
#Do iterative solver
for iter in range(0, maxIters):
#Pre initialize jacobian
J = np.zeros((2 * tp, 6))
#Pre initialize projection error
dY = np.zeros((2 * tp, 1))
#set the error sum to 0
res_sum = 0
#calculate jacobian and error for all the points
for index, row in enumerate(Wpts.T):
J[index * 2:(index + 1) * 2, :] = find_jacobian(
K, Twc, row.reshape(3, 1))
dY[index * 2:(index + 1) *
2, :] = (Ipts[:, index] -
Wpts_to_Ipts(K, Twc, Wpts)[:, index]).reshape(2, 1)
#add the error to the error sum
res_sum += np.sum(dY[index * 2:(index + 1) * 2, :])
#in case the matrix became singular through multiple iterations
try:
#calculate the increment to the X vector (parameter vector)
dx = np.dot(np.dot(inv(np.dot(J.T, J)), J.T), dY)
except:
return Twc
#move in the direction of the jacobian and update the parameter vector
X = X + dx
#get the H matrix
Twc = hpose_from_epose(X)
#check if we are below the threshold to return as well
if (np.abs(res_sum) < eps):
break
#------------------
return Twc
import numpy as np
from find_jacobian import find_jacobian
# Set up test case - fixed parameters.
K = np.array([[564.9, 0, 337.3], [0, 564.3, 226.5], [0, 0, 1]])
Wpt = np.array([[0.0635, 0, 0]]).T
# Camera pose (rotation matrix, translation vector).
C_cam = np.array([[0.960656116714365, -0.249483426036932, 0.122056730876061],
[-0.251971275568189, -0.967721063070012, 0.005140075795822],
[0.116834505638601, -0.035692635424156, -0.992509815603182]])
t_cam = np.array([[0.201090356081375, 0.114474051344464, 1.193821106321156]]).T
Twc = np.hstack((C_cam, t_cam))
Twc = np.vstack((Twc, np.array([[0, 0, 0, 1]])))
J = find_jacobian(K, Twc, Wpt)
print(J)
# J =
# -477.1016 121.4005 43.3460 -18.8900 592.2179 -71.3193
# 130.0713 468.1394 -59.8803 578.8882 -14.6399 -49.5217
def pose_estimate_nls(K, Twcg, Ipts, Wpts):
"""
Estimate camera pose from 2D-3D correspondences via NLS.
The function performs a nonlinear least squares optimization procedure
to determine the best estimate of the camera pose in the calibration
target frame, given 2D-3D point correspondences.
Parameters:
-----------
Twcg - 4x4 homogenous pose matrix, initial guess for camera pose.
Ipts - 2xn array of cross-junction points (with subpixel accuracy).
Wpts - 3xn array of world points (one-to-one correspondence with Ipts).
Returns:
--------
Twc - 4x4 np.array, homogenous pose matrix, camera pose in target frame.
"""
maxIters = 250 # Set maximum iterations.
tp = Ipts.shape[1] # Num points.
ps = np.reshape(Ipts, (2*tp, 1), 'F') # Stacked vector of observations.
dY = np.zeros((2*tp, 1)) # Residuals.
J = np.zeros((2*tp, 6)) # Jacobian.
#--- FILL ME IN ---
E = epose_from_hpose(np.linalg.inv(Twcg))
for iter in range(maxIters):
Twc = np.linalg.inv(hpose_from_epose(E))
sum1 = np.reshape(np.zeros(36),(6,6))
sum2 = np.reshape(np.zeros(6),(6,1))
for i in range(tp):
Wpt = np.vstack((np.array([Wpts[0][i]]), np.array([Wpts[1][i]]), np.array([Wpts[2][i]])))
Wpt_til = np.vstack((np.array([Wpts[0][i]]), np.array([Wpts[1][i]]), np.array([Wpts[2][i]]), np.array([1])))
Ipt = np.vstack((np.array([Ipts[0][i]]), np.array([Ipts[1][i]])))
J = find_jacobian(K, Twc, Wpt)
#find error
K_til = np.hstack((K,np.array([[0],[0],[0]])))
K_til = np.vstack((K_til,np.array([0,0,0,1])))
x_til = K_til@(np.linalg.inv(Twc))@Wpt_til
x = x_til[0:2]/x_til[2]
e = x-Ipt
sum1 += J.T@J
sum2 += J.T@e
update = np.linalg.inv(sum1)@sum2
E = epose_from_hpose(np.linalg.inv(Twc))
E[0][0] += update[0][0]
E[1][0] += update[1][0]
E[2][0] += update[2][0]
E[3][0] += update[3][0]
E[4][0] += update[4][0]
E[5][0] += update[5][0]
Twc = np.linalg.inv(hpose_from_epose(E))
#------------------
#return Twc
return Twc