2026-06-05 13:44:39 +09:00

452 lines
21 KiB
C#

using Aurora.Api;
using MilCommon;
using VisionCommon;
using Hermes;
using Matrox.MatroxImagingLibrary;
using System;
using System.Collections.Generic;
using System.Windows;
using Aurora.Api.DataType;
using System.Windows.Controls.Primitives;
namespace MilPattern {
public class ModelFinderRunParam : IRunParam, ICloneable {
public int FindNumber { get; set; } = 1;
public double Acceptance { get; set; } = 60;
public double Certainty { get; set; } = 90;
public Angle AngleTolerance { get; set; } = Angle.CreateWithDegree(180); // (0, 180)
public double ScaleTolerance { get; set; } = 0.2;
public eMilAccuracy Accuracy { get; set; } = eMilAccuracy.Default;
public eMilDetailLevel DetailLevel { get; set; } = eMilDetailLevel.Default;
public eMilSpeed Speed { get; set; } = eMilSpeed.Default;
public double Score { get; set; } = 60;
public eMilPolarity Polarity { get; set; } = eMilPolarity.Any;
public MmfParamExt MmfParamExt { get; set; } = new MmfParamExt();
public double Smoothness { get; set; } = 50.0;
public bool IsValidFindRoi(ImageSize imageSize) {
return (0 < MmfParamExt.FindROI.Width && MmfParamExt.FindROI.Width < imageSize.Width) &&
(0 < MmfParamExt.FindROI.Height && MmfParamExt.FindROI.Height < imageSize.Height);
}
public ROI GetFindRoi() {
return MmfParamExt.FindROI.Clone() as ROI;
}
public void SetFindROI(ROI findRoi) {
MmfParamExt.FindROI = findRoi;
}
public object Clone() {
return new ModelFinderRunParam() {
FindNumber = FindNumber,
Acceptance = Acceptance,
Certainty = Certainty,
AngleTolerance = AngleTolerance.Clone() as Angle,
ScaleTolerance = ScaleTolerance,
Accuracy = Accuracy,
DetailLevel = DetailLevel,
Speed = Speed,
Score = Score,
Polarity = Polarity,
Smoothness = Smoothness,
MmfParamExt = MmfParamExt.Clone() as MmfParamExt
};
}
}
internal static class MmfCommon {
private static Logger _Logger = new Logger(typeof(MmfCommon).Name);
public static MIL_ID CreateContext(MIL_ID milSystem) {
MIL_ID contextId = MIL.M_NULL;
MIL.MmodAlloc(milSystem, MIL.M_GEOMETRIC, MIL.M_DEFAULT, ref contextId);
return contextId;
}
public static MIL_ID CreateQuickFindContext(MIL_ID milSystem) {
MIL_ID contextId = MIL.M_NULL;
MIL.MmodAlloc(milSystem, MIL.M_GEOMETRIC_CONTROLLED, MIL.M_DEFAULT, ref contextId);
return contextId;
}
public static MIL_ID LoadContext(MIL_ID milSystem, string filePath) {
MIL_ID contextId = MIL.M_NULL;
MIL.MmodRestore(filePath, milSystem, MIL.M_DEFAULT, ref contextId);
if (HasModel(contextId)) {
MIL.MmodPreprocess(contextId, MIL.M_DEFAULT);
}
return contextId;
}
public static int SaveContext(MIL_ID contextId, string filePath) {
if (contextId != MIL.M_NULL && filePath != null) {
MIL.MmodSave(filePath, contextId, MIL.M_DEFAULT);
return E1.OK;
}
return E1.GENERIC;
}
public static void FreeContext(MIL_ID contextId) {
if (contextId != MIL.M_NULL) {
MIL.MmodFree(contextId);
}
}
public static MIL_ID CreateResultContext(MIL_ID contextId) {
var milSystem = GetSystemOfContext(contextId);
return MIL.MmodAllocResult(milSystem, MIL.M_DEFAULT, MIL.M_NULL);
}
public static MIL_ID GetSystemOfContext(MIL_ID contextId) {
return (MIL_ID)MIL.MmodInquire(contextId, MIL.M_CONTEXT, MIL.M_OWNER_SYSTEM, MIL.M_NULL);
}
public static bool IsPreprocessed(MIL_ID contextId) {
if (contextId == MIL.M_NULL) {
return false;
}
return MIL.M_TRUE == MIL.MmodInquire(contextId, MIL.M_CONTEXT, MIL.M_PREPROCESSED, MIL.M_NULL);
}
public static void DeleteAllModels(MIL_ID contextId) {
if (HasModel(contextId)) {
MIL.MmodDefine(contextId, MIL.M_DELETE, MIL.M_ALL, MIL.M_DEFAULT, MIL.M_DEFAULT, MIL.M_DEFAULT, MIL.M_DEFAULT);
}
}
public static bool HasModel(MIL_ID contextId) {
return MIL.MmodInquire(contextId, MIL.M_CONTEXT, MIL.M_NUMBER_MODELS, MIL.M_NULL) > 0;
}
public static ROI GetFindRoiFromMmf(MIL_ID contextId) {
if (HasModel(contextId)) {
double centerX = 0;
MIL.MmodInquire(contextId, MIL.M_DEFAULT, MIL.M_POSITION_X, ref centerX);
double centerY = 0;
MIL.MmodInquire(contextId, MIL.M_DEFAULT, MIL.M_POSITION_Y, ref centerY);
if (centerX != MIL.M_DEFAULT || centerY != MIL.M_DEFAULT) {
double deltaPosX = 0;
MIL.MmodInquire(contextId, MIL.M_DEFAULT, MIL.M_POSITION_DELTA_POS_X, ref deltaPosX);
double deltaNegX = 0;
MIL.MmodInquire(contextId, MIL.M_DEFAULT, MIL.M_POSITION_DELTA_NEG_X, ref deltaNegX);
double deltaPosY = 0;
MIL.MmodInquire(contextId, MIL.M_DEFAULT, MIL.M_POSITION_DELTA_POS_Y, ref deltaPosY);
double deltaNegY = 0;
MIL.MmodInquire(contextId, MIL.M_DEFAULT, MIL.M_POSITION_DELTA_NEG_Y, ref deltaNegY);
return new ROI(
new ImagePoint(centerX, centerY),
deltaPosX + deltaNegX,
deltaPosY + deltaNegY);
}
}
return new ROI();
}
public static int SetFindRoiToMmf(MIL_ID contextId, ROI findRoi) {
if (findRoi.Width > 0 && findRoi.Height > 0) {
if (HasModel(contextId)) {
MIL.MmodControl(contextId, MIL.M_ALL, MIL.M_POSITION_X, findRoi.CenterX);
MIL.MmodControl(contextId, MIL.M_ALL, MIL.M_POSITION_DELTA_POS_X, findRoi.Width * 0.5);
MIL.MmodControl(contextId, MIL.M_ALL, MIL.M_POSITION_DELTA_NEG_X, findRoi.Width * 0.5);
MIL.MmodControl(contextId, MIL.M_ALL, MIL.M_POSITION_Y, findRoi.CenterY);
MIL.MmodControl(contextId, MIL.M_ALL, MIL.M_POSITION_DELTA_POS_Y, findRoi.Height * 0.5);
MIL.MmodControl(contextId, MIL.M_ALL, MIL.M_POSITION_DELTA_NEG_Y, findRoi.Height * 0.5);
}
}
else {
ClearFindRoi(contextId);
}
return E1.OK;
}
public static int ClearFindRoi(MIL_ID contextId) {
if (HasModel(contextId)) {
MIL.MmodControl(contextId, MIL.M_ALL, MIL.M_POSITION_X, MIL.M_DEFAULT);
MIL.MmodControl(contextId, MIL.M_ALL, MIL.M_POSITION_DELTA_POS_X, MIL.M_DEFAULT);
MIL.MmodControl(contextId, MIL.M_ALL, MIL.M_POSITION_DELTA_NEG_X, MIL.M_DEFAULT);
MIL.MmodControl(contextId, MIL.M_ALL, MIL.M_POSITION_Y, MIL.M_DEFAULT);
MIL.MmodControl(contextId, MIL.M_ALL, MIL.M_POSITION_DELTA_POS_Y, MIL.M_DEFAULT);
MIL.MmodControl(contextId, MIL.M_ALL, MIL.M_POSITION_DELTA_NEG_Y, MIL.M_DEFAULT);
}
return E1.OK;
}
public static ModelFinderRunParam GetRunParam(MIL_ID contextId) {
var modelFinderRunParam = new ModelFinderRunParam();
if (HasModel(contextId)) {
double acceptance = 0;
MIL.MmodInquire(contextId, MIL.M_DEFAULT, MIL.M_ACCEPTANCE, ref acceptance);
if (acceptance != MIL.M_DEFAULT) {
modelFinderRunParam.Acceptance = acceptance;
}
double certainty = 0;
MIL.MmodInquire(contextId, MIL.M_DEFAULT, MIL.M_CERTAINTY, ref certainty);
if (certainty != MIL.M_DEFAULT) {
modelFinderRunParam.Certainty = certainty;
}
double angleDeg = 0;
MIL.MmodInquire(contextId, MIL.M_DEFAULT, MIL.M_ANGLE_DELTA_POS, ref angleDeg);
if (angleDeg != MIL.M_DEFAULT) {
modelFinderRunParam.AngleTolerance = Angle.CreateWithDegree(angleDeg);
}
double scaleTolerance = 0;
MIL.MmodInquire(contextId, MIL.M_DEFAULT, MIL.M_SCALE_MAX_FACTOR, ref scaleTolerance);
modelFinderRunParam.ScaleTolerance = scaleTolerance - 1;
modelFinderRunParam.Smoothness = Convert.ToDouble(MIL.MmodInquire(contextId, MIL.M_CONTEXT, MIL.M_SMOOTHNESS, MIL.M_NULL));
modelFinderRunParam.Accuracy =
(eMilAccuracy)Convert.ToInt32(MIL.MmodInquire(contextId, MIL.M_CONTEXT, MIL.M_ACCURACY, MIL.M_NULL));
modelFinderRunParam.DetailLevel =
(eMilDetailLevel)Convert.ToInt32(MIL.MmodInquire(contextId, MIL.M_CONTEXT, MIL.M_DETAIL_LEVEL, MIL.M_NULL));
modelFinderRunParam.Speed =
(eMilSpeed)Convert.ToInt32(MIL.MmodInquire(contextId, MIL.M_CONTEXT, MIL.M_SPEED, MIL.M_NULL));
modelFinderRunParam.FindNumber = Convert.ToInt32(MIL.MmodInquire(contextId, MIL.M_DEFAULT, MIL.M_NUMBER, MIL.M_NULL));
modelFinderRunParam.Polarity =
(eMilPolarity)Convert.ToInt32(MIL.MmodInquire(contextId, MIL.M_DEFAULT, MIL.M_POLARITY, MIL.M_NULL));
}
return modelFinderRunParam;
}
public static int SetRunParam(MIL_ID contextId, IRunParam runParam) {
if (runParam is ModelFinderRunParam) {
var param = runParam as ModelFinderRunParam;
if (HasModel(contextId)) {
MIL.MmodControl(contextId, MIL.M_CONTEXT, MIL.M_ACCURACY, (double)param.Accuracy);
MIL.MmodControl(contextId, MIL.M_CONTEXT, MIL.M_DETAIL_LEVEL, (double)param.DetailLevel);
MIL.MmodControl(contextId, MIL.M_CONTEXT, MIL.M_SPEED, (double)param.Speed);
MIL.MmodControl(contextId, MIL.M_CONTEXT, MIL.M_SMOOTHNESS, (double)param.Smoothness);
MIL.MmodControl(contextId, MIL.M_ALL, MIL.M_NUMBER, param.FindNumber);
MIL.MmodControl(contextId, MIL.M_ALL, MIL.M_ACCEPTANCE, param.Acceptance);
MIL.MmodControl(contextId, MIL.M_ALL, MIL.M_CERTAINTY, param.Certainty);
MIL.MmodControl(contextId, MIL.M_ALL, MIL.M_ANGLE_DELTA_POS, param.AngleTolerance.Degree);
MIL.MmodControl(contextId, MIL.M_ALL, MIL.M_ANGLE_DELTA_NEG, param.AngleTolerance.Degree);
MIL.MmodControl(contextId, MIL.M_ALL, MIL.M_SCALE_MIN_FACTOR, 1 - param.ScaleTolerance);
MIL.MmodControl(contextId, MIL.M_ALL, MIL.M_SCALE_MAX_FACTOR, 1 + param.ScaleTolerance);
MIL.MmodControl(contextId, MIL.M_ALL, MIL.M_POLARITY, (double)param.Polarity);
ClearFindRoi(contextId);
MIL.MmodPreprocess(contextId, MIL.M_DEFAULT);
}
return E1.OK;
}
return E1.GENERIC;
}
public static PatternResult Find(MIL_ID contextId, MIL_ID resultContextId, MIL_ID image, ROI findRoi, bool isShape = false) {
if (MilEnvironment.IsColorImage(image)) {
var grayImageId = MilEnvironment.ConvertToGray(image);
var patternResult = FindInternal(contextId, resultContextId, grayImageId, findRoi, isShape);
MilEnvironment.FreeImage(grayImageId);
return patternResult;
}
return FindInternal(contextId, resultContextId, image, findRoi, isShape);
}
private static PatternResult FindInternal(MIL_ID contextId, MIL_ID resultContextId, MIL_ID image, ROI findRoi, bool isShape) {
PatternResult finderResult;
MIL.MappTimer(MIL.M_DEFAULT, MIL.M_TIMER_RESET + MIL.M_SYNCHRONOUS, MIL.M_NULL);
if (HasModel(contextId)) {
if (ROI.IsNullOrEmpty(findRoi)) {
MIL.MmodFind(contextId, image, resultContextId);
finderResult = ExtractResults(contextId, resultContextId, MilEnvironment.GetImageCenter(image), findRoi, isShape);
}
else {
var fittedFindRoi = FindWithCropImage(contextId, resultContextId, image, findRoi);
finderResult = ExtractResults(contextId, resultContextId, MilEnvironment.GetImageCenter(image), fittedFindRoi, isShape);
}
}
else {
finderResult = GetNotFoundPatternResult();
}
double tactTime = 0;
MIL.MappTimer(MIL.M_DEFAULT, MIL.M_TIMER_READ + MIL.M_SYNCHRONOUS, ref tactTime);
finderResult.TactTimeInMilliSec = MathUtil.ConvertUnitSecToMilli(tactTime);
return finderResult;
}
private static ROI FindWithCropImage(MIL_ID contextId, MIL_ID resultContextId, MIL_ID image, ROI findRoi) {
MIL_ID cropImage = MIL.M_NULL;
ROI fittedFindRoi = findRoi.Clone() as ROI;
try {
var imageSzie = MilEnvironment.GetImageSize(image);
if (ROI.IsOutsideImage(findRoi, imageSzie)) {
fittedFindRoi = ROI.FitRoiToImage(findRoi, imageSzie);
}
cropImage = MilEnvironment.CropImage(image, fittedFindRoi);
MIL.MmodFind(contextId, cropImage, resultContextId);
}
catch (Exception e) {
_Logger.E(e.Message);
}
finally {
MilEnvironment.FreeImage(cropImage);
}
return fittedFindRoi;
}
public static PatternResult GetNotFoundPatternResult() {
return new PatternResult() {
Status = eFoundStatus.NotFound,
Results = new List<ModelFinderResult>()
};
}
private static PatternResult ExtractResults(MIL_ID contextId, MIL_ID contextResult, ImagePoint imageCenter, ROI findRoi, bool isShape) {
if (isShape) {
List<ShapeModelFinderResult> results = GetShapeResult(contextId, contextResult, imageCenter, findRoi);
return new PatternResult() {
Status = (results.Count > 0) ? eFoundStatus.FoundOK : eFoundStatus.NotFound,
Results = results
};
}
else {
List<ModelFinderResult> results = GetModResult(contextId, contextResult, imageCenter, findRoi);
return new PatternResult() {
Status = (results.Count > 0) ? eFoundStatus.FoundOK : eFoundStatus.NotFound,
Results = results
};
}
}
private static List<ShapeModelFinderResult> GetShapeResult(MIL_ID contextId, MIL_ID contextResult, ImagePoint imageCenter, ROI findRoi) {
var results = new List<ShapeModelFinderResult>();
List<ModelFinderResult> modResults = GetModResult(contextId, contextResult, imageCenter, findRoi);
var runParam = GetRunParam(contextId);
for (int resultIndex = 0; resultIndex < modResults.Count; resultIndex++) {
double width = 0, height = 0, radius = 0, innerRadius = 0, outerRadius = 0;
MIL.MmodGetResult(contextResult, resultIndex, MIL.M_WIDTH, ref width);
MIL.MmodGetResult(contextResult, resultIndex, MIL.M_HEIGHT, ref height);
MIL.MmodGetResult(contextResult, resultIndex, MIL.M_RADIUS, ref radius);
MIL.MmodGetResult(contextResult, resultIndex, MIL.M_INNER_RADIUS, ref innerRadius);
MIL.MmodGetResult(contextResult, resultIndex, MIL.M_OUTER_RADIUS, ref outerRadius);
if (Math.Abs(modResults[resultIndex].Angle.Degree) < runParam.AngleTolerance.Degree || 360 - modResults[resultIndex].Angle.Degree < runParam.AngleTolerance.Degree) {
results.Add(new ShapeModelFinderResult(
position: modResults[resultIndex].ImagePosition,
offset: modResults[resultIndex].Offset,
angle: modResults[resultIndex].Angle,
score: modResults[resultIndex].Score,
width: width,
height: height,
radius: radius,
innerRadius : innerRadius,
outerRadius : outerRadius));
}
}
return results;
}
private static List<ModelFinderResult> GetModResult(MIL_ID contextId, MIL_ID contextResult, ImagePoint imageCenter, ROI findRoi) {
var results = new List<ModelFinderResult>();
MIL_INT numOfResult = 0;
MIL.MmodGetResult(contextResult, MIL.M_DEFAULT, MIL.M_NUMBER + MIL.M_TYPE_MIL_INT, ref numOfResult);
var runParam = GetRunParam(contextId);
for (int resultIndex = 0; resultIndex < numOfResult; resultIndex++) {
double posX = 0, posY = 0;
double score = 0;
double angleDeg = 0;
MIL.MmodGetResult(contextResult, resultIndex, MIL.M_POSITION_X, ref posX);
MIL.MmodGetResult(contextResult, resultIndex, MIL.M_POSITION_Y, ref posY);
MIL.MmodGetResult(contextResult, resultIndex, MIL.M_SCORE, ref score);
MIL.MmodGetResult(contextResult, resultIndex, MIL.M_ANGLE, ref angleDeg); // Warning : Counter Clockwise
posX += findRoi.Left;
posY += findRoi.Top;
if (Math.Abs(angleDeg) < runParam.AngleTolerance.Degree || Math.Abs(360 - angleDeg) < runParam.AngleTolerance.Degree) {
results.Add(new ModelFinderResult(
position: new ImagePoint(posX, posY),
offset: (new ImagePoint(posX, posY) - imageCenter) as ImagePoint,
angle: Angle.NormalizeAngle(Angle.CreateWithDegree(angleDeg)),
score: score));
}
}
return results;
}
public static void Draw(
PatternResult patternResult, IMilCameraDisplay milCameraDisplay, bool isFixturePattern, MIL_ID resultContextId, ROI roi) {
if (patternResult == null || patternResult.Results == null) {
return;
}
var crosses = new List<Cross>();
foreach (BaseResult baseResult in patternResult.Results) {
var result = baseResult as ModelFinderResult;
if (result.Score > 0) {
var color = GraphicColors.GetFindResultColor(patternResult.Status, isFixturePattern);
// MIL : 시계 방향(-), 반시계 방향(+)
var cross = new Cross(result.ImagePosition.Clone() as ImagePoint, -result.Angle, 50);
crosses.Add(cross);
milCameraDisplay.SetPatternTracking(cross);
milCameraDisplay.AddCross(cross, color);
milCameraDisplay.AddPatternEdge(resultContextId, roi);
var msgs = new List<string>();
msgs.Add(string.Format("I({0:0.0}, {1:0.0})", result.ImagePosition.X, result.ImagePosition.Y));
msgs.Add(result.ConvertMaterialPosition());
msgs.Add(string.Format("Angle : {0:0.00}", result.Angle.Degree));
if (patternResult.PatternType == ePatternType.ShapeModelFinder) {
var shapeResult = baseResult as ShapeModelFinderResult;
if (shapeResult.Width != 0 && shapeResult.Height != 0) {
msgs.Add(string.Format("Width : {0:0.00}", shapeResult.Width));
msgs.Add(string.Format("Height : {0:0.00}", shapeResult.Height));
}
if (shapeResult.Radius != 0) {
msgs.Add(string.Format("Radius : {0:0.00}", shapeResult.Radius));
}
if (shapeResult.InnerRadius != 0) {
msgs.Add(string.Format("Inner Radius : {0:0.00}", shapeResult.InnerRadius));
}
if (shapeResult.OuterRadius != 0) {
msgs.Add(string.Format("Outer Radius : {0:0.00}", shapeResult.OuterRadius));
}
}
msgs.Add(string.Format("Score : {0:0.00}", result.Score));
msgs.Add(string.Format("TactTime : {0} ms", Math.Round(patternResult.TactTimeInMilliSec)));
msgs.Add(string.Format("ID : {0}_{1}", patternResult.PatternIndex.PatternId, patternResult.PatternIndex.PatternSubId));
milCameraDisplay.AddTextList(
color, new ImagePoint(result.ImagePosition.X, result.ImagePosition.Y), msgs);
}
}
milCameraDisplay.DrawPatternTrackings(crosses);
}
}
}