/* * Copyright (c) 2017-2018 ARM Limited. * * SPDX-License-Identifier: MIT * * 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. */ #include "helpers.h" #include "types.h" /* *The criteria for lost tracking is that the spatial gradient matrix has: * - Determinant less than DETERMINANT_THR * - or minimum eigenvalue is smaller then EIGENVALUE_THR * * The thresholds for the determinant and the minimum eigenvalue is * defined by the OpenVX spec * * Note: Also lost tracking happens when the point tracked coordinate is outside * the image coordinates * * https://www.khronos.org/registry/vx/specs/1.0/html/d0/d0c/group__group__vision__function__opticalflowpyrlk.html */ /* Internal Lucas-Kanade Keypoint struct */ typedef struct InternalKeypoint { float x; /**< The x coordinate. */ float y; /**< The y coordinate. */ float tracking_status; /**< A zero indicates a lost point. Initialized to 1 by corner detectors. */ float dummy; /**< Dummy member for alignment. */ } InternalKeypoint; /** Threshold for the determinant. Used for lost tracking criteria */ #define DETERMINANT_THR 1.0e-07f /** Thresholds for minimum eigenvalue. Used for lost tracking criteria */ #define EIGENVALUE_THR 1.0e-04f /** Constants used for Lucas-Kanade Algorithm */ #define W_BITS (14) #define FLT_SCALE (1.0f / (float)(1 << 20)) #define D0 ((float)(1 << W_BITS)) #define D1 (1.0f / (float)(1 << (W_BITS - 5))) /** Initializes the internal new points array when the level of pyramid is NOT equal to max. * * @param[in,out] old_points_internal An array of internal key points that are defined at the old_images high resolution pyramid. * @param[in,out] new_points_internal An array of internal key points that are defined at the new_images high resolution pyramid. * @param[in] scale Scale factor to apply for the new_point coordinates. */ __kernel void init_level( __global float4 *old_points_internal, __global float4 *new_points_internal, const float scale) { int idx = get_global_id(0); // Get old and new keypoints float4 old_point = old_points_internal[idx]; float4 new_point = new_points_internal[idx]; // Scale accordingly with the pyramid_scale old_point.xy *= (float2)(2.0f); new_point.xy *= (float2)(2.0f); old_points_internal[idx] = old_point; new_points_internal[idx] = new_point; } /** Initializes the internal new points array when the level of pyramid is equal to max. * * @param[in] old_points An array of key points that are defined at the old_images high resolution pyramid. * @param[in,out] old_points_internal An array of internal key points that are defined at the old_images high resolution pyramid. * @param[out] new_points_internal An array of internal key points that are defined at the new_images high resolution pyramid. * @param[in] scale Scale factor to apply for the new_point coordinates. */ __kernel void init_level_max( __global Keypoint *old_points, __global InternalKeypoint *old_points_internal, __global InternalKeypoint *new_points_internal, const float scale) { int idx = get_global_id(0); Keypoint old_point = old_points[idx]; // Get old keypoint to track InternalKeypoint old_point_internal; old_point_internal.x = old_point.x * scale; old_point_internal.y = old_point.y * scale; old_point_internal.tracking_status = 1.f; // Store internal keypoints old_points_internal[idx] = old_point_internal; new_points_internal[idx] = old_point_internal; } /** Initializes the new_points array when the level of pyramid is equal to max and if use_initial_estimate = 1. * * @param[in] old_points An array of key points that are defined at the old_images high resolution pyramid. * @param[in] new_points_estimates An array of estimate key points that are defined at the old_images high resolution pyramid. * @param[in,out] old_points_internal An array of internal key points that are defined at the old_images high resolution pyramid. * @param[out] new_points_internal An array of internal key points that are defined at the new_images high resolution pyramid. * @param[in] scale Scale factor to apply for the new_point coordinates. */ __kernel void init_level_max_initial_estimate( __global Keypoint *old_points, __global Keypoint *new_points_estimates, __global InternalKeypoint *old_points_internal, __global InternalKeypoint *new_points_internal, const float scale) { int idx = get_global_id(0); Keypoint old_point = old_points[idx]; Keypoint new_point_estimate = new_points_estimates[idx]; InternalKeypoint old_point_internal; InternalKeypoint new_point_internal; // Get old keypoint to track old_point_internal.x = old_point.x * scale; old_point_internal.y = old_point.y * scale; old_point_internal.tracking_status = 1.f; // Get new keypoint to track new_point_internal.x = new_point_estimate.x * scale; new_point_internal.y = new_point_estimate.y * scale; new_point_internal.tracking_status = new_point_estimate.tracking_status; // Store internal keypoints old_points_internal[idx] = old_point_internal; new_points_internal[idx] = new_point_internal; } /** Truncates the coordinates stored in new_points array * * @param[in] new_points_internal An array of estimate key points that are defined at the new_images high resolution pyramid. * @param[out] new_points An array of internal key points that are defined at the new_images high resolution pyramid. */ __kernel void finalize( __global InternalKeypoint *new_points_internal, __global Keypoint *new_points) { int idx = get_global_id(0); // Load internal keypoint InternalKeypoint new_point_internal = new_points_internal[idx]; // Calculate output point Keypoint new_point; new_point.x = round(new_point_internal.x); new_point.y = round(new_point_internal.y); new_point.strength = 0.f; new_point.scale = 0.f; new_point.orientation = 0.f; new_point.tracking_status = new_point_internal.tracking_status; new_point.error = 0.f; // Store new point new_points[idx] = new_point; } /** Computes A11, A12, A22, min_eig, ival, ixval and iyval at level 0th of the pyramid. These values will be used in step 1. * * @param[in] old_image_ptr Pointer to the input old image. Supported data types: U8 * @param[in] old_image_stride_x Stride of the input old image in X dimension (in bytes) * @param[in] old_image_step_x old_image_stride_x * number of elements along X processed per workitem(in bytes) * @param[in] old_image_stride_y Stride of the input old image in Y dimension (in bytes) * @param[in] old_image_step_y old_image_stride_y * number of elements along Y processed per workitem(in bytes) * @param[in] old_image_offset_first_element_in_bytes The offset of the first element in the input old image * @param[in] old_scharr_gx_ptr Pointer to the input scharr x image. Supported data types: S16 * @param[in] old_scharr_gx_stride_x Stride of the input scharr x image in X dimension (in bytes) * @param[in] old_scharr_gx_step_x old_scharr_gx_stride_x * number of elements along X processed per workitem(in bytes) * @param[in] old_scharr_gx_stride_y Stride of the input scharr x image in Y dimension (in bytes) * @param[in] old_scharr_gx_step_y old_scharr_gx_stride_y * number of elements along Y processed per workitem(in bytes) * @param[in] old_scharr_gx_offset_first_element_in_bytes The offset of the first element in the input scharr x image * @param[in] old_scharr_gy_ptr Pointer to the input scharr y image. Supported data types: S16 * @param[in] old_scharr_gy_stride_x Stride of the input scharr y image in X dimension (in bytes) * @param[in] old_scharr_gy_step_x old_scharr_gy_stride_x * number of elements along X processed per workitem(in bytes) * @param[in] old_scharr_gy_stride_y Stride of the input scharr y image in Y dimension (in bytes) * @param[in] old_scharr_gy_step_y old_scharr_gy_stride_y * number of elements along Y processed per workitem(in bytes) * @param[in] old_scharr_gy_offset_first_element_in_bytes The offset of the first element in the input scharr y image * @param[in] old_points An array of key points. Those key points are defined at the old_images high resolution pyramid * @param[in, out] new_points An output array of key points. Those key points are defined at the new_images high resolution pyramid * @param[out] coeff It stores | A11 | A12 | A22 | min_eig | for each keypoint * @param[out] iold_val It stores | ival | ixval | iyval | dummy | for each point in the window centered on old_keypoint * @param[in] window_dimension The size of the window on which to perform the algorithm * @param[in] window_dimension_pow2 The squared size of the window on which to perform the algorithm * @param[in] half_window The half size of the window on which to perform the algorithm * @param[in] border_limits It stores the right border limit (width - window_dimension - 1, height - window_dimension - 1,) * @param[in] eig_const 1.0f / (float)(2.0f * window_dimension * window_dimension) * @param[in] level0 It is set to 1 if level 0 of the pyramid */ void __kernel lktracker_stage0( IMAGE_DECLARATION(old_image), IMAGE_DECLARATION(old_scharr_gx), IMAGE_DECLARATION(old_scharr_gy), __global float4 *old_points, __global float4 *new_points, __global float4 *coeff, __global short4 *iold_val, const int window_dimension, const int window_dimension_pow2, const int half_window, const float3 border_limits, const float eig_const, const int level0) { int idx = get_global_id(0); Image old_image = CONVERT_TO_IMAGE_STRUCT_NO_STEP(old_image); Image old_scharr_gx = CONVERT_TO_IMAGE_STRUCT_NO_STEP(old_scharr_gx); Image old_scharr_gy = CONVERT_TO_IMAGE_STRUCT_NO_STEP(old_scharr_gy); // Get old keypoint float2 old_keypoint = old_points[idx].xy - (float2)half_window; // Get the floor value float2 iold_keypoint = floor(old_keypoint); // Check if using the window dimension we can go out of boundary in the following for loops. If so, invalidate the tracked point if(any(iold_keypoint < border_limits.zz) || any(iold_keypoint >= border_limits.xy)) { if(level0 == 1) { // Invalidate tracked point as we are at level 0 new_points[idx].s2 = 0.0f; } // Not valid coordinate. It sets min_eig to 0.0f coeff[idx].s3 = 0.0f; return; } // Compute weight for the bilinear interpolation float2 ab = old_keypoint - iold_keypoint; // Weight used for Bilinear-Interpolation on Scharr images // w_scharr.s0 = (1.0f - ab.x) * (1.0f - ab.y) // w_scharr.s1 = ab.x * (1.0f - ab.y) // w_scharr.s2 = (1.0f - ab.x) * ab.y // w_scharr.s3 = ab.x * ab.y float4 w_scharr; w_scharr.s3 = ab.x * ab.y; w_scharr.s0 = w_scharr.s3 + 1.0f - ab.x - ab.y; w_scharr.s12 = ab - (float2)w_scharr.s3; // Weight used for Bilinear-Interpolation on Old and New images // w.s0 = round(w_scharr.s0 * D0) // w.s1 = round(w_scharr.s1 * D0) // w.s2 = round(w_scharr.s2 * D0) // w.s3 = w.s3 = D0 - w.s0 - w.s1 - w.s2 float4 w; w = round(w_scharr * (float4)D0); w.s3 = D0 - w.s0 - w.s1 - w.s2; // Added for matching VX implementation // G.s0 = A11, G.s1 = A12, G.s2 = A22, G.s3 = min_eig int4 iG = (int4)0; // Window offset int window_offset = idx * window_dimension_pow2; // Compute Spatial Gradient Matrix G for(ushort ky = 0; ky < window_dimension; ++ky) { int offset_y = iold_keypoint.y + ky; for(ushort kx = 0; kx < window_dimension; ++kx) { int offset_x = iold_keypoint.x + kx; float4 px; // Load values from old_image for computing the bilinear interpolation px = convert_float4((uchar4)(vload2(0, offset(&old_image, offset_x, offset_y)), vload2(0, offset(&old_image, offset_x, offset_y + 1)))); // old_i.s0 = ival, old_i.s1 = ixval, old_i.s2 = iyval, old_i.s3 = dummy float4 old_i; // Compute bilinear interpolation (with D1 scale factor) for ival old_i.s0 = dot(px, w) * D1; // Load values from old_scharr_gx for computing the bilinear interpolation px = convert_float4((short4)(vload2(0, (__global short *)offset(&old_scharr_gx, offset_x, offset_y)), vload2(0, (__global short *)offset(&old_scharr_gx, offset_x, offset_y + 1)))); // Compute bilinear interpolation for ixval old_i.s1 = dot(px, w_scharr); // Load values from old_scharr_gy for computing the bilinear interpolation px = convert_float4((short4)(vload2(0, (__global short *)offset(&old_scharr_gy, offset_x, offset_y)), vload2(0, (__global short *)offset(&old_scharr_gy, offset_x, offset_y + 1)))); // Compute bilinear interpolation for iyval old_i.s2 = dot(px, w_scharr); // Rounding (it could be omitted. Used just for matching the VX implementation) int4 iold = convert_int4(round(old_i)); // Accumulate values in the Spatial Gradient Matrix iG.s0 += (int)(iold.s1 * iold.s1); iG.s1 += (int)(iold.s1 * iold.s2); iG.s2 += (int)(iold.s2 * iold.s2); // Store ival, ixval and iyval iold_val[window_offset + kx] = convert_short4(iold); } window_offset += window_dimension; } // Scale iA11, iA12 and iA22 float4 G = convert_float4(iG) * (float4)FLT_SCALE; // Compute minimum eigen value G.s3 = (float)(G.s2 + G.s0 - sqrt(pown(G.s0 - G.s2, 2) + 4.0f * G.s1 * G.s1)) * eig_const; // Store A11. A11, A22 and min_eig coeff[idx] = G; } /** Computes the motion vector for a given keypoint * * @param[in] new_image_ptr Pointer to the input new image. Supported data types: U8 * @param[in] new_image_stride_x Stride of the input new image in X dimension (in bytes) * @param[in] new_image_step_x new_image_stride_x * number of elements along X processed per workitem(in bytes) * @param[in] new_image_stride_y Stride of the input new image in Y dimension (in bytes) * @param[in] new_image_step_y new_image_stride_y * number of elements along Y processed per workitem(in bytes) * @param[in] new_image_offset_first_element_in_bytes The offset of the first element in the input new image * @param[in, out] new_points An output array of key points. Those key points are defined at the new_images high resolution pyramid * @param[in] coeff The | A11 | A12 | A22 | min_eig | for each keypoint * @param[in] iold_val The | ival | ixval | iyval | dummy | for each point in the window centered on old_keypoint * @param[in] window_dimension The size of the window on which to perform the algorithm * @param[in] window_dimension_pow2 The squared size of the window on which to perform the algorithm * @param[in] half_window The half size of the window on which to perform the algorithm * @param[in] num_iterations The maximum number of iterations * @param[in] epsilon The value for terminating the algorithm. * @param[in] border_limits It stores the right border limit (width - window_dimension - 1, height - window_dimension - 1,) * @param[in] eig_const 1.0f / (float)(2.0f * window_dimension * window_dimension) * @param[in] level0 It is set to 1 if level of pyramid = 0 * @param[in] term_epsilon It is set to 1 if termination = TERM_CRITERIA_EPSILON */ void __kernel lktracker_stage1( IMAGE_DECLARATION(new_image), __global float4 *new_points, __global float4 *coeff, __global short4 *iold_val, const int window_dimension, const int window_dimension_pow2, const int half_window, const int num_iterations, const float epsilon, const float3 border_limits, const float eig_const, const int level0, const int term_epsilon) { int idx = get_global_id(0); Image new_image = CONVERT_TO_IMAGE_STRUCT_NO_STEP(new_image); // G.s0 = A11, G.s1 = A12, G.s2 = A22, G.s3 = min_eig float4 G = coeff[idx]; // Determinant float D = G.s0 * G.s2 - G.s1 * G.s1; // Check if it is a good point to track if(G.s3 < EIGENVALUE_THR || D < DETERMINANT_THR) { if(level0 == 1) { // Invalidate tracked point as we are at level 0 new_points[idx].s2 = 0; } return; } // Compute inverse //D = native_recip(D); D = 1.0 / D; // Get new keypoint float2 new_keypoint = new_points[idx].xy - (float)half_window; // Get new point float2 out_new_point = new_points[idx].xy; // Keep delta obtained in the previous iteration float2 prev_delta = (float2)0.0f; int j = 0; while(j < num_iterations) { // Get the floor value float2 inew_keypoint = floor(new_keypoint); // Check if using the window dimension we can go out of boundary in the following for loops. If so, invalidate the tracked point if(any(inew_keypoint < border_limits.zz) || any(inew_keypoint >= border_limits.xy)) { if(level0 == 1) { // Invalidate tracked point as we are at level 0 new_points[idx].s2 = 0.0f; } else { new_points[idx].xy = out_new_point; } return; } // Compute weight for the bilinear interpolation float2 ab = new_keypoint - inew_keypoint; // Weight used for Bilinear-Interpolation on Old and New images // w.s0 = round((1.0f - ab.x) * (1.0f - ab.y) * D0) // w.s1 = round(ab.x * (1.0f - ab.y) * D0) // w.s2 = round((1.0f - ab.x) * ab.y * D0) // w.s3 = D0 - w.s0 - w.s1 - w.s2 float4 w; w.s3 = ab.x * ab.y; w.s0 = w.s3 + 1.0f - ab.x - ab.y; w.s12 = ab - (float2)w.s3; w = round(w * (float4)D0); w.s3 = D0 - w.s0 - w.s1 - w.s2; // Mismatch vector int2 ib = 0; // Old val offset int old_val_offset = idx * window_dimension_pow2; for(int ky = 0; ky < window_dimension; ++ky) { for(int kx = 0; kx < window_dimension; ++kx) { // ival, ixval and iyval have been computed in the previous stage int4 old_ival = convert_int4(iold_val[old_val_offset]); // Load values from old_image for computing the bilinear interpolation float4 px = convert_float4((uchar4)(vload2(0, offset(&new_image, inew_keypoint.x + kx, inew_keypoint.y + ky)), vload2(0, offset(&new_image, inew_keypoint.x + kx, inew_keypoint.y + ky + 1)))); // Compute bilinear interpolation on new image int jval = (int)round(dot(px, w) * D1); // Compute luminance difference int diff = (int)(jval - old_ival.s0); // Accumulate values in mismatch vector ib += (diff * old_ival.s12); // Update old val offset old_val_offset++; } } float2 b = convert_float2(ib) * (float2)FLT_SCALE; // Optical Flow float2 delta; delta.x = (float)((G.s1 * b.y - G.s2 * b.x) * D); delta.y = (float)((G.s1 * b.x - G.s0 * b.y) * D); // Update new point coordinate new_keypoint += delta; out_new_point = new_keypoint + (float2)half_window; if(term_epsilon == 1) { float mag2 = dot(delta, delta); if(mag2 <= epsilon) { new_points[idx].xy = out_new_point; return; } } // Check convergence analyzing the previous delta if(j > 0 && all(fabs(delta + prev_delta) < (float2)0.01f)) { out_new_point -= delta * (float2)0.5f; new_points[idx].xy = out_new_point; return; } // Update previous delta prev_delta = delta; j++; } new_points[idx].xy = out_new_point; }