Halcon 单目标定，畸变矫正，图像坐标系转世界坐标系

代码为Halcon代码，只有代码以及解释，默认已经理解世界坐标系（WCS）到相机坐标系(CCS)到图像坐标系(ICS)到像素坐标系(PCS)转换

*********************1. 标定内参，设置相机参数以及标定板参数************************************
* gen_cam_par_area_scan_division (0.035, 0, 0.0000024, 0.0000024,5472/2, 3648/2, 5472, 3648, StartCamPar)
*相机参数
gen_cam_par_area_scan_division (0.005, 0, 9.3e-6, 9.3e-6, 640 * 0.5, 512 * 0.5, 640, 512, StartCamPar)
*标定板参数
create_calib_data ('calibration_object', 1, 1, CalibDataID)
set_calib_data_cam_param (CalibDataID, 0, [], StartCamPar)
set_calib_data_calib_object (CalibDataID, 0, 'caltab_100mm.descr')
*打开文件夹
dev_open_file_dialog ('read_image', 'dir', 'default', ImageFiles)
list_files (ImageFiles, ['files'], ImageFiles)
tuple_regexp_select (ImageFiles, ['\\.(tif|tiff|gif|bmp|jpg|jpeg|jp2|png|pcx|pgm|ppm|pbm|xwd|ima|hobj)$','ignore_case'], ImageFiles)
for Index := 0 to |ImageFiles| - 1 by 1
    read_image (Image, ImageFiles[Index])
    dev_display (Image)
    try
        find_calib_object (Image, CalibDataID, 0, 0, Index, [], [])
    catch (Exception)
        dev_get_window(WindowHandle)
        disp_message (WindowHandle, '识别标定板失败', 'window', 12, 12, 'red', 'false')
        stop()
        continue
    endtry
    get_calib_data_observ_contours (Caltab, CalibDataID, 'caltab', 0, 0, Index)
    get_calib_data_observ_points (CalibDataID, 0, 0, Index, Row, Column, Index_demo, Pose)
    gen_cross_contour_xld (Cross, Row, Column, 6, 0.785398)
    dev_set_color ('green')
    dev_display (Caltab)
    dev_display (Cross)
*     stop()
endfor
stop()
*****************************2.标定，获取标定参数***************************
*外参的设置不需要手动，只需要将标准图放在第一张就默认0序列为外参，外参也就是序列0图像的calib_obj_pose的pose
*因为相机0标准是(0,0,0,0,0,0),那么转换矩阵就是这个pose
*在例程image_world_plane中，通过标定板各自的位姿，将相机转换到其标定板中心，也就是说将相机转到世界坐标系中心
*在单目情况下，无论外参设置哪一张图都没关系，其他位姿不会有太大的变化

*未设置(自动)外参的情况
*执行矫正
calibrate_cameras (CalibDataID, Error)
*获取相机内部参数
get_calib_data (CalibDataID, 'camera', 0, 'params_labels', CamParam_Label)
get_calib_data (CalibDataID, 'camera', 0, 'params', CamParam)
*获取0号相机位姿，也就是（0,0,0,0,0,0）
get_calib_data (CalibDataID, 'camera', 0, 'pose', Campose)
*获取全部图像的标定板位姿，也可以理解为0相机对应的外参，就是转换矩阵
Object_Pose:={}
for i:=0 to |ImageFiles| - 1 by 1
    get_calib_data (CalibDataID, 'calib_obj_pose', [0,i], 'pose', Object_Pose_1)
    Object_Pose.at(i):=Object_Pose_1
endfor
*把标定板的厚度算上
set_origin_pose (Object_Pose.at(0), 0.0, 0.0, 1e-03, CamParam_Pose)


* 手动设置标定外参
* dev_open_file_dialog('read_image', 'default', 'default', Selection)
* read_image (Image, Selection)
* find_calib_object (Image, CalibDataID, 0, 0, 0, [], [])
* get_calib_data_observ_contours (Caltab, CalibDataID, 'caltab', 0, 0, 0)
* get_calib_data_observ_points (CalibDataID, 0, 0, 0, Row, Column, Index_demo, Pose)
* gen_cross_contour_xld (Cross, Row, Column, 8, 0.785398)
*外参校准
* calibrate_cameras (CalibDataID, Error)
* get_calib_data (CalibDataID, 'camera', 0, 'params', CamParam_After)
* get_calib_data (CalibDataID, 'camera', 0, 'pose', CamParam_Pose_After)
* Object_Pose_After:={}
* for i:=0 to |ImageFiles| - 1 by 1
*     get_calib_data (CalibDataID, 'calib_obj_pose', [0,i], 'pose', Object_Pose_1)
*     Object_Pose_After.at(i):=Object_Pose_1
* endfor
*对比设置标准位姿,结论：设置外参后其他位置标定板位姿会变，但是差别不大
* for i:=0 to |ImageFiles| - 1 by 1
*    k:=Object_Pose_After.at(i) 
*    b:=Object_Pose.at(i)
* endfor
* K1:=Object_Pose_After
* K2:=Object_Pose_After
*****************************3.畸变矫正***************************
*畸变矫正,正常的如果只是标定畸变矫正，到这一步就可以结束了,写入写出畸变，读取畸变，矫正。线扫相机不适于这种矫正方法，需要用下面的Map图方法
read_image (Image_Standard, ImageFiles[0])
write_cam_par (CamParam, 'Camera_Distortion.dat')
read_cam_par ('Camera_Distortion.dat', CameraParam)
change_radial_distortion_cam_par('adaptive', CamParam, 0, CamParamOut)
change_radial_distortion_image(Image_Standard, Image_Standard, ImageRectified, CamParam, CamParamOut)

*****************************4.求像素当量已经生成Map标定文件***************************
*将图像转到世界坐标系中
*求像素当量,在图像坐标上row(Y)方向,col(x)方向各加一个像素,做到世界坐标后看XY差距多少,在取平均值
get_image_size (Image_Standard, Width, Height)
image_points_to_world_plane (CamParam, CamParam_Pose, [Height/2,Height/2+1,Height/2], [Width/2,Width/2,Width/2+1], 1, X, Y)
distance_pp (Y[0], X[0], Y[1], X[1], Distance_Y)
distance_pp (Y[0], X[0], Y[2], X[2], Distance_X)
Pexil_Size:=(Distance_Y+Distance_X)/2

*转换以标定板中心为(0,0)的世界坐标系
gen_image_to_world_plane_map (Map, CamParam, CamParam_Pose, Width, Height, Width, Height, Pexil_Size, 'bilinear')
map_image (Image_Standard, Map, ImageMapped)
*等价上面两式
* image_to_world_plane (Image, ImageWorld, CamParam, CamParam_Pose, 640, 512, Pexil_Size/1000, 'bilinear')

*将世界坐标的中心拖到图像的中心
set_origin_pose (CamParam_Pose,  (X[0]- Pexil_Size * Width / 2.0),  (Y[0]- Pexil_Size * Height / 2.0), 0, CamParam_NewPose)
gen_image_to_world_plane_map (Map, CamParam, CamParam_NewPose, Width, Height, Width, Height, Pexil_Size, 'bilinear')
map_image (Image_Standard, Map, ImageMapped)

*写出Hobject文件
* write_object (Map, '标定文件')
* read_object (Object, '标定文件')
* map_image (Image_Standard, Map, ImageMapped)

*直接畸变矫正和转到世界坐标系的map有何不一样
stop()