7：点云处理—眼在手外标定
1.制作模板
dev_update_off ()dev_set_color ('green')dev_close_window ()WindowHeight:=740WindowWidth :=740dev_open_window(0, 0, 540, 540, 'black', WindowHandle)Instruction := ['Rotate: Left button','Zoom: Shift + left button','Move: Ctrl + left button']read_object_model_3d('./model/modelcalib3.om3','mm', [], [],ObjectModel3D, Status)triangulate_object_model_3d(ObjectModel3D, 'greedy', [], [], \Tri3D, Information)create_surface_model (Tri3D, 0.02, [], [], SurfaceModelID)Message := 'Surface model to be searched'pra_name:=['color_0','point_size_0','disp_pose']pra_value:=['magenta',1.0,'true']visualize_object_model_3d (WindowHandle, Tri3D, [], [], pra_name,pra_value, Message, [], Instruction, PoseOut)NumCalibrationScenes := 202.模板匹配
20组左右
dev_update_off ()dev_set_color ('green')dev_close_window ()WindowHeight:=740WindowWidth :=740dev_open_window(0, 0, 540, 540, 'black', WindowHandle)Instruction := ['Rotate: Left button','Zoom: Shift + left button','Move: Ctrl + left button']read_object_model_3d('./model/modelcalib3.om3','mm', [], [],ObjectModel3D, Status)triangulate_object_model_3d(ObjectModel3D, 'greedy', [], [], \Tri3D, Information)create_surface_model (Tri3D, 0.02, [], [], SurfaceModelID)Message := 'Surface model to be searched'pra_name:=['color_0','point_size_0','disp_pose']pra_value:=['magenta',1.0,'true']
* visualize_object_model_3d (WindowHandle, Tri3D, [], [], pra_name,pra_value, Message, [], Instruction, PoseOut)NumCalibrationScenes := 20ObjInCamPose5:=[]ObjInCamPose6:=[]ObjInCamPose7:=[]ObjInCamPose8:=[]ObjInCamPose9:=[]for I := 0 to 20 by 1read_object_model_3d('./scene/calib_scene/Object'+I, 'm', \[], [], ObjectScene, Status1)select_points_object_model_3d(ObjectScene, 'point_coord_z', \0.5, 700, ObjectZ)* find_surface_model(SurfaceModelID, ObjectZ, 0.02, 0.3, 0.6,\'false', [], [], ObjInCamPose, Score, SurfaceMatchingResultID)if(|Score|>=1)rigid_trans_object_model_3d(ObjectModel3D, ObjInCamPose, ObjectdTrans)pra_name:=['color_0','color_1','point_size_0','disp_pose']pra_value:=['green','magenta',1.0,'true']Message:=I+'/20'visualize_object_model_3d (WindowHandle, [ObjectZ,ObjectdTrans], [], [], \pra_name,pra_value, Message, [], Instruction, PoseOut)write_pose(ObjInCamPose, './calib/ObjInCamPose/ObjInCamPose'+I+'.dat')endif* stop()endfor3.标定
dev_update_off ()dev_get_window(WindowHandle)* The number of files. 不要低于16， 低于16达不到精度NumCalibrationScenes :=20* 创建句柄 HECCalibDataID占内存，最终要销毁create_calib_data ('hand_eye_stationary_cam', 0, 0, HECCalibDataID)*设置标定句柄的初始参数set_calib_data (HECCalibDataID, 'model', 'general', \'optimization_method', 'nonlinear')for I := 1 to 20 by 1*1.写数据进入到标定句柄try*拍照点坐标Filename :='./calib/handeye/Pose' + (I)$'01d' + '.dat'read_pose (Filename, ToolInBasePose)ToolInBasePose[6]:=2convert_pose_type(ToolInBasePose, 'Rp+T', 'gba', 'point',ToolInBasePose1)*匹配点坐标read_pose('./calib/ObjInCamPose/ObjInCamPose' + I$'01d' + '.dat',ObjInCamPose)*两个pose 都扔到句柄里面去，扔20次 机器人的欧拉角 转换为 halcon 格式的欧拉角set_calib_data (HECCalibDataID, 'tool', I, 'tool_in_base_pose', ToolInBasePose)set_calib_data_observ_pose (HECCalibDataID, 0, 0, I, ObjInCamPose)catch (Exception)message:='第'+I+'个数据不存在'disp_message(WindowHandle, message, 'window', 12, 12, 'black', 'true')endtryendfor
stop()*2. 循环20次 分别向标定句柄 存放 ToolInBasePose和ObjInCamPose，20次， 注意一一对应关系，*ToolInBasePose*匹配全部结束 了 ，终于拿到了句柄HECCalibDataID！dev_clear_window ()disp_message (WindowHandle, 'Performing the hand-eye calibration', 'window', -1, -1, 'black', 'true')*3.计算标定*拍照点 ToolInBasePose1*工件在相机的坐标 ObjInCamPosecalibrate_hand_eye (HECCalibDataID, HECPoseError)*4.1获取结果-两个pose的保存get_calib_data (HECCalibDataID, 'camera', 0, 'base_in_cam_pose', BaseInCamPose)get_calib_data (HECCalibDataID, 'calib_obj', 0, 'obj_in_tool_pose',ObjInToolPose)*测试 ---动的pose是 计算不出来！*标定的结果， 通过20组 移动的ToolInBasePose，ObjInCamPose 标定出*两个相对 不动的 BaseInCamPose ，0bjInToolPosewrite_pose(BaseInCamPose, './calib/BaseInCamPose.dat')write_pose(ObjInToolPose, './calib/ObjInToolPose.dat')*4.2 打印出来标定精度 误差多少，量化的一个结果dev_clear_window ()Message := 'Quality of the results:'Message[1] := ' 'Message[2] := 'Translation errors in mm: 'MeanTransError := HECPoseError[0]MaxTransError := HECPoseError[2]MeanHECPoseError:=HECPoseError[1]MaxHECPoseError:=HECPoseError[3]Message[3] := 'root mean square ' + MeanTransError$'6.4f'Message[4] := 'maximum ' + MaxTransError$'6.4f'Message[5] := ' 'Message[6] := 'Rotation error in degrees: 'Message[7] := 'root mean square ' + HECPoseError[1]$'6.4f'Message[8] := 'maximum ' + HECPoseError[3]$'6.4f'stop ()disp_message (WindowHandle, Message, 'window', 12, 12, 'black', 'true')disp_continue_message (WindowHandle, 'black', 'true')query_calib_data_observ_indices (HECCalibDataID, 'camera', 0, CalibObjIdx, PoseIds)stop() 4.走点
* 使用 标定的时候 第一个点云*先得到点云 +匹配 read_pose('calib/ObjInCamPose/ObjInCamPose1.dat', ObjInCamPose)*固定拍照点*调取 两个标定结果姿态read_pose('./calib/BaseInCamPose.dat', BaseInCamPose)
read_pose('./calib/ObjInToolPose.dat', ObjInToolPose)pose_invert(BaseInCamPose, CamInBasePose)
pose_invert(ObjInToolPose, ToolInObjPose)pose_compose(ObjInCamPose,ToolInObjPose,ToolInCamPose)
pose_compose(CamInBasePose, ToolInCamPose,ToolInBasePose)
*ToolInBasePose--- 验证 旋转轴是0
*换算成为机器人坐标系
convert_pose_type(ToolInBasePose, 'Rp+T', 'abg', 'point',ToolInBasePose2)if(ToolInBasePose2[3]>180)ToolInBasePose2[3]:=ToolInBasePose2[3]-360
endif
if(ToolInBasePose2[4]>180)ToolInBasePose2[4]:=ToolInBasePose2[4]-360
endif
if(ToolInBasePose2[5]>180)ToolInBasePose2[5]:=ToolInBasePose2[5]-360
endifstop()*[-264.325, -521.181, 138.24, -161.487, -1.92762, -167.703, 2]
*[-263.446, -521.104, 138.067, -161.598, -1.86336, -167.656, 2]*拍照值
read_pose('calib/handeye/Pose1.dat', CapToolInBasePose)*计算出抓取点眼在手外：相机固定不动，机械臂夹住标定板来回变换
机械臂拍照点ToolInBasePose1
匹配点ObjInCamPose
通过手眼标定文件，找出两个不动的点 BaseInCamPose 和 0bjInToolPose
最后给出匹配点，通过两个不动的点
求解出机械臂抓取点 ToolInBasePose2