/// <summary>
/// Use method to invert an input transform matrix about a given origin.
/// Could help when combining transformation for stages with inverted axis.
/// </summary>
/// <param name="taskHandler">An implementation of the process configuration tasks</param>
/// <param name="parentTransform">The parent transform in the chain of transformations</param>
/// <param name="transform">The input transformation</param>
/// <param name="ctIn">A cancellation token</param>
/// <returns>The input matrix flip about it's parent's transform</returns>
public async Task <Matrix4x4> InvertTransform(IProcessConfigurationTasksHandler taskHandler, Matrix4x4 parentTransform, Matrix4x4 transform, CancellationToken ctIn)
{
var origin = new MarkGeometryPoint();
origin.Transform(parentTransform);
var inverts = await taskHandler.GetStageInverts(ctIn);
return(GeometricArithmeticModule.CombineTransformations(
// flip about the base transform's origin
GeometricArithmeticModule.GetTranslationTransformationMatrix(
-origin.X, -origin.Y, -origin.Z
),
// apply the next transform
transform,
// flip the next transform on the requested x-axis
GeometricArithmeticModule.GetScalingTransformationMatrix(
inverts.InvertX ? -1 : 1,
inverts.InvertY ? -1 : 1
),
// translate back to the base transform's origin
GeometricArithmeticModule.GetTranslationTransformationMatrix(
origin.X, origin.Y, origin.Z
)
));
}
public async Task <Matrix4x4> GetAbsoluteTransformFromStageOrigin(
IProcessConfigurationTasksHandler taskHandler,
ICamToBeam cam2Beam,
MRecipe recipe,
MRecipeBaseNode recipeNode,
CancellationToken ctIn
)
{
// firstly, find closest parent with fiducials
var parent = recipeNode;
while (
parent != null &&
parent.Fiducials.Count <= 0
)
{
parent = parent.Parent as MRecipeBaseNode;
}
if (parent != null)
{
// recursion - get it's parent's transform
var baseTransform = await GetAbsoluteTransformFromStageOrigin(
taskHandler, cam2Beam, recipe, parent.Parent as MRecipeBaseNode, ctIn
);
var fiducialPoints = new List <MarkGeometryPoint>();
var measuredPoints = new List <MarkGeometryPoint>();
for (int i = 0; i < parent.Fiducials.Count; i++)
{
var fiducial = parent.Fiducials[i];
if (!_cachedFiducials.ContainsKey(fiducial))
{
// estimate stage position
var stageLocation = new MarkGeometryPoint(
fiducial.X, fiducial.Y, fiducial.Z
);
stageLocation.Transform(baseTransform);
// goto estimated position
if (!await taskHandler.GotoXY(
stageLocation.X, stageLocation.Y, ctIn
))
{
throw new Exception($"Failed to goto the estimated position; origin: {await taskHandler.GetStageOrigin(ctIn)}, fiducial: {(fiducial.X, fiducial.Y)} est: {stageLocation}");
}
// find and centre
{
if (!await cam2Beam.MoveBeamToCamOffset(ctIn))
{
throw new Exception($"Failed to move beam to cam offset");
}
if (!await taskHandler.MovetoCameraFocus(ctIn))
{
throw new Exception($"Failed to move camera to focus");
}
await taskHandler.SwitchCameraLedOn(ctIn);
// attempt to centre on fiducial
if (!await taskHandler.CentreOnVisibleObject(ctIn))
{
_logger.Info($"Failed to centre on fiducial");
// ask user to locate fiducials
Application.Current.Dispatcher.Invoke(
() =>
{
var fidVm = taskHandler.GetNewFidFindViewModel();
var inspectionDialog = new InspectionWindow(fidVm, Application.Current.MainWindow);
inspectionDialog.Owner = Application.Current.MainWindow;
if (inspectionDialog.ShowDialog() != true)
{
throw new Exception("Failed to find fiducial");
}
// TODO : think about updating the machine's origin
}
);
}
if (!await cam2Beam.MoveCamToBeamOffset(ctIn))
{
throw new Exception($"Failed to move camera to beam offset");
}
}
// read fiducial position
var(stgX, stgY, success) = await taskHandler.GetStageXY(ctIn);
if (!success)
{
throw new Exception("Failed to read the current stage position");
}
// update cache
_cachedFiducials[fiducial] = new MarkGeometryPoint(stgX, stgY);
}
// update measured points
fiducialPoints.Add(new MarkGeometryPoint(fiducial.X, fiducial.Y));
measuredPoints.Add(_cachedFiducials[fiducial]);
}
// calculate transform from measured fiducials
var parentTransform = GeometricArithmeticModule.EstimateTransformationMatrixFromPoints(
fiducialPoints, measuredPoints
);
return(GeometricArithmeticModule.CombineTransformations(
parentTransform,
await InvertTransform(
taskHandler,
parentTransform,
MRecipe.GetRelativeTransformFromParent(
parent, recipeNode
),
ctIn
)
));
}
var origin = await taskHandler.GetStageOrigin(ctIn);
var stageOrigin = GeometricArithmeticModule.GetTranslationTransformationMatrix(
origin.X, origin.Y
);
return(GeometricArithmeticModule.CombineTransformations(
stageOrigin,
await InvertTransform(
taskHandler,
stageOrigin,
MRecipe.GetRelativeTransform(
recipe, recipeNode
),
ctIn
)
));
}
public async Task <Matrix4x4> GetAbsoluteTransform(MRecipeBaseNode node, Matrix4x4 parentTransform, CancellationToken ctIn)
{
if (parentTransform == null)
{
parentTransform = GeometricArithmeticModule.GetDefaultTransformationMatrix();
}
// node has no fiducials, hences it's position
// is relative to its parent
if (node.Fiducials.Count <= 0)
{
return(GeometricArithmeticModule.CombineTransformations(
parentTransform,
node.TransformInfo.ToMatrix4x4()
));
}
// attempt to retrieve tasksHandler; handles process specific stage motion
// camera, find n centre, etc.
IProcessConfigurationTasksHandler tasksHandler = GetClosestConfigurationTasksHandler(node);
if (tasksHandler == null)
{
throw new Exception("Failed to retrieve a Task Handler for the closest process mode");
}
// buffer to store estimated and measured fiducials
var estimatedPoints = new List <MarkGeometryPoint>();
var measuredPoints = new List <MarkGeometryPoint>();
for (int i = 0; i < node.Fiducials.Count; i++)
{
var estimatedPosition = new MarkGeometryPoint(
node.Fiducials[i].X, node.Fiducials[i].Y
);
// transform to parents space
estimatedPosition.Transform(parentTransform);
estimatedPoints.Add(estimatedPosition);
// move camera to position
await tasksHandler.FocusCameraAtXY(
estimatedPosition.X,
estimatedPosition.Y,
ctIn
);
// get measured position
var results = await tasksHandler.TakeMeasurement();
// handle results not found
if (!results.Found)
{
throw new Exception("Failed to find fiducial");
}
// store measured point
measuredPoints.Add(
new MarkGeometryPoint(
results.Position.X,
results.Position.Y
)
);
}
return(GeometricArithmeticModule.CombineTransformations(
parentTransform,
MAlignmentCalculator.GetAlignmentTransform(
node.AlignmentType,
estimatedPoints,
measuredPoints
)
));
}
