diff --git a/compute_gt_poses.py b/compute_gt_poses.py index 24e2faf..305746d 100644 --- a/compute_gt_poses.py +++ b/compute_gt_poses.py @@ -149,9 +149,9 @@ def full_registration(path,max_correspondence_distance_coarse, max_correspondence_distance_fine): global N_Neighbours, LABEL_INTERVAL, n_pcds - pose_graph = registration.PoseGraph() + pose_graph = pipelines.registration.PoseGraph() odometry = np.identity(4) - pose_graph.nodes.append(registration.PoseGraphNode(odometry)) + pose_graph.nodes.append(pipelines.registration.PoseGraphNode(odometry)) pcds = [[] for i in range(n_pcds)] for source_id in trange(n_pcds): @@ -183,19 +183,19 @@ def full_registration(path,max_correspondence_distance_coarse, else: transformation_icp = res - information_icp = registration.get_information_matrix_from_point_clouds( + information_icp = pipelines.registration.get_information_matrix_from_point_clouds( pcds[source_id], pcds[target_id], max_correspondence_distance_fine, transformation_icp) if target_id == source_id + 1: # odometry odometry = np.dot(transformation_icp, odometry) - pose_graph.nodes.append(registration.PoseGraphNode(np.linalg.inv(odometry))) - pose_graph.edges.append(registration.PoseGraphEdge(source_id, target_id, + pose_graph.nodes.append(pipelines.registration.PoseGraphNode(np.linalg.inv(odometry))) + pose_graph.edges.append(pipelines.registration.PoseGraphEdge(source_id, target_id, transformation_icp, information_icp, uncertain = False)) else: # loop closure - pose_graph.edges.append(registration.PoseGraphEdge(source_id, target_id, + pose_graph.edges.append(pipelines.registration.PoseGraphEdge(source_id, target_id, transformation_icp, information_icp, uncertain = True)) return pose_graph @@ -233,7 +233,7 @@ def load_pcds(path, downsample = True, interval = 1): global voxel_size, camera_intrinsics pcds= [] - for Filename in xrange(len(glob.glob1(path+"JPEGImages","*.jpg"))/interval): + for Filename in range(len(glob.glob1(path+"JPEGImages","*.jpg"))/interval): img_file = path + 'JPEGImages/%s.jpg' % (Filename*interval) cad = cv2.imread(img_file) @@ -352,13 +352,13 @@ def print_usage(): max_correspondence_distance_fine) print("Optimizing PoseGraph ...") - option =registration.GlobalOptimizationOption( + option =pipelines.registration.GlobalOptimizationOption( max_correspondence_distance = max_correspondence_distance_fine, edge_prune_threshold = 0.25, reference_node = 0) - registration.global_optimization(pose_graph, - registration.GlobalOptimizationLevenbergMarquardt(), - registration.GlobalOptimizationConvergenceCriteria(), option) + pipelines.registration.global_optimization(pose_graph, + pipelines.registration.GlobalOptimizationLevenbergMarquardt(), + pipelines.registration.GlobalOptimizationConvergenceCriteria(), option) diff --git a/get_BBs.py b/get_BBs.py index 4bf749e..85a9c1c 100644 --- a/get_BBs.py +++ b/get_BBs.py @@ -24,7 +24,7 @@ mask = cv2.imread(mask_dir,0) thresh = cv2.threshold(mask.copy(), 30, 255, cv2.THRESH_BINARY)[1] - _, contours, _ = cv2.findContours(thresh.copy(), cv2.RETR_EXTERNAL,cv2.CHAIN_APPROX_SIMPLE) + contours, _ = cv2.findContours(thresh.copy(), cv2.RETR_EXTERNAL,cv2.CHAIN_APPROX_SIMPLE) cnt = max(contours, key=cv2.contourArea) x,y,w,h = cv2.boundingRect(cnt) cv2.rectangle(img_original,(x,y),(x+w,y+h),(0,255,0),2) diff --git a/registration.py b/registration.py index 1ce2a84..6c1d86b 100644 --- a/registration.py +++ b/registration.py @@ -46,25 +46,25 @@ def icp(source,target,voxel_size,max_correspondence_distance_coarse,max_correspo assert method in ["point-to-plane","colored-icp"],"point-to-plane or colored-icp" if method == "point-to-plane": - icp_coarse = registration.registration_icp(source, target, + icp_coarse = pipelines.registration.registration_icp(source, target, max_correspondence_distance_coarse, np.identity(4), - registration.TransformationEstimationPointToPlane()) - icp_fine = registration.registration_icp(source, target, + pipelines.registration.TransformationEstimationPointToPlane()) + icp_fine = pipelines.registration.registration_icp(source, target, max_correspondence_distance_fine, icp_coarse.transformation, - registration.TransformationEstimationPointToPlane()) + pipelines.registration.TransformationEstimationPointToPlane()) transformation_icp = icp_fine.transformation if method == "colored-icp": - result_icp = registration.registration_colored_icp(source,target,voxel_size, np.identity(4), - registration.ICPConvergenceCriteria(relative_fitness = 1e-8, + result_icp = pipelines.registration.registration_colored_icp(source,target,voxel_size, np.identity(4), + pipelines.registration.ICPConvergenceCriteria(relative_fitness = 1e-8, relative_rmse = 1e-8, max_iteration = 50)) transformation_icp = result_icp.transformation - information_icp = registration.get_information_matrix_from_point_clouds( + information_icp = pipelines.registration.get_information_matrix_from_point_clouds( source, target, max_correspondence_distance_fine, transformation_icp)