// TODO: Throw an Exception if vision fails.
private Location getVisionOffsets(Head head, Location pickLocation) throws Exception {
logger.debug("getVisionOffsets({}, {})", head.getId(), pickLocation);
// Find the Camera to be used for vision
// TODO: Consider caching this
Camera camera = null;
for (Camera c : head.getCameras()) {
if (c.getVisionProvider() != null) {
camera = c;
}
}
if (camera == null) {
throw new Exception("No vision capable camera found on head.");
}
head.moveToSafeZ(1.0);
// Position the camera over the pick location.
logger.debug("Move camera to pick location.");
camera.moveTo(pickLocation, 1.0);
// Move the camera to be in focus over the pick location.
// head.moveTo(head.getX(), head.getY(), z, head.getC());
// Settle the camera
// TODO: This should be configurable, or maybe just built into
// the VisionProvider
Thread.sleep(200);
VisionProvider visionProvider = camera.getVisionProvider();
Rectangle aoi = getVision().getAreaOfInterest();
// Perform the template match
logger.debug("Perform template match.");
Point[] matchingPoints = visionProvider.locateTemplateMatches(
aoi.getX(),
aoi.getY(),
aoi.getWidth(),
aoi.getHeight(),
0,
0,
vision.getTemplateImage());
// Get the best match from the array
Point match = matchingPoints[0];
// match now contains the position, in pixels, from the top left corner
// of the image to the top left corner of the match. We are interested in
// knowing how far from the center of the image the center of the match is.
BufferedImage image = camera.capture();
double imageWidth = image.getWidth();
double imageHeight = image.getHeight();
double templateWidth = vision.getTemplateImage().getWidth();
double templateHeight = vision.getTemplateImage().getHeight();
double matchX = match.x;
double matchY = match.y;
logger.debug("matchX {}, matchY {}", matchX, matchY);
// Adjust the match x and y to be at the center of the match instead of
// the top left corner.
matchX += (templateWidth / 2);
matchY += (templateHeight / 2);
logger.debug("centered matchX {}, matchY {}", matchX, matchY);
// Calculate the difference between the center of the image to the
// center of the match.
double offsetX = (imageWidth / 2) - matchX;
double offsetY = (imageHeight / 2) - matchY;
logger.debug("offsetX {}, offsetY {}", offsetX, offsetY);
// Invert the Y offset because images count top to bottom and the Y
// axis of the machine counts bottom to top.
offsetY *= -1;
logger.debug("negated offsetX {}, offsetY {}", offsetX, offsetY);
// And convert pixels to units
Location unitsPerPixel = camera.getUnitsPerPixel();
offsetX *= unitsPerPixel.getX();
offsetY *= unitsPerPixel.getY();
logger.debug("final, in camera units offsetX {}, offsetY {}", offsetX, offsetY);