public static double StereoCalibrate(Std.VectorVectorPoint3f objectPoints,
Std.VectorVectorPoint2f imagePoints1,
Std.VectorVectorPoint2f imagePoints2, Size imageSize1, Size imageSize2, Mat cameraMatrix1, Mat xi1,
Mat distCoeffs1, Mat cameraMatrix2,
Mat xi2, Mat distCoeffs2, out Vec3d rvec, out Vec3d tvec, out Std.VectorVec3d rvecsL,
out Std.VectorVec3d tvecsL, Calib flags,
TermCriteria criteria, out Mat idx)
{
var exception = new Exception();
IntPtr rvecPtr, tvecPtr, rvecsLPtr, tvecsLPtr, idxPtr;
var error = au_cv_ccalib_omnidir_stereoCalibrate(objectPoints.CppPtr, imagePoints1.CppPtr,
imagePoints2.CppPtr, imageSize1.CppPtr,
imageSize2.CppPtr, cameraMatrix1.CppPtr, xi1.CppPtr, distCoeffs1.CppPtr, cameraMatrix2.CppPtr,
xi2.CppPtr, distCoeffs2.CppPtr, out rvecPtr,
out tvecPtr, out rvecsLPtr, out tvecsLPtr, (int)flags, criteria.CppPtr, out idxPtr,
exception.CppPtr);
rvec = new Vec3d(rvecPtr);
tvec = new Vec3d(tvecPtr);
rvecsL = new Std.VectorVec3d(rvecsLPtr);
tvecsL = new Std.VectorVec3d(tvecsLPtr);
idx = new Mat(idxPtr);
exception.Check();
return(error);
}
static void TestData()
{
var pts = readTestPoints();
var res = Calib.EstimateIntranics(pts, 9, 6);
//var res = Calib.EstimateHomography(pts, 9,6);
Console.WriteLine(res);
}
public MainWindow()
{
InitializeComponent();
if (false)
{
try
{
Calib.FileRectify();
}
catch (Exception exc)
{
Console.WriteLine(exc);
}
}
var al = new List <com.veda.LinearAlg.PointFloat>();
var ar = new List <com.veda.LinearAlg.PointFloat>();
List <com.veda.LinearAlg.CalibRect.StereoPoints> allPts = new List <com.veda.LinearAlg.CalibRect.StereoPoints>();
com.veda.LinearAlg.PointFloat imgSize = null;
foreach (var iii in images)
{
var left = CvInvoke.Imread($"{imageDir}\\Left_{iii}.jpg");
imgSize = new com.veda.LinearAlg.PointFloat(left.Width, left.Height);
var right = CvInvoke.Imread($"{imageDir}\\Right_{iii}.jpg");
var corl = convertToPF(netCvLib.calib3d.Calib.findConers(left.ToImage <Gray, Byte>()));
al.AddRange(corl);
var corr = convertToPF(netCvLib.calib3d.Calib.findConers(right.ToImage <Gray, Byte>()));
ar.AddRange(corr);
allPts.Add(new com.veda.LinearAlg.CalibRect.StereoPoints {
Left = corl, Right = corr
});
//File.WriteAllLines($"{imageDir}\\Left_{iii}.txt", cornerToString(corl));
//File.WriteAllLines($"{imageDir}\\Right_{iii}.txt", cornerToString(corr));
//var ff = com.veda.LinearAlg.Calib.CalcFundm((corl), (corr));
//Console.WriteLine(ff);
}
Console.WriteLine("F");
var F = com.veda.LinearAlg.Calib.CalcFundm(al.ToArray(), ar.ToArray());
Console.WriteLine(F);
calres = com.veda.LinearAlg.CalibRect.Rectify(allPts, imgSize);
projWin.Show();
//return;
_Capture1 = new VideoCapture(1);
_Capture2 = new VideoCapture(0);
//We will only use 1 frame ready event this is not really safe but it fits the purpose
_Capture1.ImageGrabbed += ProcessFrame;
//_Capture2.Start(); //We make sure we start Capture device 2 first
_Capture1.Start();
_Capture2.Start();
}
public void Calibrate2(double planeDistance, double baseLine, double focalLength, int width, int height, Mat[] imgs)
{
var d0 = baseLine * focalLength / planeDistance;
var decoded = code.Decode(imgs)[0];
var filtered = new Mat();
Cv2.BilateralFilter(decoded, filtered, -1, 4, 4);
Calib.CalibrateCoefficient(filtered, d0);
}
public static double Calibrate(Std.VectorVectorPoint3f objectPoints, Std.VectorVectorPoint2f imagePoints,
Size imageSize,
Mat cameraMatrix, Mat xi, Mat distCoeffs, out Std.VectorVec3d rvecs, out Std.VectorVec3d tvecs,
Calib flags)
{
var criteria = new TermCriteria(TermCriteria.Type.Count | TermCriteria.Type.Eps, 200, EPSILON);
return(Calibrate(objectPoints, imagePoints, imageSize, cameraMatrix, xi, distCoeffs, out rvecs,
out tvecs, flags, criteria));
}
public static double Calibrate(Std.VectorVectorPoint3f objectPoints, Std.VectorVectorPoint2f imagePoints,
Size imageSize,
Mat cameraMatrix, Mat xi, Mat distCoeffs, out Std.VectorVec3d rvecs, out Std.VectorVec3d tvecs,
Calib flags, TermCriteria criteria)
{
Mat idx;
return(Calibrate(objectPoints, imagePoints, imageSize, cameraMatrix, xi, distCoeffs, out rvecs,
out tvecs, flags, criteria, out idx));
}
public static double StereoCalibrate(Std.VectorVectorPoint3f objectPoints, Std.VectorVectorPoint2f imagePoints1,
Std.VectorVectorPoint2f imagePoints2, Mat cameraMatrix1, Mat distCoeffs1, Mat cameraMatrix2,
Mat distCoeffs2, Size imageSize,
out Mat rotationMatrix, out Vec3d tvec, out Mat essentialMatrix, out Mat fundamentalMatrix,
Calib flags = Calib.FixIntrinsic)
{
var criteria = new TermCriteria(TermCriteria.Type.Count | TermCriteria.Type.Eps, 30, 1e-6);
return(StereoCalibrate(objectPoints, imagePoints1, imagePoints2, cameraMatrix1, distCoeffs1, cameraMatrix2,
distCoeffs2, imageSize,
out rotationMatrix, out tvec, out essentialMatrix, out fundamentalMatrix, flags, criteria));
}
public static double StereoCalibrate(Std.VectorVectorPoint3f objectPoints,
Std.VectorVectorPoint2f imagePoints1,
Std.VectorVectorPoint2f imagePoints2, Size imageSize1, Size imageSize2, Mat cameraMatrix1, Mat xi1,
Mat distCoeffs1, Mat cameraMatrix2,
Mat xi2, Mat distCoeffs2, out Vec3d rvec, out Vec3d tvec, out Std.VectorVec3d rvecsL,
out Std.VectorVec3d tvecsL, Calib flags)
{
var criteria = new TermCriteria(TermCriteria.Type.Count | TermCriteria.Type.Eps, 200, EPSILON);
return(StereoCalibrate(objectPoints, imagePoints1, imagePoints2, imageSize1, imageSize2, cameraMatrix1,
xi1, distCoeffs1, cameraMatrix2,
xi2, distCoeffs2, out rvec, out tvec, out rvecsL, out tvecsL, flags, criteria));
}
public static double StereoCalibrate(Std.VectorVectorPoint3f objectPoints,
Std.VectorVectorPoint2f imagePoints1,
Std.VectorVectorPoint2f imagePoints2, Size imageSize1, Size imageSize2, Mat cameraMatrix1, Mat xi1,
Mat distCoeffs1, Mat cameraMatrix2,
Mat xi2, Mat distCoeffs2, out Vec3d rvec, out Vec3d tvec, out Std.VectorVec3d rvecsL,
out Std.VectorVec3d tvecsL, Calib flags, TermCriteria criteria)
{
Mat idx;
return(StereoCalibrate(objectPoints, imagePoints1, imagePoints2, imageSize1, imageSize2, cameraMatrix1,
xi1, distCoeffs1, cameraMatrix2,
xi2, distCoeffs2, out rvec, out tvec, out rvecsL, out tvecsL, flags, criteria, out idx));
}
public void Calibrate(double planeDistance, double baseLine, double focalLength, int width, int height, byte[][] capturedImages)
{
var imgs = capturedImages
.Select(buff => Array.ConvertAll(buff, b => Math.Min(255, (int)b)))
.Select(arr => (new Mat(height, width, MatType.CV_32SC1, arr) * 255).ToMat())
.ToArray();
var d0 = baseLine * focalLength / planeDistance;
var decoded = code.Decode(imgs)[0];
var filtered = new Mat();
Cv2.BilateralFilter(decoded, filtered, -1, 4, 4);
Calib.CalibrateCoefficient(filtered, d0);
}
/// <summary>
/// 构造函数
/// </summary>
public Mark()
: base()
{
this.handle = null;
this.matte = false;
this.amend = false;
this.label = null;
this.image = null;
this.arise = null;
this.fonts = null;
this.frame = null;
this.dimen = null;
this.calib = null;
this.point = null;
this.color = Colors.Blue;
this.scene = Colors.Transparent;
}
private void AddPixelCross(float x, float y)
{
if (locationShapes != null && Calib != null)
{
PointF pixel;
PointF imageCenter = new PointF();
imageCenter.X = locationShapes[OperShapeIndex].ImageSize.Width / 2f;
imageCenter.Y = locationShapes[OperShapeIndex].ImageSize.Height / 2f;
Calib.WorldPointToPixelPoint(new PointF(x, y), out pixel, imageCenter, new PointF());
crossCollection.Add(operaCross);
crossCollection.SetCrossActive(pixel.X, pixel.Y);
Repaint();
}
}
// Static methods
public static double Calibrate(Std.VectorVectorPoint3f objectPoints, Std.VectorVectorPoint2f imagePoints,
Size imageSize,
Mat cameraMatrix, Mat xi, Mat distCoeffs, out Std.VectorVec3d rvecs, out Std.VectorVec3d tvecs,
Calib flags, TermCriteria criteria,
out Mat idx)
{
var exception = new Exception();
IntPtr rvecsPtr, tvecsPtr, idxPtr;
var error = au_cv_ccalib_omnidir_calibrate(objectPoints.CppPtr, imagePoints.CppPtr, imageSize.CppPtr,
cameraMatrix.CppPtr,
xi.CppPtr, distCoeffs.CppPtr, out rvecsPtr, out tvecsPtr, (int)flags, criteria.CppPtr, out idxPtr,
exception.CppPtr);
rvecs = new Std.VectorVec3d(rvecsPtr);
tvecs = new Std.VectorVec3d(tvecsPtr);
idx = new Mat(idxPtr);
exception.Check();
return(error);
}
static void Main(string[] args)
{
//TestData();
//return;
double[] A = new double[]
{
1, 1, 2,
3, 2, 1,
4, 2, 1,
};
var jres = JacobSvd.JacobiSVD(new GMatrix(A, 3, 3));
GMatrix s = jres.getWMat();
//new GMatrix(new double[] {
// jres.W[0], 0 ,0,
// 0, jres.W[1], 0,
// 0,0,jres.W[2]
//}, 3, 3);
//Console.WriteLine("U");
//Console.WriteLine(jres.U);
//Console.WriteLine(jres.U.dot(jres.U.tranpose()));
//Console.WriteLine("S");
//Console.WriteLine(s);
//Console.WriteLine("V");
//Console.WriteLine(jres.Vt);
//Console.WriteLine(jres.Vt.dot(jres.Vt.tranpose()));
//Console.WriteLine(jres.U.dot(s).dot(jres.Vt));
//return;
//var r = new GMatrix(new double[,] { { 1, 2 }, { 3, 4 } , { 1, 1 } }).cross(new GMatrix(new double[,] { { 1, 1 ,1}, { 3, 4 ,1} }));
//Console.WriteLine(r);
//new Calib().Calc(new PointF[] {
// new Point(1,2),
// new Point(3,4),
// new Point(5,4),
// new Point(6,4),
// new Point(7,4),
// new Point(8,4),
// new Point(9,4),
// new Point(10,4),
// new Point(11,4),
// new Point(12,4),
// new Point(13,4),
// new Point(14,4),
//},
//new PointF[] {
// new Point(1,2),
// new Point(3,4),
// new Point(3,4),
// new Point(3,4),
// new Point(3,4),
// new Point(3,4),
// new Point(3,4),
// new Point(3,4),
// new Point(3,4),
// new Point(3,4),
// new Point(3,4),
// new Point(3,4),
//}
//);
var lines = File.ReadAllLines(saveFileName_corners);
var resa = stringToCorner(lines);
for (int i = 0; i < 10; i++)
{
var res = Calib.CalcFundm(resa[0][i], resa[1][i]);
Console.WriteLine(res);
}
Console.WriteLine(Calib.CalcFundm(resa[0].Take(10).SelectMany(x => x).ToArray(), resa[1].Take(10).SelectMany(x => x).ToArray()));
Console.WriteLine("\nHomo\n");
for (int i = 0; i < 10; i++)
{
var res = Calib.EstimateHomography(resa[0][i]);
Console.WriteLine(res);
var res2 = Calib.EstimateHomography(resa[1][i]);
Console.WriteLine(res2);
Console.WriteLine("====================================");
}
var intrinc1 = Calib.EstimateIntranics(resa[0]);
Console.WriteLine("==================================== INCTRINS 1");
Console.WriteLine(intrinc1);
var intrinc2 = Calib.EstimateIntranics(resa[1]);
Console.WriteLine("==================================== INCTRINS 2");
Console.WriteLine(intrinc2);
}
public static double StereoCalibrate(Std.VectorVectorPoint3f objectPoints, Std.VectorVectorPoint2f imagePoints1,
Std.VectorVectorPoint2f imagePoints2, Mat cameraMatrix1, Mat distCoeffs1, Mat cameraMatrix2, Mat distCoeffs2, Size imageSize,
out Mat rotationMatrix, out Vec3d tvec, out Mat essentialMatrix, out Mat fundamentalMatrix, Calib flags, TermCriteria criteria)
{
Exception exception = new Exception();
System.IntPtr rotationMatrixPtr, tvecPtr, essentialMatrixPtr, fundamentalMatrixPtr;
double error = au_cv_calib3d_stereoCalibrate(objectPoints.CppPtr, imagePoints1.CppPtr, imagePoints2.CppPtr, cameraMatrix1.CppPtr,
distCoeffs1.CppPtr, cameraMatrix2.CppPtr, distCoeffs2.CppPtr, imageSize.CppPtr, out rotationMatrixPtr, out tvecPtr, out essentialMatrixPtr,
out fundamentalMatrixPtr, (int)flags, criteria.CppPtr, exception.CppPtr);
rotationMatrix = new Mat(rotationMatrixPtr);
tvec = new Vec3d(tvecPtr);
essentialMatrix = new Mat(essentialMatrixPtr);
fundamentalMatrix = new Mat(fundamentalMatrixPtr);
exception.Check();
return(error);
}
/// <summary>
/// 标定开始
/// </summary>
/// <param name="sender"></param>
/// <param name="e"></param>
private void btnInhaleCalib_Click(object sender, System.EventArgs e)
{
if (MessageBox.Show(string.Format("产品标定"), "确认", MessageBoxButtons.OKCancel) == DialogResult.Cancel)
{
return;
}
stuts(1);
Global.IsLocating = true;
ArcParam <double> pos4 = new ArcParam <double>();
ArcParam <double> pos1 = new ArcParam <double>();
ArcParam <double> pos2 = new ArcParam <double>();
ArcParam <double> pos3 = new ArcParam <double>();
Point <double> posd1 = new Point <double>();
Point <double> posd2 = new Point <double>();
Task.Factory.StartNew(() =>
{
try
{
int step = 0;
while (true)
{
Thread.Sleep(10);
if (mYAMAHA.Alarm)
{
Global.IsLocating = false;
return(-4);
}
switch (step)
{
case 0: //去待机位
pt[0] = Position.Instance.RootPostion[0].X;
pt[1] = Position.Instance.RootPostion[0].Y;
pt[2] = Position.Instance.RootPostion[0].Z;
pt[3] = Position.Instance.RootPostion[0].R;
pt[4] = 0;
pt[5] = 0;
pt[6] = (int)(tbrJogSpeed.Value);
mYAMAHA.MovP(pt);
Station1.YaxisServo.MoveTo(Position.Instance.YaxisPostion[0].pos);
Station1.ZaxisServo.MoveTo(Position.Instance.ZaxisPostion[0].pos);
if (rdbLeftCalib.Checked)
{
Station1.LFXaxisServo.MoveTo(Position.Instance.LPhotoOriPostion.LFXPhoto);
Station1.LFYaxisServo.MoveTo(Position.Instance.LPhotoOriPostion.LFYPhoto);
Station1.LRXaxisServo.MoveTo(Position.Instance.LPhotoOriPostion.LRXPhoto);
Station1.LRYaxisServo.MoveTo(Position.Instance.LPhotoOriPostion.LRYPhoto);
Station1.RYaxisServo.MoveTo(Position.Instance.LPhotoOriPostion.RYPhoto);
Station1.XaxisServo.MoveTo(Position.Instance.LPhotoOriPostion.XPhoto);
Station1.SwichCylinder.Reset();
}
if (rdbRightCalib.Checked)
{
Station1.LFXaxisServo.MoveTo(Position.Instance.RPhotoOriPostion.LFXPhoto);
Station1.LFYaxisServo.MoveTo(Position.Instance.RPhotoOriPostion.LFYPhoto);
Station1.LRXaxisServo.MoveTo(Position.Instance.RPhotoOriPostion.LRXPhoto);
Station1.LRYaxisServo.MoveTo(Position.Instance.RPhotoOriPostion.LRYPhoto);
Station1.RYaxisServo.MoveTo(Position.Instance.RPhotoOriPostion.RYPhoto);
Station1.XaxisServo.MoveTo(Position.Instance.RPhotoOriPostion.XPhoto);
Station1.SwichCylinder.Set();
}
step = 10;
break;
case 10: //判断是否在待机位
if (rdbLeftCalib.Checked)
{
if (mYAMAHA.CurrentPosX == Position.Instance.RootPostion[0].X &&
mYAMAHA.CurrentPosY == Position.Instance.RootPostion[0].Y &&
mYAMAHA.CurrentPosZ == Position.Instance.RootPostion[0].Z &&
mYAMAHA.CurrentPosR == Position.Instance.RootPostion[0].R &&
Station1.YaxisServo.IsInPosition(Position.Instance.YaxisPostion[0].pos) &&
Station1.ZaxisServo.IsInPosition(Position.Instance.ZaxisPostion[0].pos) &&
Station1.LFXaxisServo.IsInPosition(Position.Instance.LPhotoOriPostion.LFXPhoto) &&
Station1.LFYaxisServo.IsInPosition(Position.Instance.LPhotoOriPostion.LFYPhoto) &&
Station1.LRXaxisServo.IsInPosition(Position.Instance.LPhotoOriPostion.LRXPhoto) &&
Station1.LRYaxisServo.IsInPosition(Position.Instance.LPhotoOriPostion.LRYPhoto) &&
Station1.RYaxisServo.IsInPosition(Position.Instance.LPhotoOriPostion.RYPhoto) &&
Station1.XaxisServo.IsInPosition(Position.Instance.LPhotoOriPostion.XPhoto) &&
Station1.SwichCylinder.OutMoveStatus)
{
step = 20;
}
}
if (rdbRightCalib.Checked)
{
if (mYAMAHA.CurrentPosX == Position.Instance.RootPostion[0].X &&
mYAMAHA.CurrentPosY == Position.Instance.RootPostion[0].Y &&
mYAMAHA.CurrentPosZ == Position.Instance.RootPostion[0].Z &&
mYAMAHA.CurrentPosR == Position.Instance.RootPostion[0].R &&
Station1.YaxisServo.IsInPosition(Position.Instance.YaxisPostion[0].pos) &&
Station1.ZaxisServo.IsInPosition(Position.Instance.ZaxisPostion[0].pos) &&
Station1.LFXaxisServo.IsInPosition(Position.Instance.LPhotoOriPostion.LFXPhoto) &&
Station1.LFYaxisServo.IsInPosition(Position.Instance.LPhotoOriPostion.LFYPhoto) &&
Station1.LRXaxisServo.IsInPosition(Position.Instance.LPhotoOriPostion.LRXPhoto) &&
Station1.LRYaxisServo.IsInPosition(Position.Instance.LPhotoOriPostion.LRYPhoto) &&
Station1.RYaxisServo.IsInPosition(Position.Instance.LPhotoOriPostion.RYPhoto) &&
Station1.XaxisServo.IsInPosition(Position.Instance.LPhotoOriPostion.XPhoto) &&
Station1.SwichCylinder.OutOriginStatus)
{
step = 20;
}
}
break;
case 20: //机械手去前安全位
if (rdbLeftCalib.Checked)
{
pt[0] = Position.Instance.RootPostion[1].X;
pt[1] = Position.Instance.RootPostion[1].Y;
pt[2] = Position.Instance.RootPostion[1].Z;
pt[3] = Position.Instance.RootPostion[1].R;
pt[4] = 0;
pt[5] = 0;
pt[6] = (int)(tbrJogSpeed.Value);
mYAMAHA.MovP(pt);
Station1.ZaxisServo.MoveTo(Position.Instance.ZaxisPostion[1].pos);
}
if (rdbRightCalib.Checked)
{
pt[0] = Position.Instance.RootPostion[2].X;
pt[1] = Position.Instance.RootPostion[2].Y;
pt[2] = Position.Instance.RootPostion[2].Z;
pt[3] = Position.Instance.RootPostion[2].R;
pt[4] = 0;
pt[5] = 0;
pt[6] = (int)(tbrJogSpeed.Value);
mYAMAHA.MovP(pt);
Station1.ZaxisServo.MoveTo(Position.Instance.ZaxisPostion[2].pos);
}
step = 30;
break;
case 30: //机械手到位
if (mYAMAHA.CurrentPosX == Position.Instance.RootPostion[1].X &&
mYAMAHA.CurrentPosY == Position.Instance.RootPostion[1].Y &&
mYAMAHA.CurrentPosZ == Position.Instance.RootPostion[1].Z &&
mYAMAHA.CurrentPosR == Position.Instance.RootPostion[1].R &&
Station1.ZaxisServo.IsInPosition(Position.Instance.ZaxisPostion[1].pos) && rdbLeftCalib.Checked)
{
step = 40;
}
if (mYAMAHA.CurrentPosX == Position.Instance.RootPostion[2].X &&
mYAMAHA.CurrentPosY == Position.Instance.RootPostion[2].Y &&
mYAMAHA.CurrentPosZ == Position.Instance.RootPostion[2].Z &&
mYAMAHA.CurrentPosR == Position.Instance.RootPostion[2].R &&
Station1.ZaxisServo.IsInPosition(Position.Instance.ZaxisPostion[2].pos) && rdbRightCalib.Checked)
{
step = 40;
}
break;
case 40: //拍照（DK）
stuts(2);
pos1 = Station1.GetDkMark1(10, 10);
pos2 = Station1.GetDkMark2(10, 10);
pos3 = Station1.GetDkMark3(10, 10);
pos4 = Station1.GetDkMark4(10, 10);
step = 50;
break;
case 50: //拍照完成，取结果
Point <double> p1 = pp(pos1);
Point <double> p2 = pp(pos2);
Point <double> p3 = pp(pos3);
Point <double> p4 = pp(pos4);
posd1 = Calib.ConventToPos(p1, p2, p3, p4, Config.Instance.PhotoAngleOffice[0], Config.Instance.PhotoAngleOffice[1],
Config.Instance.PhotoAngleOffice[2], Config.Instance.PhotoAngleOffice[3]);
step = 60;
break;
case 60: //Z轴取屏位置，机械手去上方安全位置
stuts(3);
if (rdbLeftCalib.Checked)
{
pt[0] = Position.Instance.RootPostion[3].X;
pt[1] = Position.Instance.RootPostion[3].Y;
pt[2] = Position.Instance.RootPostion[3].Z;
pt[3] = Position.Instance.RootPostion[3].R;
pt[4] = 0;
pt[5] = 0;
pt[6] = (int)(tbrJogSpeed.Value);
mYAMAHA.MovP(pt);
Station1.ZaxisServo.MoveTo(Position.Instance.ZaxisPostion[3].pos);
}
if (rdbRightCalib.Checked)
{
pt[0] = Position.Instance.RootPostion[4].X;
pt[1] = Position.Instance.RootPostion[4].Y;
pt[2] = Position.Instance.RootPostion[4].Z;
pt[3] = Position.Instance.RootPostion[4].R;
pt[4] = 0;
pt[5] = 0;
pt[6] = (int)(tbrJogSpeed.Value);
mYAMAHA.MovP(pt);
Station1.ZaxisServo.MoveTo(Position.Instance.ZaxisPostion[4].pos);
}
step = 70;
break;
case 70: //机械手到位去组装位置
if (mYAMAHA.CurrentPosX == Position.Instance.RootPostion[3].X &&
mYAMAHA.CurrentPosY == Position.Instance.RootPostion[3].Y &&
mYAMAHA.CurrentPosZ == Position.Instance.RootPostion[3].Z &&
mYAMAHA.CurrentPosR == Position.Instance.RootPostion[3].R &&
Station1.ZaxisServo.IsInPosition(Position.Instance.ZaxisPostion[3].pos) && rdbLeftCalib.Checked)
{
pt[0] = Position.Instance.RootPostion[11].X;
pt[1] = Position.Instance.RootPostion[11].Y;
pt[2] = Position.Instance.RootPostion[11].Z;
pt[3] = Position.Instance.RootPostion[11].R;
pt[4] = 0;
pt[5] = 0;
pt[6] = (int)(tbrJogSpeed.Value);
mYAMAHA.MovP(pt);
Station1.LeftVacuum.Set();
step = 80;
}
if (mYAMAHA.CurrentPosX == Position.Instance.RootPostion[4].X &&
mYAMAHA.CurrentPosY == Position.Instance.RootPostion[4].Y &&
mYAMAHA.CurrentPosZ == Position.Instance.RootPostion[4].Z &&
mYAMAHA.CurrentPosR == Position.Instance.RootPostion[4].R &&
Station1.ZaxisServo.IsInPosition(Position.Instance.ZaxisPostion[4].pos) && rdbRightCalib.Checked)
{
pt[0] = Position.Instance.RootPostion[12].X;
pt[1] = Position.Instance.RootPostion[12].Y;
pt[2] = Position.Instance.RootPostion[12].Z;
pt[3] = Position.Instance.RootPostion[12].R;
pt[4] = 0;
pt[5] = 0;
pt[6] = (int)(tbrJogSpeed.Value);
mYAMAHA.MovP(pt);
Station1.RightVacuum.Set();
step = 80;
}
break;
case 80: //到吸产品位置
stuts(4);
if (mYAMAHA.CurrentPosX == Position.Instance.RootPostion[11].X &&
mYAMAHA.CurrentPosY == Position.Instance.RootPostion[11].Y &&
mYAMAHA.CurrentPosZ == Position.Instance.RootPostion[11].Z &&
mYAMAHA.CurrentPosR == Position.Instance.RootPostion[11].R &&
Station1.LeftVacuum.OutMoveStatus && rdbLeftCalib.Checked)
{
Thread.Sleep(50);
Station1.ZaxisServo.MoveTo(Position.Instance.ZaxisPostion[0].pos);
step = 90;
}
if (mYAMAHA.CurrentPosX == Position.Instance.RootPostion[12].X &&
mYAMAHA.CurrentPosY == Position.Instance.RootPostion[12].Y &&
mYAMAHA.CurrentPosZ == Position.Instance.RootPostion[12].Z &&
mYAMAHA.CurrentPosR == Position.Instance.RootPostion[12].R &&
Station1.RightVacuum.OutMoveStatus && rdbRightCalib.Checked)
{
Thread.Sleep(50);
Station1.ZaxisServo.MoveTo(Position.Instance.ZaxisPostion[0].pos);
step = 90;
}
break;
case 90: //Z轴到位，机器人去拍照位置
if (Station1.ZaxisServo.IsInPosition(Position.Instance.ZaxisPostion[0].pos) &&
rdbLeftCalib.Checked)
{
pt[0] = Position.Instance.RootPostion[5].X;
pt[1] = Position.Instance.RootPostion[5].Y;
pt[2] = Position.Instance.RootPostion[5].Z;
pt[3] = Position.Instance.RootPostion[5].R;
pt[4] = 0;
pt[5] = 0;
pt[6] = (int)(tbrJogSpeed.Value);
mYAMAHA.MovP(pt);
step = 100;
}
if (Station1.ZaxisServo.IsInPosition(Position.Instance.ZaxisPostion[0].pos) &&
rdbLeftCalib.Checked)
{
pt[0] = Position.Instance.RootPostion[6].X;
pt[1] = Position.Instance.RootPostion[6].Y;
pt[2] = Position.Instance.RootPostion[6].Z;
pt[3] = Position.Instance.RootPostion[6].R;
pt[4] = 0;
pt[5] = 0;
pt[6] = (int)(tbrJogSpeed.Value);
mYAMAHA.MovP(pt);
step = 100;
}
break;
case 100: //机械手去拍照位置
stuts(5);
if (mYAMAHA.CurrentPosX == Position.Instance.RootPostion[5].X &&
mYAMAHA.CurrentPosY == Position.Instance.RootPostion[5].Y &&
mYAMAHA.CurrentPosZ == Position.Instance.RootPostion[5].Z &&
mYAMAHA.CurrentPosR == Position.Instance.RootPostion[5].R &&
rdbLeftCalib.Checked)
{
step = 110;
}
if (mYAMAHA.CurrentPosX == Position.Instance.RootPostion[6].X &&
mYAMAHA.CurrentPosY == Position.Instance.RootPostion[6].Y &&
mYAMAHA.CurrentPosZ == Position.Instance.RootPostion[6].Z &&
mYAMAHA.CurrentPosR == Position.Instance.RootPostion[6].R &&
rdbRightCalib.Checked)
{
step = 110;
}
break;
case 110: //拍照
stuts(6);
pos1 = Station1.GetPmMark1(10, 10);
pos2 = Station1.GetPmMark2(10, 10);
pos3 = Station1.GetPmMark3(10, 10);
pos4 = Station1.GetPmMark4(10, 10);
step = 120;
break;
case 120: //拍照完计算
Point <double> p11 = pp(pos1);
Point <double> p12 = pp(pos2);
Point <double> p13 = pp(pos3);
Point <double> p14 = pp(pos4);
posd2 = Calib.ConventToPos(p11, p12, p13, p14, Config.Instance.PhotoAngleOffice[0], Config.Instance.PhotoAngleOffice[1],
Config.Instance.PhotoAngleOffice[2], Config.Instance.PhotoAngleOffice[3]);
step = 130;
break;
case 130: //
stuts(7);
if (rdbLeftCalib.Checked)
{
Config.Instance.OfficePhoto[0].X = posd2.X + posd1.X;
Config.Instance.OfficePhoto[0].Y = posd2.Y + posd1.Y;
}
if (rdbRightCalib.Checked)
{
Config.Instance.OfficePhoto[1].X = posd2.X + posd1.X;
Config.Instance.OfficePhoto[1].Y = posd2.Y + posd1.Y;
}
step = 140;
break;
case 140: //标定完成
MessageBox.Show("标定完成");
step = 150;
break;
default:
stuts(0);
Global.IsLocating = false;
return(0);
}
}
}
catch (System.Exception ex)
{
Global.IsLocating = false;
LogHelper.Error("设备驱动程序异常" + ex);
return(-2);
}
}, TaskCreationOptions.AttachedToParent | TaskCreationOptions.LongRunning);
Global.IsLocating = false;
}
/// <summary>
/// 四相机关系标定计算
/// </summary>
/// <param name="sender"></param>
/// <param name="e"></param>
private void button1_Click(object sender, System.EventArgs e)
{
if (MessageBox.Show(string.Format("是否保存数据"), "保存", MessageBoxButtons.OKCancel) == DialogResult.Cancel)
{
return;
}
int step = 0;
Global.IsLocating = true;
Point <double> pos = new Point <double>();
Point <double> pos1 = new Point <double>();
Task.Factory.StartNew(() =>
{
try
{
while (true)
{
Thread.Sleep(10);
if (mYAMAHA.Alarm)
{
Global.IsLocating = false;
return(-4);
}
switch (step)
{
case 0:
pt[0] = Config.Instance.PhotoCalibPostion.X;
pt[1] = Config.Instance.PhotoCalibPostion.Y;
pt[2] = Config.Instance.PhotoCalibPostion.Z;
pt[3] = Config.Instance.PhotoCalibPostion.R;
pt[4] = 0;
pt[5] = 0;
pt[6] = (int)(tbrJogSpeed.Value);
mYAMAHA.MovP(pt);
step = 10;
break;
case 10:
if (mYAMAHA.CurrentPosX == Config.Instance.PhotoCalibPostion.X &&
mYAMAHA.CurrentPosY == Config.Instance.PhotoCalibPostion.Y &&
mYAMAHA.CurrentPosZ == Config.Instance.PhotoCalibPostion.Z &&
mYAMAHA.CurrentPosR == Config.Instance.PhotoCalibPostion.R)
{
step = 20;
}
break;
case 20:
CalibPos[0] = Station1.getMarkSenter1(0, 0);
CalibPos[1] = Station1.getMarkSenter2(0, 0);
CalibPos[2] = Station1.getMarkSenter3(0, 0);
CalibPos[3] = Station1.getMarkSenter4(0, 0);
step = 30;
break;
case 30:
pt[0] = Config.Instance.PhotoCalibPostion.X +
Config.Instance.PhotoCalibEndPostion.X;
pt[1] = Config.Instance.PhotoCalibPostion.Y;
pt[2] = Config.Instance.PhotoCalibPostion.Z;
pt[3] = Config.Instance.PhotoCalibPostion.R;
pt[4] = 0;
pt[5] = 0;
pt[6] = (int)(tbrJogSpeed.Value);
mYAMAHA.MovP(pt);
step = 40;
break;
case 40:
if (mYAMAHA.CurrentPosX == Config.Instance.PhotoCalibPostion.X +
Config.Instance.PhotoCalibEndPostion.X &&
mYAMAHA.CurrentPosY == Config.Instance.PhotoCalibPostion.Y &&
mYAMAHA.CurrentPosZ == Config.Instance.PhotoCalibPostion.Z &&
mYAMAHA.CurrentPosR == Config.Instance.PhotoCalibPostion.R)
{
step = 50;
}
break;
case 50:
CalibPosXend[0] = Station1.getMarkSenter1(100, 0);
CalibPosXend[1] = Station1.getMarkSenter2(100, 0);
CalibPosXend[2] = Station1.getMarkSenter3(100, 0);
CalibPosXend[3] = Station1.getMarkSenter4(100, 0);
step = 60;
break;
case 60:
pt[0] = Config.Instance.PhotoCalibPostion.X;
pt[1] = Config.Instance.PhotoCalibPostion.Y +
Config.Instance.PhotoCalibEndPostion.Y;
pt[2] = Config.Instance.PhotoCalibPostion.Z;
pt[3] = Config.Instance.PhotoCalibPostion.R;
pt[4] = 0;
pt[5] = 0;
pt[6] = (int)(tbrJogSpeed.Value);
mYAMAHA.MovP(pt);
step = 70;
break;
case 70:
if (mYAMAHA.CurrentPosX == Config.Instance.PhotoCalibPostion.X &&
mYAMAHA.CurrentPosY == Config.Instance.PhotoCalibPostion.Y +
Config.Instance.PhotoCalibEndPostion.Y &&
mYAMAHA.CurrentPosZ == Config.Instance.PhotoCalibPostion.Z &&
mYAMAHA.CurrentPosR == Config.Instance.PhotoCalibPostion.R)
{
step = 80;
}
break;
case 80:
CalibPosYend[0] = Station1.getMarkSenter1(0, 100);
CalibPosYend[1] = Station1.getMarkSenter2(0, 100);
CalibPosYend[2] = Station1.getMarkSenter3(0, 100);
CalibPosYend[3] = Station1.getMarkSenter4(0, 100);
step = 90;
break;
case 90: //计算相机偏移
for (int i = 0; i < 4; i++)
{
Point <double> p1 = new Point <double>();
Point <double> p2 = new Point <double>();
p1.X = Config.Instance.PhotoCalibEndPostion.X;
p1.Y = 0;
p2.X = 0;
p2.Y = Config.Instance.PhotoCalibEndPostion.Y;
pos.X = CalibPosXend[i].X - CalibPos[i].X;
pos.Y = CalibPosXend[i].Y - CalibPos[i].Y;
pos1.X = CalibPosYend[i].X - CalibPos[i].X;
pos1.Y = CalibPosYend[i].Y - CalibPos[i].Y;
angle[i] = Calib.RotationAngle(p1, pos, out bool result);
angle1[i] = Calib.RotationAngle(p2, pos1, out bool result1);
}
InitdgvPlatePositionRows1();
step = 100;
break;
default:
Global.IsLocating = false;
return(0);
}
}
}
catch (System.Exception ex)
{
Global.IsLocating = false;
LogHelper.Error("设备驱动程序异常" + ex);
return(-2);
}
}, TaskCreationOptions.AttachedToParent | TaskCreationOptions.LongRunning);
Global.IsLocating = false;
}
public static double CalibrateCamera(Std.VectorVectorPoint3f objectPoints, Std.VectorVectorPoint2f imagePoints, Size imageSize,
Mat cameraMatrix, Mat distCoeffs, out Std.VectorVec3d rvecs, out Std.VectorVec3d tvecs, Std.VectorDouble stdDeviationsIntrinsics,
Std.VectorDouble stdDeviationsExtrinsics, Std.VectorDouble perViewErrors, Calib flags = 0)
{
TermCriteria criteria = new TermCriteria(TermCriteria.Type.Count | TermCriteria.Type.Eps, 30, EPSILON);
return(CalibrateCamera(objectPoints, imagePoints, imageSize, cameraMatrix, distCoeffs, out rvecs, out tvecs, stdDeviationsIntrinsics,
stdDeviationsExtrinsics, perViewErrors, flags, criteria));
}
private void ProcessFrame(object sender, EventArgs arg)
{
try
{
if (!_Capture1.IsOpened || !_Capture2.IsOpened)
{
return;
}
}
catch
{
return;
}
//Aquire the frames or calculate two frames from one camera
Mat m1 = new Mat();
_Capture1.Retrieve(m1);
//var m1 = _Capture1.QueryFrame();
try
{
m1 = Calib.TransformImg(calres.H1.To2DArray(), m1);
frame_S1 = m1.ToImage <Bgr, Byte>();
Gray_frame_S1 = frame_S1.Convert <Gray, Byte>();
} catch (Exception exc)
{
return;
}
Mat m2 = new Mat();
try
{
_Capture2.Retrieve(m2);
m2 = Calib.TransformImg(calres.H2.To2DArray(), m2);
frame_S2 = m2.ToImage <Bgr, Byte>();
Gray_frame_S2 = frame_S2.Convert <Gray, Byte>();
}
catch (Exception exc)
{
return;
}
if (firstCfg.done || true)
{
if (calibRes == null)
{
calibRes = Calib.Caluculating_Stereo_Intrinsics(firstCfg.corners_points_Left, firstCfg.corners_points_Right, frame_S1.Size);
Calib.Rectify(calibRes);
}
else
{
if (dptCalc == null)
{
Calib.Rectify(calibRes);
dptCalc = new Depth(calibRes.Q);
}
var res = dptCalc.Computer3DPointsFromStereoPair(Gray_frame_S1, Gray_frame_S2, cfg);
if (save)
{
save = false;
List <String> data = new List <string>();
foreach (var p in res.points)
{
data.Add($"{p.X},{p.Y},{p.Z}");
}
File.WriteAllLines("points.txt", data.ToArray());
UIInvoke(() =>
{
info.Text = "Saved";
});
}
UIInvoke(() =>
{
disparityMap.Source = DisplayLib.Util.Convert(res.disparityMap.Bitmap);
if (show)
{
show = false;
projWin.SetData(res.points);
}
});
}
}
else
{
var foundLine = Calib.findCorners(firstCfg, Gray_frame_S1, Gray_frame_S2, doShoot);
if (foundLine)
{
doShoot = false;
}
Calib.DrawChessFound(frame_S1, frame_S2, firstCfg);
}
UIInvoke(() =>
{
video1.Source = DisplayLib.Util.Convert(frame_S1.Bitmap);
video2.Source = DisplayLib.Util.Convert(frame_S2.Bitmap);
});
}
// Static methods
public static double CalibrateCamera(Std.VectorVectorPoint3f objectPoints, Std.VectorVectorPoint2f imagePoints, Size imageSize,
Mat cameraMatrix, Mat distCoeffs, out Std.VectorVec3d rvecs, out Std.VectorVec3d tvecs, Std.VectorDouble stdDeviationsIntrinsics,
Std.VectorDouble stdDeviationsExtrinsics, Std.VectorDouble perViewErrors, Calib flags, TermCriteria criteria)
{
Exception exception = new Exception();
System.IntPtr rvecsPtr, tvecsPtr;
double error = au_cv_calib3d_calibrateCamera1(objectPoints.CppPtr, imagePoints.CppPtr, imageSize.CppPtr, cameraMatrix.CppPtr,
distCoeffs.CppPtr, out rvecsPtr, out tvecsPtr, stdDeviationsIntrinsics.CppPtr, stdDeviationsExtrinsics.CppPtr, perViewErrors.CppPtr,
(int)flags, criteria.CppPtr, exception.CppPtr);
rvecs = new Std.VectorVec3d(rvecsPtr);
tvecs = new Std.VectorVec3d(tvecsPtr);
exception.Check();
return(error);
}
public static double StereoCalibrate(Std.VectorVectorPoint3f objectPoints, Std.VectorVectorPoint2f imagePoints1,
Std.VectorVectorPoint2f imagePoints2, Mat cameraMatrix1, Mat distCoeffs1, Mat cameraMatrix2, Mat distCoeffs2, Size imageSize, out Vec3d rvec,
out Vec3d tvec, out Mat E, out Mat F, Calib flags, TermCriteria criteria)
{
Exception exception = new Exception();
System.IntPtr rvecPtr, tvecPtr, EPtr, FPtr;
double error = au_cv_calib3d_stereoCalibrate(objectPoints.CppPtr, imagePoints1.CppPtr, imagePoints2.CppPtr, cameraMatrix1.CppPtr,
distCoeffs1.CppPtr, cameraMatrix2.CppPtr, distCoeffs2.CppPtr, imageSize.CppPtr, out rvecPtr, out tvecPtr, out EPtr, out FPtr, (int)flags,
criteria.CppPtr, exception.CppPtr);
rvec = new Vec3d(rvecPtr);
tvec = new Vec3d(tvecPtr);
E = new Mat(EPtr);
F = new Mat(FPtr);
exception.Check();
return(error);
}
