Halcon例程分析11：物体位姿检测

打开halcon，按下ctrl+e打开halcon自带例程。方法->模板匹配（基于描述符）->locate_cookie_box.hdev

* This example shows how to find a cookie box aligned in different
* directions with the descriptor based matching feature. First, a
* model and a reference pose are computed from the reference
* image, then the model is searched in a sequence of images
* showing the cookie box in different orientations. Using the
* resulting pose we can also check whether the box is placed
* upside down or not.
* 
* 
*重新创建特征点模型变量，如果为true则每次运行都对ROI进行特征点提取
ReCreateDescrModel := false
* 
dev_update_off ()
dev_close_window ()
*读取图像
read_image (Image, 'packaging/cookie_box_01')
dev_open_window_fit_image (Image, 0, 0, -1, -1, WindowHandle)
set_display_font (WindowHandle, 14, 'mono', 'true', 'false')
dev_set_draw ('margin')
dev_set_line_width (4)
dev_set_color ('blue')
* 
*由摄像机内参生成摄像机参数，实际项目中需要根据自己的相机进行标定校准
gen_cam_par_area_scan_division (0.0155565, -109.42, 1.28008e-005, 1.28e-005, 322.78, 240.31, 640, 480, CamParam)
* 
*
*感兴趣区域变量定义，四个点都是图像坐标(224,115),(224,540),(406,540),(406,115)左上角开始顺时针数
RowsRoi := [224,224,406,406]
ColumnsRoi := [115,540,540,115]
*世界坐标系中相对应的点坐标，后面除1000是转换单位，与以上坐标一一对应(-189,-80),(189,-80),(189,80),(-189,80)左上角开始，顺时针
*世界坐标系的中心在盒子的正中心位置，向右是X轴，向下是Y轴z轴在盒子前平面上
WorldX := [-189,189,189,-189] / 1000.0
WorldY := [-80,-80,80,80] / 1000.0
* reference pose of the object to the camera is computed
*计算盒子位姿与图像的关系，输出参数pose即为两者间的关系，Quality是对位姿的一个评价
vector_to_pose (WorldX, WorldY, [], RowsRoi, ColumnsRoi, CamParam, 'iterative', 'error', Pose, Quality)
*根据上面映射出来的关系Pose，算出图像上某点对应的世界坐标，这么做的意义是什么？ 没什么，就是单纯验证一下而已
image_points_to_world_plane (CamParam, Pose, RowsRoi, ColumnsRoi, 'm', XOuterBox, YOuterBox)
* generate ROI for model training
*通过两个角点生成水平矩形
gen_rectangle1 (Rectangle, 224, 115, 406, 540)
*抠出矩形区域的图像
reduce_domain (Image, Rectangle, ImageReduced)
* 
* If desired re-create the descriptor-based model. Note that this may take
* several minutes dependent on the hardware.
dev_display (Image)
if (ReCreateDescrModel)
    dev_display (Rectangle)
    disp_message (WindowHandle, 'Creating the descriptor-based model ... \n(may take a minute)', 'window', 10, 10, 'black', 'true')
    count_seconds (SecondsCreation1)
    *生成一个透视变换特征点模型，要把当前物体的姿态Pose输入进去作为参考位姿
    create_calib_descriptor_model (ImageReduced, CamParam, Pose, 'harris_binomial', [], [], ['depth','number_ferns','patch_size','min_scale','max_scale'], [11,30,17,0.4,1.2], 42, ModelID)
    count_seconds (SecondsCreation2)
    TimeCreation := SecondsCreation2 - SecondsCreation1
    *把模型写入文件方便下次调用，不然每次都要创建模型
    write_descriptor_model (ModelID, 'cookie_box_model.dsm')
else
    * If desired read the descriptor-based model from disk
    disp_message (WindowHandle, 'Reading the descriptor-based model file from disk ...\n ... will take a few seconds!', 'window', 10, 10, 'black', 'true')
    dev_set_check ('~give_error')
    dev_error_var (ErrorVar, 1)
    *把前面存储的模型读出使用
    read_descriptor_model ('cookie_box_model.dsm', ModelID)
    Error := ErrorVar
    dev_set_check ('give_error')
    if (Error != H_MSG_TRUE)
        *如果没有成功读出模型，即模型还未被创建则执行以下代码创建模型
        * Create the descriptor-based model if it's not available on disk
        dev_display (Image)
        dev_display (Rectangle)
        disp_message (WindowHandle, 'Reading the descriptor-based model file from disk  ... \n... failed!', 'window', 10, 10, 'black', 'true')
        disp_message (WindowHandle, 'Creating the descriptor-based model (may take a minute)', 'window', 48, 10, 'black', 'true')
        * 
        count_seconds (SecondsCreation1)
        *创建模型
        create_calib_descriptor_model (ImageReduced, CamParam, Pose, 'harris_binomial', [], [], ['depth','number_ferns','patch_size','min_scale','max_scale'], [11,30,17,0.4,1.2], 42, ModelID)
        count_seconds (SecondsCreation2)
        TimeCreation := SecondsCreation2 - SecondsCreation1
        * Ignore writing errors as they are not critical for this example
        dev_set_check ('~give_error')
        write_descriptor_model (ModelID, 'cookie_box_model.dsm')
        dev_set_check ('give_error')
        ReCreateDescrModel := true
    endif
endif
* 
* 
if (ReCreateDescrModel)
    dev_display (Image)
    disp_message (WindowHandle, 'One model created in ' + TimeCreation$'.03f' + ' seconds.', 'window', 10, 10, 'black', 'true')
    disp_continue_message (WindowHandle, 'black', 'true')
    stop ()
endif
* 
* 
* Main loop:
* search model in image sequence
*总共10张测试图像
for Index := 1 to 10 by 1
    OutputString := []
    read_image (Image, 'packaging/cookie_box_' + Index$'.02')
    dev_display (Image)
    * 
    * find model (using default parameters)
    count_seconds (Seconds1)
    *在图像中查找模板，得到这幅图像的位姿输出Pose,Score是评价得分
    find_calib_descriptor_model (Image, ModelID, [], [], [], [], 0.25, 1, CamParam, 'num_points', Pose, Score)
    count_seconds (Seconds2)
    Time := Seconds2 - Seconds1
    * 
    * display results
    if (|Score| > 0)
        *获取前面创建的模型的特征点坐标，Row ,Col是数组
        get_descriptor_model_points (ModelID, 'search', 0, Row, Col)
        dev_set_colored (12)
        dev_set_line_width (1)
        *通过上面特征点的坐标生成小十字叉做显示用
        gen_cross_contour_xld (Cross1, Row, Col, 6, 0.785398)
        dev_display (Cross1)
        dev_set_color ('lime green')
        dev_set_line_width (3)
        *画坐标系，这个Pose位置是跟模型有关的，模型中存有坐标系原点的信息
        disp_3d_coord_system (WindowHandle, CamParam, Pose, 0.1)
        * 
        *把位姿转为3维旋转矩阵，包括旋转和平移
        pose_to_hom_mat3d (Pose, HomMat3D)
        *通过位姿转换矩阵计算出当前图像对应的四个点所在的世界坐标，即以刚才创建模板时建立的坐标系参考，现在这个被转动了的盒子对应的4个点当前的坐标位置
        *即刚才绘制矩形的这四个点现在被HomMat3D这个矩阵转到哪去了
        affine_trans_point_3d (HomMat3D, XOuterBox, YOuterBox, [0,0,0,0], XTrans, YTrans, ZTrans)
        *世界坐标映射到图像中，因为我们的相机位置是不变的，那现在要框出被转动了的物体，那肯定要知道当前物体在图像中的位置
        project_3d_point (XTrans, YTrans, ZTrans, CamParam, RowTrans, ColTrans)

        * 
        * 
        gen_contour_polygon_xld (Contour, RowTrans, ColTrans)
        close_contours_xld (Contour, Contour)
        dev_display(Image)
        dev_display (Contour)
        dev_set_color('red')
        disp_3d_coord_system (WindowHandle, CamParam, Pose, 0.07)
        * 
        * determine direction of box
        *通过位姿判断盒子的状态，有没有放倒之类的
        if ((Pose[5] > 45 and Pose[5] < 135) or (Pose[5] > 225 and Pose[5] < 315))
            OutputString := 'Box label found in ' + (Time * 1000)$'.4' + ' ms (on the side)'
        elseif (Pose[5] > 135 and Pose[5] < 225)
            OutputString := 'Box label found in ' + (Time * 1000)$'.4' + ' ms (upside down)'
        else
            OutputString := 'Box label found in ' + (Time * 1000)$'.4' + ' ms'
        endif
    endif
    if (|Score| == 0)
        OutputString := 'No label found'
    endif
    disp_message (WindowHandle, OutputString, 'window', 10, 10, 'black', 'true')
    disp_continue_message (WindowHandle, 'black', 'true')
    stop ()
endfor
dev_display (Image)
disp_message (WindowHandle, 'Program finished.\nPress \'Run\' to clear all descriptor models.', 'window', 10, 10, 'black', 'true')
stop ()
dev_update_on ()

物体位姿状态原图，箭头指示个方向信息

检测物体位姿状态