Halcon 3D 手眼标定

c 复制代码
dev_update_off ()
dev_set_color ('green')
* Open a window if the correct size.
dev_close_window ()
WindowWidth := 512
WindowHeight := 384
* Directories containing images and data files.
ImagesDir := '3d_machine_vision/hand_eye/robot_gripper_gray_'
dev_open_window (0, WindowWidth + 10, WindowWidth, WindowHeight, 'black', ImageWindowHandle)
dev_open_window (0, 0, WindowWidth, WindowHeight, 'black', WindowHandle)
set_display_font (WindowHandle, 14, 'mono', 'true', 'false')
set_display_font (ImageWindowHandle, 14, 'mono', 'true', 'false')
Instruction := ['Rotate: Left button','Zoom:   Shift + left button','Move:   Ctrl  + left button']
*1.读取点云模型
read_object_model_3d ('hand_eye/robot_gripper_3d_model.om3', 1, [], [], OM3DModel, Status)
*创建点云模型
create_surface_model (OM3DModel, 0.03, [], [], SurfaceModelID)
Message := 'Surface model to be searched'
*显示点云模型
sample_object_model_3d (OM3DModel, 'fast', 0.0009, [], [], SampledObjectModel3D)
visualize_object_model_3d (WindowHandle, SampledObjectModel3D, [], [], 'color_0', 'gray', Message, [], Instruction, PoseOut)
* The number of files.
NumCalibrationScenes := 15
* 创建手眼标定模型,同时多个角度匹配事实的点云数据
create_calib_data ('hand_eye_stationary_cam', 0, 0, HECCalibDataID)
* 设置优化速度
set_calib_data (HECCalibDataID, 'model', 'general', 'optimization_method', 'nonlinear')

for I := 1 to NumCalibrationScenes by 1
    read_image (ImageRobotGripperGray, ImagesDir + I$'02d')
    * 读取位姿,工具坐标相对于机器人坐标的姿态
    *read_pose ('tool_in_base_pose_' + I$'02d' + '.dat', ToolInBasePose)
     read_pose('C:/Users/Public/Documents/MVTec/HALCON-17.12-Progress/examples/hdevelop/Calibration/Hand-Eye/tool_in_base_pose_'+ I$'02d' + '.dat',ToolInBasePose)
     *物体相对于传感器的位姿
    filename := 'hand_eye/robot_gripper_3d_scene_' + I$'02d'
    *读取模型
    read_object_model_3d (filename, 1, [], [], OM3DScene, Status1)
 
    find_surface_model (SurfaceModelID, OM3DScene, 0.05, 1, 0, 'false', [], [], ObjInCamPose, Score, SurfaceMatchingResultID)
    refine_surface_model_pose (SurfaceModelID, OM3DScene, ObjInCamPose, 0, 'false', [], [], ObjInCamPose, Score, SurfaceMatchingResultID1)
    if (|Score|)
         *如果有得分设置到模型里面
      
        set_calib_data (HECCalibDataID, 'tool', I, 'tool_in_base_pose', ToolInBasePose)
        set_calib_data_observ_pose (HECCalibDataID, 0, 0, I, ObjInCamPose)
    endif
    *仿射运算显示
    pose_to_hom_mat3d (ObjInCamPose, HomMat3D)
    affine_trans_object_model_3d (SampledObjectModel3D, HomMat3D, OM3DModelTrans)
    *显示场景图片
      if (I < 4)
        * Clear both windows.
        dev_clear_window ()
        dev_set_window (ImageWindowHandle)
        dev_display (ImageRobotGripperGray)
        disp_message (ImageWindowHandle, 'Image from pinhole camera', 'window', 12, 12, 'black', 'true')
        dev_set_window (WindowHandle)
        Message := 'Surface model is matched in the 3D scene.'
        Message[1] := 'Points of the current scene are gray.'
        Message[2] := 'Points of the matched model are green.'
        * For better visualization, reduce the point density of the model.
        sample_object_model_3d (OM3DScene, 'fast', 0.0009, [], [], SampledOM3DScene)
        disp_message (WindowHandle, Message, 'window', 12, 12, 'black', 'true')
        Message := 'Scene: '
        Message[1] := I + ' of ' + NumCalibrationScenes
        disp_message (WindowHandle, Message, 'window', 80, 12, 'white', 'false')
        * Visualize matching result with user interaction.
        visualize_object_model_3d (WindowHandle, [SampledOM3DScene,OM3DModelTrans], [], [], ['color_0','color_1','disp_background'], ['gray','green','true'], [], [], Instruction, PoseOut)
      endif
   
endfor
clear_object_model_3d ([OM3DScene,OM3DModelTrans,SampledOM3DScene])
* 3 手眼标定
calibrate_hand_eye (HECCalibDataID, HECPoseError)
* 4.获取姿态
* 基础坐标系对于传感器(机械手)的姿态
get_calib_data (HECCalibDataID, 'camera', 0, 'base_in_cam_pose', BaseInSensorPose)
* 物体相对于传感器的姿态
get_calib_data (HECCalibDataID, 'calib_obj', 0, 'obj_in_tool_pose', ObjInToolPose)
相关推荐
GISer_Jing8 小时前
[3D GIS&Mesh]三角网格模型中的孔洞修补算法
算法·3d
cnbestec8 小时前
MinVerse 3D触觉鼠标的技术原理与创新解析
3d·计算机外设·xr·minverse·minverse 3d触觉鼠标·三维交互·触觉反馈
moonsims1 天前
无人机桥梁3D建模、巡检、检测的航线规划
3d·无人机
西哥写代码1 天前
基于cornerstone3D的dicom影像浏览器 第二十七章 设置vr相机,复位视图
3d·vue3·vr·cornerstonejs
西哥写代码2 天前
基于cornerstone3D的dicom影像浏览器 第二十五章 自定义VR调窗工具
javascript·3d·vue3·vr·cornerstonejs
SOLIDWORKS硕迪制信2 天前
3D PDF如何制作?SOLIDWORKS MBD模板定制技巧
3d·3d pdf·mbd·solidworks mbd
工业3D_大熊3 天前
从大模型加载到交互:3D Web轻量化引擎HOOPS Communicator如何打造流畅3D体验?
服务器·3d·3d可视化·3d数据格式转换·3d模型可视化·大模型可视化·3d图形渲染引擎
山楂树の3 天前
Three.js 直线拐角自动圆角化(圆弧转弯)
算法·3d·webgl
AgilityBaby3 天前
UE5 C++动态调用函数方法、按键输入绑定 ,地址前加修饰符&
开发语言·c++·3d·ue5·游戏引擎
西哥写代码4 天前
基于cornerstone3D的dicom影像浏览器 第二十四章 显示方位、坐标系、vr轮廓线
javascript·3d·vue3·vr·dicom·cornerstonejs