4：点云处理—去噪、剪切、调平

1.点云去噪

    dev_clear_window ()dev_open_window(0, 0, 560, 560, 'black', WindowHandle)GenParamNames := ['lut','intensity','light_position','disp_pose','alpha']GenParamValues := ['color1','coord_z','0.0 0.0 -0.3 1.0','true',1]DispPose := [0,-0.0005,717.04,280,0,20,0]Instructions[0] := 'Rotate: Left button'Instructions[1] := 'Zoom:   Shift + left button'Instructions[2] := 'Move:   Ctrl  + left button'Index := 1*1.读取点云*'./scene/'+Index+'.om3'read_object_model_3d('./scene/'+Index+'.om3','m', [], [], ObjectModel3DScene1, Status) *方法一：去噪*分裂开 方便 根本点的个数进行筛选， 筛选完成之后， 再联合起来， 还是一个整体*使得 点云距离 超过 1 的单位点云， 分割独立起来connection_object_model_3d( ObjectModel3DScene1, 'distance_3d', 1, ObjectConnected)*获取点云个数的参数get_object_model_3d_params(ObjectConnected, 'num_points', GenParamValue)*求取点云个数的最大值tuple_max(GenParamValue, Max)*根据点云的个数完成筛选select_object_model_3d(ObjectConnected, 'num_points', 'and', Max-10000,Max+1, \ObjectSelected)*再联合  如果选取点云个数大于1 union_object_model_3d(ObjectSelected, 'points_surface',ObjectModel3DScene)visualize_object_model_3d (WindowHandle,ObjectModel3DScene1,[], [],  \GenParamNames, GenParamValues, [], [],\Instructions,  DispPose)*相机内存配合位姿使用
*     gen_cam_par_area_scan_division (0.01,0,7e-6,7e-6,512/2, 512/2,512, 512, CamParam)CamParam := [0.01,0,7e-6,7e-6,512/2, 512/2,512, 512]DispPose:=[249.253, 57.6831, 238.687, 12.2041, 332.36, 111.077, 0]*方法二：去噪*按照x方向的筛选  多次使用这个函数， 达到 一个合理的点云，被提取出来。select_points_object_model_3d(ObjectModel3DScene, 'point_coord_x', -1000, -1, ObjectX1)select_points_object_model_3d(ObjectModel3DScene, 'point_coord_x', 1, 1000, ObjectX2)select_points_object_model_3d(ObjectModel3DScene, ['point_coord_x','point_coord_y'],\[-1,-1000], [1,-1], ObjectX3)select_points_object_model_3d(ObjectModel3DScene, ['point_coord_x','point_coord_y'],\[-1,1], [1,1000], ObjectX4)*合并点云union_object_model_3d([ObjectX1,ObjectX2,ObjectX3,ObjectX4], 'points_surface', Union3DX)visualize_object_model_3d (WindowHandle,Union3DX, CamParam,DispPose,  \GenParamNames, GenParamValues, [], [],\Instructions,  DispPose)stop()*保存点云
*     write_object_model_3d(Union3DX, 'om3', './scene/'+10+'.om3', [], [])*保存位姿
*     write_pose(DispPose, './data/Param/DispPose.dat')

2.点云剪切

dev_clear_window ()
dev_open_window(0, 0, 560, 560, 'black', WindowHandle)try*基本参数设置
*   GenKindPreParameterS (GenParamNames, GenParamValues, DispPose, Instructions, CamParam)GenParamNames := ['lut','intensity','light_position','disp_pose','alpha']GenParamValues := ['color1','coord_z','0.0 0.0 -0.3 1.0','true',1]DispPose := [0,-0.0005,717.04,280,0,20,0]Instructions[0] := 'Rotate: Left button'Instructions[1] := 'Zoom:   Shift + left button'Instructions[2] := 'Move:   Ctrl  + left button'CamParam := [0.01,0,7e-6,7e-6,512/2, 512/2,512, 512]*判断文件是否存在
*   GenSelfAnsysFileExist (FileExists, DispPose, FileExists2)file_exists('./data/Param/DispPose1.dat', FileExists)if(FileExists==0)*异常处理 ---弹框提示  return  endifread_pose('./data/Param/DispPose.dat',DispPose )file_exists('./data/Param', FileExists2)if(FileExists2==0)make_dir('./data/Param')endif*pose设置为固定的数值Index := 1read_object_model_3d('./scene/'+Index+'.om3','m', [], [], ObjectModel3DScene1, Status) *去噪
*   GenSelfPointCloud (ObjectModel3DScene1, ObjectModel3DScene)connection_object_model_3d( ObjectModel3DScene1, 'distance_3d', 1, ObjectConnected)*获取点云个数的参数get_object_model_3d_params(ObjectConnected, 'num_points', GenParamValue)*求取点云个数的最大值tuple_max(GenParamValue, Max)*根据点云的个数完成筛选select_object_model_3d(ObjectConnected, 'num_points', 'and', Max-1,Max+1,  ObjectSelected)*再联合  如果选取点云个数大于1 union_object_model_3d(ObjectSelected, 'points_surface',ObjectModel3DScene)disp_object_model_3d(WindowHandle,ObjectModel3DScene, CamParam,DispPose,GenParamNames, GenParamValues)*提示  --绘制矩形  draw_rectangle1(WindowHandle, Row1, Column1, Row2, Column2)rec1:=[Row1, Column1, Row2, Column2]write_tuple(rec1, './data/Param/rec1.tup')gen_rectangle1(Rectangle, rec1[0], rec1[1],rec1[2],rec1[3])*剪切点云reduce_object_model_3d_by_view(Rectangle, ObjectModel3DScene,CamParam, DispPose,ObjectModel3DReduced)visualize_object_model_3d (WindowHandle,ObjectModel3DReduced, CamParam,[],\GenParamNames, GenParamValues, [], [],\Instructions, PoseOut)*加写日志catch (Exception)*异常原因stop()
endtry

3.点云调平（三种方法）

方法一：最小外接箱体

*1. 打开窗口
*2. 读图 -显示
dev_update_off ()
dev_close_window ()
Height:=560
dev_open_window(0, 0,Height, Height, 'black', WindowHandle)
Index := 2
FileName:='Object0.om3'
pra_name:=['color_0','alpha_0','disp_pose']
pra_value:=['gray',0.5,'true'] *伪彩图   套色
GenParamNames := ['lut','intensity','light_position','disp_pose','alpha']
GenParamValues := ['color1','coord_z','0.0 0.0 -0.3 1.0','true',0.9]
DispPose2 := [0,-0.0005,717.04,280,0,20,0]
read_object_model_3d( FileName,'m', [], [], ObjectModel3DScene, Status) *预处理（去噪）  
select_points_object_model_3d(ObjectModel3DScene,'point_coord_z', 250,330, ObjectModel3DZ)
select_points_object_model_3d(ObjectModel3DZ,'point_coord_y',-140,140, ObjectModel3DY)
select_points_object_model_3d(ObjectModel3DY,'point_coord_x',-130, 160, ObjectModel3DX)connection_object_model_3d(ObjectModel3DX, 'distance_3d', 1, ObjectModel3DConnected)
select_object_model_3d(ObjectModel3DConnected, 'num_points', 'and',  1000, 100000000, ObjectX)*合并点云
union_object_model_3d(ObjectX, 'points_surface', Union3D)visualize_object_model_3d (WindowHandle, Union3D, [],[], GenParamNames, GenParamValues, [], [], [], PoseOut)*方法1——箱体
*principal_axes 沿着轴调正  根据密度最大的方向作为标准轴方向 去调正一个3维物体
smallest_bounding_box_object_model_3d(Union3D, 'oriented',Pose, Length1, Length2, Length3)
*生成外接箱体
gen_box_object_model_3d(Pose, Length1, Length2, Length3-12, ObjectModel3D1)pra_name:=['color_0','color_1','alpha_0','disp_pose']
pra_value:=['green','gray',1,'true'] *位姿逆变换
pose_invert (Pose, PoseInvert)
*仿射变换
rigid_trans_object_model_3d (Union3D, PoseInvert, Union3D11)
rigid_trans_object_model_3d (ObjectModel3D1, PoseInvert, ObjectModel3D111)visualize_object_model_3d(WindowHandle, [Union3D11,ObjectModel3D111],[], []\,pra_name, pra_value, [], [], [], PoseOut1)******************************************************************************************

方法二：PCA调平和SVD（光平面）调平

*1. 打开窗口
*2. 读图 -显示
dev_update_off ()
dev_close_window ()
Height:=560
dev_open_window(0, 0,Height, Height, 'black', WindowHandle)
Index := 2
FileName:='./scene/Object'+Index+'.om3'
pra_name:=['color_0','alpha_0','disp_pose']
pra_value:=['gray',0.5,'true'] *伪彩图
GenParamNames := ['lut','intensity','light_position','disp_pose','alpha']
GenParamValues := ['color1','coord_z','0.0 0.0 -0.3 1.0','true',0.9]
DispPose2 := [0,-0.0005,717.04,280,0,20,0]
read_object_model_3d( FileName,'m', [], [], ObjectModel3DScene, Status) *预处理   直接3维点云的z 筛选 获取目标区域 
select_points_object_model_3d(ObjectModel3DScene,'point_coord_z', -1050, -850, ObjectModel3DZ)
select_points_object_model_3d(ObjectModel3DZ,'point_coord_y',-120,120, ObjectModel3DY)
select_points_object_model_3d(ObjectModel3DY,'point_coord_x',-80, 80, ObjectModel3DX)
visualize_object_model_3d (WindowHandle, ObjectModel3DX, [],[], GenParamNames, GenParamValues, [], [], [], PoseOut)*方法1：PCA调平
*principal_axes 沿着轴调正  根据密度最大的方向作为标准轴方向 去调正一个3维物体
* gen_self_roiObject3d1 (ObjectModel3DScene, ObjectModel3DZ)
get_object_model_3d_params (ObjectModel3DScene, 'point_coord_z', z)* 从bottom开始往上的数据元素放入zlarge
bottom := -1095
tuple_greater_elem (z, bottom, zlarge)
tuple_find (zlarge, 1, IndicesSelect)
tuple_select (z, IndicesSelect, zSelected)
tuple_min (zSelected, Min)
tuple_max (zSelected, Max)
tuple_mean (zSelected, Mean)
select_points_object_model_3d (ObjectModel3DScene, 'point_coord_z', Mean-100, Mean+100, ObjectModel3DZ)* gen_self_moments_adjusting (ObjectModel3DZ, ObjectModel3DScene, WindowHandle, DispPose2, \pra_name, pra_value, ObjectModel3DPCA, ObjectModel3DZ1)
*利用PCA算法找到质心点
moments_object_model_3d (ObjectModel3DZ, 'principal_axes', momentPose)
*保存点
write_pose(momentPose, './calib/moments_pose.dat')
*读取
read_pose('./calib/moments_pose.dat', Pose)*质心点逆变换
pose_invert (Pose, PoseInvert)
*仿射变换
rigid_trans_object_model_3d (ObjectModel3DScene, PoseInvert, ObjectModel3DPCA)
*去噪
select_points_object_model_3d(ObjectModel3DPCA,'point_coord_z',-2, 2, ObjectModel3DZ1)visualize_object_model_3d (WindowHandle, ObjectModel3DZ1, [], DispPose2, pra_name, pra_value, [], [], [], PoseOut)
stop()
***************************************************************************************************************************************方法2  标准方法——获取光平面的姿态*Part1获取原点云数据坐标
get_object_model_3d_params (ObjectModel3DX, 'point_coord_x', point_coord_x)
get_object_model_3d_params (ObjectModel3DX, 'point_coord_y', point_coord_y)
get_object_model_3d_params (ObjectModel3DX, 'point_coord_z', point_coord_z)*求pose   ---光平面姿态求取函数********************************************************************
gen_LightPlanePose (point_coord_x, point_coord_y, point_coord_z,  WindowHandle, LightPlanePose)
*******************************************************************************************************与机器人坐标系  保持 平行 ，为了方便观看 。手眼标定
*x_t:=0
*y_t:=0
*z_t:=0
*Rx_t:=0
*Ry_t:=0
*Rz_t:=90
*TransPose:=[x_t,y_t,z_t,Rx_t,Ry_t,Rz_t,0]
*变换
rigid_trans_object_model_3d (ObjectModel3DX,  LightPlanePose, ObjectModel3DID)
*rigid_trans_object_model_3d (ObjectModel3DZ,LightPlanePose, ObjectModel3DID)
select_points_object_model_3d( ObjectModel3DID,'point_coord_z',-0.5, 0.5, ObjectModel3DZ1)
visualize_object_model_3d (WindowHandle,ObjectModel3DZ1, [],[],GenParamNames,GenParamValues, \'Oriented bounding boxes', '', [], Pose)
stop()* pose_compose (TransPose, PoseInvert ,camera_Pose)
* visualize_object_model_3d (WindowHandle, ObjectModel3DZ1, [],[], GenParamNames, GenParamValues, [], [], [], PoseOut)*显示结果
*rigid_trans_object_model_3d (ObjectModel3DScene, camera_Pose, ObjectModel3DID1)
* write_pose(LightPlanePose, './calib/Camera_Pose.tup')
*visualize_object_model_3d (WindowHandle,ObjectModel3DID1, [],[],pra_name,pra_value, 'Oriented bounding boxes', '', [], Pose)

光平面调平附带函数

gen_LightPlanePose (point_coord_x, point_coord_y, point_coord_z,  WindowHandle, LightPlanePose)* Fit the light plane in the 3D coordinates of the line
* points computed previously. Note that this requires
* nearly coplanar points. We must provide line points
* recorded at -at least- two different heights, in order
* to get an unambiguous solution. To obtain stable and
* accurate results, acquire the light line points at the
* bottom and at the top of the measurement volume.
* 在先前计算的线点的三维坐标中拟合光平面。 注意这几乎是共面的。
* 为了得到一个明确的解决方案，我们必须提供至少两个不同高度的线点记录。
* 为了获得稳定、准确的测量结果，需要获得测量体积顶部和底部的光线点。
* 
* 把一个平面的点 所有全部丢进去， 返回的就是 质心坐标和法向量，拟合的质量（其实均值）。
* MeanResidual  作为平面度 测量。0.0954
fit_3d_plane_xyz1 (X, Y, Z, Ox, Oy, Oz, Nx, Ny, Nz, MeanResidual)
if (|Nx| == 0 or |Ny| == 0 or |Nz| == 0)disp_message (WindowHandle, 'Too few 3d points have been provided to fit the light plane,\nor the points are (nearly) collinear!\nThe program will exit.', 'window', 12, 12, 'black', 'true')return ()
endif
* if (MeanResidual > 8e-5)
*     disp_message (WindowHandle, 'The light plane could not be fitted accurately!\nThe mean residual distance between the 3d-points and the\nfitted plane is too high (' + (MeanResidual * 1000)$'.3' + 'mm). Please check the\nquality and the correctness of those points.\nThe program will exit!', 'window', 12, 21, 'black', 'true')
*     return ()* endif
* 
* Compute the light plane pose: this pose must be oriented
* such that the plane defined by z=0 coincides with the
* light plane.
* 计算 线的点 来拟合光平面。
get_light_plane_pose (Ox, Oy, Oz, Nx, Ny, Nz, LightPlanePose)
if (|LightPlanePose| != 7)disp_message (WindowHandle, 'The pose of the light plane could not be\ndetermined. Please verify that the vector\npassed at input of the procedure\nget_light_plane_pose() is not null.\nThe program will exit!', 'window', -1, -2, 'black', 'true')return ()
endif
String := ['LightPlanePose: ','  Tx    = ' + LightPlanePose[0]$'.3' + ' m','  Ty    = ' + LightPlanePose[1]$'.3' + ' m','  Tz    = ' + LightPlanePose[2]$'.3' + ' m','  alpha = ' + LightPlanePose[3]$'.4' + '°','  beta  = ' + LightPlanePose[4]$'.4' + '°','  gamma = ' + LightPlanePose[5]$'.4' + '°','  type  = ' + LightPlanePose[6]]
disp_message (WindowHandle, String, 'window', 12, 12, 'black', 'true')
disp_continue_message (WindowHandle, 'black', 'true')
return ()

fit_3d_plane_xyz1 (X, Y, Z, Ox, Oy, Oz, Nx, Ny, Nz, MeanResidual)* This procedure fits a 3D-plane into a set of 3D-points
* 
* The procedure returns the coordinates [Ox, Oy,Oz] of
* the centroid of the provided input coordinates, and
* the coordinates [Nx, Ny,Nz] of the normal vector to
* the fitted plane.
* 
* WARNING: If the system of equations is under-determined
* (i.e. if it has too few input coordinates in X, Y, Z),
* it cannot be solved and the procedure returns empty tuples
* for X, Y, and Z
* 
* 此程序将一个3d 平面拟合成一组3d 点，该程序返回所提供的输入坐标的质心坐标[ ox，oy，oz ]
* 以及拟合平面的法向量坐标[ nx，ny，nz ]。
* 警告: 如果方程组确定不足(即 x，y，z 中的输入坐标太少) ，则无法求解，程序返回 x，y 和 z 的空元组
* Perform some initializations
Ox := []
Oy := []
Oz := []
Nx := []
Ny := []
Nz := []
MeanResidual := []
* 
* Test the size of X, Y and Z, and return if necessary
Size := |X|
if (Size < 3 or Size != |Y| or Size != |Z|)return ()
endif
* 
* Compute the coordinates of the centroid point.
* 计算质心点的坐标.求均值
tuple_mean (X, Ox)
tuple_mean (Y, Oy)
tuple_mean (Z, Oz)
* 
* Setup the equation system as a matrix M and compute
* its singular value decomposition. The singular vector
* of M corresponding to its smallest singular value provides
* the coordinates of the normal vector of the fitted plane.
* 将方程式系统设定为矩阵 m 并计算其奇异值分解。
* 对应于最小奇异值的 m 的奇异矢量提供了拟合平面法向量的坐标。
* stop ()
* 之前281* 2  562 *3  1686
* column := |X|
create_matrix (3, |X|, [X - Ox,Y - Oy,Z - Oz], MatrixID_Mt)
* get_full_matrix (MatrixID_Mt, MatrixID_Mt_value)
* 矩阵转置
transpose_matrix (MatrixID_Mt, MatrixID_M)
* 矩阵SVD求解
svd_matrix (MatrixID_M, 'reduced', 'right', MatrixID_U, MatrixID_S, MatrixID_V)
* 
* 从矩阵内部找到对应的数值
x := []
* 
for i := 0 to 2 by 1for j := 0 to 2 by 1get_value_matrix (MatrixID_S, [i], [j], x1)x := [x,x1]endfor
endfor
* stop ()
* SingularvaluesOfM  奇异值
get_value_matrix (MatrixID_S, [0,1,2], [0,1,2], SingularvaluesOfM)
* 
* 对s 进行排序
tuple_sort_index (SingularvaluesOfM, Indices)
* 
* Test if more than one singular value of M is (nearly) equal
* to zero. This indicates that the provided 3d points are
* inappropriate to fit the plane (e.g. they are nearly
* collinear or reduce to a single point).
if (SingularvaluesOfM[Indices[0]] < 1e-9 and SingularvaluesOfM[Indices[1]] < 1e-9)return ()
endif
* 
* Get coordinates of the normal vector to the fitted plane
* 
x := Indices[0]
* *从矩阵内部找到对应的数值
get_value_matrix (MatrixID_V, [0,1,2], [Indices[0],Indices[0],Indices[0]], N)
* 创建3 行  ，一列的矩阵
create_matrix (3, 1, N, MatrixID_N)
Nx := N[0]
Ny := N[1]
Nz := N[2]
* 
* Compute the mean residual distance between the 3d points
* and the fitted plane, in order to guess the quality of
* the fitted plane:
* 计算三维点与拟合平面之间的平均剩余距离，以估计拟合平面的质量
* multiplication 乘法
mult_matrix (MatrixID_M, MatrixID_N, 'AB', MatrixID_MN)
* 
* 获取矩阵的所有值
get_full_matrix (MatrixID_MN, Distances)
Distances := abs(Distances)
MeanResidual := sum(Distances) / Size
* 
* Clear the matrices used in the procedure
clear_matrix ([MatrixID_MN,MatrixID_N,MatrixID_V,MatrixID_S,MatrixID_U,MatrixID_M,MatrixID_Mt])
return ()

get_light_plane_pose (Ox, Oy, Oz, Nx, Ny, Nz, LightPlanePose)* This procedure determines a lightplane pose,
* e.g. a pose whose plane defined by z=0 coincides
* with the physical light plane.
* 
* 
* 这个过程决定了一个光平面的姿势，
* 例如一个由 z 0定义的平面与物理光平面一致的姿势。
* stop ()
* Test that the vector passed at input is not null
* 测试在输入处传递的向量是否为空
LightPlanePose := []
Norm := NormalVectorX * NormalVectorX + NormalVectorY * NormalVectorY + NormalVectorZ * NormalVectorZ
if (abs(Norm) < 1e-8)return ()
endif
* 
* In order to compute a light-plane pose, we determine
* two rotations which align the unit vector of the z-axis
* to the normal vector of the light plane, when applied
* successively. For example, we can compute the angles
* Alpha (rotation around the x-axis) and Beta (subsequent
* rotation around the y-axis) in this successive order.
* (The rotation around the z-axis is arbitrarily set
* to zero).
* 
* Determine the value of the angle Alpha and rotate the
* normal vector to the plane y=0. This provides the
* vector N1.
* 为了计算一个光平面的姿态，我们确定了两个旋转，使 z 轴的单位向量与光平面的法向量相一致，当连续施加时。
* 例如，我们可以计算角度 α (围绕 x 轴旋转)和 β (随后围绕 y 轴旋转)在这个连续的顺序。 (围绕 z 轴的旋转被任意设置为零)。
* 确定角 α 的值，并将正常向量旋转到平面 y 0。 它提供了向量 n1。
* 
hom_mat3d_identity (HomMat3DIdentity)
* NY NZ  反正切
tuple_atan2 (NormalVectorY, NormalVectorZ, Alpha)
hom_mat3d_rotate (HomMat3DIdentity, Alpha, 'x', 0, 0, 0, HomMat3DRotateAlpha)
* 
* 将NX NY NZ  变换为 N1x, N1y, N1z 。
affine_trans_point_3d (HomMat3DRotateAlpha, NormalVectorX, NormalVectorY, NormalVectorZ, N1x, N1y, N1z)
* 
* Determine the value of the angle Beta by using the
* coordinates of N1. Note that the rotation around the
* y-axis with angle Beta is carried out in the counter
* trigonometric direction, therefore we apply -Beta.
* 通过使用 n1的坐标来确定角 β 的值。 注意绕 y 轴的转动是沿着反三角方向进行的，因此我们应用-β。
* 
tuple_atan2 (N1x, N1z, Beta)
hom_mat3d_rotate (HomMat3DIdentity, -Beta, 'y', 0, 0, 0, HomMat3DRotateBeta)
* 
* Create the LightPlanePose
* 1. HomMat3DIdentity  平移  ----》omMat3DTranslate
hom_mat3d_translate (HomMat3DIdentity, -OriginX, -OriginY, -OriginZ, HomMat3DTranslate)
* 
* 2  omMat3DTranslate +  沿着 α 角 相乘----》HomMat3DTmp
hom_mat3d_compose (HomMat3DRotateAlpha, HomMat3DTranslate, HomMat3DTmp)
* 
* 3 HomMat3DTmp +沿着 β 角 相乘----》HomMat3DWorldToLightplane
hom_mat3d_compose (HomMat3DRotateBeta, HomMat3DTmp, HomMat3DWorldToLightplane)
* 
* 将齐次变换矩阵 转换为pose
hom_mat3d_to_pose (HomMat3DWorldToLightplane, LightPlanePose)
return ()