halcon 双目相机标定

* Application program to demonstrate the calibration of
* a binocular stereo system using the operators
* find_caltab, find_marks_and_pose, and binocular_calibration.
* 
* As an alternative, a calibration data model could be used.
* Please refer to the example program
* examples/solution_guide/3d_vision/stereo_calibration.hdev.
* 
* We have a stereo setup of two cameras ('camera1 is left of camera2').
* Both cameras will be calibrated by a couple of images of
* a 30mm calibration plate. An image pair will be rectified
* to epipolar images and the epipolar constraint will be checked.
* ---------------------------------------------------------
* Set the image path
ImgPath := 'stereo/board/'
* Read the first images to get their size
Index := 1
read_image (ImageL, ImgPath + 'calib_l_' + Index$'02d')
read_image (ImageR, ImgPath + 'calib_r_' + Index$'02d')
* Reopen the windows with an appropriate size
dev_close_window ()
dev_update_off ()
get_image_size (ImageL, WidthL, HeightL)
dev_open_window (0, 0, WidthL, HeightL, 'black', WindowHandle1)
dev_set_draw ('margin')
dev_set_color ('green')
set_display_font (WindowHandle1, 14, 'mono', 'true', 'false')
get_image_size (ImageR, WidthR, HeightR)
dev_open_window (0, WidthL + 12, WidthL, HeightL, 'black', WindowHandle2)
dev_set_draw ('margin')
dev_set_color ('green')
* Read the model calibration points.
CaltabFile := 'caltab_30mm.descr'
caltab_points (CaltabFile, X, Y, Z)
* Set the initial values for the internal camera parameters
gen_cam_par_area_scan_division (0.0125, 0, 1.48e-5, 1.48e-5, WidthL / 2.0, HeightL / 2.0, WidthL, HeightL, StartCamParL)
StartCamParR := StartCamParL
* Parameter settings for find_caltab and find_marks_and_pose
SizeGauss := 3
MarkThresh := 120
MinDiamMarks := 5
StartThresh := 128
DeltaThresh := 10
MinThresh := 18
Alpha := 0.9
MinContLength := 15
MaxDiamMarks := 100
* Create the tuples in which the image coordinates of the
* calibration marks and the initial poses will be accumulated
RowsL := []
ColsL := []
StartPosesL := []
RowsR := []
ColsR := []
StartPosesR := []
* Start the loop over the calibration images
for Index := 1 to 15 by 1
    * Read the calibration images
    read_image (ImageL, ImgPath + 'calib_l_' + Index$'02d')
    read_image (ImageR, ImgPath + 'calib_r_' + Index$'02d')
    * Search for the calibration plate
    find_caltab (ImageL, CaltabL, CaltabFile, SizeGauss, MarkThresh, MinDiamMarks)
    find_caltab (ImageR, CaltabR, CaltabFile, SizeGauss, MarkThresh, MinDiamMarks)
    * Display calibration plate regions
    dev_set_window (WindowHandle1)
    dev_display (ImageL)
    dev_display (CaltabL)
    dev_set_window (WindowHandle2)
    dev_display (ImageR)
    dev_display (CaltabR)
    * Extraction of marks and pose as well as visualization of the
    * results for the second image.
    find_marks_and_pose (ImageL, CaltabL, CaltabFile, StartCamParL, StartThresh, DeltaThresh, MinThresh, Alpha, MinContLength, MaxDiamMarks, RCoordL, CCoordL, StartPoseL)
    disp_caltab (WindowHandle1, CaltabFile, StartCamParL, StartPoseL, 1)
    * Extraction of marks and pose as well as visualization of the
    * results for the second image.
    find_marks_and_pose (ImageR, CaltabR, CaltabFile, StartCamParR, StartThresh, DeltaThresh, MinThresh, Alpha, MinContLength, MaxDiamMarks, RCoordR, CCoordR, StartPoseR)
    disp_caltab (WindowHandle2, CaltabFile, StartCamParR, StartPoseR, 1)
    * Accumulate the image coordinates of the calibration marks
    * as well as the estimated initial poses for all stereo pairs,
    * where the poses has been estimated consistently.
    RowsL := [RowsL,RCoordL]
    ColsL := [ColsL,CCoordL]
    StartPosesL := [StartPosesL,StartPoseL]
    RowsR := [RowsR,RCoordR]
    ColsR := [ColsR,CCoordR]
    StartPosesR := [StartPosesR,StartPoseR]
endfor
* Perform the actual calibration
binocular_calibration (X, Y, Z, RowsL, ColsL, RowsR, ColsR, StartCamParL, StartCamParR, StartPosesL, StartPosesR, 'all', CamParamL, CamParamR, NFinalPoseL, NFinalPoseR, cLPcR, Errors)
* If required, save the results to disk:
* write_cam_par (CamParamL, 'cam_left-125.dat')
* write_cam_par (CamParamR, 'cam_right-125.dat')
* write_pose (cLPcR, 'pos_right2left.dat')
* Generate the rectification maps
gen_binocular_rectification_map (MapL, MapR, CamParamL, CamParamR, cLPcR, 1, 'viewing_direction', 'bilinear', RectCamParL, RectCamParR, CamPoseRectL, CamPoseRectR, RectLPosRectR)
* Read in a stereo image pair, acquired with the stereo camera system,
* which has been calibrated, just now.
read_image (ImageL, ImgPath + 'calib_l_01')
read_image (ImageR, ImgPath + 'calib_r_01')
* Rectify the stereo images and display them
map_image (ImageL, MapL, ImageRectifiedL)
map_image (ImageR, MapR, ImageRectifiedR)
* Check the epipolar constraint on the rectified images,
* (the differences of the features' row coordinates should be small)
* and visualize the result (including some corresponding epipolar lines)
check_epipolar_constraint (ImageRectifiedL, ImageRectifiedR, RectCamParL, RectCamParR, WindowHandle1, WindowHandle2, CaltabFile, EpipolarError)