def absolute_pose_ransac(bs, Xs, method, threshold, iterations, probabilty):
# in-house estimation
if in_house_multiview:
threshold = np.arccos(1 - threshold)
params = pyrobust.RobustEstimatorParams()
params.iterations = 1000
result = pyrobust.ransac_absolute_pose(bs, Xs, threshold, params,
pyrobust.RansacType.RANSAC)
Rt = result.lo_model.copy()
R, t = Rt[:3, :3].copy(), Rt[:, 3].copy()
Rt[:3, :3] = R.T
Rt[:, 3] = -R.T.dot(t)
return Rt
else:
try:
return pyopengv.absolute_pose_ransac(bs,
Xs,
method,
threshold,
iterations=iterations,
probabilty=probabilty)
except Exception:
# Older versions of pyopengv do not accept the probability argument.
return pyopengv.absolute_pose_ransac(bs, Xs, method, threshold,
iterations)
def absolute_pose_known_rotation_ransac(bs, Xs, method, threshold, iterations,
probabilty):
# in-house estimation
if in_house_multiview:
threshold = np.arccos(1 - threshold)
params = pyrobust.RobustEstimatorParams()
params.iterations = 1000
result = pyrobust.ransac_absolute_pose_known_rotation(
bs, Xs, threshold, params, pyrobust.RansacType.RANSAC)
t = -result.lo_model.copy()
R = np.identity(3)
return np.concatenate((R, [[t[0]], [t[1]], [t[2]]]), axis=1)
else:
try:
return pyopengv.absolute_pose_ransac(bs,
Xs,
method,
threshold,
iterations=iterations,
probabilty=probabilty)
except Exception:
# Older versions of pyopengv do not accept the probability argument.
return pyopengv.absolute_pose_ransac(bs, Xs, method, threshold,
iterations)
def absolute_pose_ransac(bs, Xs, method, threshold, iterations, probabilty):
try:
return pyopengv.absolute_pose_ransac(
bs, Xs, method, threshold,
iterations=iterations,
probabilty=probabilty)
except Exception:
# Older versions of pyopengv do not accept the probability argument.
return pyopengv.absolute_pose_ransac(
bs, Xs, method, threshold, iterations)
def absolute_pose_ransac(bs, Xs, method, threshold, iterations, probabilty):
try:
return pyopengv.absolute_pose_ransac(
bs, Xs, method, threshold,
iterations=iterations,
probabilty=probabilty)
except Exception:
# Older versions of pyopengv do not accept the probability argument.
return pyopengv.absolute_pose_ransac(
bs, Xs, method, threshold, iterations)
def resect(data, graph, reconstruction, shot_id):
"""Try resecting and adding a shot to the reconstruction.
Return:
True on success.
"""
exif = data.load_exif(shot_id)
camera = reconstruction.cameras[exif['camera']]
# 1. collect all tracks that is in the reconstruction and this image
# pixel bearing and reconstructed 3D positions
bs = []
Xs = []
for track in graph[shot_id]:
if track in reconstruction.points:
x = graph[track][shot_id]['feature']
b = camera.pixel_bearing(x)
bs.append(b)
Xs.append(reconstruction.points[track].coordinates)
bs = np.array(bs)
Xs = np.array(Xs)
if len(bs) < 5:
return False
# 2. estimate the pose of this camera using KNEIP method
threshold = data.config.get('resection_threshold', 0.004)
T = pyopengv.absolute_pose_ransac(
bs, Xs, "KNEIP", 1 - np.cos(threshold), 1000)
R = T[:, :3]
t = T[:, 3]
# 3. reproject all points and figure out which is inliers
reprojected_bs = R.T.dot((Xs - t).T).T
reprojected_bs /= np.linalg.norm(reprojected_bs, axis=1)[:, np.newaxis]
inliers = np.linalg.norm(reprojected_bs - bs, axis=1) < threshold
ninliers = sum(inliers)
# 4. output resecting inliners
logger.info("{} resection inliers: {} / {}".format(
shot_id, ninliers, len(bs)))
if ninliers >= data.config.get('resection_min_inliers', 15):
# 5. if inliers are enough, then add this shot to the reconstruction
R = T[:, :3].T
t = -R.dot(T[:, 3])
shot = types.Shot()
shot.id = shot_id
shot.camera = camera
shot.pose = types.Pose()
shot.pose.set_rotation_matrix(R)
shot.pose.translation = t
shot.metadata = get_image_metadata(data, shot_id)
reconstruction.add_shot(shot)
# 6. and do single view bundle adjustment
bundle_single_view(graph, reconstruction, shot_id, data.config)
return True
else:
return False
def calc_inintial_guess(corners_in_img_arr, corners_in_pcd_arr, method="UPNP"):
# number_of_points_for_initial = int(corners_in_img_arr.shape[0])
img_bearing_vectors = []
if params["camera_type"] == "panoramic":
for pix in corners_in_img_arr:
angs = pixel2angle(pix)
img_bearing_vectors.append([
np.cos(angs[0]) * np.cos(angs[1]),
np.cos(angs[0]) * np.sin(angs[1]),
np.sin(angs[0])
])
elif params["camera_type"] == "perspective":
inv_K = np.linalg.inv(intrinsic_paras)
tmp_corners_in_img = np.hstack([
corners_in_img_arr,
1 + np.zeros(corners_in_img_arr.shape[0]).reshape(-1, 1)
])
for pix in tmp_corners_in_img:
tmp = np.dot(inv_K, pix.T).T
img_bearing_vectors.append(tmp / np.linalg.norm(tmp))
else:
raise Exception("camera_type define error!")
img_bearing_vectors = np.array(img_bearing_vectors)
pcd_bearing_vectors = np.array(corners_in_pcd_arr) / np.linalg.norm(
corners_in_pcd_arr, axis=1).reshape(-1, 1)
#
# ransac_transformation = pyopengv.relative_pose_ransac(img_bearing_vectors, pcd_bearing_vectors, "NISTER", 0.01,
# 1000)
# # ransac_transformation = pyopengv.relative_pose_fivept_kneip(img_bearing_vectors, pcd_bearing_vectors)
if method == "RANSAC":
transformation = pyopengv.absolute_pose_ransac(img_bearing_vectors,
pcd_bearing_vectors,
"UPNP", 0.001, 100000)
elif method == "EPNP":
transformation = pyopengv.absolute_pose_epnp(img_bearing_vectors,
pcd_bearing_vectors)
elif method == "UPNP":
transformation = pyopengv.absolute_pose_upnp(img_bearing_vectors,
pcd_bearing_vectors)[0]
else:
raise Exception("Opengv method error!")
print "initial guess by relative pose: ", transformation
# print ransac_transformation
angs = rotationMatrixToEulerAngles(transformation[:3, :3].T).tolist()
ret = []
ret.extend(angs)
# scale = least_squares(cal_scale_cost, x0=np.random.random(),
# args=(img_bearing_vectors, pcd_bearing_vectors, corners_in_pcd_arr, ransac_transformation),
# method="lm", ftol=1e-10, max_nfev=20000)
# print scale
# print "estimated scale: ", scale.x
# print scale.x * ransac_transformation[:3, 3]
# ret.extend((scale.x * ransac_transformation[:3, 3]).tolist())
ret.extend((-transformation[:3, 3]).tolist())
return np.array(ret)
def resect(data, graph, reconstruction, shot_id):
"""Try resecting and adding a shot to the reconstruction.
Return:
True on success.
"""
exif = data.load_exif(shot_id)
camera = reconstruction.cameras[exif['camera']]
bs = []
Xs = []
for track in graph[shot_id]:
if track in reconstruction.points:
x = graph[track][shot_id]['feature']
b = camera.pixel_bearing(x)
bs.append(b)
Xs.append(reconstruction.points[track].coordinates)
bs = np.array(bs)
Xs = np.array(Xs)
if len(bs) < 5:
return False, {'num_common_points': len(bs)}
threshold = data.config['resection_threshold']
T = pyopengv.absolute_pose_ransac(bs, Xs, "KNEIP", 1 - np.cos(threshold),
1000)
R = T[:, :3]
t = T[:, 3]
reprojected_bs = R.T.dot((Xs - t).T).T
reprojected_bs /= np.linalg.norm(reprojected_bs, axis=1)[:, np.newaxis]
inliers = np.linalg.norm(reprojected_bs - bs, axis=1) < threshold
ninliers = sum(inliers)
logger.info("{} resection inliers: {} / {}".format(shot_id, ninliers,
len(bs)))
report = {
'num_common_points': len(bs),
'num_inliers': ninliers,
}
if ninliers >= data.config['resection_min_inliers']:
R = T[:, :3].T
t = -R.dot(T[:, 3])
shot = types.Shot()
shot.id = shot_id
shot.camera = camera
shot.pose = types.Pose()
shot.pose.set_rotation_matrix(R)
shot.pose.translation = t
shot.metadata = get_image_metadata(data, shot_id)
reconstruction.add_shot(shot)
bundle_single_view(graph, reconstruction, shot_id, data.config)
return True, report
else:
return False, report
def resect(data, graph, reconstruction, shot_id):
"""Try resecting and adding a shot to the reconstruction.
Return:
True on success.
"""
exif = data.load_exif(shot_id)
camera = reconstruction.cameras[exif['camera']]
bs = []
Xs = []
for track in graph[shot_id]:
if track in reconstruction.points:
x = graph[track][shot_id]['feature']
b = camera.pixel_bearing(x)
bs.append(b)
Xs.append(reconstruction.points[track].coordinates)
bs = np.array(bs)
Xs = np.array(Xs)
if len(bs) < 5:
return False, {'num_common_points': len(bs)}
threshold = data.config['resection_threshold']
T = pyopengv.absolute_pose_ransac(
bs, Xs, "KNEIP", 1 - np.cos(threshold), 1000)
R = T[:, :3]
t = T[:, 3]
reprojected_bs = R.T.dot((Xs - t).T).T
reprojected_bs /= np.linalg.norm(reprojected_bs, axis=1)[:, np.newaxis]
inliers = np.linalg.norm(reprojected_bs - bs, axis=1) < threshold
ninliers = int(sum(inliers))
logger.info("{} resection inliers: {} / {}".format(
shot_id, ninliers, len(bs)))
report = {
'num_common_points': len(bs),
'num_inliers': ninliers,
}
if ninliers >= data.config['resection_min_inliers']:
R = T[:, :3].T
t = -R.dot(T[:, 3])
shot = types.Shot()
shot.id = shot_id
shot.camera = camera
shot.pose = types.Pose()
shot.pose.set_rotation_matrix(R)
shot.pose.translation = t
shot.metadata = get_image_metadata(data, shot_id)
reconstruction.add_shot(shot)
bundle_single_view(graph, reconstruction, shot_id, data.config)
return True, report
else:
return False, report
def resect(data, graph, reconstruction, shot_id):
'''Add a shot to the reconstruction.
'''
exif = data.load_exif(shot_id)
camera = reconstruction.cameras[exif['camera']]
bs = []
Xs = []
for track in graph[shot_id]:
if track in reconstruction.points:
x = graph[track][shot_id]['feature']
b = camera.pixel_bearing(x)
bs.append(b)
Xs.append(reconstruction.points[track].coordinates)
bs = np.array(bs)
Xs = np.array(Xs)
if len(bs) < 5:
return False
threshold = data.config.get('resection_threshold', 0.004)
T = pyopengv.absolute_pose_ransac(bs, Xs, "KNEIP", 1 - np.cos(threshold),
1000)
R = T[:, :3]
t = T[:, 3]
reprojected_bs = R.T.dot((Xs - t).T).T
reprojected_bs /= np.linalg.norm(reprojected_bs, axis=1)[:, np.newaxis]
inliers = np.linalg.norm(reprojected_bs - bs, axis=1) < threshold
ninliers = sum(inliers)
print 'Resection', shot_id, 'inliers:', ninliers, '/', len(bs)
if ninliers >= data.config.get('resection_min_inliers', 15):
R = T[:, :3].T
t = -R.dot(T[:, 3])
shot = types.Shot()
shot.id = shot_id
shot.camera = camera
shot.pose = types.Pose()
shot.pose.set_rotation_matrix(R)
shot.pose.translation = t
shot.metadata = get_image_metadata(data, shot_id)
reconstruction.add_shot(shot)
bundle_single_view(graph, reconstruction, shot_id, data.config)
return True
else:
return False
def resect(data, graph, reconstruction, shot_id):
'''Add a shot to the reconstruction.
'''
exif = data.load_exif(shot_id)
camera = reconstruction.cameras[exif['camera']]
bs = []
Xs = []
for track in graph[shot_id]:
if track in reconstruction.points:
x = graph[track][shot_id]['feature']
b = camera.pixel_bearing(x)
bs.append(b)
Xs.append(reconstruction.points[track].coordinates)
bs = np.array(bs)
Xs = np.array(Xs)
if len(bs) < 5:
return False
threshold = data.config.get('resection_threshold', 0.004)
T = pyopengv.absolute_pose_ransac(bs, Xs, "KNEIP", 1 - np.cos(threshold), 1000)
R = T[:, :3]
t = T[:, 3]
reprojected_bs = R.T.dot((Xs - t).T).T
reprojected_bs /= np.linalg.norm(reprojected_bs, axis=1)[:, np.newaxis]
inliers = np.linalg.norm(reprojected_bs - bs, axis=1) < threshold
ninliers = sum(inliers)
print 'Resection', shot_id, 'inliers:', ninliers, '/', len(bs)
if ninliers >= data.config.get('resection_min_inliers', 15):
R = T[:, :3].T
t = -R.dot(T[:, 3])
shot = types.Shot()
shot.id = shot_id
shot.camera = camera
shot.pose = types.Pose()
shot.pose.set_rotation_matrix(R)
shot.pose.translation = t
shot.metadata = get_image_metadata(data, shot_id)
reconstruction.add_shot(shot)
bundle_single_view(graph, reconstruction, shot_id, data.config)
return True
else:
return False
def resect(data, graph, reconstruction, shot_id):
'''Add a shot to the reconstruction.
'''
exif = data.load_exif(shot_id)
camera_id = exif['camera']
camera = reconstruction['cameras'][camera_id]
bs = []
Xs = []
for track in graph[shot_id]:
if track in reconstruction['points']:
x = graph[track][shot_id]['feature']
b = multiview.pixel_bearing(x, camera)
bs.append(b)
Xs.append(reconstruction['points'][track]['coordinates'])
bs = np.array(bs)
Xs = np.array(Xs)
if len(bs) < 5:
return False
threshold = data.config.get('resection_threshold', 0.004)
T = pyopengv.absolute_pose_ransac(bs, Xs, "KNEIP", 1 - np.cos(threshold),
1000)
R = T[:, :3]
t = T[:, 3]
reprojected_bs = R.T.dot((Xs - t).T).T
reprojected_bs /= np.linalg.norm(reprojected_bs, axis=1)[:, np.newaxis]
inliers = np.linalg.norm(reprojected_bs - bs, axis=1) < threshold
ninliers = sum(inliers)
print 'Resection', shot_id, 'inliers:', ninliers, '/', len(bs)
if ninliers >= data.config.get('resection_min_inliers', 15):
R = cv2.Rodrigues(T[:, :3].T)[0].ravel()
t = -T[:, :3].T.dot(T[:, 3])
reconstruction['shots'][shot_id] = {
"camera": camera_id,
"rotation": list(R.flat),
"translation": list(t.flat),
}
add_gps_position(data, reconstruction['shots'][shot_id], shot_id)
bundle_single_view(graph, reconstruction, shot_id, data.config)
return True
else:
return False
def calc_inintial_guess(corners_in_img_arr, corners_in_pcd_arr, method="UPNP"):
# number_of_points_for_initial = int(corners_in_img_arr.shape[0])
img_bearing_vectors = []
for pix in corners_in_img_arr:
angs = pixel2angle(pix)
img_bearing_vectors.append([
np.cos(angs[0]) * np.cos(angs[1]),
np.cos(angs[0]) * np.sin(angs[1]),
np.sin(angs[0])
])
img_bearing_vectors = np.array(img_bearing_vectors)
pcd_bearing_vectors = np.array(corners_in_pcd_arr) / np.linalg.norm(
corners_in_pcd_arr, axis=1).reshape(-1, 1)
#
# ransac_transformation = pyopengv.relative_pose_ransac(img_bearing_vectors, pcd_bearing_vectors, "NISTER", 0.01,
# 1000)
# # ransac_transformation = pyopengv.relative_pose_fivept_kneip(img_bearing_vectors, pcd_bearing_vectors)
if method == "RANSAC":
transformation = pyopengv.absolute_pose_ransac(img_bearing_vectors,
pcd_bearing_vectors,
"UPNP", 0.001, 100000)
elif method == "EPNP":
transformation = pyopengv.absolute_pose_epnp(img_bearing_vectors,
pcd_bearing_vectors)
elif method == "UPNP":
transformation = pyopengv.absolute_pose_upnp(img_bearing_vectors,
pcd_bearing_vectors)[0]
# print "initial guess by relative pose: ", transformation
# print ransac_transformation
angs = rotationMatrixToEulerAngles(transformation[:3, :3].T).tolist()
ret = []
ret.extend(angs)
# scale = least_squares(cal_scale_cost, x0=np.random.random(),
# args=(img_bearing_vectors, pcd_bearing_vectors, corners_in_pcd_arr, ransac_transformation),
# method="lm", ftol=1e-10, max_nfev=20000)
# print scale
# print "estimated scale: ", scale.x
# print scale.x * ransac_transformation[:3, 3]
# ret.extend((scale.x * ransac_transformation[:3, 3]).tolist())
ret.extend((-transformation[:3, 3]).tolist())
return np.array(ret)
def test_absolute_pose_ransac():
outliers = 0.25
noise = 0.01
d = RelativePoseDataset(100, noise, outliers)
iterations = 100
th = 100000.
print(d.bearing_vectors2.shape)
return
ransac_transformation = pyopengv.absolute_pose_ransac(
d.bearing_vectors2, d.points, "KNEIP", th, iterations)
print("\n=========================================")
print("\n=========================================")
print("\nTest Absolute Pose RANSAC with :\n")
print(" - %s points" % len(d.points))
print(" - %s d'outliers" % (outliers * len(d.points)))
print(" - Threshold : %s / Iterations : %s" % (th, iterations))
print(
"\nResult / Truth: \n %s" % np.hstack(
(ransac_transformation[0][:, 3].reshape(
3, 1), d.position.reshape(3, 1))))
print(
"Diff : %s \n" % abs(ransac_transformation[0][:, 3].reshape(3, 1) -
d.position.reshape(3, 1)).mean())
print("Inliers : \n %s \n" % len(ransac_transformation[1]))
ransac_transformation = pyopengv.absolute_pose_ransac_optimize(
d.bearing_vectors2, d.points, "KNEIP", th, iterations)
print(
"Optimized Result / Truth: \n %s" % np.hstack(
(ransac_transformation[0][:, 3].reshape(
3, 1), d.position.reshape(3, 1))))
print(
"Diff : %s \n" % abs(ransac_transformation[0][:, 3].reshape(3, 1) -
d.position.reshape(3, 1)).mean())
print "Done testing absolute pose ransac"
def resect(data, graph, reconstruction, shot_id):
"""Add a shot to the reconstruction.
"""
exif = data.load_exif(shot_id)
camera_id = exif["camera"]
camera = reconstruction["cameras"][camera_id]
bs = []
Xs = []
for track in graph[shot_id]:
if track in reconstruction["points"]:
x = graph[track][shot_id]["feature"]
b = multiview.pixel_bearing(x, camera)
bs.append(b)
Xs.append(reconstruction["points"][track]["coordinates"])
bs = np.array(bs)
Xs = np.array(Xs)
if len(bs) < 5:
return False
threshold = data.config.get("resection_threshold", 0.004)
T = pyopengv.absolute_pose_ransac(bs, Xs, "KNEIP", 1 - np.cos(threshold), 1000)
R = T[:, :3]
t = T[:, 3]
reprojected_bs = R.T.dot((Xs - t).T).T
reprojected_bs /= np.linalg.norm(reprojected_bs, axis=1)[:, np.newaxis]
inliers = np.linalg.norm(reprojected_bs - bs, axis=1) < threshold
ninliers = sum(inliers)
print "Resection", shot_id, "inliers:", ninliers, "/", len(bs)
if ninliers >= data.config.get("resection_min_inliers", 15):
R = cv2.Rodrigues(T[:, :3].T)[0].ravel()
t = -T[:, :3].T.dot(T[:, 3])
reconstruction["shots"][shot_id] = {"camera": camera_id, "rotation": list(R.flat), "translation": list(t.flat)}
add_gps_position(data, reconstruction["shots"][shot_id], shot_id)
bundle_single_view(graph, reconstruction, shot_id, data.config)
return True
else:
return False
def PnP(pc_to_align, pc_ref, desc_to_align, desc_ref, init_T, K, **kwargs):
match_function = kwargs.pop('match_function', None)
desc_function = kwargs.pop('desc_function', None)
fit_pc = kwargs.pop('fit_pc', False)
pnp_algo = kwargs.pop('pnp_algo', 'GAO')
ransac_threshold = kwargs.pop('ransac_threshold', 0.0002)
iterations = kwargs.pop('iterations', 1000)
inliers_threshold = kwargs.pop('inliers_threshold', 0.1)
diff_max = kwargs.pop('diff_max', 4.0)
return_inliers_ratio = kwargs.pop('return_inliers_ratio', False)
unfit = kwargs.pop('unfit', True)
'''
Algo are: KNEIP - GAO - EPNP - TWOPT - GP3P
'''
timing = False
if timing:
t_beg = time.time()
if kwargs:
raise TypeError('Unexpected **kwargs: %r' % kwargs)
if desc_function is not None:
desc_ref = desc_function(pc_ref, desc_ref)
else:
desc_ref = pc_ref
pc_rec = init_T.matmul(pc_to_align)
if desc_function is not None:
desc_ta = desc_function(pc_rec, desc_to_align)
else:
desc_ta = pc_rec
if fit_pc:
#match_function.fit(pc_ref[0])
match_function.fit(pc_rec)
else:
#match_function.fit(desc_ref[0])
match_function.fit(desc_ta)
#res_match = match_function(pc_rec, pc_ref, desc_ta, desc_ref)
res_match = match_function(pc_ref, pc_rec, desc_ref, desc_ta)
if 'inliers' in res_match.keys():
#pc_to_align = pc_to_align[0, :, res_match['inliers'][0].byte()].unsqueeze(0)
pc_to_align = torch.inverse(init_T).matmul(
res_match['nn'][0, :, res_match['inliers'][0].byte()].unsqueeze(0))
#res_match['nn'] = res_match['nn'][0, :, res_match['inliers'][0].byte()].unsqueeze(0)
res_match['nn'] = pc_ref[0, :,
res_match['inliers'][0].byte()].unsqueeze(0)
if pc_to_align.size(2) < 4:
logger.warning(
"Less than 4 inliers founded, retuturning intial pose")
if unfit:
match_function.unfit()
res = {'T': init_T}
if return_inliers_ratio:
res['inliers'] = 0.0
return res
keypoints = reproject_back(pc_to_align, K.squeeze())
bearing_vector = keypoints_to_bearing(keypoints, K.squeeze())
non_nan_idx, _ = torch.min(bearing_vector == bearing_vector, dim=0)
bearing_vector = bearing_vector[:, non_nan_idx]
corr3d_pt = res_match['nn'][0, :3, non_nan_idx]
T = pyopengv.absolute_pose_ransac(bearing_vector.t().cpu().numpy(),
corr3d_pt.t().cpu().numpy(),
algo_name=pnp_algo,
threshold=ransac_threshold,
iterations=iterations)
with open("ransac_inliers.txt", 'r') as f:
inliers = int(f.read())
#algo_name = pnp_algo, threshold = 0.0002, iterations = 1000)
#T = pyopengv.absolute_pose_epnp(bearing_vector.t().cpu().numpy(), corr3d_pt.t().cpu().numpy())
if pc_to_align.device == 'gpu':
T = T.cuda()
if timing:
print('Iteration on {}s'.format(time.time() - t))
if unfit:
match_function.unfit()
if timing:
print('Pnp converge on {}s'.format(time.time() - t_beg))
inliers_ratio = inliers / pc_to_align.size(2)
final_T = pc_ref.new_zeros(4, 4)
final_T[3, 3] = 1.0
final_T[:3, :] = pc_ref.new_tensor(T)
T_diff = torch.norm(init_T[0] - final_T)
logger.debug('Inliers ratio: {}'.format(inliers_ratio))
logger.debug('Diff in pose is {}'.format(T_diff.item()))
if inliers_ratio < inliers_threshold or T_diff > diff_max:
logger.debug('Not enought inliers (ratio: {})'.format(inliers_ratio))
res = {'T': init_T}
if return_inliers_ratio:
res['inliers'] = inliers_ratio
return res
res = {'T': final_T.unsqueeze(0)}
if return_inliers_ratio:
res['inliers'] = inliers_ratio
return res
def test_relative_pose_triangulated():
# Creating Random Poses / Points
d = RelativePoseDataset(500, 0.002, 0.15)
result = pyopengv.relative_pose_ransac_optimize(d.bearing_vectors1,
d.bearing_vectors2,
"NISTER", 4., 1000)
E = result[0]
print("Testing mini Odom\n")
print("Relative Pose Computed : \n")
R = E[:, :3]
t = normalized(E[:, 3])
print("Truth : \n %s \n Normalized Truth : %s \n" %
(d.position, normalized(d.position)))
print("Got : \n %s \n Normalized translation : \n %s \n" % (E, t))
print("%s inliers / %s points \n" % (len(result[1]), len(d.points)))
bearing_vectors_to_erase = []
inliers = result[1]
for i in xrange(len(d.bearing_vectors1)):
if inliers != []:
if inliers[0] == i:
inliers.pop(0)
else:
bearing_vectors_to_erase.append(i)
else:
bearing_vectors_to_erase.append(i)
bv_1 = np.delete(d.bearing_vectors1, bearing_vectors_to_erase, axis=0)
bv_2 = np.delete(d.bearing_vectors2, bearing_vectors_to_erase, axis=0)
pts_3d = pyopengv.triangulation_triangulate2(bv_1, bv_2, t, R)
abs_pose = pyopengv.absolute_pose_ransac_optimize(bv_2, pts_3d, "KNEIP",
2., 500)
print("Absolute Pose Computed : \n %s \n" % abs_pose[0])
print("%s inliers / %s points \n" % (len(abs_pose[1]), len(pts_3d)))
# for i in xrange(len(d.points)):
# d.bearing_vectors2[i] = R.T.dot(d.points[i] - t)
result_ = pyopengv.absolute_pose_ransac(d.bearing_vectors2, d.points,
"KNEIP", 0.001, 1000)
print("\n----------------------\n")
print("TEST \n")
print("Truth : \n")
print(np.hstack((d.rotation, d.position.reshape(3, 1))))
print("Essential M. : \n")
print(E)
print()
print(np.hstack((R, t.reshape(3, 1))))
print()
print(np.hstack((R, t.reshape(3, 1))) / E)
print("\n Truth : \n")
print(d.points[:5])
print()
print(d.bearing_vectors1[:5])
print()
print((d.bearing_vectors1 / d.points)[:5])
print()
print("\nAbsolute Pose : \n")
print(normalized(result_[0][:, 3]))
print()
print(t)
