/** Polynomial regression * * Polynomial regression is a form of linear regression in which the relationship between * the independent variable x and the dependent variable y is modelled as an nth order polynomial. * * Considering the regression model: * @f[ * y_i = c_0 + c_1 x_i + ... + c_p x_i^p + \epsilon_i (i = 1 ... n) * @f] * in matrix form * @f[ * y = X.c + \epsilon * @f] * where * @f[ * X_{ij} = x_i^j (i = 1 ... n; j = 1 ... p) * @f] * The vector of estimated polynomial regression coefficients using ordinary least squares estimation is * @f[ * c = (X' X)^{-1} X' y * @f] * * http://en.wikipedia.org/wiki/Polynomial_regression * http://fr.wikipedia.org/wiki/R%C3%A9gression_polynomiale * http://www.arachnoid.com/sage/polynomial.html * * @param[in] x pointer to the input array of independent variable X [n] * @param[in] y pointer to the input array of dependent variable Y [n] * @param[in] n number of input measurments * @param[in] p degree of the output polynomial * @param[out] c pointer to the output array of polynomial coefficients [p+1] */ void pprz_polyfit_float(float* x, float* y, int n, int p, float* c) { int i,j,k; // Instead of solving directly (X'X)^-1 X' y // let's build the matrices (X'X) and (X'y) // Then element ij in (X'X) matrix is sum_{k=0,n-1} x_k^(i+j) // and element i in (X'y) vector is sum_{k=0,n-1} x_k^i * y_k // Finally we can solve the linear system (X'X).c = (X'y) using SVD decomposition // First build a table of element S_i = sum_{k=0,n-1} x_k^i of dimension 2*p+1 float S[2*p + 1]; float_vect_zero(S, 2*p + 1); // and a table of element T_i = sum_{k=0,n-1} x_k^i*y_k of dimension p+1 // make it a matrix for later use float _T[p + 1][1]; MAKE_MATRIX_PTR(T, _T, p + 1); float_mat_zero(T, p + 1, 1); S[0] = n; // S_0 is always the number of input measurements for (k = 0; k < n; k++) { float x_tmp = x[k]; T[0][0] += y[k]; for (i = 1; i < 2*p + 1; i++) { S[i] += x_tmp; // add element to S_i if (i < p + 1) T[i][0] += x_tmp*y[k]; // add element to T_i if i < p+1 x_tmp *= x[k]; // multiply x_tmp by current value of x } } // Then build a [p+1 x p+1] matrix corresponding to (X'X) based on the S_i // element ij of (X'X) is S_(i+j) float _XtX[p + 1][p + 1]; MAKE_MATRIX_PTR(XtX, _XtX, p + 1); for (i = 0; i < p + 1; i++) { for (j = 0; j < p + 1; j++) { XtX[i][j] = S[i+j]; } } // Solve linear system XtX.c = T after performing a SVD decomposition of XtX // which is probably a bit overkill but looks really cool float w[p + 1], _v[p + 1][p + 1]; MAKE_MATRIX_PTR(v, _v, p + 1); pprz_svd_float(XtX, w, v, p + 1, p + 1); float _c[p + 1][1]; MAKE_MATRIX_PTR(c_tmp, _c, p + 1); pprz_svd_solve_float(c_tmp, XtX, w, v, T, p + 1, p + 1, 1); // set output vector for (i = 0; i < p + 1; i++) c[i] = c_tmp[i][0]; }
/** * Fit a linear model from samples to target values. * Effectively a wrapper for the pprz_svd_float and pprz_svd_solve_float functions. * * @param[in] targets The target values * @param[in] samples The samples / feature vectors * @param[in] D The dimensionality of the samples * @param[in] count The number of samples * @param[in] use_bias Whether to use the bias. Please note that params should always be of size D+1, but in case of no bias, the bias value is set to 0. * @param[out] parameters* Parameters of the linear fit * @param[out] fit_error* Total error of the fit */ void fit_linear_model(float *targets, int D, float (*samples)[D], uint16_t count, bool use_bias, float *params, float *fit_error) { // We will solve systems of the form A x = b, // where A = [nx(D+1)] matrix with entries [s1, ..., sD, 1] for each sample (1 is the bias) // and b = [nx1] vector with the target values. // x in the system are the parameters for the linear regression function. // local vars for iterating, random numbers: int sam, d; uint16_t n_samples = count; uint8_t D_1 = D + 1; // ensure that n_samples is high enough to ensure a result for a single fit: n_samples = (n_samples < D_1) ? D_1 : n_samples; // n_samples should not be higher than count: n_samples = (n_samples < count) ? n_samples : count; // initialize matrices and vectors for the full point set problem: // this is used for determining inliers float _AA[count][D_1]; MAKE_MATRIX_PTR(AA, _AA, count); float _targets_all[count][1]; MAKE_MATRIX_PTR(targets_all, _targets_all, count); for (sam = 0; sam < count; sam++) { for (d = 0; d < D; d++) { AA[sam][d] = samples[sam][d]; } if (use_bias) { AA[sam][D] = 1.0f; } else { AA[sam][D] = 0.0f; } targets_all[sam][0] = targets[sam]; } // decompose A in u, w, v with singular value decomposition A = u * w * vT. // u replaces A as output: float _parameters[D_1][1]; MAKE_MATRIX_PTR(parameters, _parameters, D_1); float w[n_samples], _v[D_1][D_1]; MAKE_MATRIX_PTR(v, _v, D_1); // solve the system: pprz_svd_float(AA, w, v, count, D_1); pprz_svd_solve_float(parameters, AA, w, v, targets_all, count, D_1, 1); // used to determine the error of a set of parameters on the whole set: float _bb[count][1]; MAKE_MATRIX_PTR(bb, _bb, count); float _C[count][1]; MAKE_MATRIX_PTR(C, _C, count); // error is determined on the entire set // bb = AA * parameters: MAT_MUL(count, D_1, 1, bb, AA, parameters); // subtract bu_all: C = 0 in case of perfect fit: MAT_SUB(count, 1, C, bb, targets_all); *fit_error = 0; for (sam = 0; sam < count; sam++) { *fit_error += fabsf(C[sam][0]); } *fit_error /= count; for (d = 0; d < D_1; d++) { params[d] = parameters[d][0]; } }
/** * Analyze a linear flow field, retrieving information such as divergence, surface roughness, focus of expansion, etc. * @param[in] vectors The optical flow vectors * @param[in] count The number of optical flow vectors * @param[in] error_threshold Error used to determine inliers / outliers. * @param[in] n_iterations Number of RANSAC iterations. * @param[in] n_samples Number of samples used for a single fit (min. 3). * @param[out] parameters_u* Parameters of the horizontal flow field * @param[out] parameters_v* Parameters of the vertical flow field * @param[out] fit_error* Total error of the finally selected fit * @param[out] min_error_u* Error fit horizontal flow field * @param[out] min_error_v* Error fit vertical flow field * @param[out] n_inliers_u* Number of inliers in the horizontal flow fit. * @param[out] n_inliers_v* Number of inliers in the vertical flow fit. */ 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) { // We will solve systems of the form A x = b, // where A = [nx3] matrix with entries [x, y, 1] for each optic flow location // and b = [nx1] vector with either the horizontal (bu) or vertical (bv) flow. // x in the system are the parameters for the horizontal (pu) or vertical (pv) flow field. // local vars for iterating, random numbers: int sam, p, i_rand, si, add_si; // ensure that n_samples is high enough to ensure a result for a single fit: n_samples = (n_samples < MIN_SAMPLES_FIT) ? MIN_SAMPLES_FIT : n_samples; // n_samples should not be higher than count: n_samples = (n_samples < count) ? n_samples : count; // initialize matrices and vectors for the full point set problem: // this is used for determining inliers float _AA[count][3]; MAKE_MATRIX_PTR(AA, _AA, count); float _bu_all[count][1]; MAKE_MATRIX_PTR(bu_all, _bu_all, count); float _bv_all[count][1]; MAKE_MATRIX_PTR(bv_all, _bv_all, count); for (sam = 0; sam < count; sam++) { AA[sam][0] = (float) vectors[sam].pos.x; AA[sam][1] = (float) vectors[sam].pos.y; AA[sam][2] = 1.0f; bu_all[sam][0] = (float) vectors[sam].flow_x; bv_all[sam][0] = (float) vectors[sam].flow_y; } // later used to determine the error of a set of parameters on the whole set: float _bb[count][1]; MAKE_MATRIX_PTR(bb, _bb, count); float _C[count][1]; MAKE_MATRIX_PTR(C, _C, count); // *************** // perform RANSAC: // *************** // set up variables for small linear system solved repeatedly inside RANSAC: float _A[n_samples][3]; MAKE_MATRIX_PTR(A, _A, n_samples); float _bu[n_samples][1]; MAKE_MATRIX_PTR(bu, _bu, n_samples); float _bv[n_samples][1]; MAKE_MATRIX_PTR(bv, _bv, n_samples); float w[n_samples], _v[3][3]; MAKE_MATRIX_PTR(v, _v, 3); float _pu[3][1]; MAKE_MATRIX_PTR(pu, _pu, 3); float _pv[3][1]; MAKE_MATRIX_PTR(pv, _pv, 3); // iterate and store parameters, errors, inliers per fit: float PU[n_iterations * 3]; float PV[n_iterations * 3]; float errors_pu[n_iterations]; errors_pu[0] = 0.0; float errors_pv[n_iterations]; errors_pv[0] = 0.0; int n_inliers_pu[n_iterations]; int n_inliers_pv[n_iterations]; int it, ii; for (it = 0; it < n_iterations; it++) { // select a random sample of n_sample points: int sample_indices[n_samples]; i_rand = 0; // sampling without replacement: while (i_rand < n_samples) { si = rand() % count; add_si = 1; for (ii = 0; ii < i_rand; ii++) { if (sample_indices[ii] == si) { add_si = 0; } } if (add_si) { sample_indices[i_rand] = si; i_rand ++; } } // Setup the system: for (sam = 0; sam < n_samples; sam++) { A[sam][0] = (float) vectors[sample_indices[sam]].pos.x; A[sam][1] = (float) vectors[sample_indices[sam]].pos.y; A[sam][2] = 1.0f; bu[sam][0] = (float) vectors[sample_indices[sam]].flow_x; bv[sam][0] = (float) vectors[sample_indices[sam]].flow_y; //printf("%d,%d,%d,%d,%d\n",A[sam][0],A[sam][1],A[sam][2],bu[sam][0],bv[sam][0]); } // Solve the small system: // for horizontal flow: // decompose A in u, w, v with singular value decomposition A = u * w * vT. // u replaces A as output: pprz_svd_float(A, w, v, n_samples, 3); pprz_svd_solve_float(pu, A, w, v, bu, n_samples, 3, 1); PU[it * 3] = pu[0][0]; PU[it * 3 + 1] = pu[1][0]; PU[it * 3 + 2] = pu[2][0]; // for vertical flow: pprz_svd_solve_float(pv, A, w, v, bv, n_samples, 3, 1); PV[it * 3] = pv[0][0]; PV[it * 3 + 1] = pv[1][0]; PV[it * 3 + 2] = pv[2][0]; // count inliers and determine their error on all points: errors_pu[it] = 0; errors_pv[it] = 0; n_inliers_pu[it] = 0; n_inliers_pv[it] = 0; // for horizontal flow: // bb = AA * pu: MAT_MUL(count, 3, 1, bb, AA, pu); // subtract bu_all: C = 0 in case of perfect fit: MAT_SUB(count, 1, C, bb, bu_all); for (p = 0; p < count; p++) { C[p][0] = abs(C[p][0]); if (C[p][0] < error_threshold) { errors_pu[it] += C[p][0]; n_inliers_pu[it]++; } else { errors_pu[it] += error_threshold; } } // for vertical flow: // bb = AA * pv: MAT_MUL(count, 3, 1, bb, AA, pv); // subtract bv_all: C = 0 in case of perfect fit: MAT_SUB(count, 1, C, bb, bv_all); for (p = 0; p < count; p++) { C[p][0] = abs(C[p][0]); if (C[p][0] < error_threshold) { errors_pv[it] += C[p][0]; n_inliers_pv[it]++; } else { errors_pv[it] += error_threshold; } } } // After all iterations: // select the parameters with lowest error: // for horizontal flow: int param; int min_ind = 0; *min_error_u = (float)errors_pu[0]; for (it = 1; it < n_iterations; it++) { if (errors_pu[it] < *min_error_u) { *min_error_u = (float)errors_pu[it]; min_ind = it; } } for (param = 0; param < 3; param++) { parameters_u[param] = PU[min_ind * 3 + param]; } *n_inliers_u = n_inliers_pu[min_ind]; // for vertical flow: min_ind = 0; *min_error_v = (float)errors_pv[0]; for (it = 0; it < n_iterations; it++) { if (errors_pv[it] < *min_error_v) { *min_error_v = (float)errors_pv[it]; min_ind = it; } } for (param = 0; param < 3; param++) { parameters_v[param] = PV[min_ind * 3 + param]; } *n_inliers_v = n_inliers_pv[min_ind]; // error has to be determined on the entire set without threshold: // bb = AA * pu: MAT_MUL(count, 3, 1, bb, AA, pu); // subtract bu_all: C = 0 in case of perfect fit: MAT_SUB(count, 1, C, bb, bu_all); *min_error_u = 0; for (p = 0; p < count; p++) { *min_error_u += abs(C[p][0]); } // bb = AA * pv: MAT_MUL(count, 3, 1, bb, AA, pv); // subtract bv_all: C = 0 in case of perfect fit: MAT_SUB(count, 1, C, bb, bv_all); *min_error_v = 0; for (p = 0; p < count; p++) { *min_error_v += abs(C[p][0]); } *fit_error = (*min_error_u + *min_error_v) / (2 * count); }