50 lines
1.3 KiB
C++
50 lines
1.3 KiB
C++
#include "fgc/Calibration.h"
|
|
|
|
#include <cmath>
|
|
|
|
namespace fgc {
|
|
|
|
LinearFit linearFit(const std::vector<std::pair<double, double>>& xy) {
|
|
LinearFit f;
|
|
f.n = static_cast<int>(xy.size());
|
|
if (xy.size() < 2) return f;
|
|
|
|
double sx = 0, sy = 0;
|
|
for (const auto& p : xy) { sx += p.first; sy += p.second; }
|
|
const double mx = sx / xy.size(), my = sy / xy.size();
|
|
|
|
double sxx = 0, sxy = 0, syy = 0;
|
|
for (const auto& p : xy) {
|
|
const double dx = p.first - mx, dy = p.second - my;
|
|
sxx += dx * dx;
|
|
sxy += dx * dy;
|
|
syy += dy * dy;
|
|
}
|
|
if (sxx <= 0.0) return f; // no spread in x -> undefined slope
|
|
|
|
f.slope = sxy / sxx;
|
|
f.intercept = my - f.slope * mx;
|
|
f.r2 = (syy > 0.0) ? (sxy * sxy) / (sxx * syy) : 1.0;
|
|
f.ok = true;
|
|
return f;
|
|
}
|
|
|
|
double circularMeanDeg(const std::vector<double>& degs) {
|
|
if (degs.empty()) return 0.0;
|
|
double sc = 0, ss = 0;
|
|
for (double d : degs) {
|
|
const double r = d * M_PI / 180.0;
|
|
sc += std::cos(r);
|
|
ss += std::sin(r);
|
|
}
|
|
return std::atan2(ss, sc) * 180.0 / M_PI;
|
|
}
|
|
|
|
double unwrapNear(double prev, double deg) {
|
|
while (deg - prev > 180.0) deg -= 360.0;
|
|
while (deg - prev < -180.0) deg += 360.0;
|
|
return deg;
|
|
}
|
|
|
|
} // namespace fgc
|