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 ()
cpp复制代码
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 1
for j := 0 to 2 by 1
get_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 ()
cpp复制代码
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 ()