平面法向量，点云法向量估计及可视化

爱被打了一巴掌 2022-09-05 00:12 193阅读 0赞

**目录**

单通道深度图像，转三通道法向图像

opencv 平面法向量\_任意两平面求夹角

点云3种法向量估计方法及可视化

--------------------

### **单通道深度图像，转三通道法向图像** ###

### ![20191221235911135.png][]![20191221235925217.png][] ###

import cv2
    import numpy as np
    import mxnet as mx
    def depth2normal(depth):
        w,h=depth.shape
        dx=-(depth[2:h,1:h-1]-depth[0:h-2,1:h-1])*0.5
        dy=-(depth[1:h-1,2:h]-depth[1:h-1,0:h-2])*0.5
        dz=mx.nd.ones((w-2,h-2))
        dl = mx.nd.sqrt(mx.nd.elemwise_mul(dx, dx) + mx.nd.elemwise_mul(dy, dy) + mx.nd.elemwise_mul(dz, dz))
        dx = mx.nd.elemwise_div(dx, dl) * 0.5 + 0.5
        dy = mx.nd.elemwise_div(dy, dl) * 0.5 + 0.5
        dz = mx.nd.elemwise_div(dz, dl) * 0.5 + 0.5
        return np.concatenate([dy.asnumpy()[np.newaxis,:,:],dx.asnumpy()[np.newaxis,:,:],dz.asnumpy()[np.newaxis,:,:]],axis=0)
    if __name__ == '__main__':
        depth=cv2.imread("depth.png",0)
        normal=np.array(depth2normal(mx.nd.array(depth))*255)
        normal = cv2.cvtColor(np.transpose(normal, [1, 2, 0]), cv2.COLOR_BGR2RGB)         
        cv2.imwrite("normal.png",normal.astype(np.uint8))

原文链接：https://blog.csdn.net/muyouhang/article/details/103649888

### opencv 平面法向量\_任意两平面求夹角 ###

import math#引入math模块 计算角度用
    class point(object):#定义空间点类
    	"""docstring for point"""
    	def __init__(self,x,y,z,name):
    		self.x = x
    		self.y = y
    		self.z = z
    		self.name = name
    class plane(object):#定义平面类
    	"""docstring for plane"""
    	def __init__(self, A,B,C,name):
    		self.points=[A,B,C]#一个平面三个点
    		self.points_name=[A.name,B.name,C.name]#点的名字
    		self.name = name#平面的名字
    		self.n=[]#平面的法向量
    	def isplane(self):#判断这三个点是否能构成平面
    		coors=[[],[],[]]#三个点的xyz坐标分别放在同一个列表用来比较
    		for _point in self.points:#对于每个点
    			coors[0].append(_point.x)
    			coors[1].append(_point.y)
    			coors[2].append(_point.z)
    		for coor in coors:
    			if coor[0]==coor[1]==coor[2]:#如果三个点的x或y或z坐标相等 则不能构成平面
    				return print('Points:',*self.points_name,'cannot form a plane')
    	def normal(self):#获得该平面的法向量
    		self.isplane()#获得该平面的法向量前提是能构成平面
    		A,B,C=self.points#对应三个点
    		AB=[B.x-A.x,B.y-A.y,B.z-A.z]#向量AB
    		AC=[C.x-A.x,C.y-A.y,C.z-A.z]#向量AC
    		B1,B2,B3=AB#向量AB的xyz坐标
    		C1,C2,C3=AC#向量AC的xyz坐标
    		self.n=[B2*C3-C2*B3,B3*C1-C3*B1,B1*C2-C1*B2]#已知该平面的两个向量,求该平面的法向量的叉乘公式
    	def angle(self,P2):#两个平面的夹角
    		x1,y1,z1=self.n#该平面的法向量的xyz坐标
    		x2,y2,z2=P2.n#另一个平面的法向量的xyz坐标
    		cosθ=((x1*x2)+(y1*y2)+(z1*z2))/(((x1**2+y1**2+z1**2)**0.5)*((x2**2+y2**2+z2**2)**0.5))#平面向量的二面角公式
    		degree=math.degrees(math.acos(cosθ))
    		if degree>90:#二面角∈[0°,180°] 但两个平面的夹角∈[0°,90°]
    			degree=180-degree
    		return print('平面',self.name,P2.name,'的夹角为'+str(round(degree,5))+'°')
    		#round(数值,四舍五入位数) math.degrees(弧度)将弧度转换为角度 math.acos(数值)返回该数值的反余弦弧度值
    #测试
    print('-'*25)
    A=point(0,0,1,'A')#六个点
    B=point(1,0,1,'B')
    C=point(1,1,0,'C')
    P1=plane(A,B,C,'P1')#p1平面
    D=point(0,1,1,'D')
    E=point(1,1,1,'E')
    F=point(0.5,0,0,'F')
    P2=plane(D,E,F,'P2')#p2平面
    P1.normal()#求平面p1 p2的法向量
    P2.normal()
    P1.angle(P2)#求平面p1 p2的夹角
    print('-'*25)
    G=point(2,0,0,'G')#六个点
    H=point(0,0,0,'H')
    I=point(0,3,3**0.5,'I')
    P3=plane(G,H,I,'P3')#p3平面
    J=point(2/3,4/3,0,'J')
    K=point(0,0,0,'K')
    L=point(0,3,3**0.5,'L')
    P4=plane(J,K,L,'P4')#p4平面
    P3.normal()#分别求平面p3 p4的法向量
    P4.normal()
    P3.angle(P4)#求平面p3 p4的夹角
    print('-'*25)
    M=point(0,1,0,'M')#六个点
    N=point(0,0,0,'N')
    O=point(1,1,1,'O')
    P5=plane(M,N,O,'P5')#p1平面
    Q=point(0,0,2,'Q')
    R=point(0,0,0,'R')
    S=point(1,1,1,'S')
    P6=plane(Q,R,S,'P6')#p2平面
    P5.normal()#求平面p1 p2的法向量
    P6.normal()
    P5.angle(P6)#求平面p1 p2的夹角
    print('-'*25)
    T=point(12.6,-1,63,'T')#六个点
    U=point(0,7,8,'U')
    V=point(11,9,83.2,'V')
    P7=plane(T,U,V,'P7')#p1平面
    W=point(45,2,13,'W')
    X=point(9,10,-56,'X')
    Y=point(0.5,-7,1,'Y')
    P8=plane(W,X,Y,'P8')#p2平面
    P7.normal()#求平面p1 p2的法向量
    P8.normal()
    P7.angle(P8)#求平面p1 p2的夹角

原文链接：https://blog.csdn.net/weixin\_36313588/article/details/112189888

### 点云3种法向量估计方法及可视化 ###

*  [1）点云读取可视化][1]
 *  [2）下采样可视化][2]
 *  [3）法向量三种估计方式（K近邻估计，半径近邻估计，混合搜索估计）][3_K]
 *  [4）点云每个点对应的法向量点存储及可视化][4]
 *  [5）法向量点和原始点云同时可视化][5]
 *  [6） 源码][6_]

# 1）点云读取可视化 #

**原始点云：**

![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQwOTg1OTg1_size_16_color_FFFFFF_t_70_pic_center]

# 2）下采样可视化 #

**下采样：**

![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQwOTg1OTg1_size_16_color_FFFFFF_t_70_pic_center 1]

# 3）法向量三种估计方式（K近邻估计，半径近邻估计，混合搜索估计） #

**K近邻估计法向量并可视化：**

![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQwOTg1OTg1_size_16_color_FFFFFF_t_70_pic_center 2]  
**混合搜索近邻估计可视化**  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQwOTg1OTg1_size_16_color_FFFFFF_t_70_pic_center 3]

# 4）点云每个点对应的法向量点存储及可视化 #

**法向量对应的点可视化：**

![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQwOTg1OTg1_size_16_color_FFFFFF_t_70_pic_center 4]

# 5）法向量点和原始点云同时可视化 #

**原始点云灰色，法向量点绿色**

![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQwOTg1OTg1_size_16_color_FFFFFF_t_70_pic_center 5]

# 6） 源码 #

# @Description: <Open3D估计法向量，可视化，存储为文件>
    
    import open3d as o3d
    import os
    
    path = os.path.abspath(os.path.join(os.getcwd(), "../"))
    path = path + "/pcds/bunny.pcd"
    normalPath = path.replace(".pcd", "_normal.pcd")
    print(path)
    print(normalPath)
    
    print("Load a pcd point cloud, print it, and render it")
    pcd = o3d.io.read_point_cloud(path)
    pcd.paint_uniform_color([0.5, 0.5, 0.5])  # 把所有点渲染为灰色（灰兔子）
    print(pcd)  # 输出点云点的个数
    # print(o3d.np.asarray(pcd.points))  # 输出点的三维坐标
    o3d.visualization.draw_geometries([pcd], "Open3D origin points", width=800, height=600, left=50, top=50,
                                      point_show_normal=False, mesh_show_wireframe=False,
                                      mesh_show_back_face=False)
    
    print("Downsample the point cloud with a voxel of 0.002")
    downpcd = pcd.voxel_down_sample(voxel_size=0.002)  # 下采样滤波，体素边长为0.002m
    print(downpcd)
    o3d.visualization.draw_geometries([downpcd], "Open3D downsample points", width=800, height=600, left=50, top=50,
                                      point_show_normal=False, mesh_show_wireframe=False,
                                      mesh_show_back_face=False)
    
    print("Recompute the normal of the downsampled point cloud")
    # 混合搜索  KNN搜索  半径搜索
    # downpcd.estimate_normals(
    #     search_param=o3d.geometry.KDTreeSearchParamHybrid(radius=0.01, max_nn=20))  # 计算法线，搜索半径1cm，只考虑邻域内的20个点
    downpcd.estimate_normals(
        search_param=o3d.geometry.KDTreeSearchParamKNN(knn=20))  # 计算法线，只考虑邻域内的20个点
    # downpcd.estimate_normals(
    #     search_param=o3d.geometry.KDTreeSearchParamRadius(radius=0.01))  # 计算法线，搜索半径1cm，只考虑邻域内的20个点
    
    o3d.visualization.draw_geometries([downpcd], "Open3D normal estimation", width=800, height=600, left=50, top=50,
                                      point_show_normal=True, mesh_show_wireframe=False,
                                      mesh_show_back_face=False)  # 可视化法线
    print("Print a normal vector of the 0th point")
    print(downpcd.normals[0])  # 输出0点的法向量值
    
    print("Print the normal vectors of the first 10 points")
    print(o3d.np.asarray(downpcd.normals)[:10, :])  # 输出前10个点的法向量
    
    # std::vector<Eigen::Vector3d> with 381 elements. 转换为nparry 以打印访问
    # normals = o3d.np.asarray(downpcd.normals)
    # print(normals)
    
    # 可视化法向量的点，并存储法向量点到文件
    normal_point = o3d.utility.Vector3dVector(downpcd.normals)
    normals = o3d.geometry.PointCloud()
    normals.points = normal_point
    normals.paint_uniform_color((0, 1, 0))  # 点云法向量的点都以绿色显示
    o3d.visualization.draw_geometries([pcd, normals], "Open3D noramls points", width=800, height=600, left=50, top=50,
                                      point_show_normal=False, mesh_show_wireframe=False,
                                      mesh_show_back_face=False)
    o3d.io.write_point_cloud(normalPath, normals)

[20191221235911135.png]: https://img-blog.csdnimg.cn/20191221235911135.png
[20191221235925217.png]: /images/20220829/85c4016d01ea44c88d43236c58890464.png
[1]: https://www.pythonheidong.com/blog/article/545139/67bc081dd312970e9bd3/#1_1
[2]: https://www.pythonheidong.com/blog/article/545139/67bc081dd312970e9bd3/#2_6
[3_K]: https://www.pythonheidong.com/blog/article/545139/67bc081dd312970e9bd3/#3K_10
[4]: https://www.pythonheidong.com/blog/article/545139/67bc081dd312970e9bd3/#4_16
[5]: https://www.pythonheidong.com/blog/article/545139/67bc081dd312970e9bd3/#5_20
[6_]: https://www.pythonheidong.com/blog/article/545139/67bc081dd312970e9bd3/#6__24
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQwOTg1OTg1_size_16_color_FFFFFF_t_70_pic_center]: /images/20220829/a924e0fbe9fe4567a8c7437fbe151865.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQwOTg1OTg1_size_16_color_FFFFFF_t_70_pic_center 1]: /images/20220829/0ee97da7f617405d93ef969b3f439858.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQwOTg1OTg1_size_16_color_FFFFFF_t_70_pic_center 2]: /images/20220829/5b14e5f16cf648988ff2673a87513bf4.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQwOTg1OTg1_size_16_color_FFFFFF_t_70_pic_center 3]: /images/20220829/3ad4c34ce55445edb89084b2d3def47f.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQwOTg1OTg1_size_16_color_FFFFFF_t_70_pic_center 4]: /images/20220829/e9ab782e757f478ea874b74b72f12325.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQwOTg1OTg1_size_16_color_FFFFFF_t_70_pic_center 5]: /images/20220829/5bd80a2917574df285316c5a692def5c.png