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()
