halcon 位姿估计代码

halcon 位姿估计代码

基于形状匹配的位姿估计代码，将位姿以文本形式进行保存。队原始的图像进行了处理，去除纹理信息，保存关键边缘信息。

ReCreateShapeModel3D := false
* delete_file('F:/lb/halcon3d/obj_000003/old_model.sm3')
* ReCreateShapeModel3D := true
* Calibration 01: Code generated by Calibration 01
* CamParam := [0.0163392,-778.343,2.99949e-06,3e-06,925.576,553.982,1920,1080]
* CameraPose := [0.0117945,0.00286742,0.162576,1.37971,355.714,143.877,0]
CamParam := [0.0018522, 0, 3e-6, 3e-6,323.8841552734375, 243.9881591796875,640, 480 ]
Width := 640
Height := 480
dev_close_window ()
dev_open_window (0, 0, Width, Height, 'black', WindowHandle)
set_display_font (WindowHandle, 14, 'mono', 'true', 'false')
dev_update_off ()
read_object_model_3d ('model.ply', 'm', [], [], ObjectModel3DID, DxfStatus)
prepare_object_model_3d (ObjectModel3DID, 'shape_based_matching_3d', 'true', [], [])
list_image_files('', 'bmp', [], ImageFiles)
read_image(Images, ImageFiles)

inspect_object_model_3d (Images, ObjectModel3DID, CamParam, RefRotX, RefRotY, RefRotZ, LongitudeMin, LongitudeMax, LatitudeMin, LatitudeMax, CamRollMin, CamRollMax, DistMin, DistMax, MinFaceAngle)

dev_clear_window ()
 if (ReCreateShapeModel3D)
    Message := 'Creating the 3D shape model (may take several minutes) ...'
    disp_message (WindowHandle, Message, 'window', 12, 12, 'black', 'true')
    create_shape_model_3d (ObjectModel3DID, CamParam, RefRotX, RefRotY, RefRotZ, 'gba', LongitudeMin, LongitudeMax, LatitudeMin, LatitudeMax, -rad(180), rad(180), DistMin, DistMax, 10, 'min_face_angle', MinFaceAngle, ShapeModel3DID)
    write_shape_model_3d (ShapeModel3DID, 'old_model.sm3')
else
    Message := 'Reading the 3D shape model file from disk ...'
    disp_message (WindowHandle, Message, 'window', 12, 12, 'black', 'true')
    dev_set_check ('~give_error')
    dev_error_var (ErrorVar, 1)
    read_shape_model_3d ('old_model.sm3', ShapeModel3DID)
    Error := ErrorVar
    dev_set_check ('give_error')
   if (Error != H_MSG_TRUE)
        Message := 'Reading the 3D shape model file from disk ... not found!'
        Message[1] := 'Creating the 3D shape model (may take several minutes) ...'
        disp_message (WindowHandle, Message, 'window', 12, 12, 'black', 'true')
        create_shape_model_3d (ObjectModel3DID, CamParam, rad(0), 0, rad(0), 'gba', -rad(180), rad(180), -rad(90), rad(90), rad(0), rad(360), 0.1, 0.3, 10, [], [], ShapeModel3DID)
*         create_shape_model_3d (ObjectModel3DID, CamParam, rad(180), 0, rad(90), 'gba', -rad(35), rad(35), -rad(35), rad(35), 0, rad(360), 0.2, 0.25, 10, [], [], ShapeModel3DID)
         write_shape_model_3d (ShapeModel3DID, 'old_model.sm3')
    endif
     read_shape_model_3d ('old_model.sm3', ShapeModel3DID)
 endif

dev_set_line_width (2)
list_files('rgb_gaussian','files', Files)
len:=|Files|

for ImageNo := 0 to len-1 by 1
    imgname:=Files[ImageNo]
    read_image (Image, Files[ImageNo])
    dev_display (Image)
    count_seconds (Seconds1)
    find_shape_model_3d (Image, ShapeModel3DID, 0.9, 0.8, 10, ['num_matches','pose_refinement'], [1,'least_squares_very_high'], Pose, CovPose, Score)
    count_seconds (Seconds2)
    Time := Seconds2 - Seconds1
    Message := |Score| + ' Match(es) found in ' + Time$'4.3f' + ' s'
    disp_message (WindowHandle, Message, 'window', 12, 12, 'black', 'true')
    Time := Seconds2 - Seconds1
    for I := 0 to |Score| - 1 by 1
        PoseI := Pose[I * 7:I * 7 + 6]
        CovPoseI := CovPose[I * 6:I * 6 + 5]
        ScoreI := Score[I]
        project_shape_model_3d (ModelContours, ShapeModel3DID, CamParam, PoseI, 'true', 0.523599)
        dev_set_color ('blue')
        dev_display (ModelContours)
        dev_set_color ('coral')
        disp_3d_coord_system (WindowHandle, CamParam, PoseI, 0.015)
        dev_set_color ('magenta')
        display_match_pose (ShapeModel3DID, PoseI, WindowHandle)
        pose_to_hom_mat3d(PoseI, HomMat3D)
        parse_filename( imgname , BaseName, Extension, Directory)
*         if (ImageNo < 10)
*             id:='0000'     
*         elseif (ImageNo>=10 and ImageNo<100)
*             id:='000'     
*         elseif (ImageNo>=100 and ImageNo<1000)
*             id:='00'
*         elseif (ImageNo>=1000 and ImageNo<10000)
*             id:='0'
*         elseif (ImageNo>10000)
*             id:=''
*         endif
        
        open_file ('./poses-martix/'+BaseName+'.txt', 'append', martixFileHandle)
        for i:=0 to 11 by 1
            fwrite_string (martixFileHandle, HomMat3D[i])
            fwrite_string (martixFileHandle, ' ')
        endfor
        
        open_file ('./poses-angle/'+BaseName+'.txt', 'append', posesFileHandle)
        for j:=0 to |Pose|-2 by 1
            fwrite_string (posesFileHandle, PoseI[j])
            fwrite_string (posesFileHandle, ' ')
        endfor
        
        dump_window_image (Image1, WindowHandle)
        write_image (Image1, 'png', 0,'./results/'+BaseName)

        
    endfor
*     disp_continue_message (WindowHandle, 'black', 'true')
      wait_seconds(10)
endfor
* 
Message := 'Program terminated. Press \'Run\' to clear the 3D shape model.'
disp_message (WindowHandle, Message, 'window', Height - 30, 12, 'black', 'true')
* 
stop ()
dev_update_on ()