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，完整代码

5.1，多相机配置模型标定。

标定出立体视觉模型创建所需的相机配置模型camera_setup_model。

特别注意事项：
默认情况下场景坐标系是相机坐标系，然而该camera_setup_model是基于世界坐标系标定出来，所以后续的立体视觉生成点云的场景坐标系是世界坐标系而不是相机坐标系。

* ------------------------
* 参考案例：calibrate_cameras_multiple_camera_setup.hdev
* ------------------------
* 描述
* 多相机多视图标定，通过标定获得相机内参，以及相机在统一坐标系下的位姿。

* This program performs the calibration of a multi-view stereo
* setup consisting of 4 identical cameras.
* 
* Set some parameters
dev_update_off ()
dev_get_preferences ('suppress_handled_exceptions_dlg', PreferenceValues)
dev_set_preferences ('suppress_handled_exceptions_dlg', 'true')
ImgPath := '3d_machine_vision/multi_view/'
ImgPrefix := 'multi_view_calib_'
FileName := ImgPath + ImgPrefix + 'cam_0_00'
read_image (Image, FileName)
get_image_size (Image, Width, Height)
Scale := .5
* 
* Calibration parameters
CaltabDescr := 'caltab_100mm.descr'
CaltabThickness := 0.0064
NumCameras := 4
NumCalibObjects := 1
NumPoses := 20
CameraType := 'area_scan_polynomial'
StartCamPar := [0.0085,0.0,0.0,0.0,0.0,0.0,6e-6,6e-6,Width * .5,Height * .5,Width,Height]
* 
* Create camera calibration model and set parameters
create_calib_data ('calibration_object', NumCameras, NumCalibObjects, CalibDataID)
* Set used camera type
set_calib_data_cam_param (CalibDataID, 'all', CameraType, StartCamPar)
* Set used calibration object
set_calib_data_calib_object (CalibDataID, 0, CaltabDescr)
* 
* Process the calibration images
open_windows_multi_view_setup (Height, Width, Scale, WindowHandles)
NumIgnoredImg := 0
for PoseIndex := 0 to NumPoses - 1 by 1
    for CameraIndex := 0 to NumCameras - 1 by 1
        FileName := ImgPath + ImgPrefix + 'cam_' + CameraIndex + '_' + PoseIndex$'02'
        read_image (Image, FileName)
        * 
        set_system ('flush_graphic', 'false')
        dev_set_window (WindowHandles[CameraIndex])
        dev_display (Image)
        Message := ['Camera ' + CameraIndex,'Pose # ' + PoseIndex]
        Color := ['black','black']
        * Extract observation data
        ShowErrorMsg := false
        try
            * Find the calibration table, extract the marks and store the
            * observation in the calibration data model
            find_calib_object (Image, CalibDataID, CameraIndex, 0, PoseIndex, [], [])
            * Read the caltab and the marks' contours found for visualization
            get_calib_data_observ_contours (Caltab, CalibDataID, 'caltab', CameraIndex, 0, PoseIndex)
            get_calib_data_observ_contours (Marks, CalibDataID, 'marks', CameraIndex, 0, PoseIndex)
            dev_display (Caltab)
            dev_display (Marks)
        catch (Exception)
            * If the calibration table or marks were not identified,
            * process the next image. Some missing data is
            * not dramatic since the calibration is robust.
            ShowErrorMsg := true
            NumIgnoredImg := NumIgnoredImg + 1
            if (Exception[0] == 8402)
                * find_calib_object could not find the caltab
                Message := [Message,'No calibration tab found!']
                Color := [Color,'red']
            elseif (Exception[0] == 8404)
                * find_calib_object could not extract the marks
                Message := [Message,'Marks were not identified!']
                Color := [Color,'red']
            else
                * Unknown error occurred
                Message := [Message,'Unknown Exception!.']
                Color := [Color,'red']
            endif
            Message := [Message,'This image will be ignored.']
            Color := [Color,'red']
        endtry
        disp_message (WindowHandles[CameraIndex], Message, 'window', 12, 12, Color, 'true')
        if (ShowErrorMsg)
            * Show error status
            enforce_visualization (WindowHandles)
            disp_continue_message (WindowHandles[3], 'black', 'true')
            stop ()
        endif
    endfor
    enforce_visualization (WindowHandles)
endfor
* 
* Perform the calibration if the marks within more than 10
* images per camera were extracted before
if ((NumPoses * NumCameras) - NumIgnoredImg >= NumCameras * 10)
    calibrate_cameras (CalibDataID, Error)
else
    Message := 'Too few marks were provided!'
    Message[1] := 'Please adapt the parameters for'
    Message[2] := 'extraction of the marks or provide'
    Message[3] := 'more images with better quality.'
    Color := ['red','black','black','black']
    disp_message (WindowHandles[0], Message, 'window', 12, 12, Color, 'true')
    return ()
endif
* 
* Create a camera setup model and define a suitable world
* coordinate system
get_calib_data (CalibDataID, 'model', 'general', 'camera_setup_model', CameraSetupModelID)
* -> The world coordinate system will be implicitly defined
*    by the position of the calibration table in the
*    calibration image with the pose index 0
RefPoseIndex := 0
* 参考相机ID
get_calib_data (CalibDataID, 'model', 'general', 'reference_camera', RefCameraID)
* [0,RefPoseIndex] 0#相机第RefPoseIndex#位姿，将该位姿作为world in camera pose
get_calib_data (CalibDataID, 'calib_obj_pose', [0,RefPoseIndex], 'pose', PoseCam0Indx0)
set_origin_pose (PoseCam0Indx0, 0, 0, CaltabThickness, ReferencePose)
* -> Modify the reference coordinate system within the
*    camera setup model and write the model to file
*get_camera_setup_param (CameraSetupModelID, 'general', 'coord_transf_pose', Coord_Transf_Pose)
*get_camera_setup_param (CameraSetupModelID, 3, 'pose', Camera0Pose)
set_camera_setup_param (CameraSetupModelID, 'general', 'coord_transf_pose', ReferencePose)
file_exists ('four_camera_setup_model.csm', FileExists)
if (not FileExists)
    write_camera_setup_model (CameraSetupModelID, 'four_camera_setup_model.csm')
endif
* 
* Get the results of the calibration
* -> Camera parameters
get_calib_data (CalibDataID, 'camera', 0, 'params_labels', ParLabels)
get_camera_setup_param (CameraSetupModelID, 0, 'params', CamPar0)
get_camera_setup_param (CameraSetupModelID, 1, 'params', CamPar1)
get_camera_setup_param (CameraSetupModelID, 2, 'params', CamPar2)
get_camera_setup_param (CameraSetupModelID, 3, 'params', CamPar3)
*get_calib_data (CalibDataID, 'camera', 1, 'params', DataValue)
* -> Camera poses
get_calib_data (CalibDataID, 'camera', 0, 'pose_labels', PoseLabels)
* CamPose0 ：相机0在统一世界坐标系中的位姿（注意不是世界坐标系在相机坐标系的位姿）
* pose_invert (ReferencePose, PoseInvert)
* PoseInvert完全等于CamPose0说明CamPose0是相机在世界坐标系的位姿
get_camera_setup_param (CameraSetupModelID, 0, 'pose', CamPose0)
* CamPose1 ：相机1在统一世界坐标系中的位姿
get_camera_setup_param (CameraSetupModelID, 1, 'pose', CamPose1)
* CamPose2 ：相机2在统一世界坐标系中的位姿
get_camera_setup_param (CameraSetupModelID, 2, 'pose', CamPose2)
* CamPose3 ：相机3在统一世界坐标系中的位姿
get_camera_setup_param (CameraSetupModelID, 3, 'pose', CamPose3)

* 
* 验证----------------------
* 在未设置'coord_transf_pose'时CamPose1为Cam1InCam0Pose值
*Cam1InCam0Pose:=[-0.000716111, 0.0575621, 0.00518948, 8.16759, 0.196323, 180.224, 0]
* Cam1Compose与 CamPose1完全相等
*pose_compose (CamPose0, Cam1InCam0Pose, Cam1Compose)
*----------------------------
* Display the results
* -> First, display the camera parameters
disp_camera_parameters_multi_view_4 (WindowHandles, ParLabels, CamPar0, CamPar1, CamPar2, CamPar3)
disp_continue_message (WindowHandles[3], 'black', 'true')
stop ()
* -> Now, display the poses
disp_camera_poses_multi_view_4 (WindowHandles, PoseLabels, CamPose0, CamPose1, CamPose2, CamPose3)
* 
* Clean up
dev_set_preferences ('suppress_handled_exceptions_dlg', PreferenceValues)
clear_calib_data (CalibDataID)
clear_camera_setup_model (CameraSetupModelID)

---

5.2，立体视觉重建点云，管件接头位姿查找

* ------------------------------------------
* 案例库：locate_pipe_joints_stereo.hdev
* 扩展：
* 多相机配置模型标定案例库：calibrate_cameras_multiple_camera_setup.hdev
* ------------------------------------------

* 目的：
* 查找立体视觉获取的3D对象中管道连接头的立体位置
* 
* 过程：
* 采用四个相机组成 2 套双目设备（由立体视觉模型管理），两个双目头各覆盖工件的一部分，最终融合成完整的三维模型。
* 
* 注意：
* 本示例使用的camera_setup_model是基于世界坐标系创建，所以该立体视觉重建的点云场景坐标系是世界坐标系，而不是默认的相机坐标系。
* 基于表面的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，使用camera_setup_model模型创建立体视觉。
* '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])
* 一个模型里管理了 2 套双目设备，两个双目头各覆盖工件的一部分，最终融合成完整的三维模型。
* 配置相机对为From(0,2)To(1,3)，支持多相机阵列的灵活组合
* 定义两组立体像对
* 第1组：用相机0与相机1的图像计算视差图
* 第2组：用相机2与相机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)