diff --git a/source/FAST/Algorithms/TemplateMatching/CMakeLists.txt b/source/FAST/Algorithms/TemplateMatching/CMakeLists.txt index f37efa763..903ac5376 100644 --- a/source/FAST/Algorithms/TemplateMatching/CMakeLists.txt +++ b/source/FAST/Algorithms/TemplateMatching/CMakeLists.txt @@ -1,6 +1,6 @@ fast_add_sources( - TemplateMatchingNCC.hpp - TemplateMatchingNCC.cpp + TemplateMatching.hpp + TemplateMatching.cpp ) fast_add_test_sources(Tests.cpp) \ No newline at end of file diff --git a/source/FAST/Algorithms/TemplateMatching/TemplateMatching.cpp b/source/FAST/Algorithms/TemplateMatching/TemplateMatching.cpp new file mode 100644 index 000000000..aba721a2f --- /dev/null +++ b/source/FAST/Algorithms/TemplateMatching/TemplateMatching.cpp @@ -0,0 +1,208 @@ +#include +#include "TemplateMatching.hpp" + +namespace fast { + + +TemplateMatching::TemplateMatching() { + createInputPort(0); // Image to search in + createInputPort(1); // Template + + createOutputPort(0); // Match scores +} + +static float calculateMeanIntensity(ImageAccess::pointer& access, const Vector2i start, const Vector2i size) { + float sum = 0; + for(int y = start.y(); y < start.y() + size.y(); ++y) { + for(int x = start.x(); x < start.x() + size.x(); ++x) { + sum += access->getScalar(Vector2i(x, y)); + } + } + + return sum / (size.x()*size.y()); +} + +void TemplateMatching::execute() { + auto image = getInputData(0); + auto templateImage = getInputData(1); + + if(templateImage->getWidth() % 2 == 0 || templateImage->getHeight() % 2 == 0) + throw Exception("Template image size for template matching must be odd"); + + outputScores = Image::New(); + outputScores->create(image->getSize(), TYPE_FLOAT, 1); + outputScores->fill(0); + auto outputAccess = outputScores->getImageAccess(ACCESS_READ_WRITE); + auto templateAccess = templateImage->getImageAccess(ACCESS_READ); + uchar* templatePointer = (uchar*)templateAccess->get(); + auto imageAccess = image->getImageAccess(ACCESS_READ); + uchar* imagePointer = (uchar*)imageAccess->get(); + + float templateMean = 0.0f; + if(m_type == MatchingMetric::NORMALIZED_CROSS_CORRELATION) + templateMean = calculateMeanIntensity(templateAccess, Vector2i::Zero(), Vector2i(templateImage->getWidth(), templateImage->getHeight())); + + int start_y = templateImage->getHeight(); + int start_x = templateImage->getWidth(); + int end_y = image->getHeight() - templateImage->getHeight(); + int end_x = image->getWidth() - templateImage->getWidth(); + if(m_center.x() != -1) { + start_x = m_center.x() - m_offset.x(); + end_x = m_center.x() + m_offset.x(); + start_y = m_center.y() - m_offset.y(); + end_y = m_center.y() + m_offset.y(); + } + + int halfSize_x = templateImage->getWidth() / 2; + int halfSize_y = templateImage->getHeight() / 2; + float bestMatchScore = std::numeric_limits::min(); + float maxIntensity = image->calculateMaximumIntensity(); + float minIntensity = image->calculateMinimumIntensity(); + // For every possible ROI position + switch(m_type) { + case MatchingMetric::NORMALIZED_CROSS_CORRELATION: + for (int y = start_y; y <= end_y; ++y) { + for (int x = start_x; x <= end_x; ++x) { + float imageTargetMean = calculateMeanIntensity(imageAccess, + Vector2i(x - halfSize_x, y - halfSize_y), + Vector2i(templateImage->getWidth(), + templateImage->getHeight())); + float upperPart = 0.0f; + float lowerPart1 = 0.0f; + float lowerPart2 = 0.0f; + // Loop over current ROI + for (int a = -halfSize_x; a <= halfSize_x; ++a) { + for (int b = -halfSize_y; b <= halfSize_y; ++b) { + float imagePart = (imageAccess->getScalar(Vector2i(x + a, y + b)) - imageTargetMean); + float templatePart = (templateAccess->getScalar(Vector2i(a + halfSize_x, b + halfSize_y)) - + templateMean); + upperPart += imagePart * templatePart; + lowerPart1 += imagePart * imagePart; + lowerPart2 += templatePart * templatePart; + } + } + + float result = upperPart / std::sqrt(lowerPart1 * lowerPart2); + outputAccess->setScalar(Vector2i(x, y), result); + if (result > bestMatchScore) { + bestMatchScore = result; + m_bestFitPosition = Vector2i(x, y); + } + } + } + break; + case MatchingMetric::SUM_OF_ABSOLUTE_DIFFERENCES: + for (int y = start_y; y <= end_y; ++y) { + for (int x = start_x; x <= end_x; ++x) { + float sad = 0.0f; + // Loop over current ROI + for (int a = -halfSize_x; a <= halfSize_x; ++a) { + for (int b = -halfSize_y; b <= halfSize_y; ++b) { + float imagePart = (imagePointer[x + a + (y + b)*image->getWidth()] - minIntensity) / (maxIntensity - minIntensity); + float templatePart = (templatePointer[a + halfSize_x + (b + halfSize_y)*templateImage->getWidth()] - minIntensity) / (maxIntensity - minIntensity); + sad += std::fabs(imagePart - templatePart); + } + } + const float result = 1.0f - (sad/(templateImage->getWidth()*templateImage->getHeight())); // calculate average and invert + + outputAccess->setScalar(Vector2i(x, y), result); + if (result > bestMatchScore) { + bestMatchScore = result; + m_bestFitPosition = Vector2i(x, y); + } + } + } + break; + case MatchingMetric::SUM_OF_SQUARED_DIFFERENCES: + for (int y = start_y; y <= end_y; ++y) { + for (int x = start_x; x <= end_x; ++x) { + float ssd = 0.0f; + // Loop over current ROI + for (int a = -halfSize_x; a <= halfSize_x; ++a) { + for (int b = -halfSize_y; b <= halfSize_y; ++b) { + float imagePart = (imageAccess->getScalar(Vector2i(x + a, y + b)) - minIntensity) / (maxIntensity - minIntensity); + float templatePart = (templateAccess->getScalar(Vector2i(a + halfSize_x, b + halfSize_y)) - minIntensity) / (maxIntensity - minIntensity); + ssd += (imagePart - templatePart)*(imagePart - templatePart); + } + } + const float result = 1.0f - (ssd/(templateImage->getWidth()*templateImage->getHeight())); // calculate average and invert + + outputAccess->setScalar(Vector2i(x, y), result); + if (result > bestMatchScore) { + bestMatchScore = result; + m_bestFitPosition = Vector2i(x, y); + } + } + } + break; + } + + addOutputData(0, outputScores); +} + +void TemplateMatching::setRegionOfInterest(Vector2i center, Vector2i offset) { + m_center = center; + m_offset = offset; +} + +Vector2i TemplateMatching::getBestFitPixelPosition() const { + if(outputScores) { + return m_bestFitPosition; + } else { + throw Exception("Must run update first"); + } +} + +void TemplateMatching::setMatchingMetric(MatchingMetric type) { + m_type = type; +} + +Vector2f TemplateMatching::getBestFitSubPixelPosition() const { + if(outputScores) { + // Calculate subpixel offset + // Sample data points around max position + auto access = outputScores->getImageAccess(ACCESS_READ); + Matrix3f b; + for(int x = -1; x <= 1; ++x) { + for(int y = -1; y <= 1; ++y) { + Vector2i position = m_bestFitPosition + Vector2i(x, y); + b(x + 1, y + 1) = access->getScalar(position); + } + } + + // 2D parabolic + const auto A = + (b(0, 0) - 2 * b(1, 0) + b(2, 0) + b(0, 1) - 2 * b(1, 1) + b(2, 1) + b(0, 2) - 2 * b(1, 2) + + b(2, 2)) / 6.0; + const auto B = (b(0, 0) - b(2, 0) - b(0, 2) + b(2, 2)) / 4.0; + const auto C = + (b(0, 0) + b(1, 0) + b(2, 0) - 2 * b(0, 1) - 2 * b(1, 1) - 2 * b(2, 1) + b(0, 2) + b(1, 2) + + b(2, 2)) / 6.0; + const auto D = (-b(0, 0) + b(2, 0) - b(0, 1) + b(2, 1) - b(0, 2) + b(2, 2)) / 6.0; + const auto E = (-b(0, 0) - b(1, 0) - b(2, 0) + b(0, 2) + b(1, 2) + b(2, 2)) / 6.0; + const auto F = (-b(0, 0) + 2 * b(1, 0) - b(2, 0) + 2 * b(0, 1) + 5 * b(1, 1) + 2 * b(2, 1) - b(0, 2) + + 2 * b(1, 2) - b(2, 2)) / 9.0; + + const Vector2f subpixelOffset((B * E - 2.0 * C * D) / (4.0 * A * C - B * B), + (B * D - 2.0 * A * E) / (4.0 * A * C - B * B)); + + + + /* + // 1D parabolic + const auto firstDerivativeX = 0.5f*(b(2, 1) - b(0, 1)); + const auto secondDerivativeX = b(2,1) - 2.f*b(1, 1) + b(0, 1); + const auto firstDerivativeY = 0.5f*(b(1, 2) - b(1, 0)); + const auto secondDerivativeY = b(1,2) - 2.f*b(1, 1) + b(1, 0); + + const Vector2f subpixelOffset(-firstDerivativeX/secondDerivativeX, -firstDerivativeY/secondDerivativeY); + std::cout << subpixelOffset.transpose() << std::endl; + */ + + return m_bestFitPosition.cast() + subpixelOffset; + } else { + throw Exception("Must run update first"); + } +} + +} diff --git a/source/FAST/Algorithms/TemplateMatching/TemplateMatchingNCC.hpp b/source/FAST/Algorithms/TemplateMatching/TemplateMatching.hpp similarity index 60% rename from source/FAST/Algorithms/TemplateMatching/TemplateMatchingNCC.hpp rename to source/FAST/Algorithms/TemplateMatching/TemplateMatching.hpp index 5284ec536..ea9c82b11 100644 --- a/source/FAST/Algorithms/TemplateMatching/TemplateMatchingNCC.hpp +++ b/source/FAST/Algorithms/TemplateMatching/TemplateMatching.hpp @@ -7,11 +7,17 @@ namespace fast { class Image; /** - * This algorithms matches a template image to an image using normalized cross correlation (NCC) + * This algorithms matches a template image to an image using normalized cross correlation (NCC), + * sum of absolute differences (SAD) or sum of squared differences (SSD). */ -class FAST_EXPORT TemplateMatchingNCC : public ProcessObject { - FAST_OBJECT(TemplateMatchingNCC) +class FAST_EXPORT TemplateMatching : public ProcessObject { + FAST_OBJECT(TemplateMatching) public: + enum class MatchingMetric { + NORMALIZED_CROSS_CORRELATION, + SUM_OF_SQUARED_DIFFERENCES, + SUM_OF_ABSOLUTE_DIFFERENCES, + }; /** * Set region of interest of where to do the template matching. * @param center 2D position @@ -28,10 +34,16 @@ class FAST_EXPORT TemplateMatchingNCC : public ProcessObject { * @return Vector2f */ Vector2f getBestFitSubPixelPosition() const; + /** + * Select which matching metric to use + * @param type + */ + void setMatchingMetric(MatchingMetric type); private: - TemplateMatchingNCC(); + TemplateMatching(); void execute() override; + MatchingMetric m_type = MatchingMetric::SUM_OF_ABSOLUTE_DIFFERENCES; Vector2i m_center = Vector2i(-1, -1); Vector2i m_offset; Vector2i m_bestFitPosition; diff --git a/source/FAST/Algorithms/TemplateMatching/TemplateMatchingNCC.cpp b/source/FAST/Algorithms/TemplateMatching/TemplateMatchingNCC.cpp deleted file mode 100644 index 23a5bc99f..000000000 --- a/source/FAST/Algorithms/TemplateMatching/TemplateMatchingNCC.cpp +++ /dev/null @@ -1,143 +0,0 @@ -#include -#include "TemplateMatchingNCC.hpp" - -namespace fast { - - -TemplateMatchingNCC::TemplateMatchingNCC() { - createInputPort(0); // Image to search in - createInputPort(1); // Template - - createOutputPort(0); // Match scores -} - -static float calculateMeanIntensity(ImageAccess::pointer& access, const Vector2i start, const Vector2i size) { - float sum = 0; - for(int y = start.y(); y < start.y() + size.y(); ++y) { - for(int x = start.x(); x < start.x() + size.x(); ++x) { - sum += access->getScalar(Vector2i(x, y)); - } - } - - return sum / (size.x()*size.y()); -} - -void TemplateMatchingNCC::execute() { - auto image = getInputData(0); - auto templateImage = getInputData(1); - - outputScores = Image::New(); - outputScores->create(image->getSize(), TYPE_FLOAT, 1); - outputScores->fill(0); - auto outputAccess = outputScores->getImageAccess(ACCESS_READ_WRITE); - auto templateAccess = templateImage->getImageAccess(ACCESS_READ); - auto imageAccess = image->getImageAccess(ACCESS_READ); - - const float templateMean = calculateMeanIntensity(templateAccess, Vector2i::Zero(), Vector2i(templateImage->getWidth(), templateImage->getHeight())); - - int start_y = templateImage->getHeight(); - int start_x = templateImage->getWidth(); - int end_y = image->getHeight() - templateImage->getHeight(); - int end_x = image->getWidth() - templateImage->getWidth(); - if(m_center.x() != -1) { - start_x = m_center.x() - m_offset.x(); - end_x = m_center.x() + m_offset.x(); - start_y = m_center.y() - m_offset.y(); - end_y = m_center.y() + m_offset.y(); - } - - int halfSize_x = templateImage->getWidth() / 2; - int halfSize_y = templateImage->getHeight() / 2; - float bestMatchScore = std::numeric_limits::min(); - // For every possible ROI position - for(int y = start_y; y <= end_y; ++y) { - for(int x = start_x; x <= end_x; ++x) { - float imageTargetMean = calculateMeanIntensity(imageAccess, Vector2i(x-halfSize_x, y-halfSize_y), Vector2i(templateImage->getWidth(), templateImage->getHeight())); - float upperPart = 0.0f; - float lowerPart1 = 0.0f; - float lowerPart2 = 0.0f; - // Loop over current ROI - for(int a = -halfSize_x; a < halfSize_x; ++a) { - for(int b = -halfSize_y; b < halfSize_y; ++b) { - float imagePart = (imageAccess->getScalar(Vector2i(x+a, y+b)) - imageTargetMean); - float templatePart = (templateAccess->getScalar(Vector2i(a+halfSize_x, b+halfSize_y)) - templateMean); - upperPart += imagePart*templatePart; - lowerPart1 += imagePart*imagePart; - lowerPart2 += templatePart*templatePart; - } - } - - float result = upperPart / std::sqrt(lowerPart1*lowerPart2); - outputAccess->setScalar(Vector2i(x, y), result); - if(result > bestMatchScore) { - bestMatchScore = result; - m_bestFitPosition = Vector2i(x, y); - } - } - } - - addOutputData(0, outputScores); -} - -void TemplateMatchingNCC::setRegionOfInterest(Vector2i center, Vector2i offset) { - m_center = center; - m_offset = offset; -} - -Vector2i TemplateMatchingNCC::getBestFitPixelPosition() const { - if(outputScores) { - return m_bestFitPosition; - } else { - throw Exception("Must run update first"); - } -} - -Vector2f TemplateMatchingNCC::getBestFitSubPixelPosition() const { - if(outputScores) { - // Calculate subpixel offset - // Sample data points around max position - auto access = outputScores->getImageAccess(ACCESS_READ); - Matrix3f b; - for(int x = -1; x <= 1; ++x) { - for(int y = -1; y <= 1; ++y) { - Vector2i position = m_bestFitPosition + Vector2i(x, y); - b(x + 1, y + 1) = access->getScalar(position); - } - } - - // 2D parabolic - const auto A = - (b(0, 0) - 2 * b(1, 0) + b(2, 0) + b(0, 1) - 2 * b(1, 1) + b(2, 1) + b(0, 2) - 2 * b(1, 2) + - b(2, 2)) / 6.0; - const auto B = (b(0, 0) - b(2, 0) - b(0, 2) + b(2, 2)) / 4.0; - const auto C = - (b(0, 0) + b(1, 0) + b(2, 0) - 2 * b(0, 1) - 2 * b(1, 1) - 2 * b(2, 1) + b(0, 2) + b(1, 2) + - b(2, 2)) / 6.0; - const auto D = (-b(0, 0) + b(2, 0) - b(0, 1) + b(2, 1) - b(0, 2) + b(2, 2)) / 6.0; - const auto E = (-b(0, 0) - b(1, 0) - b(2, 0) + b(0, 2) + b(1, 2) + b(2, 2)) / 6.0; - const auto F = (-b(0, 0) + 2 * b(1, 0) - b(2, 0) + 2 * b(0, 1) + 5 * b(1, 1) + 2 * b(2, 1) - b(0, 2) + - 2 * b(1, 2) - b(2, 2)) / 9.0; - - const Vector2f subpixelOffset((B * E - 2.0 * C * D) / (4.0 * A * C - B * B), - (B * D - 2.0 * A * E) / (4.0 * A * C - B * B)); - - - - /* - // 1D parabolic - const auto firstDerivativeX = 0.5f*(b(2, 1) - b(0, 1)); - const auto secondDerivativeX = b(2,1) - 2.f*b(1, 1) + b(0, 1); - const auto firstDerivativeY = 0.5f*(b(1, 2) - b(1, 0)); - const auto secondDerivativeY = b(1,2) - 2.f*b(1, 1) + b(1, 0); - - const Vector2f subpixelOffset(-firstDerivativeX/secondDerivativeX, -firstDerivativeY/secondDerivativeY); - std::cout << subpixelOffset.transpose() << std::endl; - */ - - return m_bestFitPosition.cast() + subpixelOffset; - } else { - throw Exception("Must run update first"); - } -} - -} diff --git a/source/FAST/Algorithms/TemplateMatching/Tests.cpp b/source/FAST/Algorithms/TemplateMatching/Tests.cpp index 1bc262193..4a9fd2872 100644 --- a/source/FAST/Algorithms/TemplateMatching/Tests.cpp +++ b/source/FAST/Algorithms/TemplateMatching/Tests.cpp @@ -1,5 +1,5 @@ #include -#include "TemplateMatchingNCC.hpp" +#include "TemplateMatching.hpp" #include #include #include @@ -19,7 +19,7 @@ TEST_CASE("Template matching NCC", "[fast][NCC][TemplateMatching][visual]") { Vector2i position = Vector2i(120, 100) + size/2; auto templateImage = image->crop(position - size/2, size); - auto matching = TemplateMatchingNCC::New(); + auto matching = TemplateMatching::New(); matching->enableRuntimeMeasurements(); matching->setRegionOfInterest(position, Vector2i(16, 16)); matching->setInputData(0, image); @@ -72,7 +72,7 @@ TEST_CASE("Template matching NCC synthetic sequence", "[fast][NCC][TemplateMatch streamer->setFilenameFormat(join(Config::getTestDataPath(), "Synthetic/ImageTracking/frame_#.png")); //streamer->setMainDevice(DeviceManager::getInstance()->getOneGPUDevice()); - auto matching = TemplateMatchingNCC::New(); + auto matching = TemplateMatching::New(); matching->getReporter().setReportMethod(Reporter::NONE); matching->setInputData(1, templateImage); matching->setInputConnection(0, streamer->getOutputPort());