/**************************************************************************** * * Copyright (c) 2012 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * 1. Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * 2. Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in * the documentation and/or other materials provided with the * distribution. * 3. Neither the name PX4 nor the names of its contributors may be * used to endorse or promote products derived from this software * without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * ****************************************************************************/ /** * @file calibration_routines.cpp * Calibration routines implementations. * * @author Lorenz Meier */ #include #include #include "calibration_routines.h" int sphere_fit_least_squares(const float x[], const float y[], const float z[], unsigned int size, unsigned int max_iterations, float delta, float *sphere_x, float *sphere_y, float *sphere_z, float *sphere_radius) { float x_sumplain = 0.0f; float x_sumsq = 0.0f; float x_sumcube = 0.0f; float y_sumplain = 0.0f; float y_sumsq = 0.0f; float y_sumcube = 0.0f; float z_sumplain = 0.0f; float z_sumsq = 0.0f; float z_sumcube = 0.0f; float xy_sum = 0.0f; float xz_sum = 0.0f; float yz_sum = 0.0f; float x2y_sum = 0.0f; float x2z_sum = 0.0f; float y2x_sum = 0.0f; float y2z_sum = 0.0f; float z2x_sum = 0.0f; float z2y_sum = 0.0f; for (unsigned int i = 0; i < size; i++) { float x2 = x[i] * x[i]; float y2 = y[i] * y[i]; float z2 = z[i] * z[i]; x_sumplain += x[i]; x_sumsq += x2; x_sumcube += x2 * x[i]; y_sumplain += y[i]; y_sumsq += y2; y_sumcube += y2 * y[i]; z_sumplain += z[i]; z_sumsq += z2; z_sumcube += z2 * z[i]; xy_sum += x[i] * y[i]; xz_sum += x[i] * z[i]; yz_sum += y[i] * z[i]; x2y_sum += x2 * y[i]; x2z_sum += x2 * z[i]; y2x_sum += y2 * x[i]; y2z_sum += y2 * z[i]; z2x_sum += z2 * x[i]; z2y_sum += z2 * y[i]; } // //Least Squares Fit a sphere A,B,C with radius squared Rsq to 3D data // // P is a structure that has been computed with the data earlier. // P.npoints is the number of elements; the length of X,Y,Z are identical. // P's members are logically named. // // X[n] is the x component of point n // Y[n] is the y component of point n // Z[n] is the z component of point n // // A is the x coordiante of the sphere // B is the y coordiante of the sphere // C is the z coordiante of the sphere // Rsq is the radius squared of the sphere. // //This method should converge; maybe 5-100 iterations or more. // float x_sum = x_sumplain / size; //sum( X[n] ) float x_sum2 = x_sumsq / size; //sum( X[n]^2 ) float x_sum3 = x_sumcube / size; //sum( X[n]^3 ) float y_sum = y_sumplain / size; //sum( Y[n] ) float y_sum2 = y_sumsq / size; //sum( Y[n]^2 ) float y_sum3 = y_sumcube / size; //sum( Y[n]^3 ) float z_sum = z_sumplain / size; //sum( Z[n] ) float z_sum2 = z_sumsq / size; //sum( Z[n]^2 ) float z_sum3 = z_sumcube / size; //sum( Z[n]^3 ) float XY = xy_sum / size; //sum( X[n] * Y[n] ) float XZ = xz_sum / size; //sum( X[n] * Z[n] ) float YZ = yz_sum / size; //sum( Y[n] * Z[n] ) float X2Y = x2y_sum / size; //sum( X[n]^2 * Y[n] ) float X2Z = x2z_sum / size; //sum( X[n]^2 * Z[n] ) float Y2X = y2x_sum / size; //sum( Y[n]^2 * X[n] ) float Y2Z = y2z_sum / size; //sum( Y[n]^2 * Z[n] ) float Z2X = z2x_sum / size; //sum( Z[n]^2 * X[n] ) float Z2Y = z2y_sum / size; //sum( Z[n]^2 * Y[n] ) //Reduction of multiplications float F0 = x_sum2 + y_sum2 + z_sum2; float F1 = 0.5f * F0; float F2 = -8.0f * (x_sum3 + Y2X + Z2X); float F3 = -8.0f * (X2Y + y_sum3 + Z2Y); float F4 = -8.0f * (X2Z + Y2Z + z_sum3); //Set initial conditions: float A = x_sum; float B = y_sum; float C = z_sum; //First iteration computation: float A2 = A * A; float B2 = B * B; float C2 = C * C; float QS = A2 + B2 + C2; float QB = -2.0f * (A * x_sum + B * y_sum + C * z_sum); //Set initial conditions: float Rsq = F0 + QB + QS; //First iteration computation: float Q0 = 0.5f * (QS - Rsq); float Q1 = F1 + Q0; float Q2 = 8.0f * (QS - Rsq + QB + F0); float aA, aB, aC, nA, nB, nC, dA, dB, dC; //Iterate N times, ignore stop condition. unsigned int n = 0; while (n < max_iterations) { n++; //Compute denominator: aA = Q2 + 16.0f * (A2 - 2.0f * A * x_sum + x_sum2); aB = Q2 + 16.0f * (B2 - 2.0f * B * y_sum + y_sum2); aC = Q2 + 16.0f * (C2 - 2.0f * C * z_sum + z_sum2); aA = (fabsf(aA) < FLT_EPSILON) ? 1.0f : aA; aB = (fabsf(aB) < FLT_EPSILON) ? 1.0f : aB; aC = (fabsf(aC) < FLT_EPSILON) ? 1.0f : aC; //Compute next iteration nA = A - ((F2 + 16.0f * (B * XY + C * XZ + x_sum * (-A2 - Q0) + A * (x_sum2 + Q1 - C * z_sum - B * y_sum))) / aA); nB = B - ((F3 + 16.0f * (A * XY + C * YZ + y_sum * (-B2 - Q0) + B * (y_sum2 + Q1 - A * x_sum - C * z_sum))) / aB); nC = C - ((F4 + 16.0f * (A * XZ + B * YZ + z_sum * (-C2 - Q0) + C * (z_sum2 + Q1 - A * x_sum - B * y_sum))) / aC); //Check for stop condition dA = (nA - A); dB = (nB - B); dC = (nC - C); if ((dA * dA + dB * dB + dC * dC) <= delta) { break; } //Compute next iteration's values A = nA; B = nB; C = nC; A2 = A * A; B2 = B * B; C2 = C * C; QS = A2 + B2 + C2; QB = -2.0f * (A * x_sum + B * y_sum + C * z_sum); Rsq = F0 + QB + QS; Q0 = 0.5f * (QS - Rsq); Q1 = F1 + Q0; Q2 = 8.0f * (QS - Rsq + QB + F0); } *sphere_x = A; *sphere_y = B; *sphere_z = C; *sphere_radius = sqrtf(Rsq); return 0; }