public ST_XYZ ToXYZ() { ST_XYZ xyz = new ST_XYZ(); xyz.x = x; xyz.y = y; xyz.z = z; return(xyz); }
/// <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); }