C++ Klassentemplate für Kreisbestimmungen aus Punktwolken (Circle Fitting)
C++ circle fit class template
Das hier beschriebene Klassentemplate errechnet die Parameter für einen Kreis (x, y, Radius, und ein paar Debuginformationen) aus einer 2D Punktwolke, entweder mit dem Hyperfit-Algorithmus (algebraisch) oder mittels Levenberg-Marquard (geometrisch). Bei letzerem wird muss der Kreis ungefähr bekannt sein, daher wird immer ein Hyperfit durchgeführt. Die Algorithmen sind beide recht robust und liefern auch für Kreisbögen gültige Werte. Die Klasse wurde implementiert, um die Koordinatengenauigkeit bei einer Kameramessung für eine Erkennungsmarken-Ausrichtung (fiducials) zu erhöhen.
Der Beispielquelltext zeigt, wie die Klasse verwendet wird.
The class template described here calculates the parameters of a circle (x, y, radius and some debug information) from a 2D point cloud. The used algorithms are Hyperfit (algebraic) and Levenberg-Marquard (geometric). Latter requires an initial guess, which is implicitly calculated using the Hyperfit function. Both algorithms are known to be quite robust. The class was implemented to improve the coordinate accuracy of the camera measurement for fiducial based alignment (machine position + USB microscope coordinates).
The example shows details about the usage.
Dateien
Files
Class template: circlefit.hh Example: circlefit.cc cli_args.hh csv_reader.hh
Beispiel
Example
Microtest
#include <vpu/circlefit.hh>
#include "test.hh"
#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif
template <typename T>
struct cpoints_t {
std::vector<T> x,y;
inline void clear() {
x.clear(); y.clear();
}
inline std::string to_string() {
if(x.size() != y.size()) throw "dump(): vector size missmatch.";
if(x.empty()) return "[]";
std::stringstream ss;
ss << "[[" << x[0] << "," << y[0] << "]";
for(unsigned i = 1; i < x.size(); ++i) {
ss << ",[" << x[i] << "," << y[i] << "]";
}
ss << "]";
return ss.str();
}
};
template <typename T>
cpoints_t<T> circle_points_nonoise(unsigned n_points, T r, T x, T y, T arc_deg)
{
cpoints_t<T> pts;
if(n_points < 3) throw "n_points < 3";
if(r <= std::numeric_limits<T>::epsilon()) throw "radius < eps";
if(arc_deg < (T)0) throw "arc_deg <= 0";
while(arc_deg > 0) arc_deg -= 360.0;
arc_deg += 360.0;
pts.x.reserve(n_points);
pts.y.reserve(n_points);
double sn, cs, scl = (double)M_PI/180.0 * arc_deg / n_points;
for(unsigned i=0; i < n_points; ++i) {
sincos(scl * i, &sn, &cs);
pts.x.push_back(x + sw::utest::round(r * cs, 1e-12));
pts.y.push_back(y + sw::utest::round(r * sn, 1e-12));
}
return pts;
}
void test()
{
{
test_comment("Const test: 360deg, r=10, 4 points");
cpoints_t<double> pts = circle_points_nonoise<double>(4, 10, 0, 0, 360);
test_comment(pts.to_string());
test_expect( pts.to_string() == "[[10,0],[0,10],[-10,0],[0,-10]]" );
sw::circle_fit::circle_t circle = sw::circle_fit::fit(pts.x, pts.y);
test_expect(circle.ok);
test_expect_in_tolerance(circle.r - 10, 1e-12);
test_expect_in_tolerance(circle.x - 0, 1e-12);
test_expect_in_tolerance(circle.y - 0, 1e-12);
circle = sw::circle_fit::fit_geometric(pts.x, pts.y);
test_expect(circle.ok);
test_expect_in_tolerance(circle.r - 10, 1e-12);
test_expect_in_tolerance(circle.x - 0, 1e-12);
test_expect_in_tolerance(circle.y - 0, 1e-12);
}
{
test_comment("Const test: 270deg, r=10, 3 points");
cpoints_t<double> pts = circle_points_nonoise<double>(3, 10, 0, 0, 270);
test_comment(pts.to_string());
test_expect( pts.to_string() == "[[10,0],[0,10],[-10,0]]" );
sw::circle_fit::circle_t circle = sw::circle_fit::fit(pts.x, pts.y);
test_expect(circle.ok);
test_expect_in_tolerance(circle.r - 10, 1e-12);
test_expect_in_tolerance(circle.x - 0, 1e-12);
test_expect_in_tolerance(circle.y - 0, 1e-12);
circle = sw::circle_fit::fit_geometric(pts.x, pts.y);
test_expect(circle.ok);
test_expect_in_tolerance(circle.r - 10, 1e-12);
test_expect_in_tolerance(circle.x - 0, 1e-12);
test_expect_in_tolerance(circle.y - 0, 1e-12);
}
}External test (wit Matlab/Octave)
#include <vpu/circlefit.hh>
#include <csv_reader.hh>
#include <cmath>
#include <cli_args.hh>
#include <string>
#include <sstream>
#include <iostream>
#include <iomanip>
#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif
using namespace std;
using namespace sw;
namespace sw { namespace math {
template <typename vt>
vt random(vt min, double max) throw()
{
static bool is_init = false;
if(!is_init) { ::srand(::time(0)); is_init=true; }
double mn = min, mx = max;
if(mn>mx) { double t=mn; mn=mx; mx=t; }
mn += ((mx-mn)/(double)RAND_MAX * ::rand());
return (vt)mn;
}
}}
// CSV reader for floating point numbers:
typedef sw::csv_reader<double> csv_t;
csv_t csv;
// Fit result circle
sw::circle_fit::circle_t circle;
/**
* Perform fit from csv data.
* @return bool
*/
bool fit()
{
bool ok = true;
unsigned n = csv.rows();
std::vector<double> x,y;
x.reserve(n); y.reserve(n);
for(unsigned i=0; i<n; ++i) {
x.push_back(csv.data()[i][0]);
y.push_back(csv.data()[i][1]);
}
if(!cli_args["g"]) {
// <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< ALGEBRAIC CIRCLE FIT CALL IS HERE
circle = sw::circle_fit::fit(x, y);
// >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
} else {
// <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< GEOMETRIC CIRCLE FIT CALL IS HERE
circle = sw::circle_fit::fit_geometric(x, y);
// >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
}
return ok;
}
/**
* Create test circle
*/
bool make_test_circle(std::string &result, double *result_x, double *result_y, double *result_r,
double *result_a0, double *result_a1, int *result_n, double min_x, double max_x, double min_y,
double max_y, double min_r, double max_r, double min_a, double max_a, double noise, int min_n,
int max_n)
{
double n0 = sw::math::random(min_n, max_n); // Number of circumference points
double x0 = sw::math::random(min_x, max_x); // Centre x
double y0 = sw::math::random(min_y, max_y); // Centre y
double r0 = sw::math::random(min_r, max_r); // Radius
double a0, a1; // Start / end angle
if(min_a > max_a) min_a = 0; // Input error override
if(max_a != 0) {
// Random arc
a0 = sw::math::random(0.0, 360.0);
a1 = sw::math::random(min_a, (max_a==0.0 ? 360.0 : max_a)) + a0;
for(int i=0; i<5 && a0 == a1; i++) {
a1 = a0 + sw::math::random((double)min_a, (double)(max_a==0 ? 360 : max_a));
}
// Arc modulo
if(a1 > 360.0) { a0 -= 360.0; a1-=360.0; }
} else {
a0 = 0;
a1 = 360;
}
// Resulting "input" circle description
if(result_x) *result_x = x0;
if(result_y) *result_y = y0;
if(result_r) *result_r = r0;
if(result_n) *result_n = n0;
if(result_a0) *result_a0 = a0;
if(result_a1) *result_a1 = a1;
// CSV header
std::stringstream ss;
ss << "# Test circle" << endl
<< "# cfg: x-min=" << min_x << ",x-max=" << max_x << endl
<< "# cfg: y-min=" << min_y << ",y-max=" << max_y << endl
<< "# cfg: r-min=" << min_r << ",r-max=" << max_r << endl
<< "# cfg: n-min=" << min_n << ",n-max=" << max_n << endl
<< "# cfg: r-noise=" << noise << endl
<< "#" << endl
<< "# x=" << x0 << endl
<< "# y=" << y0 << endl
<< "# r=" << r0 << endl
<< "# a=" << a0 << endl
<< "# b=" << a1 << endl
<< "# N=" << n0 << endl
<< "# arc=" << (a1-a0) << endl
<< "# " << endl
<< "x, y" << endl
;
// CSV data
a0 *= M_PI/180.0;
a1 *= M_PI/180.0;
double as = (a1-a0) / n0;
for(double ka = a0; ka < a1; ka += as) {
double a = ka;
double r = r0 * (1.0 + sw::math::random(-noise/2, noise/2));
double x = x0 + cos(a) * r;
double y = y0 + sin(a) * r;
ss << x << ", " << y << endl;
}
result = ss.str();
return true;
}
/**
* Generate test circle (wrapper - command line args)
*/
bool mk_test(std::string &result, double *x=0, double *y=0, double *r=0, double *a0=0,
double *a1=0, int *n=0, double *rnoise=0)
{
double min_x = (double) sw::cli_args["x-min"];
double max_x = (double) sw::cli_args["x-max"];
double min_y = (double) sw::cli_args["y-min"];
double max_y = (double) sw::cli_args["y-max"];
double min_r = (double) sw::cli_args["r-min"];
double max_r = (double) sw::cli_args["r-max"];
double min_a = (double) sw::cli_args["a-min"];
double max_a = (double) sw::cli_args["a-max"];
int min_n = (double) sw::cli_args["n-min"];
int max_n = (double) sw::cli_args["n-max"];
double max_noise = (double) sw::cli_args["noise-max"];
if(rnoise) *rnoise = max_noise;
make_test_circle(
result, x, y, r, a0, a1, n,
min_x, max_x, min_y, max_y, min_r, max_r,
min_a, max_a, max_noise, min_n, max_n
);
return true;
}
/**
* Main
*/
int main(int argc, char** argv)
{
cli_args(argc, argv) // Global CLI argument object.
("g", "geometric", "b", "" , "1" , "Do a Geometric fit (with algebraic initial guess)")
("i", "max-iterations", "i", "100" , "" , "Maximum number of fit iterations (algebraic and geometric fit)")
("l", "lambda", "n", "0.2" , "" , "Geometric fit: Initial lambda (\"scaled iteration step size\")")
("d", "down-factor", "n", "0.04" , "" , "Geometric fit: Lambda scaling for improving iteration "\
"(less deviation)")
("u", "up-factor", "n", "10" , "" , "Geometric fit: Lambda scaling for worse deviation than in "\
"the previous iteration")
("e", "epsilon", "n", "3.0e-8" , "" , "Geometric fit: Success break condition (aborts when "\
"position and radius differences with respect to the radius are smaller than epsilon)")
("L", "limit", "n", "1.0e6" , "" , "Geometric fit: Abort condition (aborts when x/y centre "\
"coordinates exceed this limit)")
("v", "verbose", "b", "" , "1" , "Add verbosity output to stdout")
("x", "x-min", "n", "-100.0" , "" , "Test: Minimum random X position")
("X", "x-max", "n", "100.0" , "" , "Test: Maximum random X position")
("y", "y-min", "n", "-100.0" , "" , "Test: Minimum random Y position")
("Y", "y-max", "n", "100.0" , "" , "Test: Maximum random Y position")
("r", "r-min", "n", "2.5" , "" , "Test: Minimum random radius")
("R", "r-max", "n", "100" , "" , "Test: Maximum random radius")
("a", "a-min", "n", "180" , "" , "Test: Minimum random arc in DEG")
("A", "a-max", "n", "360" , "" , "Test: Maximum random arc in DEG (0==full circle)")
("n", "n-min", "n", "10" , "" , "Test: Minimum random number of points")
("N", "n-max", "n", "1000", "" , "Test: Maximum random number of points")
("s", "noise-max", "n", "0.05", "" , "Test: Maximum point position noise")
;
cli_args
.allow_unknown_options(true)
.parse()
.handle_help_and_errors(
"Circle algorithm test.\n\n"
" circlefit [opts] <FILE> : Fit a CSV file point cloud.\n"
" circlefit [opts] - : Fit a CSV from stdin.\n"
" circlefit [opts] test : Make a random circle fit test.\n"
" circlefit [opts] mktest : Generate CSV data to stdout to double check\n"
" with other programs.\n",
"v1.0, stfwi, 2011",
"[mktest | test | <csv file path> | - ] ",
" 0: successful\n>0: error", 1, cerr
);
// First positional argument is the test command or the file path
string file;
if(cli_args.positional_arguments().size()) {
file = cli_args.positional_arguments().front().value();
if(file == "-") {
file = "/dev/stdin";
}
}
if(file.empty()) {
cerr << "No file or test command given (use '-' for stdin)" << endl;
return 1;
} else if(file == "mktest") {
// Generate a random CSV x/y data table
std::string s;
if(!mk_test(s)) {
cerr << "Failed to create test data." << endl;
return 1;
}
cout << s << endl;
return 0;
} else if(file == "test") {
// This is a simple test where we generate random data, fit it, and check the
// differences between the original circle data and the fitted one.
std::string csv_txt;
// Some data we add to the CSV header to see where the point cloud comes from.
double x, y, r, rnoise, a0, a1;
int n;
if(!mk_test(csv_txt, &x, &y, &r, &a0, &a1, &n, &rnoise)) {
cerr << "Failed to create test data." << endl;
return 1;
}
// That text is simply read back via the csv reader
csv.abort_on_error(true);
csv.with_header(csv_t::header_auto);
csv.comment_chars("#");
std::stringstream ss(csv_txt);
ss >> csv;
if(csv.error() != csv_t::ok) {
cerr << "Failed to read generated test data." << endl;
}
// Fit the data
fit();
// Evaluate errors
double dxy = sqrt((x-circle.x)*(x-circle.x) + (y-circle.y)*(y-circle.y));
double dr = (r-circle.r);
// Print result
cerr
<< "\"err_xy\"," << "\"err_r\","
<< " "
<< "\"ref_x\"," << "\"ref_y\"," << "\"ref_r\"," << "\"num_points\","
<< "\"ref_arc_start\"," << "\"ref_arc_len\"," << "\"r_noise\","
<< " "
<< "\"fit_x\"," << "\"fit_y\"," << "\"fit_r\","
<< " "
<< "\"fit_diff_x\"," << "\"fit_diff_y\"," << "\"fit_diff_r\""
<< endl;
cout
<< dxy << "," << dr << ","
<< " "
<< x << "," << y << "," << r << "," << n << "," << a0 << "," << (a1-a0) << "," << rnoise << ","
<< " "
<< circle.x << "," << circle.y << "," << circle.r << ","
<< " "
<< (x-circle.x) << "," << (y-circle.y) << "," << (r-circle.r)
<< endl;
} else {
// We assume the positional argument is a file.
// csv is a global object, it shall stop on read errors or inconsistencies,
// auto detect a header and ignore lines starting with "#" === coments.
// csv.load() returns true on success.
csv.abort_on_error(true);
csv.with_header(csv_t::header_auto);
csv.comment_chars("#");
if(!csv.load(file)) {
cerr << "failed to load " << file << endl;
if(cli_args["v"] != false) {
// This here is only debugging to see what it actually did read.
if(csv.has_header()) {
for(unsigned i=0; i<csv.header().size(); ++i) {
cerr << csv.header()[i] << " ";
}
cerr << endl;
}
for(unsigned i=0; i<csv.data().size(); ++i) {
for(unsigned j=0; j<csv.data()[i].size(); ++j) {
cerr << csv.data()[i][j] << " ";
}
cerr << endl;
}
}
return 1;
}
// Double check the read data
if(csv.cols() != 2) {
cerr << "File data must have 2 columns, column1=x, column2=y'" << endl;
return 1;
} else if(csv.rows() < 3) {
cerr << "File must have at least 3 data rows." << endl;
return 1;
} else if(!csv.data_size_consistent()) {
cerr << "The row sizes of the CSV file are not consistent." << endl;
return 1;
}
// Fit the data
fit();
// Print the result, optionally with additional verbose output
cout << "x=" << circle.x << ", y=" << circle.y << ", r=" << circle.r;
cout.flush();
if(cli_args["v"] == true) {
cerr << ", s=" << circle.s;
if(cli_args["g"] == true) {
cerr << ", g=" << circle.g << ", iterations=" << circle.i
<< ", inner_iterations=" << circle.j;
}
}
cout << endl;
}
return 0;
}Beispiel (Octave Script für Testkreis-Plots)
Example (Octave script for plotting generated test circle data)
#!/usr/bin/octave -qf
%% Import
function f = load_data(file_path)
fh = fopen(file_path,'r');
s = fgets(fh);
circle.r = 0;
while (strncmp(s,'#',1) == 1);
s = strrep(substr(s, 2), ' ', '');
if(length(strfind(s,"cfg:")) > 0), s = fgets(fh); continue; end;
[rrs, rre, rrte, rrm, rrt, rrnm, rrsp] = regexpi(s, "^(x|y|r|a|b|N|arc)=([\\d.eE+\\-]+)");
if(length(rrt) == 0), s = fgets(fh); continue; end;
circle = setfield(circle, rrt{1}{1}, str2double(rrt{1}{2}));
s = fgets(fh);
endwhile
data = dlmread(fh);
fclose(fh);
f.x = data(:,1);
f.y = data(:,2);
f.circle = circle;
endfunction
%% Plot
data = load_data('/tmp/cf.csv');
mmin = min([min(data.x) min(data.y)]);
mmax = max([max(data.x) max(data.y)]);
dmin = 10 ^ floor(log10(abs(mmin)));
dmax = 10 ^ floor(log10(abs(mmax)));
mmin = floor(mmin / dmin) * dmin;
mmax = ceil(mmax / dmax) * dmax;
clf();
plot(data.x, data.y, 'x');
axis([mmin mmax mmin mmax], 'square');
grid on;
hold on;
plot([data.circle.x], [data.circle.y], 'r-x');
a = pi()/180 * linspace(data.circle.a, data.circle.a+data.circle.arc, 100);
cx = cos(a) .* data.circle.r + data.circle.x;
cy = sin(a) .* data.circle.r + data.circle.y;
plot(cx, cy, 'r-', 'linewidth',2);
sleep(2);Quelltext
Source code
/**
* @package de.atwillys.cc.swl
* @license BSD (simplified)
* @author Stefan Wilhelm (stfwi)
*
* @file circlefit.hh
* @ccflags
* @ldflags -lm
* @platform linux, bsd, windows
* @standard >= c++98
*
* -----------------------------------------------------------------------------
*
* Class template containing circle fit algorithms.
*
* - Algebraic: Hyperfit
* - Geometric: Levenberg-Marquard
*
* Usage:
*
* // 1st possibility: arrays of x/y coords
* double x[NUM_POINTS], y[NUM_POINTS];
* [...]
* circle_fit::circle_t circle = circle_fit::fit(x, y, NUM_POINTS); // <-- Hyperfit
* circle_fit::circle_t circle = circle_fit::fit_geometric(x,y,NUM_POINTS); // Levenberg-Marquard
* double center_x = circle.x;
* double center_y = circle.y;
* double r = circle.r;
* [...]
*
* // 2nd possibility: vector of points
* circle_fit::points_t points;
* points.push_back(circle_fit::point_t(X0, Y0));
* points.push_back(circle_fit::point_t(X1, Y1));
* [...]
* circle_fit::circle_t circle = circle_fit::fit(points);
*
* Annotations:
*
* - The fit() / fit_geometric() functions have optional arguments (especially for the geometric
* fit) to optimise the algorighms.
*
* - The circle_t class has additional verbose output.
*
* This implementation is based on "Circular and Linear Regression: Fitting Circles and Lines
* by Least Squares" (ISBN-10: 143983590X), Nikolai Chernov, | cas.uab.edu.
*
* -----------------------------------------------------------------------------
* +++ BSD license header +++
* Copyright (c) 2012, Stefan Wilhelm (<cerbero s@atwilly s.de>)
* 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 of this library 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 HOLDER
* 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.
* -----------------------------------------------------------------------------
*/
#ifndef SW_CIRCLEFIT_HH
#define SW_CIRCLEFIT_HH
// <editor-fold desc="preprocessor" defaultstate="collapsed">
#include <cmath>
#include <limits>
#include <vector>
#include <climits>
#include <iostream>
#if defined(__MSC_VER)
#define cf_isfinite _isfinite
#define cf_fabs fabs
#else
#define cf_isfinite std::isfinite
#define cf_fabs fabs
#endif
// </editor-fold>
namespace sw { namespace detail {
/**
* @class basic_circle_fit
*/
template <typename float_type>
class basic_circle_fit
{
public:
// <editor-fold desc="circle_t, point_t, points_t" defaultstate="collapsed">
/**
* Describes a resulting circle with additional analysis data.
*/
class circle_t
{
public:
inline circle_t() : x(0), y(0), r(0), s(0), g(0), i(0), j(0), ok(true)
{ ; }
inline circle_t(const circle_t& c) : x(c.x), y(c.y), r(c.r), s(c.s), g(c.g), i(c.i),
j(c.j), ok(c.ok)
{ ; }
inline circle_t(float_type x_, float_type y_, float_type r_) : x(x_), y(y_),r(r_), s(0), g(0),
i(0), j(0), ok(true)
{ ; }
explicit inline circle_t(bool b): x(0), y(0), r(0), s(0), g(0), i(0), j(0), ok(b)
{ ; }
virtual ~circle_t()
{ ; }
circle_t& operator = (const circle_t& c)
{ x=c.x; y=c.y; r=c.r; s=c.s; g=c.g; i=c.i; j=c.j; ok=c.ok; return *this; }
friend std::ostream & operator<< (std::ostream & os, const circle_t& c)
{ os << "{x:" << c.x << ", y:" << c.y << ", r:" << c.r
<< ", ok:" << c.ok << ", i:" << c.i << "}"; return os; }
public:
float_type x, y, r, s, g;
int i, j;
bool ok;
};
/**
* Basic point structure
*/
struct point_t
{
public:
float_type x, y;
inline point_t() : x(0), y(0) {}
inline point_t(float_type x_, float_type y_) : x(x_), y(y_) {}
inline point_t(const point_t &p) : x(p.x), y(p.y) {}
virtual ~point_t() { ; }
inline point_t& operator = (const point_t &p) { x=p.x; y=p.y; return *this; }
};
/**
* Point vector
*/
typedef std::vector<point_t> points_t;
// </editor-fold>
// <editor-fold desc="data_t" defaultstate="collapsed">
/**
* Data class for C-array compatible, heap allocated point cloud.
*/
class data_t
{
public:
data_t() : x_mean(0), y_mean(0)
{ ; }
virtual ~data_t()
{ ; }
/**
* Reserve memory space for n values. Returns true on success.
* @param int n
* @return bool
*/
inline void reserve(int n)
{ x.reserve(n); y.reserve(n); }
/**
* Data assignment, std::vector's
* @return bool
*/
template <typename T>
inline bool set(const std::vector<T>& x_, const std::vector<T>& y_) {
clear();
if(x_.empty() || (x_.size() != y_.size())) return false;
x = x_; y = y_;
return true;
}
/**
* Data assignment, point vector/list
* @param const points_t & points
* @return bool
*/
inline bool set(const points_t & points) {
if(points.size() == 0) return false;
reserve(points.size());
for(typename points_t::const_iterator it = points.begin(); it != points.end(); ++it) {
x.push_back(it->x); y.push_back(it->y);
}
return true;
}
/**
* Clear the object, free heap memory.
*/
inline void clear()
{ x_mean = y_mean = 0; x.clear(); y.clear(); }
/**
* Average calculation over all x / y values.
*/
inline void means(void)
{
x_mean = y_mean = 0.;
unsigned n = x.size();
for(int i=n-1; i >= 0; --i) { x_mean += x[i]; y_mean += y[i]; }
x_mean /= n; y_mean /= n;
}
public:
std::vector<float_type> x, y;
float_type x_mean, y_mean;
};
// </editor-fold>
public:
// <editor-fold desc="algebraic fit interface" defaultstate="collapsed">
/**
* Fit (input pair vector)
* @param const points_t& points
* @return circle_t
*/
static circle_t fit(const points_t & points)
{ data_t data; return (data.set(points)) ? fit(data) : circle_t(false); }
/**
* Fits x/y arrays with defined length into a result circle structure.
* @param float_type *x
* @param float_type* y
* @param long n
* @return circle_t
*/
// static circle_t fit(float_type *x, float_type* y, long n)
// { data_t data; return (data.set(n, x, y)) ? fit(data) : circle_t(false); }
/**
* Fits x/y numeric vectors.
* @param float_type *x
* @param float_type* y
* @param long n
* @return circle_t
*/
template <typename T>
static circle_t fit(const std::vector<T>& x, const std::vector<T>& y)
{ data_t data; return (data.set(x,y)) ? fit(data) : circle_t(false); }
/**
* Fits a data set to a resulting 2D circle result, optionally geometric.
* @param data_t &data
* @return circle_t
*/
static circle_t fit(data_t &data)
{ circle_t c; circlefit_hyper(data, c); return c; }
// </editor-fold>
public:
// <editor-fold desc="geometric fit interface" defaultstate="collapsed">
/**
* Fit (input pair vector)
* @param const points2d &points
* @return circle_t
*/
static circle_t fit_geometric(const points_t &points, float_type lambda = 0.2,
float_type factor_up = 10., float_type factor_down = 0.04, float_type limit = 1.e+6,
float_type epsilon = 3.0e-8, int max_iterations=100)
{ data_t data; return (!data.set(points)) ? circle_t(false) : fit_geometric(data, lambda,
factor_up, factor_down, limit, epsilon, max_iterations); }
/**
* Fits x/y numeric vectors.
* @param float_type *x
* @param float_type* y
* @param long n
* @return circle_t
*/
template <typename T>
static circle_t fit_geometric(const std::vector<T>& x, const std::vector<T>& y,
float_type lambda=0.2, float_type factor_up = 10., float_type factor_down = 0.04,
float_type limit = 1.e+6, float_type epsilon = 3.0e-8, int max_iterations=100)
{ data_t data; return (!data.set(x,y)) ? circle_t(false) : fit_geometric(data, lambda,
factor_up, factor_down, limit,epsilon, max_iterations); }
/**
* Fits a data set to a resulting 2D circle result.
* @param data_t &data
* @param float_type geometric_lambda=0.2
* @return circle_t
*/
static circle_t fit_geometric(data_t &data, float_type lambda=0.2, float_type factor_up = 10.,
float_type factor_down = 0.04, float_type limit = 1.e+6, float_type epsilon = 3.0e-8,
int max_iterations=100)
{
circle_t c;
circlefit_hyper(data, c);
circle_t initial_guess = c;
circlefit_levenberg_marquard(data, c, initial_guess, lambda, factor_up, factor_down, limit,
epsilon, max_iterations);
return c;
}
// </editor-fold>
protected:
// <editor-fold desc="algorithms" defaultstate="collapsed">
#define _1 ((float_type)1.0)
#define _2 ((float_type)2.0)
#define _3 ((float_type)3.0)
#define _4 ((float_type)4.0)
#define _10 ((float_type)10.0)
/**
* Sqare
* @param float_type x
* @return float_type
*/
static inline float_type sqr(float_type x)
{ return x*x; }
/**
* Deviation
* @param data_t& data
* @param circle_t& circle
* @return float_type
*/
static float_type sigma(data_t& data, circle_t& circle)
{
int i, n = data.x.size();
float_type sum = 0., dx, dy, r;
float_type LargeCircle = _10, a0, b0, del, s, c, x, y, z, p, t, g, W, Z;
std::vector<float_type> D(n);
float_type result = 0;
if (cf_fabs(circle.x) < LargeCircle && cf_fabs(circle.y) < LargeCircle) {
for (i = 0; i < n; ++i) {
dx = data.x[i] - circle.x;
dy = data.y[i] - circle.y;
D[i] = sqrt(dx * dx + dy * dy);
sum += D[i];
}
r = sum / n;
for (sum = 0., i = 0; i < n; ++i) sum += sqr(D[i] - r);
result = sum / n;
} else {
// Case of a large circle -> prevent numerical errors
a0 = circle.x - data.x_mean;
b0 = circle.y - data.y_mean;
del = _1 / sqrt(a0 * a0 + b0 * b0);
s = b0 * del;
c = a0 * del;
for (W = Z = 0., i = 0; i < n; ++i) {
x = data.x[i] - data.x_mean;
y = data.y[i] - data.y_mean;
z = x * x + y*y;
p = x * c + y*s;
t = del * z - _2*p;
g = t / (_1 + sqrt(_1 + del * t));
W += (z + p * g) / (_2 + del * g);
Z += z;
}
W /= n;
Z /= n;
result = Z - W * (_2 + del * del * W);
}
return result;
}
/**
* Hyper ("hyperaccurate") circle fit. It is an algebraic circle fit
* with "hyperaccuracy" (with zero essential bias).
* A. Al-Sharadqah and N. Chernov, "Error analysis for circle fitting algorithms",
* Electronic Journal of Statistics, Vol. 3, pages 886-911, (2009)
* Combines the Pratt and Taubin fits to eliminate the essential bias.
*
* It works well whether data points are sampled along an entire circle or
* along a small arc.
* Its statistical accuracy is slightly higher than that of the geometric fit
* (minimizing geometric distances) and higher than that of the Pratt fit
* and Taubin fit.
* It provides a very good initial guess for a subsequent geometric fit.
*
* @param data_t& data
* @param circle_t& circle
* @param int max_iterations=100
* @return void
*/
static void circlefit_hyper(data_t& data, circle_t& circle, int max_iterations=100)
{
float_type xi, yi, zi;
float_type Mz, Mxy, Mxx, Myy, Mxz, Myz, Mzz, cov_xy, var_z;
float_type A0, A1, A2, A22;
float_type dy, xnew, x, ynew, y;
float_type det, x_center, y_center;
data.means();
int n = data.x.size();
// Moments
Mxx = Myy = Mxy = Mxz = Myz = Mzz = 0.;
for(int i = 0; i < n; ++i) {
xi = data.x[i] - data.x_mean; // centered x-coordinates
yi = data.y[i] - data.y_mean; // centered y-coordinates
zi = xi * xi + yi*yi;
Mxy += xi*yi; Mxx += xi*xi;
Myy += yi*yi; Mxz += xi*zi;
Myz += yi*zi; Mzz += zi*zi;
}
Mxx /= n; Myy /= n;
Mxy /= n; Mxz /= n;
Myz /= n; Mzz /= n;
// Coefficients of the characteristic polynomial
Mz = Mxx + Myy;
cov_xy = Mxx * Myy - Mxy*Mxy;
var_z = Mzz - Mz*Mz;
A2 = _4 * cov_xy - _3 * Mz * Mz - Mzz;
A1 = var_z * Mz + _4 * cov_xy * Mz - Mxz * Mxz - Myz*Myz;
A0 = Mxz * (Mxz * Myy - Myz * Mxy) + Myz * (Myz * Mxx - Mxz * Mxy) - var_z*cov_xy;
A22 = A2 + A2;
// Finding the root of the characteristic polynomial using Newton's method starting
// at x=0. (it is guaranteed to converge to the right root)
// Usually, 4-6 iterations are enough
int it;
for (x = 0., y = A0, it = 0; it < max_iterations; ++it) {
dy = A1 + x * (A22 + 16. * x * x);
xnew = x - y / dy;
if ((xnew == x) || (!cf_isfinite(xnew))) break;
ynew = A0 + xnew * (A1 + xnew * (A2 + _4 * xnew * xnew));
if (cf_fabs(ynew) >= cf_fabs(y)) break;
x = xnew;
y = ynew;
}
// Circle parameters
det = x * x - x * Mz + cov_xy;
x_center = (Mxz * (Myy - x) - Myz * Mxy) / det / _2;
y_center = (Myz * (Mxx - x) - Mxz * Mxy) / det / _2;
circle.x = x_center + data.x_mean;
circle.y = y_center + data.y_mean;
circle.r = sqrt(x_center * x_center + y_center * y_center + Mz - x - x);
circle.s = sigma(data, circle);
circle.i = 0;
circle.j = it;
circle.ok = true;
}
/**
* Levenberg-Marquard algorithm for x,y,r
*
* @param data_t& data
* @param circle_t& circle
* @param circle_t& initial_guess
* @param float_type initial_lambda=1
* @param float_type factor_up = 10
* @param float_type factor_down = 0.04
* @param float_type limit = 1.e+6
* @param float_type epsilon = 3.0e-8
* @param int max_iterations=100
* @return void
*/
static void circlefit_levenberg_marquard(data_t& data, circle_t& circle,
circle_t& initial_guess, float_type initial_lambda=1, float_type factor_up = 10.,
float_type factor_down = 0.04, float_type limit = 1.e+6, float_type epsilon = 3.0e-8,
int max_iterations=100)
{
float_type lambda, dx, dy, ri, u, v;
float_type Mu, Mv, Muu, Mvv, Muv, Mr, UUl, VVl, Nl, F1, F2, F3, dX, dY, dR;
float_type G11, G22, G33, G12, G13, G23, D1, D2, D3;
circle_t curr;
curr = initial_guess;
curr.s = sigma(data, curr);
lambda = initial_lambda;
circle.i = circle.j = 0;
int n = data.x.size();
for(circle.i=1; circle.i <= max_iterations && circle.j < max_iterations; ++circle.i) {
circle.x = curr.x;
circle.y = curr.y;
circle.r = curr.r;
circle.s = curr.s;
circle.g = curr.g;
Mu = Mv = Muu = Mvv = Muv = Mr = 0.; // Moments
for(int i=0; i < n; ++i) {
dx = data.x[i] - circle.x;
dy = data.y[i] - circle.y;
ri = sqrt(dx * dx + dy * dy);
u = dx / ri; v = dy / ri;
Mu += u; Mv += v; Muu += u*u; Mvv += v*v; Muv += u*v; Mr += ri;
}
Mu /= n; Mv /= n; Muu /= n; Mvv /= n; Muv /= n; Mr /= n;
F1 = circle.x + circle.r * Mu - data.x_mean; // Matrices
F2 = circle.y + circle.r * Mv - data.y_mean;
F3 = circle.r - Mr;
circle.g = curr.g = sqrt(F1 * F1 + F2 * F2 + F3 * F3);
for(; circle.j < max_iterations; ++circle.j) {
UUl = Muu + lambda;
VVl = Mvv + lambda;
Nl = _1 + lambda;
G11 = sqrt(UUl); // Cholesly decomposition
G12 = Muv / G11;
G13 = Mu / G11;
G22 = sqrt(VVl - G12 * G12);
G23 = (Mv - G12 * G13) / G22;
G33 = sqrt(Nl - G13 * G13 - G23 * G23);
D1 = F1 / G11;
D2 = (F2 - G12 * D1) / G22;
D3 = (F3 - G13 * D1 - G23 * D2) / G33;
dR = D3 / G33;
dY = (D2 - G23 * dR) / G22;
dX = (D1 - G12 * dY - G13 * dR) / G11;
if ((cf_fabs(dR) + cf_fabs(dX) + cf_fabs(dY)) / (_1 + circle.r) < epsilon) {
circle.ok = true;
return;
}
curr.x = circle.x - dX; // Parameter update
curr.y = circle.y - dY;
if (cf_fabs(curr.x) > limit || cf_fabs(curr.y) > limit) {
return;
}
curr.r = circle.r - dR;
if (curr.r <= 0.) {
lambda *= factor_up;
continue;
}
curr.s = sigma(data, curr);
if (curr.s < circle.s) {
lambda *= factor_down; // improvement --> next iteration
break;
} else {
lambda *= factor_up;
}
}
}
}
#undef _1
#undef _2
#undef _3
#undef _4
#undef _10
#undef cf_isfinite
#undef cf_fabs
// </editor-fold>
};
}}
// <editor-fold desc="default specialisation" defaultstate="collapsed">
namespace sw {
typedef detail::basic_circle_fit<double> circle_fit;
}
// </editor-fold>
#endif