public override FrameworkReturnCode Proceed(Image inputImage, Transform3Df pose, ICamera camera)
{
overlay3DComponent.setCameraParameters(camera.getIntrinsicsParameters(), camera.getDistorsionParameters());
// Convert Image from RGB to grey
imageConvertor.convert(inputImage, greyImage, Image.ImageLayout.LAYOUT_GREY).Check();
// Convert Image from grey to black and white
imageFilterBinary.filter(greyImage, binaryImage).Check();
// Extract contours from binary image
contoursExtractor.extract(binaryImage, contours).Check();
#if !NDEBUG
var contoursImage = binaryImage.copy();
overlay2DContours.drawContours(contours, contoursImage);
#endif
// Filter 4 edges contours to find those candidate for marker contours
contoursFilter.filter(contours, filtered_contours).Check();
#if !NDEBUG
var filteredContoursImage = binaryImage.copy();
overlay2DContours.drawContours(filtered_contours, filteredContoursImage);
#endif
// Create one warpped and cropped image by contour
perspectiveController.correct(binaryImage, filtered_contours, patches).Check();
// test if this last image is really a squared binary marker, and if it is the case, extract its descriptor
if (patternDescriptorExtractor.extract(patches, filtered_contours, recognizedPatternsDescriptors, recognizedContours) == FrameworkReturnCode._SUCCESS)
{
#if !NDEBUG
var std__cout = new System.Text.StringBuilder();
/*
* LOG_DEBUG("Looking for the following descriptor:");
* for (var i = 0; i < markerPatternDescriptor.getNbDescriptors() * markerPatternDescriptor.getDescriptorByteSize(); i++)
* {
*
* if (i % patternSize == 0)
* std__cout.Append("[");
* if (i % patternSize != patternSize - 1)
* std__cout.Append(markerPatternDescriptor.data()[i]).Append(", ");
* else
* std__cout.Append(markerPatternDescriptor.data()[i]).Append("]").AppendLine();
* }
* std__cout.AppendLine();
*/
/*
* std__cout.Append(recognizedPatternsDescriptors.getNbDescriptors()).Append(" recognized Pattern Descriptors ").AppendLine();
* uint desrciptorSize = recognizedPatternsDescriptors.getDescriptorByteSize();
* for (uint i = 0; i < recognizedPatternsDescriptors.getNbDescriptors() / 4; i++)
* {
* for (int j = 0; j < patternSize; j++)
* {
* for (int k = 0; k < 4; k++)
* {
* std__cout.Append("[");
* for (int l = 0; l < patternSize; l++)
* {
* std__cout.Append(recognizedPatternsDescriptors.data()[desrciptorSize*((i*4)+k) + j*patternSize + l]);
* if (l != patternSize - 1)
* std__cout.Append(", ");
* }
* std__cout.Append("]");
* }
* std__cout.AppendLine();
* }
* std__cout.AppendLine().AppendLine();
* }
*/
/*
* std__cout.Append(recognizedContours.Count).Append(" Recognized Pattern contour ").AppendLine();
* for (int i = 0; i < recognizedContours.Count / 4; i++)
* {
* for (int j = 0; j < recognizedContours[i].Count; j++)
* {
* for (int k = 0; k < 4; k++)
* {
* std__cout.Append("[").Append(recognizedContours[i * 4 + k][j].getX()).Append(", ").Append(recognizedContours[i * 4 + k][j].getY()).Append("] ");
* }
* std__cout.AppendLine();
* }
* std__cout.AppendLine().AppendLine();
* }
* std__cout.AppendLine();
*/
#endif
// From extracted squared binary pattern, match the one corresponding to the squared binary marker
if (patternMatcher.match(markerPatternDescriptor, recognizedPatternsDescriptors, patternMatches) == Api.Features.IDescriptorMatcher.RetCode.DESCRIPTORS_MATCHER_OK)
{
#if !NDEBUG
std__cout.Append("Matches :").AppendLine();
for (int num_match = 0; num_match < patternMatches.Count; num_match++)
{
std__cout.Append("Match [").Append(patternMatches[num_match].getIndexInDescriptorA()).Append(",").Append(patternMatches[num_match].getIndexInDescriptorB()).Append("], dist = ").Append(patternMatches[num_match].getMatchingScore()).AppendLine();
}
std__cout.AppendLine().AppendLine();
#endif
// Reindex the pattern to create two vector of points, the first one corresponding to marker corner, the second one corresponding to the poitsn of the contour
patternReIndexer.reindex(recognizedContours, patternMatches, pattern2DPoints, img2DPoints).Check();
#if !NDEBUG
LOG_DEBUG("2D Matched points :");
for (int i = 0; i < img2DPoints.Count; i++)
{
LOG_DEBUG("{0}", img2DPoints[i]);
}
for (int i = 0; i < pattern2DPoints.Count; i++)
{
LOG_DEBUG("{0}", pattern2DPoints[i]);
}
overlay2DCircles.drawCircles(img2DPoints, inputImage);
#endif
// Compute the 3D position of each corner of the marker
img2worldMapper.map(pattern2DPoints, pattern3DPoints).Check();
#if !NDEBUG
std__cout.Append("3D Points position:").AppendLine();
for (int i = 0; i < pattern3DPoints.Count; i++)
{
LOG_DEBUG("{0}", pattern3DPoints[i]);
}
#endif
// Compute the pose of the camera using a Perspective n Points algorithm using only the 4 corners of the marker
if (PnP.estimate(img2DPoints, pattern3DPoints, pose) == FrameworkReturnCode._SUCCESS)
{
overlay3DComponent.draw(pose, inputImage);
return(FrameworkReturnCode._SUCCESS);
}
}
#if !NDEBUG
//LOG_DEBUG(std__cout.ToString());
std__cout.Clear();
#endif
}
return(FrameworkReturnCode._ERROR_);
}
public override void SetCameraParameters(Matrix3x3f intrinsics, Vector5f distorsion)
{
PnP.setCameraParameters(intrinsics, distorsion);
}