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

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

       3D逆向建模，是逆向工程的核心要素。

       3D逆向建模，除了目前通用的3D点云模式，通过2D图像实现快速3D建模，也是目前的重要手段。

       2D图像的3D逆向建模，目前常用的有两种模式，一个是左右视距（或多角度取景）图片叠加处理，google的卫星地图3D化，就是这个模式。

       另外一种，就是本文要介绍的3D定标模式，就是在现场先拍摄一张标准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_tool107 * where to position the tool to grasp the nut108 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 it114 convert_pose_type (PoseRobotGrasp, 'Rp+T', 'abg', 'point', PoseRobotGrasp_ZYX)115 * Alternatively, the PoseRobotGrasp can be computed using only poses instead of116 * matrices.117 pose_invert (BaseInCamPose, CamInBasePose)118 pose_compose (CamInBasePose, GripperInCamPose, GripperInBasePose)119 pose_invert (GripperInToolPose, ToolInGripper)120 * The computed ToolInBasePose equals PoseRobotGrasp121 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 robot127 disp_continue_message (WindowHandle, 'black', 'true')128 stop ()129 dev_close_inspect_ctrl (PoseRobotGrasp_ZYX)

 

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