pointcloud_homework4

task
Use KITTI 3D object detection dataset, do the followings.
Step 1. Remove the ground from the lidar points. Visualize ground as blue.
• Any method you want – LSQ, Hough, RANSAC
Step 2. Clustering over the remaining points. Visualize the clusters with random colors.
• Any method you want

solution

在jupyter notebook上完成并运行

1.读取KITTI的一个点云数据集并可视化

import open3d
import os
import struct
import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
def read_velodyne_bin(path):
    '''
    :param path:
    :return: homography matrix of the point cloud, N*3
    '''
    pc_list = []
    with open(path, 'rb') as f:
        content = f.read()
        pc_iter = struct.iter_unpack('ffff', content)
        for idx, point in enumerate(pc_iter):
            pc_list.append([point[0], point[1], point[2]])
    return np.asarray(pc_list, dtype=np.float32)

path='D:\\lesson2code\\000000.bin'
#应替换为自己的数据路径
origindata=read_velodyne_bin(path)

pcd = open3d.geometry.PointCloud()

pcd.points = open3d.utility.Vector3dVector(origindata)
open3d.visualization.draw_geometries([pcd])

2.将点云地面的点提取并标记为蓝色

假设地面为水平，使用LeastSquare与RANSAC进行平面拟合，并取出平面附近的点作为地面

（1）最小二乘法函数

import numpy as np
import random

def PlaneLeastSquare(X:np.ndarray):
    #z=ax+by+c,return a,b,c
    A=X.copy()
    b=np.expand_dims(X[:,2],axis=1)
    A[:,2]=1
    
    #通过X=(AT*A)-1*AT*b直接求解

    A_T = A.T
    A1 = np.dot(A_T,A)
    A2 = np.linalg.inv(A1)
    A3 = np.dot(A2,A_T)
    x= np.dot(A3, b)
    return x

（2）RANSAC函数实现

import numpy as np
import random
import math
import sys

def PlaneRANSAC(X:np.ndarray,tao:float,e=0.4,N_regular=100):
    #return plane ids
    s=X.shape[0]
    
    count=0
    p=0.99
    dic={}
    
    #确定迭代次数
    if math.log(1-(1-e)**s)<sys.float_info.min:
        N=N_regular
    else:
        N=math.log(1-p)/math.log(1-(1-e)**s)
        
    #开始迭代
    while count < N:
        
        
        ids=random.sample(range(0,s),3)
        Points=X[ids]
        p1,p2,p3=X[ids]
        #判断是否共线
        L=p1-p2
        R=p2-p3
        if 0 in L or 0 in R:
            continue
        else:
            if L[0]/R[0]==L[1]/R[1]==L[2]/R[2]:
                continue
                
        #计算平面参数
        a = (p2[1] - p1[1])*(p3[2] - p1[2]) - (p2[2] - p1[2])*(p3[1] - p1[1]);     
        b = (p2[2] - p1[2])*(p3[0] - p1[0]) - (p2[0] - p1[0])*(p3[2] - p1[2]);     
        c = (p2[0] - p1[0])*(p3[1] - p1[1]) - (p2[1] - p1[1])*(p3[0] - p1[0]);     
        d = 0 - (a * p1[0] + b*p1[1] + c*p1[2]);
        
        dis=abs(a*X[:,0]+b*X[:,1]+c*X[:,2]+d)/(a**2+b**2+c**2)**0.5
        
        idset=[]
        for i ,d in enumerate(dis):
            if d <tao:
                idset.append(i)
        
        
        #再使用最小二乘法
        p=PlaneLeastSquare(X[idset])
        a,b,c,d=p[0],p[1],-1,p[2]
        
        
        dic[len(idset)]=[a,b,c,d]
        
        if len(idset)>s*(1-e):
            break
        
        count+=1
    
    parm=dic[max(dic.keys())]
    a,b,c,d=parm
    dis=abs(a*X[:,0]+b*X[:,1]+c*X[:,2]+d)/(a**2+b**2+c**2)**0.5
        
    idset=[]
    for i ,d in enumerate(dis):
        if d <tao:
            idset.append(i)
    return np.array(idset)

（3）调用并可视化

planeids=PlaneRANSAC(origindata,0.35)
planedata=origindata[planeids]
planepcd = open3d.geometry.PointCloud()
planepcd.points = open3d.utility.Vector3dVector(planedata)

c=[0,0,255]
cs=np.tile(c,(planedata.shape[0],1))
planepcd.colors=open3d.Vector3dVector(cs)

othersids=[]
for i in range(origindata.shape[0]):
    if i not in planeids:
        othersids.append(i)
otherdata=origindata[othersids]
otherpcd = open3d.geometry.PointCloud()
otherpcd.points = open3d.utility.Vector3dVector(otherdata)
c=[255,0,0]
cs=np.tile(c,(otherdata.shape[0],1))
otherpcd.colors=open3d.Vector3dVector(cs)
open3d.visualization.draw_geometries([planepcd,otherpcd])

3.对地面外的点云进行聚类

由于较难确定类别的数量，kmeans与GMM方法不太适用。现选用DBSCAN方法。

（1）自己的DBSCAN实现

代码的少部分片断借鉴于博客https://www.cnblogs.com/tiaozistudy/p/dbscan_algorithm.html.

import numpy as np
import random
from scipy.spatial import KDTree
import time
import sklearn.cluster #import DBSCAN

class visitlist:
    def __init__(self, count=0):
        self.unvisitedlist=[i for i in range(count)]
        self.visitedlist=list()
        self.unvisitednum=count

    def visit(self, pointId):
        self.visitedlist.append(pointId)
        self.unvisitedlist.remove(pointId)
        self.unvisitednum -= 1
        

        
class cluster:
    def __init__(self,ctype):
        self.ctypr=ctype
        self.points=list()
        
        
    
        
def DBSCAN(X:np.ndarray,r:float,minPts:int):
    pointnum=X.shape[0]
    v=visitlist(pointnum)
    clustersSet=list()
    noise=cluster(-1)
    tree=KDTree(X)
    k=0
    
    while v.unvisitednum>0:
        randid=random.choice(v.unvisitedlist)
        v.visit(randid)
        N = tree.query_ball_point(X[randid], r)
        if len(N)<minPts:            
            noise.points.append(randid)
        else:
            clus=cluster(k)
            clus.points.append(randid)
            N.remove(randid)
            while len(N)>0:
                p=N.pop()
                if p in v.unvisitedlist:
                    v.visit(p)
                    clus.points.append(p)
                    pN=tree.query_ball_point(X[p], r)
                    if len(pN)>=minPts:
                        pN.remove(p)
                        N=N+pN
            clustersSet.append(clus)
    
    clustersSet.append(noise)
    return clustersSet

测试自己的DBSCAN效果

import time
from sklearn import datasets
import matplotlib.pyplot as plt
centers = [[1, 1], [-1, -1], [-1, 1], [1, -1]]
pointlala, labelsTrue = datasets.make_blobs(n_samples=600, centers=centers, cluster_std=0.4,random_state=0)
C=DBSCAN(pointlala,0.3,10)


plt.scatter(pointlala[:,0],pointlala[:,1])
trys1=pointlala[C[0].points]
plt.scatter(trys1[:,0],trys1[:,1])
trys2=pointlala[C[1].points]
plt.scatter(trys2[:,0],trys2[:,1])
trys3=pointlala[C[2].points]
plt.scatter(trys3[:,0],trys3[:,1])
trys4=pointlala[C[3].points]
plt.scatter(trys4[:,0],trys4[:,1])

实践发现，自己实现的DBSCAN方法处理上述的点云数据耗时较久，预计完成聚类需要几个小时这样。最终还是放弃使用自己的DBSCAN方法对点云聚类，改用sklearn中的DBSCAN聚类方法。

（2）使用sklearn内的DBSCAN方法进行聚类

内心os:
原本打算每一个类用一种颜色，总共大概有七八十个类这样，但是发现运行时open3d可视化窗口一片空白（可能是电脑配置的缘故）。后来选择每个类依次使用6种颜色中的一种。相同的类会是同一颜色，距离得较远的但颜色相同应是属于不同类了。
PS：噪声点标记为黑色

import sklearn.cluster

Css=sklearn.cluster.DBSCAN(eps=0.50, min_samples=4).fit(otherdata)
ypred=np.array(Css.labels_)
ddraw=[]



colorset=[[222,0,0],[0,224,0],[0,255,255],[222,244,0],[255,0,255],[128,0,0]]
for cluuus in set(ypred):
    
    kaka=np.where(ypred==cluuus)
    ppk=open3d.geometry.PointCloud()
    ppk.points = open3d.utility.Vector3dVector(otherdata[kaka])

    c=colorset[cluuus%6]
    if cluuus==-1:
        c=[0,0,0]

    cs=np.tile(c,(otherdata[kaka].shape[0],1))
    ppk.colors=open3d.Vector3dVector(cs)
    ddraw.append(ppk)



ddraw.append(planepcd)
open3d.visualization.draw_geometries(ddraw)

离雷达越远处的噪声越多，且聚类得较乱。但离雷达近处的聚类效果还可以，如图一小车与旁边的类似电线杆、供电箱被分成不同类。

可以调节DBSCAN的超参数以获得更好的聚类效果。

附其他点云数据的处理效果