点云拟合平面原理和实现（Halcon）

最近学习了一下拟合平面的原理，看了这篇文章最小二乘拟合平面(C++版) - 知乎

讲到了以下几种方法，我这里在halcon中对其一一实现。

一、算法原理

1，直接求解法

2.使用拉格朗日乘子法

3 SVD分解法  

二、Halcon实现

1.各方法对比

在halcon中其实有对应的算子直接实现拟合平面Ax+By+C=D

如fit_primitives_object_model_3d 

我这里取了一张点云图，对其中一小块点云进行平面拟合测试

取红色圈内一部分数据测试

直接使用halcon算子 fit_primitives_object_model_3d得到的结果

[0.376856, -0.602116, -0.703873, -0.553981]

使用拉格朗日乘子的那个方法得到的结果

[0.376856, -0.602116, -0.703873, -0.553981]

第一个直接求解法得到的结果

[0.376656, -0.601768, -0.704277, -0.554283]

用SVD方法求解的结果

[-0.376856, 0.602116, 0.703873, 0.553981]

结果都一样，除了那个直接求解法的，没有做去质心处理，可能有点细微的差别

2.halcon代码

read_image (XYZ, '3d_machine_vision/segmentation/3d_primitives_xyz_01.tif')
 
access_channel (XYZ, X, 1)
access_channel (XYZ, Y, 2)
access_channel (XYZ, Z, 3)
 
gen_ellipse (ROI_0, 84.5473, 95.7705, rad(-35.9506), 10.9232, 8.60448)
reduce_domain (Z, ROI_0, ImageReduced)
xyz_to_object_model_3d (X, Y, ImageReduced, ObjectModel3D)
 
 
*直接使用halcon拟合平面算子处理
fit_primitives_object_model_3d (ObjectModel3D, 'primitive_type', 'plane', ObjectModel3DOut)
get_object_model_3d_params (ObjectModel3DOut, 'primitive_parameter', Result1)
get_object_model_3d_params (ObjectModel3DOut, 'primitive_rms', GenParamValue1)
 
 
 
*获取截取的3d模型x、y、z的坐标值
get_object_model_3d_params (ObjectModel3D, 'point_coord_x', pX)
get_object_model_3d_params (ObjectModel3D, 'point_coord_y', pY)
get_object_model_3d_params (ObjectModel3D, 'point_coord_z', pZ)
 
 
*测试用拉格朗日乘子法
Num:=|pX|
 
*求均值
XM:=mean(pX)
YM:=mean(pY)
ZM:=mean(pZ)
 
*去质心
DX:=pX-XM
DY:=pY-YM
DZ:=pZ-ZM
 
*求矩阵各个位置的值
MA11 := sum(DX * DX)
MA22 := sum(DY * DY)
MA33 := sum(DZ * DZ)
MA12 := sum(DX * DY)
MA13 := sum(DX * DZ)
MA23 := sum(DY * DZ)
create_matrix (3, 3, [MA11,MA12,MA13,MA12,MA22,MA23,MA13,MA23,MA33], MatrixID)
 
*实对称矩阵，求特征值和特征向量
eigenvalues_symmetric_matrix (MatrixID, 'true', EigenvaluesID, EigenvectorsID)
 
*特征值按小到大排列，所以平面法向量是第一列特征向量
get_value_matrix (EigenvectorsID, 0, 0, NX)
get_value_matrix (EigenvectorsID, 1, 0, NY)
get_value_matrix (EigenvectorsID, 2, 0, NZ)
 
*算C  平面方程NX*X+NY*Y+NZ*Z=C
C := NX * XM + NY * YM + NZ * ZM
Result2:=[NX,NY,NZ,C]
 
 
*对应使用直接求解法
*算矩阵各个位置的值
MB11:=sum(pX*pX)
MB12:=sum(pX*pY)
MB13:=sum(pX)
MB22:=sum(pY*pY)
MB23:=sum(pY)
MB33:=|pX|
MC1:=sum(pX*pZ)
MC2:=sum(pY*pZ)
MC3:=sum(pZ)
 
create_matrix (3, 3, [MB11,MB12,MB13,MB12,MB22,MB23,MB13,MB23,MB33], MB)
create_matrix (3, 1, [MC1,MC2,MC3], MC)
solve_matrix (MB, 'general', 0, MC, MatrixResultID)
get_full_matrix (MatrixResultID, Values)
*要求a^2+b^2+c^2=1 求解真正的a、c
dd:=Values[0]*Values[0]+Values[1]*Values[1]+1
a:=Values[0]/sqrt(dd)
b:=Values[1]/sqrt(dd)
c:=-1/sqrt(dd)
*因为这里的平面方程为a*x+b*y+c*z=d 与文章中方程A*x+B*y+C*z+D=0中d相差个负号，所以
d:=-Values[2]/sqrt(dd)
Result3:=[a,b,c,d]
 
*SVD分解法  
create_matrix (3, Num, [DX,DY,DZ], A)
transpose_matrix_mod (A)
svd_matrix (A, 'full', 'both', U, S, V)
 
get_full_matrix (V, VValues)
get_value_matrix (V, 0, 2, Value1)
get_value_matrix (V, 1, 2, Value2)
get_value_matrix (V, 2, 2, Value3)
 
Value4:=Value1*XM+Value2*YM+Value3*ZM
Result4:=[Value1,Value2,Value3,Value4]

那个求各个位置的值，可以直接用矩阵相乘好了，简写成

*求特征向量法
create_matrix(3,Num,[DX,DY,DZ],B)
mult_matrix (B, B, 'ABT', MatrixMultID)
eigenvalues_symmetric_matrix (MatrixMultID, 'true', EigenvaluesID1, EigenvectorsID1)
get_full_matrix (EigenvectorsID1, Values3)
aa:=Values3[0]
bb:=Values3[3]
cc:=Values3[6]
dd:=aa*XM+bb*YM+cc*ZM
Result5:=[aa,bb,cc,dd]