《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/】