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 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)
