Hi. I am trying to implement the circle fitting algorithm shown here. This post is similar to this topic but I am trying the Matrix approach.
I have two problems in my implementation:
The first problem is that I can’t get the intersection of the spheres working as described in the paper. The normal works but the center and radius don’t. This must be a bug somewhere in my code but I can’t find it. I implemented a working sphere intersection as described here (see also here) but I would like to know my error in the geometric algebra approach.
The second problem is that I can find many cases where the normal calculation fails as well. I noticed that two eigenvalues are the same (resulting in the same spheres). This problem greatly improves when adding noise to the original points.
In my implementation I only consider the real part of the eigenvalues / eigenvectors. However I found cases with imaginary eigenvalues / eigenvectors. How should I consider imaginary values?
Here is my code:
#include <Eigen/Core>
#include <Eigen/Eigenvalues>
#include <Eigen/Geometry>
#include <iostream>
#include <iomanip>
struct Circle
{
Eigen::Vector3d center;
Eigen::Vector3d normal;
double radius = -1;
// Changes circle representation from {center, normal, radius} to {pose, radius}.
// - The circle is defined in its local coordinate system in the x-y plane, with the normal aligned with the z axis, the center in the origin and the radius as it is
// - Applying the pose to points -- sampled on a circle in this local coordinate system -- moves those points onto the actual circle
// - The x-axis of the pose will point towards the point 'align_x_point'
//
// I use this for
// - Creating point samples for this test
// - Find circle coordinate system for my application (not shown here)
bool poseAlignedWithPoint(const Eigen::Vector3d& align_x_point,
Eigen::Isometry3d& pose) const
{
// This implementation is similar to gluLookAt, but with different axes 🪓
Eigen::Vector3d x = align_x_point - center;
const double norm_x = x.norm();
if (norm_x == 0)
{
std::cerr << "align_x_point and center must be different points\n";
return false;
}
x /= norm_x;
const double norm_z = normal.norm();
if (norm_z == 0)
{
std::cerr << "Invalid circle normal\n";
return false;
}
const Eigen::Vector3d z = normal / norm_z;
const Eigen::Vector3d y = z.cross(x).normalized();
// x and z don't necessarily have to be orthogonal. Make them orthogonal now.
x = y.cross(z).normalized();
Eigen::Matrix4d result = Eigen::Matrix4d::Identity();
result.block<3, 1>(0, 0) = x;
result.block<3, 1>(0, 1) = y;
result.block<3, 1>(0, 2) = z;
result.block<3, 1>(0, 3) = center;
pose = Eigen::Isometry3d(result);
return true;
}
};
std::ostream& operator<<(std::ostream& os, const Circle& circle)
{
return os << "[center ["
<< std::setw(12) << circle.center.x() << " "
<< std::setw(12) << circle.center.y() << " "
<< std::setw(12) << circle.center.z() << "]"
<< " normal ["
<< std::setw(12) << circle.normal.x() << " "
<< std::setw(12) << circle.normal.y() << " "
<< std::setw(12) << circle.normal.z() << "]"
<< " radius " << circle.radius << "]";
}
struct Sphere
{
Eigen::Vector3d center;
double radius = -1;
};
std::ostream& operator<<(std::ostream& os, const Sphere& sphere)
{
return os << "[center ["
<< std::setw(12) << sphere.center.x() << " "
<< std::setw(12) << sphere.center.y() << " "
<< std::setw(12) << sphere.center.z() << "]"
<< " radius " << sphere.radius << "]";
}
////////////////////////////////////////////////////////////////////////////////
//
// For fitCircle: The references are according to the paper
// - "Leo Dorst: Total Least Squares Fitting of k-Spheres in n-D Euclidean Space Using an (n+2)-D Isometric Representation"
// - https://www.researchgate.net/publication/266149530
//
// See also
// - https://www.ime.unicamp.br/~agacse2018/spheresAGACSE218.pdf
// - https://youtu.be/9YR3WK2Rqto?t=830
//
////////////////////////////////////////////////////////////////////////////////
using Vector5d = Eigen::Matrix<double, 5, 1>;
using Vector10d = Eigen::Matrix<double, 10, 1>;
using Matrix5d = Eigen::Matrix<double, 5, 5>;
using Matrix5Xd = Eigen::Matrix<double, 5, Eigen::Dynamic>;
using Matrix10x5d = Eigen::Matrix<double, 10, 5>;
using EigenSolver = Eigen::EigenSolver<Matrix5d>;
// Outer product in matrix form
// 5.4.2 on page 14
//
// [y^x] is the cross product matrix (skew symmetric matrix)
// 5.4.1 on page 14
// https://en.wikipedia.org/wiki/Skew-symmetric_matrix#Cross_product
//
// [1] is the 3x3 identity matrix
// 5.4.1 on page 14
Vector10d outerProduct(const Vector5d& y, const Vector5d& x)
{
Matrix10x5d Y;
Y << 0, -y(2), y(1), 0, 0,
y(2), 0, -y(0), 0, 0,
-y(1), y(0), 0, 0, 0,
y(3), 0, 0, -y(0), 0,
0, y(3), 0, -y(1), 0,
0, 0, y(3), -y(2), 0,
-y(4), 0, 0, 0, y(0),
0, -y(4), 0, 0, y(1),
0, 0, -y(4), 0, y(2),
0, 0, 0, -y(4), y(3);
return Y * x;
}
// Extract sphere parameters from eigenvector (xyz is the sphere center and w is the sphere radius)
// 3.3, 3 on page 8
Sphere getSphere(const Vector5d& x)
{
const Vector5d normalized = x.array() / x(3);
Sphere sphere;
sphere.center = normalized.head<3>();
sphere.radius = std::sqrt(sphere.center.squaredNorm() - 2 * normalized(4));
return sphere;
}
// Fits a circle through points in 3D
bool fitCircle(const Eigen::MatrixX3d& points, Circle& circle)
{
if (points.rows() < 4)
{
std::cerr << "Not enough points to fit circle " << points.rows() << " < 4\n";
return false;
}
const Eigen::Index N = points.rows();
// Data matrix (3.3, 1 on page 8)
Matrix5Xd D(5, N);
D.topRows<3>() = points.transpose();
D.row(3).setOnes();
D.row(4) = points.transpose().colwise().squaredNorm() / 2;
// Indefinite metric matrix of CGA (3.3 on page 8)
Matrix5d M;
M << 1, 0, 0, 0, 0,
0, 1, 0, 0, 0,
0, 0, 1, 0, 0,
0, 0, 0, 0, -1,
0, 0, 0, -1, 0;
// eq (24) on page 8
const Matrix5d P = (D * D.transpose() * M).array() / static_cast<double>(N);
// 3.3, 2 on page 8
const EigenSolver solver(P);
const Vector5d& eigenvalues = solver.eigenvalues().real();
const Matrix5d& eigenvectors = solver.eigenvectors().real();
std::cerr << "eigenvalues^T\n";
std::cerr << solver.eigenvalues().transpose() << "\n\n";
std::cerr << "eigenvectors\n";
std::cerr << solver.eigenvectors() << "\n\n";
// Select two best fitting spheres: minimum non-negative eigenvalue & eigenvector
Eigen::Index min_ind[2] = {eigenvalues.rows(), eigenvalues.rows()}; // Initialize with out-of-bound index
double min_val[2] = {std::numeric_limits<double>::max(), std::numeric_limits<double>::max()};
for (uint32_t i = 0; i < 2; ++i)
{
for (Eigen::Index row = 0; row < eigenvalues.rows(); ++row)
{
if (row != min_ind[0])
{
const double val = eigenvalues(row);
if (val >= 0 && val < min_val[i])
{
min_ind[i] = row;
min_val[i] = val;
}
}
}
}
if (min_ind[0] >= 5 || min_ind[1] >= 5)
{
std::cerr << "fitCircle failed [" << min_ind[0] << " || " << min_ind[1] << "] >= 5\n";
return false;
}
// Best fitting spheres
const Vector5d x1 = eigenvectors.col(min_ind[0]);
const Vector5d x2 = eigenvectors.col(min_ind[1]);
std::cerr << "Sphere 1 (index " << min_ind[0] << ") = " << getSphere(x1) << "\n";
std::cerr << "Sphere 2 (index " << min_ind[1] << ") = " << getSphere(x2) << "\n\n";
// Extract circle parameters
// Circle is the intersection of the 2 best-fitting spheres,
// with the 10-D bivector basis {e23, e31, e12 | eo1, eo2, eo3 | e1inf e2inf e3inf | eoinf}
// 5.4.1, page 14 and comments on eq (31)
//
// TODO: Doesn't work right now (This works https://mathworld.wolfram.com/Sphere-SphereIntersection.html)
const Vector10d outer = outerProduct(x1, x2);
const Eigen::Vector3d& neg_alpha_n = outer.segment<3>(3);
const double alpha = neg_alpha_n.norm();
circle.normal = -neg_alpha_n / alpha;
const double B0 = outer(9);
const double B1 = outer(0);
const double B2 = outer(1);
const double B3 = outer(2);
Eigen::Matrix3d B;
B << B0, -B3, B2,
B3, B0, -B1,
-B2, B1, B0;
circle.center = (B * circle.normal) / circle.normal.squaredNorm(); // eq (33). TODO: Why divide through normal.squaredNorm() == 1?
const Eigen::Vector3d& v_inf = outer.segment<3>(6);
const double cn = circle.center.dot(circle.normal);
circle.radius = std::sqrt(circle.center.squaredNorm() - 2 * circle.normal.dot(v_inf) / alpha - 2 * cn * cn);
return true;
}
////////////////////////////////////////////////////////////////////////////////
void testFitCircle(const Eigen::Vector3d& center,
const Eigen::Vector3d& normal,
const double radius,
const int n_samples,
const bool add_noise,
const double noise)
{
Circle original_circle;
original_circle.center = center;
original_circle.normal = normal;
original_circle.radius = radius;
// Sample points on circle in x-y plane
Eigen::MatrixX3d original_samples = Eigen::MatrixX3d(n_samples, 3);
for (int i = 0; i < n_samples; ++i)
{
const double alpha = static_cast<float>(i) / static_cast<float>(n_samples) * 2 * M_PI;
original_samples.row(i) = original_circle.radius * Eigen::Vector3d(std::cos(alpha),
std::sin(alpha),
0);
}
if (add_noise)
{
original_samples += static_cast<double>(noise) * Eigen::MatrixX3d::Random(n_samples, 3);
}
// Align points in the x-y plane with the actual circle
Eigen::Isometry3d original_pose;
Eigen::Vector3d align_x_point = Eigen::Vector3d::Zero();
if (!original_circle.poseAlignedWithPoint(align_x_point, original_pose))
{
return;
}
original_samples = (original_pose * original_samples.transpose()).transpose();
// Fit
std::cerr << "--------------------------------------------------------------------------------\n";
std::cerr << "Fitting circle through " << n_samples << " samples";
if (add_noise)
{
std::cerr << ", adding noise " << noise;
}
std::cerr << ":\n" << original_samples.transpose() << "\n\n";
Circle fitted_circle;
if (!fitCircle(original_samples, fitted_circle))
{
return;
}
std::cerr << "Original circle " << original_circle << "\n";
std::cerr << "Fitted circle " << fitted_circle << "\n\n";
}
int main()
{
// Normal is correct and everything would work if using https://mathworld.wolfram.com/Sphere-SphereIntersection.html
testFitCircle(Eigen::Vector3d(1, 2, -5),
Eigen::Vector3d(1, 1.5, 2).normalized(),
0.9,
11,
false,
0.1);
// Normal is incorrect as well
// First sphere is fine but second sphere has really big radius
testFitCircle(Eigen::Vector3d(1.3, 2, -5),
Eigen::Vector3d(1, 1.5, 2).normalized(),
0.9,
11,
false,
0.1);
// Results in complex eigenvalues / vectors.
// Also chosen eigenvalues result in the same sphere
testFitCircle(Eigen::Vector3d(1, 2, -5),
Eigen::Vector3d(1, 1.5, 2).normalized(),
1.75,
11,
false,
0.1);
return 0;
}
And this is the output of the program:
--------------------------------------------------------------------------------
Fitting circle through 11 samples:
0.671261 1.13879 1.56226 1.80721 1.79588 1.53186 1.09898 0.634674 0.286355 0.164613 0.308101
1.41695 1.25669 1.33242 1.62011 2.0284 2.42768 2.69118 2.73523 2.54585 2.18316 1.76233
-4.39835 -4.51191 -4.78045 -5.11869 -5.41924 -5.58669 -5.56787 -5.36876 -5.05256 -4.71968 -4.4758
eigenvalues^T
(-0.81,0) (0.405,0) (0.405,0) (3.35024e-13,0) (2.55026e-15,0)
eigenvectors
(-0.061049,0) (-0.0475146,0) (0.730608,0) (-0.0655981,0) (-0.510396,0)
(-0.122098,0) (-0.141551,0) (-0.343841,0) (-0.130625,0) (-0.841765,0)
(0.305245,0) (0.12992,0) (-0.107423,0) (0.319987,0) (0.0456009,0)
(-0.061049,0) (-2.9814e-15,0) (-2.70384e-09,0) (-0.0644547,0) (-0.152342,0)
(-0.940459,0) (-0.980218,0) (0.58004,0) (-0.933857,0) (-0.0751043,0)
Sphere 1 (index 4) = [center [ 3.35033 5.5255 -0.299333] radius 6.39214]
Sphere 2 (index 3) = [center [ 1.01774 2.02661 -4.96452] radius 0.901267]
Original circle [center [ 1 2 -5] normal [ 0.371391 0.557086 0.742781] radius 0.9]
Fitted circle [center [ 0.163748 0.276457 -0.104203] normal [ 0.371391 0.557086 0.742781] radius nan]
--------------------------------------------------------------------------------
Fitting circle through 11 samples:
0.933244 1.3974 1.83063 2.0954 2.10762 1.86344 1.44036 0.972725 0.608994 0.464653 0.585528
1.44108 1.26153 1.31645 1.58838 1.99101 2.39649 2.67608 2.74103 2.5707 2.21918 1.79807
-4.39743 -4.49485 -4.75265 -5.08899 -5.39707 -5.57908 -5.57724 -5.39213 -5.08252 -4.74671 -4.49132
eigenvalues^T
(-0.81,0) (0.405,0) (0.405,0) (3.46434e-13,0) (-1.87525e-15,0)
eigenvectors
(-0.0777242,0) (-0.714297,0) (-0.0551069,0) (-0.0815025,0) (-0.528214,0)
(-0.119576,0) (0.369647,0) (-0.134204,0) (-0.125386,0) (-0.798531,0)
(0.298939,0) (0.0799133,0) (0.128206,0) (0.313166,0) (-0.112542,0)
(-0.0597879,0) (2.68278e-09,0) (-2.16097e-15,0) (-0.0626541,0) (-0.124196,0)
(-0.941659,0) (-0.588858,0) (-0.981079,0) (-0.935755,0) (0.235049,0)
Sphere 1 (index 3) = [center [ 1.30083 2.00125 -4.99834] radius 0.900003]
Sphere 2 (index 1) = [center [-2.66252e+08 1.37785e+08 2.97874e+07] radius 3.01268e+08]
Original circle [center [ 1.3 2 -5] normal [ 0.371391 0.557086 0.742781] radius 0.9]
Fitted circle [center [ 0.000660719 0.135089 -0.245816] normal [ -0.883773 0.45735 0.0988737] radius nan]
--------------------------------------------------------------------------------
Fitting circle through 11 samples:
0.360786 1.26988 2.09329 2.56958 2.54755 2.03418 1.19246 0.289645 -0.387643 -0.624364 -0.34536
0.866299 0.554676 0.701933 1.26132 2.05523 2.8316 3.34395 3.4296 3.06137 2.35615 1.53786
-3.83012 -4.05095 -4.57309 -5.23078 -5.8152 -6.14079 -6.1042 -5.71703 -5.1022 -4.45493 -3.98072
eigenvalues^T
(-3.0625,0) (1.53125,0) (1.53125,0) (2.12778e-15,1.57176e-14) (2.12778e-15,-1.57176e-14)
eigenvectors
(-0.0573273,0) (-0.730608,0) (-0.0475146,0) (0.0580404,-0.0874084) (0.0580404,0.0874084)
(-0.114655,0) (0.343841,0) (-0.141551,0) (0.118844,-0.149754) (0.118844,0.149754)
(0.286637,0) (0.107423,0) (0.12992,0) (-0.328881,0.0861598) (-0.328881,-0.0861598)
(-0.0573273,0) (1.39055e-09,0) (-1.12417e-15,0) (0.0635659,-0.0372824) (0.0635659,0.0372824)
(-0.947692,0) (-0.58004,0) (-0.980218,0) (0.889307,-0.201391) (0.889307,0.201391)
Sphere 1 (index 3) = [center [ 0.913074 1.86961 -5.17385] radius 1.76558]
Sphere 2 (index 4) = [center [ 0.913074 1.86961 -5.17385] radius 1.76558]
Original circle [center [ 1 2 -5] normal [ 0.371391 0.557086 0.742781] radius 1.75]
Fitted circle [center [ 4.56673e-19 -1.15292e-18 2.94017e-18] normal [ -0.352206 0.888888 0.292966] radius nan]