/// <summary>
/// Tính toán ma trận chuyển đổi điểm TT(PBASE) từ cmd
/// </summary>
/// <returns></returns>
public static Matrix4x4 Calculate(PointWithTheta inputPoint, PointWithTheta alignPointOCam, Matrix4x4 transMatrixPOROC)
{
float tempX = inputPoint.X;
float tempY = inputPoint.Y;
float tempTheta = inputPoint.Theta;
Matrix4x4 PGET = new Matrix4x4((float)Math.Cos(tempTheta), (float)-Math.Sin(tempTheta), 0, tempX,
(float)Math.Sin(tempTheta), (float)Math.Cos(tempTheta), 0, tempY,
0, 0, 1, 0,
0, 0, 0, 1);
tempX = alignPointOCam.X;
tempY = alignPointOCam.Y;
tempTheta = alignPointOCam.Theta;
Matrix4x4 PVISION = new Matrix4x4((float)Math.Cos(tempTheta), (float)-Math.Sin(tempTheta), 0, tempX,
(float)Math.Sin(tempTheta), (float)Math.Cos(tempTheta), 0, tempY,
0, 0, 1, 0,
0, 0, 0, 1);
Matrix4x4 PGET_OCINV;
Matrix4x4.Invert(PGET, out PGET_OCINV);
Matrix4x4 outputMatrix = Matrix4x4.Multiply(PGET_OCINV, transMatrixPOROC);
outputMatrix = Matrix4x4.Multiply(outputMatrix, PVISION);
return(outputMatrix);
}
/// <summary>
/// Tính toán TT
/// Lấy điểm Input từ Robot
/// Chuyển đổi từ điểm Align Camera qua Tool chuyển NPoint
/// Tính toán từ 2 điểm trên ra ma trận TT
/// </summary>
/// <param name="cmd"></param>
public void CalTTTransMatrix(string cmd, PointWithTheta alignPoint)
{
PointWithTheta inputPoint = Helper.GetRobotPointFromCmd(cmd);
PointWithTheta alignPointOCam = Helper.TransPointFromNPoint(PointTransformToolFromNPointCalib, alignPoint);
TransformTT = TransformTTCal.Calculate(inputPoint, alignPointOCam, TransMatrixPOROV);
CalTTMatrixOK = true;
}
/// <summary>
///
/// </summary>
/// <returns></returns>
public PointWithTheta CalculateAlignRB()
{
PointWithTheta outputAlign = null;
CogAcqFifoEdit.Subject.Run();
// Chụp ảnh, gửi ảnh sang Tool Calib
if (CogAcqFifoEdit.Subject.RunStatus.Result != CogToolResultConstants.Accept)
{
MessageBox.Show(CogAcqFifoEdit.Subject.RunStatus.Exception.Message);
return(null);
}
else
{
CogCalibGrid.Subject.InputImage = CogAcqFifoEdit.Subject.OutputImage;
}
CogCalibGrid.Subject.Run();
// Calib xong gửi ảnh qua Tool Align
if (CogCalibGrid.Subject.RunStatus.Result != CogToolResultConstants.Accept)
{
MessageBox.Show(CogCalibGrid.Subject.RunStatus.Exception.Message);
return(null);
}
else
{
CogPMAlign.Subject.InputImage = CogCalibGrid.Subject.OutputImage;
}
CogPMAlign.Subject.Run();
// Chạy xong Tool Align trả tọa độ + góc ra đầu ra
if (CogPMAlign.Subject.RunStatus.Result != CogToolResultConstants.Accept)
{
MessageBox.Show(CogPMAlign.Subject.RunStatus.Exception.Message);
return(null);
}
else
{
DisplayGraphic();
try
{
float tempX = (float)CogPMAlign.Subject.Results[0].GetPose().TranslationX;
float tempY = (float)CogPMAlign.Subject.Results[0].GetPose().TranslationY;
float tempTheta = (float)CogPMAlign.Subject.Results[0].GetPose().Rotation;
// Trả hiển thị ra CogDisplay
//CogDisplayOut.Image = CogAcqFifoEdit.Subject.OutputImage;
//CogDisplayOut.StaticGraphics.Add(CogPMAlign.Subject.Results[0].CreateResultGraphics(CogPMAlignResultGraphicConstants.CoordinateAxes), "");
outputAlign = new PointWithTheta(tempX, tempY, tempTheta);
}
catch
{
outputAlign = null;
}
}
if (outputAlign != null)
{
return(RTCAutoCalibTool.Trans(outputAlign));
}
return(null);
}
/// <summary>
/// Chuyển đổi tọa độ 1 điểm qua ma trận chuyển đổi N Point
/// </summary>
/// <param name="pointTransformToolFromNPointCalib"></param>
/// <param name="inputPoint"></param>
/// <returns></returns>
public static PointWithTheta TransPointFromNPoint(ICogTransform2D pointTransformToolFromNPointCalib, PointWithTheta inputPoint)
{
double tempX, tempY;
pointTransformToolFromNPointCalib.MapPoint(inputPoint.X, inputPoint.Y, out tempX, out tempY);
PointWithTheta outputPoint = new PointWithTheta((float)tempX, (float)tempY, inputPoint.Theta);
return(outputPoint);
}
public static Matrix4x4[] CalPBaseAndPOROC(PointWithTheta robotPoint1, PointWithTheta robotPoint2, PointWithTheta camPoint1, PointWithTheta camPoint2)
{
Matrix4x4 PGetOC = CalPGET_OC(robotPoint1, robotPoint2, camPoint1, camPoint2);
Matrix4x4 PBase = CalPBASE(PGetOC, robotPoint1, camPoint1);
Matrix4x4 POROC = CalPOROC(robotPoint1, PGetOC);
return(new Matrix4x4[2] {
PBase, POROC
});
}
/// <summary>
/// Tính toán ma trận chuyển đổi hệ Robot sang hệ Camera
/// Đầu vào 2 điểm góc xoay hệ tọa độ Camera + điểm tọa độ Robot
/// </summary>
/// <param name="pointCam1"></param>
/// <param name="pointCam2"></param>
/// <param name="pointRB"></param>
/// <returns></returns>
public static Matrix4x4 Calculate(PointWithTheta pointCam1, PointWithTheta pointCam2, PointWithTheta pointRB)
{
Matrix4x4 outputMatrixInv;
double[] PxyGetOV = CalPxPy(pointCam1.X, pointCam1.Y, pointCam1.Theta, pointCam2.X, pointCam2.Y, pointCam2.Theta);
Matrix4x4 outputMatrix = CalMatrix(pointRB.X, pointRB.Y, pointRB.Theta, PxyGetOV[0], PxyGetOV[1]);
Matrix4x4.Invert(outputMatrix, out outputMatrixInv);
return(outputMatrixInv);
}
/// <summary>
/// Tool chuyển đổi tọa độ Align trả về từ Camera thành tọa độ Robot
/// Nếu chưa đủ điều kiện trả về null
/// </summary>
/// <param name="outputAlign"></param>
/// <returns></returns>
public PointWithTheta Trans(PointWithTheta outputAlign)
{
if (CalNpointOK && CalTTMatrixOK)
{
PointWithTheta inputPointOCam = Helper.TransPointFromNPoint(PointTransformToolFromNPointCalib, outputAlign);
PointWithTheta outputRBPoint = Helper.CalTransAlignToRobot(inputPointOCam, TransformTT, TransMatrixPOROV);
return(outputRBPoint);
}
else
{
return(null);
}
}
public static Matrix4x4 CalPOROC(PointWithTheta robotpoint1, Matrix4x4 PGetOC)
{
float tempX = robotpoint1.X;
float tempY = robotpoint1.Y;
float tempTheta = robotpoint1.Theta;
Matrix4x4 PGet = new Matrix4x4((float)Math.Cos(tempTheta), (float)-Math.Sin(tempTheta), 0, (float)tempX,
(float)Math.Sin(tempTheta), (float)Math.Cos(tempTheta), 0, (float)tempY,
0, 0, 1, 0,
0, 0, 0, 1);
Matrix4x4 invPgetOC;
Matrix4x4.Invert(PGetOC, out invPgetOC);
return(Matrix4x4.Multiply(PGet, invPgetOC));
}
public static Matrix4x4 CalPBASE(Matrix4x4 mPGETOC, PointWithTheta robotPoint1, PointWithTheta camPoint1)
{
float x1 = camPoint1.X;
float y1 = camPoint1.Y;
float thetaGet = robotPoint1.Theta;
float xGetOC = mPGETOC.M14;
float yGetOC = mPGETOC.M24;
float xBase = xBase = (float)((y1 * Math.Sin((System.Double)(2 * thetaGet)) - yGetOC * Math.Sin((System.Double)(2 * thetaGet)) + x1 * Math.Cos((System.Double)(2 * thetaGet)) - xGetOC * Math.Cos((System.Double)(2 * thetaGet)) + x1 - xGetOC) / Math.Cos((System.Double)thetaGet) / 0.2e1);
float yBase = (float)(-Math.Sin(thetaGet) * x1 + Math.Cos(thetaGet) * y1 + Math.Sin(thetaGet) * xGetOC - yGetOC * Math.Cos(thetaGet));
Matrix4x4 mPBASEReturn = new Matrix4x4(1, 0, 0, (float)xBase,
0, 1, 0, (float)yBase,
0, 0, 1, 0,
0, 0, 0, 1);
return(mPBASEReturn);
}
/// <summary>
/// Tính toán điểm khi qua ma trận chuyển vị
/// </summary>
/// <param name="pointWithTheta"></param>
/// <param name="transformMatrixPOROV"></param>
/// <returns></returns>
public static PointWithTheta TransPoint(PointWithTheta pointWithTheta, Matrix4x4 transformMatrixPOROV)
{
float tempX = pointWithTheta.X;
float tempY = pointWithTheta.Y;
float tempTheta = pointWithTheta.Theta;
float tempOffX = (float)((Math.Cos(tempTheta)) * tempX - (Math.Sin(tempTheta)) * tempY);
float tempOffY = (float)((Math.Sin(tempTheta)) * tempX + (Math.Cos(tempTheta)) * tempY);
Matrix4x4 inputMatrix = new Matrix4x4((float)Math.Cos(tempTheta), (float)-Math.Sin(tempTheta), 0, tempX,
(float)Math.Sin(tempTheta), (float)Math.Cos(tempTheta), 0, tempY,
0, 0, 1, 0,
0, 0, 0, 1);
Matrix4x4 outputMatrix = Matrix4x4.Multiply(transformMatrixPOROV, inputMatrix);
PointWithTheta outputPoint = new PointWithTheta(outputMatrix.M14, outputMatrix.M24, (float)Math.Asin(outputMatrix.M21));
return(outputPoint);
}
/// <summary>
/// P_VisionCal = POROC^-1 * PGET * PBASE
/// </summary>
/// <param name="pointWithTheta"></param>
/// <param name="transMatrixPOROV_Inv"></param>
/// <param name="transMatrixBASE"></param>
/// <returns></returns>
private static PointWithTheta TransRobotToCam(PointWithTheta pointWithTheta, Matrix4x4 transMatrixPOROV_Inv, Matrix4x4 transMatrixBASE)
{
float tempX = pointWithTheta.X;
float tempY = pointWithTheta.Y;
float tempTheta = pointWithTheta.Theta;
Matrix4x4 PGET = new Matrix4x4((float)Math.Cos(tempTheta), (float)-Math.Sin(tempTheta), 0, tempX,
(float)Math.Sin(tempTheta), (float)Math.Cos(tempTheta), 0, tempY,
0, 0, 1, 0,
0, 0, 0, 1);
Matrix4x4 outputMatrix = Matrix4x4.Multiply(transMatrixPOROV_Inv, PGET);
outputMatrix = Matrix4x4.Multiply(outputMatrix, transMatrixBASE);
PointWithTheta outputPoint = new PointWithTheta(outputMatrix.M14, outputMatrix.M24, (float)Math.Asin(outputMatrix.M21));
return(outputPoint);
}
public static Matrix4x4 CalPGET_OC(PointWithTheta robotPoint1, PointWithTheta robotPoint2, PointWithTheta camPoint1, PointWithTheta camPoint2)
{
float x1 = camPoint1.X;
float y1 = camPoint1.Y;
float x2 = camPoint2.X;
float y2 = camPoint2.Y;
float thetaGet = robotPoint1.Theta;
float thetaGet2 = robotPoint2.Theta;
float xGetOC = (float)((x1 * Math.Cos((System.Double)(-2 * thetaGet2 + 2 * thetaGet)) + x2 * Math.Cos((System.Double)(-2 * thetaGet2 + 2 * thetaGet)) - 0.2e1 * x1 * Math.Cos((System.Double)(-thetaGet2 + thetaGet)) - 0.2e1 * x2 * Math.Cos((System.Double)(-thetaGet2 + thetaGet)) - 0.2e1 * x1 * Math.Cos((System.Double)(thetaGet2 + thetaGet)) - 0.2e1 * x2 * Math.Cos((System.Double)(thetaGet2 + thetaGet)) + x1 * Math.Cos((System.Double)(2 * thetaGet2)) + x2 * Math.Cos((System.Double)(2 * thetaGet2)) + x1 * Math.Cos((System.Double)(2 * thetaGet)) + x2 * Math.Cos((System.Double)(2 * thetaGet)) - Math.Sin((System.Double)(2 * thetaGet2)) * y1 + y2 * Math.Sin((System.Double)(2 * thetaGet2)) + y1 * Math.Sin((System.Double)(2 * thetaGet)) - Math.Sin((System.Double)(2 * thetaGet)) * y2 + y1 * Math.Sin((System.Double)(-2 * thetaGet2 + 2 * thetaGet)) - y2 * Math.Sin((System.Double)(-2 * thetaGet2 + 2 * thetaGet)) + x1 + x2) / (0.2e1 - 0.4e1 * Math.Cos((System.Double)(-thetaGet2 + thetaGet)) - 0.4e1 * Math.Cos((System.Double)(thetaGet2 + thetaGet)) + 0.2e1 * Math.Cos((System.Double)(-2 * thetaGet2 + 2 * thetaGet)) + 0.2e1 * Math.Cos((System.Double)(2 * thetaGet2)) + 0.2e1 * Math.Cos((System.Double)(2 * thetaGet))));
float yGetOC = (float)((y1 * Math.Cos((System.Double)(-2 * thetaGet2 + 2 * thetaGet)) + y2 * Math.Cos((System.Double)(-2 * thetaGet2 + 2 * thetaGet)) - 0.2e1 * y1 * Math.Cos((System.Double)(-thetaGet2 + thetaGet)) - 0.2e1 * y2 * Math.Cos((System.Double)(-thetaGet2 + thetaGet)) - 0.2e1 * y1 * Math.Cos((System.Double)(thetaGet2 + thetaGet)) - 0.2e1 * y2 * Math.Cos((System.Double)(thetaGet2 + thetaGet)) + Math.Cos((System.Double)(2 * thetaGet2)) * y1 + y2 * Math.Cos((System.Double)(2 * thetaGet2)) + y1 * Math.Cos((System.Double)(2 * thetaGet)) + Math.Cos((System.Double)(2 * thetaGet)) * y2 + x1 * Math.Sin((System.Double)(2 * thetaGet2)) - x2 * Math.Sin((System.Double)(2 * thetaGet2)) - x1 * Math.Sin((System.Double)(2 * thetaGet)) + x2 * Math.Sin((System.Double)(2 * thetaGet)) - x1 * Math.Sin((System.Double)(-2 * thetaGet2 + 2 * thetaGet)) + x2 * Math.Sin((System.Double)(-2 * thetaGet2 + 2 * thetaGet)) + y1 + y2) / (0.2e1 - 0.4e1 * Math.Cos((System.Double)(-thetaGet2 + thetaGet)) - 0.4e1 * Math.Cos((System.Double)(thetaGet2 + thetaGet)) + 0.2e1 * Math.Cos((System.Double)(-2 * thetaGet2 + 2 * thetaGet)) + 0.2e1 * Math.Cos((System.Double)(2 * thetaGet2)) + 0.2e1 * Math.Cos((System.Double)(2 * thetaGet))));
Matrix4x4 mPGETReturn = new Matrix4x4((float)Math.Cos(thetaGet), (float)-Math.Sin(thetaGet), 0, (float)xGetOC,
(float)Math.Sin(thetaGet), (float)Math.Cos(thetaGet), 0, (float)yGetOC,
0, 0, 1, 0,
0, 0, 0, 1);
return(mPGETReturn);
}
internal static PointWithTheta CalTransAlignToRobot(PointWithTheta pointWithTheta, Matrix4x4 transformTT, Matrix4x4 transformMatrixPOROV)
{
float tempX = pointWithTheta.X;
float tempY = pointWithTheta.Y;
float tempTheta = pointWithTheta.Theta;
Matrix4x4 PVISION = new Matrix4x4((float)Math.Cos(tempTheta), (float)-Math.Sin(tempTheta), 0, tempX,
(float)Math.Sin(tempTheta), (float)Math.Cos(tempTheta), 0, tempY,
0, 0, 1, 0,
0, 0, 0, 1);
Matrix4x4 PBASE_INV;
Matrix4x4.Invert(transformTT, out PBASE_INV);
Matrix4x4 outputMatrix = Matrix4x4.Multiply(PVISION, PBASE_INV);
outputMatrix = Matrix4x4.Multiply(transformMatrixPOROV, outputMatrix);
PointWithTheta outputPoint = new PointWithTheta(outputMatrix.M14, outputMatrix.M24, (float)Math.Asin(outputMatrix.M21));
return(outputPoint);
}
/// <summary>
/// Xử lý thêm điểm vào list từ lệnh HE nhận từ Robot
/// </summary>
/// <param name="tempCamPoint"></param>
/// <param name="tempRobotPoint"></param>
public void AddPoint(PointWithTheta tempCamPoint, PointWithTheta tempRobotPoint)
{
ListAutoCalibPointsRB.Add(tempRobotPoint);
ListAutoCalibPointsCam.Add(tempCamPoint);
NumberPoints += 1;
}
/// <summary>
/// Xử lý Command nhận vào từ cổng TCP
/// </summary>
/// <param name="cmd"></param>
public string Command(string cmd)
{
Console.WriteLine(cmd);
switch (Helper.GetCommandId(cmd))
{
case "HEB":
string result = CheckConditionStartAutoCalib();
if (result == "OK")
{
RTCAutoCalibTool.ResetReceiveData();
AutoCalibRunning = true;
}
else
{
MessageBox.Show("Không đủ điều kiện chạy!\r\n" + result);
return("HE,0");
}
break;
case "HE":
PointWithTheta tempCamPoint = GetNormalAlign();
PointWithTheta tempRobotPoint = Helper.GetRobotPointFromCmd(cmd);
if (tempCamPoint != null)
{
RTCAutoCalibTool.AddPoint(tempCamPoint, tempRobotPoint);
}
else
{
return("HE,0");
}
break;
case "HEE":
if (RTCAutoCalibTool.NumberPoints < 11)
{
MessageBox.Show("Not Enough Calib Point! N = " + RTCAutoCalibTool.NumberPoints.ToString());
return("HE,0");
}
if (RTCAutoCalibTool.Calculate())
{
MessageBox.Show("AutoCalib Done!");
AutoCalibRunning = false;
}
else
{
MessageBox.Show("AutoCalib Fail!");
AutoCalibRunning = false;
return("HE,0");
}
break;
case "TT":
var tempPointTT = GetNormalAlign();
if (tempPointTT != null)
{
RTCAutoCalibTool.CalTTTransMatrix(cmd, tempPointTT);
}
return("TT,1");
case "XT":
string tempMessage;
PointWithTheta tempPoint = CalculateAlignRB();
if (tempPoint != null)
{
tempMessage = Helper.CreatXTMessage(tempPoint);
}
else
{
return("XT,0");
}
return(tempMessage);
default:
break;
}
return("HE,1");
}
/// <summary>
/// Chuyển đổi điểm sang định dạng string trả về cho Robot
/// </summary>
/// <param name="point"></param>
/// <returns></returns>
public static string CreatXTMessage(PointWithTheta point)
{
return($"XT,1,{point.X.ToString("###.##")},{point.Y.ToString("###.##")},{point.Theta.ToString("###.##")}");
}
