3D线激光成像数学模型简析与实现

点云的获取方式有多种，比如三维成像传感器、Lidar激光探测与测量、逆向工程等... 对于寻常百姓家，后2者的成本是十分昂贵的，所以我们可以尝试玩一下自己搭建三维成像传感器中的一种——3D线激光，一个单目相机，一个激光足矣。

对于3D线扫，要产生点云，首先需要对系统进行标定，这里我们需要获取到的信息有：

1.相机的内外参 2.激光的平面方程   3.移动方向的平移矩阵

在计算出这三步骤结果后，我们就可以通过每一张2D像素图片，计算出3D点的空间位置。

图像中每个点都可以得到一个像素坐标P(x,y)，这个点是实际空间中的某个点，和相机中心点的连线在相机平面上的投影得到的。因此根据相机内外参的标定结果，我们可以得到相机的中心点C(Xc,Yc,Zc)，以及这个像素坐标 $P$ 映射到的实际空间坐标P0(X0,Y0,Z0)，这里的P0理论上应该是一组点，我们可以直接取Z0=0的点来方便计算。由这两个点组成的空间直线与激光平面的交点就是其三维空间中的点坐标。我们可以通过直线方程和激光平面方程联立解出该点的坐标。

1）直线方程：(x-Xc)/(x-X0) ＝ (y-Yc)/(y-Y0) ＝ (z-Zc)/(z-Z0)2）平面方程：Ax+By+Cz+D=0

在一张图片中的所有点都计算完毕后，再次计算下一张图片中的所有点。每张图片的计算结果都要根据移动方向的平移矩阵来进行叠加映射。

halcon有非常简单实用的线激光3D点云成像的相关例程，我们可以通过例程自己搭建平台进行3D相机的开发和测试，那就是halcon的例程calibrate_sheet_of_line_calplate.hdev，这个例程就是对自己用线激光和相机搭建的3D平台进行参数标定。

整个例程的步骤非常明确，每一步在halcon的例程中都有配文字说明，但是有些细节和关键参数需要了解透彻，这样才能方便我们在搭建自己的系统时可以替换和改变关键的参数。下面我们来详细解析这个例程：

* Part 1: Perform the calibration of the camera

首先第一步，对相机参数的标定，实际上是外参的标定

gen_cam_par_area_scan_polynomial (0.0125, 0.0, 0.0, 0.0, 0.0, 0.0, 0.000006, 0.000006, 376.0, 120.0, 752, 240, StartParameters)

设置相机的内参（焦距，畸变参数K1,K2,K3,P1,P2,像元尺寸X，像元尺寸Y，图片中点CX，图片中心点CY，图片宽W，图片高K）

这些基础相机内参可以直接手动填写，不需要特别准确，例如镜头多少mm，像元尺寸和图像大小这些都是可以已知的。

CalTabDescription := 'caltab_30mm.descr'

标定板标准文件，实际上是halcon的圆点标定板的尺寸30mm，如果大家在用不同长度的标定板需要在这里修改，这个文件名实际上是对应到halcon默认路径下的标定板描述文件，在halcon安装目录下面有个calib文件夹，而.descr文件实际上就是对标定板的描述文件，可直接用txt或notepad打开，里面对标定板的长宽，边界，圆点半径等的一些设定，用户有需要也可直接找一个文件自己手动修改成自己的标定板尺寸。

CalTabThickness := .00063

标定板厚度，单位是m，halcon有些例程经常会在单位mm和单位m之间来回切换，大家需要注意

NumCalibImages := 20

用于外参标定的图片的数目，这里设置为20，大家可以根据自己实际上可以拍摄的图片进行修改。这里需要拍摄20张标定板的图片，这20张标定板图片用来计算相机的外参。

后面一段代码就是循环对标定板图片进行处理，获取标定板对应的位置坐标。因为是多张图片进行拟合计算，所以理论上图片数目多，标定板位置变化越大，对于最后的结果越准确，包括标定板需要各种角度的倾斜，在不同平面上拍摄等等。

*Part 2: Calibrate the orientation of the light plane with respect to the world coordinate system

设置标准世界坐标系坐标和激光平面的计算标定

MinThreshold := 80

这里设置最小阈值，是用来后面计算激光线图片时提取激光的有效范围用的，也可根据实际情况修改，不过影响不大，因为激光图片的黑白分明非常明显。

Index := 19

这里需要特别注意，在这里设置第19张图片标定板的坐标系基准世界坐标，后面的点云坐标系就是基于这个了。所以大家在拍照的时候第19张（或者可以改成任何你想要的一张）最好选择一个平面而且将标定板放正一些，这样在后续很多调试的时候你能明白自己的世界坐标大概在什么位置（可以看到halcon例程中这一张图片也是很正的位置）。

同时导入与这个标定板位置对应的一张激光图像，这张激光图像的拍摄理论上是跟标定板在同一位置，即拍摄完第19张标定板后，标定板拿开，在同样的位置打上激光，拍摄激光图片。

Index := 20

这是和之前第19张图片位置有一定高度差的另外一个平面的位置拍摄的标定板图片，同样在这个位置拍摄完标定板后需要再拍摄一张激光图片，这是因为需要确定激光平面的最简单方式就是在空间上找到两条平行线，即可确定一个平面。这里第19和第20最好是两个平行的面，这样激光打在上面就会呈现出两条平行线，这样拟合出来的激光平面就更准确。

后面的代码都是对激光图片的有效点提取，以及计算拟合激光平面。拟合完成后有一个RMS值用来判断拟合的精度效果：如果拟合RMS大于这个设定值，就认为拟合失败，这里单位也是m。

if (MeanResidual > 5e-5)
  return ()
endif

在计算完成后，halcon会将激光平面的参数show出来：

这里实际上是代表两个坐标系之间的pose的转换关系，type=0表示的是ZYX模式，从这6个量我们大概能判断出最后的激光平面是否计算正常，这也是我之前建议第19张标定板图片尽量摆正，而且激光器打出的线激光也尽量和标定板平行。从这个结果可以看出beta角接近360°，gamma接近0度，基本上可以理解为激光和世界坐标只有一个方向的夹角就是alpha，然后这个角度也是我们安装激光时自己可控的。

在上面两个步骤完成之后，实际上就可以通过某一张激光图来计算出这上面的激光点对应的世界坐标系下的3D点的XYZ值了。

* Part 3: Calibration of the movement of the object between the acquisition of two successive profiles

第三部分，很好理解，就是计算出运动平面的方向pose，方便每一条激光线的叠加。

因为线激光3D成像最后一定是需要一个运动平台的，不论是相机动还是物体动，都一样，需要将运动平台动的方向（这里的方向指的是XYZ三个方向）计算出来。

read_image (CaltabImagePos1, 'sheet_of_light/caltab_at_position_1.png')
read_image (CaltabImagePos20, 'sheet_of_light/caltab_at_position_2.png')
StepNumber := 19

这里应该是提前拍好了两张标定板，这两张标定板是经过运动平面运动一定的步长之后拍摄的，那么就把这个步长值设置给StepNumber。这个步长值是我们自己可以控制的，根据实际运动平台和实际工作时的触发拍照的情况来设置。

这里在计算完两张图片的标定板位置后，需要将它们的坐标转换到第一步的第19张标定板对应的基准世界坐标位置下，再进行两张标定板的相对关系计算，最后除以StepNumber即可。

MovementPose := MovementPoseNSteps / StepNumber

从这个运动平面计算结果可以看出，在X,Z两个方向上基本上都是0，在Y方向上有一定的位移。这里同样建议大家在拍摄这两张运动步长的标定板时，尽量将标定板放正，这样在计算出来步长结果时可以直接判断是否准确。比如我们已经知道自己的轴或者机械手运动一个单位是多少mm，最后根据计算出来的结果进行比对即可。到这里sheetofline的所有参数就计算完毕了。

最后总结一下：这三个步骤实际上需要拍摄N张标定板图片，2张激光平面图片，2张运动前后的标定板图片。

那么最合理的标定步骤应该时怎样的呢：

1.首先需要用设备拍摄N-2张姿态、位置和角度不同的标定板图片。标定板放在视野范围内，各种平移和倾斜旋转；2.拍摄倒数第二张标定板图片，这张标定板图片将用来作为后续的基准坐标系用于计算，在拍摄时尽量将标定板放置于平面，放正，与相机平行，这样后续方便自我检查；3.拍摄完后，将标定板拿走，在同一个位置打上激光线，激光线最好也与相机平行，与标定板的边也平行；4.重复第2步，拍摄倒数第一张标定板图片，这张标定板所在的平面最好是与之前的标准平面有一个台阶落差，同样尽量放平放正；5.重复第3步拍摄激光图片；6.再将标定板放在运动平面上，尽量放平放正，拍摄一张；7.控制运动平面运动一定的步长N，记录N，并且拍摄一张运动后的标定板图片，注意在第6步和第7步中间标定板不能移动。

OK，到这里halcon搭建3D线激光的原理和步骤都讲解完了，看下最终的效果图吧~