def clip_pcd_by_distance_plane(pcd, vec1, vec2, pt1, threshold): plane = Plane3D.create_plane_from_vectors_and_point(vec1, vec2, pt1) distance = plane.distance_to_plane(pcd.data.T) data_close = pcd.data[:,distance<threshold] data_far = pcd.data[:,distance>=threshold] pcd_close = PointCloud(data_close) pcd_far = PointCloud(data_far) return pcd_close, pcd_far
def filter(self, point_cloud): result = PointCloud(point_cloud.lidar_origin, point_cloud.lidar_orientation) for point in point_cloud.map_frame_list: element = self.index_to_key_(self.get_cell_index_(point)) if not(element in self.voxel_set_): self.voxel_set_.add(element) result.add_point([point]) return result
def apply(self, point_cloud): result = PointCloud(point_cloud.lidar_origin, point_cloud.lidar_orientation) copy_list = copy.deepcopy(point_cloud.map_frame_list) shuffle(copy_list) list_of_points = copy_list[0:self.wished_size] for point in list_of_points: result.add_point([point]) return result
def apply(self, point_cloud): bin_size = 2 * np.pi / self.__number_of_bins histogram = [list() for _ in range(self.__number_of_bins)] result = PointCloud(point_cloud.lidar_origin, point_cloud.lidar_orientation) list_of_points = point_cloud.map_frame_list # generate histogram for index in range(len(list_of_points)): angle = point_cloud.get_descriptor("normals_direction", index) bin_index = int(np.floor(angle / bin_size)) histogram[bin_index].append(index) # remove points of the largest remaining bin to maximize entropy of resulting normal angle distribution for _ in range(point_cloud.count - self.wished_size): lengths = list( map(lambda histogram_bin: len(histogram_bin), histogram)) index = lengths.index(max(lengths)) # remove random point from bin point_index = random.randint(0, len(histogram[index]) - 1) histogram[index].pop(point_index) # create result point cloud for histogram_bin in histogram: for index in histogram_bin: result.add_point([list_of_points[index]]) result.add_descriptor( "normals", point_cloud.get_descriptor("normals", index)) result.add_descriptor( "normals_direction", point_cloud.get_descriptor("normals_direction", index)) return result
def get_point_cloud(self, pair): """Get 3D point cloud from image pair.""" disparity = self.block_matcher.compute_disparity(pair) points = self.block_matcher.compute_3d( disparity, self.calibration.disp_to_depth_mat) colors = cv2.cvtColor(pair[0], cv2.COLOR_BGR2RGB) return PointCloud(points, colors)
def images_from_cloud(self, src_path, dst_path, testing=False): src_suffix = ".h5" dst_suffix = ".jpg" files = [ f for f in glob.glob(src_path + "*" + src_suffix, recursive=True) ] if not os.path.exists(dst_path): os.mkdir(dst_path) for f in tqdm(files): sample_name = self.extract_name(f, src_path, src_suffix) sub_directory = dst_path + "/" + sample_name if not os.path.exists( sub_directory): # Create a directory for each video file os.mkdir(sub_directory) hf = h5py.File(f, 'r') cloud = hf['cloud2'][:] hf.close() frames = PointCloud().pc2voxel_reshaped_depth(cloud, depth=20) for i in range(frames.shape[0]): frame = frames[i, :, :] output_filename = sub_directory + "/" + str(i) + dst_suffix cv2.imwrite(output_filename, frame) if testing == True: break
def read_points_data(filename): planes = [] point_clouds = [] with open(filename, "r") as fp: lines = fp.readlines() if lines[0][0] == 'T': for line in lines[1:4]: plane_param = list(map(float, line.split(","))) planes.append(Plane3D.create_plane_from_list(plane_param)) last_num = 0 start_line = 6 end_line = start_line for line in lines[6::]: if line[0] != '\n' and end_line != len(lines) - 1 and int( line[0]) == last_num: end_line += 1 else: end_line += 1 data = np.zeros((3, end_line - start_line)) if line[0] == '\n': break for idx, line in enumerate(lines[start_line:end_line]): points = list(map(float, line.split(','))) data[:, idx] = points[1::] point_clouds.append(PointCloud(data)) start_line = end_line last_num = int(line[0]) return planes, point_clouds
def main(): parser = OptionParser() parser.add_option("-t", "--track", dest="track_file", help="GPS track file name", metavar="TRACK_FILE", type="string") parser.add_option("-o", "--output", dest="output_filename", help="Output point cloud file name, e.g., output_point_cloud.dat", type="string") parser.add_option("--test_case", dest="test_case", type="int", help="Test cases: 0: region-0; 1: region-1; 2: SF-region.", default=0) (options, args) = parser.parse_args() if not options.track_file: parser.error("Track file not given.") if not options.output_filename: parser.error("Output pointcloud filename not given.") R = const.R if options.test_case == 0: LOC = const.Region_0_LOC elif options.test_case == 1: LOC = const.Region_1_LOC elif options.test_case == 2: LOC = const.SF_LOC else: parser.error("Test case indexed %d not supported!"%options.test_case) tracks = gps_track.load_tracks(options.track_file) point_cloud = PointCloud(np.array([]), np.array([])) point_cloud.extract_point_cloud(tracks, LOC, R) point_cloud.visualize_point_cloud(LOC, R) point_cloud.save(options.output_filename)
def populate(self): """ Function that creates the dataset samples. :return: """ s = time() renderer = Renderer(mode=0) lightmanager = LightManager() cameramanager = CameraManager() for i in range(self.size): lightmanager.make() building = self.factory.produce() building.make() if use_materials: _monomaterial = np.random.random() < MATERIAL_PROB mat = self.material_factory.produce() for v in building.volumes: if not _monomaterial: mat = self.material_factory.produce() v.apply(mat) v.add_modules() self.json.add(building, '{}.png'.format(i), '{}.obj'.format(i)) cameramanager.make_main() renderer.render(filename='building_{}'.format(i)) if RENDER_VIEWS > 1: for view in range(1, RENDER_VIEWS): cameramanager.make() lightmanager.make() if RANDOMIZE_TEXTURES: if use_materials: _monomaterial = np.random.random() < MATERIAL_PROB mat = self.material_factory.produce() for v in building.volumes: if not _monomaterial: mat = self.material_factory.produce() v.apply(mat) renderer.render(filename='building_{}_{}'.format(i, view)) building.save(i) building.save(i, ext='ply') building.demolish() cloud = PointCloud() cloud.make(i) print('Whole process took: {}'.format(time() - s))
def plot_image(pc: point_cloud.PointCloud, img: np.ndarray, depth_map: np.ndarray, scale=0.2, step=1): """ Renders pixels in 3D with depth as Z :param pc: PointCloud object :param img: colorized image to plot (shape (w, h, 3)) :param depth_map: depth map (shape (w, h)) :param scale: distance between points :param step: step size of loop through pixels :return: void """ for y in range(0, img.shape[1], step): for x in range(0, img.shape[0], step): if x >= img.shape[1] or y >= img.shape[0]: break pc.add_point((x * scale, y * scale, depth_map[x, y]), tuple(img[x, y]))
def test2(self): filename = "../../dataset/micro/100109.mp4" extractor = VideoProcessor(filename) image_collection = extractor.process_video(roi_extraction=True, filter_enabled=True, average_frames=True) thresh = int(np.percentile(image_collection.ravel(), 95)) cloud, labels =ImageProcessor3D().point_cloud_from_collecton(image_collection, threshold=thresh, filter_outliers=True) for i in range(9): if i == 0: projection = PointCloud().cloud_projection(cloud=cloud) else: cloud_rotated = PointCloud().rotate(cloud=cloud, pos=i) projection = PointCloud().cloud_projection(cloud=cloud_rotated) plt.subplot(3, 3, i+1) plt.imshow(projection, cmap='gray') plt.show()
def collectFeats(PC_set, n_3dpoints, ind_frame, fea_pos, fea_frame, Hw2c,cprams): '''collect features in one frame''' for label in range(n_3dpoints): if ind_frame in fea_frame[label]: ind = fea_frame[label].index(ind_frame) if label in PC_set: PC_cur = PC_set[label] PC_cur.newCorres(fea_pos[label][ind], ind_frame, Hw2c,cprams) else: PC_set[label] = PointCloud(label, np.zeros(3), fea_pos[label][ind], ind_frame, Hw2c,cprams) return PC_set
def projections_from_cloud(self, src_path, dst_path, testing=False): src_suffix = ".h5" dst_suffix = ".jpg" files = [ f for f in glob.glob(src_path + "*" + src_suffix, recursive=True) ] if not os.path.exists(dst_path): os.mkdir(dst_path) for f in tqdm(files): sample_name = self.extract_name(f, src_path, src_suffix) sub_directory = dst_path + "/" + sample_name if not os.path.exists( sub_directory): # Create a directory for each video file os.mkdir(sub_directory) hf = h5py.File(f, 'r') cloud = hf['cloud2'][:] hf.close() for i in range(9): if i == 0: projection = PointCloud().cloud_projection(cloud=cloud) else: cloud_rotated = PointCloud().rotate(cloud=cloud, pos=i) projection = PointCloud().cloud_projection( cloud=cloud_rotated) # projection = cv2.resize(projection, (224, 224), interpolation=cv2.INTER_AREA) output_filename = sub_directory + "/" + str(i) + dst_suffix cv2.imwrite(output_filename, projection) if testing == True: break
def populate(self): for i in range(self.size): building = self.factory.produce() building.make() if use_materials: _monomaterial = np.random.random() < MATERIAL_PROB mat = self.material_factory.produce() print(mat.name) for v in building.volumes: if not _monomaterial: mat = self.material_factory.produce() v.apply(mat) for module_name in MODULES: for side in range(2): mod = GridApplier( ModuleFactory().mapping[module_name]) module = ModuleFactory().produce(module_name) module.connect(v, side) step = (np.random.randint(ceil(module.scale[0]), 6), np.random.randint(ceil(module.scale[0]), 6)) mod.apply(module, step=step, offset=(2.0, 2.0, 2.0, 1.0)) self.json.add(building, '{}.png'.format(i), '{}.obj'.format(i)) # building.save(filename=str(i)) renderer = Renderer(mode=0) renderer.render(filename='building_{}'.format(i)) building.save(i) building.save(i, ext='ply') building.demolish() cloud = PointCloud() cloud.make(i)
def main(): parser = OptionParser() parser.add_option("-t", "--track", dest="track_file", help="GPS track file name", metavar="TRACK_FILE", type="string") parser.add_option( "-o", "--output", dest="output_filename", help="Output point cloud file name, e.g., output_point_cloud.dat", type="string") parser.add_option( "--test_case", dest="test_case", type="int", help="Test cases: 0: region-0; 1: region-1; 2: SF-region.", default=0) (options, args) = parser.parse_args() if not options.track_file: parser.error("Track file not given.") if not options.output_filename: parser.error("Output pointcloud filename not given.") R = const.R if options.test_case == 0: LOC = const.Region_0_LOC elif options.test_case == 1: LOC = const.Region_1_LOC elif options.test_case == 2: LOC = const.SF_LOC else: parser.error("Test case indexed %d not supported!" % options.test_case) tracks = gps_track.load_tracks(options.track_file) point_cloud = PointCloud(np.array([]), np.array([])) point_cloud.extract_point_cloud(tracks, LOC, R) point_cloud.visualize_point_cloud(LOC, R) point_cloud.save(options.output_filename)
def scan(self, environment, origin): point_cloud = PointCloud(origin, 0) for ray_angle in np.arange(self.min_angle, self.max_angle, self.angular_resultion): ray_angle_noise = np.random.normal(0, self.angular_variance, 1) ray_angle += ray_angle_noise dx = np.sin(ray_angle) dy = np.cos(ray_angle) ray = (origin[0] + self.max_range * dx, origin[1] + self.max_range * dy) ray_line = LineString([origin, ray]) # x, y = ray_line.xy # ax.plot(x, y, color='black', linewidth=1) intersect = ray_line.intersection(environment) if not intersect.is_empty: if (isinstance(intersect, shapely.geometry.point.Point)): np_ray = np.array([ intersect.coords[0][0] - origin[0], intersect.coords[0][1] - origin[1] ]) ray_length = np.linalg.norm(np_ray) range_noise = np.random.normal(0, self.range_variance, 1) point_cloud.add_point([ origin + np_ray * ((ray_length + range_noise) / ray_length) ]) elif (isinstance(intersect, shapely.geometry.multipoint.MultiPoint)): # get observation closest to the sensor # print(intersect) nearest_observation_distance = np.inf nearest_observation = None for p in intersect: candidate_distance = np.linalg.norm( np.array(origin) - np.array(p)) if candidate_distance < nearest_observation_distance: nearest_observation = p nearest_observation_distance = candidate_distance np_ray = np.array([ nearest_observation.x - origin[0], nearest_observation.y - origin[1] ]) ray_length = np.linalg.norm(np_ray) range_noise = np.random.normal(0, self.range_variance, 1) point_cloud.add_point([ origin + np_ray * ((ray_length + range_noise) / ray_length) ]) else: print('Unhandled intersection type', type(intersect)) return point_cloud
def main(): """ Convert TXT to PLY. """ parser = argparse.ArgumentParser(description='Convert TXT to PLY.') parser.add_argument('input', type=str, help='TXT file.') parser.add_argument('output', type=str, help='PLY file.') args = parser.parse_args() if not os.path.exists(args.input): print('Input file does not exist.') exit(1) point_cloud = PointCloud.from_txt(args.input) print('Read %s.' % args.input) point_cloud.to_ply(args.output) print('Wrote %s.' % args.output)
def read_points_raw(filename): data = np.loadtxt(filename, delimiter=',') pcd = PointCloud(data[:, 1:4].T) return pcd
import numpy as np import argparse from point_cloud import PointCloud if __name__ == '__main__': parser = argparse.ArgumentParser(description='Write an example point cloud as TXT.') parser.add_argument('output', type=str, help='TXT file for example point cloud.') args = parser.parse_args() point_cloud = PointCloud(np.random.random((100, 3))) point_cloud.to_txt(args.output) print('Wrote %s.' % args.output)
# The Euclidean distance matrix. Unit is squared meters EDM = np.array( [ [ 0, 0.79, 1.63, 2.42, 2.82, 3.55, 2.44, 2.87, 2.22, 1.46 ], [ 0.79, 0, 1.45, 2.32, 2.49, 3.67, 2.32, 2.54, 1.92, 1.35 ], [ 1.63, 1.45, 0, 1.92, 2.09, 4.02, 3.48, 3.66, 2.50, 1.68 ], [ 2.42, 2.32, 1.92, 0, 0.86, 2.43, 2.92, 3.14, 1.56, 1.14 ], [ 2.82, 2.49, 2.09, 0.86, 0, 3.15, 3.10, 3.07, 1.58, 1.56 ], [ 3.55, 3.76, 4.02, 2.43, 3.15, 0, 2.44, 2.88, 2.11, 2.45 ], [ 2.44, 2.32, 3.48, 2.92, 3.10, 2.44, 0, 0.85, 1.52, 2.00 ], [ 2.87, 2.54, 3.66, 3.14, 3.07, 2.88, 0.85, 0, 1.60, 2.31 ], [ 2.22, 1.92, 2.50, 1.56, 1.58, 2.11, 1.52, 1.60, 0, 0.97 ], [ 1.46, 1.35, 1.68, 1.14, 1.56, 2.45, 2.00, 2.31, 0.97, 0 ] ] )**2 # Create the marker objects markers = PointCloud(EDM=EDM, labels=labels) # We know that these markers should be roughly on a plane markers.flatten(['7','5','3','4']) # Let the FPGA ref point be the center markers.center('FPGA') # And align x-axis onto speaker 7 markers.align('7','z') # Now there a few correction vectors to apply between the measurement points # and the center of the baffles corr_twitter = { '7' : np.array([+0.01, 0, -0.05]), '3' : np.array([0., -0.01, -0.05]),
def pcd_callback(self, msg): rospy.logwarn("Getting pcd at: %d.%09ds, (%d,%d)", msg.header.stamp.secs, msg.header.stamp.nsecs, msg.height, msg.width) pcd_original = pointcloud2_to_xyz_array(msg) pcd = PointCloud(pcd_original.T) self.plane, pcd_inlier, pcd_outlier = self.estimate_plane(pcd) transform_matrix, trans, rot, euler = get_transformation( frame_from='/velodyne', frame_to='/world', time_from=msg.header.stamp, time_to=msg.header.stamp, static_frame='/world', tf_listener=self.tf_listener, tf_ros=self.tf_ros) if not transform_matrix is None: plane_world_param = np.matmul( np.linalg.inv(transform_matrix).T, np.array( [[self.plane.a, self.plane.b, self.plane.c, self.plane.d]]).T) plane_world_param = plane_world_param / np.linalg.norm( plane_world_param[0:3]) if self.plane_tracker is None: self.plane_tracker = Tracker(msg.header.stamp, plane_world_param) else: self.plane_tracker.predict(msg.header.stamp) self.plane_tracker.update(plane_world_param) print("plane_world:", plane_world_param.T) print("plane_traker:", self.plane_tracker.filter.x_post.T) # self.plane_world = Plane3D(plane_world_param[0,0], plane_world_param[1,0], plane_world_param[2,0], plane_world_param[3,0]) self.plane_world = Plane3D(self.plane_tracker.filter.x_post[0, 0], self.plane_tracker.filter.x_post[1, 0], self.plane_tracker.filter.x_post[2, 0], self.plane_tracker.filter.x_post[3, 0]) center_pos = np.matmul( transform_matrix, np.array([[ 10, 0, (-self.plane.a * 10 - self.plane.d) / self.plane.c, 1 ]]).T) center_pos = center_pos[0:3].flatten() # normal = np.matmul( transform_matrix, np.array([[0., 0., 1., 0.]]).T) # normal = normal[0:3] normal = None marker_array = self.create_and_publish_plane_markers( self.plane_world, frame_id='world', center=center_pos, normal=normal) self.pub_plane_markers.publish(marker_array) plane_msg = Plane() plane_msg.coef[0], plane_msg.coef[1], plane_msg.coef[ 2], plane_msg.coef[ 3] = self.plane.a, self.plane.b, self.plane.c, self.plane.d self.pub_plane.publish(plane_msg) # pcd_msg_inlier = create_point_cloud(pcd_inlier.T, frame_id='velodyne') # pcd_msg_outlier = create_point_cloud(pcd_outlier.T, frame_id='velodyne') pcd_msg_inlier = xyz_array_to_pointcloud2(pcd_inlier.T, stamp=msg.header.stamp, frame_id='velodyne') pcd_msg_outlier = xyz_array_to_pointcloud2(pcd_outlier.T, stamp=msg.header.stamp, frame_id='velodyne') self.pub_pcd_inlier.publish(pcd_msg_inlier) self.pub_pcd_outlier.publish(pcd_msg_outlier) rospy.logwarn("Finished plane estimation")
for j, v in enumerate(collection.collection): mod = GridApplier(Window) w = Window() w.connect(v, 1) step = (np.random.randint(1, 6), np.random.randint(1, 6)) if j == 0: mod.apply(w, step=step, offset=(2.0, 2.0, 2.0, 1.0)) else: mod.apply(w, step=step) w = Window() w.connect(v, 0, 0) step = (np.random.randint(1, 6), np.random.randint(1, 6)) if j == 0: mod.apply(w, step=step, offset=(2.0, 2.0, 2.0, 1.0)) else: mod.apply(w, step=step) renderer = Renderer(mode=0) renderer.render(filename='building_{}'.format(image)) building.save(image) building.save(image, ext='ply') building.demolish() cloud = PointCloud() cloud.make(image) # cloud = PyntCloud.from_file("Models/{}.obj".format(image)) # cloud.to_file("{}.ply".format(image)) # cloud.to_file("{}.npz".format(image))
def filter_point_cloud_using_grid(point_cloud, grid_size, loc, R, threshold=1): """ Filter GPS point_cloud using a grid. Args: - point_cloud: GPS point cloud - grid_size: grid size in meters - loc: center of the region - R: radius of the region Return: - sample_point_cloud: an object of PointCloud. """ min_easting = loc[0] - R max_easting = loc[0] + R min_northing = loc[1] - R max_northing = loc[1] + R n_grid_x = int((max_easting - min_easting) / grid_size + 0.5) n_grid_y = int((max_northing - min_northing) / grid_size + 0.5) if n_grid_x > 1E4 or n_grid_y > 1E4: print "ERROR in filter_point_cloud_using_grid! The sampling grid is too small!" sys.exit(1) geo_hash = {} dir_hash = {} geo_hash_count = {} geo_hash_direction = {} # Traversing through all GPS points for pt_idx in range(0, len(point_cloud.locations)): pt = point_cloud.locations[pt_idx] pt_dir = point_cloud.directions[pt_idx] pt_dir_norm = np.linalg.norm(pt_dir) px = int((pt[0] - min_easting) / grid_size) py = int((pt[1] - min_northing) / grid_size) if px < 0 or px > n_grid_x or py < 0 or py > n_grid_y: print "ERROR! Point outside the grid!" sys.exit(1) if geo_hash.has_key((px, py)): geo_hash_count[(px, py)] += 1 geo_hash[(px, py)] += pt if pt_dir_norm > 0.1: geo_hash_direction[(px, py)].append(np.copy(pt_dir)) else: geo_hash_count[(px, py)] = 1.0 geo_hash[(px, py)] = np.copy(pt) if pt_dir_norm > 0.1: geo_hash_direction[(px, py)] = [pt_dir] else: geo_hash_direction[(px, py)] = [] # Deal with each non-empty box sample_point_locations = [] sample_point_directions = [] for key in geo_hash.keys(): if geo_hash_count[key] < threshold: continue pt = geo_hash[key] / geo_hash_count[key] directions = geo_hash_direction[key] if len(directions) == 0: sample_point_locations.append(pt) sample_point_directions.append(np.array([0.0, 0.0])) continue # Clustering the directions n_angle_bin = 16 delta_angle = 2 * np.pi / n_angle_bin angle_directions = [] tmp_directions = list(directions) while True: angle_bin_count = np.zeros(n_angle_bin) for direction in tmp_directions: angle = np.arccos(np.dot(direction, np.array([1.0, 0.0]))) if direction[1] < 0: angle = 2 * np.pi - angle angle_bin_idx = int(angle / delta_angle) angle_bin_count[angle_bin_idx] += 1 # Find max max_idx = np.argmax(angle_bin_count) if angle_bin_count[max_idx] < 1: break max_angle_dir = (max_idx + 0.5) * delta_angle max_angle_vec = np.array( [np.cos(max_angle_dir), np.sin(max_angle_dir)]) new_directions = [] result_direction = np.array([0.0, 0.0]) for direction in tmp_directions: if np.dot(direction, max_angle_vec) > np.cos(np.pi / 12.0): result_direction += direction else: new_directions.append(np.copy(direction)) tmp_directions = new_directions result_direction /= np.linalg.norm(result_direction) sample_point_locations.append(np.copy(pt)) sample_point_directions.append(np.copy(result_direction)) sample_point_cloud = PointCloud(np.array(sample_point_locations), np.array(sample_point_directions)) return sample_point_cloud
print('[Data] read ' + common.filename(config, 'part_space_file', '_f.h5', dataset)) #statistics = statistics.reshape(1, 1, statistics.shape[0], statistics.shape[1], statistics.shape[2]) #statistics = np.repeat(statistics, space.shape[0], axis=0) #print(space.shape, statistics.shape) #invalid_space = space*statistics points = [] point_dir = common.filename(config, 'bounding_box_txt_directory', '', dataset) + '/' for file in os.listdir(point_dir): point_file = point_dir + file point_cloud = PointCloud.from_txt(point_file) #print('[Data] read ' + point_file) points.append(point_cloud.points.shape[0]) frames = [1] * inputs.shape[0] frame_dir = common.filename(config, 'velodyne_individual_gt_txt_directory', '', dataset) + '/' for i in range(inputs.shape[0]): for k in range(-config['gt_range'], config['gt_range'] + 1, config['gt_skip']): if k == 0: continue txt_file = frame_dir + '%d_%d_%d.txt' % (i, k, frames[i])
def initial_PC(): # import calibration data for rgb camera rgb_Calib = pkl.load(open("data/RGBcamera_Calib_result.pkl","rb")) focal_length = rgb_Calib['fc'] # focal length dist_k = rgb_Calib['kc'] # distortion factor princip_center = rgb_Calib['cc'] # principle points # import feature result feature_pos = np.load('data/feature_pos.npy') feature_frame = np.load('data/feature_frame.npy') # import prediction result pred_pos_rob = np.load('data/pos_pred.npy') pred_q_rob = np.load('data/q_pred.npy') pred_T = np.load('data/T_pred.npy') # import time line of lidar and camera time_camera = np.load('data/time_camera.npy') time_lidar = np.load('data/time_lidar.npy') # number of detected physical points n_points_3d = len(feature_pos) # number of camera n_camera = time_camera.shape[0] # focal length and distortion factor f, k1, k2 = np.sum(focal_length) / 2, dist_k[0], dist_k[1] pu, pv = princip_center[0], princip_center[1] # match time step between camera and lidar lidar_ind = [] for i in range(time_camera.shape[0]): lidar_ind.append(np.argmin(np.abs(time_camera[i] - time_lidar))) # initialize Point cloud PC_set = {} # ind_frame is the current camera frame for ind_frame in range(time_camera.shape[0]): # extract corresponding lidar frame based on camera frame ind_lidar = lidar_ind[ind_frame] # current transformation of camera cur_T = pred_T[4 * ind_lidar: 4 * ind_lidar + 4, :] # construct camera parameters given by transformation cur_camp = get_cparams(cur_T, f, k1, k2) # i is the label of physical 3D point for i in range(len(feature_frame)): # if current physical point has less than 2 correspondences, filter it out. if len(feature_frame[i]) < 2: continue # check whether this physical point is detected in current camera frame if ind_frame in feature_frame[i]: # if so, extract its 2d information as well as store camera frame ind = feature_frame[i].index(ind_frame) cur_2d = feature_pos[i][ind] # if this physical point has been already added into PC set # update exist object information if i in PC_set: PC_set[i].newCorres(p2d=cur_2d, cam_ind=feature_frame[i][ind], Hwc2=cur_T, cprams=cur_camp) # if meet new physical point, create a new Point cloud structure and add into set else: PC_set[i] = PointCloud(label=i, ini_3D=None, ini_2D=cur_2d, ini_frame=feature_frame[i][ind], ini_Hw2c=cur_T, cprams=cur_camp) return PC_set, pu, pv
def test_clean_PC_set(): PC_set = {} Hw2c = np.zeros((4,4)) cprams = np.zeros((1,9)) PC1 = PointCloud(label=0, ini_3D=None, ini_2D=[0,0], ini_frame=1, ini_Hw2c=Hw2c,cprams=cprams,inliners=None) PC2 = PointCloud(label=1, ini_3D=[0,0,0], ini_2D=[10,10], ini_frame=0, ini_Hw2c=Hw2c, cprams=cprams, inliners=1) PC3 = PointCloud(label=2, ini_3D=[10, 10, 10], ini_2D=[20, 20], ini_frame=0, ini_Hw2c=Hw2c, cprams=cprams, inliners=1) PC4 = PointCloud(label=3, ini_3D=[20, 20, 20], ini_2D=[30, 30], ini_frame=1, ini_Hw2c=Hw2c + 1, cprams=cprams + 0.1, inliners=1) PC5 = PointCloud(label=4, ini_3D=[30, 30, 30], ini_2D=[40, 40], ini_frame=1, ini_Hw2c=Hw2c + 1, cprams=cprams + 0.1, inliners=0) PC6 = PointCloud(label=5, ini_3D=None, ini_2D=None, ini_frame=None, ini_Hw2c=Hw2c, cprams=cprams, inliners=None) PC7 = PointCloud(label=6, ini_3D=None, ini_2D=None, ini_frame=None, ini_Hw2c=Hw2c, cprams=cprams, inliners=None) # add new correspondences PC2.newCorres(p2d=[100,100], cam_ind=1, Hwc2=Hw2c + 1, cprams=cprams + 0.1, inliners=0) # add new correspondences PC2.newCorres(p2d=[1000, 1000], cam_ind=2, Hwc2=Hw2c + 2, cprams=cprams + 0.2, inliners=1) PC_set[1] = PC1 PC_set[2] = PC2 PC_set[3] = PC3 PC_set[4] = PC4 PC_set[5] = PC5 PC_set[6] = PC6 PC_set[7] = PC7 # do cleaning PC_set_after_cleaning = clean_PC_set(PC_set) points_3d, points_2d, points_indices, camera_indices, camera_params = convertPC(PC_set_after_cleaning) print('------- final result: -------------') print(points_2d) print('------------------------------------') print(camera_indices) print('------------------------------------') print(points_indices) print('------------------------------------')
def test_linearT(Hw2c_set, points_2d, points_3d, points_indices, camera_indices, camera_params, gt_points_3d): n_points = points_3d.shape[0] n_cameras = camera_params.shape[0] # Obtain first frame print('\n---------------Testing linearT()------------------\n') # collect point clouds pkl_folder = 'pkl' if not os.path.exists(pkl_folder): os.makedirs(pkl_folder) if not os.path.exists(os.path.join(pkl_folder, 'PC_set.pkl')): PC_set = {} gt_3D_set = {} for p in range(points_3d.shape[0]): p3d = points_3d[p, :] gt_p3d = gt_points_3d[p] gt_3D_set[p] = gt_p3d print('- Point {0}/ {1}'.format(p, points_3d.shape[0])) label = points_indices[p] PC = PointCloud(label=label, ini_3D=p3d) # Find 2d (u,v) add2 = 0 for i in range(points_indices.shape[0]): if points_indices[i] == label: PC.newCorres(points_2d[i], camera_indices[i], Hw2c_set[i], camera_params[camera_indices[i], :]) # add2 += 1 PC_set[p] = PC PC_data = {} PC_data['pred'] = PC_set PC_data['gt'] = gt_3D_set with open(os.path.join(pkl_folder, 'PC_set.pkl'), 'wb') as f: pkl.dump(PC_data, f, pkl.HIGHEST_PROTOCOL) else: with open(os.path.join(pkl_folder, 'PC_set.pkl'), 'rb') as f: PC_data = pkl.load(f) PC_set = PC_data['pred'] gt_3D_set = PC_data['gt'] # Test linear Errors_orig = [] Errors_pred = [] total_ignore = 0 for i in PC_set.keys(): cur_PC = PC_set[i] gt_p3d = gt_3D_set[i] if len(cur_PC.Pos2D) > 1: orgerror = compare_with_groundtruth(PC_set[i].Pos3D, gt_p3d) # Errors_orig.append(orgerror) # print('\n * Point',cur_PC.Pos3D) # print('* Has {0} frames'.format(len(cur_PC.Pos2D))) cur_PC, opt_inliers = RansacLinearT(cur_PC, pu=0, pv=0) if cur_PC == None: print('-------- Ignore! ------------') total_ignore += 1 PC_set.pop(i) continue PC_set[i] = cur_PC # perror = compare_with_groundtruth(cur_PC.Pos3D, gt_p3d) perror = linearT_errEst(cur_PC=cur_PC, pu=0, pv=0, inliners=opt_inliers) print('>> ', i, '/', len(PC_set.keys()), ' Error:', orgerror, ' vs ', perror) Errors_pred.append(perror) # time.sleep(0.5) print('Total errors:') print('Original:', np.sum(np.array(Errors_orig)) / len(Errors_pred)) print('Pred:', np.sum(np.array(Errors_pred)) / len(Errors_pred)) print('Total ignore PC:', total_ignore) # save the obtained PC_set to the file points_3d, points_2d, points_indices, camera_indices, camera_params = util.convertPC( PC_set, n_cameras=n_cameras, n_3dpoints=n_points) DATA = {} DATA['params'] = camera_params DATA['cam_indices'] = camera_indices DATA['points_indices'] = points_indices DATA['2d'] = points_2d DATA['3d'] = points_3d with open('linearTed_online_data.pkl', 'wb') as f: pkl.dump(DATA, f, pkl.HIGHEST_PROTOCOL) plt.figure() plt.scatter(range(len(Errors_pred)), Errors_pred) plt.show() return Errors_orig, Errors_pred
def correct_direction_with_lines(sample_point_cloud, lines, search_radius=15, angle_threshold=np.pi / 6.0): """ Correct sample_point_cloud using the extracted lines Args: - sample_point_cloud: an object of PointCloud - lines: an array of [(p_start_e, p_start_n), (p_end_e, p_end_n)] - search_radius: distance to search, in meters. Default is 10m. Return: - new_sample_point_cloud: an object of PointCloud """ new_sample_locations = [] new_sample_directions = [] line_starts = lines[:, 0, :] line_ends = lines[:, 1, :] line_vecs = line_ends - line_starts sample_point_hash = {} for pt_idx in range(0, sample_point_cloud.locations.shape[0]): if np.linalg.norm(sample_point_cloud.locations[pt_idx]) < 0.1: new_sample_locations.append(sample_point_cloud.locations[pt_idx]) new_sample_locations.append(sample_point_cloud.directions[pt_idx]) pt = sample_point_cloud.locations[pt_idx] pt_key = (int(pt[0]), int(pt[1])) # Find nearby line segments nearby_line_idxs = [] vec1s = pt - line_starts vec2s = pt - line_ends for line_idx in range(0, len(vec1s)): if np.dot(vec1s[line_idx], vec2s[line_idx]) > 0: continue line_dir = line_vecs[line_idx] / np.linalg.norm( line_vecs[line_idx]) line_norm = np.array([-1 * line_dir[1], line_dir[0]]) dist = abs(np.dot(vec1s[line_idx], line_norm)) if dist <= search_radius: nearby_line_idxs.append(line_idx) if len(nearby_line_idxs) == 0: continue # Check if direction is consistent potential_direction = np.array([0.0, 0.0]) direction_acceptable = False for line_idx in nearby_line_idxs: line_dir = line_vecs[line_idx] / np.linalg.norm( line_vecs[line_idx]) dot_value = np.dot(line_dir, sample_point_cloud.directions[pt_idx]) if abs(dot_value) > np.cos(angle_threshold): direction_acceptable = True if dot_value > 0: potential_direction += line_dir else: potential_direction += -1 * line_dir if direction_acceptable: potential_direction /= np.linalg.norm(potential_direction) if not sample_point_hash.has_key(pt_key): sample_point_hash[pt_key] = [potential_direction] new_sample_locations.append( sample_point_cloud.locations[pt_idx]) new_sample_directions.append(potential_direction) else: should_insert = True for pt_direction in sample_point_hash[pt_key]: if np.dot(pt_direction, potential_direction) > np.cos(angle_threshold): should_insert = False if should_insert: new_sample_locations.append( sample_point_cloud.locations[pt_idx]) new_sample_directions.append(potential_direction) sample_point_hash[pt_key].append(potential_direction) return PointCloud(np.array(new_sample_locations), np.array(new_sample_directions))
from point_cloud import PointCloud if __name__ == '__main__': if len(sys.argv) < 2: print('[Data] Usage python 13_ply_observations.py config_folder') exit(1) config_folder = sys.argv[1] + '/' assert os.path.exists( config_folder), 'directory %s does not exist' % config_folder config_files = [ config_file for config_file in os.listdir(config_folder) if not (config_file.find('prior') > 0) ] for config_file in config_files: print('[Data] reading ' + config_folder + config_file) config = utils.read_json(config_folder + config_file) for k in range(config['n_observations']): txt_directory = common.dirname(config, 'txt_gt_dir') + str(k) + '/' assert os.path.exists(txt_directory) ply_directory = common.dirname(config, 'ply_gt_dir') + str(k) + '/' if not os.path.exists(ply_directory): os.makedirs(ply_directory) for filename in os.listdir(txt_directory): point_cloud = PointCloud.from_txt(txt_directory + filename) point_cloud.to_ply(ply_directory + filename[:-4] + '.ply') print('[Data] wrote ' + ply_directory + filename[:-4] + '.ply')
def filter_point_cloud_using_grid(point_cloud, point_directions, sample_grid_size, loc, R): """ Sample the input point cloud using a uniform grid. If there are points in the cell, we will use the average. """ min_easting = loc[0]-R max_easting = loc[0]+R min_northing = loc[1]-R max_northing = loc[1]+R n_grid_x = int((max_easting - min_easting)/sample_grid_size + 0.5) n_grid_y = int((max_northing - min_northing)/sample_grid_size + 0.5) if n_grid_x > 1E4 or n_grid_y > 1E4: print "ERROR! The sampling grid is too small!" sys.exit(1) sample_points = [] #sample_directions = [] sample_canonical_directions = [] geo_hash = {} dir_hash = {} geo_hash_count = {} geo_hash_direction = {} for pt_idx in range(0, len(point_cloud.locations)): pt = np.copy(point_cloud.locations[pt_idx]) #pt_dir = point_cloud.directions[pt_idx] px = int((pt[0] - min_easting) / sample_grid_size) py = int((pt[1] - min_northing) / sample_grid_size) if px<0 or px>n_grid_x or py<0 or py>n_grid_y: print "ERROR! Point outside the grid!" sys.exit(1) if geo_hash.has_key((px, py)): geo_hash_count[(px, py)] += 1 geo_hash[(px, py)] += pt #dir_hash[(px, py)] += pt_dir for direction in point_directions[pt_idx]: geo_hash_direction[(px, py)].append(np.copy(direction)) else: geo_hash_count[(px, py)] = 1.0 geo_hash_direction[(px, py)] = [] for direction in point_directions[pt_idx]: geo_hash_direction[(px, py)].append(np.copy(direction)) geo_hash[(px, py)] = pt #dir_hash[(px, py)] = pt_dir for key in geo_hash.keys(): pt = geo_hash[key] / geo_hash_count[key] #dir_hash[key] /= geo_hash_count[key] sample_points.append(pt) #sample_directions.append(dir_hash[key]) directions = [] for direction in geo_hash_direction[key]: if len(directions) == 0: directions.append(direction) else: found_match = False for idx in range(0, len(directions)): dot_value = np.dot(direction, directions[idx]) if abs(dot_value) > 0.7: found_match = True break if not found_match: directions.append(np.copy(direction)) sample_canonical_directions.append(list(directions)) sample_point_cloud = PointCloud(sample_points, [-1]*len(sample_points), [-1]*len(sample_points)) return sample_point_cloud, sample_canonical_directions
ROOT = os.path.dirname(os.path.abspath(__file__)) HOME = str(Path.home()) device = torch.device('cuda') if torch.cuda.is_available() else torch.device( 'cpu') model = torch.load(os.path.join(ROOT, args.model)) model.to(device) data_dir = os.path.join(HOME, args.data_dir) frame_dir = os.path.join(data_dir, 'frame') front_color_dir = os.path.join(data_dir, 'front_color') side_color_dir = os.path.join(data_dir, 'side_color') plane_estimator = PlaneEstimator(args, model) tracker = MOT_Tracker(args, model) side_pcd = PointCloud(vis=False) history = defaultdict(list) ################ MAIN LOOP, READ FRAMES ONE BY ONE ############### num_frames = len(os.listdir(frame_dir)) print("Found %d frames, start loading now....") i_start = 1 num_frames = 500 for i in range(i_start, i_start + num_frames): try: frame = pickle.load( open(os.path.join(frame_dir, 'frame_%07d.pkl' % (i)), 'rb')) print('Loaded frame number %d' % i)