def calibrate(points2D=None, points3D=None):
# Load corresponding points
folder = os.path.join(PKG_PATH, CALIB_PATH)
if points2D is None: points2D = np.load(os.path.join(folder, 'img_corners.npy'))
if points3D is None: points3D = np.load(os.path.join(folder, 'pcl_corners.npy'))
# Check points shape
assert(points2D.shape[0] == points3D.shape[0])
if not (points2D.shape[0] >= 5):
rospy.logwarn('PnP RANSAC Requires minimum 5 points')
return
# Obtain camera matrix and distortion coefficients
camera_matrix = CAMERA_MODEL.intrinsicMatrix()
dist_coeffs = CAMERA_MODEL.distortionCoeffs()
# Estimate extrinsics
success, rotation_vector, translation_vector, inliers = cv2.solvePnPRansac(points3D,
points2D, camera_matrix, dist_coeffs, flags=cv2.SOLVEPNP_ITERATIVE)
# Compute re-projection error.
points2D_reproj = cv2.projectPoints(points3D, rotation_vector,
translation_vector, camera_matrix, dist_coeffs)[0].squeeze(1)
assert(points2D_reproj.shape == points2D.shape)
error = (points2D_reproj - points2D)[inliers] # Compute error only over inliers.
#rmse = np.sqrt(np.mean(error[:, 0] ** 2 + error[:, 1] ** 2))
rospy.loginfo('Error: ' + str(error))
# Refine estimate using LM
if not success:
rospy.logwarn('Initial estimation unsuccessful, skipping refinement')
elif not hasattr(cv2, 'solvePnPRefineLM'):
rospy.logwarn('solvePnPRefineLM requires OpenCV >= 4.1.1, skipping refinement')
else:
assert len(inliers) >= 3, 'LM refinement requires at least 3 inlier points'
rotation_vector, translation_vector = cv2.solvePnPRefineLM(points3D[inliers],
points2D[inliers], camera_matrix, dist_coeffs, rotation_vector, translation_vector)
# Compute re-projection error.
points2D_reproj = cv2.projectPoints(points3D, rotation_vector,
translation_vector, camera_matrix, dist_coeffs)[0].squeeze(1)
assert(points2D_reproj.shape == points2D.shape)
error = (points2D_reproj - points2D)[inliers] # Compute error only over inliers.
#rmse = np.sqrt(np.mean(error[:, 0] ** 2 + error[:, 1] ** 2))
rospy.loginfo('Error: ' + str(error))
# Convert rotation vector
rotation_matrix = cv2.Rodrigues(rotation_vector)[0]
euler = euler_from_matrix(rotation_matrix)
# Save extrinsics
np.savez(os.path.join(folder, 'extrinsics.npz'),
euler=euler, R=rotation_matrix, T=translation_vector.T)
# Display results
print('Euler angles (RPY):', euler)
print('Rotation Matrix:', rotation_matrix)
print('Translation Offsets:', translation_vector.T)
def calibrate():
'''
Calibrate the LiDAR and image points using OpenCV PnP RANSAC, Requires minimum 5 point correspondences
Inputs:
points2D - [numpy array] - (N, 2) array of image points
points3D - [numpy array] - (N, 3) array of 3D points
Outputs:
Extrinsics saved in PKG_PATH/CALIB_PATH/extrinsics.npz
'''
# Load corresponding points
points2D = np.load(
'/home/eugen/catkin_ws/src/Camera_Lidar/scripts/img_corners.npy')
points3D = np.load(
'/home/eugen/catkin_ws/src/Camera_Lidar/scripts/pcl_corners.npy')
print('points2D:{}, points3D:{}'.format(np.shape(points2D),
np.shape(points3D)))
if not (points2D.shape[0] >= 5):
print('PnP RANSAC Requires minimum 5 points')
return
# Obtain camera matrix and distortion coefficients
K = np.array([[484.130454, 0.000000, 457.177461],
[0.000000, 484.452449, 364.861413],
[0.000000, 0.000000, 1.000000]])
d = np.array([-0.199619, 0.068964, 0.003371, 0.000296, 0.000000])
# Estimate extrinsics
success, rotation_vector, t, _ = cv2.solvePnPRansac(
points3D, points2D, K, d, flags=cv2.SOLVEPNP_ITERATIVE)
print('success ', success)
# Refine estimate using LM
if not success:
print('Initial estimation unsuccessful, skipping refinement')
elif not hasattr(cv2, 'solvePnPRefineLM'):
print('solvePnPRefineLM requires OpenCV >= 4.1.1, skipping refinement')
else:
print('Here')
rotation_vector, t = cv2.solvePnPRefineLM(points3D, points2D, K, d,
rotation_vector, t)
# Convert rotation vector
R = cv2.Rodrigues(rotation_vector)[0]
euler = None #euler_from_matrix(R)
# Save extrinsics
np.savez(os.path.join('/home/eugen/catkin_ws/src/Camera_Lidar/scripts',
'extrinsics.npz'),
euler=euler,
R=R,
T=t.T)
# Display results
print('Euler angles (RPY):', euler)
print('Rotation Matrix:', R)
print('Translation Offsets:', t.T)
def calibrate(points2D=None, points3D=None):
# Load corresponding points
folder = os.path.join(PKG_PATH, CALIB_PATH)
if points2D is None:
points2D = np.load(os.path.join(folder, 'img_corners.npy'))
if points3D is None:
points3D = np.load(os.path.join(folder, 'pcl_corners.npy'))
# Check points shape
assert (points2D.shape[0] == points3D.shape[0])
if not (points2D.shape[0] >= 5):
rospy.logwarn('PnP RANSAC Requires minimum 5 points')
return
# Obtain camera matrix and distortion coefficients
camera_matrix = CAMERA_MODEL.intrinsicMatrix()
dist_coeffs = CAMERA_MODEL.distortionCoeffs()
# Estimate extrinsics
success, rotation_vector, translation_vector, _ = cv2.solvePnPRansac(
points3D,
points2D,
camera_matrix,
dist_coeffs,
flags=cv2.SOLVEPNP_ITERATIVE)
# Refine estimate using LM
if not success:
rospy.logwarn('Initial estimation unsuccessful, skipping refinement')
elif not hasattr(cv2, 'solvePnPRefineLM'):
rospy.logwarn(
'solvePnPRefineLM requires OpenCV >= 4.1.1, skipping refinement')
else:
rotation_vector, translation_vector = cv2.solvePnPRefineLM(
points3D, points2D, camera_matrix, dist_coeffs, rotation_vector,
translation_vector)
# Convert rotation vector
rotation_matrix = cv2.Rodrigues(rotation_vector)[0]
euler = euler_from_matrix(rotation_matrix)
# Save extrinsics
np.savez(os.path.join(folder, 'extrinsics.npz'),
euler=euler,
R=rotation_matrix,
T=translation_vector.T)
# Display results
print('Euler angles (RPY):', euler)
print('Rotation Matrix:', rotation_matrix)
print('Translation Offsets:', translation_vector.T)
def calib(self, imgPath, cameraCalibPath):
# imgFileList = readFileList(imgFolder)
data = np.load(cameraCalibPath)
camMtx = data["mtx"]
dist = data["dist"]
objP_pixel = np.ceil(self.readCheckerObjPoint(self.checker_file))
objP_pixel[:, 2] = 0
objP = np.array(objP_pixel)
for i in range(self.checker_pattern[1]):
for j in range(math.floor(self.checker_pattern[0] / 2)):
tmp = objP[self.checker_pattern[0] * i + j, 0]
objP[self.checker_pattern[0] * i + j,
0] = objP[self.checker_pattern[0] * i +
self.checker_pattern[0] - j - 1, 0]
objP[self.checker_pattern[0] * i + self.checker_pattern[0] -
j - 1, 0] = tmp
objP[:, 0] -= (self.display_width / 2 - 1)
objP[:, 1] -= (self.display_height / 2 - 1)
objP *= self.checker_size
rtA = []
rB = []
tB = []
rC2Ss = []
tC2Ss = []
# define valid image
validImg = -1
# for i in trange(len(imgFileList), desc="Images"):
for i in range(1):
img = cv2.imread(imgPath, cv2.IMREAD_UNCHANGED)
# Yunhao
# img = (img/65535*255).astype(np.uint8)
# Aruco marker for Mirror position
cornersAruco, ids = self.detectAruco(img, debug=False)
if cornersAruco is None and ids is None and len(cornersAruco) <= 3:
continue
# Checker for Display
ret, cornersChecker = self.detectChecker(img, debug=False)
if not ret:
print("no Checker!!!")
continue
# for a valid image, aruco and checker must be both detected
validImg += 1
# Calibrate Mirror Pose with Aruco
rvecMirror, tvecMirror = self.postEst(cornersAruco, ids, camMtx,
dist)
img_axis = aruco.drawAxis(img, camMtx, dist, rvecMirror,
tvecMirror, self.aruco_size)
cv2.namedWindow('Img_axis', cv2.WINDOW_NORMAL)
cv2.imshow('Img_axis', img_axis)
cv2.waitKey(0) # any key
cv2.destroyWindow('Img_axis')
## Reproejct Camera Extrinsic
self.reProjAruco(img, camMtx, dist, rvecMirror, tvecMirror,
cornersAruco)
rMatMirror, _ = cv2.Rodrigues(
rvecMirror) # rotation vector to rotation matrix
normalMirror = rMatMirror[:, 2]
rC2W, tC2W = self.invTransformation(rMatMirror, tvecMirror)
dW2C = abs(np.dot(normalMirror, tvecMirror))
# Householder transformation
p1, p2, p3 = self.householderTransform(normalMirror, dW2C)
# Calibrate virtual to Camera with Checker
rpe, rvecVirtual, tvecVirtual = cv2.solvePnP(
objP, cornersChecker, camMtx, dist, flags=cv2.SOLVEPNP_IPPE
) # cv2.SOLVEPNP_IPPE for 4 point solution #cv2.SOLVEPNP_ITERATIVE
# iterationsCount=200, reprojectionError=8.0,
rvecVirtual, tvecVirtual = cv2.solvePnPRefineLM(
objP, cornersChecker, camMtx, dist, rvecVirtual, tvecVirtual)
proj, jac = cv2.projectPoints(objP, rvecVirtual, tvecVirtual,
camMtx, dist)
img_rep = img
cv2.drawChessboardCorners(img_rep, self.checker_size, proj, True)
width = 960
height = int(img_rep.shape[0] * 960 / img_rep.shape[1])
smallimg = cv2.resize(img_rep, (width, height))
cv2.imshow("img_rep", smallimg)
cv2.waitKey(0) # any key
cv2.destroyWindow("img_rep")
rMatVirtual, _ = cv2.Rodrigues(
rvecVirtual) # rotation vector to rotation matrix
print(tvecVirtual)
if validImg == 0:
rtA = p1
rB = np.matmul(rMatVirtual, p2)
tB = np.squeeze(tvecVirtual) + p3
else:
rtA = np.concatenate((rtA, p1))
rB = np.concatenate((rB, np.matmul(rMatVirtual, p2)))
tB = np.concatenate((tB, np.squeeze(tvecVirtual) + p3))
rS2C = p1 @ rMatVirtual
tS2C = p1 @ np.squeeze(tvecVirtual) + p3
rC2S, tC2S = self.invTransformation(rS2C, tS2C)
print("rC2S:", rC2S)
print("tC2S:", tC2S)
rC2Ss.append(rC2S)
tC2Ss.append(tC2S)
# rC2Ss = np.array(rC2Ss)
# tC2Ss = np.array(tC2Ss)
# fout = os.path.join(imgFolder, "Cam2Screen.npz")
# np.savez(fout, rC2S=rC2Ss, tC2S=tC2Ss)
return rC2Ss, tC2Ss
def calib(imgPath, camMtx, dist, half_length, half_height, displayScaleFactor):
ARUCO_BOARD_HEIGHT = half_height * 2
ARUCO_BOARD_WIDTH = half_length * 2
CHECKSQUARE_SIZE = displayScaleFactor
# objP_pixel = np.ceil(readCheckerObjPoint(CHECKER_FILE))
objP_pixel = np.ceil(generateObjP(14, 10, 80))
objP_pixel[:, 2] = 0
objP = np.array(objP_pixel)
for i in range(CHECKERPATTERN_SIZE[1]):
for j in range(math.floor(CHECKERPATTERN_SIZE[0] / 2)):
tmp = objP[CHECKERPATTERN_SIZE[0] * i + j, 0]
objP[CHECKERPATTERN_SIZE[0] * i + j,
0] = objP[CHECKERPATTERN_SIZE[0] * i +
CHECKERPATTERN_SIZE[0] - j - 1, 0]
objP[CHECKERPATTERN_SIZE[0] * i + CHECKERPATTERN_SIZE[0] - j - 1,
0] = tmp
objP[:, 0] -= (SCREEN_WIDTH / 2 - 1)
objP[:, 1] -= (SCREEN_HEIGHT / 2 - 1)
objP *= CHECKSQUARE_SIZE
rtA = []
rB = []
tB = []
rC2Ss = []
tC2Ss = []
# define valid image
validImg = -1
#for i in trange(len(imgFileList), desc="Images"):
for i in range(1):
#img = cv2.imread(imgPath, cv2.IMREAD_UNCHANGED)
img = cv2.imread(imgPath)
# print(img.shape)
# Yunhao
# img = (img/65535*255).astype(np.uint8)
#print(len(img.shape))
# plt.imshow(img)
# plt.colorbar()
# Aruco marker for Mirror position
cornersAruco, ids = detectAruco(img, debug=False)
if cornersAruco is None and ids is None and len(cornersAruco) <= 3:
continue
# Checker for Display
# print(len(img.shape))
ret, cornersChecker = detectChecker(img,
CHECKERPATTERN_SIZE,
debug=True)
# print(ret)
# print(cornersChecker)
if not ret:
print("no Checker!!!")
continue
#for a valid image, aruco and checker must be both detected
validImg += 1
# Calibrate Mirror Pose with Aruco
rvecMirror, tvecMirror = postEst(cornersAruco, ids, camMtx, dist,
ARUCO_MARKER_LENGTH,
ARUCO_BOARD_HEIGHT, ARUCO_BOARD_WIDTH)
img_axis = aruco.drawAxis(img, camMtx, dist, rvecMirror, tvecMirror,
ARUCO_MARKER_LENGTH)
width = 960
height = int(img_axis.shape[0] * 960 / img_axis.shape[1])
smallimg = cv2.resize(img_axis, (width, height))
# plt.figure(figsize=(15,10))
# plt.imshow(smallimg)
# plt.show()
## Reproejct Camera Extrinsic
reProjAruco(img, camMtx, dist, rvecMirror, tvecMirror, cornersAruco)
rMatMirror, _ = cv2.Rodrigues(
rvecMirror) # rotation vector to rotation matrix
normalMirror = rMatMirror[:, 2]
rC2W, tC2W = invTransformation(rMatMirror, tvecMirror)
dW2C = abs(np.dot(normalMirror, tvecMirror))
# Householder transformation
p1, p2, p3 = householderTransform(normalMirror, dW2C)
# Calibrate virtual to Camera with Checker
rpe, rvecVirtual, tvecVirtual = cv2.solvePnP(
objP, cornersChecker, camMtx, dist, flags=cv2.SOLVEPNP_IPPE
) # cv2.SOLVEPNP_IPPE for 4 point solution #cv2.SOLVEPNP_ITERATIVE
#iterationsCount=200, reprojectionError=8.0,
rvecVirtual, tvecVirtual = cv2.solvePnPRefineLM(
objP, cornersChecker, camMtx, dist, rvecVirtual, tvecVirtual)
proj, jac = cv2.projectPoints(objP, rvecVirtual, tvecVirtual, camMtx,
dist)
img_rep = img
# cv2.drawChessboardCorners(img_rep, CHECKERPATTERN_SIZE, proj, True)
width = 960
height = int(img_rep.shape[0] * 960 / img_rep.shape[1])
smallimg = cv2.resize(img_rep, (width, height))
fig = plt.figure(figsize=(15, 10))
plt.imshow(smallimg)
imgPath = os.path.join(
os.path.join(os.path.join(os.getcwd(), 'CalibrationImages'),
'Geometric'), 'geoCalibResults')
fig.savefig(imgPath + '/chesssBoard' + '.png')
plt.show()
rMatVirtual, _ = cv2.Rodrigues(
rvecVirtual) # rotation vector to rotation matrix
print(tvecVirtual)
if validImg == 0:
rtA = p1
rB = np.matmul(rMatVirtual, p2)
tB = np.squeeze(tvecVirtual) + p3
else:
rtA = np.concatenate((rtA, p1))
rB = np.concatenate((rB, np.matmul(rMatVirtual, p2)))
tB = np.concatenate((tB, np.squeeze(tvecVirtual) + p3))
rS2C = p1 @ rMatVirtual
tS2C = p1 @ np.squeeze(tvecVirtual) + p3
rC2S, tC2S = invTransformation(rS2C, tS2C)
print("rC2S:", rC2S)
print("tC2S:", tC2S)
rC2Ss.append(rC2S)
tC2Ss.append(tC2S)
return rC2Ss, tC2Ss
def flow_to_trafo_PnP(*args, **kwargs):
"""
input:
real_br: torch.tensor torch.Size([2])
real_tl: torch.tensor torch.Size([2])
ren_br: torch.tensor torch.Size([2])
ren_tl: torch.tensor torch.Size([2])
flow_mask: torch.Size([480, 640])
u_map: torch.Size([480, 640])
v_map: torch.Size([480, 640])
K_ren: torch.Size([3, 3])
render_d: torch.Size([480, 640])
h_render: torch.Size([4, 4])
h_real_est: torch.Size([4, 4])
output:
suc: bool
h: torch.Size([4, 4])
"""
real_br = kwargs['real_br']
real_tl = kwargs['real_tl']
ren_br = kwargs['ren_br']
ren_tl = kwargs['ren_tl']
flow_mask = kwargs['flow_mask']
u_map = kwargs['u_map']
v_map = kwargs['v_map']
K_ren = kwargs['K_ren']
K_real = kwargs['K_real']
render_d = kwargs['render_d']
h_render = kwargs['h_render']
h_real_est = kwargs['h_real_est']
typ = u_map.dtype
# Grid for upsampled real
grid_real_h = torch.linspace(int(real_tl[0]) ,int(real_br[0]) , 480, device=u_map.device)[:,None].repeat(1,640)
grid_real_w = torch.linspace(int(real_tl[1]) ,int(real_br[1]) , 640, device=u_map.device)[None,:].repeat(480,1)
# Project depth map to the pointcloud real
cam_scale = 10000
real_pixels = torch.stack( [grid_real_w[flow_mask], grid_real_h[flow_mask], torch.ones(grid_real_h.shape, device = u_map.device, dtype= u_map.dtype)[flow_mask]], dim=1 ).type(typ)
grid_ren_h = torch.linspace(int(ren_tl[0]) ,int(ren_br[0]), 480, device=u_map.device)[:,None].repeat(1,640)
grid_ren_w = torch.linspace(int(ren_tl[1]) ,int(ren_br[1]) , 640, device=u_map.device)[None,:].repeat(480,1)
crop_d_pixels = torch.stack( [grid_ren_w.flatten(), grid_ren_h.flatten(), torch.ones(grid_ren_w.shape, device = u_map.device, dtype= torch.float64).flatten()], dim=1 ).type(typ)
K_inv = torch.inverse(K_ren.type(torch.float64)).type(typ)
P_crop_d = K_inv @ crop_d_pixels.T.type(typ)
P_crop_d = P_crop_d.type(torch.float64) * render_d.flatten() / cam_scale
P_crop_d = P_crop_d.T
render_d_ind_h = torch.linspace(0 ,479 , 480, device=u_map.device)[:,None].repeat(1,640)
render_d_ind_w= torch.linspace(0 ,639 , 640, device=u_map.device)[None,:].repeat(480,1)
render_d_ind_h = torch.clamp((render_d_ind_h - u_map).type(torch.float64) ,0,479).type( torch.long )[flow_mask]
render_d_ind_w = torch.clamp((render_d_ind_w - v_map).type(torch.float32),0,639).type( torch.long )[flow_mask]
index = render_d_ind_h*640 + render_d_ind_w # hacky indexing along two dimensions
P_crop_d = P_crop_d[index]
m = filter_pcd( P_crop_d)
if torch.sum(m) < 50:
return False, torch.eye(4, dtype= u_map.dtype, device=u_map.device )
P_crop_d = P_crop_d[ m ]
real_pixels = real_pixels[m]
P_ren = P_crop_d
# random shuffel
pts_trafo = min(P_ren.shape[0], 50000)
idx = torch.randperm( P_ren.shape[0] )[0:pts_trafo]
P_ren = P_ren[idx]
real_pixels = real_pixels[idx]
nr = 10000
# Move the rendered points to the origin
P_ren_in_origin = (get_H( P_ren ).type(typ) @ torch.inverse( h_render.type(torch.float64) ).type(typ).T) [:,:3]
# PNP estimation
objectPoints = P_ren_in_origin.clone().cpu().type(torch.float32).numpy()
imagePoints = real_pixels[:,:2].cpu().type(torch.float32).numpy()
dist = np.array( [[0.0,0.0,0.0,0.0]] )
if objectPoints.shape[0] < 8:
print(f'Failed due to missing corsspondences ({ objectPoints.shape[0]})')
return False, torch.eye(4, dtype= u_map.dtype, device=u_map.device )
# set current guess as the inital estimate
rvec = R.from_matrix(h_real_est[:3,:3].cpu().numpy()).as_rotvec().astype(np.float32)
tvec = h_real_est[:3,3].cpu().numpy().astype(np.float32)
# calculate PnP between the pixels coordinates in the real image and the corrosponding points in the origin frame
r_vec2, t_vec2 = cv2.solvePnPRefineLM(copy.deepcopy(objectPoints), \
copy.deepcopy(imagePoints),
K_real.cpu().type(torch.float32).numpy(),
dist,
copy.deepcopy(rvec),
copy.deepcopy( tvec))
r_vec2 = r_vec2[:,None]
# retval, r_vec2, t_vec2, inliers = cv2.solvePnPRansac(copy.deepcopy(objectPoints), \
# copy.deepcopy(imagePoints),
# K_real.cpu().numpy(),
# dist,
# iterationsCount = 1000, reprojectionError= 0.3)
h = rvec_tvec_to_H(r_vec2[:,0],t_vec2)
return True, torch.tensor(h, device=u_map.device ).type(u_map.dtype)
def test(val_loader, model, criterion, epoch, args):
batch_time = AverageMeter('Time', ':6.4f')
loss_meter = AverageMeter('Loss', ':6.4f')
correspondence_probability_meter = AverageMeter('Outlier-Inlier Prob', ':6.4f')
rotation_meter = AverageMeter('Rotation Error', ':6.4f')
translation_meter = AverageMeter('Translation Error', ':6.4f')
progress = ProgressMeter(
len(val_loader),
[batch_time, loss_meter, correspondence_probability_meter, rotation_meter, translation_meter],
prefix='Test: ')
poseloss = True
criterion.gamma = 1.0
model.eval()
with torch.no_grad():
end = time.time()
rotation_errors0, rotation_errors, rotation_errorsLM = [], [], []
translation_errors0, translation_errors, translation_errorsLM = [], [], []
reprojection_errors0, reprojection_errors, reprojection_errorsLM, reprojection_errorsGT = [], [], [], []
num_inliers0, num_inliers, num_inliersLM, num_inliersGT = [], [], [], []
thetas0, thetas, thetasLM = [], [], []
num_points_2d_list, num_points_3d_list = [], []
inference_times = []
start_time = time.time()
for batch_index, (p2d, p3d, R_gt, t_gt, C_gt, num_points_2d, num_points_3d) in enumerate(val_loader):
p2d = p2d.float()
p3d = p3d.float()
R_gt = R_gt.float()
t_gt = t_gt.float()
if args.frac_outliers == 0.0:
# Convert C_gt into matrix (stored as a b x n index tensor with outliers indexed by m)
m = p2d.size(-2)
C_gt = torch.nn.functional.one_hot(C_gt, num_classes=(m + 1))[:, :, :m].transpose(-2, -1).float()
elif args.frac_outliers > 0.0:
# Add random outliers:
# Get bounding boxes
bb2d_min = p2d.min(dim=-2)[0]
bb2d_width = p2d.max(dim=-2)[0] - bb2d_min
bb3d_min = p3d.min(dim=-2)[0]
bb3d_width = p3d.max(dim=-2)[0] - bb3d_min
num_outliers = int(args.frac_outliers * p2d.size(-2))
p2d_outliers = bb2d_width * torch.rand_like(p2d[:, :num_outliers, :]) + bb2d_min
p3d_outliers = bb3d_width * torch.rand_like(p3d[:, :num_outliers, :]) + bb3d_min
p2d = torch.cat((p2d, p2d_outliers), -2)
p3d = torch.cat((p3d, p3d_outliers), -2)
num_points_2d[0, 0] = p2d.size(-2)
num_points_3d[0, 0] = p3d.size(-2)
# Expand C_gt with outlier indices (b x n index tensor with outliers indexed by m)
b = p2d.size(0)
m = p2d.size(-2)
outlier_indices = C_gt.new_full((b, num_outliers), m)
C_gt = torch.cat((C_gt, outlier_indices), -1)
C_gt = torch.nn.functional.one_hot(C_gt, num_classes=(m + 1))[:, :, :m].transpose(-2, -1).float()
# For memory reasons, if num_points > 10000, downsample first
if p2d.size(-2) > 10000:
idx = torch.randint(p2d.size(-2), size=(10000,))
p2d = p2d[:, idx, :]
p3d = p3d[:, idx, :]
num_points_2d[0, 0] = p2d.size(-2)
num_points_3d[0, 0] = p3d.size(-2)
C_gt = C_gt[:, idx, :]
C_gt = C_gt[:, :, idx]
if args.gpu is not None:
p2d = p2d.cuda(args.gpu, non_blocking=True)
p3d = p3d.cuda(args.gpu, non_blocking=True)
R_gt = R_gt.cuda(args.gpu, non_blocking=True)
t_gt = t_gt.cuda(args.gpu, non_blocking=True)
C_gt = C_gt.cuda(args.gpu, non_blocking=True)
# Compute output
P, theta, theta0 = model(p2d, p3d, num_points_2d, num_points_3d, poseloss)
# Measure elapsed time for reporting (includes dataloading time, but not evaluation / loss time)
inference_time = (time.time() - start_time)
loss, losses = criterion(theta, P, R_gt, t_gt, C_gt)
loss_correspondence_probability = losses[0]
loss_meter.update(loss.item(), p2d.size(0))
correspondence_probability_meter.update(loss_correspondence_probability.item(), p2d.size(0))
if poseloss:
loss_rotation = losses[1]
loss_translation = losses[2]
rotation_meter.update(loss_rotation.item(), p2d.size(0))
translation_meter.update(loss_translation.item(), p2d.size(0))
# Compute refined pose estimate:
# 1. Find inliers based on RANSAC estimate
inlier_threshold = 1.0 * math.pi / 180.0 # 1 degree threshold for LM
C = correspondenceMatricesTheta(theta0, p2d, p3d, inlier_threshold)
K = np.float32(np.array([[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]))
dist_coeff = np.float32(np.array([0.0, 0.0, 0.0, 0.0]))
thetaLM = P.new_zeros((P.size(0), 6))
inlier_indices = C[0, ...].nonzero(as_tuple=True) # Assumes test batch size = 1
# Skip if point-set has < 4 inlier points:
if (inlier_indices[0].size()[0] >= 4):
p2d_np = p2d[0, inlier_indices[0], :].cpu().numpy()
p3d_np = p3d[0, inlier_indices[1], :].cpu().numpy()
rvec = theta0[0, :3].cpu().numpy()
tvec = theta0[0, 3:].cpu().numpy()
rvec, tvec = cv2.solvePnPRefineLM(p3d_np, p2d_np, K, dist_coeff, rvec, tvec)
if rvec is not None and tvec is not None:
thetaLM[0, :3] = torch.as_tensor(rvec, dtype=P.dtype, device=P.device).squeeze(-1)
thetaLM[0, 3:] = torch.as_tensor(tvec, dtype=P.dtype, device=P.device).squeeze(-1)
inlier_threshold = 0.1 * math.pi / 180.0 # 0.1 degree threshold for reported inlier count
rotation_errors0 += [rotationErrorsTheta(theta0, R_gt, eps=0.0).item()]
rotation_errors += [rotationErrorsTheta(theta, R_gt, eps=0.0).item()]
rotation_errorsLM += [rotationErrorsTheta(thetaLM, R_gt, eps=0.0).item()]
translation_errors0 += [translationErrorsTheta(theta0, t_gt).item()]
translation_errors += [translationErrorsTheta(theta, t_gt).item()]
translation_errorsLM += [translationErrorsTheta(thetaLM, t_gt).item()]
reprojection_errors0 += [reprojectionErrorsTheta(theta0, p2d, p3d, C_gt, eps=0.0).item()]
reprojection_errors += [reprojectionErrorsTheta(theta, p2d, p3d, C_gt, eps=0.0).item()]
reprojection_errorsLM += [reprojectionErrorsTheta(thetaLM, p2d, p3d, C_gt, eps=0.0).item()]
reprojection_errorsGT += [reprojectionErrors(R_gt, t_gt, p2d, p3d, C_gt, eps=0.0).item()]
num_inliers0 += [numInliersTheta(theta0, p2d, p3d, inlier_threshold).item()]
num_inliers += [numInliersTheta(theta, p2d, p3d, inlier_threshold).item()]
num_inliersLM += [numInliersTheta(thetaLM, p2d, p3d, inlier_threshold).item()]
num_inliersGT += [numInliers(R_gt, t_gt, p2d, p3d, inlier_threshold).item()]
num_points_2d_list += [num_points_2d[0].item()]
num_points_3d_list += [num_points_3d[0].item()]
inference_times += [inference_time]
thetas0 += [theta0[0, 0].item(), theta0[0, 1].item(), theta0[0, 2].item(), theta0[0, 3].item(), theta0[0, 4].item(), theta0[0, 5].item()]
thetas += [theta[0, 0].item(), theta[0, 1].item(), theta[0, 2].item(), theta[0, 3].item(), theta[0, 4].item(), theta[0, 5].item()]
thetasLM += [thetaLM[0, 0].item(), thetaLM[0, 1].item(), thetaLM[0, 2].item(), thetaLM[0, 3].item(), thetaLM[0, 4].item(), thetaLM[0, 5].item()]
# Measure elapsed time
batch_time.update(time.time() - end)
end = time.time()
start_time = time.time()
if batch_index % args.print_freq == 0:
progress.display(batch_index)
print('Loss: {loss.avg:6.4f}, Outlier-Inlier Prob: {correspondence_probability.avg:6.4f}, Rotation Error: {rot.avg:6.4f}, Translation Error: {transl.avg:6.4f}'
.format(loss=loss_meter, correspondence_probability=correspondence_probability_meter, rot=rotation_meter, transl=translation_meter))
# Print all results in a text file:
if args.poseloss == 0:
loss_string = 'LcLp'
else:
loss_string = 'Lc'
if args.frac_outliers == 0.0:
append_string = ''
elif args.frac_outliers > 0.0:
append_string = '_outliers_' + str(args.frac_outliers)
dataset_string = args.dataset
os.makedirs('./results', exist_ok=True)
os.makedirs('./results/' + dataset_string, exist_ok=True)
os.makedirs('./results/' + dataset_string + '/' + loss_string, exist_ok=True)
with open('./results/' + dataset_string + '/' + loss_string + '/results' + append_string + '.txt', 'w') as save_file:
save_file.write("rotation_errors0, rotation_errors, rotation_errorsLM, translation_errors0, translation_errors, translation_errorsLM, reprojection_errors0, reprojection_errors, reprojection_errorsLM, reprojection_errorsGT, num_inliers0, num_inliers, num_inliersLM, num_inliersGT, num_points_2d, num_points_3d, inference_time\n")
for i in range(len(rotation_errors)):
save_file.write("{:.9f}, {:.9f}, {:.9f}, {:.9f}, {:.9f}, {:.9f}, {:.9f}, {:.9f}, {:.9f}, {:.9f}, {}, {}, {}, {}, {}, {}, {:.9f}\n".format(
rotation_errors0[i], rotation_errors[i], rotation_errorsLM[i],
translation_errors0[i], translation_errors[i], translation_errorsLM[i],
reprojection_errors0[i], reprojection_errors[i], reprojection_errorsLM[i], reprojection_errorsGT[i],
num_inliers0[i], num_inliers[i], num_inliersLM[i], num_inliersGT[i],
num_points_2d_list[i], num_points_3d_list[i],
inference_times[i]
))
with open('./results/' + dataset_string + '/' + loss_string + '/poses' + append_string + '.txt', 'w') as save_file:
for i in range(len(rotation_errors)):
save_file.write("{:.9f}, {:.9f}, {:.9f}, {:.9f}, {:.9f}, {:.9f}, {:.9f}, {:.9f}, {:.9f}, {:.9f}, {:.9f}, {:.9f}, {:.9f}, {:.9f}, {:.9f}, {:.9f}, {:.9f}, {:.9f}\n".format(
thetas0[6*i + 0], thetas0[6*i + 1], thetas0[6*i + 2], thetas0[6*i + 3], thetas0[6*i + 4], thetas0[6*i + 5],
thetas[6*i + 0], thetas[6*i + 1], thetas[6*i + 2], thetas[6*i + 3], thetas[6*i + 4], thetas[6*i + 5],
thetasLM[6*i + 0], thetasLM[6*i + 1], thetasLM[6*i + 2], thetasLM[6*i + 3], thetasLM[6*i + 4], thetasLM[6*i + 5]
))
# Pickle all output camera poses:
poses = {}
poses["theta0"] = np.array(thetas0).reshape(-1, 6)
poses["theta"] = np.array(thetas).reshape(-1, 6)
poses["thetaLM"] = np.array(thetasLM).reshape(-1, 6)
pickle.dump(poses, open('./results/' + dataset_string + '/' + loss_string + '/poses' + append_string + '.pkl', 'wb'))
return loss_meter.avg