/// <summary> /// 根据上下相机数据与校准数据,计算放置位置 /// </summary> /// <param name="PosUpCam">上相机拍照位置</param> /// <param name="VsDatUpCam">上相机视觉数据</param> /// <param name="PosDwCam">下相机拍照位置</param> /// <param name="VsDatDwCam">下相机视觉数据</param> /// <param name="CaliOfs">校准值=放料位置-下拍照位置-上拍照位置</param> /// <param name="RotCent">旋转中心</param> /// <param name="RotCap">对应旋转中心的拍照位置</param> /// <param name="VsOfs">校准值=下相机识别点与放置识别点的偏差</param> /// <param name="VsUpCamAglRef">校准VsOfs时上相机的视觉角度</param> /// <param name="VsDwCamAglRef">校准VsOfs时下相机的视觉角度</param> /// <returns></returns> public static ST_XYA CalcCamPos(ST_XY PosUpCam, ST_XYA VsDatUpCam, ST_XY PosDwCam, ST_XYA VsDatDwCam, ST_XYA CaliOfs, ST_XY RotCent, ST_XYZ RotCap, ST_XYA VsOfs, double VsUpCamAglRef, double VsDwCamAglRef) { //cali_ofs = st_pos_affine_place - st_pos_affine_downcam_cap - st_pos_affine_upcam_cap //点对点 ST_XYA StPlacePos = CaliOfs + PosDwCam.ToXYA() + PosUpCam.ToXYA() + (VsDatDwCam - VsDatUpCam); //偏差旋转 double a = VsDatUpCam.a - VsUpCamAglRef; ST_XYA NewOfs = new ST_XYA(); Utility.PointRotToNewPoint(VsOfs, new ST_XY(), a, ref NewOfs); StPlacePos.x += NewOfs.x; StPlacePos.y += NewOfs.y; //吸头旋转 ST_XY NewRot = RotCent - (PosDwCam - RotCap.ToXY()); ST_XYA NewPos = new ST_XYA(); a = -VsOfs.a + (VsDatDwCam.a - VsDwCamAglRef) - (VsDatUpCam.a - VsUpCamAglRef); Utility.PointRotToNewPoint(VsDatDwCam, NewRot, -a, ref NewPos); NewPos = NewPos - VsDatDwCam; StPlacePos = StPlacePos + NewPos; StPlacePos.a = a; return(StPlacePos); }
public ST_XY ToXY() { ST_XY xy = new ST_XY(); xy.x = x; xy.y = y; return(xy); }
/// <summary> /// 获取以指定点为中心旋转指定角度后的新坐标 /// </summary> /// <param name="CurPos">需要旋转的点</param> /// <param name="RolCenter">旋转中心</param> /// <param name="Angle">旋转角度</param> public static void PointRotToNewPoint(ST_XYA CurPos, ST_XY RotCenter, double Angle, ref ST_XYA NewPos) { //转为弧度 Angle = Angle / 180.0 * Math.PI; //计算旋转后XY偏差 NewPos.x = Math.Cos(Angle) * (CurPos.x - RotCenter.x) - Math.Sin(Angle) * (CurPos.y - RotCenter.y) + RotCenter.x; NewPos.y = Math.Cos(Angle) * (CurPos.y - RotCenter.y) + Math.Sin(Angle) * (CurPos.x - RotCenter.x) + RotCenter.y; NewPos.a = CurPos.a + Angle; }
// VsTg 目标识别点坐标(上相机),st_VsShapeOrgOffset轮廓原点坐标(上相机),st_VsShapeInit轮廓初始识别点(下相机),返回偏差Offset public static EM_RES CalcOffset(ST_XYA st_VsTg, ST_XYA st_VsShapeInit, ST_XYA st_VsShapeOrgOffset, ref ST_XYA st_Offset) { //目标点(0,0)绕偏移点(原点偏移)旋转 ST_XYA st_new = new ST_XYA(); ST_XY st_org = new ST_XY(); // st_Offset.a = st_VsShapeOrgOffset.a;// -st_VsShapeInit.z; PointRotToNewPoint(st_VsShapeInit, st_org, st_VsShapeOrgOffset.a, ref st_new); st_Offset.x = st_VsTg.x - (st_VsShapeOrgOffset.x + st_new.x); st_Offset.y = st_VsTg.y - (st_VsShapeOrgOffset.y + st_new.y); st_Offset.a = st_VsShapeOrgOffset.a;// -st_VsShapeInit.z; return(EM_RES.OK); }