def ppf_icp_registration(source_cloud, target_cloud, n_points=3000, n_iter=100, tolerance=0.001, num_levels=5, scale=0.001):
""" align the source cloud to the target cloud using point pair feature (PPF) match
Args:
source_cloud (open3d.geometry.PointCloud): source open3d point cloud
target_cloud (open3d.geometry.PointCloud): target open3d point cloud
for other parameter, go to https://docs.opencv.org/master/dc/d9b/classcv_1_1ppf__match__3d_1_1ICP.html
Returns:
pose (np.array): 4x4 transformation between source and targe cloud
residual (float): the output resistration error
"""
source_cloud = copy.deepcopy(source_cloud)
source_cloud = source_cloud.voxel_down_sample(scale)
target_cloud = copy.deepcopy(target_cloud)
n_source_points = np.shape(source_cloud.points)[0]
n_target_points = np.shape(target_cloud.points)[0]
n_sample = np.min([n_source_points, n_target_points, n_points])
if n_sample == 0:
return None, 10000
if n_source_points > n_points:
source_idxes = np.random.choice(n_source_points, n_sample, replace=False)
source_cloud = source_cloud.select_down_sample(source_idxes)
if n_target_points > n_points:
target_idxes = np.random.choice(n_target_points, n_sample, replace=False)
target_cloud = target_cloud.select_down_sample(target_idxes)
target_cloud.estimate_normals(search_param=open3d.geometry.KDTreeSearchParamHybrid(radius=0.1, max_nn=30))
source_np_cloud = np.concatenate([np.asarray(source_cloud.points), np.asarray(source_cloud.normals)], axis=1).astype(np.float32)
target_np_cloud = np.concatenate([np.asarray(target_cloud.points), np.asarray(target_cloud.normals)], axis=1).astype(np.float32)
icp_fnc = cv2.ppf_match_3d_ICP(n_iter, tolerence=tolerance, rejectionScale=2.5, numLevels=num_levels)
try:
retval, residual, pose = icp_fnc.registerModelToScene(source_np_cloud, target_np_cloud)
except:
return None, 10000
else:
return pose, residual
def icp_refinement(pts_tgt, obj_model, rot_pred, tra_pred, cam_K, ren):
centroid_tgt = np.array([
np.mean(pts_tgt[:, 0]),
np.mean(pts_tgt[:, 1]),
np.mean(pts_tgt[:, 2])
])
if (tra_pred[2] < 300 or tra_pred[2] > 5000):
tra_pred = centroid_tgt * 1000
#opengl renderer (bit slower, more precise)
if (gpu_rendering):
img_init, depth_init = render_obj_gpu(obj_model, rot_pred,
tra_pred / 1000, cam_K, ren)
else:
img_init, depth_init = render_obj(obj_models[obj_order_id], rot_pred,
tra_pred / 1000, cam_K, ren)
init_mask = depth_init > 0
bbox_init = get_bbox_from_mask(init_mask > 0)
tf = np.eye(4)
if (bbox_init[2] - bbox_init[0] < 10 or bbox_init[3] - bbox_init[1] < 10):
return tf, -1
if (np.sum(init_mask) < 10):
return tf, -1
points_src = np.zeros(
(bbox_init[2] - bbox_init[0], bbox_init[3] - bbox_init[1], 6),
np.float32)
points_src[:, :, :3] = getXYZ(depth_init, cam_K[0, 0], cam_K[1, 1],
cam_K[0, 2], cam_K[1, 2], bbox_init)
points_src[:, :, 3:] = get_normal(depth_init,
fx=cam_K[0, 0],
fy=cam_K[1, 1],
cx=cam_K[0, 2],
cy=cam_K[1, 2],
refine=True,
bbox=bbox_init)
points_src = points_src[init_mask[bbox_init[0]:bbox_init[2],
bbox_init[1]:bbox_init[3]] > 0]
#adjust the initial translation using centroids of visible points
centroid_src = np.array([
np.mean(points_src[:, 0]),
np.mean(points_src[:, 1]),
np.mean(points_src[:, 2])
])
trans_adjust = centroid_tgt - centroid_src
tra_pred = tra_pred + trans_adjust * 1000
points_src[:, :3] += trans_adjust
icp_fnc = cv2.ppf_match_3d_ICP(100, tolerence=0.05, numLevels=4) #1cm
retval, residual, pose = icp_fnc.registerModelToScene(
points_src.reshape(-1, 6), pts_tgt.reshape(-1, 6))
tf[:3, :3] = rot_pred
tf[:3, 3] = tra_pred / 1000 #in m
tf = np.matmul(pose, tf)
return tf, residual
def icp_refinement(self, pts_tgt, obj_model, rot_pred, tra_pred):
centroid_tgt = np.array([
np.mean(pts_tgt[:, 0]),
np.mean(pts_tgt[:, 1]),
np.mean(pts_tgt[:, 2])
])
if (tra_pred[2] < 300 or tra_pred[2] > 5000):
#when estimated translation is weired, set centroid of tgt points as translation
tra_pred = centroid_tgt * 1000
img_init, depth_init = self.render_obj_pyrender(
obj_model, rot_pred, tra_pred / 1000)
init_mask = depth_init > 0
bbox_init = get_bbox_from_mask(init_mask > 0)
if (bbox_init[2] - bbox_init[0] < 10
or bbox_init[3] - bbox_init[1] < 10):
return tf, -1
if (np.sum(init_mask) < 10):
return tf, -1
points_src = np.zeros(
(bbox_init[2] - bbox_init[0], bbox_init[3] - bbox_init[1], 6),
np.float32)
points_src[:, :, :3] = getXYZ(depth_init, self.camK[0, 0],
self.camK[1, 1], self.camK[0, 2],
self.camK[1, 2], bbox_init)
points_src[:, :, 3:] = get_normal(depth_init,
fx=self.camK[0, 0],
fy=self.camK[1, 1],
cx=self.camK[0, 2],
cy=self.camK[1, 2],
refine=True,
bbox=bbox_init)
points_src = points_src[init_mask[bbox_init[0]:bbox_init[2],
bbox_init[1]:bbox_init[3]] > 0]
centroid_src = np.array([
np.mean(points_src[:, 0]),
np.mean(points_src[:, 1]),
np.mean(points_src[:, 2])
])
trans_adjust = centroid_tgt - centroid_src
tra_pred = tra_pred + trans_adjust * 1000
points_src[:, :3] += trans_adjust
icp_fnc = cv2.ppf_match_3d_ICP(100, tolerence=0.05, numLevels=5) #1cm
retval, residual, pose = icp_fnc.registerModelToScene(
points_src.reshape(-1, 6), pts_tgt.reshape(-1, 6))
tf = np.eye(4)
tf[:3, :3] = rot_pred
tf[:3, 3] = tra_pred / 1000 #in m
tf = np.matmul(pose, tf)
return tf, residual
def computeMatch(scene):
# pick a height
height = int(scene.height / 2)
# pick x coords near front and center
middle_x = int(scene.width / 2)
# examine point
middle, scenePCL = read_depth(middle_x, height, scene)
# do stuff with middle
pcd = open3d.geometry.PointCloud()
pcd.points = open3d.utility.Vector3dVector(scenePCL)
scenePCL = cv2.ppf_match_3d.loadPLYSimple(pcd, 1)
rospy.loginfo(scenePCL.shape)
#o3d.visualization.draw_geometries([pcd])
#rospy.loginfo(scenePCL.shape)
#scenePCL = scene
#xyz_array = ros_numpy.point_cloud2.get_xyz_points(scenePCL)
#matchPub(xyz_array)
# Find the matching for the scene we have with our detector
rospy.loginfo('Matching...')
resultsDetect = detector.match(pcd.points, 1.0 / 40.0, 0.05)
print('Performing ICP...')
icp = cv2.ppf_match_3d_ICP(100)
_, results = icp.registerModelToScene(pc, scenePCL, resultsDetect[:N])
print("Poses: ")
for i, result in enumerate(results):
result.printPose()
print(
"\n-- Pose to Model Index %d: NumVotes = %d, Residual = %f\n%s\n" %
(result.modelIndex, result.numVotes, result.residual, result.pose))
if i == 0:
pct = cv2.ppf_match_3d.transformPCPose(pc, result.pose)
rospy.loginfo('Finish reprojecting the Matching 3d object')
point_cloud2 = convert_numpy_2_pointcloud2_color(pct)
matchPub.publish(point_cloud2)
# Testing new cloud
"""
sparse=False,
indexing='xy')
z = np.zeros((height, width))
cloud[0] = np.dstack((x, y, z)).reshape((-1, 3)).astype(np.float32)
cloud[1] = noise.astype(np.float32) + cloud[0]
cloud[2] = cloud[1]
cloud[2][:, 2] += noise2.astype(np.float32)
R = rotation([
0, #np.random.uniform(-max_deg, max_deg),
np.random.uniform(-max_deg, max_deg),
0, #np.random.uniform(-max_deg, max_deg)
])
t = np.zeros((3, 1))
Rt = np.vstack((np.hstack((R, t)), np.array([0, 0, 0, 1]))).astype(np.float32)
icp = cv.ppf_match_3d_ICP(100)
I = np.eye(4)
print("Unaligned error:\t%.6f" % np.linalg.norm(I - Rt))
sprintfStr = "Unaligned error:\t%.6f\n" % np.linalg.norm(I - Rt)
for i in range(3):
rotated_cloud[i] = np.matmul(Rt[0:3, 0:3], cloud[i].T).T + Rt[:3, 3].T
retval[i], residual[i], pose[i] = icp.registerModelToScene(
rotated_cloud[i], cloud[i])
print("ICP error:\t\t%.6f" % np.linalg.norm(I - np.matmul(pose[0], Rt)))
sprintfStr += "ICP error:\t\t%.6f\n" % np.linalg.norm(
I - np.matmul(pose[0], Rt))
Mbox('ICP complete', sprintfStr, 1)
range(-width//2, width//2),
range(-height//2, height//2),
sparse=False, indexing='xy'
)
z = np.zeros((height, width))
cloud[0] = np.dstack((x, y, z)).reshape((-1, 3)).astype(np.float32)
cloud[1] = noise.astype(np.float32) + cloud[0]
cloud[2] = cloud[1]
cloud[2][:, 2] += noise2.astype(np.float32)
R = rotation([
0, #np.random.uniform(-max_deg, max_deg),
np.random.uniform(-max_deg, max_deg),
0, #np.random.uniform(-max_deg, max_deg)
])
t = np.zeros((3, 1))
Rt = np.vstack((
np.hstack((R, t)),
np.array([0, 0, 0, 1])
)).astype(np.float32)
icp = cv.ppf_match_3d_ICP(100)
I = np.eye(4)
print("Unaligned error:\t%.6f" % np.linalg.norm(I - Rt))
for i in range(3):
rotated_cloud[i] = np.matmul(Rt[0:3,0:3], cloud[i].T).T + Rt[:3,3].T
retval[i], residual[i], pose[i] = icp.registerModelToScene(rotated_cloud[i], cloud[i])
print("ICP error:\t\t%.6f" % np.linalg.norm(I - np.matmul(pose[0], Rt)))
points_tgt = np.zeros((depth.shape[0], depth.shape[1], 6), np.float32)
points_tgt[:, :, :3] = to.getXYZ(depth,
fx=cam_K[0, 0],
fy=cam_K[1, 1],
cx=cam_K[0, 2],
cy=cam_K[1, 2])
points_tgt[:, :, 3:], _ = to.get_normal(depth,
fx=cam_K[0, 0],
fy=cam_K[1, 1],
cx=cam_K[0, 2],
cy=cam_K[1, 2])
pts_tgt = points_tgt[mask]
icp_fnc = cv2.ppf_match_3d_ICP(100, tolerence=0.05, numLevels=4) #1cm
retval, residual, pose = icp_fnc.registerModelToScene(
points_src.reshape(-1, 6), pts_tgt.reshape(-1, 6))
tf_mat = np.matmul(pose, tf_mat)
simple_xyz = renderer.predict(
[np.array([vertices]),
np.array([faces]),
np.array([tf_mat])])[0]
depth_r = simple_xyz[:, :, 3]
mask = np.logical_and(np.abs(depth_r - depth) < depth_th, depth_r > 0)
#compute valid bbox and resize a cropped image and mask to 256,256
img_masked = np.copy(img)
img_masked = img_masked / 255
def run_estimation(image, image_dep, model, score_threshold, threeD_boxes,
model_6, model_9, model_10, model_11, model_61):
obj_names = []
obj_poses = []
obj_confs = []
image_mask = copy.deepcopy(image)
image = preprocess_image(image)
#image_mask = copy.deepcopy(image)
#cv2.imwrite('/home/sthalham/retnetpose_image.jpg', image)
if keras.backend.image_data_format() == 'channels_first':
image = image.transpose((2, 0, 1))
#with graph.as_default():
boxes3D, scores, mask = model.predict_on_batch(
np.expand_dims(image, axis=0))
for inv_cls in range(scores.shape[2]):
if inv_cls == 0:
true_cls = 5
elif inv_cls == 1:
true_cls = 8
elif inv_cls == 2:
true_cls = 9
elif inv_cls == 3:
true_cls = 10
elif inv_cls == 4:
true_cls = 21
#true_cat = inv_cls + 1
#true_cls = true_cat
cls_mask = scores[0, :, inv_cls]
cls_indices = np.where(cls_mask > score_threshold)
#cls_indices = np.argmax(cls_mask)
#print(cls_mask[cls_indices])
cls_img = image
obj_mask = mask[0, :, inv_cls]
if inv_cls == 0:
obj_col = [1, 255, 255]
elif inv_cls == 1:
obj_col = [1, 1, 128]
elif inv_cls == 2:
obj_col = [255, 255, 1]
elif inv_cls == 3:
obj_col = [220, 245, 245]
elif inv_cls == 4:
obj_col = [128, 1, 1]
cls_img = np.where(obj_mask > 0.5, 1, 0)
cls_img = cls_img.reshape((60, 80)).astype(np.uint8)
cls_img = np.asarray(
Pilimage.fromarray(cls_img).resize((640, 480), Pilimage.NEAREST))
depth_mask = copy.deepcopy(cls_img)
cls_img = np.repeat(cls_img[:, :, np.newaxis], 3, 2)
cls_img = cls_img.astype(np.uint8)
cls_img[:, :, 0] *= obj_col[0]
cls_img[:, :, 1] *= obj_col[1]
cls_img[:, :, 2] *= obj_col[2]
image_mask = np.where(cls_img > 0, cls_img,
image_mask.astype(np.uint8))
#cv2.imwrite('/stefan/mask.png', image_mask)
#if len(cls_indices[0]) < 1:
if len(cls_indices[0]) < 1:
continue
if true_cls == 5:
name = '006_mustard_bottle'
pcd_model = model_6
elif true_cls == 8:
name = '009_gelatin_box'
pcd_model = model_9
elif true_cls == 9:
name = '010_potted_meat_can'
pcd_model = model_10
elif true_cls == 10:
name = '011_banana'
pcd_model = model_11
elif true_cls == 21:
name = '061_foam_brick'
pcd_model = model_61
else:
continue
obj_names.append(name)
#obj_confs.append(np.sum(cls_mask[cls_indices[0]]))
obj_confs.append(np.sum(cls_mask[cls_indices]))
k_hyp = len(cls_indices[0])
#k_hyp = 1
ori_points = np.ascontiguousarray(
threeD_boxes[(true_cls), :, :],
dtype=np.float32) # .reshape((8, 1, 3))
K = np.float32([fxkin, 0., cxkin, 0., fykin, cykin, 0., 0.,
1.]).reshape(3, 3)
##############################
# pnp
pose_votes = boxes3D[0, cls_indices, :]
est_points = np.ascontiguousarray(pose_votes,
dtype=np.float32).reshape(
(int(k_hyp * 8), 1, 2))
obj_points = np.repeat(ori_points[np.newaxis, :, :], k_hyp, axis=0)
obj_points = obj_points.reshape((int(k_hyp * 8), 1, 3))
retval, orvec, otvec, inliers = cv2.solvePnPRansac(
objectPoints=obj_points,
imagePoints=est_points,
cameraMatrix=K,
distCoeffs=None,
rvec=None,
tvec=None,
useExtrinsicGuess=False,
iterationsCount=300,
reprojectionError=5.0,
confidence=0.99,
flags=cv2.SOLVEPNP_ITERATIVE)
R_est, _ = cv2.Rodrigues(orvec)
t_est = otvec[:, 0]
if np.sum(depth_mask) > 3000:
print('--------------------- ICP refinement -------------------')
print('cls: ', true_cls)
pcd_img = np.where(depth_mask, image_dep, np.NaN)
pcd_img = create_point_cloud(pcd_img, fxkin, fykin, cxkin, cykin,
1.0)
pcd_img = pcd_img[~np.isnan(pcd_img).any(axis=1)]
pcd_crop = open3d.PointCloud()
pcd_crop.points = open3d.Vector3dVector(pcd_img)
open3d.estimate_normals(
pcd_crop,
search_param=open3d.KDTreeSearchParamHybrid(radius=20.0,
max_nn=30))
guess = np.zeros((4, 4), dtype=np.float32)
guess[:3, :3] = R_est
guess[:3, 3] = t_est.T * 1000.0
guess[3, :] = np.array([0.0, 0.0, 0.0, 1.0], dtype=np.float32).T
pcd_model = open3d.geometry.voxel_down_sample(pcd_model,
voxel_size=5.0)
pcd_crop = open3d.geometry.voxel_down_sample(pcd_crop,
voxel_size=5.0)
open3d.estimate_normals(
pcd_crop,
search_param=open3d.KDTreeSearchParamHybrid(radius=10.0,
max_nn=10))
open3d.estimate_normals(
pcd_model,
search_param=open3d.KDTreeSearchParamHybrid(radius=10.0,
max_nn=10))
pcd_model.transform(guess)
# remove model vertices facing away from camera
points_unfiltered = np.asarray(pcd_model.points)
last_pcd_temp = []
for i, normal in enumerate(pcd_model.normals):
if normal[2] < 0:
last_pcd_temp.append(points_unfiltered[i, :])
pcd_model.points = open3d.Vector3dVector(np.asarray(last_pcd_temp))
open3d.estimate_normals(
pcd_model,
search_param=open3d.KDTreeSearchParamHybrid(radius=20.0,
max_nn=10))
# align model with median depth of scene
mean_crop = np.median(np.array(pcd_crop.points), axis=0)
mean_model = np.mean(np.array(pcd_model.points), axis=0)
pcd_crop_filt = copy.deepcopy(pcd_crop)
pcd_min = guess[2, 3] - 75
pcd_max = guess[2, 3] + 75
new_pcd_scene = []
for i, point in enumerate(pcd_crop.points):
if point[2] > pcd_min and point[2] < pcd_max:
new_pcd_scene.append(point)
#mean_crop = np.mean(np.array(pcd_crop_filt.points), axis=0)
print(mean_crop, mean_crop.shape)
#print(guess[:3, 3], guess[:3, 3].shape)
#print(mean_crop-guess[:3, 3])
print('euclid: ', np.linalg.norm((mean_crop - guess[:3, 3]),
ord=2))
print('num_points: ', len(new_pcd_scene))
if len(new_pcd_scene) < 50 or np.linalg.norm(
(mean_crop - guess[:3, 3]), ord=2) > 75:
print('use pcd mean')
if len(new_pcd_scene) > 50 and np.linalg.norm(
(mean_crop - guess[:3, 3]), ord=2) > 75:
print('recalc mean')
pcd_crop_filt.points = open3d.Vector3dVector(
np.asarray(new_pcd_scene))
mean_crop = np.mean(np.array(pcd_crop_filt.points), axis=0)
pcd_diff = mean_crop - mean_model
pcd_model.translate(pcd_diff)
open3d.estimate_normals(
pcd_model,
search_param=open3d.KDTreeSearchParamHybrid(radius=10.0,
max_nn=10))
guess[:3, 3] = mean_crop
else:
print('use pose')
pcd_crop.points = open3d.Vector3dVector(
np.asarray(new_pcd_scene))
open3d.estimate_normals(
pcd_crop,
search_param=open3d.KDTreeSearchParamHybrid(radius=20.0,
max_nn=10))
#reg_p2p = open3d.registration.registration_icp(pcd_model, pcd_crop, 5.0, np.eye(4),
# open3d.registration.TransformationEstimationPointToPlane(), open3d.registration.ICPConvergenceCriteria(max_iteration=100))
reg_icp = cv2.ppf_match_3d_ICP(100, tolerence=0.075, numLevels=4)
model_points = np.asarray(pcd_model.points, dtype=np.float32)
model_normals = np.asarray(pcd_model.normals, dtype=np.float32)
crop_points = np.asarray(pcd_crop.points, dtype=np.float32)
crop_normals = np.asarray(pcd_crop.normals, dtype=np.float32)
pcd_source = np.zeros((model_points.shape[0], 6), dtype=np.float32)
pcd_target = np.zeros((crop_points.shape[0], 6), dtype=np.float32)
pcd_source[:, :3] = model_points * 0.001
pcd_source[:, 3:] = model_normals
pcd_target[:, :3] = crop_points * 0.001
pcd_target[:, 3:] = crop_normals
retval, residual, pose = reg_icp.registerModelToScene(
pcd_source, pcd_target)
print('residual: ', residual)
#pcd_model.transform(reg_p2p.transformation)
guess[:3, 3] = guess[:3, 3] * 0.001
guess = np.matmul(pose, guess)
R_est = guess[:3, :3]
t_est = guess[:3, 3]
#print('guess: ', guess)
est_pose = np.zeros((7), dtype=np.float32)
est_pose[:3] = t_est
est_pose[3:] = tf3d.quaternions.mat2quat(R_est)
obj_poses.append(est_pose)
bridge = CvBridge()
image_mask_msg = bridge.cv2_to_imgmsg(image_mask)
mask_pub.publish(image_mask_msg)
return obj_names, obj_poses, obj_confs
def icp_refinement(depth_measured,
depth_rendered,
object_mask_measured,
cam_K,
TCO_pred,
n_min_points=1000):
# Inspired from https://github.com/kirumang/Pix2Pose/blob/843effe0097e9982f4b07dd90b04ede2b9ee9294/tools/5_evaluation_bop_icp3d.py#L57
points_tgt = np.zeros(
(depth_measured.shape[0], depth_measured.shape[1], 6), np.float32)
points_tgt[:, :, :3] = getXYZ(depth_measured,
fx=cam_K[0, 0],
fy=cam_K[1, 1],
cx=cam_K[0, 2],
cy=cam_K[1, 2])
points_tgt[:, :, 3:] = get_normal(depth_measured,
fx=cam_K[0, 0],
fy=cam_K[1, 1],
cx=cam_K[0, 2],
cy=cam_K[1, 2],
refine=True)
depth_valid = np.logical_and(depth_measured > 0.2, depth_measured < 5)
depth_valid = np.logical_and(depth_valid, object_mask_measured)
points_tgt = points_tgt[depth_valid]
points_src = np.zeros(
(depth_measured.shape[0], depth_measured.shape[1], 6), np.float32)
points_src[:, :, :3] = getXYZ(depth_rendered, cam_K[0, 0], cam_K[1, 1],
cam_K[0, 2], cam_K[1, 2])
points_src[:, :, 3:] = get_normal(depth_rendered,
fx=cam_K[0, 0],
fy=cam_K[1, 1],
cx=cam_K[0, 2],
cy=cam_K[1, 2],
refine=True)
points_src = points_src[np.logical_and(depth_valid, depth_rendered > 0)]
if len(points_tgt) < n_min_points or len(points_src) < n_min_points:
return np.eye(4) * float('nan'), -1
TCO_pred_refined = TCO_pred.copy()
# adjust the initial translation using centroids of visible points
centroid_src = np.mean(points_src[:, :3], axis=0)
centroid_tgt = np.mean(points_tgt[:, :3], axis=0)
dists_centroid = centroid_tgt - centroid_src
TCO_pred_refined[:3, -1] += dists_centroid.reshape(-1)
points_src[:, :3] += dists_centroid[None]
# import trimesh
# print(points_src.shape, points_tgt.shape)
# trimesh.Trimesh(vertices=points_src[:, :3], normals=points_src[:, 3:]).export(DEBUG_DATA_DIR / 'src.ply')
# trimesh.Trimesh(vertices=points_tgt[:, :3], normals=points_tgt[:, 3:]).export(DEBUG_DATA_DIR / 'tgt.ply')
# raise ValueError
tolerence = 0.05
icp_fnc = cv2.ppf_match_3d_ICP(100, tolerence=tolerence, numLevels=4)
retval, residual, pose = icp_fnc.registerModelToScene(
points_src.reshape(-1, 6), points_tgt.reshape(-1, 6))
TCO_pred_refined = pose @ TCO_pred_refined
TCO_pred_refined = torch.tensor(TCO_pred_refined,
dtype=torch.float32).cuda()
# print(retval, residual, pose)
# # print(retval, residual)
# # raise ValueError
if residual > tolerence or residual < 0:
retval = -1
return TCO_pred_refined, retval
#!/usr/bin/python
import cv2
import numpy as np
model = cv2.ppf_match_3d.loadPLYSimple('model-low.ply', 0) #No nominal
print model.shape
scene = cv2.ppf_match_3d.loadPLYSimple('scene-high.ply', 0) #No nominal
icp = cv2.ppf_match_3d_ICP(100, 0.2, 2.0, 8)
pose = np.array([[1, 0, 0, 0], [0, 1, 0, 14], [0, 0, 1, 0], [0, 0, 0, 1]])
modela = cv2.ppf_match_3d.transformPCPose(model, pose)
ret, residu, posea = icp.registerModelToScene(modela, scene)
print "pose", posea