GIT repositories

Index page of all the GIT repositories that are clonable form this server via HTTPS. Übersichtsseite aller GIT-Repositories, die von diesem Server aus über git clone (HTTPS) erreichbar sind.

Services

A bunch of service scripts to convert, analyse and generate data. Ein paar Services zum Konvertieren, Analysieren und Generieren von Daten.

GNU octave web interface

A web interface for GNU Octave, which allows to run scientific calculations from netbooks, tables or smartphones. The interface provides a web form generator for Octave script parameters with pre-validation, automatic script list generation, as well presenting of output text, figures and files in a output HTML page. Ein Webinterface für GNU-Octave, mit dem wissenschaftliche Berechnungen von Netbooks, Tablets oder Smartphones aus durchgeführt werden können. Die Schnittstelle beinhaltet einen Formulargenerator für Octave-Scriptparameter, mit Einheiten und Einfabevalidierung. Textausgabe, Abbildungen und generierte Dateien werden abgefangen und in einer HTML-Seite dem Nutzer als Ergebnis zur Verfügung gestellt.

C++ Klassentemplate für 2D Ausrichtung an Erkennungsmarken

C++ class template for fiducial alignment calculations

Ein Klassentemplate zum Errechnen der 2D-Ausrichtung einer "Target"-Punktwolke an eine Referenzpunktwolke (oder umgekehrt). Die Ausgabe ist eine Liste der möglichen Ausrichtungen, welche angegebene Kriterien erfüllen. Jede Ausrichtungsangabe beschreibt dabei die Rotation und Translation, mit der das Target auf die Referenz ausgerichtet wird. Auch Zusatzinformationen wie die maximale und mittlere Abweichung sind enthalten.

Sinn und Zweck hiervon ist es, für CNC-Router oder andere Produktionsgeräte eine Angabe zu erhalten, mit der das Werkzeugkoordinatensystem an ein ungenau positioniertes Werkstück angepasst werden kann (oder Belichtungsmasken / "Screens" für Siebdrucker). Dazu werden Erkennungsmarken ("alignment marks", "fiducials") am Werkstück (evtl. auch an den Masken/Screens) vermessen. Diese beiden Punktlisten können mit dieser Klasse verarbeitet werden.

Im Kern funktioniert die Ausrichtung über eine distanzgewichtete Rotationsanpassung und folgender Fehlermittelwertbestimmung für die Translation. Die Ergebnisse sind ähnlich genau wie bei einer Rotationsbestimmung über Singulärwertzerlegung, in der Praxis ist die Methode jedoch robuster.

Annahmen und Kriterien dieser Implementation:

  • Die Punktwolken haben wenige Werte. Oftmals werden nur zwei bis drei Erkennungsmarken verwendet (offenbar ausreichnd genau, verkürzte Messzeit, weniger Platzverbrauch für Erkennungsmarken ...). Zehn Marken stellen ebenfalls kein Problem dar.

  • Der Messfehler ist viel kleiner als der minimale Abstand zwischen zwei Erkennungsmarken. Technisch sind Fiducials strategisch weit auseinander plaziert, damit die Rotationsbestimmung so wenig wie möglich unter Messfehlern leidet.

  • Es gibt mehr oder gleich viele Referenzpunkte wie Target-Punkte. D.h. es soll möglich sein, Messungen wegzulassen - entweder aus Zeitgründen oder wegen schlechten Messwerten durch Verschmutzung, Produktionsfehler, usw. So lange noch zwei Punkte übergeben werden soll die Klasse Ergebnisse zurückgeben.

  • Die Rotation kann 360° betragen. Dies deckt Fälle ab, bei denen z.B. eine Platine beliebig auf den Tisch eines CNC-Routers gelegt werden kann.

  • Es gibt keine speziellen Designregeln für die Platzierung der Erkennungsmarken (öfter gesehen: Drei Punkte benötigt, die einen 90° Winkel ergeben müssen). Die Erkennung soll mit jeder Konfiguration rechnen können.

  • Die Markierungen dürfen mehrdeutig sein, für jede Möglichkeit soll ein Ergebnis ausgegeben werden: Wenn z.B. die Punkte aus Designgründen im Quadrat angeordnet sind, so kann aus den Messungen nicht herausgehen, ob die Platine 0°, 90°, 180° oder 270° rotiert liegt.

  • Die Punktlisten müssen nicht sortiert sein, d.h. target1?==referenz1 usw. Die Klasse soll dies selbst erkennen.

Einstellungen:

  • "Matching"-Toleranz: Mit der Methode tolerance(..) kann eine maximale Punktabweichung festgelegt werden, die beim Vorfiltern (welcher Targetpunkt welcher Referenzpunkt sein könnte) und der Vorausrichtung zum tragen kommt. Default ist 0, dann wird der Wert automatisch auf ein Zehntel der minimalen Punktdistanz gesetzt.

  • Maximale Rotation: Mit max_rotation(..) lassen sich mehrdeutige Ergebnisse herausfiltern, z.B. wenn bekannt ist, dass die Platine maximal -45° bis 45° falsch positioniert ist. Auch als generelle Fehlerprüfung geeignet, z.B. wenn der Ausrichtungsaktuator nur 30° rotieren kann.

  • Maximale Transkation: Mit max_translation(dbl) kann die maximale Verschiebung gesetzt werden - ebenfalls eine Plausibilitätsprüfung.

  • Ergebnisse invertieren: Normalerweise sind die Ergebnisse so angegeben dass das Target auf die Referenz verschoben wird. Für CNC-Router hat man den anderen Fall - das Koordinatensystem des Werkzeugs muss auf die Platine angepasst werden. Dazu kann inverse(true) gesetzt werden.

  • Eindeutiges Ergebnis: Mit unique(true) wirft die Klasse einen Fehler, wenn das Ergebnis nicht eindeutig ist. Bei unique(false) werden alle Möglichleiten ausgegeben.

Beispielprogramm:

  • Das angegebene Beispielprogramm kann direkt mit der Kommandozeile als Schnittstelle verwendet werden. Die Daten werden als csv-Dateien angegeben, die Einstellungen als Kommandozeilenargumente. Einfach kompilieren und fiducial-alignment --help tippen. Mögliche Ausgabeformate sind Text, "human readable", csv, json.

Class template for calculating the 2D alignment of a measured "target" point cloud to a measured or fixed reference point cloud (or vice versa). The output is a list of possible alignments matching all criteria, where each alignment basically describes how you have to rotate and translate the target so that it is aligned to the reference. Additional information are maximum and average point-to-point errors, matching points, and the like.

Purpose is to yield a coordinate system transformation for CNC machines or manufacturing devices that have to process targets/work pieces aligned (at "identical positions") to a previous process. Do detect the misalignment, some strategic points are measured on the target ("alignment marks" / "fiducials") and matched to the correspondent positions in the reference model (the reference points can be measured, too). Feeding the point data of target and reference into this class will calculate what rotation and translation is required to align target and reference.

The core alignment is based on distance-weighted rotation and following centeroid translation matching, which has under the assumption described below similar accuracy as the singular value decomposition method, but it is more robust in practice.

Some assumptions made according that the functionality of this class is based on:

  • The point clouds have a small number of elements. Often two or three points are used in practice due to the time consumption for fiducial measurements. The class has to be reasonably fast for 10 points.

  • The measurement error is much smaller than the distances between the fiducials. Technically fiducials are strategically placed far away from another to optimise the angular accuracy.

  • The number of reference points is equal or higher than the amount of target points. That means you can omit target points, e.g. if a fiducial is defect, stained, malformed, etc. As long as there are at least two left, the class shall calculate.

  • The rotation angle can be 360°, means in case of a CNC milling router it should be possible to rotate the PCB as it fits best in the machine. Screen printers or mask aligners don't need this feature.

  • There is no required "predefined structure" of the fiducials, e.g. (sometimes seen) three points that have to be 90°. Fiducials can be anywhere where the part/layer design allows it, as long as they are far enough away from each other.

  • Fiducial setups can be ambiguous, e.g. four points as a square will result in four possible rotations (0, 90, 180, 270)°. Four points in a rectangle two possible rotations (0, 180)°.

  • The resulting translations and rotations shall be error-vector-length-square minimised over all points (similar to a LES fit or SVD).

  • The point lists do not need to be sorted, an auto detection has to determine which reference point corresponds to which target point.

Settings:

  • Matching tolerance: You can define a fixed matching tolerance (or leave it 0 for ""auto). That tolerance is mainly used to pre-detect which target point COULD be which reference point, which is done in a point-to-point-distance matching an a pre-alignment-matching. The method for this is tolerance(dbl).

  • Maximum rotation: If your fiducial setup (for resign restriction reasons) ambiguous, e.g. rectangular fiducial setup, but you know that the target is placed better than 45°, you can exclude the redundant result by setting max_rotation(PI/4.). It is also simply a good process error check, maybe if the device cannot rotate more than 45°.

  • Maximum translation: The maximum allowed translation, mainly intended for process error checks - e.g. the if the alignment manipulators can only move 10mm, or the like. The setter for this parameter is max_translation(dbl).

  • Inverse results: Normally the result angle and shift is specified to move the target to the reference. Say inverse(true) if you like to move the reference to the target.

  • Unique result: The class can raise an error if there is more than one possible result (method unique(bool)).

Annotations:

  • The file can be compiled with c++98 standard or higher.

  • If you indent to use long double as float_t template argument, double check the SINCOS macro (default sincos(x,s,c) for GNU or s=sin(x);c=cos(x)) if this function actually uses this accuracy.

  • This class is not intended to be an exception source, errors are given as return values and via error() and error_text(). Only STL containers may throw in case of memory problems.

Datei

File

fiducial_alignment.hh fiducial_alignment.cc (test program) fiducial_alignment.m (Octave test script) csv_reader.hh (example requirement) cli_args.hh (example requirement)

Beispiel

Example

#include <vpu/fiducial_alignment.hh>
#include <csv_reader.hh>
#include <cli_args.hh>
#include <iostream>
#include <vector>
#include <string>
#include <cmath>
#include <limits>
#include <algorithm>
#include <sstream>
 
#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif
 
using namespace std;
using sw::fiducial_alignment;
using sw::cli_args;
 
bool read_points(const std::string& file, fiducial_alignment::points_t& pts)
{
  sw::csv_reader<double> csv(file, true, sw::csv_reader<double>::header_auto, "@#%;!'*/");
  if(csv.error() || !csv.data_size_consistent()) {
    return false;
  } else if(csv.data().empty() || csv.data().front().size() != 2) {
    return false;
  }
  sw::csv_reader<double>::row_type::const_iterator it;
  for(it = csv.data().begin(); it != csv.data().end(); ++it) {
    pts.push_back(fiducial_alignment::point_t(it->front(), it->back()));
  }
  return true;
}
 
int main(int argc, const char* argv[])
{
  cli_args(argc, argv) // Global CLI argument object.
    ("T", "tolerance", "n", "0" , "" , "Maximum accepted deviation when matching target points "
                                       "to reference points.")
    ("t", "max-translation", "n", "0" , "" , "Maximum accepted translation (0=check disabled).")
    ("r", "max-rotation", "n", "0" , "" , "Maximum accepted rotation in degrees (0=check disabled).")
    ("i", "inverse", "b", "" , "1" , "Align the reference to the target.")
    ("m", "multiple", "b", "" , "1" , "Accept and output ambiguous result possibilities.")
    ("H", "human-readable", "b", "" , "1" , "Output results in human readable form.")
    ("c", "csv", "b", "" , "1" , "Output results in csv form --> rotation (rad), x, y.")
    ("j", "json", "b", "" , "1" , "Output results in json form (rotation in rad).")
    ("v", "verbose", "b", "" , "1" , "Add verbosity output to stderr.")
    ("q", "quiet", "b", "" , "1" , "No text output except the result.")
    ;
  cli_args
    .allow_unknown_options(true)
    .parse()
    .handle_help_and_errors(
      "Fiducial alignment calculation.\n\n"
      "  fiducials [opts] <references-file.csv> <target-file.csv>\n",
      "v1.0, stfwi, 2011",
      "<reference points csv file> <target points csv file>",
      " 0: successful\n>0: error"
    );
 
  std::stringstream cnul;
  std::ostream & cverb = (!!cli_args["q"]) ? cnul : cerr;
 
  if(cli_args.positional_arguments().size() < 2) {
    cverb << "Need two files, try --help for more information." << endl;
    return 1;
  }
 
  string ref_file = cli_args.positional_arguments()[0].value();
  string trg_file = cli_args.positional_arguments()[1].value();
  if(ref_file == "-") ref_file = "/dev/stdin";
  if(trg_file == "-") trg_file = "/dev/stdin";
 
  fiducial_alignment alignment;
  fiducial_alignment::points_t ref_points, fid_points;
 
  if(!read_points(ref_file, ref_points)) {
    cverb << "Fiducial points input file: Failed to read or data inconsistent" << endl;
    return 1;
  }
  if(!read_points(trg_file, fid_points)) {
    cverb << "Fiducial points input file: Failed to read or data inconsistent" << endl;
    return 1;
  }
 
  if(!!cli_args["v"] && !cli_args["q"]) fiducial_alignment::dump_stream(cerr);
 
  alignment
    .tolerance((double)cli_args["T"])
    .max_rotation((double)cli_args["r"] * M_PI/180.)
    .max_translation((double)cli_args["t"])
    .inverse(!!cli_args["i"])
    .unique(!cli_args["m"])
    .reference(ref_points)
    .target(fid_points)
    .calculate();
 
  if(alignment.error()) {
    cverb << "Error: " << alignment.error_text() << endl;
    return (int) alignment.error();
  }
 
  if(!!cli_args["H"]) {
    fiducial_alignment::alignments_t::const_iterator it;
    int i=0;
    std::string t1 = !cli_args["i"] ? "target" : "reference";
    std::string t2 = !cli_args["i"] ? "reference" : "target";
    for(it=alignment.results().begin(); it != alignment.results().end(); ++it) {
      ++i;
      cout << "Possibility " << i << ": "
           << "Rotate the " << t1 << " " << std::setprecision(5) << (it->rotation*180./M_PI)
           << "° around the origin [0,0] and translate "
           << it->translation << " to fit the " << t2 << ". The length of the maximum point-to-point "
           << "error is " << it->max_error << ", the average error is " << it->avg_error << "."
           << endl;
    }
  } else if(!!cli_args["c"]) {
    fiducial_alignment::alignments_t::const_iterator it;
    cverb << "rotation, translation_x, translation_y, max_error, avg_error" << endl;
    cverb.flush();
    for(it=alignment.results().begin(); it != alignment.results().end(); ++it) {
      cout << ""  << std::fixed << std::setprecision(10) << it->rotation
           << ", " << std::fixed << std::setprecision(10) << it->translation.x
           << ", " << std::fixed << std::setprecision(10) << it->translation.y
           << ", " << std::fixed << std::setprecision(10) << it->max_error
           << ", " << std::fixed << std::setprecision(10) << it->avg_error
           << endl;
    }
  } else if(!!cli_args["j"]) {
    fiducial_alignment::alignments_t::const_iterator it;
    cout << "[" << endl;
    int i = alignment.results().size() - 1;
    for(it = alignment.results().begin(); it != alignment.results().end(); ++it, --i) {
      cout << " { rotation: \""  << std::fixed << std::setprecision(10) << it->rotation << "\", "
           << " translation { x: \"" << std::fixed << std::setprecision(10) << it->translation.x << "\","
           << " y: \"" << std::fixed << std::setprecision(10) << it->translation.y << "\""
           << " }, max_error: \"" << std::fixed << std::setprecision(10) << it->max_error << "\", "
           << " average_error: \"" << std::fixed << std::setprecision(10) << it->avg_error << "\""
           << " }" << (i <= 0 ? "" : ",") << endl;
    }
    cout << "]" << endl;
  } else {
    fiducial_alignment::alignments_t::const_iterator it;
    for(it=alignment.results().begin(); it != alignment.results().end(); ++it) {
      cout << "R="  << std::fixed << std::setprecision(10) << it->rotation
           << " X=" << std::fixed << std::setprecision(10) << it->translation.x
           << " Y=" << std::fixed << std::setprecision(10) << it->translation.y
           << " ERR_MAX=" << std::fixed << std::setprecision(10) << it->max_error
           << " ERR_AVERAGE=" << std::fixed << std::setprecision(10) << it->avg_error
           << endl;
    }
  }
  return 0;
}

Beispiel GNU Octave Gegencheck mit SVD und Zufallspunkten

Example Octave checking script

#/usr/local/bin/octave -qf
max_xy_noise = 3;
max_xy = 100;
max_T = 10;
num_points = 5; % number of points
do_inverse = 0;
global tmpdir;
global binary;
tmpdir = "/tmp/fid";
binary = "../../bin/fiducial_alignment";
 
function [rotation, translation] = svd_transform(pts1, pts2)
  if nargin != 2, error("Missing parameters"); end
  assert(size(pts1) == size(pts2));
  centre1 = mean(pts1);
  centre2 = mean(pts2);
  N = size(pts1,1);
  H = (pts1 - repmat(centre1, N, 1))' * (pts2 - repmat(centre2, N, 1));
  [U,S,V] = svd(H);
  R = V*U';
  if det(R) < 0, V(:,2) *= -1; R = V*U'; end
  translation = -R * centre1' + centre2';
  rotation = atan2(R(2,1),R(2,2));
end
 
function [rotation, translation, refs, fids, refs_svd, fids_svd, ret_rotation, ret_translation ...
    ] = mk_test_data(num_points, max_xy, max_T, max_xy_noise, do_inverse)
  rotation = rand(1) * 2 * pi;
  R = [ cos(rotation), -sin(rotation) ; sin(rotation), cos(rotation) ];
  translation = max_T * rand(2,1);
  refs = max_xy * rand(num_points, 2);
  fids = R * refs' + repmat(translation, 1, num_points);
  fids = fids';
  fids = fids + max_xy_noise * rand(num_points, 2);
  if(do_inverse),
    [ret_rotation, ret_translation] = svd_transform(refs, fids);
    ret_R = [ cos(ret_rotation), -sin(ret_rotation) ; sin(ret_rotation), cos(ret_rotation) ];
    ret_T = ret_translation;
    refs_svd = ((ret_R * refs') + repmat(ret_T, 1, num_points))';
    fids_svd = fids;
  else
    [ret_rotation, ret_translation] = svd_transform(fids, refs);
    ret_R = [ cos(ret_rotation), -sin(ret_rotation) ; sin(ret_rotation), cos(ret_rotation) ];
    ret_T = ret_translation;
    fids_svd = ((ret_R * fids') + repmat(ret_T, 1, num_points))';
    refs_svd = refs;
  end
end
 
function [ok, rotation, translation, max_err, avg_err, refs_bin, fids_bin] = run_binary(refs, fids, do_inverse)
  global tmpdir;
  global binary;
  global max_xy_noise;
  tolerance = 0;
  ref_file = strcat(tmpdir, "/refs.csv");
  fid_file = strcat(tmpdir, "/fids.csv");
  if(do_inverse), ii=" --inverse"; else ii=""; end;
  cmd = sprintf("%s -v --csv%s --tolerance='%f' '%s' '%s'", binary, ii, tolerance, ref_file, fid_file);
  csvwrite(ref_file, refs);
  csvwrite(fid_file, fids);
  [status, text] = system(cmd);
  if(status ~= 0)
    ok = 0;
    rotation = 0;
    translation = [ 0 ; 0 ];
    max_err = 0;
    avg_err = 0;
    fids_bin = fids;
    refs_bin = refs;
  else
    ok = 1;
    c = textscan(text, "%f,%f,%f,%f,%f");
    rotation = c{1};
    translation = [ c{2} ; c{3} ];
    max_err = c{4};
    avg_err = c{5};
    ret_R = [ cos(rotation), -sin(rotation) ; sin(rotation), cos(rotation) ];
    ret_T = translation;
    if(do_inverse)
      fids_bin = fids;
      refs_bin = ((ret_R * refs') + repmat(ret_T, 1, rows(refs)))';
    else
      fids_bin = ((ret_R * fids') + repmat(ret_T, 1, rows(fids)))';
      refs_bin = refs;
    end
  end;
end
 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
if(exist(binary, "file") == 0), error("Binary to test not found"); end;
if(exist(tmpdir, "dir") == 0), error("Temp dir not found"); end;
 
for(i=1:25)
  [r, t, refs, fids, refs_svd, fids_svd, r1, t1] = mk_test_data(num_points, max_xy, max_T, max_xy_noise, do_inverse);
  [ok, r2, t2, max_err, avg_err, refs_bin, fids_bin] = run_binary(refs, fids, do_inverse);
  if(ok), break; end;
end
 
if(ok == 0), error("25 trys done, stopping here"); end;
 
err1 = refs_svd - fids_svd;
err1 = err1 .* err1;
err1 = sum(err1(:));
err_sq_svd = sqrt(err1/num_points)
 
err2 = refs_bin - fids_bin;
err2 = err2 .* err2;
err2 = sum(err2(:));
err_sq_bin = sqrt(err2/num_points)
 
figure();
if(do_inverse)
  plot(fids(:,1), fids(:,2), 'ok');
  hold on;
  plot(refs_svd(:,1),refs_svd(:,2), 'xr');
  plot(refs_bin(:,1),refs_bin(:,2), '*b');
else
  plot(refs(:,1), refs(:,2), 'ok');
  hold on;
  plot(fids_svd(:,1),fids_svd(:,2), 'xr');
  plot(fids_bin(:,1),fids_bin(:,2), '*b');
end
 
saveas(gcf(), sprintf("%s/plot.png", tmpdir));
 
sleep(3);

Quelltext

Source code

/**
 * @package de.atwillys.cc.swl.im
 * @license BSD (simplified)
 * @author Stefan Wilhelm (stfwi)
 *
 * @file fiducial_alignment.hh
 * @ccflags
 * @ldflags -lm
 * @platform linux, bsd, windows
 * @standard >= c++98
 *
 * -----------------------------------------------------------------------------
 *
 * Class template for calculating the 2D alignment of a measured "target" point cloud
 * to a measured or fixed reference point cloud (or vice versa). The output is a list
 * of possible alignments matching all criteria, where each alignment basically describes
 * how you have to rotate and translate the target so that it is aligned to the reference.
 * Additional information are maximum and average point-to-point errors, matching points,
 * and the like.
 *
 * Purpose is to yield a coordinate system transformation for CNC machines or manufacturing
 * devices that have to process targets/work pieces aligned (at "identical positions") to
 * a previous process. Do detect the misalignment, some strategic points are measured on
 * the target ("alignment marks" / "fiducials") and matched to the correspondent positions
 * in the reference model (the reference points can be measured, too). Feeding the point
 * data of target and reference into this class will calculate what rotation and translation
 * is required to align target and reference.
 *
 * The core alignment is based on distance-weighted rotation and following centeroid
 * translation matching, which has under the assumption described below similar accuracy
 * as the singular value decomposition method, but it is more robust in practice.
 *
 * Some assumptions made according that the functionality of this class is based on:
 *
 *  - The point clouds have a small number of elements. Often two or three points are used
 *    in practice due to the time consumption for fiducial measurements. The class has to be
 *    reasonably fast for 10 points.
 *
 *  - The measurement error is much smaller than the distances between the fiducials. Technically
 *    fiducials are strategically placed far away from another to optimise the angular accuracy.
 *
 *  - The number of reference points is equal or higher than the amount of target points. That
 *    means you can omit target points, e.g. if a fiducial is defect, stained, malformed, etc.
 *    As long as there are at least two left, the class shall calculate.
 *
 *  - The rotation angle can be 360°, means in case of a CNC milling router it should be possible
 *    to rotate the PCB as it fits best in the machine. Screen printers or mask aligners don't
 *    need this feature.
 *
 *  - There is no required "predefined structure" of the fiducials, e.g. (sometimes seen) three
 *    points that have to be 90°. Fiducials can be anywhere where the part/layer design allows
 *    it, as long as they are far enough away from each other.
 *
 *  - Fiducial setups can be ambiguous, e.g. four points as a square will result in four possible
 *    rotations (0, 90, 180, 270)°. Four points in a rectangle two possible rotations (0, 180)°.
 *
 *  - The resulting translations and rotations shall be error-vector-length-square minimised over
 *    all points (similar to a LES fit or SVD).
 *
 *  - The point lists do not need to be sorted, an auto detection has to determine which reference
 *    point corresponds to which target point.
 *
 * Settings:
 *
 *  - Matching tolerance: You can define a fixed matching tolerance (or leave it 0 for ""auto).
 *    That tolerance is mainly used to pre-detect which target point COULD be which reference
 *    point, which is done in a point-to-point-distance matching an a pre-alignment-matching.
 *    The method for this is `tolerance(dbl)`.
 *
 *  - Maximum rotation: If your fiducial setup (for resign restriction reasons) ambiguous, e.g.
 *    rectangular fiducial setup, but you know that the target is placed better than 45°, you
 *    can exclude the redundant result by setting `max_rotation(PI/4.)`. It is also simply a
 *    good process error check, maybe if the device cannot rotate more than 45°.
 *
 *  - Maximum translation: The maximum allowed translation, mainly intended for process error
 *    checks - e.g. the if the alignment manipulators can only move 10mm, or the like. The
 *    setter for this parameter is `max_translation(dbl)`.
 *
 *  - Inverse results: Normally the result angle and shift is specified to move the target to
 *    the reference. Say `inverse(true)` if you like to move the reference to the target.
 *
 *  - Unique result: The class can raise an error if there is more than one possible result.
 *
 * Annotations:
 *
 *  - The file can be compiled with c++98 standard.
 *
 *  - If you indent to use long double as float_t template argument, double check the SINCOS
 *    macro (default sincos(x,s,c) for GNU or s=sin(x);c=cos(x)) if this function actually
 *    uses this accuracy.
 *
 *  - This class is not intended to be an exception source, errors are given as return values
 *    and via `error()` and `error_text()`. Only STL containers may throw in case of memory
 *    problems.
 *
 * -----------------------------------------------------------------------------
 *
 * Usage:
 *
 * int main()
 * {
 *    // Instantiate object and the point list objects
 *    sw::fiducial_alignment alignment;
 *    sw::fiducial_alignment::points_t ref_points, trg_points;
 *
 *    // Import from file or whatever
 *    read_file(...);
 *    for(...) { trg_points.push_back(...); }
 *    read_file(...);
 *    for(...) { ref_points.push_back(...); }
 *
 *    // Setup and calculate
 *    alignment
 *      .tolerance(double_maximum_ref_trg_point_deviation_for_initial_matching_tests)
 *      .max_rotation(double_maximum_rotation_angle_in_rad_to_accept_a_possible_result)
 *      .max_translation(double_maximum_allowed_translation_to_accept_a_possible_result)
 *      .inverse(bool_align_ref_to_trg___not_trg_to_ref)
 *      .unique(bool_accept_only_one_result_otherwise_throw_ambiguous_error)
 *      .reference(ref_points)
 *      .target(trg_points)
 *      .calculate();
 *
 *    // Retults
 *    if(alignment.error()) {
 *      cerr << "Error: " << alignment.error_text() << endl;
 *      return (int) alignment.error();
 *    } else {
 *      fiducial_alignment::alignments_t::const_iterator it;
 *      int i = 1;
 *      for(it = alignment.results().begin(); it != alignment.results().end(); ++it, ++i) {
 *        cout << "result " << i << ":" << endl
 *             << "rotation= " << it->rotation << endl;
 *             << "translation= [" << it->translation.x << "," << it->translation.y << "]" << endl;
 *             << "maximum error"= it->max_error << endl;
 *             << "agerave error"= it->avg_error << endl;
 *      }
 *    }
 *    return 0;
 *  }
 * -----------------------------------------------------------------------------
 * +++ BSD license header +++
 * Copyright (c) 2011-2014, Stefan Wilhelm (stfwi, <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 atwillys.de 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 FIDUCIAL_ALIGNMENT_HH
#define FIDUCIAL_ALIGNMENT_HH
 
// <editor-fold desc="preprocessor" defaultstate="collapsed">
#include <vector>
#include <string>
#include <sstream>
#include <iostream>
#include <iomanip>
#include <cmath>
#include <limits>
/* M_PI(l) not def everywhere */
#ifndef MM_PI
#define MM_PI ((float_t)3.141592653589793238462643383279502884L)
#endif
/* Compile time options */
#ifndef FIDUCIAL_EPSILON_T
#define FIDUCIAL_EPSILON_T 1e-9
#endif
#ifndef FIDUCIAL_EPSILON_A
#define FIDUCIAL_EPSILON_A 1e-12
#endif
#ifndef SINCOS
#ifdef _GNU_SOURCE
#define SINCOS(A,S,C) {sincos(A,S,C);}
#else
#define SINCOS(A,S,C) {(*(S))=sin(A);(*(C))=cos(A);}
#endif
#endif
#ifndef FIDUCIAL_NODEBUG
#define FIDUCIAL_VERBOSED(X) if(dump_stream_) { (*dump_stream_) << X; }
#define FIDUCIAL_VERBOSE(X) X
#else
#define FIDUCIAL_VERBOSED(X) {;}
#define FIDUCIAL_VERBOSE(X)
#endif
#define nl std::endl
// </editor-fold>
 
namespace sw { namespace detail {
 
template <typename Float_Type=double>
class basic_fiducial_alignment
{
public:
 
  // <editor-fold desc="types" defaultstate="collapsed">
  struct vect_t;
  typedef Float_Type float_t;
  typedef vect_t point_t;
  typedef std::vector<point_t> points_t;
 
  typedef enum {
    fa_ok=0, fa_no_values, fa_too_many_fiducials, fa_err_recalculate,
    fa_fiducial_distances_dont_match, fa_fiducials_dont_match, fa_fiducials_dont_match_criteria,
    fa_double_checked_alignment_missmatch, fa_ambiguous_matches
  } error_t;
 
private:
 
  typedef std::vector<float_t> fvect_t;
  typedef std::vector<fvect_t> ftab_t;
  // </editor-fold>
 
  // <editor-fold desc="vect_t" defaultstate="collapsed">
public:
 
  /**
   * Auxiliary vector, implemented in one block (nothing big happening here):
   */
  struct vect_t
  {
    float_t x, y;
    inline vect_t() : x(0), y(0) {;}
    inline vect_t(float_t x_, float_t y_) : x(x_), y(y_) {;}
    inline vect_t(const vect_t & p) : x(p.x), y(p.y) {;}
    virtual ~vect_t() {;}
    inline vect_t operator - () const { return vect_t(-x, -y); }
    inline vect_t operator + (const vect_t & p) const { return vect_t(x+p.x, y+p.y); }
    inline vect_t operator - (const vect_t & p) const { return vect_t(x-p.x, y-p.y); }
    inline vect_t operator * (float_t d) const { return vect_t(x*d, y*d); } // S-mult
    inline vect_t operator / (float_t d) const { return vect_t(x/d, y/d); } // reciproque S-mult
    inline vect_t & operator +=(const vect_t & p) { x+=p.x; y+=p.y; return *this; }
    inline vect_t & operator -=(const vect_t & p) { x-=p.x; y-=p.y; return *this; }
    inline vect_t & operator *= (float_t d)  { x*=d; y*=d; return *this; }
    inline vect_t & operator /= (float_t d)  { x/=d; y/=d; return *this; }
    inline float_t operator * (const vect_t & p) const { return x*p.x + y*p.y; } // scalar prod
    inline float_t norm() const { return sqrt(norm2()); }
    inline float_t norm2() const { return x*x+y*y; } // norm square
    inline float_t dist(const vect_t & p) const { return sqrt(dist2(p)); }
    inline float_t dist2(const vect_t & p) const { return vect_t(x-p.x, y-p.y).norm2(); }
    inline float_t angle() const { float_t a = x != 0. ? atan(y/x) : (y > 0 ? MM_PI/2. :
      (MM_PI*3./2.)); if(x<0.) a += MM_PI; if(a < 0.) a += MM_PI*2.; return a; }
    inline float_t angle1() const { float_t a = x != 0. ? (atan(y/x)/(MM_PI*2.)) :
      (y>0. ? 0.25 : 0.75); if(x<0.) a += .5; if(a < 0.) a += 1.; return a; } // normed to 1
    friend std::ostream & operator << (std::ostream & os, const vect_t & v)
    { os << "[" << v.x << "," << v.y << "]"; return os; }
    inline operator std::string () const
    { std::stringstream s; s << *this; return s.str(); }
  };
  // </editor-fold>
 
  // <editor-fold desc="alignment/alignments_t" defaultstate="collapsed">
  /**
   * Result / calculation intermediate type
   */
  struct alignment_t // used "_t" although no typedef, prevents other confusions
  {
    alignment_t() : max_error(0), avg_error(0), rotation(0), trg_origin(), used_refs(),
      used_trgs(), translation()
    {;}
 
    alignment_t(const alignment_t & e) : max_error(e.max_error), avg_error(e.avg_error),
      rotation(e.rotation), trg_origin(e.trg_origin), used_refs(e.used_refs),
      used_trgs(e.used_trgs), translation(e.translation)
    {;}
 
    virtual ~alignment_t()
    {;}
 
    float_t max_error, avg_error, rotation;
    point_t trg_origin;
    points_t used_refs, used_trgs;
    vect_t translation;
  } ;
 
  typedef std::vector<alignment_t> alignments_t;
  // </editor-fold>
 
public:
 
  // <editor-fold desc="constructors/destructor" defaultstate="collapsed">
  inline basic_fiducial_alignment() : error_(fa_ok), tolerance_(0), max_rotation_(0),
          max_translation_(0), inverse_(false), unique_(true)
  { ; }
 
  inline basic_fiducial_alignment(const basic_fiducial_alignment & fa) : error_(fa.error_),
          tolerance_(fa.tolerance_), max_rotation_(fa.max_rotation_),
          max_translation_(fa.max_translation_), inverse_(fa.inverse_), unique_(fa.unique_),
          ref_(fa.ref_), trg_(fa.trg_), ref_distances_(fa.ref_distances_),
          trg_distances_(fa.trg_distances_), distance_matches_(fa.distance_matches_),
          results_(fa.results_)
  { ; }
 
  virtual ~basic_fiducial_alignment()
  { ; }
  // </editor-fold>
 
public:
 
  // <editor-fold desc="getters/setters" defaultstate="collapsed">
  /**
   * Sets the stream for FIDUCIAL_VERBOSED outputs
   * @param std::ostream & stream
   */
  static void dump_stream(std::ostream & stream)
  { dump_stream_ = &stream; }
 
  /**
   * Reset the object
   */
  inline void clear()
  {
    ref_.clear(); trg_.clear(); ref_distances_.clear(); trg_distances_.clear();
    distance_matches_.clear(); results_.clear();
    error_ = 0;
    tolerance_ = max_rotation_ = max_translation_ = 0;
    inverse_ = false;
  }
 
  /**
   * Returns the current error code
   * @return error_t
   */
  inline error_t error() const throw()
  { return error_; }
 
  /**
   * Returns the text for the current error code
   * @return std::string
   */
  inline std::string error_text() const throw()
  {
    switch(error_) {
      case fa_ok: return "ok";
      case fa_no_values: return "No values";
      case fa_too_many_fiducials: return "Too many fiducials";
      case fa_err_recalculate: return "Calculation failed";
      case fa_fiducial_distances_dont_match: return "Fiducials do not_match (no distance match)";
      case fa_fiducials_dont_match: return "Fiducials do not_match";
      case fa_fiducials_dont_match_criteria: return "No fiducial matches the specified criteria";
      case fa_ambiguous_matches: return "Fiducial results are ambiguous";
      case fa_double_checked_alignment_missmatch: return "Alignment mismatch during double check (bug)";
      default: return "UNKNOWN/UNEXPECTED ERROR CODE";
    }
  }
 
  /**
   * Returns the matching tolerance
   * @return float_t
   */
  inline float_t tolerance() const throw()
  { return tolerance_; }
 
  /**
   * Sets the matching tolerance
   * @param float_t d
   * @return basic_fiducial_alignment &
   */
  inline basic_fiducial_alignment & tolerance(float_t d) throw()
  { tolerance_ = d < 0 ? 0 : d; return *this;}
 
  /**
   * Returns list of reference points
   * @return const points_t &
   */
  inline const points_t & reference() const throw()
  { return ref_; }
 
  /**
   * Sets the list of reference points
   * @param const points_t & pts
   * @return basic_fiducial_alignment &
   */
  inline basic_fiducial_alignment & reference(const points_t & pts)
  { ref_ = pts; return *this; }
 
  /**
   * Returns the target points
   * @return const points_t &
   */
  inline const points_t & target() const throw()
  { return trg_; }
 
  /**
   * Sets the list of target points
   * @param const points_t & pts
   * @return basic_fiducial_alignment &
   */
  inline basic_fiducial_alignment & target(const points_t & pts)
  { trg_ = pts; return *this; }
 
  /**
   * Returns the maximum accepted rotation.
   * @return float_t
   */
  inline float_t max_rotation() const throw()
  { return max_rotation_; }
 
  /**
   * Sets the maximum accepted rotation.
   * @param float_t r
   * @return basic_fiducial_alignment &
   */
  inline basic_fiducial_alignment & max_rotation(float_t r) throw()
  { max_rotation_ = r; return *this; }
 
  /**
   * Returns the maximum accepted translation.
   * @return float_t
   */
  inline float_t max_translation() const throw()
  { return max_translation_; }
 
  /**
   * Sets the maximum translation
   * @param float_t t
   * @return  basic_fiducial_alignment &
   */
  inline basic_fiducial_alignment & max_translation(float_t t) throw()
  { max_translation_ = t; return *this; }
 
  /**
   * Returns if the reference has to be aligned to the target, target to reference.
   * @return bool
   */
  inline bool inverse() const throw()
  { return inverse_; }
 
  /**
   * Sets if the reference has to be aligned to the target, target to reference.
   * @param bool b
   * @return basic_fiducial_alignment &
   */
  inline basic_fiducial_alignment & inverse(bool b) throw()
  { inverse_ = b; return *this; }
 
  /**
   * Returns if the result must be unique, means not more than one result must match.
   * @return bool
   */
  inline bool unique() const throw()
  { return unique_; }
 
  /**
   * Sets if the result must be unique, means not more than one result must match.
   * @param bool b
   * @return basic_fiducial_alignment &
   */
  inline basic_fiducial_alignment & unique(bool b) throw()
  { unique_ = b; return *this; }
 
  /**
   * Returns the list of results
   * @return const alignments_t &
   */
  inline const alignments_t & results() const throw()
  { return results_; }
 
  /**
   * Returns the list of results
   * @return alignments_t &
   */
  inline alignments_t & results() throw()
  { return results_; }
  // </editor-fold>
 
  // <editor-fold desc="calculate" defaultstate="collapsed">
  /**
   * Main calculation.
   * @return error_t
   */
  error_t calculate()
  {
    FIDUCIAL_VERBOSED(
         "Settings: { match tolerance: " << tolerance()
      << ", maximum translation: " << max_translation()
      << ", maximum rotation: " << (max_rotation() / MM_PI * 180.0) << "°"
      << ", (compiled) cartesian epsilon:" << FIDUCIAL_EPSILON_T
      << ", (compiled) angular epsilon:" << FIDUCIAL_EPSILON_A
      << " }" << nl
    );
    // Init, input error checks
    results_.clear();
    error_ = fa_err_recalculate;
    if(trg_.empty() || ref_.empty()) {
      error_ = fa_no_values;
      FIDUCIAL_VERBOSED("Error: " << error_text() << nl);
      return error_;
    }
    if(trg_.size() > ref_.size()) {
      error_ = fa_too_many_fiducials;
      FIDUCIAL_VERBOSED("Error: " << error_text() << nl);
      return error_;
    }
    fix0(trg_); fix0(ref_);
 
    FIDUCIAL_VERBOSED("Input reference points : " << ref_ << nl);
    FIDUCIAL_VERBOSED("Input target points    : " << trg_ << nl << nl);
    FIDUCIAL_VERBOSED("Calculate distance maps ... " << nl);
    calculate_distances(ref_, ref_distances_);
    calculate_distances(trg_, trg_distances_);
    FIDUCIAL_VERBOSED(" Reference distance map: " << dump(ref_distances_, " ") << nl);
    FIDUCIAL_VERBOSED(" Target distance map   : " << dump(trg_distances_, " ") << nl);
 
    // Auto settings
    float_t tolerance = tolerance_;
    {
      float_t min_dist = std::numeric_limits<float_t>::infinity();
      if(tolerance <= 0) {
        FIDUCIAL_VERBOSED(nl << "Auto determine tolerance ... " << nl);
        for(typename ftab_t::const_iterator rit = ref_distances_.begin();
            rit != ref_distances_.end(); ++rit) {
          for(typename fvect_t::const_iterator it = rit->begin(); it != rit->end(); ++it) {
            if(*it != 0. && min_dist > *it) {
              min_dist = *it;
            }
          }
        }
        // Reason: Tolerance must be smaller than min_dist/2. Technically an alignment
        //         makes no sense if the measurement error is higher than one percent
        //         of the maximum distance. As default we use 1/10 to leave some space
        //         for angular errors during the pre-alignment, and let the remaining
        //         error match decide later on which results are best.
        tolerance = min_dist / 10.0;
        FIDUCIAL_VERBOSED(" Minimum nonzero reference point distance: " << min_dist << nl);
        FIDUCIAL_VERBOSED(" Tolerance set to: " << tolerance << nl);
      }
    }
 
    // Compare the distance tables of references and targets to determine which target point
    // points MAY correspond to reference points. This pre-check reduces the number of
    // combinations that have to be iterated later on. The loop nesting has a complexity of 4,
    // but it is worth it.
    {
      FIDUCIAL_VERBOSED(nl << "Comparing distance tables ... " << nl);
      distance_matches_.resize(trg_.size());
      typename ftab_t::size_type r_i, f_i;
      for(f_i = 0; f_i < trg_distances_.size(); ++f_i) {
        for(r_i = 0; r_i < ref_distances_.size(); ++r_i) {
          unsigned n=0;
          fvect_t fv = trg_distances_[f_i];
          fvect_t rv = ref_distances_[r_i];
          for(typename fvect_t::iterator r_it=rv.begin(); r_it != rv.end(); ++r_it) {
            if(*r_it == 0.) continue; // distance == 0 --> ignored points
            for(typename fvect_t::iterator f_it = fv.begin(); f_it != fv.end(); ++f_it) {
              if(*f_it == 0.) continue;
              float_t d = *f_it - *r_it;
              if(d > -tolerance  && d < tolerance) {
                *f_it = *r_it = 0;
                ++n;
              }
            }
          }
          if(n >= trg_.size()-1) {
            distance_matches_[f_i].push_back(r_i);
          }
        }
      }
      while(!distance_matches_.empty() && distance_matches_.back().empty()) {
        distance_matches_.resize(distance_matches_.size()-1);
      }
      if(dump_stream_) {
        FIDUCIAL_VERBOSED("Distance matches: {" << nl);
        for(unsigned i=0; i<distance_matches_.size(); ++i) {
          FIDUCIAL_VERBOSED(" trg" << (int) i << ": ");
          for(unsigned j=0; j<distance_matches_[i].size(); ++j) {
            FIDUCIAL_VERBOSED("ref" << (int) distance_matches_[i][j] << " ");
          }
          FIDUCIAL_VERBOSED(nl);
        }
        FIDUCIAL_VERBOSED("}" << nl);
      }
      if(distance_matches_.size() < trg_.size()) {
        error_ = fa_fiducial_distances_dont_match;
        FIDUCIAL_VERBOSED("Error: " << error_text() << nl);
        return error_;
      }
    }
 
    {
      // Now we have a list of references for each fiducial. We select the two fiducials
      // with the least amount of possible reference possibilities - one "origin point list"
      // and one "alignment point list". If the setup/model of the fiducials is designed in
      // a nonambiguous way there is only one combination left after this - means we know
      // exactly which fiducial is which reference.
      FIDUCIAL_VERBOSED(nl << "Mapping possible references to targets ... " << nl);
      unsigned origins_index = 0, aligns_index = 0;
      unsigned min = ref_.size() + 1;
      float_t max_dist = 0;
      for(unsigned i=0; i<distance_matches_.size(); ++i) {
        if(distance_matches_[i].size() < min) {
          // Less possibilities to iterate later on
          min = distance_matches_[i].size();
          origins_index = i;
        }
        else if(distance_matches_[i].size() == min) {
          float_t dist = 0;
          for(typename fvect_t::const_iterator it = trg_distances_[i].begin();
            it != trg_distances_[i].end(); ++it) {
            if(*it > dist) dist = *it;
          }
          if(dist > max_dist) {
            // Replace because it contains a higher maximum distance (better polar pre-alignment)
            FIDUCIAL_VERBOSED(" replaced origins_index " << (int)origins_index << " with " << (int) i
                 << " because max distance " << max_dist << " < " << dist << nl);
            max_dist = dist;
            origins_index = i;
          }
        }
      }
      min = ref_.size() + 1;
      max_dist = 0;
      for(unsigned i=0; i<distance_matches_.size(); ++i) {
        if(i != origins_index) {
          if(distance_matches_[i].size() < min) {
            min = distance_matches_[i].size();
            aligns_index = i;
          } else if(distance_matches_[i].size() == min) {
            float_t dist = 0;
            for(typename fvect_t::const_iterator it = trg_distances_[i].begin();
              it != trg_distances_[i].end(); ++it) {
              if(*it > dist) dist = *it;
            }
            if(dist > max_dist) {
              // Replace because it contains a higher maximum distance (better polar pre-alignment)
              FIDUCIAL_VERBOSED(" replaced aligns_index " << (int)aligns_index << " with " << (int) i
                   << " because max distance " << max_dist << " < " << dist << nl);
              max_dist = dist;
              aligns_index = i;
            }
          }
        }
      }
      FIDUCIAL_VERBOSED(" Using trg" << (int)origins_index << " data for pre-aligning (origins)" << nl);
      FIDUCIAL_VERBOSED(" Using trg" << (int)aligns_index  << " data for pre-aligning (aligned)" << nl);
 
      // Check point cloud matches by aligning each origins point to each aligns point, save
      // only the index pairs of which all fiducial points match reference points.
      // This pre-alignment is explicitly in the coordinate system of the origin/align, so
      // the the maximum translation is only minor affected by the rotation (the calculated
      // translation would be higher when directly rotating around the global coordinate system
      // origin [0/0]). Later on the coordinate system is going to be changed.
      FIDUCIAL_VERBOSED(nl << "Pre-aligning ... " << nl);
      for(unsigned i=0; i<distance_matches_[origins_index].size(); ++i) {
        for(unsigned j=0; j<distance_matches_[aligns_index].size(); ++j) {
          if(distance_matches_[origins_index][i] != distance_matches_[aligns_index][j]) {
            vect_t translation;
            point_t trg0;
            float_t angle;
            if(prealign(
              ref_, trg_, tolerance ,
              ref_[distance_matches_[origins_index][i]],
              ref_[distance_matches_[aligns_index][j]],
              trg_[origins_index],
              trg_[aligns_index],
              translation, trg0, angle
            )) {
              alignment_t a;
              a.rotation = angle;
              a.translation = translation;
              a.trg_origin = trg0;
              results_.push_back(a);
            }
          }
        }
      }
      if(results_.empty()) {
        error_ = fa_fiducials_dont_match;
        FIDUCIAL_VERBOSED("Error: " << error_text() << nl);
        return error_;
      }
    }
 
    // Now we have the list of real possibilities how the target cloud could match the
    // reference cloud. All these data are principally equally valid. We filter according
    // to the max criteria given.
    if(max_rotation_ > 0 || max_translation_ > 0) {
      FIDUCIAL_VERBOSED(nl << "Max translation/rotation filter ... " << nl);
      FIDUCIAL_VERBOSE(int id = 0;)
      float_t max_t = max_translation_ * max_translation_;
      typename alignments_t::iterator it=results_.begin();
      while(it != results_.end()) {
        if(max_rotation_ > 0 && fabs(it->rotation) > max_rotation_) {
          FIDUCIAL_VERBOSED("   result " << (id++) << ": rotation too high: |" << (it->rotation * 180./MM_PI)
                  << "°| > " << (max_rotation_ * 180./MM_PI) << "°" << nl);
          it = results_.erase(it);
        } else if(max_t > 0 && it->translation.norm2() > max_t) {
          FIDUCIAL_VERBOSED("   result " << (id++) << ": translation too high: " << it->translation.norm2() << nl);
          it = results_.erase(it);
        } else {
          FIDUCIAL_VERBOSED("   result " << (id++) << ": ok" << nl);
          ++it;
        }
      }
    }
    if(results_.empty()) {
      error_ = fa_fiducials_dont_match_criteria;
      FIDUCIAL_VERBOSED("Error: " << error_text() << nl);
      return error_;
    }
 
    // Operations with the pre-detected/preprocessed results:
    int id = 0;
    for(typename alignments_t::iterator it = results_.begin(); it != results_.end(); ++it, ++id) {
      FIDUCIAL_VERBOSED(nl << "Result[" << id << "] ----------------" << nl);
      alignment_t & a = *it;
 
      // Rematch, assign the point correspondencies for targets --> refs.
      FIDUCIAL_VERBOSED(nl << " Point matching ... " << nl);
      if(!find_matches(ref_, trg_, tolerance, a)) {
        error_ = fa_double_checked_alignment_missmatch;
        FIDUCIAL_VERBOSED(" Error: " << error_text() << nl);
        return error_;
      }
 
      // Improve the accuracy (the real aligning step). Changes rotation origin to [0,0], too.
      FIDUCIAL_VERBOSED(nl << " Aligning ... " << nl);
      // This function must succeed, otherwise there is a bug in this code.
      if(!align(a)) {
        error_ = fa_double_checked_alignment_missmatch;
        FIDUCIAL_VERBOSED(" Error: " << error_text() << nl);
        return error_;
      }
 
      // Invert for invert() trg->ref --> ref->trg (added feature). Simply re-rotate and
      // rematch translation to the centeroid.
      if(inverse_) {
        FIDUCIAL_VERBOSED(nl << "Inverting results ... " << nl);
        points_t trgs = a.used_trgs;
        points_t refs = a.used_refs;
        rotate(refs, -a.rotation);
        point_t p = average(errors(refs, trgs));
        a.translation = p;
        a.rotation = -a.rotation;
      }
 
      // (8) Last rematch check to detect numerical errors and the like, add statistics
      {
        FIDUCIAL_VERBOSED(nl << " Result checks ... " << nl);
        points_t trgs = trg_;
        points_t refs = ref_;
        if(inverse_) {
          rotate(refs, a.trg_origin, a.rotation);
          translate(refs, a.translation);
        } else {
          rotate(trgs, a.trg_origin, a.rotation);
          translate(trgs, a.translation);
        }
        if(filter_matches(trgs, refs, tolerance) != trg_.size()) {
          FIDUCIAL_VERBOSED(" Error: !!! filter_matches() != targets.size()" << nl);
          return error_;
        }
        points_t errs = errors(refs, trgs);
        float_t max_err = 0;
        float_t avg_err = 0;
        for(typename points_t::const_iterator iit = errs.begin(); iit != errs.end(); ++iit) {
          float_t e = iit->norm();
          if(e > max_err) max_err = e;
          avg_err += e;
        }
        avg_err /= errs.size();
        a.max_error = max_err;
        a.avg_error = avg_err;
        FIDUCIAL_VERBOSED("   remaining errors  : " << errs << nl);
        FIDUCIAL_VERBOSED("   maximum error norm: " << max_err << nl;);
        FIDUCIAL_VERBOSED("   average error norm: " << avg_err << nl;);
      }
    }
 
    // Check if the result is not ambiguous
    if(unique_ && results_.size() > 1) {
      error_ = fa_ambiguous_matches;
      FIDUCIAL_VERBOSED(nl << error_text() << nl);
      return error_;
    }
 
    FIDUCIAL_VERBOSED(nl << "Done. No errors." << nl << nl);
    return error_ = fa_ok;
  }
  // </editor-fold>
 
private:
 
  // <editor-fold desc="calculation auxiliaries" defaultstate="collapsed">
  /**
   * Coordinate translation of all points in the list
   * @param points_t &pts
   * @param const vect_t & v
   * @return void
   */
  static inline void translate(points_t &pts, const vect_t & v)
  {
    for(typename points_t::iterator it=pts.begin(); it != pts.end(); ++it) *it += v;
  }
 
  /**
   * Rotation of all points in the list around the origin 0/0
   * @param points_t &pts
   * @param float_t angle
   * @return void
   */
  static inline void rotate(points_t &pts, float_t angle)
  {
    float_t sa, ca;
    SINCOS(angle, &sa, &ca);
    for(typename points_t::iterator it = pts.begin(); it != pts.end(); ++it) {
      *it = vect_t(it->x * ca - it->y * sa, it->x * sa + it->y * ca);
    }
  }
 
  /**
   * Rotation of all points in the list around the given origin point
   * @param points_t &pts
   * @param const point_t &origin
   * @param float_t angle
   * @return void
   */
  static inline void rotate(points_t &pts, const point_t &origin, float_t angle)
  { translate(pts, -origin); rotate(pts, angle); translate(pts, origin); }
 
  /**
   * Round values near zero to exactly zero.
   * @param points_t & pts
   */
  static inline void fix0(points_t & pts)
  {
    for(typename points_t::iterator it = pts.begin(); it != pts.end(); ++it) {
      if(it->x > -FIDUCIAL_EPSILON_T && it->x < FIDUCIAL_EPSILON_T) it->x = 0;
      if(it->y > -FIDUCIAL_EPSILON_T && it->y < FIDUCIAL_EPSILON_T) it->y = 0;
    }
  }
 
  /**
   * Wrap angle, so that it is -pi --> pi
   * @param float_t angle
   * @return float_t
   */
  static inline float_t fix_angle(float_t angle)
  {
    while(angle >  MM_PI) angle -= MM_PI * 2.0;
    while(angle < -MM_PI) angle += MM_PI * 2.0;
    if(angle > -FIDUCIAL_EPSILON_A && angle < FIDUCIAL_EPSILON_A) angle = 0;
    return angle;
  }
 
  /**
   * Coordinate translation of all points in the list
   * @param points_t &pts
   * @param const vect_t & v
   * @return void
   */
  static inline point_t average(const points_t &pts)
  {
    point_t p;
    if(pts.empty()) return p;
    for(typename points_t::const_iterator it=pts.begin(); it != pts.end(); ++it) p += *it;
    return p / (float_t) pts.size();
  }
 
  /**
   * Calculate error vectors. Point clouds must not be empty and have the same size.
   * @param const points_t & refs
   * @param const points_t & trgs
   * @return points_t
   */
  static inline points_t errors(const points_t & refs, const points_t & trgs)
  {
    points_t errs;
    errs.reserve(refs.size());
    typename points_t::const_iterator rit=refs.begin();
    typename points_t::const_iterator fit=trgs.begin();
    for(; rit != refs.end(); ++rit, ++fit) errs.push_back(*fit-*rit);
    return errs;
  }
 
  /**
   * Sum vector of vectors / points
   * @param const points_t & pts
   * @return vect_t
   */
  static inline vect_t sum(const points_t & pts)
  {
    vect_t s;
    for(typename points_t::const_iterator it=pts.begin(); it != pts.end(); ++it) s += *it;
    return s;
  }
 
  /**
   * Calculates distances from each point to all other points.
   * @param const points_t & pts
   * @param ftab_t & dist
   */
  static inline void calculate_distances(const points_t & pts, ftab_t & dist)
  {
    unsigned n_points = pts.size();
    dist.clear();
    dist.reserve(n_points);
    fvect_t row(n_points, 0.0);
    for(unsigned i=n_points; i>0; --i) {
      dist.push_back(row);
    }
    for(unsigned i=0; i<n_points; ++i) {
      for(unsigned j=i+1; j<n_points; ++j) {
        float_t d = pts[i].dist(pts[j]);
        if(d > -FIDUCIAL_EPSILON_T && d < FIDUCIAL_EPSILON_T) d = 0;
        dist[i][j] = dist[j][i] = d;
      }
    }
  }
 
  /**
   * Returns how many points of a list match points of another list respecting a given tolerance.
   * @param const points_t & pts1
   * @param const points_t & pts2
   * @param float_t tolerance
   * @return unsigned
   */
  static inline unsigned num_matches(const points_t & pts1, const points_t & pts2, float_t tolerance)
  {
    unsigned n_matches = 0;
    tolerance *= tolerance;
    for(typename points_t::const_iterator it1 = pts1.begin(); it1 != pts1.end(); ++it1) {
      for(typename points_t::const_iterator it2 = pts2.begin(); it2 != pts2.end(); ++it2) {
        if(it1->dist2(*it2) < tolerance) ++n_matches;
      }
    }
    return n_matches;
  }
 
  /**
   * Filters out points that don't match. Implicitly sorts the points so that all indices of
   * matching points in both vectors are identical. Returns the number of matched points.
   * @param points_t & trgs
   * @param points_t & pts2
   * @param float_t tolerance
   * @return unsigned
   */
  static inline unsigned filter_matches(points_t & trgs, points_t & refs, float_t tolerance)
  {
    if(!trgs.size() || !refs.size()) return 0;
    unsigned n_matches = 0;
    tolerance *= tolerance;
    points_t tpts1, tpts2;
    tpts1.swap(trgs);
    tpts2.swap(refs);
    trgs.reserve((tpts1.size() < tpts2.size()) ? tpts1.size() : tpts2.size());
    refs.reserve(trgs.size());
    typename points_t::iterator it1 = tpts1.begin();
    while(!tpts1.empty() && it1 != tpts1.end()) {
      bool next = false;
      for(typename points_t::iterator it2 = tpts2.begin(); it2 != tpts2.end(); ++it2) {
        if(it1->dist2(*it2) < tolerance) {
          ++n_matches;
          trgs.push_back(*it1);
          refs.push_back(*it2);
          it2 = tpts2.erase(it2);
          next = true;
          break;
        }
      }
      if(next) {
        it1 = tpts1.erase(it1);
      } else {
        ++it1;
      }
    }
    return n_matches;
  }
 
  /**
   * Transform targets to references, check matches, save trgs-->refs mapping.
   * @param const points_t & references
   * @param const points_t & targets
   * @param float_t tolerance
   * @param alignment_t & alignment
   * @return bool
   */
  static inline bool find_matches(const points_t & references, const points_t & targets,
    float_t tolerance, alignment_t & alignment)
  {
    points_t trgs = targets;
    points_t refs = references;
    rotate(trgs, alignment.trg_origin, alignment.rotation);
    translate(trgs, alignment.translation);
    // That was already checked, and there is a bug if the number of matches differ now.
    if(filter_matches(trgs, refs, tolerance) != targets.size()) {
      FIDUCIAL_VERBOSED(" find_matches: !!! filter_matches() != targets.size()" << nl);
      return false;
    }
    alignment.used_refs = refs;
    alignment.used_trgs = targets;
    return true;
  }
 
  /**
   * Aligns a reference/target pair, double checks distance match, checks if all target
   * match a reference and assigns the result reference& arguments. Returns true on success,
   * false if the points don't match.
   *
   * @param const points_t & references
   * @param const points_t & targets
   * @param float_t tolerance
   * @param const point_t & ref0
   * @param const point_t & ref1
   * @param const point_t & fid0
   * @param const point_t & fid1
   * @param vect_t & translation
   * @param float_t & angle
   * @return bool
   */
  static inline bool prealign(const points_t & references, const points_t & targets, float_t tolerance,
    const point_t & ref0, const point_t & ref1, const point_t & fid0, const point_t & fid1,
    vect_t & translation, point_t & rotation_origin, float_t & rotation)
  {
    points_t points = targets;
    float_t ref_angle = (ref1-ref0).angle();
    float_t fid_angle = (fid1-fid0).angle();
    rotation = fix_angle(ref_angle - fid_angle);
    rotation_origin = fid0;
    translate(points, -rotation_origin);
    rotate(points, rotation);
    translate(points, ref0);
    translation = ref0 - fid0;
    unsigned n_matches = num_matches(points, references, tolerance);
    FIDUCIAL_VERBOSED(
      " Pre-alignment for refs(" << ref0 << ", " << ref1 << "), trgs(" << fid0 << ", " << fid1 << "):"
      << nl << "   translation: " << translation << ", rotation: " << (rotation*180.0/MM_PI) << "°"
      << ", rotation origin: " << rotation_origin
      << ", number of point matches: " << (int) n_matches  << nl
      << "   aligned target points : " << points << nl
    );
    return n_matches == targets.size();
  }
  // </editor-fold>
 
  // <editor-fold desc="align" defaultstate="collapsed">
  /**
   * Result correction.
   * @param const alignment_t & data
   * @return bool
   */
  static inline bool align(alignment_t & alignment)
  {
    points_t trgs = alignment.used_trgs;
    points_t refs = alignment.used_refs;
 
    // After double checking translate both refs and trgs so that the rotation origin is 0/0.
    {
      rotate(trgs, alignment.trg_origin, alignment.rotation);
      translate(trgs, -alignment.trg_origin);
      translate(refs, -alignment.trg_origin - alignment.translation);
      fix0(trgs);
      fix0(refs);
    }
    FIDUCIAL_VERBOSED("   targets (pre-aligned) : " << trgs << nl);
    FIDUCIAL_VERBOSED("   references            : " << refs << nl);
 
    // Rotation. A weighted average rotation correction over all reference/fiducial vectors.
    // Weights depend on the distance between the points. All is normed to the maximum distance.
    {
      fvect_t errors;
      fvect_t weights;
      float_t sum_len = 0;
      unsigned i;
      if(!refs.size()) return false; // Should never be
      errors.reserve(refs.size() * 2);
      weights.reserve(refs.size() * 2);
      for(i = 0; i < refs.size(); ++i) {
        if(refs[i].x == 0 && refs[i].y == 0) break;
      }
      if(i >= refs.size()) {
        FIDUCIAL_VERBOSED("   BUG! Failed to find previously shifted reference point 0/0." << nl);
        return false;
      }
      for(unsigned j = i+1; j < refs.size(); ++j) {
        float_t ref_angle = (refs[j]-refs[i]).angle();
        float_t fid_angle = (trgs[j]-trgs[i]).angle();
        float_t err = fid_angle - ref_angle;
        float_t len = (refs[j]-refs[i]).norm();
        errors.push_back(err);
        weights.push_back(len);
        sum_len += len;
      }
      for(typename fvect_t::iterator it = weights.begin(); it != weights.end(); ++it) {
        *it /= sum_len;
      }
      float_t weighted_error = 0;
      typename fvect_t::iterator wit = weights.begin();
      typename fvect_t::iterator eit = errors.begin();
      for(; wit != weights.end(); ++wit, ++eit) weighted_error += (*wit) * (*eit);
      alignment.rotation -= weighted_error;
      FIDUCIAL_VERBOSED("   rotation error angles : " << dump(errors) << nl);
FIDUCIAL_VERBOSED("   !!!!!!!: " << nl);
      FIDUCIAL_VERBOSED("   rotation error weights: " << dump(weights) << nl);
      FIDUCIAL_VERBOSED("   weighted angle error  : " << weighted_error * (180./MM_PI) << "°" << nl);
    }
 
    // Rotation origin [0,0] transformation
    {
      points_t rpts;
      rpts.push_back(alignment.trg_origin);
      rotate(rpts, alignment.rotation);
      alignment.translation += (alignment.trg_origin - rpts.front());
      alignment.trg_origin = point_t();
    }
 
    // Translation
    {
      points_t errs = errors(refs, trgs);
      point_t p = average(errs);
      alignment.translation -= p;
      translate(refs, p);
      errs = errors(refs, trgs);
      point_t p2 = average(errs);
      FIDUCIAL_VERBOSED("   transl error vects    : " << errs << nl);
      FIDUCIAL_VERBOSED("   transl error averg    : " << p << nl);
      FIDUCIAL_VERBOSED("   transl error total    : " << p.norm2() << nl);
      FIDUCIAL_VERBOSED("   transl error remaining: " << p2.norm2() << nl);
    }
    return true;
  }
  // </editor-fold>
 
  // <editor-fold desc="dump/stream operators" defaultstate="collapsed">
  static std::string dump(const fvect_t & values)
  {
    std::stringstream os;
    if(values.empty()) return std::string("[]");
    typename fvect_t::const_iterator it = values.begin();
    os << "[ " << (*it);
    for(++it; it != values.end(); ++it) os << ", " << *it;
    os << " ]";
    return os.str();
  }
 
  static std::string dump(const ftab_t & tab, const char* indent="")
  {
    if(!indent) indent = "";
    std::stringstream os;
    os << "[[" << nl;
    for(typename ftab_t::const_iterator it=tab.begin(); it != tab.end(); ++it) {
      os << indent << " ";
      for(typename fvect_t::const_iterator iit=it->begin(); iit != it->end(); ++iit) {
        os << std::setprecision(5) << *iit << " ";
      }
      os << nl;
    }
    os << indent << "]]";
    return os.str();
  }
 
  friend std::ostream & operator << (std::ostream & os, const points_t & pts)
  {
    os << "[ ";
    typename points_t::const_iterator it = pts.begin();
    os << *it;
    for(++it; it != pts.end(); ++it) os << ", " << *it;
    os << " ]";
    return os;
  }
  // </editor-fold>
 
private:
 
  // <editor-fold desc="instance variables" defaultstate="collapsed">
  error_t error_;         // Error code
  float_t tolerance_;     // Tolerance of the fiducials / measurement. Used for matching.
  float_t max_rotation_;  // Maximum accepted rotation
  float_t max_translation_; // Maximum accepted translation
  bool inverse_;          // Align ref to trg, not trg to ref
  bool unique_;           // true: raise error if there is more than one possible result.
  points_t ref_;          // Reference point cloud, can be reused for multiple
  points_t trg_;          // Measured input point cloud
  ftab_t ref_distances_;  // Distances from each point to all other points
  ftab_t trg_distances_;  // Distances from each point to all other points
  std::vector<std::vector<unsigned> > distance_matches_; // Ref/trg indices that may correspond
  alignments_t results_;  // List of results.
  static std::ostream *dump_stream_; // Stream pointer for verbosity
  // </editor-fold>
};
 
// <editor-fold desc="statics" defaultstate="collapsed">
template <typename float_t>
std::ostream * basic_fiducial_alignment<float_t>::dump_stream_ = 0;
// </editor-fold>
 
}}
 
// <editor-fold desc="specialisation" defaultstate="collapsed">
namespace sw {
  typedef detail::basic_fiducial_alignment<> fiducial_alignment;
}
// </editor-fold>
 
// <editor-fold desc="undefs" defaultstate="collapsed">
#undef MM_PI
#undef nl
#undef FIDUCIAL_VERBOSED
// </editor-fold>
 
#endif