110 float *parameters_u,
float *parameters_v,
float *fit_error,
float *min_error_u,
float *min_error_v,
int *n_inliers_u,
120 int sam,
p, i_rand, si, add_si;
127 n_iterations = (n_iterations < 1) ? 1 : n_iterations;
133 float _bu_all[count][1];
135 float _bv_all[count][1];
137 for (sam = 0; sam < count; sam++) {
138 AA[sam][0] = (float) vectors[sam].pos.x;
139 AA[sam][1] = (float) vectors[sam].pos.y;
141 bu_all[sam][0] = (float) vectors[sam].flow_x;
142 bv_all[sam][0] = (float) vectors[sam].flow_y;
170 float PU[n_iterations * 3];
171 float PV[n_iterations * 3];
172 float errors_pu[n_iterations];
174 float errors_pv[n_iterations];
176 int n_inliers_pu[n_iterations];
177 int n_inliers_pv[n_iterations];
179 for (it = 0; it < n_iterations; it++) {
182 n_inliers_pu[it] = 0;
183 n_inliers_pv[it] = 0;
185 for (it = 0; it < n_iterations; it++) {
194 for (ii = 0; ii < i_rand; ii++) {
195 if (sample_indices[ii] == si) { add_si = 0; }
198 sample_indices[i_rand] = si;
205 A[sam][0] = (float) vectors[sample_indices[sam]].pos.x;
206 A[sam][1] = (float) vectors[sample_indices[sam]].pos.y;
208 bu[sam][0] = (float) vectors[sample_indices[sam]].flow_x;
209 bv[sam][0] = (float) vectors[sample_indices[sam]].flow_y;
220 PU[it * 3] = pu[0][0];
221 PU[it * 3 + 1] = pu[1][0];
222 PU[it * 3 + 2] = pu[2][0];
226 PV[it * 3] = pv[0][0];
227 PV[it * 3 + 1] = pv[1][0];
228 PV[it * 3 + 2] = pv[2][0];
233 n_inliers_pu[it] = 0;
234 n_inliers_pv[it] = 0;
238 MAT_MUL(count, 3, 1, bb, AA, pu);
240 MAT_SUB(count, 1, C, bb, bu_all);
242 for (
p = 0;
p < count;
p++) {
243 C[
p][0] = fabsf(C[
p][0]);
244 if (C[
p][0] < error_threshold) {
245 errors_pu[it] += C[
p][0];
248 errors_pu[it] += error_threshold;
254 MAT_MUL(count, 3, 1, bb, AA, pv);
256 MAT_SUB(count, 1, C, bb, bv_all);
258 for (
p = 0;
p < count;
p++) {
259 C[
p][0] = fabsf(C[
p][0]);
260 if (C[
p][0] < error_threshold) {
261 errors_pv[it] += C[
p][0];
264 errors_pv[it] += error_threshold;
274 *min_error_u = (float)errors_pu[0];
275 for (it = 1; it < n_iterations; it++) {
276 if (errors_pu[it] < *min_error_u) {
277 *min_error_u = (float)errors_pu[it];
281 for (param = 0; param < 3; param++) {
282 parameters_u[param] = PU[min_ind * 3 + param];
284 *n_inliers_u = n_inliers_pu[min_ind];
288 *min_error_v = (float)errors_pv[0];
289 for (it = 0; it < n_iterations; it++) {
290 if (errors_pv[it] < *min_error_v) {
291 *min_error_v = (float)errors_pv[it];
295 for (param = 0; param < 3; param++) {
296 parameters_v[param] = PV[min_ind * 3 + param];
298 *n_inliers_v = n_inliers_pv[min_ind];
302 MAT_MUL(count, 3, 1, bb, AA, pu);
304 MAT_SUB(count, 1, C, bb, bu_all);
306 for (
p = 0;
p < count;
p++) {
307 *min_error_u += fabsf(C[
p][0]);
310 MAT_MUL(count, 3, 1, bb, AA, pv);
312 MAT_SUB(count, 1, C, bb, bv_all);
314 for (
p = 0;
p < count;
p++) {
315 *min_error_v += fabsf(C[
p][0]);
317 *fit_error = (*min_error_u + *min_error_v) / (2 * count);
344 (im_height / 2.0f) * parameters_u[1]);
346 (im_height / 2.0f) * parameters_v[1]);
352 float threshold_slope = 1.0;
355 if (fabsf(parameters_v[1]) < eta && arv_y < threshold_slope && arv_x >= 2 * threshold_slope) {
358 }
else if (arv_y >= 2 * threshold_slope) {
367 if (fabsf(parameters_u[0]) < eta && arv_x < threshold_slope && arv_y >= 2 * threshold_slope) {
370 }
else if (arv_x >= 2 * threshold_slope) {
383 float denominator = parameters_v[0] * parameters_u[1] - parameters_u[0] * parameters_v[1];
384 if (fabsf(denominator) > 1
E-5) {
385 info->
focus_of_expansion_x = ((parameters_u[2] * parameters_v[1] - parameters_v[2] * parameters_u[1]) / denominator);
388 denominator = parameters_u[1];
389 if (fabsf(denominator) > 1
E-5) {
void fit_linear_flow_field(struct flow_t *vectors, int count, float error_threshold, int n_iterations, int n_samples, float *parameters_u, float *parameters_v, float *fit_error, float *min_error_u, float *min_error_v, int *n_inliers_u, int *n_inliers_v)
Analyze a linear flow field, retrieving information such as divergence, surface roughness,...
bool analyze_flow_field(struct flow_t *vectors, int count, float error_threshold, int n_iterations, int n_samples, int im_width, int im_height, float focal_length, struct linear_flow_fit_info *info)
Analyze a flow field, retrieving information on relative velocities and rotations,...
bool analyze_linear_flow_field(struct flow_t *vectors, int count, float error_threshold, int n_iterations, int n_samples, int im_width, int im_height, struct linear_flow_fit_info *info)
Analyze a linear flow field, retrieving information such as divergence, surface roughness,...