- Read the pointcloud
- For every point in the pointcloud
- Identify the neighboring points
- Run PCA on the points
- Identify the orientaiton : eigen vector and eigen values
- keep data of all the points and their eigen vector and values
- Define eigen vector as feature
- Use K-Means clustering with eigen vector and eigenvalues as feature
- Visualize the clustered Pointcloud
import open3d as o3d
import os
import numpy as np
import cv2
from scipy.spatial import KDTree, cKDTree
# function ： Calculation PCA Function of
# Input ：
# data： Point cloud ,NX3 Matrix
# correlation： distinguish np Of cov and corrcoef, When it is not input, it defaults to False
# sort: Eigenvalue sorting , Sorting is for the convenience of other functions , When it is not input, it defaults to True
# Output ：
# eigenvalues： The eigenvalue
# eigenvectors： Eigenvector
def PCA(data, correlation=False, sort=True):
# Homework 1
# # Shielding starts
# # axis = 0, So the output matrix is 1 That's ok , Find the average of each column （ Average each line ）;
# # axis = 1, The output matrix is 1 Column , Find the average of each line （ Average according to each column ）.
# # It's understandable ,axis How many , That shows which dimensions add up to average .
# n*3
data = data.transpose()
# Subtract the mean of each dimension X
data = data - data.mean(axis = 1,keepdims=True)
#X^T
data_T = data.transpose()
H = np.matmul(data,data_T)
# u The size is (M, M),s The size is (M, N),v The size is (N, N).
# A = u * s * v
eigenvectors,eigenvalues,_ = np.linalg.svd(H,full_matrices=True)
# End of shielding
if sort:
# argsort It's in descending order [::-1] It's from big to small
sort = eigenvalues.argsort()[::-1]
# Eigenvalues arranged from large to small
eigenvalues = eigenvalues[sort]
# Eigenvectors corresponding to large to small eigenvalues
eigenvectors = eigenvectors[:, sort]
return eigenvalues, eigenvectors
def main():
# Load the original point cloud
# point_cloud_o3d = o3d.io.read_point_cloud('21.ply')
point_cloud_o3d = o3d.io.read_point_cloud('/media/achyut/Extreme SSD/Olsen_farms_Martin_Pointclouds/20220119-131337.ply')
# point_cloud_o3d = point_cloud_o3d.uniform_down_sample(every_k_points=10)
o3d.visualization.draw_geometries([point_cloud_o3d]) # Displays the original point cloud
# pt_cloud_xyz = np.array(point_cloud_o3d.points)
# color_xyz = np.array(point_cloud_o3d.colors)
# asd = pt_cloud_xyz[:,2]>0.1
# asd = [np.bitwise_and(asd ,pt_cloud_xyz[:,0]<0.8 )]
# asd[0] =np.squeeze(asd[0])
# xyz1 = pt_cloud_xyz[asd[0]]
# xyz2 = xyz1.reshape(-1,3)
# c1 = color_xyz[asd[0]]
# c2 = c1.reshape(-1,3)
# point_cloud_o3d = o3d.geometry.PointCloud()
# point_cloud_o3d.points = o3d.utility.Vector3dVector(xyz2)
# point_cloud_o3d.colors = o3d.utility.Vector3dVector(c2)
# Get points from the point cloud , Only points are processed
points = np.asarray(point_cloud_o3d.points)
#n×3
print('total points number is:', points.shape[0])
# PCA Analyze the main direction of the point cloud
#w The eigenvalue , Eigenvector
# w, v = PCA(points)
# point_cloud_vector = v[:, 1] # The vector corresponding to the main direction of the point cloud
# print('the main orientation of this pointcloud is: ', point_cloud_vector)
# # Three dimensional to two dimensional
# projected_points = np.dot(points, v[:, :2])
# ##np.vstack(): Stack in the vertical direction
# # np.hstack(): Tile horizontally
# projected_points = np.hstack([projected_points,np.zeros((projected_points.shape[0],1))])
# # Draw a point cloud image
# projected_point_cloud_o3d = o3d.geometry.PointCloud()
# projected_point_cloud_o3d.points = o3d.utility.Vector3dVector(projected_points)
# projected_point_cloud_o3d.colors = o3d.utility.Vector3dVector(np.asarray(point_cloud_o3d.colors))
# o3d.visualization.draw_geometries([projected_point_cloud_o3d])
# PCA Analysis for normal vector estimation
# Because nearest neighbor search is the content of Chapter 2 , Therefore, it is allowed to directly call open3d The function in
# Circularly calculate the normal vector of each point
pcd_tree = o3d.geometry.KDTreeFlann(point_cloud_o3d)
# tree = KDTree(points)
normals = []
eigv = []
eigm = []
# One dimension maximum minus one dimension minimum
cloud_range = points.max(axis=0)-points.min(axis=0)
# dadius: set to 5% of the cloud's max range
radius = cloud_range.max() * 0.05
for point in point_cloud_o3d.points:
# Number , Indexes , Distance to this point
cnt,idxs,dists = pcd_tree.search_radius_vector_3d(point,radius)
# ds,idxs = tree.query(point,500)
# print("count",cnt)
# print("dists",len(dists))
# v:3*3 matrix
# Find the eigenvalue and direction of these points
w,v = PCA(points[idxs])
#v[:,-1]:3*1 matrix
# The eigenvector corresponding to the minimum eigenvalue
normal = v[:,0]
n = v.flatten()
normals.append(normal)
w_normal = np.linalg.norm(w)
w_n = np.divide(w, w_normal, out=np.zeros_like(w), where=w_normal!=0)
# eigv.append(np.hstack((n,w_n)))
eigv.append(np.hstack((w,n)))
i = np.argmax(w_n)
eigm.append(v[:,i])
normals = np.array(eigm, dtype=np.float64)
print("normals",normals)
# TODO: Here the normal vector is stored in normals in
point_cloud_o3d.normals = o3d.utility.Vector3dVector(normals)
point_cloud_o3d.orient_normals_consistent_tangent_plane(50)
# etc1 = np.array(point_cloud_o3d.normals)
# o3d.visualization.draw_geometries([point_cloud_o3d])
o3d.visualization.draw_geometries([point_cloud_o3d], "Open3D normal estimation", width=800, height=600, left=50, top=50,
point_show_normal=True, mesh_show_wireframe=False,
mesh_show_back_face=False)
# # Visualize the points of the normal vector , And store the normal vector points to the file
# normal_point1 = o3d.utility.Vector3dVector(point_cloud_o3d.normals)
# normals1 = o3d.geometry.PointCloud()
# normals1.points = normal_point1
# normals1.paint_uniform_color((0, 1, 0)) # The points of the point cloud normal vector are displayed in green
# o3d.visualization.draw_geometries([point_cloud_o3d, normals1], "Open3D noramls points", width=800, height=600, left=50, top=50,
# point_show_normal=False, mesh_show_wireframe=False,
# mesh_show_back_face=False)
eig = np.array(eigv)
# eig = np.array(etc1)
# crit = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 1000, 0.2)
# gr = 3
# _, labels, (centers) = cv2.kmeans(eig, gr, None, crit, 100, cv2.KMEANS_PP_CENTERS)
from sklearn.cluster import KMeans
import matplotlib.pyplot as plt
kmeans = KMeans(n_clusters=2, random_state=0).fit(eig)
labels = kmeans.labels_
centers = kmeans.cluster_centers_
# centers = np.uint8(centers)/360
# centers = matplotlib.colors.hsv_to_rgb(centers)
# flatten the labels array
labels = labels.flatten()
max_label = len(np.unique(labels))
col = plt.get_cmap("tab10")(labels / (max_label if max_label > 0 else 1))
col1 = col[:,:3]
col2 = centers[labels.flatten()]
pcd2 = o3d.geometry.PointCloud()
pcd2.colors = o3d.utility.Vector3dVector(col1)
pcd2.points = o3d.utility.Vector3dVector(np.asarray(point_cloud_o3d.points))
o3d.visualization.draw_geometries_with_editing([pcd2])
if __name__ == '__main__':
main()