def refine_registration(source: op3.PointCloud, target: op3.PointCloud, voxel_size: float, gross_matching: np.ndarray, verbose=False): """ Refine the matching :param source, target: 2 objects of Open3D, that are point clouds of source and target :param voxel_size: a float value, that is how sparse you want the sample points is :param gross_matching: a transformation matrix, that grossly matches source to target :param verbose: a boolean value, display notification and visualization when True and no notification when False :return: a transformation object """ distance_threshold = voxel_size * 0.4 if verbose: print( ":: Point-to-plane ICP registration is applied on original point") print(" clouds to refine the alignment. This time we use a strict") print(" distance threshold %.3f." % distance_threshold) result = op3.registration_icp( source, target, distance_threshold, gross_matching.transformation, op3.TransformationEstimationPointToPlane(), # op3.ICPConvergenceCriteria(max_iteration=2000) ) return result
def solve(datArray, prm): global scnFtArray, scnPcArray, Param Param.update(prm) scnFtArray = [] scnPcArray = [] for dat in datArray: pc = fromNumpy(dat) scnPcArray.append(pc) scnFtArray.append(_get_features(pc)) t1 = time.time() resft = o3.registration_ransac_based_on_feature_matching( modPcArray[0], scnPcArray[0], modFtArray[0], scnFtArray[0], Param["distance_threshold"], o3.TransformationEstimationPointToPoint(False), 4, [ o3.CorrespondenceCheckerBasedOnEdgeLength(0.9), o3.CorrespondenceCheckerBasedOnDistance( Param["distance_threshold"]) ], o3.RANSACConvergenceCriteria(2000000, 500)) print "time for feature matching", time.time() - t1 print "feature matching", resft.transformation, resft.fitness resicp = o3.registration_icp(modPcArray[0], scnPcArray[0], Param["icp_threshold"], resft.transformation, o3.TransformationEstimationPointToPlane()) return { "transform": [resicp.transformation], "fitness": [resicp.fitness], "rmse": [resicp.inlier_rmse] }
def icp_registration(source, target, distance_threshold): pn.estimate_normals(source) pn.estimate_normals(target) icp_result = pn.registration_icp(source, target, distance_threshold, np.eye(4), pn.TransformationEstimationPointToPlane()) return icp_result
def globalRansacMatchAndICPRefine(self): source_down, source_fpfh, target_down, target_fpfh = self.prepareDataset() # showPointCloud([target_down]) # exit() # result_ransac = open3d.registration_ransac_based_on_feature_matching( # source_down, target_down, source_fpfh, target_fpfh, self._distance_threshold_ransac_m, # open3d.TransformationEstimationPointToPoint(False), 4, [ # open3d.CorrespondenceCheckerBasedOnEdgeLength(0.9), # open3d.CorrespondenceCheckerBasedOnDistance( # self._distance_threshold_ransac_m) # ], open3d.RANSACConvergenceCriteria(4000000, 500)) result_ransac = open3d.registration_fast_based_on_feature_matching( source_down, target_down, source_fpfh, target_fpfh, open3d.FastGlobalRegistrationOption( maximum_correspondence_distance=self._distance_threshold_ransac_m) ) # ], open3d.RANSACConvergenceCriteria(40000, 500)) if self._show_result: self.showMatchResult(source_down, target_down, result_ransac.transformation) # result_icp = open3d.registration_icp( # self._source, self._target, self._distance_threshold_icp_m, result_ransac.transformation, # open3d.TransformationEstimationPointToPlane()) result_icp = open3d.registration_icp( source_down, target_down, self._distance_threshold_icp_m, result_ransac.transformation, open3d.TransformationEstimationPointToPlane()) if self._show_result: self.showMatchResult(self._source, self._target, result_icp.transformation) return result_ransac, result_icp
def update_source_to_target_transformation(self): """ Function to update the source to target transformation matrix """ source = copy.deepcopy(self.source_pcl) target = pn.voxel_down_sample(self.target_pcl, target_voxel_size) if self.source_to_target_transformation.size > 0: # if the global registration have already been done source.transform(self.source_to_target_transformation) icp_result = pn.registration_icp( source, target, distance_threshold_icp, np.eye(4), pn.TransformationEstimationPointToPlane()) if icp_result.fitness > 0.8: self.source_to_target_transformation =\ icp_result.transformation.dot(self.source_to_target_transformation) return # we fail to align source & target with ICP # GLOBAL REGISTRATION # PREPROCESSING POINT CLOUD source = copy.deepcopy(self.source_pcl) source_down, source_fpfh = preprocess_point_cloud(source, voxel_size) target_down, target_fpfh = preprocess_point_cloud(target, voxel_size) # GLOBAL REGISTRATION ransac_result = pn.registration_ransac_based_on_feature_matching( source_down, target_down, source_fpfh, target_fpfh, distance_threshold_ransac, pn.TransformationEstimationPointToPoint(False), 4, [ pn.CorrespondenceCheckerBasedOnDistance( distance_threshold_ransac) ], pn.RANSACConvergenceCriteria(10000000, 10000)) source.transform(ransac_result.transformation) icp_result = pn.registration_icp( source, target, distance_threshold_icp, np.eye(4), pn.TransformationEstimationPointToPlane()) self.source_to_target_transformation = \ icp_result.transformation.dot(ransac_result.transformation) return
def refine_registration(source, target, source_fpfh, target_fpfh, voxel_size): distance_threshold = voxel_size * 1.5 print(":: Point-to-plane ICP registration is applied on original point") print(" clouds to refine the alignment. This time we use a strict") print(" distance threshold %.3f." % distance_threshold) result = open3d.registration_icp( source, target, distance_threshold, result_fast.transformation, open3d.TransformationEstimationPointToPlane()) return result
def registration_point_to_plane(scene1, scene2, voxel_size): # scene1 and 2 are point cloud data # voxel_size is grid size #draw_registration_result(scene1,scene2,np.identity(4)) # voxel down sampling scene1_down = open3d.voxel_down_sample(scene1, voxel_size) scene2_down = open3d.voxel_down_sample(scene2, voxel_size) #draw_registration_result(scene1_down,scene2_down,np.identity(4)) # estimate normal with search radius voxel_size*2.0 radius_normal = voxel_size * 2.0 open3d.estimate_normals( scene1, open3d.KDTreeSearchParamHybrid(radius=radius_normal, max_nn=30)) open3d.estimate_normals( scene2, open3d.KDTreeSearchParamHybrid(radius=radius_normal, max_nn=30)) open3d.estimate_normals( scene1_down, open3d.KDTreeSearchParamHybrid(radius=radius_normal, max_nn=30)) open3d.estimate_normals( scene2_down, open3d.KDTreeSearchParamHybrid(radius=radius_normal, max_nn=30)) # compute fpfh feature with search radius voxel_size/2.0 radius_feature = voxel_size * 2.0 scene1_fpfh = open3d.compute_fpfh_feature( scene1_down, search_param=open3d.KDTreeSearchParamHybrid(radius=radius_feature, max_nn=100)) scene2_fpfh = open3d.compute_fpfh_feature( scene2_down, search_param=open3d.KDTreeSearchParamHybrid(radius=radius_feature, max_nn=100)) # compute ransac registration ransac_result = execute_global_registration(scene1_down, scene2_down, scene1_fpfh, scene2_fpfh, voxel_size) #draw_registration_result(scene1,scene2,ransac_result.transformation) # point to point ICP resigtration distance_threshold = voxel_size * 10.0 result = open3d.registration_icp( scene1, scene2, distance_threshold, ransac_result.transformation, open3d.TransformationEstimationPointToPlane(), open3d.ICPConvergenceCriteria(max_iteration=1000)) #draw_registration_result(scene1,scene2,result.transformation) print(result) return result
def refine_registration(source, target, voxel_size, gross_matching): distance_threshold = voxel_size * 0.4 print(":: Point-to-plane ICP registration is applied on original point") print(" clouds to refine the alignment. This time we use a strict") print(" distance threshold %.3f." % distance_threshold) result = op3.registration_icp( source, target, distance_threshold, gross_matching.transformation, op3.TransformationEstimationPointToPlane(), op3.ICPConvergenceCriteria(max_iteration=2000)) return result
def icp_two_pc(source, target, draw_result=False, point2plane=False): """ return target * result = source :param source: :param target: :param draw_result: :param point2plane: :return: """ if isinstance(source, PointCloud): tmp = source source = o3d.PointCloud() source.points = o3d.Vector3dVector(tmp.position) elif isinstance(source, np.ndarray): tmp = source source = o3d.PointCloud() source.points = o3d.Vector3dVector(tmp) if isinstance(target, PointCloud): tmp = target target = o3d.PointCloud() target.points = o3d.Vector3dVector(tmp.position) elif isinstance(target, np.ndarray): tmp = target target = o3d.PointCloud() target.points = o3d.Vector3dVector(tmp) if point2plane: result = o3d.registration_icp( source, target, 0.002, estimation_method=o3d.TransformationEstimationPointToPlane()) else: print('using point to point icp method') result = o3d.registration_icp( source, target, 0.002, estimation_method=o3d.TransformationEstimationPointToPoint()) transformation = result.transformation print('result transformation:', transformation) if draw_result: source.transform( transformation ) # source point cloud apply the transformation to get target point cloud draw_registration_result(source, target) return transformation
def get_evaluation(pcd_temp_, pcd_scene_, inlier_thres, tf, final_th=0, n_iter=5): #queue tf_pcd = np.eye(4) reg_p2p = open3d.registration_icp( pcd_temp_, pcd_scene_, inlier_thres, np.eye(4), open3d.TransformationEstimationPointToPoint(), open3d.ICPConvergenceCriteria(max_iteration=1)) #5? tf = np.matmul(reg_p2p.transformation, tf) tf_pcd = np.matmul(reg_p2p.transformation, tf_pcd) pcd_temp_.transform(reg_p2p.transformation) for i in range(4): inlier_thres = reg_p2p.inlier_rmse * 3 if inlier_thres == 0: continue reg_p2p = open3d.registration_icp( pcd_temp_, pcd_scene_, inlier_thres, np.eye(4), open3d.TransformationEstimationPointToPlane(), open3d.ICPConvergenceCriteria(max_iteration=1)) #5? tf = np.matmul(reg_p2p.transformation, tf) tf_pcd = np.matmul(reg_p2p.transformation, tf_pcd) pcd_temp_.transform(reg_p2p.transformation) inlier_rmse = reg_p2p.inlier_rmse ##Calculate fitness with depth_inlier_th if (final_th > 0): inlier_thres = final_th #depth_inlier_th*2 #reg_p2p.inlier_rmse*3 reg_p2p = registration_icp( pcd_temp_, pcd_scene_, inlier_thres, np.eye(4), TransformationEstimationPointToPlane(), ICPConvergenceCriteria(max_iteration=1)) #5? if (np.abs(np.linalg.det(tf[:3, :3]) - 1) > 0.001): tf[:3, 0] = tf[:3, 0] / np.linalg.norm(tf[:3, 0]) tf[:3, 1] = tf[:3, 1] / np.linalg.norm(tf[:3, 1]) tf[:3, 2] = tf[:3, 2] / np.linalg.norm(tf[:3, 2]) if (np.linalg.det(tf) < 0): tf[:3, 2] = -tf[:3, 2] return tf, inlier_rmse, tf_pcd, reg_p2p.fitness
def evaluate_calibration(s, t, H): """ Evaluate by ICP :param s: source point cloud with reference to camera :param t: target point cloud with reference to robot base :param H: transformation matrix to be evaluated, camera pose with reference to robot base :return: (cam',cam)H * cam_P = Base_P * (cam', Base)H (cam',cam)H * s = tTs """ tTs = copy.deepcopy(t) tTs = tTs.transform(np.matrix(H).getI()) HI = np.matrix([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]]) reg_p2l = o3d.registration_icp(s, tTs, 1, HI, o3d.TransformationEstimationPointToPlane()) R = reg_p2l.transformation[:3, :3] T = reg_p2l.transformation[:3, 3] al, be, ga = tf.transformations.euler_from_matrix(R, 'sxyz') return T, np.array([al, be, ga]) * 180 / 3.1415
def _perform_registration(self): if self.registration == 0: self.registration_name = 'None' return fixed = open3dmeshopen3dpcd(self.fixed) moving = open3dmeshopen3dpcd(self.moving) if self.registration == 1: estimator = open3d.TransformationEstimationPointToPoint() else: estimator = open3d.TransformationEstimationPointToPlane() init = np.asarray([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]]) registration = open3d.registration_icp(moving, fixed, np.Inf, init, estimator) self.registration_name = type(estimator) self.transform = registration.transformation self.moving.transform(self.transform)
def solve(datArray,prm): global scnFtArray,scnPcArray,Param Param.update(prm) scnFtArray=[] scnPcArray=[] t1=time.time() for dat in datArray: pc=fromNumpy(dat) scnPcArray.append(pc) scnFtArray.append(_get_features(pc)) print "time for extracting feature",time.time()-t1 t1=time.time() resft=o3.registration.registration_fast_based_on_feature_matching( modPcArray[0],scnPcArray[0],modFtArray[0],scnFtArray[0], o3.registration.FastGlobalRegistrationOption( maximum_correspondence_distance=Param["distance_threshold"])) print "time for feature matching",time.time()-t1 print "feature matching",resft.transformation # return {"transform":[resft.transformation],"fitness":[0]} resicp=o3.registration_icp( modPcArray[0],scnPcArray[0], Param["icp_threshold"], resft.transformation,o3.TransformationEstimationPointToPlane()) return {"transform":[resicp.transformation],"fitness":[resicp.fitness],"rmse":[resicp.inlier_rmse]}
mesh = open3d.read_triangle_mesh(sfile) source = mesh2pcd(mesh) open3d.draw_geometries([target]) open3d.draw_geometries([source]) trans_init = np.asarray([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]]) draw_registration_result(source, target, trans_init) print("Initial alignment") evaluation = open3d.evaluate_registration(source, target, np.Inf, trans_init) print(evaluation) print("Apply point-to-point ICP") reg_p2p = open3d.registration_icp(source, target, np.Inf, trans_init, open3d.TransformationEstimationPointToPoint()) print(reg_p2p) print("Transformation is:") print(reg_p2p.transformation) print("") draw_registration_result(source, target, reg_p2p.transformation) print("Apply point-to-plane ICP") reg_p2l = open3d.registration_icp(source, target, np.Inf, trans_init, open3d.TransformationEstimationPointToPlane()) print(reg_p2l) print("Transformation is:") print(reg_p2l.transformation) print("") draw_registration_result(source, target, reg_p2l.transformation)
def open3d_icp_normal(pa, pb, trans_init=np.eye(4), max_distance=0.02, b_graph=False): ''' (Transformation A->B) pa : numpy array pb : numpy array return: transformation - 4x4 numpy array result - transformation - fitness - inlier_rmse - correspondence_set ''' import open3d as op3 import copy def draw_registration_result(source, target, transformation): source_temp = copy.deepcopy(source) target_temp = copy.deepcopy(target) source_temp.paint_uniform_color([1, 0.706, 0]) target_temp.paint_uniform_color([0, 0.651, 0.929]) source_temp.transform(transformation) op3.draw_geometries([source_temp, target_temp]) pc1 = op3.PointCloud() pc1.points = op3.Vector3dVector(pa) pc2 = op3.PointCloud() pc2.points = op3.Vector3dVector(pb) op3.estimate_normals(pc1, search_param=op3.KDTreeSearchParamHybrid(radius=10.0, max_nn=30)) op3.estimate_normals(pc2, search_param=op3.KDTreeSearchParamHybrid(radius=10.0, max_nn=30)) # op3.draw_geometries([pc1, pc2]) # max_distance = 0.2 # trans_init = np.asarray( # [[0.862, 0.011, -0.507, 0.5], # [-0.139, 0.967, -0.215, 0.7], # [0.487, 0.255, 0.835, -1.4], # [0.0, 0.0, 0.0, 1.0]]) # trans_init = np.eye(4) # tsl = pb.mean(axis=0) - pa.mean(axis=0) # trans_init = TransformationMatrixFromEulerAngleTranslation(euler, tsl) # trans_init = transform #### # print("Apply point-to-point ICP") #### reg_p2p = op3.registration_icp(pc1, pc2, max_distance, trans_init, #### # op3.TransformationEstimationPointToPoint()) #### op3.TransformationEstimationPointToPoint(), #### op3.ICPConvergenceCriteria(max_iteration = 2000)) #### # print(reg_p2p) #### # print("Transformation is:") #### # print(reg_p2p.transformation) #### # print(EulerAngleTranslationFromTransformationMatrix(reg_p2p.transformation)) #### # print("") # print("Apply point-to-plane ICP") reg_p2l = op3.registration_icp( pc1, pc2, max_distance, trans_init, op3.TransformationEstimationPointToPlane(), op3.ICPConvergenceCriteria(max_iteration=2000)) # print(reg_p2l) # print("Transformation is:") # print(reg_p2l.transformation) # print("") if b_graph: draw_registration_result(pc1, pc2, reg_p2l.transformation) # print(reg_p2p.transformation) # print(reg_p2p.fitness) # print(reg_p2p.inlier_rmse) # print(reg_p2p.correspondence_set) return reg_p2l.transformation, reg_p2l
print("Fast global registration took %.3f sec.\n" % (time.time() - start)) fgr.draw_registration_result(source_down, target_down, result_fast.transformation) # start accurate registration threshold = 0.5 trans_init = result_fast.transformation print("Initial alignment") evaluation = open3d.evaluate_registration(source, target, threshold, trans_init) print(evaluation) print("Apply point-to-point ICP") reg_p2p = open3d.registration_icp( source, target, threshold, trans_init, open3d.TransformationEstimationPointToPoint()) print(reg_p2p) print("Transformation is:") print(reg_p2p.transformation) print("") fgr.draw_registration_result(source, target, reg_p2p.transformation) print("Apply point-to-plane ICP") reg_p2l = open3d.registration_icp( source, target, threshold, trans_init, open3d.TransformationEstimationPointToPlane()) print(reg_p2l) print("Transformation is:") print(reg_p2l.transformation) print("") fgr.draw_registration_result(source, target, reg_p2l.transformation)
def registerLocalCloud(self, target, source): """ get local transformation matrix :param target: Target Point Cloud :param source: Source Point Cloud :return: transformation from target cloud to source cloud """ source_temp = copy.deepcopy(source) target_temp = copy.deepcopy(target) source_temp = o3d.voxel_down_sample(source_temp, 0.004) target_temp = o3d.voxel_down_sample(target_temp, 0.004) o3d.estimate_normals(source_temp, search_param=o3d.KDTreeSearchParamHybrid( radius=0.1, max_nn=30)) o3d.estimate_normals(target_temp, search_param=o3d.KDTreeSearchParamHybrid( radius=0.1, max_nn=30)) current_transformation = np.identity(4) # use Point-to-plane ICP registeration to obtain initial pose guess result_icp_p2l = o3d.registration_icp(source_temp, target_temp, 0.02, current_transformation, o3d.TransformationEstimationPointToPlane()) # 0.1 is searching distance print("----------------") print("initial guess from Point-to-plane ICP registeration") print(result_icp_p2l) print(result_icp_p2l.transformation) p2l_init_trans_guess = result_icp_p2l.transformation print("----------------") # print("Colored point cloud registration") # Double Normal ICP result_icp = o3d.registration_icp( source_temp, target_temp, 0.01, p2l_init_trans_guess, o3d.TransformationEstimationPointToPlane()) # Color ICP # result_icp = o3d.registration_colored_icp( # source_temp, target_temp, 0.01, p2l_init_trans_guess) print(result_icp) print(result_icp.transformation) # print(result_icp.fitness) # draw_registration_result_original_color(source, target, result_icp.transformation) # write intermediate result for test, # but found it will make registration result worse... # ---->>>> cause source.transform will change the source!!!!!! # source.transform(result_icp.transformation) # temp_cloud = target + source # write_point_cloud("/home/dylan2/catkin_ws/src/temp/pointCloudInRviz/data/result/{}-{}.pcd".format( # self.cloud_index,self.cloud_index-1), temp_cloud , write_ascii = False) return result_icp.transformation ###################################### # kick-out rule ###################################### # New rule: # 1/ rotation is out of plane (5 degree, or 0.087266 in radians); # 2/ too big rotation; # 3/ too big translation; # first calculate what is the rotation direction and rotation angle """
def stitch_pc(files_directory, output_dir, r, with_images=False, downsample=2.5, cam_settings=(cam_angle_of_view, cam_rot, cam_pos, image_flip_h, image_flip_v), omsws_settings=(scan_id, inverse_x, rotation)): (cam_angle_of_view, cam_rot, cam_pos, image_flip_h, image_flip_v) = cam_settings # icp setting - how close should the point be to be a correspondence threshold = 2.5 file_paths = [join(files_directory, path) for path in os.listdir(files_directory) if path.endswith('.omsws')] file_paths.sort() target = su.StitchPart(file_paths[0], omsws_settings, cam_settings, r) crawler_position = target.distance_driven * 1000 # create transformation array which identity matrix for now transformation = np.identity(4) print('base ', split(file_paths[0])[1]) # initialize straightening rotation, which will centrecorrect and rotate # the scans, so they are palallel to z axis rot_trans_straight = None # initialize the output point cloud that will save all the measured points stitched = su.StitchResult() if downsample > 0: target.full_copy = o3d.voxel_down_sample(target.full_copy, voxel_size=downsample) stitched.Pc = copy.deepcopy(target.full_copy) # save no of points and path of current file to the class stitched.points_no.append(len(target.full_copy.points)) stitched.omsws_paths.append(target.omsws.path) if with_images: colors = target.load_images()/255 stitched.Pc.colors = o3d.Vector3dVector(colors) # break for loop if we have only one file path dont_stitch = len(file_paths) == 1 for i, path in enumerate(file_paths[1:]): if dont_stitch: break print('stitching {}'.format(split(path)[1])) # initialize source point cloud source = su.StitchPart(path, omsws_settings, cam_settings, r) # o3d.draw_geometries([source.full_copy, target.lower]) if downsample > 0: source.full_copy = o3d.voxel_down_sample(source.full_copy, voxel_size = downsample) if with_images: colors = source.load_images()/255 stitched.Pc.colors.extend(o3d.Vector3dVector(colors)) # calculate the centroids of point clouds and find the differrence transition = su.estimate_xy_translation(source.lower, target.lower, rot_trans_straight) # update x and y of translation so the centroids are in the same place trans_init = np.identity(4) trans_init[:2, 3] = transition[:2] # update z from the cralwer position distance saved in omsws file trans_init[2, 3] = source.omsws.distance_driven * 1000 - crawler_position print('crawler position: %.2f' % crawler_position) crawler_position = source.omsws.distance_driven * 1000 # stitch the point clouds of the lower part using icp reg_p2p = o3d.registration_icp(source.lower, target.lower, threshold, trans_init, o3d.TransformationEstimationPointToPlane()) # stitch the full point clouds using the registration matrix from # previous stitch reg_p2p = o3d.registration_icp(source.full, target.full, threshold, reg_p2p.transformation, o3d.TransformationEstimationPointToPlane()) print('fitness: {}'.format(reg_p2p.fitness)) trans_init = reg_p2p.transformation # multiply transformation matrix of current scan that best fits the # target scan that is placed in (0,0) point with the global # transformation of the target scan (where is it placed in real live) transformation = np.dot(transformation, reg_p2p.transformation) # transform our copy with this transformation and that will place # the source in correct position source.full_copy.transform(transformation) # add all the points of the scan to the stitched scan stitched.Pc.points.extend(source.full_copy.points) stitched.points_no.append(len(source.full_copy.points)) # add omsws obj to the list in the stitched stitched.omsws_paths.append(source.omsws.path) stitched.transformations.append(transformation) # update target and target full target = source # create output folder if not os.path.exists(output_dir): os.makedirs(output_dir) output_path = join(output_dir, "stitched.pcd") # save point cloud, downsample and display o3d.write_point_cloud(output_path, stitched.Pc) stitched.Pc = su.o3d_estimate_normals(stitched.Pc) o3d.write_point_cloud(join(files_directory, "stitched_downsampled_colored.pcd"), stitched.Pc) ## stitched.Pc.paint_uniform_color([1, 0.706, 0]) # mesh_frame = o3d.create_mesh_coordinate_frame(size = 200, origin = [0, 0, 0]) # o3d.draw_geometries([stitched.Pc, mesh_frame]) # save transformation of the stitched point clouds to json file transformations = [list([list(el) for el in t]) for t in stitched.transformations] with open(join(output_dir, 'stitched_data.json'), 'w') as outfile: data = {'paths': stitched.omsws_paths, 'points_no': stitched.points_no, 'transformations': transformations} json.dump([data], outfile) return (output_path, join(output_dir, 'stitched_data.json'))
def update_keyframe(self, cloud): criteria = open3d.ICPConvergenceCriteria(max_iteration=100) reg = open3d.registration_icp(cloud, self.keyframes[-1].cloud, 1.0, self.last_frame_transformation, open3d.TransformationEstimationPointToPlane(), criteria=criteria) angle = pyquaternion.Quaternion(matrix=reg.transformation[:3, :3]).degrees trans = numpy.linalg.norm(reg.transformation[:3, 3]) if abs(angle) < self.keyframe_angle_thresh_deg and abs(trans) < self.keyframe_trans_thresh_m: self.last_frame_transformation = reg.transformation return False odom = reg.transformation.dot(self.keyframes[-1].odom) self.keyframes.append(Keyframe(len(self.keyframes), cloud, odom)) self.graph.nodes.append(self.keyframes[-1].node) self.vis.add_geometry(self.keyframes[-1].transformed) self.last_frame_transformation = numpy.identity(4) information = open3d.get_information_matrix_from_point_clouds(self.keyframes[-1].cloud, self.keyframes[-2].cloud, 1.0, reg.transformation) edge = open3d.PoseGraphEdge(self.keyframes[-1].id, self.keyframes[-2].id, reg.transformation, information, uncertain=False) self.graph.edges.append(edge) return True
print("err_R: ", err_R) print() draw_registration_result_original_color(source, target, result_icp.transformation) # point to plane ICP current_transformation = np.identity(4) print("Point-to-plane ICP") o3.estimate_normals(source, o3.KDTreeSearchParamHybrid(radius=0.4, max_nn=30)) o3.estimate_normals(target, o3.KDTreeSearchParamHybrid(radius=0.4, max_nn=30)) t3 = time.time() result_icp = o3.registration_icp(source, target, 0.25, current_transformation, o3.TransformationEstimationPointToPlane(), myCriteria) t4 = time.time() print("Time Consumed: ", t4 - t3) print(result_icp) print(result_icp.transformation) err_t, err_R = eval_err(result_icp.transformation, Tg) print("err_t: ", err_t) print("err_R: ", err_R) print() draw_registration_result_original_color(source, target, result_icp.transformation) # colored pointcloud registration # coarse to fine voxel_radius = [0.4, 0.2, 0.1]