def distance_error(X, lengthdiscoff, objPoints_list, imgPoints_list,
depth_list):
error = np.array([])
[alpha, gamma, uc, vc] = X[0:4]
discoff = X[4:4 + lengthdiscoff]
A = np.array([[alpha, 0, uc], [0, gamma, vc], [0, 0, 1]])
n = len(objPoints_list)
for i in range(n):
imapoints = imgPoints_list[i].reshape([-1, 1, 2])
Point_cam_cood = cv2.undistortPoints(imapoints, A, discoff)
Point_cam_cood = Point_cam_cood.reshape(-1, 2)
depth_point_acc = np.append(Point_cam_cood, depth_list[i], 1)
if Point_cam_cood.shape[0] < 5:
continue
for j in range(depth_point_acc.shape[0]):
depth_point_acc[
j, 0] = Point_cam_cood[j, 0] * depth_point_acc[j, 2]
depth_point_acc[
j, 1] = Point_cam_cood[j, 1] * depth_point_acc[j, 2]
plane = utils.get_nice_plane(depth_point_acc)
for j in range(depth_point_acc.shape[0]):
depth_point_acc[j, 2] = plane[0] * depth_point_acc[
j, 0] + plane[1] * depth_point_acc[j, 1] + plane[2]
# depth_point_acc[j, 0] = Point_cam_cood[j,0]*depth_point_acc[j, 2]
# depth_point_acc[j, 1] = Point_cam_cood[j,1]*depth_point_acc[j, 2]
m = Point_cam_cood.shape[0]
for a in range(int(m / 2 - 1)):
distance1 = np.linalg.norm(objPoints_list[i][a, :] -
objPoints_list[i][m - a - 1, :])
distance2 = np.linalg.norm(depth_point_acc[a, :] -
depth_point_acc[m - a - 1, :])
error = np.append(error, np.abs(distance2 - distance1))
return error
def distance_error(X, lengthdiscoff, objPoints_list, imgPoints_list,
depth_list):
error = np.array([])
[alpha, gamma, uc, vc] = X[0:4]
discoff = X[4:4 + lengthdiscoff]
A = np.array([[alpha, 0, uc], [0, gamma, vc], [0, 0, 1]])
n = len(objPoints_list)
for i in range(n):
extrinsic_rgb = extrinsic(imgPoints_list[i], objPoints_list[i],
A, discoff)
Point_cam_cood = cv2.undistortPoints(
imgPoints_list[i].reshape(-1, 1, 2), A, discoff)
Point_cam_cood = Point_cam_cood.reshape(-1, 2)
depth_point_acc = np.append(Point_cam_cood, depth_list[i], 1)
if Point_cam_cood.shape[0] < 5:
continue
for j in range(depth_point_acc.shape[0]):
depth_point_acc[
j, 0] = Point_cam_cood[j, 0] * depth_point_acc[j, 2]
depth_point_acc[
j, 1] = Point_cam_cood[j, 1] * depth_point_acc[j, 2]
plane = utils.get_nice_plane(depth_point_acc)
for j in range(depth_point_acc.shape[0]):
point = np.array([
objPoints_list[i][j, 0], objPoints_list[i][j, 1], 0, 1
]).reshape([4, 1])
point_in_cam = np.dot(extrinsic_rgb, point)
depth_rgb = point_in_cam[2, 0]
depth_real = plane[0] * depth_point_acc[
j, 0] + plane[1] * depth_point_acc[j, 1] + plane[2]
error = np.append(error, depth_rgb - depth_real)
return error
def extrisic_depth(self, objpoint, imgpoint, point_depth, intrinsic, dist):
if point_depth.shape[0] < 10:
print("深度缺失比较严重")
return False, None
Point_cam_cood = cv2.undistortPoints(imgpoint.reshape([-1, 1, 2]),
intrinsic, dist)
Point_cam_cood = Point_cam_cood.reshape(-1, 2)
point_in_camera = np.append(Point_cam_cood,
np.ones([Point_cam_cood.shape[0], 1]), 1)
for i in range(point_depth.shape[0]):
point_in_camera[i, :] = point_in_camera[i, :] * point_depth[i, 0]
plane = utils.get_nice_plane(point_in_camera)
error_plane = np.array([])
for j in range(point_in_camera.shape[0]):
t = point_in_camera[j, 2]
point_in_camera[j, 2] = plane[0] * point_in_camera[
j, 0] + plane[1] * point_in_camera[j, 1] + plane[2]
error_plane = np.append(error_plane, t - point_in_camera[j, 2])
def rigid_transform_3D(A, B):
assert len(A) == len(B)
N = A.shape[0] # total points
centroid_A = np.mean(A, axis=0)
centroid_B = np.mean(B, axis=0)
# centre the points
AA = A - np.tile(centroid_A, (N, 1))
BB = B - np.tile(centroid_B, (N, 1))
# dot is matrix multiplication for array
H = np.dot(np.transpose(AA), BB)
U, S, Vt = np.linalg.svd(H)
# R = Vt.T * U.T
R = np.dot(Vt.T, U.T)
# special reflection case
if np.linalg.det(R) < 0:
# print "Reflection detected"
Vt[2, :] *= -1
R = np.dot(Vt.T, U.T)
t = -np.dot(R, centroid_A.T) + centroid_B.T
t = B.T - np.dot(R, A.T)
t = np.mean(t, axis=1)
# print t
BB = np.dot(R, A.T) + np.tile(np.transpose([t]), (1, N))
error = BB.T - B
return R, t
objpoint = np.append(objpoint, np.zeros([objpoint.shape[0], 1]), 1)
R, T = rigid_transform_3D(objpoint, point_in_camera)
camepose = np.append(np.append(R, np.transpose([T]), 1),
np.array([[0, 0, 0, 1]]), 0)
return True, camepose