John Richardson | 8de9261 | 2018-02-22 14:09:31 +0000 | [diff] [blame] | 1 | /* |
| 2 | * Copyright (c) 2018 ARM Limited. |
| 3 | * |
| 4 | * SPDX-License-Identifier: MIT |
| 5 | * |
| 6 | * Permission is hereby granted, free of charge, to any person obtaining a copy |
| 7 | * of this software and associated documentation files (the "Software"), to |
| 8 | * deal in the Software without restriction, including without limitation the |
| 9 | * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or |
| 10 | * sell copies of the Software, and to permit persons to whom the Software is |
| 11 | * furnished to do so, subject to the following conditions: |
| 12 | * |
| 13 | * The above copyright notice and this permission notice shall be included in all |
| 14 | * copies or substantial portions of the Software. |
| 15 | * |
| 16 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR |
| 17 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, |
| 18 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE |
| 19 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER |
| 20 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, |
| 21 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE |
| 22 | * SOFTWARE. |
| 23 | */ |
| 24 | #include "OpticalFlow.h" |
| 25 | |
| 26 | #include "GaussianPyramidHalf.h" |
| 27 | #include "Scharr.h" |
| 28 | #include "Utils.h" |
| 29 | |
| 30 | namespace arm_compute |
| 31 | { |
| 32 | namespace test |
| 33 | { |
| 34 | namespace validation |
| 35 | { |
| 36 | namespace reference |
| 37 | { |
| 38 | namespace |
| 39 | { |
| 40 | using KeyPointArray = std::vector<KeyPoint>; |
| 41 | using InternalKeyPointArray = std::vector<InternalKeyPoint>; |
| 42 | |
| 43 | // Constants used for Lucas-Kanade Algorithm |
| 44 | constexpr int W_BITS = 14; |
| 45 | constexpr float D0 = 1 << W_BITS; |
| 46 | constexpr float DETERMINANT_THRESHOLD = 1.0e-07f; |
| 47 | constexpr float EIGENVALUE_THRESHOLD = 1.0e-04f; |
| 48 | constexpr float FLT_SCALE = 1.0f / (1 << 20); |
| 49 | |
| 50 | // Creates an InternalKeyPointArray for tracking non-integral pixel coordinates |
| 51 | InternalKeyPointArray create_internal_keypoints(const KeyPointArray &keypoints) |
| 52 | { |
| 53 | InternalKeyPointArray internal_keypoints; |
| 54 | |
| 55 | for(auto keypoint : keypoints) |
| 56 | { |
| 57 | InternalKeyPoint internal_keypoint; |
| 58 | |
| 59 | internal_keypoint.x = static_cast<float>(keypoint.x); |
| 60 | internal_keypoint.y = static_cast<float>(keypoint.y); |
| 61 | internal_keypoint.tracking_status = static_cast<bool>(keypoint.tracking_status); |
| 62 | |
| 63 | internal_keypoints.push_back(internal_keypoint); |
| 64 | } |
| 65 | |
| 66 | return internal_keypoints; |
| 67 | } |
| 68 | |
| 69 | // Scale tracked points based on Pyramid level |
| 70 | void scale_tracked_points(size_t level, size_t num_levels, bool use_initial_estimate, |
| 71 | InternalKeyPointArray &old_points_internal, InternalKeyPointArray &new_points_internal, |
| 72 | const KeyPointArray &old_points, const KeyPointArray &new_points_estimates) |
| 73 | { |
| 74 | if(level == num_levels - 1) // lowest resolution |
| 75 | { |
| 76 | const float scale = std::pow(SCALE_PYRAMID_HALF, level); |
| 77 | |
| 78 | for(size_t i = 0; i < old_points.size(); ++i) |
| 79 | { |
| 80 | old_points_internal.at(i).x = old_points.at(i).x * scale; |
| 81 | old_points_internal.at(i).y = old_points.at(i).y * scale; |
| 82 | old_points_internal.at(i).tracking_status = true; |
| 83 | |
| 84 | InternalKeyPoint keypoint_to_track; |
| 85 | |
| 86 | if(use_initial_estimate) |
| 87 | { |
| 88 | keypoint_to_track.x = new_points_estimates.at(i).x * scale; |
| 89 | keypoint_to_track.y = new_points_estimates.at(i).y * scale; |
| 90 | keypoint_to_track.tracking_status = (new_points_estimates.at(i).tracking_status == 1); |
| 91 | } |
| 92 | else |
| 93 | { |
| 94 | keypoint_to_track.x = old_points_internal.at(i).x; |
| 95 | keypoint_to_track.y = old_points_internal.at(i).y; |
| 96 | keypoint_to_track.tracking_status = true; |
| 97 | } |
| 98 | |
| 99 | new_points_internal.at(i) = keypoint_to_track; |
| 100 | } |
| 101 | } |
| 102 | else |
| 103 | { |
| 104 | for(size_t i = 0; i < old_points.size(); ++i) |
| 105 | { |
| 106 | old_points_internal.at(i).x /= SCALE_PYRAMID_HALF; |
| 107 | old_points_internal.at(i).y /= SCALE_PYRAMID_HALF; |
| 108 | new_points_internal.at(i).x /= SCALE_PYRAMID_HALF; |
| 109 | new_points_internal.at(i).y /= SCALE_PYRAMID_HALF; |
| 110 | } |
| 111 | } |
| 112 | } |
| 113 | |
| 114 | bool is_invalid_keypoint(const InternalKeyPoint &keypoint, const ValidRegion &valid_region, size_t window_dimension) |
| 115 | { |
| 116 | const int half_window = window_dimension / 2; |
| 117 | const int x = std::floor(keypoint.x); |
| 118 | const int y = std::floor(keypoint.y); |
| 119 | |
| 120 | return (x - half_window < valid_region.start(0)) || (x + half_window >= valid_region.end(0) - 1) || (y - half_window < valid_region.start(1)) || (y + half_window >= valid_region.end(1) - 1); |
| 121 | } |
| 122 | |
| 123 | template <typename T> |
| 124 | constexpr int INT_ROUND(T x, int n) |
| 125 | { |
| 126 | return (x + (1 << (n - 1))) >> n; |
| 127 | } |
| 128 | |
| 129 | // Return the bilinear value at a specified coordinate with different border modes |
| 130 | template <typename T> |
| 131 | int bilinear_interpolate(const SimpleTensor<T> &in, Coordinates id, float wx, float wy, BorderMode border_mode, T constant_border_value, int scale) |
| 132 | { |
| 133 | const int level = id.x(); |
| 134 | const int idy = id.y(); |
| 135 | |
| 136 | const float dx = wx; |
| 137 | const float dy = wy; |
| 138 | const float dx_1 = 1.0f - dx; |
| 139 | const float dy_1 = 1.0f - dy; |
| 140 | |
| 141 | const T border_value = constant_border_value; |
| 142 | |
| 143 | id.set(0, level); |
| 144 | id.set(1, idy); |
| 145 | const T tl = tensor_elem_at(in, id, border_mode, border_value); |
| 146 | id.set(0, level + 1); |
| 147 | id.set(1, idy); |
| 148 | const T tr = tensor_elem_at(in, id, border_mode, border_value); |
| 149 | id.set(0, level); |
| 150 | id.set(1, idy + 1); |
| 151 | const T bl = tensor_elem_at(in, id, border_mode, border_value); |
| 152 | id.set(0, level + 1); |
| 153 | id.set(1, idy + 1); |
| 154 | const T br = tensor_elem_at(in, id, border_mode, border_value); |
| 155 | |
| 156 | // weights |
| 157 | const int w00 = roundf(dx_1 * dy_1 * D0); |
| 158 | const int w01 = roundf(dx * dy_1 * D0); |
| 159 | const int w10 = roundf(dx_1 * dy * D0); |
| 160 | const int w11 = D0 - w00 - w01 - w10; |
| 161 | |
| 162 | return static_cast<int>(INT_ROUND(tl * w00 + tr * w01 + bl * w10 + br * w11, scale)); |
| 163 | } |
| 164 | |
| 165 | template <typename T> |
| 166 | std::vector<int> compute_derivative(const SimpleTensor<T> &input, const InternalKeyPoint &keypoint, |
| 167 | BorderMode border_mode, uint8_t constant_border_value, size_t window_dimension, int scale) |
| 168 | { |
| 169 | std::vector<int> bilinear_values; |
| 170 | |
| 171 | const int half_window = window_dimension / 2; |
| 172 | |
| 173 | float keypoint_int_x = 0; |
| 174 | float keypoint_int_y = 0; |
| 175 | |
| 176 | const float wx = std::modf(keypoint.x, &keypoint_int_x); |
| 177 | const float wy = std::modf(keypoint.y, &keypoint_int_y); |
| 178 | |
| 179 | Coordinates tl_window(static_cast<int>(keypoint_int_x) - half_window, static_cast<int>(keypoint_int_y) - half_window); |
| 180 | Coordinates br_window(static_cast<int>(keypoint_int_x) + half_window, static_cast<int>(keypoint_int_y) + half_window); |
| 181 | |
| 182 | for(int y = tl_window.y(); y <= br_window.y(); ++y) |
| 183 | { |
| 184 | for(int x = tl_window.x(); x <= br_window.x(); ++x) |
| 185 | { |
| 186 | bilinear_values.push_back(bilinear_interpolate(input, Coordinates(x, y), wx, wy, border_mode, static_cast<T>(constant_border_value), scale)); |
| 187 | } |
| 188 | } |
| 189 | |
| 190 | return bilinear_values; |
| 191 | } |
| 192 | |
| 193 | std::tuple<float, float, float> compute_spatial_gradient_matrix(const std::vector<int> &bilinear_ix, const std::vector<int> &bilinear_iy) |
| 194 | { |
| 195 | ARM_COMPUTE_ERROR_ON(bilinear_ix.size() != bilinear_iy.size()); |
| 196 | |
| 197 | int iA11 = 0; |
| 198 | int iA12 = 0; |
| 199 | int iA22 = 0; |
| 200 | |
| 201 | for(size_t i = 0; i < bilinear_ix.size(); ++i) |
| 202 | { |
| 203 | int ixval = bilinear_ix[i]; |
| 204 | int iyval = bilinear_iy[i]; |
| 205 | |
| 206 | iA11 += ixval * ixval; |
| 207 | iA12 += ixval * iyval; |
| 208 | iA22 += iyval * iyval; |
| 209 | } |
| 210 | |
| 211 | return std::make_tuple(iA11 * FLT_SCALE, iA12 * FLT_SCALE, iA22 * FLT_SCALE); |
| 212 | } |
| 213 | |
| 214 | std::tuple<double, double> compute_temporal_gradient_vector(const std::vector<int> &bilinear_it_old, |
| 215 | const std::vector<int> &bilinear_it_new, |
| 216 | const std::vector<int> &bilinear_ix, |
| 217 | const std::vector<int> &bilinear_iy) |
| 218 | { |
| 219 | ARM_COMPUTE_ERROR_ON(bilinear_ix.size() != bilinear_iy.size()); |
| 220 | ARM_COMPUTE_ERROR_ON(bilinear_it_old.size() != bilinear_it_new.size()); |
| 221 | |
| 222 | int ib1 = 0; |
| 223 | int ib2 = 0; |
| 224 | |
| 225 | for(size_t i = 0; i < bilinear_ix.size(); ++i) |
| 226 | { |
| 227 | int ixval = bilinear_ix[i]; |
| 228 | int iyval = bilinear_iy[i]; |
| 229 | int ival = bilinear_it_old[i]; |
| 230 | int jval = bilinear_it_new[i]; |
| 231 | |
| 232 | const int diff = jval - ival; |
| 233 | |
| 234 | ib1 += diff * ixval; |
| 235 | ib2 += diff * iyval; |
| 236 | } |
| 237 | |
| 238 | const double b1 = ib1 * FLT_SCALE; |
| 239 | const double b2 = ib2 * FLT_SCALE; |
| 240 | |
| 241 | return std::make_tuple(b1, b2); |
| 242 | } |
| 243 | } // namespace |
| 244 | |
| 245 | template <typename T> |
| 246 | std::vector<KeyPoint> optical_flow(const SimpleTensor<T> &old_input, const SimpleTensor<T> &new_input, |
| 247 | const OpticalFlowParameters ¶ms, size_t num_levels, |
| 248 | const std::vector<KeyPoint> &old_points, const std::vector<KeyPoint> &new_points_estimates, |
| 249 | BorderMode border_mode, uint8_t constant_border_value) |
| 250 | { |
| 251 | const int filter_size = 3; // scharr filter size |
| 252 | const size_t max_iterations = 1000; // fixed by kernel |
| 253 | const size_t window_dimension = params.window_dimension; |
| 254 | const size_t num_iterations = (params.termination == Termination::TERM_CRITERIA_EPSILON) ? max_iterations : params.num_iterations; |
| 255 | |
| 256 | KeyPointArray new_points(old_points.size()); |
| 257 | |
| 258 | InternalKeyPointArray old_points_internal = create_internal_keypoints(old_points); |
| 259 | InternalKeyPointArray new_points_internal = create_internal_keypoints(new_points_estimates); |
| 260 | |
| 261 | SimpleTensor<int16_t> scharr_gx; |
| 262 | SimpleTensor<int16_t> scharr_gy; |
| 263 | |
| 264 | // Create pyramids |
| 265 | std::vector<SimpleTensor<T>> old_pyramid = gaussian_pyramid_half(old_input, border_mode, constant_border_value, num_levels); |
| 266 | std::vector<SimpleTensor<T>> new_pyramid = gaussian_pyramid_half(new_input, border_mode, constant_border_value, num_levels); |
| 267 | |
| 268 | // Iterate over each level of the pyramid |
| 269 | for(size_t idx = num_levels; idx > 0; --idx) |
| 270 | { |
| 271 | const size_t level = idx - 1; |
| 272 | |
| 273 | // Calculate scharr gradients |
| 274 | std::tie(scharr_gx, scharr_gy) = scharr<int16_t, T>(old_pyramid[level], filter_size, border_mode, constant_border_value, GradientDimension::GRAD_XY); |
| 275 | |
| 276 | scale_tracked_points(level, num_levels, params.use_initial_estimate, old_points_internal, new_points_internal, old_points, new_points_estimates); |
| 277 | |
| 278 | // Calculate valid region based on image dimensions of current pyramid level |
| 279 | const ValidRegion valid_region = shape_to_valid_region(old_pyramid[level].shape(), (border_mode == BorderMode::UNDEFINED), BorderSize(filter_size / 2)); |
| 280 | |
| 281 | for(size_t i = 0; i < old_points.size(); ++i) |
| 282 | { |
| 283 | InternalKeyPoint &old_keypoint = old_points_internal.at(i); |
| 284 | InternalKeyPoint &new_keypoint = new_points_internal.at(i); |
| 285 | |
| 286 | // Helper function for untracking keypoints when on the lowest pyramid level (high resolution) |
| 287 | const auto untrack_keypoint = [&](bool predicate) |
| 288 | { |
| 289 | if(predicate && (level == 0)) |
| 290 | { |
| 291 | new_keypoint.tracking_status = false; |
| 292 | return true; |
| 293 | } |
| 294 | return predicate; |
| 295 | }; |
| 296 | |
| 297 | if(!old_keypoint.tracking_status) |
| 298 | { |
| 299 | continue; |
| 300 | } |
| 301 | |
| 302 | // Check if tracked coordinate is outside image coordinate |
| 303 | if(untrack_keypoint(is_invalid_keypoint(old_keypoint, valid_region, window_dimension))) |
| 304 | { |
| 305 | continue; |
| 306 | } |
| 307 | |
| 308 | // Compute spatial derivative |
| 309 | std::vector<int> bilinear_ix = compute_derivative(scharr_gx, old_keypoint, border_mode, constant_border_value, window_dimension, W_BITS); |
| 310 | std::vector<int> bilinear_iy = compute_derivative(scharr_gy, old_keypoint, border_mode, constant_border_value, window_dimension, W_BITS); |
| 311 | |
| 312 | float A11 = 0.f; |
| 313 | float A12 = 0.f; |
| 314 | float A22 = 0.f; |
| 315 | std::tie(A11, A12, A22) = compute_spatial_gradient_matrix(bilinear_ix, bilinear_iy); |
| 316 | |
| 317 | // Calculate criteria for lost tracking : Matrix A is invertible |
| 318 | // 1. The determinant of the matrix is less than DETERMINANT_THRESHOLD |
| 319 | // 2. The minimum eigenvalue of the matrix is less than EIGENVALUE_THRESHOLD |
| 320 | const float trace_A = A11 + A22; |
| 321 | const float determinant = A11 * A22 - A12 * A12; |
| 322 | const float discriminant = (trace_A * trace_A) - 4.0f * (determinant); |
| 323 | const float eigenvalue_A = (trace_A - std::sqrt(discriminant)) / 2.0f; |
| 324 | |
| 325 | // Divide by window_dimension squared to reduce the floating point accummulation error |
| 326 | const float eigenvalue = eigenvalue_A / (window_dimension * window_dimension); |
| 327 | |
| 328 | // Check if it is a good point to track |
| 329 | if(untrack_keypoint(eigenvalue < EIGENVALUE_THRESHOLD || determinant < DETERMINANT_THRESHOLD)) |
| 330 | { |
| 331 | continue; |
| 332 | } |
| 333 | |
| 334 | float prev_delta_x = 0.f; |
| 335 | float prev_delta_y = 0.f; |
| 336 | |
| 337 | for(size_t j = 0; j < num_iterations; ++j) |
| 338 | { |
| 339 | // Check if tracked coordinate is outside image coordinate |
| 340 | if(untrack_keypoint(is_invalid_keypoint(new_keypoint, valid_region, window_dimension))) |
| 341 | { |
| 342 | break; |
| 343 | } |
| 344 | |
| 345 | // Compute temporal derivative |
| 346 | std::vector<int> bilinear_it_old = compute_derivative(old_pyramid[level], old_keypoint, border_mode, constant_border_value, window_dimension, W_BITS - 5); |
| 347 | std::vector<int> bilinear_it_new = compute_derivative(new_pyramid[level], new_keypoint, border_mode, constant_border_value, window_dimension, W_BITS - 5); |
| 348 | |
| 349 | double b1 = 0.f; |
| 350 | double b2 = 0.f; |
| 351 | std::tie(b1, b2) = compute_temporal_gradient_vector(bilinear_it_old, bilinear_it_new, bilinear_ix, bilinear_iy); |
| 352 | |
| 353 | // Compute motion vector -> A^-1 * -b |
| 354 | const float delta_x = (A12 * b2 - A22 * b1) / determinant; |
| 355 | const float delta_y = (A12 * b1 - A11 * b2) / determinant; |
| 356 | |
| 357 | // Update the new position |
| 358 | new_keypoint.x += delta_x; |
| 359 | new_keypoint.y += delta_y; |
| 360 | |
| 361 | const float magnitude_squared = delta_x * delta_x + delta_y * delta_y; |
| 362 | |
| 363 | // Check if termination criteria is EPSILON and if it is satisfied |
| 364 | if(magnitude_squared <= params.epsilon && (params.termination == Termination::TERM_CRITERIA_EPSILON || params.termination == Termination::TERM_CRITERIA_BOTH)) |
| 365 | { |
| 366 | break; |
| 367 | } |
| 368 | |
| 369 | // Check convergence analyzing the previous delta |
| 370 | if(j > 0 && (std::fabs(delta_x + prev_delta_x) < 0.01f && std::fabs(delta_y + prev_delta_y) < 0.01f)) |
| 371 | { |
| 372 | new_keypoint.x -= delta_x * SCALE_PYRAMID_HALF; |
| 373 | new_keypoint.y -= delta_y * SCALE_PYRAMID_HALF; |
| 374 | |
| 375 | break; |
| 376 | } |
| 377 | |
| 378 | prev_delta_x = delta_x; |
| 379 | prev_delta_y = delta_y; |
| 380 | } |
| 381 | } |
| 382 | } |
| 383 | |
| 384 | // Copy optical flow coordinates to output vector |
| 385 | for(size_t i = 0; i < old_points.size(); ++i) |
| 386 | { |
| 387 | const InternalKeyPoint &new_keypoint = new_points_internal.at(i); |
| 388 | |
| 389 | new_points.at(i).x = roundf(new_keypoint.x); |
| 390 | new_points.at(i).y = roundf(new_keypoint.y); |
| 391 | new_points.at(i).tracking_status = new_keypoint.tracking_status ? 1 : 0; |
| 392 | } |
| 393 | |
| 394 | return new_points; |
| 395 | } |
| 396 | |
| 397 | template std::vector<KeyPoint> optical_flow(const SimpleTensor<uint8_t> &old_input, const SimpleTensor<uint8_t> &new_input, |
| 398 | const OpticalFlowParameters ¶ms, size_t num_levels, |
| 399 | const std::vector<KeyPoint> &old_points, const std::vector<KeyPoint> &new_points_estimates, |
| 400 | BorderMode border_mode, uint8_t constant_border_value); |
| 401 | } // namespace reference |
| 402 | } // namespace validation |
| 403 | } // namespace test |
| 404 | } // namespace arm_compute |