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>
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)
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.
Terminate on whichever of the other conditions occurs first.
This file contains all available output stages for GEMMLowp on OpenCL.
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.
SimpleTensor< T > scale(const SimpleTensor< T > &in, float scale_x, float scale_y, InterpolationPolicy policy, BorderMode border_mode, T constant_border_value, SamplingPolicy sampling_policy, bool ceil_policy_scale)
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.
fixed_point< T > pow(fixed_point< T > x, fixed_point< T > a)
Container for valid region of a window.
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 ...