此示例显示如何基于由SCARA手眼校准确定的校准信息，使用SCARA机器人执行拾取和放置应用程序。 在第一步骤中，根据模型图像定义形状模型。 然后，基于该形状模型，在每个图像中搜索对象。 对于一个选定的对象，计算可用于掌握该对象的机器人坐标。 为了使该示例适用于实际应用，必须从相机获取图像（而不是从文件读取），并且必须实施机器人的控制（而不是在本示例中被注释掉的相应行）。 通常，图像必须在匹配之前校正。 如果摄像机完全正交于测量平面，则此步骤可以仅被省略。 要使用提供的示例图像运行示例程序，RectifyImages必须设置为true。
RectifyImages := true
try
//读取手眼校正的结果
read_pose ('cam_in_base_pose.dat', CamInBasePose)
read_cam_par ('camera_parameters.dat', CameraParam)
//读取要抓取的对象的姿态估计所需要的参数
read_pose ('measurement_plane_in_cam_pose.dat', MPInCamPose)
catch (Exception)
CamInBasePose:=[0.05592166548,0.19497621789,0.48025117245,180.09816119,29.85593363,179.94389014,]
CameraParam := [0.0165251,-642.277,.65521e-,.65e-,595.817,521.75,, ]
MPInCamPose := [0.0045679683065,-0.0028695297318,0.4088853425,359.78658429,29.7320275 ,0.22946472765,]
endtry
//准备校准map以消除图像的失真
if (RectifyImages)
prepare_rectification_map (Map, CameraParam, MPInCamPose, MappingScale,MPInCamPoseMapping)
image_points_to_world_plane (CameraParam, MPInCamPoseMapping, , , 'm',MapUpperLeftX, MapUpperLeftY)
endif
dev_update_off ()
set_system ('border_shape_models', 'true')
//这里，应该建立与机器人的连接，并且机器人应该移动到一个定义的待机姿态，允许获取测量平面的未被遮挡的图像
//定义要抓取的对象形状模型
//获取模型的图像
read_image (Image, '3d_machine_vision/handeye/scara_stationary_cam_setup_01_metal_parts_01')
if (RectifyImages)
map_image (Image, Map, ModelImage)
else
copy_image (Image, ModelImage)
endif
dev_close_window ()
dev_open_window_fit_image (ModelImage, , , , , WindowHandle)
set_display_font (WindowHandle, , 'mono', 'true', 'false')
dev_clear_window ()
dev_display (ModelImage)
dev_set_line_width ()
//创建形状模型
gen_rectangle1 (ModelROI, , , , )
gauss_filter (ModelImage, ImageGauss, )
reduce_domain (ImageGauss, ModelROI, ImageReduced)
create_shape_model (ImageReduced, 'auto', rad(), rad(), 'auto', 'auto', 'use_polarity', [,], 'auto', ModelID)
area_center (ModelROI, ModelROIArea, ModelROIRow, ModelROIColumn)
dev_display_shape_matching_results (ModelID, 'green', ModelROIRow, ModelROIColumn, , , , )
//指定对象的抓取点
//它要在图像中（只有当对象可以由工具以任何方向拾取时）
//或者通过用机器人抓取它并记录相应的机器人姿态
DefineGraspingPointByRobot := true
if (DefineGraspingPointByRobot)
dev_set_colored ()
GraspingPointModelInBasePose := [0.2592,0.1997,0.1224,,,1.2572,]
pose_invert (CamInBasePose, BaseInCamPose)
pose_to_hom_mat3d (BaseInCamPose, BaseInCamHomMat3D)
affine_trans_point_3d (BaseInCamHomMat3D,GraspingPointModelInBasePose[], GraspingPointModelInBasePose[], GraspingPointModelInBasePose[], Qx, Qy, Qz)
project_3d_point (Qx, Qy, Qz, CameraParam, GraspingPointModelRow,GraspingPointModelColumn)
GraspingPointModelAngle := GraspingPointModelInBasePose[]
if (RectifyImages)
//计算校正图像坐标
image_points_to_world_plane (CameraParam, MPInCamPoseMapping, GraspingPointModelRow, GraspingPointModelColumn, MappingScale, GraspingPointModelXMP, GraspingPointModelYMP)
GraspingPointModelRow := GraspingPointModelYMP - MapUpperLeftY / MappingScale
GraspingPointModelColumn := GraspingPointModelXMP - MapUpperLeftX / MappingScale
//在以校正模型图片中显示抓取点
get_image_size (ModelImage, WidthM, HeightM)
CamParamRect := [,,MappingScale,MappingScale,-MapUpperLeftX / MappingScale,-MapUpperLeftY / MappingScale,WidthM,HeightM]
GraspingPointModelXMP := MapUpperLeftX + GraspingPointModelColumn * MappingScale
GraspingPointModelYMP := MapUpperLeftY + GraspingPointModelRow * MappingScale
PoseCoordSystemVis := [GraspingPointModelXMP,GraspingPointModelYMP,,,,GraspingPointModelA ngle,]
dev_set_colored ()
disp_3d_coord_system (WindowHandle, CamParamRect,PoseCoordSystemVis, 0.02)
else
//在原始模型图像中显示抓取点
pose_invert (CamInBasePose, BaseInCamPose)
pose_compose (BaseInCamPose, GraspingPointModelInBasePose,PoseCoordSystemVis)
dev_set_colored ()
disp_3d_coord_system (WindowHandle, CameraParam,PoseCoordSystemVis, 0.02)
endif
disp_message (WindowHandle, 'Model contours and grasping pose', 'window', , , 'black', 'true')
else
binary_threshold (ImageReduced, Region, 'max_separability', 'light', UsedThreshold)
fill_up (Region, RegionFillUp)
erosion_rectangle1 (RegionFillUp, RegionErosion, , )
smallest_rectangle2 (RegionErosion, GraspingPointModelRow,GraspingPointModelColumn, Phi, Length1, Length2)
gen_cross_contour_xld (GraspingPointModel, GraspingPointModelRow,GraspingPointModelColumn, , 0.785398)
dev_set_color ('yellow')
dev_display (GraspingPointModel)
disp_message (WindowHandle, 'Model contours and grasping point', 'window',, , 'black', 'true')
endif
area_center (ModelROI, ModelROIArea, ModelROIRow, ModelROIColumn)
set_shape_model_origin (ModelID, GraspingPointModelRow - ModelROIRow, GraspingPointModelColumn - ModelROIColumn)
disp_continue_message (WindowHandle, 'black', 'true')
stop ()
pose_to_hom_mat3d (MPInCamPoseMapping, MPInCamHomMat3DMapping)
//在下列图像中找被抓取的物体
for ImageIdx := to by 1
//下一幅图片
read_image (Image,'3d_machine_vision/handeye/scara_stationary_cam_setup_01_metal_parts_' +ImageIdx$'02d')
//校正图片来适应标准模型的匹配
//寻找实例
if (RectifyImages)
map_image (Image, Map, SearchImage)
else
copy_image (Image, SearchImage)
endif
dev_clear_window ()
dev_display (SearchImage)
//寻找物体实例
find_shape_model (SearchImage, ModelID, rad(), rad(), 0.5, , 0.5, 'least_squares', [,], 0.9, Row, Column, Angle, Score)
if (|Row| < )
disp_message (WindowHandle, 'No objects found', 'window', , , 'black', 'true')
continue
endif
//选择一个指定的实例，这儿是最左边的
LeftmostIdx := sort_index(Column)[]
GraspingPointRow := Row[LeftmostIdx]
GraspingPointColumn := Column[LeftmostIdx]
GraspingPointAngle := Angle[LeftmostIdx]
//显示匹配结果和指出要抓取的物体
dev_display_shape_matching_results (ModelID, 'blue', Row, Column, Angle, , , )
dev_display_shape_matching_results (ModelID, 'green', GraspingPointRow, GraspingPointColumn, GraspingPointAngle, , , )
disp_message (WindowHandle, |Row| + ' objects found (Green: Object to be grasped)', 'window', , , 'black', 'true')
disp_continue_message (WindowHandle, 'black', 'true')
stop()
//计算需要靠近的点
calculate_point_to_approach_scara_stationary (GraspingPointRow, GraspingPointColumn, GraspingPointAngle + rad(GraspingPointModelAngle), RectifyImages, MapUpperLeftX, MapUpperLeftY, MappingScale, MPInCamHomMat3DMapping, CameraParam, MPInCamPose, CamInBasePose, ObjInBasePose)
//显示要被抓取的物体和抓取点
dev_clear_window ()
dev_display (SearchImage)
dev_display_shape_matching_results (ModelID, 'green', GraspingPointRow, GraspingPointColumn, GraspingPointAngle, , , )
dev_set_colored ()
if (RectifyImages)
get_image_size (SearchImage, Width, Height)
CamParamRect := [,,MappingScale,MappingScale,-MapUpperLeftX / MappingScale,-MapUpperLeftY / MappingScale,Width,Height]
GraspingPointXMP := MapUpperLeftX + GraspingPointColumn * MappingScale
GraspingPointYMP := MapUpperLeftY + GraspingPointRow * MappingScale
PoseCoordSystemVis := [GraspingPointXMP,GraspingPointYMP,,,,-deg(GraspingPointAngle) + GraspingPointModelAngle,]
disp_3d_coord_system (WindowHandle, CamParamRect, PoseCoordSystemVis, 0.02)
else
pose_invert (CamInBasePose, BaseInCamPose)
pose_compose (BaseInCamPose, ObjInBasePose, PoseCoordSystemVis)
disp_3d_coord_system (WindowHandle, CameraParam, PoseCoordSystemVis, 0.02)
endif
disp_message (WindowHandle, 'Press F5 to pick and place indicated object', 'window', , , 'black', 'true')
disp_message (WindowHandle, ['ObjInBasePose:','Tx: ','Ty: ','Tz: ','Alpha: ','Beta: ','Gamma: '] + ['',ObjInBasePose[:]$'.3f' + [' m',' m',' m',' deg',' deg',' deg']], 'window', , , 'black', 'true')
disp_continue_message (WindowHandle, 'black', 'true')
stop ()
//转换目标位置为mm
ToolInBasePoseMM := [ObjInBasePose[:] * ,ObjInBasePose[:]]
//抓取和放置物体
endfor
//这儿机器人连接被关闭
clear_shape_model (ModelID)
set_system ('border_shape_models', 'false')
dev_clear_window ()