def compute_normal_and_curvature(points, k = 30):
"""
Args:
points: pointcloud-Nx3
k: number of pointset
Returns:
data = [x, y, z, nx, ny, nz, curvature]
"""
assert points.shape[1] == 3
points = pd.DataFrame(points)
points.columns = ['x', 'y', 'z']
cloud = PyntCloud(points)
k_neighbors = cloud.get_neighbors(k=k)
cloud_normal = copy.copy(cloud)
cloud_normal = cloud_normal.add_scalar_field("normals", k_neighbors=k_neighbors)
ev = copy.copy(cloud)
ev = cloud.add_scalar_field("eigen_values", k_neighbors=k_neighbors)
cloud.add_scalar_field("curvature", ev=ev)
points = cloud.points
nx = "nx(" + (k + 1).__str__() + ")"
ny = "ny(" + (k + 1).__str__() + ")"
nz = "nz(" + (k + 1).__str__() + ")"
curvature = "curvature(" + (k + 1).__str__() + ")"
filter_columns = ['x', 'y', 'z', nx, ny, nz, curvature]
data = points.reindex(columns=filter_columns)
return data
class CorrespondingLidarPointCloud():
def __init__(self, pcl_path, pointsensor):
self._points = np.fromfile(str(pcl_path), dtype=np.float32).reshape((-1, 5))[:, :3]
self._pointsensor = pointsensor
self._pc = PyntCloud(self._points.T) #[3, N] -> [N, 3]
def getPointCloud(self):
return self._points
def getPointsensor(self):
return self._pointsensor
def getOccupancyMatrix(self, as_tensor=True):
assert self._occupancy, "Call voxelize() first"
if as_tensor:
return torch.tensor(self._occupancy)
return self._occupancy
def getNeighbors(self):
assert self._neighbors, "Call generateKNN() first"
return self._neighbors
def getKNN(self, k):
return self._pc.get_neighbors(k=k)
def nbr_points(self) -> int:
return self._points.shape[1]
def voxelize(self, size_x, size_y, size_z):
voxelgrid_id = self._pc.add_structure("voxelgrid", size_x=size_x, size_y=size_y, size_z=size_z, regular_bounding_box=False)
voxelgrid = self._pc.structures[voxelgrid_id]
self._occupancy = voxelgrid.get_feature_vector(mode='binary')
def generateKNN(self):
self._neighbors = self._pc.get_neighbors() #(N, k)
def translate(self, x):
for i in range(3):
self._points[i, :] = self._points[i, :] + x[i]
def rotate(self, rot_matrix):
self._points = np.dot(rot_matrix, self._points)
def transform(self, transf_matrix):
self._points = transf_matrix.dot(np.vstack((self._points, np.ones(self.nbr_points()))))
def pyntcloud_and_eigenvalues():
cloud = PyntCloud(pd.DataFrame(
data={
"x": np.array([0, 0, 1, -1, 0, 0], dtype=np.float32),
"y": np.array([0, 1, 0, 0, -1, 0], dtype=np.float32),
"z": np.array([0, 0, 0, 0, 0, 2], dtype=np.float32)
}))
k_neighbors = cloud.get_neighbors(k=4)
ev = cloud.add_scalar_field("eigen_values", k_neighbors=k_neighbors)
return cloud, ev
def computeNormals(model):
cloudable = pd.DataFrame(data=np.transpose(model), columns=['x', 'y', 'z'])
# calculates a pointcloud of the input model using a pandas DataFrame
cloud = PyntCloud(cloudable)
# use neighbors to get normals from pointcloud
neighbors = cloud.get_neighbors(k=10)
cloud.add_scalar_field('normals', k_neighbors=neighbors)
# extract normals from the altered DataFrame
normals = np.transpose(np.asarray(cloudable.loc[:, 'nx(10)':'nz(10)']))
master = np.repeat(masterOrig, len(normals[0]), axis=1)
# taking the dot product column-wise; the 'ij,ij->' notation is saying:
# take dot product of ith row, jth column of normals and ith row, jth column of master
# then, create boolean mask array for normal comparison
I = np.einsum('ij,ij->j', normals, master) < 0
# flip all values in column if I is true at that column (dot prod < 0)
normals[:, I] = -normals[:, I]
return (normals)
