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

《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多行

 *
 * Prior to executing this example, the example
 * 'handeye_stationarycam_calibration.hdev' has to be executed.
 * Given the transformations computed by the hand-eye calibration
 * this example computes the pose of the robot tool in robot base
 * coordinates for grasping the nut with the gripper.
 dev_update_off ()
 dev_close_window ()
 * Directories with calibration images and data files
 ImageNameStart := '3d_machine_vision/handeye/stationarycam_'
 DataNameStart := 'handeye/stationarycam_'
 read_image (Image, ImageNameStart + 'nut12_square')
 dev_close_window ()
 get_image_size (Image, Width, Height)
 dev_open_window_fit_image (Image, , , Width, Height, WindowHandle)
 dev_set_draw ('margin')
 dev_set_line_width ()
 set_display_font (WindowHandle, , 'mono', 'true', 'false')
 dev_display (Image)
 disp_message (WindowHandle, 'Object to grasp', 'window', , , 'black', 'true')
 * Read internal camera parameters and calibrated poses
 read_cam_par (DataNameStart + 'final_campar.dat', CamParam)
 read_pose (DataNameStart + 'final_pose_cam_base.dat', BaseInCamPose)
 pose_to_hom_mat3d (BaseInCamPose, cam_H_base)
 read_pose (DataNameStart + 'final_pose_tool_calplate.dat', CalplateInToolPose)
 pose_to_hom_mat3d (CalplateInToolPose, tool_H_calplate)
 * Read pose of gripper in tool coordinates
 read_pose (DataNameStart + 'pose_tool_gripper.dat', GripperInToolPose)
 pose_to_hom_mat3d (GripperInToolPose, tool_H_gripper)
 stop ()
 * Define reference coordinate system and display it
 CalplateFile := 'caltab_30mm.descr'
 define_reference_coord_system (ImageNameStart + 'calib3cm_00', CamParam, CalplateFile, WindowHandle, PoseRef)
 pose_to_hom_mat3d (PoseRef, cam_H_ref)
 Message := 'Defining a reference coordinate system'
 Message[] := 'based on a calibration image'
 disp_message (WindowHandle, Message, 'window', , , 'black', 'true')
 disp_continue_message (WindowHandle, 'black', 'true')
 stop ()
 dev_display (Image)
 disp_3d_coord_system (WindowHandle, CamParam, PoseRef, 0.01)
 * Find parallel sides of the nut
 dev_set_color ('yellow')
 threshold (Image, BrightRegion, , )
 connection (BrightRegion, BrightRegions)
 select_shape (BrightRegions, Nut, 'area', 'and', , )
 fill_up (Nut, NutFilled)
 gen_contour_region_xld (NutFilled, NutContours, 'border')
 segment_contours_xld (NutContours, LineSegments, 'lines', , , )
 fit_line_contour_xld (LineSegments, 'tukey', -, , , , RowBegin, ColBegin, RowEnd, ColEnd, Nr, Nc, Dist)
 gen_empty_obj (Lines)
 for I :=  to |RowBegin| -  by
 gen_contour_polygon_xld (Contour, [RowBegin[I],RowEnd[I]], [ColBegin[I],ColEnd[I]])
 concat_obj (Lines, Contour, Lines)
 endfor
 gen_polygons_xld (Lines, Polygon, 'ramer', )
 gen_parallels_xld (Polygon, ParallelLines, , , rad(), 'true')
 dev_display (ParallelLines)
 * Accumulate corner points
 get_parallels_xld (ParallelLines, Row1, Col1, Length1, Phi1, Row2, Col2, Length2, Phi2)
 CornersRow := [Row1[],Row1[],Row2[],Row2[]]
 CornersCol := [Col1[],Col1[],Col2[],Col2[]]
 * Method : transform corners into reference coordinate system and determine grasp
 image_points_to_world_plane (CamParam, PoseRef, CornersRow, CornersCol, 'm', CornersX_ref, CornersY_ref)
 * Determine center and orientation of the grasp
 CenterPointX_ref := sum(CornersX_ref) * 0.25
 CenterPointY_ref := sum(CornersY_ref) * 0.25
 GraspPointsX_ref := [(CornersX_ref[] + CornersX_ref[]) * 0.5,(CornersX_ref[] + CornersX_ref[]) * 0.5]
 GraspPointsY_ref := [(CornersY_ref[] + CornersY_ref[]) * 0.5,(CornersY_ref[] + CornersY_ref[]) * 0.5]
 GraspPhiZ_ref := atan((GraspPointsY_ref[] - GraspPointsY_ref[]) / (GraspPointsX_ref[] - GraspPointsX_ref[]))
 * Display grasping points after projecting them into the image
 affine_trans_point_3d (cam_H_ref, GraspPointsX_ref, GraspPointsY_ref, [,], GraspPointsX_cam, GraspPointsY_cam, GraspPointsZ_cam)
 project_3d_point (GraspPointsX_cam, GraspPointsY_cam, GraspPointsZ_cam, CamParam, GraspPointsRow, GraspPointsCol)
 display_grasping_points (GraspPointsRow, GraspPointsCol, WindowHandle)
 disp_message (WindowHandle, 'Finding grasping points', 'window', -, -, 'black', 'true')
 disp_continue_message (WindowHandle, 'black', 'true')
 stop ()
 * Transform it into a homogeneous transformation matrix
 hom_mat3d_identity (HomMat3DIdentity)
 hom_mat3d_rotate (HomMat3DIdentity, GraspPhiZ_ref, 'z', , , , HomMat3D_RZ_Phi)
 hom_mat3d_translate (HomMat3D_RZ_Phi, CenterPointX_ref, CenterPointY_ref, , ref_H_grasp)
 * Display coordinate system of the gripper
 hom_mat3d_compose (cam_H_ref, ref_H_grasp, cam_H_grasp)
 hom_mat3d_to_pose (cam_H_grasp, GripperInCamPose)
 dev_set_colored ()
 disp_3d_coord_system (WindowHandle, CamParam, GripperInCamPose, 0.01)
 Message := 'Determining the gripper pose'
 Message[] := 'via the reference coordinate system'
 disp_message (WindowHandle, Message, 'window', , , 'black', 'true')
 disp_continue_message (WindowHandle, 'black', 'true')
 stop ()
 * Method : pose estimation using the four corner points of the nut
 NX := [0.009,-0.009,-0.009,0.009]
 NY := [0.009,0.009,-0.009,-0.009]
 NZ := [,,,]
 sort_corner_points (CornersRow, CornersCol, WindowHandle, NRow, NCol)
 vector_to_pose (NX, NY, NZ, NRow, NCol, CamParam, 'iterative', 'error', PoseCamNut, Quality)
 dev_set_colored ()
 disp_3d_coord_system (WindowHandle, CamParam, GripperInCamPose, 0.01)
 Message := 'Alternative: Determining the gripper pose'
 Message[] := 'via pose estimation using the corners'
 disp_message (WindowHandle, Message, 'window', , , 'black', 'true')
 disp_continue_message (WindowHandle, 'black', 'true')
 stop ()
 * Determine corresponding robot position (pose of the tool in base coordinates)
 * base_H_tool = base_H_cam * cam_H_ref * ref_H_grasp * gripper_H_tool
 * where to position the tool to grasp the nut
 hom_mat3d_invert (cam_H_base, base_H_cam)
 hom_mat3d_compose (base_H_cam, cam_H_grasp, base_H_grasp)
 hom_mat3d_invert (tool_H_gripper, gripper_H_tool)
 hom_mat3d_compose (base_H_grasp, gripper_H_tool, base_H_tool)
 hom_mat3d_to_pose (base_H_tool, PoseRobotGrasp)
 * Convert pose type to the one used by the robot controller (ZYX) and display it
 convert_pose_type (PoseRobotGrasp, 'Rp+T', 'abg', 'point', PoseRobotGrasp_ZYX)
 * Alternatively, the PoseRobotGrasp can be computed using only poses instead of
 * matrices.
 pose_invert (BaseInCamPose, CamInBasePose)
 pose_compose (CamInBasePose, GripperInCamPose, GripperInBasePose)
 pose_invert (GripperInToolPose, ToolInGripper)
 * The computed ToolInBasePose equals PoseRobotGrasp
 pose_compose (GripperInBasePose, ToolInGripper, ToolInBasePose)
 dev_display (Image)
 disp_3d_coord_system (WindowHandle, CamParam, GripperInCamPose, 0.01)
 disp_message (WindowHandle, 'Converting the pose into robot coordinates', 'window', -, -, 'black', 'true')
 dev_inspect_ctrl (PoseRobotGrasp_ZYX)
 * This pose should then be sent to the robot
 disp_continue_message (WindowHandle, 'black', 'true')
 stop ()
 dev_close_inspect_ctrl (PoseRobotGrasp_ZYX)

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