手眼標定 下相機，相機向上拍照 8-16-2020
1.先吸起產品拍照，進行九點標定，設定好拍照位（以后都將在這個位置上拍照），得到Mark點在圖像的row,col,angle坐標，
經過九點標定得到的矩陣，可以轉換出Mark當前的機械手坐標RobotX,RobotY,然后將產品放到指定的位置（治具位），
這個位置將是偏差補償位。以上動作必須一次做完（如果多次吸放產品，有可能每次吸到的產品位置有變化，得到的數據不准）。
2.我用的算子中所有都是先X后Y，halcon一般是先row,后col.這也是我最后X軸補y，Y軸補X的原因吧。
3.以下所寫並非halcon專業的手眼標定的方法，hacon提供了些算子（calibrate_hand_eye,get_calib_data_observ_pose,hand_eye_calibration,set_calib_data_obsev_pose）,我還不太會使用。
*Mark點的9點標定
read_tuple('D:/智能電表/SmartMeters/SmartMeters/SmartMeters/bin/Debug/BelowHomMat2D2.tup',homMat2D)
*新拍到的Mark點角度
NewAngleRad:=-1.21136
*作為標准的圖像Mark點的角度
OldAngleRad:=-0.925952333217503
*得到角度差單位是rad弧度，相機是下向上拍照和旋轉方向的方向相反，所以乘-1.
CmpAngleRad:=(NewAngleRad-OldAngleRad)*(-1)
*將角度轉從弧度單位轉為角度
CmpAngleDeg:=deg(CmpAngleRad)
*定義一個空的旋轉標定矩陣
hom_mat2d_identity (HomMat2DRotate)
*5095.75942462271,-485.546275250741旋轉中心的XY軸坐標（Mark點在圖像上的旋轉中心可以不在線上的3點求圓心（用AUTOCAD算很方便），或擬合，不太准）
*旋轉中心很難算的特別准，最好的辦法是MARK靠近機械手的R軸中心來減少誤差
*把旋轉角度和旋轉中心加入到旋轉標定矩陣中
hom_mat2d_rotate (HomMat2DRotate,CmpAngleRad, 5095.75942462271,-485.546275250741, HomMat2DRotate)
*（Mark點）經過旋轉標定矩陣后得到旋轉過后的中心點2（圖像點）
affine_trans_point_2d(HomMat2DRotate,1258.16,368.857, Qx, Qy)
*經過旋轉后點通過矩陣得到機械手的XY軸坐標
affine_trans_point_2d(homMat2D,Qx, Qy, Qx1, Qy1)
*作為標准的圖像Mark點的XY軸坐標
qx1:=-327.404362374422
qy1:=-57.913909306769
*得到的偏差值
MoveX:=Qx1-qx1
MoveY:=Qy1-qy1
Angle:=CmpAngleDeg
*目的地的修正（偏差值已經求出來了，至於是加還是減要看機械手的旋轉角度，需要看實際情況，自己驗證一下就有結果了，把一個軸的偏差大一些，補的時候就很明顯該怎么補了，是加還是減，是Y補Y,還是Y補X）
*為什么一個是加一個是減，這個是實踐所得
tempx1:=-179.778-MoveY
tempy1:=348.303+MoveX
Angle1:=428.837+CmpAngleDeg