/// <summary>
/// 更新量測結果.
/// 量測演算法放這裡
/// </summary>
public override void UpdateResults()
{
if (mMeasAssist.mImage == null)
{
return;
}
//init result
mResult = new FitLineResult()
{
Col1 = new HTuple(),
Row1 = new HTuple(),
Col2 = new HTuple(),
Row2 = new HTuple(),
};
var image = mMeasAssist.getImage();
HObject imageReduced;
HOperatorSet.GenEmptyObj(out imageReduced);
//建 ROI
var roiModel = mRoi.getModelData();
var row = roiModel[0];
var column = roiModel[1];
var phi = roiModel[2].D * -1; //修正為 Retangle 2 的方向
var length1 = roiModel[3];
var length2 = roiModel[4];
HRegion region = new HRegion();
region.GenRectangle2(row.D, column.D, phi, length1.D, length2.D);
HOperatorSet.ReduceDomain(image, region, out imageReduced);
try
{
HOperatorSet.WriteImage(imageReduced, "tiff", 0, @"D:\ttt.tif");
}
catch (Exception)
{
}
var contoursSplit = extractEdges(imageReduced);
try
{
if (mMeasAssist.ApplyCalibration && mMeasAssist.IsCalibrationValid)
{
if (_cameraOut.TupleLength() == 0)
{
HOperatorSet.ChangeRadialDistortionCamPar("adaptive", mMeasAssist.CameraIn, 0, out _cameraOut);
}
HObject calibrationContoursSplits;
HOperatorSet.GenEmptyObj(out calibrationContoursSplits);
//var imageRect = image.ChangeRadialDistortionImage(region, mMeasAssist.CameraIn, _cameraOut);
//mResult = fitline(imageRect, true);
HOperatorSet.ChangeRadialDistortionContoursXld(contoursSplit, out calibrationContoursSplits, mMeasAssist.CameraIn, _cameraOut);
mResult = fitline(calibrationContoursSplits);
}
else
{
mResult = fitline(contoursSplit);
}
mResultWorld = new FitLineResult(mResult);
}
catch (HOperatorException ex)
{
Hanbo.Log.LogManager.Error(ex);
}
UpdateXLD();
}
// Main procedure
private void RunPick(bool leftpallet, bool lastlayer)
{
// Stack for temporary objects
HObject[] OTemp = new HObject[20];
// Local iconic variables
HObject ho_Image, ho_ImageRectified = null, ho_FOV;
HObject ho_Box, ho_ImageReduced, ho_Objects, ho_Cross = null;
HObject ho_GripPoint = null, ho_ImageReducedGripPoint = null;
HObject ho_ImageMedian = null, ho_ObjectGripPoint = null, ho_Cross1 = null;
HObject ho_Arrow = null, ho_Cross2 = null, ho_ContCircle;
// Local control variables
HTuple hv_AcqHandle = null, hv_pi = null, hv_angle = null;
HTuple hv_CamParam = null, hv_CamPose = null, hv_GMMHandle = null;
HTuple hv_Width = null, hv_Height = null, hv_CamParamOut = null;
HTuple hv_Classes1 = null, hv_Row1 = null, hv_Column1 = null;
HTuple hv_Row2 = null, hv_Column2 = null, hv_Diameter = null;
HTuple hv_Area1 = null, hv_Row3 = null, hv_Column3 = null;
HTuple hv_len_area = null, hv_MAX = new HTuple(), hv_index = new HTuple();
HTuple hv_out = new HTuple(), hv_Length = new HTuple();
HTuple hv_i = new HTuple(), hv_INDEX = new HTuple(), hv_j = new HTuple();
HTuple hv_pom = new HTuple(), hv_DevDia = new HTuple();
HTuple hv_joint = new HTuple(), hv_x_ = new HTuple(), hv_y_ = new HTuple();
HTuple hv_w_ = new HTuple(), hv_a = new HTuple(), hv_b = new HTuple();
HTuple hv_k = new HTuple(), hv_x_cross = new HTuple();
HTuple hv_y_cross = new HTuple(), hv_WorldPose = new HTuple();
HTuple hv_HomMat3D = new HTuple(), hv_HomMat3DRotate = new HTuple();
HTuple hv_Area2 = new HTuple();
HTuple hv_Row5 = new HTuple(), hv_Column5 = new HTuple();
HTuple hv_SumX = new HTuple(), hv_SumY = new HTuple();
HTuple hv_GripPointX = new HTuple(), hv_GripPointY = new HTuple();
HTuple hv_maxdist = new HTuple(), hv_distgrippoints = new HTuple();
HTuple hv_indexgrip = new HTuple(), hv_distgrip = new HTuple();
HTuple hv_OrientX = new HTuple(), hv_OrientY = new HTuple();
HTuple hv_maxX = new HTuple(), hv_diff01 = new HTuple();
HTuple hv_diff02 = new HTuple(), hv_diff12 = new HTuple();
HTuple hv_mindiff = new HTuple(), hv_GX = new HTuple();
HTuple hv_GY = new HTuple(), hv_OX = new HTuple(), hv_OY = new HTuple();
HTuple hv_DY = new HTuple(), hv_DX = new HTuple(), hv_theta = new HTuple();
HTuple hv_anglerad = new HTuple();
//
HTuple hv_Pixel1Y = new HTuple(), hv_Pixel2X = new HTuple(), hv_Pixel1X = new HTuple();
HTuple hv_Pixel2Y = new HTuple(), hv_distance = new HTuple();
// Initialize local and output iconic variables
HOperatorSet.GenEmptyObj(out ho_Image);
HOperatorSet.GenEmptyObj(out ho_ImageRectified);
HOperatorSet.GenEmptyObj(out ho_FOV);
HOperatorSet.GenEmptyObj(out ho_Box);
HOperatorSet.GenEmptyObj(out ho_ImageReduced);
HOperatorSet.GenEmptyObj(out ho_Objects);
HOperatorSet.GenEmptyObj(out ho_Cross);
HOperatorSet.GenEmptyObj(out ho_GripPoint);
HOperatorSet.GenEmptyObj(out ho_ImageReducedGripPoint);
HOperatorSet.GenEmptyObj(out ho_ImageMedian);
HOperatorSet.GenEmptyObj(out ho_ObjectGripPoint);
HOperatorSet.GenEmptyObj(out ho_Cross1);
HOperatorSet.GenEmptyObj(out ho_Arrow);
HOperatorSet.GenEmptyObj(out ho_Cross2);
HOperatorSet.GenEmptyObj(out ho_ContCircle);
// Set Default Variables
hv_angledeg = 0;
// Wait for CAM4 thread to be closed
_waitHandleCam1.WaitOne();
// Close te thread DOOR
_waitHandleCam1.Reset();
//Image Acquisition
try
{
HOperatorSet.OpenFramegrabber("GigEVision", 0, 0, 0, 0, 0, 0, "default", -1, "default", -1, "false", "default", "RobotPick", 0, -1, out hv_AcqHandleCam1);
HOperatorSet.SetFramegrabberParam(hv_AcqHandleCam1, "ExposureAuto", "Continuous");
}
catch (HalconException exceptionCameraNotConnected)
{
}
// Constants
hv_pi = 3.14159265359;
hv_angle = (0 * hv_pi) / 180;
// Pallet calibration selection
if (leftpallet)
{
// Relative path to file
string IntrinsicsFileName = "CamParametersLeft.cal";
string IntrinsicsPath = Path.Combine(Environment.CurrentDirectory, @"CamPar", IntrinsicsFileName);
//
string ExtrinsicsFileName = "CamPoseLeft.dat";
string ExtrinsicsPath = Path.Combine(Environment.CurrentDirectory, @"CamPar", ExtrinsicsFileName);
HOperatorSet.ReadCamPar(IntrinsicsPath, out hv_CamParam);
HOperatorSet.ReadPose(ExtrinsicsPath, out hv_CamPose);
}
else
{
// Relative path to file
string IntrinsicsFileName = "CamParametersRight.cal";
string IntrinsicsPath = Path.Combine(Environment.CurrentDirectory, @"CamPar", IntrinsicsFileName);
//
string ExtrinsicsFileName = "CamPoseRight.dat";
string ExtrinsicsPath = Path.Combine(Environment.CurrentDirectory, @"CamPar", ExtrinsicsFileName);
HOperatorSet.ReadCamPar(IntrinsicsPath, out hv_CamParam);
HOperatorSet.ReadPose(ExtrinsicsPath, out hv_CamPose);
}
//
string TrainegGmmFileName = "traineg_gmm_pick_1.4.ggc";
string TrainegGmmPath = Path.Combine(Environment.CurrentDirectory, @"Train", TrainegGmmFileName);
// Camera intrinsics and extrinsics call
HOperatorSet.ReadClassGmm(TrainegGmmPath, out hv_GMMHandle);
// Grab operator
ho_Image.Dispose();
HOperatorSet.GrabImage(out ho_Image, hv_AcqHandleCam1);
HOperatorSet.GetImageSize(ho_Image, out hv_Width, out hv_Height);
ho_FOV.Dispose();
HOperatorSet.GenRectangle1(out ho_FOV, 0, 0, hv_Height, hv_Width);
ho_Box.Dispose();
if (leftpallet)
{
HOperatorSet.GenRectangle1(out ho_Box, 190, 160, 870, 1130);
}
else
{
HOperatorSet.GenRectangle1(out ho_Box, 190, 160, 870, 1130);
}
HOperatorSet.ChangeRadialDistortionCamPar("adaptive", hv_CamParam, 0, out hv_CamParamOut);
ho_ImageRectified.Dispose();
HOperatorSet.ChangeRadialDistortionImage(ho_Image, ho_FOV, out ho_ImageRectified, hv_CamParam, hv_CamParamOut);
ho_ImageReduced.Dispose();
HOperatorSet.ReduceDomain(ho_ImageRectified, ho_Box, out ho_ImageReduced);
ho_ImageRectified.Dispose();
HOperatorSet.Illuminate(ho_ImageReduced, out ho_ImageRectified, 230, 230, 1.15);
ho_ImageMedian.Dispose();
HOperatorSet.MedianImage(ho_ImageRectified, out ho_ImageRectified, "circle", 3, "mirrored");
ho_ObjectGripPoint.Dispose();
//segmentacija i klasifikacija
ho_Objects.Dispose();
segment(ho_ImageRectified, out ho_Objects);
classify(ho_Objects, hv_GMMHandle, out hv_Classes1);
disp_obj_class(ho_Objects, hv_Classes1);
{
HObject ExpTmpOutVar_0;
HOperatorSet.Connection(ho_Objects, out ExpTmpOutVar_0);
ho_Objects.Dispose();
ho_Objects = ExpTmpOutVar_0;
}
//prefiltriranje klasificiranih predmeta
{
HObject ExpTmpOutVar_0;
HOperatorSet.SelectShape(ho_Objects, out ExpTmpOutVar_0, "circularity", "and", 0.5, 1.0);
ho_Objects.Dispose();
ho_Objects = ExpTmpOutVar_0;
}
{
HObject ExpTmpOutVar_0;
HOperatorSet.SelectShape(ho_Objects, out ExpTmpOutVar_0, "area", "and", 1000, 100000);
ho_Objects.Dispose();
ho_Objects = ExpTmpOutVar_0;
}
//** Carolija ***
HOperatorSet.DiameterRegion(ho_Objects, out hv_Row1, out hv_Column1, out hv_Row2, out hv_Column2, out hv_Diameter);
HOperatorSet.AreaCenter(ho_Objects, out hv_Area1, out hv_Row3, out hv_Column3);
HOperatorSet.TupleLength(hv_Area1, out hv_len_area);
//* If objects exist sort them and find their 2D position
if ((int)(new HTuple(hv_len_area.TupleGreater(0))) != 0)
{
hv_MAX = 0;
hv_index = 0;
hv_out = 0;
HOperatorSet.TupleLength(hv_Diameter, out hv_Length);
HOperatorSet.DispObj(ho_ImageRectified, hv_ExpDefaultWinHandle);
HOperatorSet.DispLine(hv_ExpDefaultWinHandle, hv_Height / 2, 0, hv_Height / 2, hv_Width);
HOperatorSet.DispLine(hv_ExpDefaultWinHandle, 0, hv_Width / 2, hv_Height, hv_Width / 2);
hv_i = 1;
HOperatorSet.TupleGenSequence(0, hv_Length - 1, 1, out hv_INDEX);
while ((int)(new HTuple(hv_i.TupleLess(hv_Length))) != 0)
{
hv_j = hv_i.Clone();
while ((int)(new HTuple(((hv_Diameter.TupleSelect(hv_j - 1))).TupleLess(hv_Diameter.TupleSelect(hv_j)))) != 0)
{
hv_pom = hv_Diameter.TupleSelect(hv_j);
if (hv_Diameter == null)
{
hv_Diameter = new HTuple();
}
hv_Diameter[hv_j] = hv_Diameter.TupleSelect(hv_j - 1);
if (hv_Diameter == null)
{
hv_Diameter = new HTuple();
}
hv_Diameter[hv_j - 1] = hv_pom;
hv_pom = hv_INDEX.TupleSelect(hv_j);
if (hv_INDEX == null)
{
hv_INDEX = new HTuple();
}
hv_INDEX[hv_j] = hv_INDEX.TupleSelect(hv_j - 1);
if (hv_INDEX == null)
{
hv_INDEX = new HTuple();
}
hv_INDEX[hv_j - 1] = hv_pom;
hv_j = hv_j - 1;
if ((int)(new HTuple(hv_j.TupleEqual(0))) != 0)
{
break;
}
}
hv_i = hv_i + 1;
}
HOperatorSet.TupleMedian(hv_Diameter, out hv_MAX);
HOperatorSet.TupleDeviation(hv_Diameter, out hv_DevDia);
HTuple end_val75 = hv_Length;
HTuple step_val75 = 1;
for (hv_i = 1; hv_i.Continue(end_val75, step_val75); hv_i = hv_i.TupleAdd(step_val75))
{
if ((int)((new HTuple(((hv_Diameter.TupleSelect(hv_i - 1))).TupleGreaterEqual(
hv_MAX - (5 * hv_DevDia)))).TupleAnd(new HTuple(((hv_Diameter.TupleSelect(
hv_i - 1))).TupleLessEqual(hv_MAX + (5 * hv_DevDia))))) != 0)
{
if ((int)(new HTuple(hv_MAX.TupleGreaterEqual(2 * 300))) != 0)
{
hv_joint = 1;
}
hv_index = hv_index + 1;
}
}
//* w_ => weight factor for L1 norm **
//* row component weighting a => sort by column **
//* column component weighting b => sort by row **
hv_x_ = new HTuple();
hv_y_ = new HTuple();
hv_w_ = new HTuple();
hv_a = 0.2;
hv_b = 1.0;
HTuple end_val93 = hv_index;
HTuple step_val93 = 1;
for (hv_k = 1; hv_k.Continue(end_val93, step_val93); hv_k = hv_k.TupleAdd(step_val93))
{
if (hv_x_ == null)
{
hv_x_ = new HTuple();
}
hv_x_[hv_k - 1] = hv_Row3.TupleSelect(hv_k - 1);
if (hv_y_ == null)
{
hv_y_ = new HTuple();
}
hv_y_[hv_k - 1] = hv_Column3.TupleSelect(hv_k - 1);
if (hv_w_ == null)
{
hv_w_ = new HTuple();
}
hv_w_[hv_k - 1] = ((hv_x_.TupleSelect(hv_k - 1)) * hv_a) + ((hv_y_.TupleSelect(hv_k - 1)) * hv_b);
}
//* Sort objects by cost value **
hv_i = 1;
while ((int)(new HTuple(hv_i.TupleLess(hv_index))) != 0)
{
hv_j = hv_i.Clone();
while ((int)(new HTuple(((hv_w_.TupleSelect(hv_j - 1))).TupleGreater(hv_w_.TupleSelect(hv_j)))) != 0)
{
hv_pom = hv_x_.TupleSelect(hv_j);
if (hv_x_ == null)
{
hv_x_ = new HTuple();
}
hv_x_[hv_j] = hv_x_.TupleSelect(hv_j - 1);
if (hv_x_ == null)
{
hv_x_ = new HTuple();
}
hv_x_[hv_j - 1] = hv_pom;
hv_pom = hv_y_.TupleSelect(hv_j);
if (hv_y_ == null)
{
hv_y_ = new HTuple();
}
hv_y_[hv_j] = hv_y_.TupleSelect(hv_j - 1);
if (hv_y_ == null)
{
hv_y_ = new HTuple();
}
hv_y_[hv_j - 1] = hv_pom;
hv_pom = hv_w_.TupleSelect(hv_j);
if (hv_w_ == null)
{
hv_w_ = new HTuple();
}
hv_w_[hv_j] = hv_w_.TupleSelect(hv_j - 1);
if (hv_w_ == null)
{
hv_w_ = new HTuple();
}
hv_w_[hv_j - 1] = hv_pom;
hv_j = hv_j - 1;
if ((int)(new HTuple(hv_j.TupleEqual(0))) != 0)
{
break;
}
}
hv_i = hv_i + 1;
}
//* Display detected objects **
HTuple end_val124 = hv_index;
HTuple step_val124 = 1;
for (hv_k = 1; hv_k.Continue(end_val124, step_val124); hv_k = hv_k.TupleAdd(step_val124))
{
hv_x_cross = hv_x_.TupleSelect(hv_k - 1);
hv_y_cross = hv_y_.TupleSelect(hv_k - 1);
HOperatorSet.SetColor(hv_ExpDefaultWinHandle, "green");
ho_Cross.Dispose();
HOperatorSet.GenCrossContourXld(out ho_Cross, hv_x_cross, hv_y_cross, 200, 0);
HOperatorSet.DispObj(ho_Cross, hv_ExpDefaultWinHandle);
HOperatorSet.SetColor(hv_ExpDefaultWinHandle, "red");
HOperatorSet.SetTposition(hv_ExpDefaultWinHandle, hv_x_cross, hv_y_cross);
HOperatorSet.WriteString(hv_ExpDefaultWinHandle, hv_k);
}
HOperatorSet.SetOriginPose(hv_CamPose, 0, 0, 0.00, out hv_WorldPose);
HOperatorSet.PoseToHomMat3d(hv_WorldPose, out hv_HomMat3D);
HOperatorSet.HomMat3dRotateLocal(hv_HomMat3D, hv_angle, "z", out hv_HomMat3DRotate);
HOperatorSet.HomMat3dToPose(hv_HomMat3DRotate, out hv_WorldPose);
HOperatorSet.ImagePointsToWorldPlane(hv_CamParamOut, hv_WorldPose, hv_x_.TupleSelect(0), hv_y_.TupleSelect(0), "mm", out hv_X, out hv_Y);
// Diameter check // Still not working
//HOperatorSet.ImagePointsToWorldPlane(hv_CamParamOut, hv_WorldPose, hv_Row1, hv_Column1, "mm", out hv_Pixel1X, out hv_Pixel1Y);
//HOperatorSet.ImagePointsToWorldPlane(hv_CamParamOut, hv_WorldPose, hv_Row2, hv_Column2, "mm", out hv_Pixel2X, out hv_Pixel2Y);
//HOperatorSet.DistancePp(hv_Pixel1X, hv_Pixel1Y, hv_Pixel2X, hv_Pixel2Y, out hv_distance);
//HOperatorSet.TupleMean(hv_distance, out hv_distance_mean);
HOperatorSet.SetTposition(hv_ExpDefaultWinHandle, 100, 100);
HOperatorSet.WriteString(hv_ExpDefaultWinHandle, hv_a);
HOperatorSet.WriteString(hv_ExpDefaultWinHandle, ", ");
HOperatorSet.WriteString(hv_ExpDefaultWinHandle, hv_b);
HOperatorSet.SetTposition(hv_ExpDefaultWinHandle, 20, 20);
HOperatorSet.WriteString(hv_ExpDefaultWinHandle, hv_Length);
}
//* If objects not found check for layer holes
else if ((int)(new HTuple(hv_len_area.TupleGreater(0))) == 0 && lastlayer)
{
hv_X = 0;
hv_Y = 0;
hv_angledeg = 0;
HOperatorSet.ClearWindow(hv_ExpDefaultWinHandle);
HOperatorSet.DispObj(ho_ImageReduced, hv_ExpDefaultWinHandle);
HOperatorSet.SetColor(hv_ExpDefaultWinHandle, "red");
HOperatorSet.SetTposition(hv_ExpDefaultWinHandle, 120, 450);
HOperatorSet.WriteString(hv_ExpDefaultWinHandle, "NA ZADNJEM SLOJU NISU PRONADENI KOMADI! ROBOT IDE NA IDUCU ULAZNU PALETU");
App.PaletaPrazna();
}
else
{
try
{
ho_GripPoint.Dispose();
HOperatorSet.GenRectangle1(out ho_GripPoint, 420, 520, 620, 770);
ho_ImageReducedGripPoint.Dispose();
HOperatorSet.ReduceDomain(ho_ImageRectified, ho_GripPoint, out ho_ImageReducedGripPoint);
ho_ImageMedian.Dispose();
HOperatorSet.MedianImage(ho_ImageReducedGripPoint, out ho_ImageMedian, "circle", 3, "mirrored");
ho_ObjectGripPoint.Dispose();
segment(ho_ImageMedian, out ho_ObjectGripPoint);
{
HObject ExpTmpOutVar_0;
HOperatorSet.Connection(ho_ObjectGripPoint, out ExpTmpOutVar_0);
ho_ObjectGripPoint.Dispose();
ho_ObjectGripPoint = ExpTmpOutVar_0;
}
{
HObject ExpTmpOutVar_0;
HOperatorSet.SelectShape(ho_ObjectGripPoint, out ExpTmpOutVar_0, "circularity", "and", 0.5,
1.0);
ho_ObjectGripPoint.Dispose();
ho_ObjectGripPoint = ExpTmpOutVar_0;
}
{
HObject ExpTmpOutVar_0;
HOperatorSet.SelectShape(ho_ObjectGripPoint, out ExpTmpOutVar_0, "area", "and", 100, 1000);
ho_ObjectGripPoint.Dispose();
ho_ObjectGripPoint = ExpTmpOutVar_0;
}
HOperatorSet.AreaCenter(ho_ObjectGripPoint, out hv_Area2, out hv_Row5, out hv_Column5);
HOperatorSet.TupleSum(hv_Row5, out hv_SumX);
HOperatorSet.TupleSum(hv_Column5, out hv_SumY);
hv_GripPointX = hv_SumX / 9;
hv_GripPointY = hv_SumY / 9;
ho_Cross1.Dispose();
HOperatorSet.GenCrossContourXld(out ho_Cross1, hv_GripPointX, hv_GripPointY, 10, 0);
hv_maxdist = 0;
hv_distgrippoints = new HTuple();
hv_indexgrip = 0;
for (hv_i = 1; (int)hv_i <= 9; hv_i = (int)hv_i + 1)
{
HOperatorSet.DistancePp(hv_Row5.TupleSelect(hv_i - 1), hv_Column5.TupleSelect(hv_i - 1),
hv_GripPointX, hv_GripPointY, out hv_distgrip);
HOperatorSet.TupleConcat(hv_distgrippoints, hv_distgrip, out hv_distgrippoints);
if ((int)(new HTuple(hv_distgrip.TupleGreater(hv_maxdist))) != 0)
{
hv_indexgrip = hv_i - 1;
hv_maxdist = hv_distgrip.Clone();
}
}
hv_indexgrip = new HTuple();
for (hv_i = 1; (int)hv_i <= 9; hv_i = (int)hv_i + 1)
{
if ((int)(new HTuple(
((hv_distgrippoints.TupleSelect(hv_i - 1))).TupleGreater(0.9 * hv_maxdist))) != 0)
{
HOperatorSet.TupleConcat(hv_indexgrip, hv_i - 1, out hv_indexgrip);
}
}
hv_OrientX = 0;
hv_OrientY = 0;
hv_maxX = 0;
HOperatorSet.TupleAbs(
(hv_Row5.TupleSelect(hv_indexgrip.TupleSelect(0))) -
(hv_Row5.TupleSelect(hv_indexgrip.TupleSelect(1))), out hv_diff01);
HOperatorSet.TupleAbs(
(hv_Row5.TupleSelect(hv_indexgrip.TupleSelect(0))) -
(hv_Row5.TupleSelect(hv_indexgrip.TupleSelect(2))), out hv_diff02);
HOperatorSet.TupleAbs(
(hv_Row5.TupleSelect(hv_indexgrip.TupleSelect(1))) -
(hv_Row5.TupleSelect(hv_indexgrip.TupleSelect(2))), out hv_diff12);
HOperatorSet.TupleMin(((hv_diff01.TupleConcat(hv_diff02))).TupleConcat(hv_diff12), out hv_mindiff);
if ((int)(new HTuple(hv_mindiff.TupleEqual(hv_diff01))) != 0)
{
hv_OrientX = hv_Row5.TupleSelect(hv_indexgrip.TupleSelect(2));
hv_OrientY = hv_Column5.TupleSelect(hv_indexgrip.TupleSelect(2));
}
else if ((int)(new HTuple(hv_mindiff.TupleEqual(hv_diff02))) != 0)
{
hv_OrientX = hv_Row5.TupleSelect(hv_indexgrip.TupleSelect(1));
hv_OrientY = hv_Column5.TupleSelect(hv_indexgrip.TupleSelect(1));
}
else if ((int)(new HTuple(hv_mindiff.TupleEqual(hv_diff12))) != 0)
{
hv_OrientX = hv_Row5.TupleSelect(hv_indexgrip.TupleSelect(0));
hv_OrientY = hv_Column5.TupleSelect(hv_indexgrip.TupleSelect(0));
}
HOperatorSet.DispObj(ho_ImageReduced, hv_ExpDefaultWinHandle);
HOperatorSet.SetColor(hv_ExpDefaultWinHandle, "red");
ho_Arrow.Dispose();
gen_arrow_contour_xld(out ho_Arrow, hv_GripPointX, hv_GripPointY, hv_OrientX, hv_OrientY, 15, 15);
HOperatorSet.DispObj(ho_Arrow, hv_ExpDefaultWinHandle);
HOperatorSet.SetColor(hv_ExpDefaultWinHandle, "green");
ho_Cross2.Dispose();
HOperatorSet.GenCrossContourXld(out ho_Cross2, hv_GripPointX, hv_GripPointY, 30, 0);
//HOperatorSet.DispObj(ho_Cross2, hv_ExpDefaultWinHandle);
HOperatorSet.SetOriginPose(hv_CamPose, 0, 0, 0.00, out hv_WorldPose);
HOperatorSet.ImagePointsToWorldPlane(hv_CamParamOut, hv_WorldPose, hv_GripPointX, hv_GripPointY, "mm", out hv_GX, out hv_GY);
HOperatorSet.ImagePointsToWorldPlane(hv_CamParamOut, hv_WorldPose, hv_OrientX, hv_OrientY, "mm", out hv_OX, out hv_OY);
hv_DY = hv_OY - hv_GY;
hv_DX = hv_OX - hv_GX;
HOperatorSet.TupleAtan2(hv_DY, hv_DX, out hv_theta);
hv_theta = hv_theta.Clone();
hv_X = hv_GX;
hv_Y = hv_GY;
hv_angledeg = (hv_theta * 180) / hv_pi;
}
catch (HalconException exceptionLayerNotFound)
{
hv_X = 0;
hv_Y = 0;
hv_angledeg = 0;
HOperatorSet.ClearWindow(hv_ExpDefaultWinHandle);
HOperatorSet.DispObj(ho_ImageReduced, hv_ExpDefaultWinHandle);
HOperatorSet.SetColor(hv_ExpDefaultWinHandle, "red");
HOperatorSet.SetTposition(hv_ExpDefaultWinHandle, 120, 512);
HOperatorSet.WriteString(hv_ExpDefaultWinHandle, "NA PALETI NISU PRONADENI KOMADI NI SLOJEVI!!!");
App.LayerNijePronaden();
}
}
// HOperatorSet.ClearWindow(hv_ExpDefaultWinHandle);
HOperatorSet.SetColor(hv_ExpDefaultWinHandle, "red");
ho_ContCircle.Dispose();
HOperatorSet.GenCircleContourXld(out ho_ContCircle, hv_Height / 2, hv_Width / 2, 100, 0, 6.28318, "positive", 10);
ho_Image.Dispose();
ho_ImageRectified.Dispose();
ho_FOV.Dispose();
ho_Box.Dispose();
ho_ImageReduced.Dispose();
ho_Objects.Dispose();
ho_Cross.Dispose();
ho_GripPoint.Dispose();
ho_ImageReducedGripPoint.Dispose();
ho_ImageMedian.Dispose();
ho_ObjectGripPoint.Dispose();
ho_Cross1.Dispose();
ho_Arrow.Dispose();
ho_Cross2.Dispose();
ho_ContCircle.Dispose();
HOperatorSet.CloseFramegrabber(hv_AcqHandleCam1);
// Open the thread DOOR
_waitHandleCam1.Set();
}