/* * SPDX-License-Identifier: MIT * * Copyright (C) 2013-2024 OpenMV, LLC. * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal * in the Software without restriction, including without limitation the rights * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * copies of the Software, and to permit persons to whom the Software is * furnished to do so, subject to the following conditions: * * The above copyright notice and this permission notice shall be included in * all copies or substantial portions of the Software. * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * THE SOFTWARE. * * Template matching with NCC (Normalized Cross Correlation) using exhaustive and diamond search. * * References: * Briechle, Kai, and Uwe D. Hanebeck. "Template matching using fast normalized cross correlation." Aerospace * Lewis, J. P. "Fast normalized cross-correlation." * Zhu, Shan, and Kai-Kuang Ma. "A new diamond search algorithm for fast block-matching motion estimation." */ #include #include #include #include "imlib.h" static void set_dsp(int cx, int cy, point_t *pts, bool sdsp, int step) { if (sdsp) { // Small DSP // 4 // 3 0 1 // 2 pts[0].x = cx; pts[0].y = cy; pts[1].x = cx + step / 2; pts[1].y = cy; pts[2].x = cx; pts[2].y = cy + step / 2; pts[3].x = cx - step / 2; pts[3].y = cy; pts[4].x = cx; pts[4].y = cy - step / 2; } else { // Large DSP // 7 // 6 8 // 5 0 1 // 4 2 // 3 pts[0].x = cx; pts[0].y = cy; pts[1].x = cx + step; pts[1].y = cy; pts[2].x = cx + step / 2; pts[2].y = cy + step / 2; pts[3].x = cx; pts[3].y = cy + step; pts[4].x = cx - step / 2; pts[4].y = cy + step / 2; pts[5].x = cx - step; pts[5].y = cy; pts[6].x = cx - step / 2; pts[6].y = cy - step / 2; pts[7].x = cx; pts[7].y = cy - step; pts[8].x = cx + step / 2; pts[8].y = cy - step / 2; } } static float find_block_ncc(image_t *f, image_t *t, i_image_t *sum, int t_mean, uint32_t t_sumsq, int u, int v) { int w = t->w; int h = t->h; int num = 0; uint32_t f_sumsq = 0; if (u < 0) { u = 0; } if (v < 0) { v = 0; } if (u + w >= f->w) { w = f->w - u; } if (v + h >= f->h) { h = f->h - v; } // Find the mean of the current patch uint32_t f_sum = imlib_integral_lookup(sum, u, v, w, h); uint32_t f_mean = f_sum / (w * h); // Find the normalized sum of squares of the image for (int y = v; y < v + h; y++) { for (int x = u; x < u + w; x++) { int a = (int) f->data[y * f->w + x] - f_mean; int b = (int) t->data[(y - v) * t->w + (x - u)] - t_mean; num += a * b; f_sumsq += a * a; } } // Find the normalized cross-correlation return (num / (fast_sqrtf(f_sumsq) * fast_sqrtf(t_sumsq))); } float imlib_template_match_ds(image_t *f, image_t *t, rectangle_t *r) { point_t pts[9]; // Integral images i_image_t sum; imlib_integral_image_alloc(&sum, f->w, f->h); imlib_integral_image(f, &sum); // Normalized sum of squares of the template int t_mean = 0; uint32_t t_sumsq = 0; imlib_image_mean(t, &t_mean, &t_mean, &t_mean); for (int i = 0; i < (t->w * t->h); i++) { int c = (int) t->data[i] - t_mean; t_sumsq += c * c; } int px = 0; int py = 0; // Initial center point int cx = f->w / 2 - t->w / 2; int cy = f->h / 2 - t->h / 2; // Max cross-correlation float max_xc = -FLT_MAX; // Start with the Large Diamond Search Pattern (LDSP) 9 points. bool sdsp = false; // Step size == template width int step = t->w; while (step > 0) { // Set the Diamond Search Pattern (DSP). set_dsp(cx, cy, pts, sdsp, step); // Set the number of search blocks (5 or 9 for SDSP and LDSP respectively). int num_pts = (sdsp == true)? 5: 9; // Find the block with the highest NCC for (int i = 0; i < num_pts; i++) { if (pts[i].x >= f->w || pts[i].y >= f->h) { continue; } float blk_xc = find_block_ncc(f, t, &sum, t_mean, t_sumsq, pts[i].x, pts[i].y); if (blk_xc > max_xc) { px = pts[i].x; py = pts[i].y; max_xc = blk_xc; } } // If the highest correlation is found at the center block and search is using // LDSP then the highest correlation is found, if not then switch search to SDSP. if (px == cx && py == cy) { // Note instead of switching to the smaller pattern, the step size can be reduced // each time the highest correlation is found at the center, and break on step == 0. // This makes DS much more accurate, but slower. step--; } // Set the new search center to the block with highest correlation cx = px; cy = py; } r->x = cx; r->y = cy; r->w = t->w; r->h = t->h; if (cx < 0) { r->x = 0; } if (cy < 0) { r->y = 0; } if (cx + t->w > f->w) { r->w = f->w - cx; } if (cy + t->h > f->h) { r->h = f->h - cy; } imlib_integral_image_free(&sum); //printf("max xc: %f\n", (double) max_xc); return max_xc; } /* The NCC can be optimized using integral images and rectangular basis functions. * See Kai Briechle's paper "Template Matching using Fast Normalized Cross Correlation". * * NOTE: only the denominator is optimized. * */ float imlib_template_match_ex(image_t *f, image_t *t, rectangle_t *roi, int step, rectangle_t *r) { int den_b = 0; float corr = 0.0f; // Integral images i_image_t sum; i_image_t sumsq; imlib_integral_image_alloc(&sum, f->w, f->h); imlib_integral_image_alloc(&sumsq, f->w, f->h); imlib_integral_image(f, &sum); imlib_integral_image_sq(f, &sumsq); // Normalized sum of squares of the template int t_mean = 0; imlib_image_mean(t, &t_mean, &t_mean, &t_mean); for (int i = 0; i < (t->w * t->h); i++) { int c = (int) t->data[i] - t_mean; den_b += c * c; } for (int v = roi->y; v <= (roi->y + roi->h - t->h); v += step) { for (int u = roi->x; u <= (roi->x + roi->w - t->w); u += step) { int num = 0; // The mean of the current patch uint32_t f_sum = imlib_integral_lookup(&sum, u, v, t->w, t->h); uint32_t f_sumsq = imlib_integral_lookup(&sumsq, u, v, t->w, t->h); uint32_t f_mean = f_sum / (float) (t->w * t->h); // Normalized sum of squares of the image for (int y = v; y < (v + t->h); y++) { for (int x = u; x < (u + t->w); x++) { int a = (int) f->data[y * f->w + x] - f_mean; int b = (int) t->data[(y - v) * t->w + (x - u)] - t_mean; num += a * b; } } uint32_t den_a = f_sumsq - f_sum * (f_sum / (float) (t->w * t->h)); // Find normalized cross-correlation float c = num / (fast_sqrtf(den_a) * fast_sqrtf(den_b)); if (c > corr) { corr = c; r->x = u; r->y = v; r->w = t->w; r->h = t->h; } } } imlib_integral_image_free(&sum); imlib_integral_image_free(&sumsq); return corr; }