commander: mag_calibration move to Magnetometer calibration helper

src/modules/commander/calibration_routines.cpp

@@ -62,225 +62,23 @@
 
 using namespace time_literals;
 
-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;
-}
-
-int ellipsoid_fit_least_squares(const float x[], const float y[], const float z[],
-				unsigned int size, int max_iterations, float *offset_x, float *offset_y, float *offset_z,
-				float *sphere_radius, float *diag_x, float *diag_y, float *diag_z,
-				float *offdiag_x, float *offdiag_y, float *offdiag_z, bool sphere_fit_only)
-{
-	float _fitness = 1.0e30f, _sphere_lambda = 1.0f, _ellipsoid_lambda = 1.0f;
-
-	for (int i = 0; i < max_iterations; i++) {
-		run_lm_sphere_fit(x, y, z, _fitness, _sphere_lambda,
-				  size, offset_x, offset_y, offset_z,
-				  sphere_radius, diag_x, diag_y, diag_z, offdiag_x, offdiag_y, offdiag_z);
-
-	}
-
-	if (!sphere_fit_only) {
-		_fitness = 1.0e30f;
-
-		for (int i = 0; i < max_iterations; i++) {
-			run_lm_ellipsoid_fit(x, y, z, _fitness, _ellipsoid_lambda,
-					     size, offset_x, offset_y, offset_z,
-					     sphere_radius, diag_x, diag_y, diag_z, offdiag_x, offdiag_y, offdiag_z);
-		}
-	}
-
-	return 0;
-}
-
 int run_lm_sphere_fit(const float x[], const float y[], const float z[], float &_fitness, float &_sphere_lambda,
-		      unsigned int size, float *offset_x, float *offset_y, float *offset_z,
+		      unsigned int samples_collected, float *offset_x, float *offset_y, float *offset_z,
 		      float *sphere_radius, float *diag_x, float *diag_y, float *diag_z, float *offdiag_x, float *offdiag_y, float *offdiag_z)
 {
-	//Run Sphere Fit using Levenberg Marquardt LSq Fit
-	const float lma_damping = 10.0f;
-	float _samples_collected = size;
+	// Run Sphere Fit using Levenberg Marquardt LSq Fit
+	const float lma_damping = 10.f;
 	float fitness = _fitness;
-	float fit1 = 0.0f, fit2 = 0.0f;
+	float fit1 = 0.f;
+	float fit2 = 0.f;
 
-	matrix::SquareMatrix<float, 4> JTJ;
-	matrix::SquareMatrix<float, 4> JTJ2;
-	float JTFI[4] = {};
+	matrix::SquareMatrix<float, 4> JTJ{};
+	matrix::SquareMatrix<float, 4> JTJ2{};
+	float JTFI[4] {};
 	float residual = 0.0f;
 
 	// Gauss Newton Part common for all kind of extensions including LM
-	for (uint16_t k = 0; k < _samples_collected; k++) {
+	for (uint16_t k = 0; k < samples_collected; k++) {
 
 		float sphere_jacob[4];
 		//Calculate Jacobian
@@ -310,7 +108,7 @@ int run_lm_sphere_fit(const float x[], const float y[], const float z[], float &
 
 
 	//------------------------Levenberg-Marquardt-part-starts-here---------------------------------//
-	//refer: http://en.wikipedia.org/wiki/Levenberg%E2%80%93Marquardt_algorithm#Choice_of_damping_parameter
+	// refer: http://en.wikipedia.org/wiki/Levenberg%E2%80%93Marquardt_algorithm#Choice_of_damping_parameter
 	float fit1_params[4] = {*sphere_radius, *offset_x, *offset_y, *offset_z};
 	float fit2_params[4];
 	memcpy(fit2_params, fit1_params, sizeof(fit1_params));
@@ -335,8 +133,8 @@ int run_lm_sphere_fit(const float x[], const float y[], const float z[], float &
 		}
 	}
 
-	//Calculate mean squared residuals
-	for (uint16_t k = 0; k < _samples_collected; k++) {
+	// Calculate mean squared residuals
+	for (uint16_t k = 0; k < samples_collected; k++) {
 		float A = (*diag_x    * (x[k] - fit1_params[1])) + (*offdiag_x * (y[k] - fit1_params[2])) + (*offdiag_y *
 				(z[k] + fit1_params[3]));
 		float B = (*offdiag_x * (x[k] - fit1_params[1])) + (*diag_y    * (y[k] - fit1_params[2])) + (*offdiag_z *
@@ -358,8 +156,8 @@ int run_lm_sphere_fit(const float x[], const float y[], const float z[], float &
 		fit2 += residual * residual;
 	}
 
-	fit1 = sqrtf(fit1) / _samples_collected;
-	fit2 = sqrtf(fit2) / _samples_collected;
+	fit1 = sqrtf(fit1) / samples_collected;
+	fit2 = sqrtf(fit2) / samples_collected;
 
 	if (fit1 > _fitness && fit2 > _fitness) {
 		_sphere_lambda *= lma_damping;
@@ -389,26 +187,25 @@ int run_lm_sphere_fit(const float x[], const float y[], const float z[], float &
 }
 
 int run_lm_ellipsoid_fit(const float x[], const float y[], const float z[], float &_fitness, float &_sphere_lambda,
-			 unsigned int size, float *offset_x, float *offset_y, float *offset_z,
+			 unsigned int samples_collected, float *offset_x, float *offset_y, float *offset_z,
 			 float *sphere_radius, float *diag_x, float *diag_y, float *diag_z, float *offdiag_x, float *offdiag_y, float *offdiag_z)
 {
-	//Run Sphere Fit using Levenberg Marquardt LSq Fit
+	// Run Sphere Fit using Levenberg Marquardt LSq Fit
 	const float lma_damping = 10.0f;
-	float _samples_collected = size;
 	float fitness = _fitness;
 	float fit1 = 0.0f;
 	float fit2 = 0.0f;
 
-	float JTJ[81] = {};
-	float JTJ2[81] = {};
-	float JTFI[9] = {};
+	float JTJ[81] {};
+	float JTJ2[81] {};
+	float JTFI[9] {};
 	float residual = 0.0f;
 	float ellipsoid_jacob[9];
 
 	// Gauss Newton Part common for all kind of extensions including LM
-	for (uint16_t k = 0; k < _samples_collected; k++) {
+	for (uint16_t k = 0; k < samples_collected; k++) {
 
-		//Calculate Jacobian
+		// Calculate Jacobian
 		float A = (*diag_x    * (x[k] - *offset_x)) + (*offdiag_x * (y[k] - *offset_y)) + (*offdiag_y * (z[k] - *offset_z));
 		float B = (*offdiag_x * (x[k] - *offset_x)) + (*diag_y    * (y[k] - *offset_y)) + (*offdiag_z * (z[k] - *offset_z));
 		float C = (*offdiag_y * (x[k] - *offset_x)) + (*offdiag_z * (y[k] - *offset_y)) + (*diag_z    * (z[k] - *offset_z));
@@ -432,7 +229,7 @@ int run_lm_ellipsoid_fit(const float x[], const float y[], const float z[], floa
 			// compute JTJ
 			for (uint8_t j = 0; j < 9; j++) {
 				JTJ[i * 9 + j] += ellipsoid_jacob[i] * ellipsoid_jacob[j];
-				JTJ2[i * 9 + j] += ellipsoid_jacob[i] * ellipsoid_jacob[j]; //a backup JTJ for LM
+				JTJ2[i * 9 + j] += ellipsoid_jacob[i] * ellipsoid_jacob[j]; // a backup JTJ for LM
 			}
 
 			JTFI[i] += ellipsoid_jacob[i] * residual;
@@ -441,7 +238,7 @@ int run_lm_ellipsoid_fit(const float x[], const float y[], const float z[], floa
 
 
 	//------------------------Levenberg-Marquardt-part-starts-here---------------------------------//
-	//refer: http://en.wikipedia.org/wiki/Levenberg%E2%80%93Marquardt_algorithm#Choice_of_damping_parameter
+	// refer: http://en.wikipedia.org/wiki/Levenberg%E2%80%93Marquardt_algorithm#Choice_of_damping_parameter
 	float fit1_params[9] = {*offset_x, *offset_y, *offset_z, *diag_x, *diag_y, *diag_z, *offdiag_x, *offdiag_y, *offdiag_z};
 	float fit2_params[9];
 	memcpy(fit2_params, fit1_params, sizeof(fit1_params));
@@ -467,8 +264,8 @@ int run_lm_ellipsoid_fit(const float x[], const float y[], const float z[], floa
 		}
 	}
 
-	//Calculate mean squared residuals
-	for (uint16_t k = 0; k < _samples_collected; k++) {
+	// Calculate mean squared residuals
+	for (uint16_t k = 0; k < samples_collected; k++) {
 		float A = (fit1_params[3]    * (x[k] - fit1_params[0])) + (fit1_params[6] * (y[k] - fit1_params[1])) + (fit1_params[7] *
 				(z[k] - fit1_params[2]));
 		float B = (fit1_params[6] * (x[k] - fit1_params[0])) + (fit1_params[4]   * (y[k] - fit1_params[1])) + (fit1_params[8] *
@@ -490,8 +287,8 @@ int run_lm_ellipsoid_fit(const float x[], const float y[], const float z[], floa
 		fit2 += residual * residual;
 	}
 
-	fit1 = sqrtf(fit1) / _samples_collected;
-	fit2 = sqrtf(fit2) / _samples_collected;
+	fit1 = sqrtf(fit1) / samples_collected;
+	fit2 = sqrtf(fit2) / samples_collected;
 
 	if (fit1 > _fitness && fit2 > _fitness) {
 		_sphere_lambda *= lma_damping;

src/modules/commander/calibration_routines.h

@@ -48,7 +48,6 @@
  * @param z point coordinates on the Z axis
  * @param size number of points
  * @param max_iterations abort if maximum number of iterations have been reached. If unsure, set to 100.
- * @param delta abort if error is below delta. If unsure, set to 0 to run max_iterations times.
  * @param sphere_x coordinate of the sphere center on the X axis
  * @param sphere_y coordinate of the sphere center on the Y axis
  * @param sphere_z coordinate of the sphere center on the Z axis
@@ -56,23 +55,19 @@
  *
  * @return 0 on success, 1 on failure
  */
-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);
-int ellipsoid_fit_least_squares(const float x[], const float y[], const float z[],
-				unsigned int size, int max_iterations, float *offset_x, float *offset_y, float *offset_z,
-				float *sphere_radius, float *diag_x, float *diag_y, float *diag_z, float *offdiag_x, float *offdiag_y,
-				float *offdiag_z, bool sphere_fit_only);
-int run_lm_sphere_fit(const float x[], const float y[], const float z[], float &_fitness, float &_sphere_lambda,
-		      unsigned int size, float *offset_x, float *offset_y, float *offset_z,
-		      float *sphere_radius, float *diag_x, float *diag_y, float *diag_z, float *offdiag_x, float *offdiag_y,
-		      float *offdiag_z);
-int run_lm_ellipsoid_fit(const float x[], const float y[], const float z[], float &_fitness, float &_sphere_lambda,
-			 unsigned int size, float *offset_x, float *offset_y, float *offset_z,
-			 float *sphere_radius, float *diag_x, float *diag_y, float *diag_z, float *offdiag_x, float *offdiag_y,
-			 float *offdiag_z);
-bool inverse4x4(float m[], float invOut[]);
-bool mat_inverse(float *A, float *inv, uint8_t n);
+int run_lm_sphere_fit(const float x[], const float y[], const float z[],
+		      float &fitness, float &sphere_lambda, unsigned int size,
+		      float *offset_x, float *offset_y, float *offset_z,
+		      float *sphere_radius,
+		      float *diag_x, float *diag_y, float *diag_z,
+		      float *offdiag_x, float *offdiag_y, float *offdiag_z);
+
+int run_lm_ellipsoid_fit(const float x[], const float y[], const float z[],
+			 float &fitness, float &sphere_lambda, unsigned int size,
+			 float *offset_x, float *offset_y, float *offset_z,
+			 float *sphere_radius,
+			 float *diag_x, float *diag_y, float *diag_z,
+			 float *offdiag_x, float *offdiag_y, float *offdiag_z);
 
 // The order of these cannot change since the calibration calculations depend on them in this order
 enum detect_orientation_return {