def test_linearT(Hw2c_set, points_2d, points_3d, points_indices,
camera_indices, camera_params, gt_points_3d):
n_points = points_3d.shape[0]
n_cameras = camera_params.shape[0]
# Obtain first frame
print('\n---------------Testing linearT()------------------\n')
# collect point clouds
pkl_folder = 'pkl'
if not os.path.exists(pkl_folder):
os.makedirs(pkl_folder)
if not os.path.exists(os.path.join(pkl_folder, 'PC_set.pkl')):
PC_set = {}
gt_3D_set = {}
for p in range(points_3d.shape[0]):
p3d = points_3d[p, :]
gt_p3d = gt_points_3d[p]
gt_3D_set[p] = gt_p3d
print('- Point {0}/ {1}'.format(p, points_3d.shape[0]))
label = points_indices[p]
PC = PointCloud(label=label, ini_3D=p3d)
# Find 2d (u,v)
add2 = 0
for i in range(points_indices.shape[0]):
if points_indices[i] == label:
PC.newCorres(points_2d[i], camera_indices[i], Hw2c_set[i],
camera_params[camera_indices[i], :])
# add2 += 1
PC_set[p] = PC
PC_data = {}
PC_data['pred'] = PC_set
PC_data['gt'] = gt_3D_set
with open(os.path.join(pkl_folder, 'PC_set.pkl'), 'wb') as f:
pkl.dump(PC_data, f, pkl.HIGHEST_PROTOCOL)
else:
with open(os.path.join(pkl_folder, 'PC_set.pkl'), 'rb') as f:
PC_data = pkl.load(f)
PC_set = PC_data['pred']
gt_3D_set = PC_data['gt']
# Test linear
Errors_orig = []
Errors_pred = []
total_ignore = 0
for i in PC_set.keys():
cur_PC = PC_set[i]
gt_p3d = gt_3D_set[i]
if len(cur_PC.Pos2D) > 1:
orgerror = compare_with_groundtruth(PC_set[i].Pos3D, gt_p3d)
# Errors_orig.append(orgerror)
# print('\n * Point',cur_PC.Pos3D)
# print('* Has {0} frames'.format(len(cur_PC.Pos2D)))
cur_PC, opt_inliers = RansacLinearT(cur_PC, pu=0, pv=0)
if cur_PC == None:
print('-------- Ignore! ------------')
total_ignore += 1
PC_set.pop(i)
continue
PC_set[i] = cur_PC
# perror = compare_with_groundtruth(cur_PC.Pos3D, gt_p3d)
perror = linearT_errEst(cur_PC=cur_PC,
pu=0,
pv=0,
inliners=opt_inliers)
print('>> ', i, '/', len(PC_set.keys()),
' Error:',
orgerror, ' vs ', perror)
Errors_pred.append(perror)
# time.sleep(0.5)
print('Total errors:')
print('Original:', np.sum(np.array(Errors_orig)) / len(Errors_pred))
print('Pred:', np.sum(np.array(Errors_pred)) / len(Errors_pred))
print('Total ignore PC:', total_ignore)
# save the obtained PC_set to the file
points_3d, points_2d, points_indices, camera_indices, camera_params = util.convertPC(
PC_set, n_cameras=n_cameras, n_3dpoints=n_points)
DATA = {}
DATA['params'] = camera_params
DATA['cam_indices'] = camera_indices
DATA['points_indices'] = points_indices
DATA['2d'] = points_2d
DATA['3d'] = points_3d
with open('linearTed_online_data.pkl', 'wb') as f:
pkl.dump(DATA, f, pkl.HIGHEST_PROTOCOL)
plt.figure()
plt.scatter(range(len(Errors_pred)), Errors_pred)
plt.show()
return Errors_orig, Errors_pred
def NonLT_main(data_file='linearTed_our_data_PC_set.pkl',
dump_file_name='linearTed_to_nonlinearT_our_data.pkl',
PC_set=None):
# camera_params with shape (n_cameras, 9) contains initial estimates of parameters for all cameras. First 3 components in each row form a rotation vector (https://en.wikipedia.org/wiki/Rodrigues%27_rotation_formula), next 3 components form a translation vector, then a focal distance and two distortion parameters.
# points_3d with shape (n_points, 3) contains initial estimates of point coordinates in the world frame.
# camera_ind with shape (n_observations,) contains indices of cameras (from 0 to n_cameras - 1) involved in each observation.
# point_ind with shape (n_observations,) contatins indices of points (from 0 to n_points - 1) involved in each observation.
# points_2d with shape (n_observations, 2) contains measured 2-D coordinates of points projected on images in each observations.
# open file to retrieve data
if PC_set == None:
with open(data_file, 'rb') as f:
PC_set = pkl.load(f)
PC_set = util.clean_PC_set(PC_set)
print('Doing NonlinearT')
points_3d, points_2d, points_indices, camera_indices, camera_params = util.convertPC(
PC_set)
n_cameras = camera_params.shape[0]
n_points = points_3d.shape[0]
n = 9 * n_cameras + 3 * n_points
m = 2 * points_2d.shape[0]
print("n_cameras: {}".format(n_cameras))
print("n_points: {}".format(n_points))
print("Total number of parameters: {}".format(n))
print("Total number of residuals: {}".format(m))
cprams = camera_params[0]
_, pu, pv = util.initial_PC()
K = np.array([[cprams[6], 0, pu], \
[0, cprams[6], pv], \
[0, 0, 1]])
x0 = points_3d.ravel()
f0 = fun(x0, camera_params, n_cameras, n_points, camera_indices,
points_indices, points_2d, K)
# plt.plot(f0)
A = nonlinear_triangulation_sparsity(n_points, camera_indices,
points_indices)
t0 = time.time()
res = least_squares(fun, x0, jac_sparsity = A, verbose = 2, x_scale = 'jac', ftol = 1e-4, method = 'trf', \
args = (camera_params, n_cameras, n_points, camera_indices, points_indices, points_2d,K))
t1 = time.time()
print('---Final params:', res.x[0:10])
print("Optimization took {0:.0f} seconds".format(t1 - t0))
plt.scatter(range(len(res.fun)), res.fun)
plt.show()
# Extract results from res.x
points_3d = np.reshape(res.x, (n_points, 3))
# Save data
# optimized data
DATA = {}
DATA['params'] = camera_params
DATA['cam_indices'] = camera_indices
DATA['points_indices'] = points_indices
DATA['2d'] = points_2d
DATA['3d'] = points_3d
with open(dump_file_name, 'wb') as f:
pkl.dump(DATA, f, pkl.HIGHEST_PROTOCOL)
def bundle(data_file='linearTed_our_data_PC_set.pkl',
input_type='PC_set',
dump_file_name='linearTed_to_bundle_online_data.pkl',
PC_set=None):
# camera_params with shape (n_cameras, 9) contains initial estimates of parameters for all cameras. First 3 components in each row form a rotation vector (https://en.wikipedia.org/wiki/Rodrigues%27_rotation_formula), next 3 components form a translation vector, then a focal distance and two distortion parameters.
# points_3d with shape (n_points, 3) contains initial estimates of point coordinates in the world frame.
# camera_ind with shape (n_observations,) contains indices of cameras (from 0 to n_cameras - 1) involved in each observation.
# point_ind with shape (n_observations,) contatins indices of points (from 0 to n_points - 1) involved in each observation.
# points_2d with shape (n_observations, 2) contains measured 2-D coordinates of points projected on images in each observations.
# open file to retrieve data
if PC_set == None and input_type == 'PC_set':
with open(data_file, 'rb') as f:
PC_set = pkl.load(f)
elif PC_set is not None:
points_3d, points_2d, points_indices, camera_indices, camera_params = util.convertPC(
PC_set)
else:
with open(data_file, 'rb') as f:
data = pkl.load(f)
camera_params = data['params']
camera_indices = data['cam_indices']
points_indices = data['points_indices']
points_2d = data['2d'] #
points_3d = data['3d']
n_cameras = camera_params.shape[0]
n_points = points_3d.shape[0]
n = 9 * n_cameras + 3 * n_points
m = 2 * points_2d.shape[0]
print("n_cameras: {}".format(n_cameras))
print("n_points: {}".format(n_points))
print("Total number of parameters: {}".format(n))
print("Total number of residuals: {}".format(m))
cprams = camera_params[0]
_, pu, pv = util.initial_PC()
K = np.array([[cprams[6], 0, pu], \
[0, cprams[6], pv], \
[0, 0, 1]])
x0 = np.hstack((camera_params.ravel(), points_3d.ravel()))
f0 = fun(x0, n_cameras, n_points, camera_indices, points_indices,
points_2d, K)
f0 = np.reshape(f0, (points_indices.shape[0], 2))
plt.subplot(1, 2, 1)
plt.plot(f0)
print('Done ploting')
A = bundle_adjustment_sparsity(n_cameras, n_points, camera_indices,
points_indices, PC_set)
t0 = time.clock()
res = least_squares(fun, x0, jac_sparsity = A, verbose = 2, x_scale = 'jac',\
ftol = 1e-4, method = 'trf', max_nfev=100,\
args = (n_cameras, n_points, camera_indices, points_indices, points_2d, K))
t1 = time.clock()
print('---Final params:', res.x[0:10])
final_rot, _ = cv2.Rodrigues(res.x[0:3])
# print('---Init rot:\n',init_rot)
print('---Final rot:\n', final_rot)
print("Optimization took {0:.0f} seconds".format(t1 - t0))
plt.subplot(1, 2, 2)
plt.plot(res.fun)
plt.show()
# Extract results from res.x
camera_params = np.reshape(res.x[0:n_cameras * 9], (n_cameras, 9))
points_3d = np.reshape(res.x[n_cameras * 9:], (n_points, 3))
# Save data
# optimized data
DATA = {}
DATA['params'] = camera_params
DATA['cam_indices'] = camera_indices
DATA['points_indices'] = points_indices
DATA['2d'] = points_2d
DATA['3d'] = points_3d
with open(dump_file_name, 'wb') as f:
pkl.dump(DATA, f, pkl.HIGHEST_PROTOCOL)
def apply_linearT(etol=100000):
'''etol: error tolaration for each point cloud. If the reprojection error is higher than this value, just don't
do triangulation.'''
# Intialize PC set
PC_set, pu, pv = util.initial_PC()
print(pu, pv)
# Apply linearT
Errors_pred = []
total_ignore = 0
for i in PC_set.keys():
cur_PC = PC_set[i]
if len(cur_PC.Pos2D) <= 1:
print(
'-------- Ignore because there is only 1 correspondence! ------------'
)
total_ignore += 1
continue
if len(cur_PC.Pos2D) >= 2:
# Use ransacLinearT
# TODO: using window truncatation of 10
cur_PC, opt_inliers = RansacLinearT(cur_PC=cur_PC,
window_frame=10,
pu=pu,
pv=pv)
# If return None, ignore the point
if cur_PC == None:
print('-------- Ignore due to return None! ------------')
total_ignore += 1
continue
# perror = compare_with_groundtruth(cur_PC.Pos3D, gt_p3d)
perror = linearT_errEst(cur_PC=cur_PC,
pu=pu,
pv=pv,
inliners=opt_inliers)
print('>> ', i, '/', len(PC_set.keys()),
' Error:', perror)
PC_set[i] = cur_PC
if perror > etol:
cur_PC.Pos3D = None
print('-------- Ignore! ------------')
total_ignore += 1
continue
# Check why error is large
# if perror > 100:
# print('Why it is larger than 100?')
# time.sleep(2)
Errors_pred.append(perror)
print('Total errors Pred:',
np.sum(np.array(Errors_pred)) / len(Errors_pred))
print('Total ignore PC:', total_ignore)
plt.figure()
plt.scatter(range(len(Errors_pred)), Errors_pred)
plt.show()
# save the obtained PC_set to the file
points_3d, points_2d, points_indices, camera_indices, camera_params = util.convertPC(
PC_set)
DATA = {}
DATA['params'] = camera_params
DATA['cam_indices'] = camera_indices
DATA['points_indices'] = points_indices
DATA['2d'] = points_2d
DATA['3d'] = points_3d
with open('pkl/linearTed.pkl', 'wb') as f:
pkl.dump(DATA, f, pkl.HIGHEST_PROTOCOL)
# save PC_set:
with open('pkl/linearTed_PC_set.pkl', 'wb') as f:
pkl.dump(PC_set, f, pkl.HIGHEST_PROTOCOL)
return Errors_pred
def test_bundle_sparse_matrix():
PC_set = {}
PC1 = PointCloud(label=0,
ini_3D=None,
ini_2D=[0, 0],
ini_frame=1,
ini_Hw2c=None,
cprams=None,
inliners=None)
PC2 = PointCloud(label=1,
ini_3D=[0, 0, 0],
ini_2D=[10, 10],
ini_frame=0,
ini_Hw2c=None,
cprams=None,
inliners=1)
PC3 = PointCloud(label=2,
ini_3D=[10, 10, 10],
ini_2D=[20, 20],
ini_frame=0,
ini_Hw2c=None,
cprams=None,
inliners=1)
PC4 = PointCloud(label=3,
ini_3D=[20, 20, 20],
ini_2D=[30, 30],
ini_frame=1,
ini_Hw2c=None,
cprams=None,
inliners=1)
PC5 = PointCloud(label=4,
ini_3D=[30, 30, 30],
ini_2D=[40, 40],
ini_frame=1,
ini_Hw2c=None,
cprams=None,
inliners=0)
PC6 = PointCloud(label=5,
ini_3D=None,
ini_2D=None,
ini_frame=None,
ini_Hw2c=None,
cprams=None,
inliners=None)
PC7 = PointCloud(label=6,
ini_3D=None,
ini_2D=None,
ini_frame=None,
ini_Hw2c=None,
cprams=None,
inliners=None)
# add new correspondences
PC2.newCorres(p2d=[100, 100],
cam_ind=1,
Hwc2=None,
cprams=None,
inliners=0)
# add new correspondences
PC2.newCorres(p2d=[1000, 1000],
cam_ind=2,
Hwc2=None,
cprams=None,
inliners=1)
PC_set[1] = PC1
PC_set[2] = PC2
PC_set[3] = PC3
PC_set[4] = PC4
PC_set[5] = PC5
PC_set[6] = PC6
PC_set[7] = PC7
points_3d, points_2d, points_indices, camera_indices, camera_params = util.convertPC(
PC_set)
print('Set of points_2d:')
print(points_2d)
n_cameras = camera_params.shape[0]
n_points = points_3d.shape[0]
A = bundle_adjustment_sparsity(n_cameras, n_points, camera_indices,
points_indices, PC_set)
print A