41 using InternalKeyPointArray = std::vector<InternalKeyPoint>;
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);
51 InternalKeyPointArray create_internal_keypoints(
const KeyPointArray &keypoints)
53 InternalKeyPointArray internal_keypoints;
55 for(
auto keypoint : keypoints)
57 InternalKeyPoint internal_keypoint;
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);
63 internal_keypoints.push_back(internal_keypoint);
66 return internal_keypoints;
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,
74 if(level == num_levels - 1)
78 for(
size_t i = 0; i < old_points.size(); ++i)
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;
84 InternalKeyPoint keypoint_to_track;
86 if(use_initial_estimate)
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);
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;
99 new_points_internal.at(i) = keypoint_to_track;
104 for(
size_t i = 0; i < old_points.size(); ++i)
114 bool is_invalid_keypoint(
const InternalKeyPoint &keypoint,
const ValidRegion &
valid_region,
size_t window_dimension)
116 const int half_window = window_dimension / 2;
117 const int x = std::floor(keypoint.x);
118 const int y = std::floor(keypoint.y);
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);
123 template <
typename T>
124 constexpr
int INT_ROUND(T x,
int n)
126 return (x + (1 << (n - 1))) >> n;
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)
133 const int level =
id.x();
134 const int idy =
id.y();
138 const float dx_1 = 1.0f - dx;
139 const float dy_1 = 1.0f - dy;
141 const T border_value = constant_border_value;
146 id.set(0, level + 1);
152 id.set(0, level + 1);
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;
162 return static_cast<int>(INT_ROUND(tl * w00 + tr * w01 + bl * w10 + br * w11, scale));
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)
169 std::vector<int> bilinear_values;
171 const int half_window = window_dimension / 2;
173 float keypoint_int_x = 0;
174 float keypoint_int_y = 0;
176 const float wx = std::modf(keypoint.x, &keypoint_int_x);
177 const float wy = std::modf(keypoint.y, &keypoint_int_y);
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);
182 for(
int y = tl_window.y(); y <= br_window.y(); ++y)
184 for(
int x = tl_window.x(); x <= br_window.x(); ++x)
186 bilinear_values.push_back(
bilinear_interpolate(input, Coordinates(x, y), wx, wy, border_mode, static_cast<T>(constant_border_value), scale));
190 return bilinear_values;
193 std::tuple<float, float, float> compute_spatial_gradient_matrix(
const std::vector<int> &bilinear_ix,
const std::vector<int> &bilinear_iy)
201 for(
size_t i = 0; i < bilinear_ix.size(); ++i)
203 int ixval = bilinear_ix[i];
204 int iyval = bilinear_iy[i];
206 iA11 += ixval * ixval;
207 iA12 += ixval * iyval;
208 iA22 += iyval * iyval;
211 return std::make_tuple(iA11 * FLT_SCALE, iA12 * FLT_SCALE, iA22 * FLT_SCALE);
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)
225 for(
size_t i = 0; i < bilinear_ix.size(); ++i)
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];
232 const int diff = jval - ival;
241 return std::make_tuple(b1, b2);
245 template <
typename T>
248 const std::vector<KeyPoint> &old_points,
const std::vector<KeyPoint> &new_points_estimates,
249 BorderMode border_mode, uint8_t constant_border_value)
251 const int filter_size = 3;
252 const size_t max_iterations = 1000;
258 InternalKeyPointArray old_points_internal = create_internal_keypoints(old_points);
259 InternalKeyPointArray new_points_internal = create_internal_keypoints(new_points_estimates);
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);
269 for(
size_t idx = num_levels; idx > 0; --idx)
271 const size_t level = idx - 1;
276 scale_tracked_points(level, num_levels, params.
use_initial_estimate, old_points_internal, new_points_internal, old_points, new_points_estimates);
281 for(
size_t i = 0; i < old_points.size(); ++i)
287 const auto untrack_keypoint = [&](
bool predicate)
289 if(predicate && (level == 0))
303 if(untrack_keypoint(is_invalid_keypoint(old_keypoint, valid_region, window_dimension)))
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);
315 std::tie(A11, A12, A22) = compute_spatial_gradient_matrix(bilinear_ix, bilinear_iy);
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;
326 const float eigenvalue = eigenvalue_A / (window_dimension * window_dimension);
329 if(untrack_keypoint(eigenvalue < EIGENVALUE_THRESHOLD || determinant < DETERMINANT_THRESHOLD))
334 float prev_delta_x = 0.f;
335 float prev_delta_y = 0.f;
337 for(
size_t j = 0; j < num_iterations; ++j)
340 if(untrack_keypoint(is_invalid_keypoint(new_keypoint, valid_region, window_dimension)))
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);
351 std::tie(b1, b2) = compute_temporal_gradient_vector(bilinear_it_old, bilinear_it_new, bilinear_ix, bilinear_iy);
354 const float delta_x = (A12 * b2 - A22 * b1) / determinant;
355 const float delta_y = (A12 * b1 - A11 * b2) / determinant;
358 new_keypoint.
x += delta_x;
359 new_keypoint.
y += delta_y;
361 const float magnitude_squared = delta_x * delta_x + delta_y * delta_y;
370 if(j > 0 && (std::fabs(delta_x + prev_delta_x) < 0.01f && std::fabs(delta_y + prev_delta_y) < 0.01f))
378 prev_delta_x = delta_x;
379 prev_delta_y = delta_y;
385 for(
size_t i = 0; i < old_points.size(); ++i)
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;
399 const std::vector<KeyPoint> &old_points,
const std::vector<KeyPoint> &new_points_estimates,
400 BorderMode border_mode, uint8_t constant_border_value);
BorderMode
Methods available to handle borders.
constexpr float SCALE_PYRAMID_HALF
Constant value used to indicate a half-scale pyramid.
float y
y coordinate of the keypoint
bool tracking_status
the tracking status of the keypoint
T tensor_elem_at(const SimpleTensor< T > &src, Coordinates coord, BorderMode border_mode, T constant_border_value)
bool use_initial_estimate
Container for 2D border size.
Terminate when within epsilon of a threshold.
std::vector< KeyPoint > optical_flow(const SimpleTensor< T > &old_input, const SimpleTensor< T > &new_input, const OpticalFlowParameters ¶ms, size_t num_levels, const std::vector< KeyPoint > &old_points, const std::vector< KeyPoint > &new_points_estimates, BorderMode border_mode, uint8_t constant_border_value)
#define ARM_COMPUTE_ERROR_ON(cond)
If the condition is true then an error message is printed and an exception thrown.
const ValidRegion valid_region
Terminate on whichever of the other conditions occurs first.
Copyright (c) 2017-2021 Arm Limited.
Basic implementation of the IArray interface which allocates a static number of T values...
Array< KeyPoint > KeyPointArray
Array of Key Points.
float x
x coordinate of the keypoint
Internal keypoint class for Lucas-Kanade Optical Flow.
Simple tensor object that stores elements in a consecutive chunk of memory.
x and y gradient dimension
Borders are left undefined.
std::vector< SimpleTensor< T > > gaussian_pyramid_half(const SimpleTensor< T > &src, BorderMode border_mode, uint8_t constant_border_value, size_t num_levels)
#define W_BITS
Constants used for Lucas-Kanade Algorithm.
Container for valid region of a window.
SimpleTensor< T > scale(const SimpleTensor< T > &src, float scale_x, float scale_y, InterpolationPolicy policy, BorderMode border_mode, T constant_border_value, SamplingPolicy sampling_policy, bool ceil_policy_scale, bool align_corners)
ValidRegion shape_to_valid_region(const TensorShape &a_shape, bool border_undefined=false, BorderSize border_size=BorderSize(0))
Create a valid region based on tensor shape, border mode and border size.
Parameters of Optical Flow algorithm.
const DATA_TYPE4 bilinear_interpolate(const Image *in, const float8 coords, const float width, const float height)
Computes the bilinear interpolation for each set of coordinates in the vector coords and returns the ...