码迷,mamicode.com
首页 > Windows程序 > 详细

《zw版·Halcon-delphi系列原创教程》 2d照片-3d逆向建模脚本

时间:2015-10-05 23:18:38      阅读:920      评论:0      收藏:0      [点我收藏+]

标签:

《zw版·Halcon-delphi系列原创教程》

       2d照片-3d逆向建模脚本


       3D逆向建模,是逆向工程的核心要素。
       3D逆向建模,除了目前通用的3D点云模式,通过2D图像实现快速3D建模,也是目前的重要手段。
       2D图像的3D逆向建模,目前常用的有两种模式,一个是左右视距(或多角度取景)图片叠加处理,google的卫星地图3D化,就是这个模式。
       另外一种,就是本文要介绍的3D定标模式(handeye??模式),就是在现场先拍摄一张标准3D定标图片,获取定位参数,再采集目标图像。

技术分享

 

下面Halcon自带demo脚本:handeye_stationarycam_calibration.hdev
脚本挺长的,有129行,而且只提供了单点的3D参数,可见3D建模还是蛮复杂的

真正建模,需要采集的10万-100万个数据点,不过这些只是简单的循环扫描scan采样,

最核心的代码,还是上面的100多行

 

 

  1 * 
  2 * Prior to executing this example, the example
  3 * handeye_stationarycam_calibration.hdev has to be executed.
  4 * Given the transformations computed by the hand-eye calibration
  5 * this example computes the pose of the robot tool in robot base
  6 * coordinates for grasping the nut with the gripper.
  7 dev_update_off ()
  8 dev_close_window ()
  9 * Directories with calibration images and data files
 10 ImageNameStart := 3d_machine_vision/handeye/stationarycam_
 11 DataNameStart := handeye/stationarycam_
 12 read_image (Image, ImageNameStart + nut12_square)
 13 dev_close_window ()
 14 get_image_size (Image, Width, Height)
 15 dev_open_window_fit_image (Image, 0, 0, Width, Height, WindowHandle)
 16 dev_set_draw (margin)
 17 dev_set_line_width (2)
 18 set_display_font (WindowHandle, 14, mono, true, false)
 19 dev_display (Image)
 20 disp_message (WindowHandle, Object to grasp, window, 12, 12, black, true)
 21 * Read internal camera parameters and calibrated poses
 22 read_cam_par (DataNameStart + final_campar.dat, CamParam)
 23 read_pose (DataNameStart + final_pose_cam_base.dat, BaseInCamPose)
 24 pose_to_hom_mat3d (BaseInCamPose, cam_H_base)
 25 read_pose (DataNameStart + final_pose_tool_calplate.dat, CalplateInToolPose)
 26 pose_to_hom_mat3d (CalplateInToolPose, tool_H_calplate)
 27 * Read pose of gripper in tool coordinates
 28 read_pose (DataNameStart + pose_tool_gripper.dat, GripperInToolPose)
 29 pose_to_hom_mat3d (GripperInToolPose, tool_H_gripper)
 30 stop ()
 31 * Define reference coordinate system and display it
 32 CalplateFile := caltab_30mm.descr
 33 define_reference_coord_system (ImageNameStart + calib3cm_00, CamParam, CalplateFile, WindowHandle, PoseRef)
 34 pose_to_hom_mat3d (PoseRef, cam_H_ref)
 35 Message := Defining a reference coordinate system
 36 Message[1] := based on a calibration image
 37 disp_message (WindowHandle, Message, window, 12, 12, black, true)
 38 disp_continue_message (WindowHandle, black, true)
 39 stop ()
 40 dev_display (Image)
 41 disp_3d_coord_system (WindowHandle, CamParam, PoseRef, 0.01)
 42 * Find parallel sides of the nut
 43 dev_set_color (yellow)
 44 threshold (Image, BrightRegion, 60, 255)
 45 connection (BrightRegion, BrightRegions)
 46 select_shape (BrightRegions, Nut, area, and, 500, 99999)
 47 fill_up (Nut, NutFilled)
 48 gen_contour_region_xld (NutFilled, NutContours, border)
 49 segment_contours_xld (NutContours, LineSegments, lines, 5, 4, 2)
 50 fit_line_contour_xld (LineSegments, tukey, -1, 0, 5, 2, RowBegin, ColBegin, RowEnd, ColEnd, Nr, Nc, Dist)
 51 gen_empty_obj (Lines)
 52 for I := 0 to |RowBegin| - 1 by 1
 53 gen_contour_polygon_xld (Contour, [RowBegin[I],RowEnd[I]], [ColBegin[I],ColEnd[I]])
 54 concat_obj (Lines, Contour, Lines)
 55 endfor
 56 gen_polygons_xld (Lines, Polygon, ramer, 2)
 57 gen_parallels_xld (Polygon, ParallelLines, 50, 100, rad(10), true)
 58 dev_display (ParallelLines)
 59 * Accumulate corner points
 60 get_parallels_xld (ParallelLines, Row1, Col1, Length1, Phi1, Row2, Col2, Length2, Phi2)
 61 CornersRow := [Row1[0],Row1[1],Row2[0],Row2[1]]
 62 CornersCol := [Col1[0],Col1[1],Col2[0],Col2[1]]
 63 * Method 1: transform corners into reference coordinate system and determine grasp
 64 image_points_to_world_plane (CamParam, PoseRef, CornersRow, CornersCol, m, CornersX_ref, CornersY_ref)
 65 * Determine center and orientation of the grasp
 66 CenterPointX_ref := sum(CornersX_ref) * 0.25
 67 CenterPointY_ref := sum(CornersY_ref) * 0.25
 68 GraspPointsX_ref := [(CornersX_ref[0] + CornersX_ref[1]) * 0.5,(CornersX_ref[2] + CornersX_ref[3]) * 0.5]
 69 GraspPointsY_ref := [(CornersY_ref[0] + CornersY_ref[1]) * 0.5,(CornersY_ref[2] + CornersY_ref[3]) * 0.5]
 70 GraspPhiZ_ref := atan((GraspPointsY_ref[1] - GraspPointsY_ref[0]) / (GraspPointsX_ref[1] - GraspPointsX_ref[0]))
 71 * Display grasping points after projecting them into the image
 72 affine_trans_point_3d (cam_H_ref, GraspPointsX_ref, GraspPointsY_ref, [0,0], GraspPointsX_cam, GraspPointsY_cam, GraspPointsZ_cam)
 73 project_3d_point (GraspPointsX_cam, GraspPointsY_cam, GraspPointsZ_cam, CamParam, GraspPointsRow, GraspPointsCol)
 74 display_grasping_points (GraspPointsRow, GraspPointsCol, WindowHandle)
 75 disp_message (WindowHandle, Finding grasping points, window, -1, -1, black, true)
 76 disp_continue_message (WindowHandle, black, true)
 77 stop ()
 78 * Transform it into a homogeneous transformation matrix
 79 hom_mat3d_identity (HomMat3DIdentity)
 80 hom_mat3d_rotate (HomMat3DIdentity, GraspPhiZ_ref, z, 0, 0, 0, HomMat3D_RZ_Phi)
 81 hom_mat3d_translate (HomMat3D_RZ_Phi, CenterPointX_ref, CenterPointY_ref, 0, ref_H_grasp)
 82 * Display coordinate system of the gripper
 83 hom_mat3d_compose (cam_H_ref, ref_H_grasp, cam_H_grasp)
 84 hom_mat3d_to_pose (cam_H_grasp, GripperInCamPose)
 85 dev_set_colored (3)
 86 disp_3d_coord_system (WindowHandle, CamParam, GripperInCamPose, 0.01)
 87 Message := Determining the gripper pose
 88 Message[1] := via the reference coordinate system
 89 disp_message (WindowHandle, Message, window, 12, 12, black, true)
 90 disp_continue_message (WindowHandle, black, true)
 91 stop ()
 92 * Method 2: pose estimation using the four corner points of the nut
 93 NX := [0.009,-0.009,-0.009,0.009]
 94 NY := [0.009,0.009,-0.009,-0.009]
 95 NZ := [0,0,0,0]
 96 sort_corner_points (CornersRow, CornersCol, WindowHandle, NRow, NCol)
 97 vector_to_pose (NX, NY, NZ, NRow, NCol, CamParam, iterative, error, PoseCamNut, Quality)
 98 dev_set_colored (3)
 99 disp_3d_coord_system (WindowHandle, CamParam, GripperInCamPose, 0.01)
100 Message := Alternative: Determining the gripper pose
101 Message[1] := via pose estimation using the corners
102 disp_message (WindowHandle, Message, window, 12, 12, black, true)
103 disp_continue_message (WindowHandle, black, true)
104 stop ()
105 * Determine corresponding robot position (pose of the tool in base coordinates)
106 * base_H_tool = base_H_cam * cam_H_ref * ref_H_grasp * gripper_H_tool
107 * where to position the tool to grasp the nut
108 hom_mat3d_invert (cam_H_base, base_H_cam)
109 hom_mat3d_compose (base_H_cam, cam_H_grasp, base_H_grasp)
110 hom_mat3d_invert (tool_H_gripper, gripper_H_tool)
111 hom_mat3d_compose (base_H_grasp, gripper_H_tool, base_H_tool)
112 hom_mat3d_to_pose (base_H_tool, PoseRobotGrasp)
113 * Convert pose type to the one used by the robot controller (ZYX) and display it
114 convert_pose_type (PoseRobotGrasp, Rp+T, abg, point, PoseRobotGrasp_ZYX)
115 * Alternatively, the PoseRobotGrasp can be computed using only poses instead of
116 * matrices.
117 pose_invert (BaseInCamPose, CamInBasePose)
118 pose_compose (CamInBasePose, GripperInCamPose, GripperInBasePose)
119 pose_invert (GripperInToolPose, ToolInGripper)
120 * The computed ToolInBasePose equals PoseRobotGrasp
121 pose_compose (GripperInBasePose, ToolInGripper, ToolInBasePose)
122 dev_display (Image)
123 disp_3d_coord_system (WindowHandle, CamParam, GripperInCamPose, 0.01)
124 disp_message (WindowHandle, Converting the pose into robot coordinates, window, -1, -1, black, true)
125 dev_inspect_ctrl (PoseRobotGrasp_ZYX)
126 * This pose should then be sent to the robot
127 disp_continue_message (WindowHandle, black, true)
128 stop ()
129 dev_close_inspect_ctrl (PoseRobotGrasp_ZYX)

 




【《zw版·Halcon-delphi系列原创教程》,网址,cnblogs.com/ziwang/】

《zw版·Halcon-delphi系列原创教程》 2d照片-3d逆向建模脚本

标签:

原文地址:http://www.cnblogs.com/ziwang/p/4856452.html

(0)
(0)
   
举报
评论 一句话评论(0
登录后才能评论!
© 2014 mamicode.com 版权所有  联系我们:gaon5@hotmail.com
迷上了代码!