手眼标定 下相机，相机向上拍照   8-16-2020

1.先吸起产品拍照，进行九点标定，设定好拍照位（以后都将在这个位置上拍照），得到Mark点在图像的row,col,angle坐标，

经过九点标定得到的矩阵可能转换出Mark当前的机械手坐标RobotX,RobotY,然后将产品放到指定的位置（治具位），

这个位置将是偏差补偿位。以上动作必须一次做完（如果多次吸放产品，有可能每次吸到的产品位置有变化，得到的数据不准）。

2.我用的算子中所有都是先X后Y，halcon一般是先row,后col.这也是我最后X轴补y，Y轴补X的原因吧。

 

*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)
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

*目的地的修正（我使用的时候，所有算子的XY轴反了，所以就成X轴补Y轴的偏差值，Y轴补X轴的偏差值）
*为什么一个是加一个是减，这个是实践所得还不太清楚
tempx1:=-179.778-MoveY
tempy1:=348.303+MoveX
Angle1:=428.837+CmpAngleDeg