def test_clean_PC_set():
PC_set = {}
Hw2c = np.zeros((4,4))
cprams = np.zeros((1,9))
PC1 = PointCloud(label=0, ini_3D=None, ini_2D=[0,0], ini_frame=1, ini_Hw2c=Hw2c,cprams=cprams,inliners=None)
PC2 = PointCloud(label=1, ini_3D=[0,0,0], ini_2D=[10,10], ini_frame=0, ini_Hw2c=Hw2c, cprams=cprams, inliners=1)
PC3 = PointCloud(label=2, ini_3D=[10, 10, 10], ini_2D=[20, 20], ini_frame=0, ini_Hw2c=Hw2c, cprams=cprams, inliners=1)
PC4 = PointCloud(label=3, ini_3D=[20, 20, 20], ini_2D=[30, 30], ini_frame=1, ini_Hw2c=Hw2c + 1, cprams=cprams + 0.1, inliners=1)
PC5 = PointCloud(label=4, ini_3D=[30, 30, 30], ini_2D=[40, 40], ini_frame=1, ini_Hw2c=Hw2c + 1, cprams=cprams + 0.1, inliners=0)
PC6 = PointCloud(label=5, ini_3D=None, ini_2D=None, ini_frame=None, ini_Hw2c=Hw2c, cprams=cprams, inliners=None)
PC7 = PointCloud(label=6, ini_3D=None, ini_2D=None, ini_frame=None, ini_Hw2c=Hw2c, cprams=cprams, inliners=None)
# add new correspondences
PC2.newCorres(p2d=[100,100], cam_ind=1, Hwc2=Hw2c + 1, cprams=cprams + 0.1, inliners=0)
# add new correspondences
PC2.newCorres(p2d=[1000, 1000], cam_ind=2, Hwc2=Hw2c + 2, cprams=cprams + 0.2, 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
# do cleaning
PC_set_after_cleaning = clean_PC_set(PC_set)
points_3d, points_2d, points_indices, camera_indices, camera_params = convertPC(PC_set_after_cleaning)
print('------- final result: -------------')
print(points_2d)
print('------------------------------------')
print(camera_indices)
print('------------------------------------')
print(points_indices)
print('------------------------------------')
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 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
