def generate_freespace2(self, pc=None, pose=None, resolution=None, minR=0.5, maxR=5): ''' Uses existing ray-trace method to get around the current limitations of above implementation. Generates an occuplied list consisting of the observed free space at the specified resolution. This is useful for detecting object removal from a map. ''' if resolution == None: res = self.get_resolution() else: res = resolution assert res >= self.get_resolution() free_space_ol = occupiedlist.OccupiedList(res, maxvoxels=2e6) if pc == None and pose == None: # generate from map using pose list if self._sensor_pts is None: sensor_model_ol = _build_Kinect_sensor_model(res, minR=minR, maxR=maxR) self._sensor_pts = pointcloud.PointCloud( sensor_model_ol.getpoints()) print "Sensor model points:", len(self._sensor_pts) base_ol = self.mrol.getfinest() for pose in self.trajectory: print pose inliers, outliers = base_ol.segment_ovl( pose, self._sensor_pts.points) inlier_pc = pointcloud.PointCloud(inliers) free_pts = self.free_space_ray_trace(inliers, pose.getTuple()[:3], self.get_resolution()) free_space_ol.addpoints(free_pts.points) else: assert xyzs != None and pose != None, "Both xyzs and pose must be specified" # resampling to align with grid. bad hack, but makes ray_cast easier. voxs = occupiedlist.pointstovoxels(xyzs, res) voxs = quantizer.uniquerows(voxs, 0) X = occupiedlist.voxelstopoints(voxs, res) X_pc = pointcloud.PointCloud(X) free_pts = self.free_space_ray_trace(X_pc, pose.getTuple()[:3], self.get_resolution()) free_space_ol.addpoints(free_pts.points) return free_space_ol
def ray_trace_pts(self, point1, point2, return_extras=False): ''' perform a ray-trace from point1 to point2 and return all map voxels at the base resolution along the ray as a pointcloud object. ''' if not return_extras: ray_vox_hits = self.mrol.raytracepts(point1, point2, return_extras=False) return pointcloud.PointCloud(ray_vox_hits) else: ray_vox_hits, range2, dir_unit, all_ray_pts = self.mrol.raytracepts( point1, point2, return_extras=True) return pointcloud.PointCloud( ray_vox_hits), range2, dir_unit, pointcloud.PointCloud( all_ray_pts)
def free_space_ray_trace(self, pc, origin, resolution, voxel_offset=0): # This is essentially independant of the VoxelMap; put elsewhere? free_pts = [] res = resolution v_off = np.ceil(voxel_offset * (res / self.get_resolution())) free_pts_OL = occupiedlist.OccupiedList(res, maxvoxels=3e6) count = 0 for point in pc.points: ray_vox_hits, range2, dir_unit, all_ray_pts = self.ray_trace_pts( origin, point, return_extras=True) all_ray_pts = all_ray_pts.points[:-(v_off + 1)] all_ray_vox = occupiedlist.pointstovoxels(all_ray_pts, res) all_ray_vox_ol = occupiedlist.OccupiedList(1) all_ray_vox_ol.addpoints(all_ray_vox) ray_vox_hits_ol = occupiedlist.OccupiedList(1) ray_vox_hits_ol.addpoints(ray_vox_hits.points) free_ray_vox_ol = all_ray_vox_ol - ray_vox_hits_ol free_pts_OL.addpoints(free_ray_vox_ol.getpoints() * res) count += 1 #if np.remainder(count / len(pc) * 100,1) == 0: # print count / len(pc) * 100 , '%' return pointcloud.PointCloud(free_pts_OL.getpoints())
def forward(self, proto_path, caffe_model_path, PCD_Path, USE_CAFFE_GPU=False): if USE_CAFFE_GPU: caffe.set_mode_gpu() caffe.set_device(0) else: caffe.set_mode_cpu() # step1 : feature_generate rawpoint = pc.PointCloud() rawpoint.ReadFromBinFile(PCD_Path) self.vldpc = vp.ValidPoints(pcdata=rawpoint, rows=640, cols=640, lrange=60, max_height=5, min_height=-5) ffg = fg.FeatureGenerator() self.feature_blob = ffg.Generate(self.vldpc) # setp2 : load caffe model and forword cnnseg_net = caffe.Net(proto_path, caffe_model_path, caffe.TEST) cnnseg_net.blobs['data'].data[...] = self.feature_blob self.outblobs = cnnseg_net.forward()
def get_points(self, level=-1): '''Returns the map as a point cloud of the voxel centers. If the map has been setup with multiple resolutions, return the voxels at the desired resolution. Defaults to the finest resolution.''' voxel_centers = self.mrol.getpoints(level=level) pc = pointcloud.PointCloud(voxel_centers) return pc
def forward(self, proto_path, caffe_model_path, PCD_Path, USE_CAFFE_GPU=False, readformat='bin'): # 上面提示函数外部已有相同变量名,做到不同 if USE_CAFFE_GPU: caffe.set_mode_gpu() caffe.set_device(0) else: caffe.set_mode_cpu() # step1 : feature_generate rawpoint = pc.PointCloud() if readformat == 'bin': rawpoint.ReadFromBinFile(PCD_Path) elif readformat == 'pcd': rawpoint.ReadFromPcdFile(PCD_Path) else: print('未知格式,点云读取失败!\n unknow format,Failure!') self.vldpc = rawpoint.project2map([-60, 60], [-60, 60], [-5, 5], 640, 640) feature = fg.FeatureGenerator() self.feature_blob = feature.Generate(self.vldpc) # setp2 : load caffe model and forword self.cnnseg_Net = caffe.Net(proto_path, caffe_model_path, caffe.TEST) self.cnnseg_Net.blobs['data'].data[...] = self.feature_blob self.outblobs = self.cnnseg_Net.forward()
def _grid_dataset(self, dataset, label=None): """ Slice the pointcloud dataset to grid cells.""" # x and y centres and edges to loop with xs = self.centres[0, 0, :] ys = self.centres[1, :, 0] xe = self.x_edges ye = self.y_edges # Activate tileflag if tile passed if isinstance(dataset, tiles.Tiles): tf = True first = True tile = dataset else: tf = False PC = dataset dataset_grid = np.empty(self.shape, dtype=object) for i, (yc, y1, y2) in enumerate(zip(ys, ye[:-1], ye[1:])): for j, (xc, x1, x2) in enumerate(zip(xs, xe[:-1], xe[1:])): bounds = {'x': (x1, x2), 'y': (y1, y2)} # call method to get a PC from dataset, if necessary if tf: if first: # generate starting tiles and pointcloud tile = dataset[bounds] PC = pc.PointCloud(tile) first = False else: newtile = dataset[ bounds] # tiles proposed for current cell if newtile.bounds != tile.bounds: # update PC if new area tile = newtile PC = pc.PointCloud(tile) PC_slice = PC[bounds] setattr(PC_slice, 'centre', (xc, yc)) setattr(PC_slice, 'label', label) dataset_grid[i, j] = PC_slice return dataset_grid
def __init__(self, channels, ls, beta, th=-92, p_th=0.7): assert len(channels) % 2 == 0, "Expected even number of channels" assert len(channels) == 2 * len( ls), "Mismatch: number of regions and number of queue parameters" self.n = len(ls) self.channels = channels self.p_th = p_th self.gamma_th = th self.region = channels[0].region self.res = channels[0].res self.findJointConnectivityField() self.Xis = [ _indices_to_pts(cf, self.region, self.res) for cf in self.cfields ] self.cregions = [PC.PointCloud(Xi['points']) for Xi in self.Xis] for pc in self.cregions: pc.partition(algo=4) self.ps = PS.PollingSystem(ls, beta)
self.idx_row = None # 投射到map图中的行 self.GetValid() def GetValid(self): idx_row = self.F2I(self.RawPoints.x, self.rows, self.lrange) idx_col = self.F2I(self.RawPoints.y, self.cols, self.lrange) valid_idx = np.all([ idx_row >= 0, idx_row < self.rows, idx_col >= 0, idx_col < self.cols, self.RawPoints.z >= self.min_height, self.RawPoints.z <= self.max_height ], axis=0) self.valid_x = self.RawPoints.x[valid_idx] self.valid_y = self.RawPoints.y[valid_idx] self.valid_z = self.RawPoints.z[valid_idx] self.valid_intensity = self.RawPoints.intensity[valid_idx] self.validindex = np.where(valid_idx == True)[0] self.idx_row = idx_row[valid_idx] self.idx_col = idx_col[valid_idx] @staticmethod def F2I(x, rows, lrange): return np.floor(rows * (lrange - x) / (2 * lrange)).astype(np.int32) if __name__ == '__main__': import pointcloud as pc tst = pc.PointCloud() tst.ReadFromPcdFile('/home/reme/桌面/CNNSeg/data/test.pcd') vldpc = ValidPoints(tst, 640, 640, 5, -5, 60) pass
def ray_trace(self, pose, length, axis=0): '''perform a ray-trace from the pose and return all map voxels at the base resolution along the ray as a pointcloud object. ''' return pointcloud.PointCloud( self.mrol.raytrace(pose, length, axis=axis))
def display(self): pc = pointcloud.PointCloud(self.getpoints()) pc.display()
# -*- coding:utf-8 -*- import sys sys.path.append("/home/reme/桌面/Lidar_Perception_Apollo/src/Pointcloud") import numpy as np import pointcloud as pc pcdata = pc.PointCloud() pcdata.ReadFromPcdFile("/home/reme/桌面/Lidar_Perception_Apollo/data/test.pcd") pcdata.ReadFromBinFile("/home/reme/桌面/Lidar_Perception_Apollo/data/51.bin") pass
# -*- coding:utf-8 -*- import mayavi.mlab as mlab import pointcloud as pc def display(pcdata): fig = mlab.figure(bgcolor=(0, 0, 0), size=(900, 600)) s = mlab.points3d(pcdata.x, pcdata.y, pcdata.z, pcdata.intensity, figure=fig, mode='point', colormap='spectral') s.scene.show_axes = True return s if __name__ == '__main__': pc_data = pc.PointCloud() pc_data.ReadFromBinFile('../../data/67.bin') display(pc_data) mlab.show()