halcon: 3D相机手眼标定（二）——棋盘格

在halcon中，基于棋盘格，进行手眼标定

主要工作：

1、生成标定板坐标

2、识别提取棋盘格标定板

3、计算棋盘格在相机中的姿态

其实就主要针对棋盘格的识别和提取角点

1、生成标定板坐标

BoradLength:=25
BordCornerRowNum:=11
BordCornerColNum:=8
genchessboardCalibParam (BordCornerRowNum, BordCornerColNum, BoradLength, x, y, z)

x := []
y := []
z := []
zz := []

xx := []
yy := []
tuple_gen_const (BordCornerRowNum*BordCornerColNum, 0, z)
tuple_gen_const (BordCornerRowNum*BordCornerColNum, 1, zz)

for Index := 0 to BordCornerRowNum-1 by 1
    xx := [xx,Index*BoradLength*0.001]
    yy := [yy,0]
endfor

for Index1 := 0 to BordCornerColNum-1 by 1
    x := [x,xx]
    y := [y,(yy+Index1)*BoradLength*0.001]
endfor
return ()

在我们创建手眼标定名柄后，把xyz丢进去就行

*创建标定句柄
create_calib_data ('hand_eye_stationary_cam', 1, 1, CalibDataID)
* 添加棋盘格标定板参数到标定句柄
set_calib_data_calib_object (CalibDataID, 0, [x,y,z])

2、识别提取棋盘格标定板

2.1、提取标定板：ps：鲁棒性较差

thresholdMaxGray:=100
while (true)
    rgb1_to_gray (Image, GrayImage)
*     scale_image (GrayImage1, GrayImage, 0.5, 0)
    gauss_filter (GrayImage, ImageGauss, 3)
    emphasize (ImageGauss, ImageEmphasize, 18, 18, 2)
    
    
    *提取标定板整体
    threshold (ImageEmphasize, Regions, 50, 255)
    dilation_rectangle1 (Regions, RegionDilation1, 5, 5)
    connection (RegionDilation1, ConnectedRegions)
    select_shape (ConnectedRegions, SelectedRegions, ['area','rectangularity','area_holes'], 'and', [105094,0.2,5000], [800000,1,300000])
   
    shape_trans (SelectedRegions, RegionTrans1, 'convex')
    reduce_domain (ImageEmphasize, RegionTrans1, ImageReduced1)
    
    *提取标定板黑色棋盘
    ***********
    *方案一
    scale_image_max (ImageReduced1, ImageScaleMax)
    illuminate (ImageScaleMax, ImageIlluminate, 10, 10, 0.7)
    emphasize (ImageIlluminate, ImageEmphasize1, 15, 15, 0.1)
    equ_histo_image (ImageEmphasize1, ImageEquHisto)    
    fast_threshold (ImageEquHisto, Region, 0, 30, 20)
    dilation_rectangle1 (Region, RegionDilation2, 4, 4)
     

    connection (RegionDilation2, ConnectedRegions2)    
    
    diameter_region (ConnectedRegions2, Row11, Column1, Row2, Column2, Diameter)

    tuple_sort_index (Diameter, Indices)
    tuple_inverse (Indices, Inverted)
    select_shape (ConnectedRegions2, SelectedRegions1, 'max_diameter', 'and', Diameter[Inverted[5]], max(Diameter)) 

    union1 (SelectedRegions1, RegionUnion)
    opening_rectangle1 (RegionUnion, RegionOpening, 2, 2)
*     opening_circle (RegionUnion, RegionOpening, 5)
    dilation_rectangle1 (RegionOpening, RegionDilation, 2, 2)
    connection (RegionDilation, ConnectedRegions1)
    select_shape (ConnectedRegions1, SelectedRegions2, ['area','rectangularity','area_holes'], 'and', [10000,0.2,200], [800000,1,300000])
    dev_display (SelectedRegions2)
    area_center (SelectedRegions2, Area, Row1, Column)
    if (|Row1|=0)
        thresholdMaxGray:=thresholdMaxGray+5
    else
        break
    endif
endwhile




shape_trans (SelectedRegions2, RegionTrans, 'convex')

erosion_rectangle1 (RegionTrans, RegionErosion, 11, 11)
dev_clear_window ()

reduce_domain (ImageEmphasize, RegionErosion, ImageReduced)


dev_display (ImageReduced)

2.2 提取角点

主要使用saddle_points_sub_pix算子

*搜索棋盘格中的角点
SigmaSaddlePoints := 1
Threshold := 1
MaxThreshold:=20


for Index := Threshold to MaxThreshold by 1
    *saddle_points_sub_pix亚像素精确检测图像中的鞍点。
    saddle_points_sub_pix (ImageReduced, 'facet', SigmaSaddlePoints, Threshold, Row, Col)
    *删除相邻的点
    DeleteNearPoint (Row, Col, Row, Col)
*     dev_clear_window ()
*     dev_display (ImageReduced)
*     for Index1 := 0 to |Row|-1 by 1
*         gen_cross_contour_xld (Cross1, Row[Index1], Col[Index1], 6, 0.785398)
*         dev_disp_text (Index1, 'image', Row[Index1], Col[Index1], 'red', [], [])  
*     endfor
*     stop ()
    saddleNum:=|Row|

    if (saddleNum=BordCornerRowNum*BordCornerColNum)
        *判断角点的间隔是否正常，两点间小于最小像素距离报错
        distance_pp (Row[0:saddleNum-2], Col[0:saddleNum-2], Row[1:saddleNum-1], Col[1:saddleNum-1], Distance1)
        tuple_min (Distance1, Min)
        if (Min<20 and SigmaSaddlePoints<5)
            SigmaSaddlePoints:=SigmaSaddlePoints+0.5
            if (Threshold>5)
                Threshold:=1
            endif
            continue
        endif
        break
    elseif(saddleNum>BordCornerRowNum*BordCornerColNum)
        Threshold:=Threshold+1
        if (Index>6)
            SigmaSaddlePoints:=SigmaSaddlePoints+1
            Threshold:=1
        endif
        continue
    elseif(saddleNum<BordCornerRowNum*BordCornerColNum)
        SigmaSaddlePoints:=SigmaSaddlePoints+1
        Threshold:=1
        continue
    endif
 
endfor

其中DeleteNearPoint算子，主要是解决saddle_points_sub_pix算子，某个角点处，提取了2个点

del_index:=[]
for i := 0 to |Row|-1 by 1
    if (del_index!=[])
        tuple_find (del_index, i, Indices)
        if (Indices!=-1 )
            continue
        endif
    endif

    tuple_gen_const (|Row|, Row[i], temp_row)
    tuple_gen_const (|Row|, Col[i], temp_col)
    distance_pp (temp_row, temp_col, Row, Col, Distance)
*     tuple_sort (Distance, Sorted1)
    *方法一
    tuple_less_elem (Distance, 20, Less)
    tuple_greater_elem (Distance, 0, Greater)
    tuple_and (Less, Greater, And)
    tuple_find (And, 1, Indices1)
    if (Indices1!=-1)
        del_index:=[del_index,Indices1]
    endif
    
    *方法二
*     for j := 0 to |Distance|-1 by 1
*         if (Distance[j]>0 and Distance[j]<20)
*             del_index:=[del_index,j]
*         endif
*     endfor
    
*      stop ()
endfor
tuple_sort (del_index, Sorted)
*去除重复的点
uniq12:=uniq(Sorted)
tuple_remove (Row, uniq12, del_Row)
tuple_remove (Col, uniq12, del_Col)
return ()

从上图看，角点的排序其实是基于row坐标从上至下排列的，这不是我们想要的，需要重新对角点进行排序

2.3 角点排序

* ******找出4个角点**********
gen_contour_region_xld (RegionTrans, Contours, 'border')
 dev_display (GrayImage)
dev_display (Contours)
segment_contours_xld (Contours, ContoursSplit, 'lines_circles', 5, 4, 2)
* 按长度排序后选择
length_xld (ContoursSplit, Length)
tuple_sort (Length, Sorted)
min1 := Sorted[|Sorted|-4]
max1 := max(Length)

select_contours_xld (ContoursSplit, SelectedContours, 'contour_length', min1, max1, -0.5, 0.5)

fit_line_contour_xld (SelectedContours, 'tukey', -1, 0, 5, 2, RowBegin, ColBegin, RowEnd, ColEnd, Nr, Nc, Dist)

intersection_lines (RowBegin[0], ColBegin[0], RowEnd[0], ColEnd[0], RowBegin[1], ColBegin[1], RowEnd[1], ColEnd[1], RowPoint1, ColumnPoint1, IsOverlapping)
intersection_lines (RowBegin[1], ColBegin[1], RowEnd[1], ColEnd[1], RowBegin[2], ColBegin[2], RowEnd[2], ColEnd[2], RowPoint2, ColumnPoint2, IsOverlapping)
intersection_lines (RowBegin[2], ColBegin[2], RowEnd[2], ColEnd[2], RowBegin[3], ColBegin[3], RowEnd[3], ColEnd[3], RowPoint3, ColumnPoint3, IsOverlapping)
intersection_lines (RowBegin[3], ColBegin[3], RowEnd[3], ColEnd[3], RowBegin[0], ColBegin[0], RowEnd[0], ColEnd[0], RowPoint4, ColumnPoint4, IsOverlapping)

gen_cross_contour_xld (Cross5, RowPoint1, ColumnPoint1, 16, 0.785398)
gen_cross_contour_xld (Cross6, RowPoint2, ColumnPoint2, 16, 0.785398)
gen_cross_contour_xld (Cross7, RowPoint3, ColumnPoint3, 16, 0.785398)
gen_cross_contour_xld (Cross8, RowPoint4, ColumnPoint4, 16, 0.785398)
dev_display (GrayImage)
dev_display (Contours)
dev_display (Cross5)
dev_display (Cross6)
dev_display (Cross7)
dev_display (Cross8)



tuple_gen_const (|Row|, RowPoint1, RowPoint1s)
tuple_gen_const (|Row|, ColumnPoint1, ColumnPoint1s)
tuple_gen_const (|Row|, RowPoint2, RowPoint2s)
tuple_gen_const (|Row|, ColumnPoint2, ColumnPoint2s)
tuple_gen_const (|Row|, RowPoint3, RowPoint3s)
tuple_gen_const (|Row|, ColumnPoint3, ColumnPoint3s)
tuple_gen_const (|Row|, RowPoint4, RowPoint4s)
tuple_gen_const (|Row|, ColumnPoint4, ColumnPoint4s)

distance_pp (RowPoint1s, ColumnPoint1s, Row, Col, Distance1s)
distance_pp (RowPoint2s, ColumnPoint2s, Row, Col, Distance2s)
distance_pp (RowPoint3s, ColumnPoint3s, Row, Col, Distance3s)
distance_pp (RowPoint4s, ColumnPoint4s, Row, Col, Distance4s)

tuple_find (Distance1s, min(Distance1s), Indices21)
tuple_find (Distance2s, min(Distance2s), Indices22)
tuple_find (Distance3s, min(Distance3s), Indices23)
tuple_find (Distance4s, min(Distance4s), Indices24)

gen_cross_contour_xld (Cross11, Row[Indices21], Col[Indices21], 26, 0.785398)
gen_cross_contour_xld (Cross12, Row[Indices22], Col[Indices22], 26, 0.785398)
gen_cross_contour_xld (Cross13, Row[Indices23], Col[Indices23], 26, 0.785398)
gen_cross_contour_xld (Cross14, Row[Indices24], Col[Indices24], 26, 0.785398)

dev_display (GrayImage)
dev_display (Cross11)
dev_display (Cross12)
dev_display (Cross13)
dev_display (Cross14)

diag_corner1 := [Row[Indices21], Col[Indices21]]
diag_corner2 := [Row[Indices22], Col[Indices22]]
diag_corner3 := [Row[Indices23], Col[Indices23]]
diag_corner4 := [Row[Indices24], Col[Indices24]]

* diag_corner1 := [RowPoint1, ColumnPoint1]
* diag_corner2 := [RowPoint2, ColumnPoint2]
* diag_corner3 := [RowPoint3, ColumnPoint3]
* diag_corner4 := [RowPoint4, ColumnPoint4]

* *****根据角点做透视变换*****
* 确定图像上坐标角点的顺序。根据上面找到的坐标，顺序应该为[1,2,3,4]。但存在起点1-4为长边的情况，正常1-2为短边，1-4为长边
* 先判断1-2，1-4两对点的距离大小，如果1-4长，起始角点变为4，反之起始点为1
distance_pp (diag_corner1[0], diag_corner1[1], diag_corner2[0], diag_corner2[1], DistHeight)
distance_pp (diag_corner1[0], diag_corner1[1], diag_corner4[0], diag_corner4[1], DistWidth)
P1 := []
P2 := []
P3 := []
P4 := []
if (DistHeight>DistWidth)
    P1 := diag_corner1
    P2 := diag_corner2
    P3 := diag_corner3
    P4 := diag_corner4

else
    P1 := diag_corner4
    P2 := diag_corner1
    P3 := diag_corner2
    P4 := diag_corner3

endif

ImageCornersX := [P1[0],P2[0],P3[0],P4[0]]
ImageCornersY := [P1[1],P2[1],P3[1],P4[1]]
ImageCornersZ := [1,1,1,1]

gen_cross_contour_xld (Cross2, ImageCornersX, ImageCornersY, 40, 1)
* for Index10 := 0 to 3 by 1
*     select_obj (Cross2, ObjectSelected, Index10+1)
*     dev_display (ObjectSelected)
*     dev_disp_text (Index10+1, 'image', ImageCornersX[Index10], ImageCornersY[Index10], 'red', [], [])
* endfor
* stop ()
* 假设世界坐标下的4个角点
* 注意像素坐标的坐标系,Row是轴向Y,Col是轴向X,，这里排列的是像素坐标，即以row\col轴为坐标系
* 四边形排序以左上角为起点，按顺时针方向排列

WordPointX := [50,50,(BordCornerColNum-1)*BoradLength,(BordCornerColNum-1)*BoradLength]
WordPointY := [50,(BordCornerRowNum-1)*BoradLength,(BordCornerRowNum-1)*BoradLength,50]
WordPointZ := [1,1,1,1]

* 使用给定的点对应计算齐次变换矩阵。
* 从图像中的点，映射到正常图像的变换矩阵
hom_vector_to_proj_hom_mat2d (ImageCornersX, ImageCornersY, ImageCornersZ, WordPointX, WordPointY, WordPointZ, 'normalized_dlt', HomMat2D)

projective_trans_image (GrayImage, TransImage, HomMat2D, 'bilinear', 'true', 'false')


* 判断P1点左上侧的灰度值，如果是黑格，则P1点变为P3，依然按逆时针取P3P4P1P2,再重新计算透视变换矩阵
* 更换用外边框做角点，对应的P1点灰度值要取右下方的
projective_trans_pixel (HomMat2D, P1[0], P1[1], P11, P12)
get_grayval (TransImage, P12, P11, Grayval1)
get_grayval (TransImage, P12-6, P11-6, Grayval)
get_grayval (TransImage, P12-6, P11+8, Grayval2)
gen_cross_contour_xld (Cross, P12-6, P11-6, 6, 0.785398)
gen_cross_contour_xld (Cross2, P12-6, P11+6, 6, 0.785398)
gen_cross_contour_xld (Cross100, P12, P11, 6, 0.785398)
dev_display (TransImage)
dev_display (Cross)
dev_display (Cross2)
dev_display (Cross100)


 *如果左边格子灰度值小于右边格子，即左边格子为黑色
 if (Grayval<Grayval2)
    ImageCornersX := [P3[0],P4[0],P1[0],P2[0]]
    ImageCornersY := [P3[1],P4[1],P1[1],P2[1]]
    ImageCornersZ := [1,1,1,1]
    hom_vector_to_proj_hom_mat2d (ImageCornersX, ImageCornersY, ImageCornersZ, WordPointX, WordPointY, WordPointZ, 'normalized_dlt', HomMat2D)
    projective_trans_image (GrayImage, TransImage, HomMat2D, 'bilinear', 'true', 'false')
endif
 



* 将原无序点，通过透视变换矩阵，矫正到新的图像上
projective_trans_pixel (HomMat2D, Row, Col, RowTrans, ColTrans)

r1 := sort(RowTrans)
c1 := sort(ColTrans)

* 根据从上到下，从左到右的原则拿到重新排列后的索列号
tuple_sort_index (RowTrans, Indices)
selectRow := RowTrans[Indices]
selectCol := ColTrans[Indices]

SelectX := []
SelectY := []
for Index22 := 0 to BordCornerColNum-1 by 1
    rows := selectRow[Index22*BordCornerRowNum:Index22*BordCornerRowNum+BordCornerRowNum-1]
    cols := selectCol[Index22*BordCornerRowNum:Index22*BordCornerRowNum+BordCornerRowNum-1]
    tuple_sort_index (cols, Indices1)
    SelectX := [SelectX,rows[Indices1]]
    SelectY := [SelectY,cols[Indices1]]

endfor

hom_mat2d_invert (HomMat2D, HomMat2DInvert)
projective_trans_pixel (HomMat2DInvert, SelectX, SelectY, RowTrans1, ColTrans1)

3、计算棋盘格在相机中的姿态

    gen_cam_par_area_scan_division (0.008, 0, 5.2e-006, 5.2e-006, 640, 512, 1280, 1024, CameraParam)
    vector_to_pose (x, y, z, RowTrans1, ColTrans1, Left_CameraParam_Intri1, 'analytic', 'error', Pose, Error2)

最终把棋盘格在相机坐标系中的位置，输入到标定句柄中

 set_calib_data_observ_points (CalibDataID, 0, 0, I, RowTrans1, ColTrans1, 'all', Pose)