管件接头的无序抓取

1，目的

获取管件堆中管件接头的位姿，从而实现无序抓取。

效果如下：

2，过程

• 多相机获取多视角的图像，这里是四个相机获取四个角度的图像。
• 结合四个角度图像进行表面点云重建，获取图像中管件堆的点云。
• 进而再使用3D表面匹配实现对管道接头位姿的获取。

3，易混易错点

1， 通过标定获取的CameraPose实质是标定板在相机坐标系的位姿，并不是相机的位姿。相机的位姿其实是CameraPose的逆变换。

calibrate_cameras (CalibHandle, TmpCtrl_Errors)
get_calib_data (CalibHandle, 'camera', 0, 'params', CameraParameters)
get_calib_data (CalibHandle, 'calib_obj_pose', [0, >TmpCtrl_ReferenceIndex], 'pose', CameraPose)
* Calibration 01: Adjust origin for plate thickness
set_origin_pose (CameraPose, 0.0, 0.0, 0.001, CameraPose)

2，disp_3d_coord_system算子中的仿射变换，并不是使标定板坐标系原点与相机坐标系原点重合，而是为了在相机坐标系中‌可视化标定板坐标系的位置和方向，标定板坐标系原点与相机坐标系原点位置关系并不发生改变‌。 该算子直接使用CameraPose参数将标定板坐标系渲染到图像空间，保持其相对于相机坐标系的原始位姿关系。

3，pose_compose (Pose1,Pose2, PoseCompose)执行顺序是先执行Pose2变换，再执行 Pose1 变化。参数是有执行顺序，并不可以随意互换。
例如以下：

正确的合并：

pose_compose (BaseInCamPose, ToolInBasePose, ToolInCamPose)
pose_compose (ToolInCamPose, CalObjInToolPose, CalObjInCamPose)

错误的合并

pose_compose (ToolInBasePose,BaseInCamPose,  ToolInCamPose)
pose_compose ( CalObjInToolPose,ToolInCamPose, CalObjInCamPose)

4，代码详解

4.1，初始化窗口

* 案例库：locate_pipe_joints_stereo.hdev

* 目的：
* 查找立体视觉获取的3D对象中管道连接头的立体位置

* 过程：
* 采用四个相机采集的图像立体重建一堆管道连接头的三维表面

* 基于表面的3D匹配，查找
* 
* 
*---------------------part01:初始化窗口
* *****
* Initializations:
* *****
* 
dev_close_window ()
dev_update_off ()
dev_get_preferences ('suppress_handled_exceptions_dlg', PreferenceValue)
dev_set_preferences ('suppress_handled_exceptions_dlg', 'true')
ImagePath := '3d_machine_vision/multi_view/'
ImagePrefix := 'multi_view_pipe_joints'
read_image (Image, ImagePath + ImagePrefix + '_cam_0_01')
get_image_size (Image, Width, Height)
dev_open_window_fit_image (Image, 0, 0, -1, -1, WindowHandle)
dev_set_draw ('margin')
set_display_font (WindowHandle, 16, 'mono', 'true', 'false')
* 
* *****

4.2，创建多视角立体视觉模型。

*-------------------- Part02: 多视角立体视觉模型创建
* *****
* 2.1，Read the camera setup model from file and get the parameters
* and the poses of the cameras
try
    read_camera_setup_model ('../four_camera_setup_model.csm', CameraSetupModelID)
catch (Exception)
    if (Exception[0] == 5200)
        * 初始化生成相机内参+外参
        
        init_camera_setup (CameraSetupModelID)
        
    else
        throw (Exception)
    endif
endtry
* 
* 2.2 ，获取多视角立体视觉模型参数
get_camera_setup_param (CameraSetupModelID, 'general', 'num_cameras', NumCameras)
* 需要特别注意:结合上下文可知Pose0应该是常见标定获取的camerapose的逆变换
* （camerapose为标定板在相机坐标系的位姿）
get_camera_setup_param (CameraSetupModelID, 0, 'pose', Pose0)
get_camera_setup_param (CameraSetupModelID, 1, 'pose', Pose1)
get_camera_setup_param (CameraSetupModelID, 2, 'pose', Pose2)
get_camera_setup_param (CameraSetupModelID, 3, 'pose', Pose3)
get_camera_setup_param (CameraSetupModelID, 0, 'params', CamParam0)
get_camera_setup_param (CameraSetupModelID, 1, 'params', CamParam1)
get_camera_setup_param (CameraSetupModelID, 2, 'params', CamParam2)
get_camera_setup_param (CameraSetupModelID, 3, 'params', CamParam3)

* 
* Create a multi-view stereo model, initialize it, and clear
* the camera setup, which is no longer required

* 2.3，创建双目立体视觉。
* 'surface_pairwise':基于成对图像进行表面重建
create_stereo_model (CameraSetupModelID, 'surface_pairwise', [], [], StereoModelID)
clear_camera_setup_model (CameraSetupModelID)

* 2.4，多视角立体视觉模型参数设置

*-----图像校正参数组

* -> Subsampling X, Y, Z
* 设置下采样步长为3，可降低计算量但可能导致锯齿效应，需配合抗锯齿参数使用
set_stereo_model_param (StereoModelID, 'sub_sampling_step', 3)
* -> Interpolation aliasing by binocular image rectification
* 双线性插值('bilinear')，平衡校正图像的质量与计算效率
set_stereo_model_param (StereoModelID, 'rectif_interpolation', 'bilinear')
* 子采样系数1.2，轻微降低分辨率以加速处理
set_stereo_model_param (StereoModelID, 'rectif_sub_sampling', 1.2)

* -----双目视差计算参数组

* 采用归一化互相关('ncc')匹配算法，对光照变化鲁棒性强
set_stereo_model_param (StereoModelID, 'binocular_method', 'ncc')
* 视差计算的金字塔层级数，设置为1时单层直接计算，应用于高纹理/实时性要求高的场景
set_stereo_model_param (StereoModelID, 'binocular_num_levels', 1)
* 匹配掩模尺寸19×19像素，适合中等纹理场景
set_stereo_model_param (StereoModelID, 'binocular_mask_width', 19)
set_stereo_model_param (StereoModelID, 'binocular_mask_height', 19)
* 纹理阈值设为0，禁用纹理过滤，适用于低纹理场景
set_stereo_model_param (StereoModelID, 'binocular_texture_thresh', 0)
* 匹配分数阈值0.4，过滤低置信度匹配点
set_stereo_model_param (StereoModelID, 'binocular_score_thresh', 0.4)
* 启用左右一致性检查('left_right_check')，消除遮挡区域误匹配
set_stereo_model_param (StereoModelID, 'binocular_filter', 'left_right_check')
* 子像素优化模式为插值法('interpolation')，提升深度分辨率
set_stereo_model_param (StereoModelID, 'binocular_sub_disparity', 'interpolation')

*------ 空间约束和相机约束

* 定义3D工作空间为[-0.2,-0.07,-0.075]到[0.2,0.07,-0.004]（单位：米），自动计算视差范围
* 边界框（bounding_box）可约束重建范围，提升效率
set_stereo_model_param (StereoModelID, 'bounding_box', [-0.2,-0.07,-0.075,0.2,0.07,-0.004])
* 配置相机对为(0,2)和(1,3)，支持多相机阵列的灵活组合
set_stereo_model_image_pairs (StereoModelID, [0,2], [1,3])

释疑解惑：
1，算子create_stereo_model 。

• 功能概述：
  是 HALCON 中用于创建立体视觉模型的操作符，主要用于从多视角校准的相机配置中重建 3D 点或表面。其核心功能包括：
  ‌3D 点重建‌：通过多幅校准图像中的点对应关系，计算 3D 点坐（Method='points_3d'）。
  表面重建‌：基于立体图像对生成视差图，进而重建表面（Method='surface_pairwise' 或 'surface_fusion'）。

• 参数详解

  • 输入参数
    • ‌ CameraSetupModelID ：校准后的多视角相机配置模型句柄，包含相机内外参等信息。
  • Method：指定重建类型
    • 'points_3d'：用于 3D 点云重建。
    • 'surface_pairwise'：基于成对图像重建表面。
    • 'surface_fusion'：融合多视角数据重建表面。
  • 输出参数
    • ‌StereoModelID‌：生成的立体模型句柄，用于后续操作（如 reconstruct_surface_stereo）。

• 典型工作流程

  • 创建模型‌ ```

  create_stereo_model(CameraSetupModelID, 'surface_pairwise', [], [], StereoModelID)
  

创建表面重建模型，初始化参数为空列表。

• ‌配置参数‌
  通过 set_stereo_model_param 设置关键参数：

  • 'sub_sampling_step'：控制重建分辨率（如 3 表示每 3 个像素采样一次）。
  • 'rectif_interpolation'：指定图像矫正插值方法（如 'bilinear'）。

• ‌执行重建‌
  调用 reconstruct_surface_stereo 生成 3D 表面模型。

• 应用场景

  1. ‌工业检测‌：如管道接头的多视角 3D 重建，通过 4 相机系统生成高精度表面模型。

  2. ‌机器人导航‌：结合手眼标定，将重建的 3D 点云转换到机器人坐标系。

• 注意事项

  • ‌相机标定‌：需提前完成多相机标定，确保 CameraSetupModelID 参数准确。

  • ‌图像对选择‌：使用 set_stereo_model_image_pairs 指定有效的图像对索引（如 [0,2], [1,3]）以优化视差计算。

• 错误处理

  • 若 Method 与模型类型不匹配（如对 'points_3d' 模型调用表面重建），会触发错误。

  • 无效的相机索引或未校准参数会导致重建失败。

2，set_stereo_model_image_pairs (StereoModelID, [0,2], [1,3])作用？

​ 指定用于表面重建的立体图像对‌，其核心原理和必要性如下：

• 函数功能与参数含义

  1. ‌功能定位‌
     该函数用于为 'surface_pairwise' 或 'surface_fusion' 类型的立体模型（StereoModelID）配置图像对列表，通过视差计算实现表面重建。若模型类型不匹配（如 'points_3d'），则会报错。

     • 参数解析：
       • [0,2] 表示第一组图像对的左视图索引为0，右视图索引为2；
       • [1,3] 表示第二组图像对的左视图索引为1，右视图索引为3。
         这些索引需在相机标定模型（CameraSetupModelID）的有效范围内。

  2. 多视角重建流程‌
     该函数是重建流程的关键步骤之一，需在 create_stereo_model 创建模型后调用，并在 reconstruct_surface_stereo 执行前完成图像对配置。

• 图像对选择的依据

  1. ‌视差计算需求‌
     多视角重建通过计算图像对的视差图生成3D表面。例如，[0,2] 表示从相机0到相机2的视差计算，覆盖不同视角的几何关系。
     • ‌优化策略‌：选择交会角适中（如30°-60°）、特征匹配数量多的图像对，可提升重建精度。
  2. 覆盖性与效率平衡‌
     1. ‌覆盖性‌：[0,2] 和 [1,3] 的组合可能覆盖场景的不同区域，避免单一视角的盲区。
     2. ‌效率‌：减少冗余图像对（如相邻视角）可降低计算量，但需保证重建完整性。

• 参数设置示例分析

  1. ‌ 相机布局假设
     • 相机0和2、1和3可能分别位于场景的左右两侧，形成交叉基线，增强深度感知。
  2. 视差多样性
     • 不同基线的图像对（如短基线0-1和长基线0-2）可兼顾细节与深度范围。
     • 注意事项
       1. 参数验证
          • 相机索引需与标定模型一致，否则会报错。
       2. 重建质量优化
          • 结合 set_stereo_model_param 设置边界框（bounding_box）可约束重建范围，提升效率。

• 总结
  设置 set_stereo_model_image_pairs 的核心目的是 ‌通过合理配置图像对，优化视差计算与表面重建的精度和效率‌。参数 [0,2], [1,3] 体现了多视角立体视觉中交叉基线、覆盖性与计算资源的平衡设计。

4.3，创建表面匹配模型

* --------------------Part03: 创建表面匹配模型
* *****
* 3.1，读取管道接头3D对象
read_object_model_3d ('pipe_joint', 'm', [], [], PipeJointOM3DID, Status)
create_surface_model (PipeJointOM3DID, 0.03, [], [], PipeJointSMID)

* 3.2，预处理匹配模型，提高匹配效率
* 'shape_base _matching_3d':激活基于表面集合特征的匹配模式，适用于管道接头等工业零件的定位
* 为true时进行法向量计算，曲率特征提取，关键点自动选择，提高匹配效率
prepare_object_model_3d (PipeJointOM3DID, 'shape_based_matching_3d', 'true', [], [])
* 
* *****

PipeJointOM3DID

4.4，多视角立体视觉重建管件堆表面模型

*-------------------Part04:多视角立体视觉重建管件堆表面模型

* 4.1，设置多视角立体模型重建表面3D模型所需必要参数
* 重建管件堆的表面，并利用基于表面的三维匹配技术确定多个管道接头的位置。
* *****
NumMatches := 3
MinScore := .3

* pose_ref_scoring_dist_rel：HALCON中用于曲面匹配（Surface-Based Matching）的关键参数，主要控制精细化姿态优化阶段的评分距离容差
* 该参数以相对值形式（相对于物体尺寸）设置匹配评分时的最大允许距离偏差
Params := ['num_matches','pose_ref_scoring_dist_rel','scene_normal_computation']
Values := [NumMatches,0.02,'mls']
Instructions[0] := 'Rotate: Left button'
Instructions[1] := 'Zoom:   Shift + left button'
Instructions[2] := 'Move:   Ctrl  + left button'
* 
* 注意：这里的Pose0是相机在世界坐标系的位姿，不是常见标定产生的标定板在相机坐标系的位姿camerapose
pose_invert (Pose0, WorldPose0)
* 
gen_empty_obj (EmptyObject)
NumImages := 15
for Index := 1 to NumImages by 1
    * 
    * 4.2，读取重建多视角立体表面模型所需的图像
    read_multi_view_stereo_images (Images, ImagePath, ImagePrefix, Index, NumCameras)
    * 
    * Reconstruct the 3D scene (the pile of pipe fittings)
    Message := 'Performing the reconstruction...'
    * 4.3，显示表面重建所用的图像
    display_multi_view_stereo_images (Images, WindowHandle)

Images

本地函数：

1,read_multi_view_stereo_images

* Read the images of the multi-view stereo setup
* 
read_image (Images, ImagePath + ImagePrefix + '_cam_0_' + SceneIndex$'.02')
for Index := 1 to NumCamera - 1 by 1
    read_image (Img, ImagePath + ImagePrefix + '_cam_' + Index + '_' + SceneIndex$'.02')
    concat_obj (Images, Img, Images)
endfor
return ()

2，display_multi_view_stereo_images

* Display the images of a multi-view stereo setup
* consisting of four cameras
* 
dev_set_window (WindowHandle)
* 
count_obj (Images, NumImages)
if (NumImages != 4)
    disp_message (WindowHandle, 'Wrong number of images provided!', 'window', 12, 12, 'black', 'true')
    stop ()
endif
* 
NumCols := 2
select_obj (Images, Img, 1)
get_image_size (Img, Width, Height)
tile_images (Images, TiledImage, NumCols, 'horizontal')
dev_set_part (0, 0, (2 * Height) - 1, (2 * Width) - 1)
dev_display (TiledImage)
* 
for Index := 0 to NumImages - 1 by 1
    RowIdx := (Index / NumCols) + 1
    ColIdx := Index % NumCols
    disp_message (WindowHandle, 'Camera ' + Index, 'image', (RowIdx * Height) - 72, (ColIdx * Width) + 12, 'white', 'false')
endfor
return ()
* 

 disp_message (WindowHandle, Message, 'window', 12, 12, 'black', 'true')
    count_seconds (T0)
   
   ---
    * 4.4，多视角立体模型重建表面3D模型
    reconstruct_surface_stereo (Images, StereoModelID, PipeJointPileOM3DID)
    count_seconds (T1)
    * 重建所用时间
    ReconsTime := T1 - T0
    * 
    PoseIn := [0.0,0.0,0.5,-30,0,180,0]
    * 4.5，显示重建的3D模型   
    if (Index == 1)
        visualize_object_model_3d (WindowHandle, PipeJointPileOM3DID, CamParam0, PoseIn, ['color','point_size'], ['yellow',1], 'Reconstructed scene in ' + ReconsTime$'.3' + ' s', [], Instructions, PoseOut)
    endif

PipeJointPileOM3DID

4.5，管道接头查找匹配

 * Perform surface-based 3D matching
    Message := 'Search ' + NumMatches + ' best parts with surface based matching...'
    disp_message (WindowHandle, Message, 'window', 36, 12, 'black', 'true')
    count_seconds (T2)
    
    * 4.6，表面匹配查找
    find_surface_model (PipeJointSMID, PipeJointPileOM3DID, 0.03, 0.05, MinScore, 'false', Params, Values, Poses, Scores, SurfaceMatchingResultID)
    count_seconds (T3)
    * 表面匹配耗时：
    MatchingDiff := T3 - T2
    * 
    * Display the results
    
    * 4.7, 禁用形输出的实时刷新，将多个图形操作缓存后统一显示，可有效解决界面闪烁问题   
    set_system ('flush_graphic', 'false')
    select_obj (Images, Img, 1)
    dev_set_part (0, 0, Height - 1, Width - 1)
    dev_display (Img)
*     count_seconds (T4)
    for MatchIndex := 0 to |Scores| - 1 by 1
        * 管道接头在世界坐标系中位姿
        
        PoseObjInWorld := Poses[MatchIndex * 7:(MatchIndex * 7) + 6]
        * 世界坐标系在对象坐标系的位姿
*         rigid_trans_object_model_3d (PipeJointOM3DID, PoseObjInWorld, ObjectModel3DRigidTrans1)
*         visualize_object_model_3d (WindowHandle, [PipeJointPileOM3DID,ObjectModel3DRigidTrans1], [], [],\
                                     ['color_0','color_1','alpha_1'], ['gray','green',0.5], [], [], [], PoseOut1)
        
        pose_invert (PoseObjInWorld, PoseWorldInObj)
        * 相机在世界坐标系的位姿+世界坐标系在对象坐标系的位姿=相机坐标系在对象坐标系的位姿        
        pose_compose (PoseWorldInObj, Pose0, PoseCamInObj)   
        *对象在相机坐标系中的位姿 
        pose_invert (PoseCamInObj, ObjPoseInCam0)
        * Display the coordinate system of the part
        dev_set_colored (3)
        dev_set_line_width (3)
        
        *  4.8,显示匹配到的管道接头的坐标系        
        disp_3d_coord_system (WindowHandle, CamParam0, ObjPoseInCam0, 0.03)
        * Display the faces of the part
        rigid_trans_object_model_3d (PipeJointOM3DID, PoseObjInWorld, ObjectModel3DRigidTrans)
        
        * 4.9，投影到平面形成轮廓
        * 'data':'face',指定投影时处理模型的三角面片(faces)而非原始点云数据
        * 'hidden_surface_removal':'true',使用隐藏面移除技术，消除被遮挡的不可见面，提升投影结果的真实感  
        project_object_model_3d (ModelContours, ObjectModel3DRigidTrans, CamParam0, WorldPose0, ['data','hidden_surface_removal'], ['faces','true'])
        dev_set_line_width (2)
        dev_set_color ('green')
        dev_display (ModelContours)
        * 
        clear_object_model_3d (ObjectModel3DRigidTrans)
    endfor
    set_system ('flush_graphic', 'true')
*     count_seconds (T5)
*     DispTime := T5 - T4
    * 
    Message := '立体视觉重建耗时: ' + ReconsTime$'.2f' + ' s'
    Message[1] := '找到： ' + |Scores| + ' 管道接头表面匹配耗时： ' + MatchingDiff$'.2f' + ' s'
*     Message[2] := 'Visualization: ' + DispTime$'.1f' + ' s'
    disp_message (WindowHandle, Message, 'window', 12, 12, 'black', 'true')
    disp_message (WindowHandle, 'Camera 0', 'window', Height - 36, 12, 'white', 'false')
    if (Index < NumImages)
        disp_continue_message (WindowHandle, 'black', 'true')
        stop ()
    endif
    * 
    * clear the 3D object model
    clear_object_model_3d (PipeJointPileOM3DID)
    copy_obj (Images, OldImage, 1, 1)
endfor
* 

ModelContours

5，完整代码

* 案例库：locate_pipe_joints_stereo.hdev

* 目的：
* 查找立体视觉获取的3D对象中管道连接头的立体位置

* 过程：
* 采用四个相机采集的图像立体重建一堆管道连接头的三维表面

* 基于表面的3D匹配，查找
* 
* 
*---------------------part01:初始化窗口
* *****
* Initializations:
* *****
* 
dev_close_window ()
dev_update_off ()
dev_get_preferences ('suppress_handled_exceptions_dlg', PreferenceValue)
dev_set_preferences ('suppress_handled_exceptions_dlg', 'true')
ImagePath := '3d_machine_vision/multi_view/'
ImagePrefix := 'multi_view_pipe_joints'
read_image (Image, ImagePath + ImagePrefix + '_cam_0_01')
get_image_size (Image, Width, Height)
dev_open_window_fit_image (Image, 0, 0, -1, -1, WindowHandle)
dev_set_draw ('margin')
set_display_font (WindowHandle, 16, 'mono', 'true', 'false')
* 
* *****
*-------------------- Part02: 多视角立体视觉模型创建
* *****
* 2.1，Read the camera setup model from file and get the parameters
* and the poses of the cameras
try
    read_camera_setup_model ('../four_camera_setup_model.csm', CameraSetupModelID)
catch (Exception)
    if (Exception[0] == 5200)
        * 初始化生成相机内参+外参
        * 需要特别注意这里的外参是常见标定获取的camerapose的逆变换
        init_camera_setup (CameraSetupModelID)
        
    else
        throw (Exception)
    endif
endtry
* 
* 2.2 ，获取多视角立体视觉模型参数
get_camera_setup_param (CameraSetupModelID, 'general', 'num_cameras', NumCameras)
* 需要特别注意:结合上下文可知Pose0应该是常见标定获取的camerapose的逆变换
* （camerapose为标定板在相机坐标系的位姿）
get_camera_setup_param (CameraSetupModelID, 0, 'pose', Pose0)
get_camera_setup_param (CameraSetupModelID, 1, 'pose', Pose1)
get_camera_setup_param (CameraSetupModelID, 2, 'pose', Pose2)
get_camera_setup_param (CameraSetupModelID, 3, 'pose', Pose3)
get_camera_setup_param (CameraSetupModelID, 0, 'params', CamParam0)
get_camera_setup_param (CameraSetupModelID, 1, 'params', CamParam1)
get_camera_setup_param (CameraSetupModelID, 2, 'params', CamParam2)
get_camera_setup_param (CameraSetupModelID, 3, 'params', CamParam3)

* 
* Create a multi-view stereo model, initialize it, and clear
* the camera setup, which is no longer required

* 2.3，创建双目立体视觉。
* 'surface_pairwise':基于成对图像进行表面重建
create_stereo_model (CameraSetupModelID, 'surface_pairwise', [], [], StereoModelID)
clear_camera_setup_model (CameraSetupModelID)

* 2.4，多视角立体视觉模型参数设置

*-----图像校正参数组

* -> Subsampling X, Y, Z
* 设置下采样步长为3，可降低计算量但可能导致锯齿效应，需配合抗锯齿参数使用
set_stereo_model_param (StereoModelID, 'sub_sampling_step', 3)
* -> Interpolation aliasing by binocular image rectification
* 双线性插值('bilinear')，平衡校正图像的质量与计算效率
set_stereo_model_param (StereoModelID, 'rectif_interpolation', 'bilinear')
* 子采样系数1.2，轻微降低分辨率以加速处理
set_stereo_model_param (StereoModelID, 'rectif_sub_sampling', 1.2)

* -----双目视差计算参数组

* 采用归一化互相关('ncc')匹配算法，对光照变化鲁棒性强
set_stereo_model_param (StereoModelID, 'binocular_method', 'ncc')
* 视差计算的金字塔层级数，设置为1时单层直接计算，应用于高纹理/实时性要求高的场景
set_stereo_model_param (StereoModelID, 'binocular_num_levels', 1)
* 匹配掩模尺寸19×19像素，适合中等纹理场景
set_stereo_model_param (StereoModelID, 'binocular_mask_width', 19)
set_stereo_model_param (StereoModelID, 'binocular_mask_height', 19)
* 纹理阈值设为0，禁用纹理过滤，适用于低纹理场景
set_stereo_model_param (StereoModelID, 'binocular_texture_thresh', 0)
* 匹配分数阈值0.4，过滤低置信度匹配点
set_stereo_model_param (StereoModelID, 'binocular_score_thresh', 0.4)
* 启用左右一致性检查('left_right_check')，消除遮挡区域误匹配
set_stereo_model_param (StereoModelID, 'binocular_filter', 'left_right_check')
* 子像素优化模式为插值法('interpolation')，提升深度分辨率
set_stereo_model_param (StereoModelID, 'binocular_sub_disparity', 'interpolation')

*------ 空间约束和相机约束

* 定义3D工作空间为[-0.2,-0.07,-0.075]到[0.2,0.07,-0.004]（单位：米），自动计算视差范围
* 边界框（bounding_box）可约束重建范围，提升效率
set_stereo_model_param (StereoModelID, 'bounding_box', [-0.2,-0.07,-0.075,0.2,0.07,-0.004])
* 配置相机对为(0,2)和(1,3)，支持多相机阵列的灵活组合
set_stereo_model_image_pairs (StereoModelID, [0,2], [1,3])
* 
* *****
* --------------------Part03: 创建表面匹配模型
* *****
* 3.1，读取管道接头3D对象
read_object_model_3d ('pipe_joint', 'm', [], [], PipeJointOM3DID, Status)
create_surface_model (PipeJointOM3DID, 0.03, [], [], PipeJointSMID)

* 3.2，预处理匹配模型，提高匹配效率
* 'shape_base _matching_3d':激活基于表面集合特征的匹配模式，适用于管道接头等工业零件的定位
* 为true时进行法向量计算，曲率特征提取，关键点自动选择，提高匹配效率
prepare_object_model_3d (PipeJointOM3DID, 'shape_based_matching_3d', 'true', [], [])
* 
* *****
*-------------------Part04:多视角立体视觉重建管件堆表面模型

* 4.1，设置多视角立体模型重建表面3D模型所需必要参数
* 重建管件堆的表面，并利用基于表面的三维匹配技术确定多个管道接头的位置。
* *****
NumMatches := 3
MinScore := .3

* pose_ref_scoring_dist_rel：HALCON中用于曲面匹配（Surface-Based Matching）的关键参数，主要控制精细化姿态优化阶段的评分距离容差
* 该参数以相对值形式（相对于物体尺寸）设置匹配评分时的最大允许距离偏差
Params := ['num_matches','pose_ref_scoring_dist_rel','scene_normal_computation']
Values := [NumMatches,0.02,'mls']
Instructions[0] := 'Rotate: Left button'
Instructions[1] := 'Zoom:   Shift + left button'
Instructions[2] := 'Move:   Ctrl  + left button'
* 
* 注意：这里的Pose0是相机在世界坐标系的位姿，不是常见标定产生的标定板在相机坐标系的位姿camerapose
pose_invert (Pose0, WorldPose0)
* 
gen_empty_obj (EmptyObject)
NumImages := 15
for Index := 1 to NumImages by 1
    * 
    * 4.2，读取重建多视角立体表面模型所需的图像
    read_multi_view_stereo_images (Images, ImagePath, ImagePrefix, Index, NumCameras)
    * 
    * Reconstruct the 3D scene (the pile of pipe fittings)
    Message := 'Performing the reconstruction...'
    * 4.3，显示表面重建所用的图像
    display_multi_view_stereo_images (Images, WindowHandle)
    disp_message (WindowHandle, Message, 'window', 12, 12, 'black', 'true')
    count_seconds (T0)
    
    * 4.4，多视角立体模型重建表面3D模型
    reconstruct_surface_stereo (Images, StereoModelID, PipeJointPileOM3DID)
    count_seconds (T1)
    * 重建所用时间
    ReconsTime := T1 - T0
    * 
    PoseIn := [0.0,0.0,0.5,-30,0,180,0]
    * 4.5，显示重建的3D模型   
    if (Index == 1)
        visualize_object_model_3d (WindowHandle, PipeJointPileOM3DID, CamParam0, PoseIn, ['color','point_size'], ['yellow',1], 'Reconstructed scene in ' + ReconsTime$'.3' + ' s', [], Instructions, PoseOut)
    endif
    * 
    * Perform surface-based 3D matching
    Message := 'Search ' + NumMatches + ' best parts with surface based matching...'
    disp_message (WindowHandle, Message, 'window', 36, 12, 'black', 'true')
    count_seconds (T2)
    
    * 4.6，表面匹配查找
    find_surface_model (PipeJointSMID, PipeJointPileOM3DID, 0.03, 0.05, MinScore, 'false', Params, Values, Poses, Scores, SurfaceMatchingResultID)
    count_seconds (T3)
    * 表面匹配耗时：
    MatchingDiff := T3 - T2
    * 
    * Display the results
    
    * 4.7, 禁用形输出的实时刷新，将多个图形操作缓存后统一显示，可有效解决界面闪烁问题   
    set_system ('flush_graphic', 'false')
    select_obj (Images, Img, 1)
    dev_set_part (0, 0, Height - 1, Width - 1)
    dev_display (Img)
*     count_seconds (T4)
    for MatchIndex := 0 to |Scores| - 1 by 1
        * 管道接头在世界坐标系中位姿
        
        PoseObjInWorld := Poses[MatchIndex * 7:(MatchIndex * 7) + 6]
        * 世界坐标系在对象坐标系的位姿
*         rigid_trans_object_model_3d (PipeJointOM3DID, PoseObjInWorld, ObjectModel3DRigidTrans1)
*         visualize_object_model_3d (WindowHandle, [PipeJointPileOM3DID,ObjectModel3DRigidTrans1], [], [],\
                                     ['color_0','color_1','alpha_1'], ['gray','green',0.5], [], [], [], PoseOut1)
        
        pose_invert (PoseObjInWorld, PoseWorldInObj)
        * 相机在世界坐标系的位姿+世界坐标系在对象坐标系的位姿=相机坐标系在对象坐标系的位姿        
        pose_compose (PoseWorldInObj, Pose0, PoseCamInObj)   
        *对象在相机坐标系中的位姿 
        pose_invert (PoseCamInObj, ObjPoseInCam0)
        * Display the coordinate system of the part
        dev_set_colored (3)
        dev_set_line_width (3)
        
        *  4.8,显示匹配到的管道接头的坐标系        
        disp_3d_coord_system (WindowHandle, CamParam0, ObjPoseInCam0, 0.03)
        * Display the faces of the part
        rigid_trans_object_model_3d (PipeJointOM3DID, PoseObjInWorld, ObjectModel3DRigidTrans)
        
        * 4.9，投影到平面形成轮廓
        * 'data':'face',指定投影时处理模型的三角面片(faces)而非原始点云数据
        * 'hidden_surface_removal':'true',使用隐藏面移除技术，消除被遮挡的不可见面，提升投影结果的真实感  
        project_object_model_3d (ModelContours, ObjectModel3DRigidTrans, CamParam0, WorldPose0, ['data','hidden_surface_removal'], ['faces','true'])
        dev_set_line_width (2)
        dev_set_color ('green')
        dev_display (ModelContours)
        * 
        clear_object_model_3d (ObjectModel3DRigidTrans)
    endfor
    set_system ('flush_graphic', 'true')
*     count_seconds (T5)
*     DispTime := T5 - T4
    * 
    Message := '立体视觉重建耗时: ' + ReconsTime$'.2f' + ' s'
    Message[1] := '找到： ' + |Scores| + ' 管道接头表面匹配耗时： ' + MatchingDiff$'.2f' + ' s'
*     Message[2] := 'Visualization: ' + DispTime$'.1f' + ' s'
    disp_message (WindowHandle, Message, 'window', 12, 12, 'black', 'true')
    disp_message (WindowHandle, 'Camera 0', 'window', Height - 36, 12, 'white', 'false')
    if (Index < NumImages)
        disp_continue_message (WindowHandle, 'black', 'true')
        stop ()
    endif
    * 
    * clear the 3D object model
    clear_object_model_3d (PipeJointPileOM3DID)
    copy_obj (Images, OldImage, 1, 1)
endfor
* 
* Clear the stereo model and the surface model
clear_stereo_model (StereoModelID)
clear_surface_model (PipeJointSMID)
clear_object_model_3d (PipeJointOM3DID)
dev_set_preferences ('suppress_handled_exceptions_dlg', PreferenceValue)