激光三角 光平面标定

零、简介激光三角系统光平面是什么如图所示红色的区域就是光平面那我们看下数学模型是啥为什么不能直接知道因为激光器安装以后Camera Laser 安装误差 ↓ 不知道真正光平面在哪里所以必须标定。一、总概括光平面标定 求相机坐标系下的激光平面方程AXc​BYc​CZc​D0激光三角测量 像素射线与该平面求交点 3D 坐标光平面AXc​BYc​CZc​D0像素→射线(x,y,1)激光三角射线与平面求交深度公式s−D/(AxByC)Halcon 标定 内参 外参 平面 SVD 拟合标定流程棋盘图像 │ ▼ 张正友标定 │ ▼ Camera 内参 K Camera 外参 R,t │ ▼ 激光照射棋盘 │ ▼ 提取亚像素激光中心 (Steger / lines_gauss) │ ▼ 图像坐标 (u,v) │ ▼ K⁻¹ 反投影得到射线 │ ▼ 射线与棋盘平面求交 │ ▼ 三维激光点 P(x,y,z) │ ▼ 最小二乘拟合光平面 │ ▼ AxByCzD0二、光平面标定的本质光平面 激光在相机坐标系下的一个空间平面A⋅Xc​B⋅Yc​C⋅Zc​D0Halcon 做的事求相机内参 K、畸变 Dist求棋盘格外参相机 ↔ 标定板求激光平面 A,B,C,D测量时像素 → 射线 → 与平面求交 → 得到 3D 点激光三角三、坐标系与相机标定1、相机坐标系Camera1相机坐标系Cam原点相机光心Zc​ →光轴向前Xc​ → 右Yc​ → 下2标定板坐标系World Obj原点在棋盘格角点Zw​0 平面 标定板平面3光平面方程相机系AXc​BYc​CZc​D0(A,B,C) 平面单位法向量D 平面到光心的有向距离2、相机标定针孔模型-内参针孔模型 标定内参1理想投影2归一化坐标Zc13去畸变矫正camera_calib输出内参 K 畸变系数 D3、求标定的外参R,T对每一张图 求出pose意义知道任意棋盘点 (Xw​,Yw​,0)就能求它在相机中的 3D 位置四、激光平面标定目标求平面 A,B,C,D约束条件所有激光点必须落在平面上对每一个激光点AXc​BYc​CZc​D01、从像素反推3D点已知像素 (u,v)求相机系射线1去畸变x,yundistort(u,v)2构建射线2、激光点必须在标定板平面上Zw03、构建平面方程Halcon 内部 SVD每个激光点给出一行方程AXc​BYc​CZc​−D写成矩阵svd(M) → V → 最小奇异值向量 → [A,B,C,D]五、测量已知像素 (u,v)内参 K光平面 A,B,C,D求 3D 坐标 (Xc​,Yc​,Zc​)s深度Zcx,y像素去畸变后的归一化坐标几何意义相机射线与光平面求交点1、像素 → 相机系射线2、射线与光平面求交核心公式3、得到3D点六、总结Halcon 算子数学含义calibrate_cameras求 K,Distortionfind_calib_object求外参 R,tcreate_planar_laser_planeSVD 求解 A,B,C,Dget_line_of_sight像素 → 射线 x,yintersect_ray_with_planes−D/(AxByC)get_world_coordinates3D 点计算步骤输入输出数学模型相机标定多张棋盘图像、棋盘角点世界坐标内参 (K)、每帧外参 ((R_i,t_i))张正友标定 重投影误差最小化激光线提取激光照射标定板图像亚像素激光中心 ((u,v))高斯导数 Steger /lines_gauss图像点恢复三维((u,v))、(K)、((R_i,t_i))激光三维点 (P_i(x_i,y_i,z_i))射线与标定板平面求交光平面拟合所有三维激光点光平面参数 ((A,B,C,D))最小二乘 SVD棋盘图像 │ ▼ 亚像素角点Foerstner / Harris │ ▼ 张正友标定 │ ▼ 相机内参 K 外参 (R,t) │ ▼ 激光图像 │ ▼ Steger / lines_gauss │ ▼ 亚像素激光中心 (u,v) │ ▼ K⁻¹ 反投影得到相机射线 │ ▼ 射线与标定板平面求交 │ ▼ 大量三维激光点 │ ▼ 最小二乘SVD拟合 │ ▼ 光平面方程Ax By Cz D 0 │ ▼ 后续激光三角测量图像点 → 相机射线 → 与光平面求交 → 三维坐标七、Halcon 案例* This program demonstrates(证明prove) how to perform the calibration * of a sheet-of-light measurement system. * * The measurement() system consists(由) of an area scan camera and * a light line projector (e.g. a laser line projector). The * position and the orientation of the projector with respect * to the camera are fixed. In order to reconstruct the surface * of the whole object (and not only single profiles), the * object must be moved under the measurement system, * or alternatively the measurement system over the object. * * Therefore, the calibration is performed in three steps: * - At first we determine the internal and external * parameters of the camera by using the standard camera * calibration procedure. * - Then, we determine the orientation of the light plane with * respect to the world coordinate system. This is done by * computing a pose which is oriented such that the plane * defined by z0 coincides with the light plane. * - At last, we calibrate the movement of the object between * the acquisition of two successive profiles. The transform * corresponding to this movement is also expressed with * respect to the world coordinate system. * - Finally, we show how to apply the calibration transforms * to an already acquired disparity image. * * Perform some initializations dev_update_off () dev_close_window () read_image (ProfileImage, sheet_of_light/connection_rod_001.png) get_image_size (ProfileImage, Width, Height) dev_open_window (0, 0, Width, Height, black, WindowHandle) dev_set_draw (margin) dev_set_line_width (3) dev_set_color (lime green) dev_set_lut (default) set_display_font (WindowHandle, 14, mono, true, false) * * ------- * Part 1: Perform the calibration of the camera * ------- * * Initialize some parameters required for the camera calibration 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) CalTabDescription : caltab_30mm.descr * Note that the thickness of the calibration target used for this example is 0.63 mm. * If you adapt this example program to your application, it is necessary to determine * the thickness of your specific calibration target and to use this value instead. CalTabThickness : 0.00063 NumCalibImages : 20 * * Initialize a calibration data model create_calib_data (calibration_object, 1, 1, CalibDataID) set_calib_data_cam_param (CalibDataID, 0, [], StartParameters) set_calib_data_calib_object (CalibDataID, 0, CalTabDescription) * * Collect mark positions and estimated poses for all * calibration images for Index : 1 to NumCalibImages by 1 read_image (Image, sheet_of_light/connection_rod_calib_ Index$.2) dev_display (Image) find_calib_object (Image, CalibDataID, 0, 0, Index, [], []) get_calib_data_observ_points (CalibDataID, 0, 0, Index, Row, Column, _Index, Pose) get_calib_data_observ_contours (Contours, CalibDataID, caltab, 0, 0, Index) dev_set_color (green) dev_display (Contours) gen_cross_contour_xld (Cross, Row, Column, 6, 0.785398) dev_set_color (yellow) dev_display (Cross) endfor stop () * * Perform the actual calibration calibrate_cameras (CalibDataID, Errors) disp_message (WindowHandle, The camera calibration has been performed successfully, window, 12, 12, black, true) disp_continue_message (WindowHandle, black, true) stop () * * ------- * Part 2: Calibrate the orientation of the light plane * with respect to the world coordinate system * ------- dev_set_colored (3) MinThreshold : 80 * * Definition of the world coordinate system (WCS): * Here, the WCS is defined implicitly by choosing one * specific calibration image. In order to take the thickness * of the calibration table into account, we shift the origin * of the pose that corresponds to the chosen calibration image * (and that was computed during the calibration). Index : 19 get_calib_data (CalibDataID, calib_obj_pose, [0,Index], pose, CalTabPose) set_origin_pose (CalTabPose, 0.0, 0.0, CalTabThickness, CameraPose) read_image (CalTabImage1, sheet_of_light/connection_rod_calib_ Index$.2) dev_display (CalTabImage1) get_calib_data (CalibDataID, camera, 0, params, CameraParameters) disp_3d_coord_system (WindowHandle, CameraParameters, CameraPose, .01) disp_message (WindowHandle, World coordinate system, window, 12, 12, black, true) disp_continue_message (WindowHandle, black, true) stop () * * Definition of a temporary coordinate system (TCS): * The TCS is also defined implicitly by choosing another * calibration image. Here again we shift the origin of * the coordinate system in order to take the thickness * of the calibration table into account. Index : 20 get_calib_data (CalibDataID, calib_obj_pose, [0,Index], pose, CalTabPose) set_origin_pose (CalTabPose, 0.0, 0.0, CalTabThickness, TmpCameraPose) read_image (CalTabImage2, sheet_of_light/connection_rod_calib_ Index$.2) dev_display (CalTabImage2) disp_3d_coord_system (WindowHandle, CameraParameters, TmpCameraPose, .01) disp_message (WindowHandle, Temporary coordinate system, window, 12, 12, black, true) disp_continue_message (WindowHandle, black, true) stop () * * * Compute the 3D coordinates of the light line points * in the plane z0 of the WCS dev_clear_window () read_image (ProfileImage1, sheet_of_light/connection_rod_lightline_019.png) compute_3d_coordinates_of_light_line (ProfileImage1, MinThreshold, CameraParameters, [], CameraPose, X19, Y19, Z19) if (|X19| 0 or |Y19| 0 or |Z19| 0) dev_display (ProfileImage1) disp_message (WindowHandle, The profile MUST be oriented horizontally\nfor successfull processing!\nThe program will exit., window, 12, 12, black, true) return () endif * * Compute the 3D coordinates of the light line points * in the plane z0 of the TCS read_image (ProfileImage2, sheet_of_light/connection_rod_lightline_020.png) compute_3d_coordinates_of_light_line (ProfileImage2, MinThreshold, CameraParameters, TmpCameraPose, CameraPose, X20, Y20, Z20) if (|X20| 0 or |Y20| 0 or |Z20| 0) disp_message (WindowHandle, The profile MUST be oriented horizontally\nfor successfull processing!\nThe program will exit., window, 12, 12, black, true) return () endif * * 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. fit_3d_plane_xyz ([X19,X20], [Y19,Y20], [Z19,Z20], 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 5e-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 z0 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, 12, 12, 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) stop () dev_clear_window () * * ------- * Part 3: Calibration of the movement of the object between * the acquisition of two successive profiles * ------- * * In order to determine the movement of the object between * two successive profile images, we acquire two images of a * calibration table which describe the same movement. * In order to get a good accuracy, we usually move the * calibration table by more than one step. read_image (CaltabImagePos1, sheet_of_light/caltab_at_position_1.png) read_image (CaltabImagePos20, sheet_of_light/caltab_at_position_2.png) StepNumber : 19 * * Set the optimized camera parameter as new start camera parameters for the * calibration data model to extract the following poses using * these calibrated parameters. set_calib_data_cam_param (CalibDataID, 0, [], CameraParameters) * Compute the pose of the calibration table in each image find_calib_object (CaltabImagePos1, CalibDataID, 0, 0, NumCalibImages 1, [], []) * Extract the unoptimized pose from the calibration data model get_calib_data_observ_points (CalibDataID, 0, 0, NumCalibImages 1, Row1, Column1, Index1, CameraPosePos1) find_calib_object (CaltabImagePos20, CalibDataID, 0, 0, NumCalibImages 2, [], []) get_calib_data_observ_points (CalibDataID, 0, 0, NumCalibImages 2, Row1, Column1, Index1, CameraPosePos20) * * Compute the coordinates of the origin of the calibration * table in the two positions with respect to the world * coordinate system and determine the coordinates of the * corresponding translation vector set_origin_pose (CameraPosePos1, 0.0, 0.0, CalTabThickness, CameraPosePos1) set_origin_pose (CameraPosePos20, 0.0, 0.0, CalTabThickness, CameraPosePos20) pose_to_hom_mat3d (CameraPosePos1, HomMat3DPos1ToCamera) pose_to_hom_mat3d (CameraPosePos20, HomMat3DPos20ToCamera) pose_to_hom_mat3d (CameraPose, HomMat3DWorldToCamera) hom_mat3d_invert (HomMat3DWorldToCamera, HomMat3DCameraToWorld) hom_mat3d_compose (HomMat3DCameraToWorld, HomMat3DPos1ToCamera, HomMat3DPos1ToWorld) hom_mat3d_compose (HomMat3DCameraToWorld, HomMat3DPos20ToCamera, HomMat3DPos20ToWorld) affine_trans_point_3d (HomMat3DPos1ToWorld, 0, 0, 0, StartX, StartY, StartZ) affine_trans_point_3d (HomMat3DPos20ToWorld, 0, 0, 0, EndX, EndY, EndZ) create_pose (EndX - StartX, EndY - StartY, EndZ - StartZ, 0, 0, 0, RpT, gba, point, MovementPoseNSteps) MovementPose : MovementPoseNSteps / StepNumber String : [MovementPose: , Tx MovementPose[0]$.3 m, Ty MovementPose[1]$.3 m, Tz MovementPose[2]$.3 m, alpha MovementPose[3] °, beta MovementPose[4] °, gamma MovementPose[5] °, type MovementPose[6]] disp_message (WindowHandle, String, window, 12, 12, black, true) disp_continue_message (WindowHandle, black, true) stop () dev_clear_window () * * ------- * Part 4: Apply the calibration transforms to an already * acquired disparity image. * ------- * * Read an already acquired disparity map from file read_image (Disparity, sheet_of_light/connection_rod_disparity.tif) * Create a model and set the required parameters gen_rectangle1 (ProfileRegion, 120, 75, 195, 710) create_sheet_of_light_model (ProfileRegion, [min_gray,num_profiles,ambiguity_solving], [70,290,first], SheetOfLightModelID) set_sheet_of_light_param (SheetOfLightModelID, calibration, xyz) set_sheet_of_light_param (SheetOfLightModelID, camera_parameter, CameraParameters) set_sheet_of_light_param (SheetOfLightModelID, camera_pose, CameraPose) set_sheet_of_light_param (SheetOfLightModelID, lightplane_pose, LightPlanePose) set_sheet_of_light_param (SheetOfLightModelID, movement_pose, MovementPose) * * Apply the calibration transforms and * get the resulting calibrated coordinates apply_sheet_of_light_calibration (Disparity, SheetOfLightModelID) get_sheet_of_light_result (X, SheetOfLightModelID, x) get_sheet_of_light_result (Y, SheetOfLightModelID, y) get_sheet_of_light_result (Z, SheetOfLightModelID, z) * * Display the resulting Z-coordinates dev_close_window () get_image_size (Disparity, Width, Height) dev_open_window (Height 10, 0, Width * .5, Height * .5, black, WindowHandle3) set_display_font (WindowHandle3, 14, mono, true, false) dev_set_lut (temperature) dev_display (Z) disp_message (WindowHandle3, Calibrated Z-coordinates, window, 12, 12, black, true) * * Display the resulting Y-coordinates dev_open_window ((Height 10) * .5, 0, Width * .5, Height * .5, black, WindowHandle2) set_display_font (WindowHandle2, 14, mono, true, false) dev_display (Y) disp_message (WindowHandle2, Calibrated Y-coordinates, window, 12, 12, black, true) * * Display the resulting X-coordinates dev_open_window (0, 0, Width * .5, Height * .5, black, WindowHandle1) dev_display (X) set_display_font (WindowHandle1, 14, mono, true, false) disp_message (WindowHandle1, Calibrated X-coordinates, window, 12, 12, black, true) * * * Visualize setup with reconstructed object * Note that the camera observes that part of the plane which the object traverses get_sheet_of_light_result_object_model_3d (SheetOfLightModelID, ObjectModel3D) gen_sheet_of_light_object_model_3d (SheetOfLightModelID, 0.1, 0.05, 0.3, OM3DLightPlane, OM3DMovement, OM3DCamera, OM3DCone) dev_open_window (0, Width * .5 10, Width, Height * 1.5, black, WindowHandle) dev_set_lut (default) set_display_font (WindowHandle, 14, mono, true, false) visualize_object_model_3d (WindowHandle, [ObjectModel3D,OM3DLightPlane,OM3DMovement,OM3DCamera,OM3DCone], [], [-0.002989894371,0.1325031046,8.667736001,288.0583956,2.798360231,297.2537796,0], [alpha_1,alpha_3,alpha_4,alpha_5,color_0,color_3,color_4,color_5], [0.5,0.5,0.5,0.5,blue,green,green,green], Setup with reconstructed object, [], [], PoseOut)整个HALCON程序对应数学流程相机标定 │ ▼ CameraParameters(K,Dist) CameraPose(R,t) │ ▼ 世界坐标系(WCS)建立 │ ▼ 激光线亚像素提取 │ ▼ compute_3d_coordinates_of_light_line │ ▼ 大量三维激光点 │ ▼ fit_3d_plane_xyz │ ▼ 光平面 AxByCzD0 │ ▼ get_light_plane_pose │ ▼ 得到LightPlanePose重点讲解Part1、相机标定求相机内参 K、畸变、以及每张标定板相对于相机的 Pose。每一张棋盘都有R tPart2、建立世界坐标Index:19HALCON把第19张棋盘定义成世界坐标。然后再选 Index20只是为了以后拟合光平面。Part3、compute_3d_coordinates_of_light_line最重要输入的是什么compute_3d_coordinates_of_light_line ( ProfileImage1, 激光图像 MinThreshold, 激光图像筛选阈值 CameraParameters, 相机的内参K [], CameraPose, 第 19张图像对应的外参(R,t) 世界平面(z0) X19, Y19, Z19)输入实际上只有四样东西① 激光图像 ② 相机内参 K ③ 相机外参(R,t) ④ 世界平面(z0)画出来就是图像中的一个激光点假设Steger算法提取出一个亚像素点(u,v) (420.37 ,125.82)图像------------------------- * -------------------------像素→相机坐标即上一步 计算像素→相机坐标相机内参例如fx2000 fy2000 cx376 cy120HALCON做注意这里只是方向不是空间点形成一条射线于是 空间里面 就是因为相机中心 就是C(0,0,0)所以Part4、拟合光平面HALCON已经知道第19张棋盘就是世界坐标。因此棋盘就是 z0为什么还要第20张因为第19张只能得到一条空间直线还不能唯一确定光平面。于是把棋盘升高第20张再次1000个像素 ↓ 1000条射线 ↓ 1000个交点然后组合起来组合起来就算法向量总流程【① 相机标定】 棋盘1 棋盘2 ... 棋盘20 \ | / \ | / \ | / Camera │ ▼ K Distortion Pose1...Pose20 │ ▼ 选择第19张作为世界坐标(Z0) │ ▼ ---------------------------- ← WCS (z0) ////// 激光线 ////// ← 光平面∩棋盘 ---------------------------- ▲ │ 光平面(Laser Plane) │ 激光器 Camera 对激光线上的每个像素执行 (u,v) │ ▼ 反投影得到射线 │ ▼ 射线 ∩ z0平面 │ ▼ 得到 P1、P2、...、Pn三维点 第19张得到一条3D激光线 第20张得到另一条3D激光线 P1────────────Pn ╲ ╲ ╲ Q1────────────Qn │ ▼ fit_3d_plane_xyz() │ ▼ 光平面 AxByCzD0 │ ▼ LightPlanePose