/// <summary>
/// 计算直线1的夹角
/// </summary>
/// <param name="Row1"></param>
/// <param name="Col1"></param>
/// <param name="Row2"></param>
/// <param name="Col2"></param>
/// <param name="CurHandEyeMat"></param>
/// <param name="Th"></param>
/// <returns></returns>
public static bool CalculateLineTh(double Row1, double Col1, double Row2, double Col2, MyHomMat2D CurHandEyeMat, out double Th)
{
Th = 0;
try {
//调整原点位置
Row1 = 2448 - Row1;
Row2 = 2448 - Row2;
Point2Db Pt1 = new Point2Db(true), Pt2 = new Point2Db(true);
Pt1.Row = Row1;
Pt1.Col = Col1;
Pt2.Row = Row2;
Pt2.Col = Col2;
Point2Db WorldPt1 = new Point2Db(true), WorldPt2 = new Point2Db(true);
MyVisionBase.AffineTransPoint2D(Pt1, CurHandEyeMat, out WorldPt1);
MyVisionBase.AffineTransPoint2D(Pt2, CurHandEyeMat, out WorldPt2);
HTuple AngleX = 0;
if (WorldPt1.Col < WorldPt2.Col)
{
HOperatorSet.AngleLl(0, 0, 1, 0, WorldPt1.Col, WorldPt1.Row, WorldPt2.Col, WorldPt2.Row, out AngleX);
Th = AngleX.D;
}
else
{
HOperatorSet.AngleLl(0, 0, 1, 0, WorldPt2.Col, WorldPt2.Row, WorldPt1.Col, WorldPt1.Row, out AngleX);
Th = AngleX.D;
}
return(true);
}
catch (Exception e0) {
Logger.PopError(e0.Message + e0.Source + e0.StackTrace);
return(false);
}
}
public static Point2Db operator -(Point2Db Pt2D1, Point2Db Pt2D2)
{
Point2Db aPt2D = new Point2Db(true);
aPt2D.Col = Pt2D1.Col - Pt2D2.Col;
aPt2D.Row = Pt2D1.Row - Pt2D2.Row;
return(aPt2D);
}
public void UpdateCurrImage(HObject img, Point2Db tempOffsetPt)
{
if (GrabedImg != null)
{
GrabedImg.Dispose();
}
if (img.IsInitialized())
{
HOperatorSet.CopyImage(img, out GrabedImg);
}
}
private void StartCaliBtn_Click(object sender, EventArgs e)
{
System.Threading.Tasks.Task.Factory.StartNew(new Action(() => {
try {
#region
List <Point2Db> PixelPtList = new List <Point2Db>();//像素点的坐标集合
LocalPara NowLocalPara = TeachCaliPara.localPara;
RectangleF RectFi = new RectangleF();
LocalManager MyLocal = new LocalManager(); //定位类初始化
LocalResult MyResult = new LocalResult();
Point2Db Pt = new Point2Db();
HObject CaliContour = new HObject();
HOperatorSet.GenEmptyObj(out CaliContour);
if (GrabedImg == null)
{
MessageBox.Show(" 请先加载一张图片");
return;
}
for (int i = 0; i < TeachCaliPara.ListRectRegion.Count; i++)
{
RectFi = TeachCaliPara.ListRectRegion[i];
TeachCaliPara.localPara.localSetting.SearchAreaX = (int)RectFi.X;
TeachCaliPara.localPara.localSetting.SearchAreaY = (int)RectFi.Y;
TeachCaliPara.localPara.localSetting.SearchWidth = (int)RectFi.Width;
TeachCaliPara.localPara.localSetting.SearchHeight = (int)RectFi.Height;
MyLocal.SetLocalModel(TeachCaliPara.localPara.localSetting.localModel);
MyLocal.SetParam(GrabedImg, TeachCaliPara.localPara);
MyLocal.doLocal();
MyResult = MyLocal.GetResult();
CaliContour = CaliContour.ConcatObj(MyResult.ShowContour);
//GetCaliMarkPts1(GrabedImg, MinGray, MaxGray, MarkR, out ContourU, out ListMarkCenterI); //找出Mark点
HObject CenterContour = new HObject();
HOperatorSet.GenCrossContourXld(out CenterContour, MyResult.row, MyResult.col, 50, 0); //生成找到的圆心轮廓
HOperatorSet.ConcatObj(CaliContour, CenterContour, out CaliContour);
Pt.Col = MyResult.col;
MyVisionBase.AdjImgRow(GrabedImg, ref MyResult.row);
Pt.Row = MyResult.row;
PixelPtList.Add(Pt);
view1.Refresh();
view1.AddViewImage(GrabedImg);
view1.AddViewObject(CaliContour);
view1.Repaint();
}
#endregion
bool IsTrue = true;
MyVisionBase.VectorToHomMat(PixelPtList, TeachCaliPara.ListPt2D, out TeachCaliPara.HomMat, out IsTrue);
}
catch
{ }
}));
}
private void CaliBtn_Click(object sender, EventArgs e)
{
System.Threading.Tasks.Task.Factory.StartNew(new Action(() => {
DialogResult dr = MessageBox.Show("是否开始旋转中心的九点标定？", "提示", MessageBoxButtons.YesNo);
if (dr == DialogResult.No)
{
return;
}
try{
#region
if (TeachCaliPara.StartCaliPt.x == 0 && TeachCaliPara.StartCaliPt.y == 0 && TeachCaliPara.EndCaliPt.x == 0 &&
TeachCaliPara.EndCaliPt.y == 0)
{
MessageBox.Show("请先示教标定的起始和终止坐标");
}
List <Point2Db> ListPosP = new List <Point2Db>();
List <Point2Db> ListPosW = new List <Point2Db>();
Point2Db NowHandEyeCaliStartPt = new Point2Db();
Point2Db NowHandEyeCaliEndPt = new Point2Db();
NowHandEyeCaliStartPt.Col = TeachCaliPara.StartCaliPt.x;
NowHandEyeCaliStartPt.Row = TeachCaliPara.StartCaliPt.y;
NowHandEyeCaliEndPt.Col = TeachCaliPara.EndCaliPt.x;
NowHandEyeCaliEndPt.Row = TeachCaliPara.EndCaliPt.y;
//移动九个点拍照，获得手眼标定的9个机械坐标点，9个像素坐标点
bool isOK = GetHandEyeCaliPt(NowHandEyeCaliStartPt, NowHandEyeCaliEndPt, TeachCaliPara.IsMoveX,
TeachCaliPara.IsMoveY, out ListPosP, out ListPosW); //获取9点标定的坐标
if (isOK)
{
HTuple PixelRows = new HTuple(), PixelCols = new HTuple(), MotionX = new HTuple(), MotionY = new HTuple();
MyVisionBase.Pt2dToHTuple(ListPosP, out PixelRows, out PixelCols);
MyVisionBase.Pt2dToHTuple(ListPosW, out MotionY, out MotionX);
//机械坐标9点减去标定中点的坐标
MyVisionBase.HtupleAddValue(MotionX, -0.5 * (NowHandEyeCaliStartPt.Col + NowHandEyeCaliEndPt.Col), out MotionX);
MyVisionBase.HtupleAddValue(MotionY, -0.5 * (NowHandEyeCaliStartPt.Row + NowHandEyeCaliEndPt.Row), out MotionY);
MyVisionBase.AdjImgRow(GrabedImg, ref PixelRows);//调整像素坐标
HTuple hv_HandEyeHomMat = new HTuple();
MyVisionBase.Calibra9(PixelCols, PixelRows, MotionX, MotionY, out hv_HandEyeHomMat);
MyVisionBase.HalconToMyHomMat(hv_HandEyeHomMat, out NowHandEyeHomMat);
TeachCaliPara.HomMat = NowHandEyeHomMat;
}
#endregion
}
catch (Exception e0) {
Logger.PopError(e0.Message, true);
}
}));
}
private void PositionSaveBtn_Click(object sender, EventArgs e)
{
if (CaliPtSelectCbx.SelectedIndex >= TeachCaliPara.ListPt2D.Count)
{
for (int i = 0; i < CaliPtSelectCbx.SelectedIndex - TeachCaliPara.ListPt2D.Count + 1; i++)
{
Point2Db pt = new Point2Db();
TeachCaliPara.ListPt2D.Add(pt);
}
TeachCaliPara.ListPt2D[CaliPtSelectCbx.SelectedIndex] = TeachPt;
}
else
{
TeachCaliPara.ListPt2D[CaliPtSelectCbx.SelectedIndex] = TeachPt;
}
PositionSaveBtn.Enabled = false;
PositionTeachBtn.Enabled = true;
}
private void TryDebugBtn_Click(object sender, EventArgs e)
{
Stopwatch sw = new Stopwatch();
sw.Start();
try{
#region
Point2Db MotionPos = new Point2Db(0, 0);
HObject CirContour = new HObject();
HObject circle = new HObject();
HObject centerCont = new HObject();
//模板的VectorAngle
double TempTeachRow = LocalPara0.Template.CenterY;
double TempTeachCol = LocalPara0.Template.CenterX;
double TempTeachAngle = LocalPara0.Template.TemplateAngle;
view1.Refresh();
HTuple Y = new HTuple(), X = new HTuple();
//找出要定位的的圆
St_CirclesParam AdjCirclePara = new St_CirclesParam(2);
St_VectorAngle VectorAngle0 = new St_VectorAngle(LocalPara0.Template.CenterY, LocalPara0.Template.CenterX,
LocalPara0.Template.TemplateAngle);
HTuple CircleStartPhi = new HTuple(), CircleEndPhi = new HTuple();
CircleTypePos.FindCircle(GrabedImg, AdjCirclePara, out Y, out X, out FindCircleR, out CircleStartPhi,
out CircleEndPhi, out circle, out CirContour, out centerCont);
FindCircleRow = Y;
FindCircleCol = X;
view1.AddImage(GrabedImg);
Thread.Sleep(100);
view1.SetDraw("green", "margin");
view1.AddViewObject(circle);
view1.SetDraw("blue", "margin");
view1.AddViewObject(CirContour);
view1.SetDraw("red", "margin");
view1.AddViewObject(centerCont);
HObject CirCenterCross = new HObject();
HOperatorSet.GenCrossContourXld(out CirCenterCross, Y, X, 120, 0.6);
view1.AddViewObject(CirCenterCross);
#endregion
}
catch (Exception e0) {
Logger.PopError(e0.Message.ToString(), true);
}
}
/// <summary>
/// 扎针定位
/// </summary>
/// <param name="PlcItem"></param>
/// <param name="Obj"></param>
public void DoProject(LD.Config.PlcDataItem PlcItem, Object Obj)
{
LD.Common.PlcDevice plcDevice = PlcItem.PlcDevice;
if (plcDevice == LD.Common.PlcDevice.V_01_TriggerGrab) //
{
NowVisionProcess = new DoVisionProcess();
//起始信号清零
LD.Logic.PlcHandle.Instance.WriteValue(LD.Common.PlcDevice.V_01_TriggerGrab, 0);
FileLib.Logger.Pop("接受到PLC信号，开始扎针对位", false, "运行日志");
short ClearValue = 1;
object obj = new object();
while (true)
{
obj = LD.Logic.PlcHandle.Instance.ReadValue(LD.Common.PlcDevice.V_01_TriggerGrab);
ClearValue = (short)obj;
if (ClearValue == 0)
{
break;
}
else
{
System.Threading.Thread.Sleep(10);
}
}
System.Threading.Thread.Sleep(1);
//读取机台编号
int StageNum = (int)LD.Logic.PlcHandle.Instance.ReadValue(LD.Common.PlcDevice.ArtTestStageNumRead);
FileLib.Logger.Pop(" 读取机台编号：" + StageNum.ToString(), false, "运行日志");
System.Threading.Thread.Sleep(1);
//读取拍照位编号
short GrabNum = (short)LD.Logic.PlcHandle.Instance.ReadValue(LD.Common.PlcDevice.ArtTestGrabNum);
System.Threading.Thread.Sleep(1);
//获取视觉示教的定位参数
NowProjectPara = ProjectParaManager.Instance.GetProjectPara(StageNum);
//获取当前拍照点的视觉坐标
NowVisionPara = NowProjectPara.GetVisionPara(GrabNum);
NowVisionProcess.SetVisionPara(NowVisionPara);
NowVisionProcess.Do();
if (GrabNum == 0)
{
FirstLocalResult = NowVisionProcess.MyLocalResult;
FileLib.Logger.Pop(" 左边拍照定位完成，开始右边拍照定位：", false, "运行日志");
}
else
{
SecondLocalResult = NowVisionProcess.MyLocalResult;
//示教点到旋转中心的坐标
Point2Db TeachPt2d1 = new Point2Db(FirstLocalResult.TeachPosToRot.Col, FirstLocalResult.TeachPosToRot.Row);
Point2Db TeachPt2d2 = new Point2Db(SecondLocalResult.TeachPosToRot.Col, SecondLocalResult.TeachPosToRot.Row);
FileLib.Logger.Pop(" 示教产品到旋转中心的坐标(左右两点)：" + TeachPt2d1.Col.ToString("f3") + " " + TeachPt2d1.Row.ToString("f3") + " "
+ TeachPt2d2.Col.ToString("f3") + " " + TeachPt2d2.Row.ToString("f3"), false, "运行日志");
//当前产品到旋转中心的坐标
Point2Db NowPosPt2d1 = new Point2Db(FirstLocalResult.PosToRot.Col, FirstLocalResult.PosToRot.Row);
Point2Db NowPosPt2d2 = new Point2Db(SecondLocalResult.PosToRot.Col, SecondLocalResult.PosToRot.Row);
FileLib.Logger.Pop(" 当前产品到旋转中心的坐标(左右两点)：" + NowPosPt2d1.Col.ToString("f3") + " " + NowPosPt2d1.Row.ToString("f3") + " "
+ NowPosPt2d2.Col.ToString("f3") + " " + NowPosPt2d2.Row.ToString("f3"), false, "运行日志");
List <Point2Db> TeachPtList = new List <Point2Db>();
List <Point2Db> NowPtList = new List <Point2Db>();
//当前产品移动到产品示教时的位置，先计算出偏移矩阵
TeachPtList.Add(TeachPt2d1);
TeachPtList.Add(TeachPt2d2);
NowPtList.Add(NowPosPt2d1);
NowPtList.Add(NowPosPt2d2);
MyHomMat2D NowHomat = new MyHomMat2D();
bool IsTrue = false;
MyVisionBase.VectorToRigidHomMat(NowPtList, TeachPtList, out NowHomat, out IsTrue);
double AddX = 0, AddY = 0, AddTheta = 0;
MyVisionBase.CalculateThreeTapePos(NowPosPt2d1, NowPosPt2d2, TeachPt2d1, TeachPt2d2, out AddX, out AddY, out AddTheta);
FileLib.Logger.Pop(" 计算出的偏移补偿量：" + AddX.ToString("f3") + " " + AddY.ToString("f3") + " " + AddTheta.ToString("f3"), false, "运行日志");
///再根据偏移矩阵计算出偏移量
double AddX1 = 0, AddY1 = 0, AddTheta1 = 0;
MyVisionBase.HomMat2dToAffinePara(NowHomat, out AddTheta1, out AddY1, out AddX1);
ViewControl view1 = DisplaySystem.GetViewControl(CameraTest.UpCam1);
view1.SetString(500, 500, "red", " AddX： " + AddX.ToString("f4") + " mm ");
view1.SetString(500, 600, "red", " AddY： " + AddY.ToString("f4") + " mm ");
view1.SetString(500, 700, "red", " AddTheta： " + AddTheta.ToString("f4") + " 度 ");
///
double addx0 = -AddX * 1000;
double addy0 = AddY * 1000;
double addtheta0 = AddTheta * 1000;
int addx = (int)addx0;
int addy = (int)addy0;
int addtheta = (int)addtheta0;
NowVisionPara = NowProjectPara.GetVisionPara(0);
int offsetx = (int)(NowVisionPara.localPara.localSetting.Offset_x * 1000);
int offsety = (int)(NowVisionPara.localPara.localSetting.Offset_y * 1000);
int offsettheta = (int)(NowVisionPara.localPara.localSetting.Offset_theta * 1000);
int newAddX = addx + offsetx;
int NewAddY = addy + offsety;
int NewAddTheta = -addtheta + offsettheta;
if (StageNum == 0 || StageNum == 2)//左边两个平台X轴为正常方向，后面两个平台为反方向
{
newAddX = addx + offsetx;
}
else
{
newAddX = -addx + offsetx;
}
LD.Logic.PlcHandle.Instance.WriteValue(LD.Common.PlcDevice.ArtTestAddX, newAddX);
System.Threading.Thread.Sleep(5);
LD.Logic.PlcHandle.Instance.WriteValue(LD.Common.PlcDevice.ArtTestAddY, NewAddY);
System.Threading.Thread.Sleep(5);
LD.Logic.PlcHandle.Instance.WriteValue(LD.Common.PlcDevice.ArtTestAddTheta, NewAddTheta);//图像坐标系为逆时针，机械坐标系为顺时针
System.Threading.Thread.Sleep(5);
int ReadAddX = 0;
int ReadAddY = 0;
int ReadAddTheta = 0;
object ObjRead = 0;
int WriteCount = 0;
while (true)
{
ObjRead = LD.Logic.PlcHandle.Instance.ReadValue(LD.Common.PlcDevice.ArtTestAddX);
System.Threading.Thread.Sleep(5);
ReadAddX = (int)ObjRead;
ObjRead = LD.Logic.PlcHandle.Instance.ReadValue(LD.Common.PlcDevice.ArtTestAddY);
System.Threading.Thread.Sleep(5);
ReadAddY = (int)ObjRead;
ObjRead = LD.Logic.PlcHandle.Instance.ReadValue(LD.Common.PlcDevice.ArtTestAddTheta);
System.Threading.Thread.Sleep(5);
ReadAddTheta = (int)ObjRead;
if (ReadAddX == newAddX && ReadAddY == NewAddY && ReadAddTheta == NewAddTheta)
{
FileLib.Logger.Pop(" 补偿值成功写入", false, "运行日志");
break;
}
else
{
WriteCount++;
if (WriteCount % 10 == 0)
{
LD.Logic.PlcHandle.Instance.WriteValue(LD.Common.PlcDevice.ArtTestAddX, newAddX);
System.Threading.Thread.Sleep(5);
LD.Logic.PlcHandle.Instance.WriteValue(LD.Common.PlcDevice.ArtTestAddY, NewAddY);
System.Threading.Thread.Sleep(5);
LD.Logic.PlcHandle.Instance.WriteValue(LD.Common.PlcDevice.ArtTestAddTheta, NewAddTheta);//图像坐标系为逆时针，机械坐标系为顺时针
System.Threading.Thread.Sleep(5);
}
if (WriteCount > 300)
{
break;
}
}
}
if (SecondLocalResult.IsLocalOk)
{
LD.Logic.PlcHandle.Instance.WriteValue(LD.Common.PlcDevice.ArtTestCamLocalResult, 1); //告诉PLC定位结果OK
FileLib.Logger.Pop(" 告诉PLC补偿量已经发给PLC：", false, "运行日志");
}
}
}
}
/// <summary>
/// 获取九点标定的九个世界坐标点，九个像素坐标点
/// </summary>
/// <param name="StartPt">示教起始点</param>
/// <param name="EndPt">示教终点</param>
/// <param name="IsCameraMovingWithAxisX"></param>
/// <param name="IsCameraMovingWithAxisY"></param>
/// <param name="PixelPosP"></param>
/// <param name="MotionPosW"></param>
/// <returns></returns>
public bool GetHandEyeCaliPt(Point2Db StartPt, Point2Db EndPt, bool IsCameraMovingWithAxisX, bool IsCameraMovingWithAxisY,
out List <Point2Db> PixelPosP, out List <Point2Db> MotionPosW)
{
int StepCount = 3;
PixelPosP = new List <Point2Db>();
MotionPosW = new List <Point2Db>();
Point2Db startPt = new Point2Db(Math.Min(StartPt.Col, EndPt.Col), Math.Min(StartPt.Row, EndPt.Row));
Point2Db endPt = new Point2Db(Math.Max(StartPt.Col, EndPt.Col), Math.Max(StartPt.Row, EndPt.Row));
double recWidth = endPt.Col - startPt.Col;
double recHeight = endPt.Row - startPt.Row;
double stepX = recWidth / (StepCount - 1);
double stepY = recHeight / (StepCount - 1);
List <double> supposeX = new List <double>(), supposeY = new List <double>();
List <double> currentX = new List <double>(), currentY = new List <double>();
LocalManager MyLocal = new LocalManager(); //定位类初始化
LocalResult MyResult = new LocalResult();
MyLocal.SetLocalModel(TeachCaliPara.localPara.localSetting.localModel);
HOperatorSet.GenEmptyObj(out CaliContour);
for (int row = 0; row <= StepCount - 1; row += 1)
{
for (int col = 0; col <= StepCount - 1; col += 1)
{
Point2Db movePt = new Point2Db(); //九点标定时，相机移动到一个点，然后开始拍照
Point2Db CaliMovePt = new Point2Db(); //九点标定时的点，坐下角为第一个点，右上角为第九个点
if (IsCameraMovingWithAxisX && IsCameraMovingWithAxisY) //相机跟随XY周移动
{
movePt = new Point2Db(startPt.Col + (StepCount - 1 - col) * stepX, startPt.Row + (StepCount - 1 - row) * stepY);
}
else if ((IsCameraMovingWithAxisX) && (!IsCameraMovingWithAxisY)) //相机跟随X轴移动
{
movePt = new Point2Db(startPt.Col + (StepCount - 1 - col) * stepX, startPt.Row + row * stepY);
}
else if ((!IsCameraMovingWithAxisX) && IsCameraMovingWithAxisY) //相机跟随Y轴移动
{
movePt = new Point2Db(startPt.Col + col * stepX, startPt.Row + (StepCount - 1 - row) * stepY);
}
else if ((!IsCameraMovingWithAxisX) && (!IsCameraMovingWithAxisY)) //相机固定
{
movePt = new Point2Db(startPt.Col + col * stepX, startPt.Row + row * stepY);
}
CaliMovePt = new Point2Db(startPt.Col + col * stepX, startPt.Row + row * stepY);
// 控制平台移动到目标位置
if (!MotionManager.Instance.SetCoordiPos(movePt.Col, movePt.Row, TeachCaliPara.EndCaliPt.angle))
{
MessageBox.Show("X Y轴运动失败!");
return(false);
}
Thread.Sleep(2000);
GrabedImg = new HObject();
CameraCtrl.Instance.GrabImg(TeachCaliPara.cam, out GrabedImg); //相机拍照抓取图片
Point2Db Pt = new Point2Db();
if (GrabedImg.IsInitialized())
{
MyLocal.SetParam(GrabedImg, TeachCaliPara.localPara); //设置定位参数
MyLocal.doLocal(); //执行定位算法
MyResult = MyLocal.GetResult(); //获取定位结果
view1.ResetView();
CaliContour = CaliContour.ConcatObj(MyResult.ShowContour);
view1.AddViewImage(GrabedImg);
view1.AddViewObject(MyResult.ShowContour);
//GetCaliMarkPts1(GrabedImg, MinGray, MaxGray, MarkR, out ContourU, out ListMarkCenterI); //找出Mark点
HObject CenterContour = new HObject();
HOperatorSet.GenCrossContourXld(out CenterContour, MyResult.row, MyResult.col, 50, 0); //生成找到的圆心轮廓
HOperatorSet.ConcatObj(CaliContour, CenterContour, out CaliContour);
view1.AddViewObject(CaliContour);
view1.Repaint();
Pt.Col = MyResult.col;
Pt.Row = MyResult.row;
PixelPosP.Add(Pt);
MotionPosW.Add(CaliMovePt);
}
else
{
MessageBox.Show("采集图片失败");
return(false);
}
}
}
return(true);
}
public ST_UpCamMarkPos(Point2Db Pt1, Point2Db Pt2)
{
MarkPt2D1 = Pt1;
MarkPt2D2 = Pt2;
}
public ST_UpCamMarkPos(bool isInit = true)
{
MarkPt2D1 = new Point2Db();
MarkPt2D2 = new Point2Db();
}
/// <summary>
/// 计算胶带或者金手指中心在平台中的坐标
/// </summary>
/// <param name="ImgIn"></param>
/// <param name="CurHandEyeHomMat">手眼标定矩阵</param>
/// <param name="IsDn">是否是下相机</param>
/// <param name="CurMotionPos">当前平台的坐标</param>
/// <param name="TeachCirclePara0">圆的示教参数</param>
/// <param name="Row1">模板示教时的行坐标</param>
/// <param name="Col1">模板示教时的列坐标</param>
/// <param name="Theta1">模板示教时的角度</param>
/// <param name="Row2"></param>
/// <param name="Col2"></param>
/// <param name="Theta2"></param>
/// <param name="CenterRow">胶带或者金手指的中心在平台中的行坐标</param>
/// <param name="CenterCol">胶带或者金手指的中心在平台中的列坐标</param>
/// <param name="RotAngle"></param>
/// <returns></returns>
public static En_ImageProcessResult FindCenter(HObject ImgIn, MyHomMat2D CurHandEyeHomMat, bool IsUp, Point2Db CurMotionPos, St_CirclesParam TeachCirclePara0, HTuple Row1, HTuple Col1, HTuple Theta1,
HTuple Row2, HTuple Col2, HTuple Theta2, out double CenterRow, out double CenterCol, out double RotAngle, out HObject circleCont, out HObject CirContour, out HObject centerCont)
{
CenterRow = 0;
CenterCol = 0;
RotAngle = 0;
St_CirclesParam TeachCirclePara;
TeachCirclePara = TeachCirclePara0;
HObject ObjContour = new HObject();
CirContour = new HObject();
HOperatorSet.GenEmptyObj(out CirContour);
HObject CircleObj = new HObject();
centerCont = new HObject();
circleCont = new HObject();
// string strLog = "";
//1.0调整示教的坐标
try
{
MyVisionBase.VectorAngleToTransPt(Row1, Col1, Theta1, Row2, Col2, Theta2, TeachCirclePara0.CenterRows, TeachCirclePara0.CenterCols, out TeachCirclePara.CenterRows, out TeachCirclePara.CenterCols);
//if (TeachCirclePara.IsThreeFeeder)
//{
// TeachCirclePara.CenterRows[2] = TeachCirclePara0.CenterRows[2];
// TeachCirclePara.CenterRows[3] = TeachCirclePara0.CenterRows[3];
// TeachCirclePara0.CenterCols[2] = TeachCirclePara0.CenterCols[2];
// TeachCirclePara0.CenterCols[3] = TeachCirclePara0.CenterCols[3];
//}
HTuple CircleRows = new HTuple(), CircleCols = new HTuple();
HObject RoiContour = new HObject();
HTuple ResultRows = new HTuple(), ResultCols = new HTuple(), ArcType0 = new HTuple();
HTuple CircleCenterRow = new HTuple(), CircleCenterCol = new HTuple(), CircleRadius = new HTuple();
HTuple StartPhi = new HTuple(), EndPhi = new HTuple(), PointOrder = new HTuple(), IsTrueFlag = new HTuple();
int cirCount;
#region //用示教的参数找出圆
for (int i = 0; i < TeachCirclePara.CenterRows.Count; i++)
{
//2.0利用存下来的圆的信息，创建圆的边缘点坐标
MyVisionBase.GenCirclePts2(TeachCirclePara.CenterRows[i], TeachCirclePara.CenterCols[i], TeachCirclePara.CircleRs[i], TeachCirclePara.StartPhis[i], TeachCirclePara.EndPhis[i]
, TeachCirclePara.PointOrders[i], out CircleRows, out CircleCols);
//3.0找出边缘点用来拟合圆
MyVisionBase.spoke2(ImgIn, TeachCirclePara.Elements[i], TeachCirclePara.DetectHeights[i], 2, 2, TeachCirclePara.Thresholds[i], "all", "first", CircleRows, CircleCols,
TeachCirclePara.Directs[i], out ResultRows, out ResultCols, out ArcType0);
HOperatorSet.GenCrossContourXld(out CircleObj, ResultRows, ResultCols, TeachCirclePara.CircleRs[i] / 10, 0.6);
HOperatorSet.ConcatObj(CircleObj, CirContour, out CirContour);
cirCount = CirContour.CountObj();
//4.拟合圆找到圆的中心
HObject CircleContour = new HObject();
HTuple FitRow = new HTuple(), FitCol = new HTuple(), FitR = new HTuple(), SartP = new HTuple(), EndP = new HTuple(), IsFlag = new HTuple();
MyVisionBase.PtsToBestCircle1(out CircleObj, ResultRows, ResultCols, 4, "arc", out FitRow, out FitCol, out FitR, out SartP, out EndP, out IsFlag);
if (Math.Abs(TeachCirclePara.CircleRs[i] - FitR.D) > TeachCirclePara.CircleRs[i] * 0.5)
{
if (ObjContour != null)
{
ObjContour.Dispose();
}
if (CircleObj != null)
{
CircleObj.Dispose();
}
Logger.PopError("拟合的圆的半径和理论值偏差过大。");
return(En_ImageProcessResult.圆半径和理论值偏差过大);
}
cirCount = CircleObj.CountObj();
HOperatorSet.ConcatObj(CircleObj, CirContour, out CirContour);
CircleCenterRow[i] = FitRow.D;
CircleCenterCol[i] = FitCol.D;
CircleRadius[i] = FitR.D;
StartPhi[i] = SartP.D;
EndPhi[i] = EndP.D;
IsTrueFlag[i] = IsFlag[0];
}
#endregion
HTuple AngleLxPixel = new HTuple(), AngleLx = new HTuple();
if (CircleCenterRow.TupleLength() != TeachCirclePara0.CenterRows.Count)
{
if (ObjContour != null)
{
ObjContour.Dispose();
}
if (CircleObj != null)
{
CircleObj.Dispose();
}
Logger.PopError("CircleTypePos.FindCenter" + "没有找到圆");
return(En_ImageProcessResult.圆的数量少于示教的数量);
}
#region //生成圆心十字轮廓，非功能性代码
HTuple cRow = new HTuple(), cColumn = new HTuple();
int count = CircleCenterRow.TupleLength();
cRow = 0;
cColumn = 0;
if (count < 4)
{
for (int i = 0; i < count; i++)
{
cRow += CircleCenterRow[i];
cColumn += CircleCenterCol[i];
}
HOperatorSet.GenCrossContourXld(out centerCont, cRow / count, cColumn / count, 150, 0);
}
else if (count == 4)
{
for (int i = 0; i < 2; i++)
{
cRow += CircleCenterRow[i];
cColumn += CircleCenterCol[i];
}
HOperatorSet.GenCrossContourXld(out centerCont, cRow / 2, cColumn / 2, 150, 0);
}
#endregion
//5.0X轴到圆的中心连线的角度
HTuple hv_HandEyeHomMat = new HTuple();
MyVisionBase.MyHomMatToHalcon(CurHandEyeHomMat, out hv_HandEyeHomMat); //手眼标定矩阵格式的转换
HTuple FitHomMat = new HTuple();
HTuple CircleRowW = new HTuple(), CircleColW = new HTuple();
HTuple CircleRowAdj = new HTuple();
//5.1调整行坐标
//CircleRowAdj =CircleCenterRow;
CircleRowAdj = CircleCenterRow.Clone();
MyVisionBase.AdjImgRow(ImgIn, ref CircleRowAdj);
//5.2像素坐标调整到世界坐标，消除相机坐标系与世界坐标系之间的夹角
HOperatorSet.AffineTransPoint2d(hv_HandEyeHomMat, CircleCenterCol, CircleRowAdj, out CircleColW, out CircleRowW);
if (CircleColW.Length >= 2)
{
if (CircleColW[0].D < CircleColW[1].D)
{
HOperatorSet.AngleLl(0, 0, 1, 0, CircleColW[0].D, CircleRowW[0].D, CircleColW[1].D, CircleRowW[1].D, out AngleLx); //圆中心连线与世界坐标X轴之间的夹角
HOperatorSet.AngleLl(0, 0, 1, 0, CircleCenterCol[0].D, CircleRowAdj[0].D, CircleCenterCol[1].D, CircleRowAdj[1].D, out AngleLxPixel);
}
else
{
HOperatorSet.AngleLl(0, 0, 1, 0, CircleColW[1].D, CircleRowW[1].D, CircleColW[0].D, CircleRowW[0].D, out AngleLx);
HOperatorSet.AngleLl(0, 0, 1, 0, CircleCenterCol[1].D, CircleRowAdj[1].D, CircleCenterCol[0].D, CircleRowAdj[0].D, out AngleLxPixel);
}
if ((Math.PI / 4) < AngleLxPixel && AngleLxPixel < (Math.PI * 0.75))
{
AngleLxPixel = AngleLxPixel - Math.PI / 2;
}
else if (AngleLxPixel < (-45.0 / 180.0 * Math.PI))
{
AngleLxPixel = AngleLxPixel + Math.PI / 2;
}
}
else if (CircleColW.Length == 1)
{
AngleLxPixel = 0;
AngleLx = 0;
}
//6.0生成图像坐标原点到示教中心的平移放射矩阵
if (CircleColW.Length >= 2)
{
HOperatorSet.VectorAngleToRigid(0, 0, 0, (CircleCenterRow[1].D + CircleCenterRow[0].D) / 2, (CircleCenterCol[1].D + CircleCenterCol[0].D) / 2, AngleLxPixel, out FitHomMat);
}
else if (CircleColW.Length == 1)
{
HOperatorSet.VectorAngleToRigid(0, 0, 0, CircleCenterRow[0].D, CircleCenterCol[0].D, AngleLxPixel, out FitHomMat);
}
HTuple TapeCenterRow = new HTuple(), TapeCenterCol = new HTuple();
HOperatorSet.AffineTransPoint2d(FitHomMat, TeachCirclePara.OffsetPixelY, TeachCirclePara.OffsetPixelX, out TapeCenterRow, out TapeCenterCol); //计算出贴标中心在图像坐标系中的位置
HObject LineContour = new HObject(), OffsetArrowContour = new HObject();
HOperatorSet.GenContourPolygonXld(out LineContour, CircleCenterRow, CircleCenterCol);
double Dist = new double();
Dist = Math.Sqrt(TeachCirclePara.OffsetPixelX * TeachCirclePara.OffsetPixelX + TeachCirclePara.OffsetPixelY * TeachCirclePara.OffsetPixelY);
//7.0定位中心指向实际贴胶的箭头
if (CircleColW.Length >= 2)
{
MyVisionBase.GenArrowContourXld(out OffsetArrowContour, (CircleCenterRow[1].D + CircleCenterRow[0].D) / 2, (CircleCenterCol[1].D + CircleCenterCol[0].D) / 2, TapeCenterRow, TapeCenterCol, Dist / 20, Dist / 20);
}
else if (CircleColW.Length == 1)
{
MyVisionBase.GenArrowContourXld(out OffsetArrowContour, CircleCenterRow[0].D, CircleCenterCol[0].D, TapeCenterRow, TapeCenterCol, Dist / 20, Dist / 20);
}
HOperatorSet.ConcatObj(LineContour, OffsetArrowContour, out OffsetArrowContour);
HOperatorSet.ConcatObj(OffsetArrowContour, CirContour, out CirContour);
//8.0需要转换到平台坐标
HTuple TransRow = new HTuple(), TransCol = new HTuple();
//8.0图片原点调整到右下角
MyVisionBase.AdjImgRow(ImgIn, ref TapeCenterRow);
HOperatorSet.AffineTransPoint2d(hv_HandEyeHomMat, TapeCenterCol, TapeCenterRow, out TransCol, out TransRow);
//9.0减去平台的坐标，得到产品相对于平台旋转中心的坐标，下相机加上平台坐标，得到胶带在平台中的绝对位置
if (ObjContour != null)
{
ObjContour.Dispose();
}
if (CircleObj != null)
{
CircleObj.Dispose();
}
if (IsUp)
{
CenterRow = TransRow.D - CurMotionPos.Row;
CenterCol = TransCol.D - CurMotionPos.Col;
}
else
{
CenterRow = TransRow.D + CurMotionPos.Row;
CenterCol = TransCol.D + CurMotionPos.Col;
}
RotAngle = AngleLx.D;
St_Position NowPos = new St_Position();
// St_Position NowPos = MotionController.GetCurrentPos(true);
//if (TeachCirclePara.IsThreeFeeder)
//{
// HTuple DistMarkToFinger = new HTuple();
// HOperatorSet.DistancePl((CircleCenterCol[0].D + CircleCenterCol[1].D) / 2.0, (CircleCenterRow[0].D + CircleCenterRow[1].D) / 2.0,
// CircleCenterCol[2].D, CircleCenterRow[2].D, CircleCenterCol[3].D, CircleCenterRow[3].D, out DistMarkToFinger);
// HTuple Sx = new HTuple(), Sy = new HTuple(), Phi = new HTuple(), Theta = new HTuple(), Tx = new HTuple(), Ty = new HTuple();
// HOperatorSet.HomMat2dToAffinePar(hv_HandEyeHomMat, out Sx, out Sy, out Phi, out Theta, out Tx, out Ty);
// DistMarkToFinger = DistMarkToFinger.D * Sx;
// double OffSetX0 = 0,OffSetX1 =0;
// OffSetX0 = (DistMarkToFinger.D - 3.5) / 2;
// //CenterCol = CenterCol - OffSetX0 + 0.3 * OffSetX0;//laobanyaojiade
// CenterCol = CenterCol - OffSetX0;
// OffSetX1 = DistMarkToFinger.D - 3.5;
// //if (DistMarkToFinger.D < TeachCirclePara.DistMarkToFinger)
// // return En_ImageProcessResult.产品三号位置弯曲;
// // Logger.Pop1(string.Format("Y轴位置={0},变形量={1}", CurMotionPos.Row, OffSetX0), "产品变形量");
// if (Math.Abs(OffSetX1) > 0.3)
// return En_ImageProcessResult.产品三号位置弯曲;
//}
if (double.IsNaN(CenterCol) || double.IsNaN(CenterRow))
{
Logger.PopError("找圆定位出错坐标非数字");
return(En_ImageProcessResult.找圆定位出错坐标非数字);
}
return(En_ImageProcessResult.OK);
}
catch (Exception e)
{
Logger.PopError("CircleTypePos.FindCenter:" + e.Message + e.Source);
return(En_ImageProcessResult.找圆失败);
}
}
private void CaliRotCenterBtn_Click(object sender, EventArgs e)
{
DialogResult dr = MessageBox.Show("是否示开始旋转中心标定？", "提示", MessageBoxButtons.YesNo);
if (dr == DialogResult.No)
{
return;
}
System.Threading.Tasks.Task.Factory.StartNew(new Action(() => {
ListMarkCenterU = new List <Point2Db>();
HObject GrabedImg = new HObject();
LocalManager MyLocal = new LocalManager(); //定位类初始化
LocalResult MyResult = new LocalResult();
MyLocal.SetLocalModel(TeachCaliPara.localPara.localSetting.localModel);
HOperatorSet.GenEmptyObj(out FindCenterCont);
//旋转一个角度拍一次照片，用定位的点拟合圆，计算出0度时标定点到旋转中心的偏移量
for (int i = 0; i < TeachCaliPara.AngleStep; i++)
{
if (MotionManager.Instance.SetCoordiPos(TeachCaliPara.StartRotPt.x, TeachCaliPara.StartRotPt.y, TeachCaliPara.StartRotPt.angle
+ TeachCaliPara.AngleRange / TeachCaliPara.AngleStep * i))
{
if (GrabedImg != null)
{
GrabedImg.Dispose();
}
CameraCtrl.Instance.GrabImg(TeachCaliPara.cam, out GrabedImg); //采集图像
MyResult = new LocalResult();
MyLocal.SetParam(GrabedImg, TeachCaliPara.localPara);
MyLocal.doLocal();
MyResult = MyLocal.GetResult();
if (!MyResult.IsLocalOk)
{
MessageBox.Show("定位失败");
return;
}
view1.ResetView();
view1.Refresh();
view1.AddViewImage(GrabedImg);
view1.AddViewObject(MyResult.ShowContour);
view1.Repaint();
//view1.Refresh();
//this.Refresh();
Thread.Sleep(50);
Point2Db NowPt = new Point2Db();
NowPt.Col = MyResult.col;
NowPt.Row = MyResult.row;
ListMarkCenterU.Add(NowPt);
FindCenterCont = FindCenterCont.ConcatObj(MyResult.ShowContour);
}
else
{
MessageBox.Show("移动失败");
return;
}
}
#region 零度时候的定位坐标
Point2Db ZeoroDegreePos = new Point2Db();
if (MotionManager.Instance.SetCoordiPos(TeachCaliPara.StartRotPt.x, TeachCaliPara.StartRotPt.y, 0.0))
{
if (GrabedImg != null)
{
GrabedImg.Dispose();
}
CameraCtrl.Instance.GrabImg(TeachCaliPara.cam, out GrabedImg); //采集图像
MyResult = new LocalResult();
MyLocal.SetParam(GrabedImg, TeachCaliPara.localPara);
MyLocal.doLocal();
MyResult = MyLocal.GetResult();
if (!MyResult.IsLocalOk)
{
MessageBox.Show("定位失败");
return;
}
ZeoroDegreePos = new Point2Db(MyResult.col, MyResult.row);
}
#endregion
//通过找到的标定mark中心点集合，拟合出旋转中心
HTuple CircleRows = new HTuple();
HTuple CircleCols = new HTuple();
MyVisionBase.ListPt2dToHTuple(ListMarkCenterU, out CircleRows, out CircleCols);
HObject CircleContourU = new HObject();
HTuple CenterRow = new HTuple(), CenterCol = new HTuple();
HTuple RotRadius = new HTuple(), StartPhi = new HTuple(), EndPhi = new HTuple(), FitFlag = new HTuple();
if (FindCirCenterCbx.SelectedValue.ToString() == "拟合圆")
{
MyVisionBase.PtsToBestCircle1(out CircleContourU, CircleRows, CircleCols, 4, "arc", out CenterRow, out CenterCol,
out RotRadius, out StartPhi, out EndPhi, out FitFlag);//通过边界点拟合圆
RotR = RotRadius.D;
RotCenterRow = CenterRow.D;
RotCenterCol = CenterCol.D;
}
else if (FindCirCenterCbx.SelectedValue.ToString() == "中垂线找圆心")
{
FindCircleCenter(ListMarkCenterU, out RotCenterRow, out RotCenterCol, out RotR);
HOperatorSet.GenCircleContourXld(out CircleContourU, RotCenterRow, RotCenterCol, RotR, 0, Math.PI, "positive", 1);
}
HObject PtContour = new HObject();
HOperatorSet.GenCrossContourXld(out PtContour, CircleRows, CircleCols, 20, 0.6);
HOperatorSet.ConcatObj(PtContour, CircleContourU, out CircleContourU);
// MyVisionBase.hDispObj(hWindowControl1.HalconWindow, CircleContourU);
view1.AddViewObject(CircleContourU);
view1.Repaint();
MyVisionBase.HTupleToListPt2d(CircleRows, CircleCols, out ListMarkCenterU);
//计算0度标定Mark中心到旋转中心的偏移量
VectorCaliMarkToRot.Row = (CenterRow - ZeoroDegreePos.Row);
VectorCaliMarkToRot.Col = (CenterCol - ZeoroDegreePos.Col);
HObject MarkToRotArrow = new HObject();
MyVisionBase.GenArrowContourXld(out MarkToRotArrow, ZeoroDegreePos.Row, ZeoroDegreePos.Col, CenterRow, CenterCol, 30, 30);
view1.AddViewObject(MarkToRotArrow);
view1.Repaint();
}));
}
