def load_pcd_content(content,
w=2,
color_mode="intensity",
intensity_filter=50):
pc = pypcd.PointCloud.from_buffer(content)
fmt = GeomVertexFormat() #3 component vertex, w/ 4 comp color
fmt_arr = GeomVertexArrayFormat()
fmt_arr.addColumn(InternalName.make('vertex'), 3, Geom.NTFloat32,
Geom.CPoint)
fmt_color_arr = GeomVertexArrayFormat()
fmt_color_arr.addColumn(InternalName.make('color'), 4, Geom.NTUint8,
Geom.CColor)
fmt.addArray(fmt_arr)
fmt.addArray(fmt_color_arr)
fmt = GeomVertexFormat.registerFormat(fmt)
vertexData = GeomVertexData('points', fmt, Geom.UHStatic)
pointCloud = GeomPoints(Geom.UHStatic)
pc.pc_data.dtype = numpy.dtype("<f4")
v, c = arrayfilter.vertices_filter(pc.pc_data.reshape((pc.points, 4)))
for i in xrange(len(v)):
pointCloud.addVertex(i)
pointCloud.closePrimitive()
arr = GeomVertexArrayData(fmt.getArray(0), GeomEnums.UHStream)
datahandle = arr.modifyHandle()
datahandle.copyDataFrom(v)
vertexData.setArray(0, arr)
arr = GeomVertexArrayData(fmt.getArray(1), GeomEnums.UHStream)
datahandle = arr.modifyHandle()
datahandle.copyDataFrom(c)
vertexData.setArray(1, arr)
cloud = Geom(vertexData)
cloud.addPrimitive(pointCloud)
cloudNode = GeomNode('points')
cloudNode.addGeom(cloud)
cloudNodePath = NodePath(cloudNode)
cloudNodePath.setRenderModeThickness(w)
cloudNodePath.setRenderModePerspective(True)
return cloudNode
def getVertexData(vertex,
vertex_index,
normal=None,
normal_index=None,
texcoordset=(),
texcoord_indexset=(),
textangentset=(),
textangent_indexset=(),
texbinormalset=(),
texbinormal_indexset=()):
format = GeomVertexFormat()
formatArray = GeomVertexArrayFormat()
indices2stack = [vertex_index.reshape(-1, 1)]
alldata = [vertex]
formatArray.addColumn(InternalName.make("vertex"), 3, Geom.NTFloat32,
Geom.CPoint)
if normal is not None:
indices2stack.append(normal_index.reshape(-1, 1))
alldata.append(collada.util.normalize_v3(numpy.copy(normal)))
formatArray.addColumn(InternalName.make("normal"), 3, Geom.NTFloat32,
Geom.CVector)
if len(texcoordset) > 0:
indices2stack.append(texcoord_indexset[0].reshape(-1, 1))
alldata.append(texcoordset[0])
formatArray.addColumn(InternalName.make("texcoord"), 2, Geom.NTFloat32,
Geom.CTexcoord)
if len(textangentset) > 0:
indices2stack.append(textangent_indexset[0].reshape(-1, 1))
alldata.append(textangentset[0])
formatArray.addColumn(InternalName.make("tangent"), 3, Geom.NTFloat32,
Geom.CVector)
if len(texbinormalset) > 0:
indices2stack.append(texbinormal_indexset[0].reshape(-1, 1))
alldata.append(texbinormalset[0])
formatArray.addColumn(InternalName.make("binormal"), 3, Geom.NTFloat32,
Geom.CVector)
#have to flatten and reshape like this so that it's contiguous
stacked_indices = numpy.hstack(indices2stack).flatten().reshape(
(-1, len(indices2stack)))
#index_map - maps each unique value back to a location in the original array it came from
# eg. stacked_indices[index_map] == unique_stacked_indices
#inverse_map - maps original array locations to their location in the unique array
# e.g. unique_stacked_indices[inverse_map] == stacked_indices
unique_stacked_indices, index_map, inverse_map = numpy.unique(
stacked_indices.view([('', stacked_indices.dtype)] *
stacked_indices.shape[1]),
return_index=True,
return_inverse=True)
unique_stacked_indices = unique_stacked_indices.view(
stacked_indices.dtype).reshape(-1, stacked_indices.shape[1])
#unique returns as int64, so cast back
index_map = numpy.cast['uint32'](index_map)
inverse_map = numpy.cast['uint32'](inverse_map)
#sort the index map to get a list of the index of the first time each value was encountered
sorted_map = numpy.cast['uint32'](numpy.argsort(index_map))
#since we're sorting the unique values, we have to map the inverse_map to the new index locations
backwards_map = numpy.zeros_like(sorted_map)
backwards_map[sorted_map] = numpy.arange(len(sorted_map),
dtype=numpy.uint32)
#now this is the new unique values and their indices
unique_stacked_indices = unique_stacked_indices[sorted_map]
inverse_map = backwards_map[inverse_map]
#combine the unique stacked indices into unique stacked data
data2stack = []
for idx, data in enumerate(alldata):
data2stack.append(data[unique_stacked_indices[:, idx]])
unique_stacked_data = numpy.hstack(data2stack).flatten()
unique_stacked_data.shape = (-1)
all_data = unique_stacked_data.tostring()
format.addArray(formatArray)
format = GeomVertexFormat.registerFormat(format)
vdata = GeomVertexData("dataname", format, Geom.UHStatic)
arr = GeomVertexArrayData(format.getArray(0), GeomEnums.UHStream)
datahandle = arr.modifyHandle()
datahandle.setData(all_data)
all_data = None
vdata.setArray(0, arr)
datahandle = None
arr = None
indexFormat = GeomVertexArrayFormat()
indexFormat.addColumn(InternalName.make("index"), 1, Geom.NTUint32,
Geom.CIndex)
indexFormat = GeomVertexArrayFormat.registerFormat(indexFormat)
indexArray = GeomVertexArrayData(indexFormat, GeomEnums.UHStream)
indexHandle = indexArray.modifyHandle()
indexData = inverse_map.tostring()
indexHandle.setData(indexData)
return vdata, indexArray
def getVertexData(vertex, vertex_index, normal=None, normal_index=None,
texcoordset=(), texcoord_indexset=(),
textangentset=(), textangent_indexset=(),
texbinormalset=(), texbinormal_indexset=()):
format = GeomVertexFormat()
formatArray = GeomVertexArrayFormat()
indices2stack = [vertex_index.reshape(-1, 1)]
alldata = [vertex]
formatArray.addColumn(InternalName.make("vertex"), 3, Geom.NTFloat32, Geom.CPoint)
if normal is not None:
indices2stack.append(normal_index.reshape(-1, 1))
alldata.append(collada.util.normalize_v3(numpy.copy(normal)))
formatArray.addColumn(InternalName.make("normal"), 3, Geom.NTFloat32, Geom.CVector)
if len(texcoordset) > 0:
indices2stack.append(texcoord_indexset[0].reshape(-1, 1))
alldata.append(texcoordset[0])
formatArray.addColumn(InternalName.make("texcoord"), 2, Geom.NTFloat32, Geom.CTexcoord)
if len(textangentset) > 0:
indices2stack.append(textangent_indexset[0].reshape(-1, 1))
alldata.append(textangentset[0])
formatArray.addColumn(InternalName.make("tangent"), 3, Geom.NTFloat32, Geom.CVector)
if len(texbinormalset) > 0:
indices2stack.append(texbinormal_indexset[0].reshape(-1, 1))
alldata.append(texbinormalset[0])
formatArray.addColumn(InternalName.make("binormal"), 3, Geom.NTFloat32, Geom.CVector)
#have to flatten and reshape like this so that it's contiguous
stacked_indices = numpy.hstack(indices2stack).flatten().reshape((-1, len(indices2stack)))
#index_map - maps each unique value back to a location in the original array it came from
# eg. stacked_indices[index_map] == unique_stacked_indices
#inverse_map - maps original array locations to their location in the unique array
# e.g. unique_stacked_indices[inverse_map] == stacked_indices
unique_stacked_indices, index_map, inverse_map = numpy.unique(stacked_indices.view([('',stacked_indices.dtype)]*stacked_indices.shape[1]), return_index=True, return_inverse=True)
unique_stacked_indices = unique_stacked_indices.view(stacked_indices.dtype).reshape(-1,stacked_indices.shape[1])
#unique returns as int64, so cast back
index_map = numpy.cast['uint32'](index_map)
inverse_map = numpy.cast['uint32'](inverse_map)
#sort the index map to get a list of the index of the first time each value was encountered
sorted_map = numpy.cast['uint32'](numpy.argsort(index_map))
#since we're sorting the unique values, we have to map the inverse_map to the new index locations
backwards_map = numpy.zeros_like(sorted_map)
backwards_map[sorted_map] = numpy.arange(len(sorted_map), dtype=numpy.uint32)
#now this is the new unique values and their indices
unique_stacked_indices = unique_stacked_indices[sorted_map]
inverse_map = backwards_map[inverse_map]
#combine the unique stacked indices into unique stacked data
data2stack = []
for idx, data in enumerate(alldata):
data2stack.append(data[unique_stacked_indices[:,idx]])
unique_stacked_data = numpy.hstack(data2stack).flatten()
unique_stacked_data.shape = (-1)
all_data = unique_stacked_data.tostring()
format.addArray(formatArray)
format = GeomVertexFormat.registerFormat(format)
vdata = GeomVertexData("dataname", format, Geom.UHStatic)
arr = GeomVertexArrayData(format.getArray(0), GeomEnums.UHStream)
datahandle = arr.modifyHandle()
datahandle.setData(all_data)
all_data = None
vdata.setArray(0, arr)
datahandle = None
arr = None
indexFormat = GeomVertexArrayFormat()
indexFormat.addColumn(InternalName.make("index"), 1, Geom.NTUint32, Geom.CIndex)
indexFormat = GeomVertexArrayFormat.registerFormat(indexFormat)
indexArray = GeomVertexArrayData(indexFormat, GeomEnums.UHStream)
indexHandle = indexArray.modifyHandle()
indexData = inverse_map.tostring()
indexHandle.setData(indexData)
return vdata, indexArray
