一种数值稳定的改进最小二乘椭圆拟合方法，附完整cpp代码

写在前面

最近有需求要进行椭圆拟合，调研了一下，找到的很多内容，都是使用 最小二乘法，实现语言多为Matlab，在其基础上，增加一些条件实现

1. 椭圆 —— 从理论推导到最小二乘法拟合 https://blog.youkuaiyun.com/zh471021698/article/details/108812050
2. 椭圆拟合理论推导和Matlab实现 https://blog.youkuaiyun.com/LLeetion/article/details/113091785

但在测试时，发现会出现 特征值 极小接近 0 或者很小的负数 的问题（当数据点 精确 分布在椭圆上），造成无法求解的情况；然后找到了一种稳定的实现，即使数据点 分布 在椭圆上 也可以稳定求解。

来自《NUMERICALLY STABLE DIRECT LEAST SQUARES FITTING OF ELLIPSES》这篇论文提出的方法

这个方法 在最小二乘法的基础上，对矩阵进行分析后【约束矩阵等存在着 奇异的情况】，进行分解，简化寻找特征值，能够保证数值稳定求解

下面的数学理解可能有点困难，可以看原始论文；文章最后提供了cpp代码实现，支持pcd点云(使用PCA处理后)读取，txt读取，并与Python库进行了对比，测试精度能保证一致。

点云的PCA处理，在之前的博客上有写，可以参考。

如有问题，需要帮助，欢迎留言、私信或加群 交流【群号：392784757】

椭圆拟合

椭圆是一般二次曲线的一种特殊情况，使用隐式二阶多项式描述
$$F(x,y)=a{x}^2+bxy+c{y}^2+dx+ey+f$$
针对椭圆存在约束
$$b^2-4ac<0\text{\hspace{1em}(2)}$$
变量声明【向量式表达】
$$\begin{gathered} \text{a}=[a,b,c,d,e{]}^T \\ \text{x}=[x^2,xy,y^2,x,y,1]\text{\hspace{1em}(3)} \end{gathered}$$

$$F_a(\text{x})=\text{x}\cdot \text{a}=0$$

最小二乘法使用，系数a表示的点 到 圆锥曲线 的代数距离的平方和 来逼近

$$\begin{gathered} \underset{a}{\min }\sum _{i=1}^{N}F(x_i,y_i{)}^2=\underset{a}{\min }\sum _{i=1}^N(F_a(x_i){)}^2 \\ =\underset{a}{\min }\sum _{i=1}^N(F_a(x_i\cdot a){)}^2 \end{gathered}$$

这里论文中指出，这种拟合的结果是一般的二次曲线，而不一定是椭圆

为了保证解 椭圆特异性，需要考虑适当的约束 Eq 2

当a取任何不为0的值， α≠0 时， α⋅a 代表了相同的圆锥曲线。在一个合适的尺度下，不等式约束可以转化为等式约束

$$4AC-B^2=1\text{\hspace{1em}(6)}$$

针对 椭圆拟合问题重新公式化
$$\begin{gathered} \underset{a}{\min }||D\text{a}|{|}^2 \\ {\text{a}}^T\text{C}\text{a}=1\text{\hspace{1em}(7)} \end{gathered}$$

$$D=\left[\begin{array}{cccccc}{x}_1^2& x_1{y}_1& y_1^2& x_1& y_1& 1\\ ⋮& ⋮& ⋮& ⋮& ⋮& ⋮\\ x_i^2& x_i{y}_i& y_i^2& x_i& y_i& 1\\ ⋮& ⋮& ⋮& ⋮& ⋮& ⋮\\ x_N^2& x_N{y}_N& y_N^2& x_N& y_N& 1\end{array}\right]\text{\hspace{1em}(8)}$$

$$C=\left[\begin{array}{cccccc}0& 0& 2& 0& 0& 0\\ 0& -1& 0& 0& 0& 0\\ 2& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\end{array}\right]\text{\hspace{1em}(9)}$$

将等约束： $4AC-B^2=1$ 写成矩阵形式: $a^TCa=1 $

下面对 $\min ||D\text{a}|{|}^2$ 问题进行分解： $\min ||D\text{a}|{|}^2=D\text{a}{\text{a}}^T{D}^T$ 在 $\textbf{a}^TC\textbf{a}=1 $ 的条件下。

构造拉格朗日函数$L(D,\lambda )=D\text{a}{\text{a}}^T{D}^T-\lambda ({\text{a}}^{T}C\text{a}-1)$

求偏导数 令其为0：$\frac{\partial L(D,\lambda )}{\partial D}=0$

得到：$D^{T}D\text{a}-\lambda C\text{a}=0$

即 $D^{T}D\text{a}=\lambda C\text{a}$，令$D^{T}D=S$，等式变为 $S\text{a}=\lambda C\text{a}$

$$\begin{gathered} a^{T}Ca=1 \\ S\text{a}=\lambda C\text{a}\text{\hspace{1em}(10)} \end{gathered}$$

$$S=\left[\begin{array}{cccccc}{S}_{x^4}& S_{x^{3}y}& S_{x^2{y}^2}& S_{x^3}& S_{x^{2}y}& S_{x^2}\\ S_{x^{3}y}& S_{x^2{y}^2}& S_{x{y}^3}& S_{x^{2}y}& S_{x{y}^2}& S_{xy}\\ S_{x^2{y}^2}& S_{x{y}^3}& S_{y^4}& S_{x{y}^2}& S_{y^3}& S_{y^2}\\ S_{x^3}& S_{x^{2}y}& S_{x{y}^2}& S_{x^2}& S_{xy}& S_x\\ S_{x^{2}y}& S_{x{y}^2}& S_{y^3}& S_{xy}& S_{y^2}& S_y\\ S_{x^2}& S_{xy}& S_{y^2}& S_x& S_y& S_1\end{array}\right]\text{\hspace{1em}(11)}$$

再简化为求取特征值形式 $S^{-1}C\text{a}=\frac{1}{\lambda }\text{a}$

到此为止，椭圆拟合问题转换为求取 $S^{-1}C$ 的特征值 ，特征向量问题

除了理论上的正确性之外，前一节中描述的 原始方法有几个缺点（也是Fitzgibbon方法）。矩阵C(Eq. 9)是奇异的，矩阵S (Eq. 11)也几乎是奇异的(如果所有的数据点都精确地位于一个椭圆上，它是奇异的)。因此，Eq. 10的特征值 计算在数值上是不稳定的，可能会产生错误的结果(如无穷大或复数)。

该算法的另一个问题是对拟合最优解的低化。作者证明了Eq. 10恰好有一个正特征值，并指出其对应的特征向量是Eq. 7的最优解。不幸的是，事实并非如此。在理想情况下，当所有数据点都恰好位于椭圆上时，eienvalue为零。此外，对于特征值的数值计算，最优特征值甚至可以是一个很小的负数

改进方法

等价于以下两个方程
$$S_1{a}_1+S_2{a}_2=\lambda C_1{a}_1\text{\hspace{1em}(21)}$$

$$S_2^T{a}_1+S_3{a}_2=0\text{\hspace{1em}(22)}$$

$$a_2=-S_3^{-1}{S}_2^T{a}_1\text{\hspace{1em}(24)}$$
(24) 带入 （21）
$$(S_1-S_2{S}_3^{-1}{S}_2^T)a_1=\lambda C_1{a}_1\text{\hspace{1em}(25)}$$
C1是正则的

$$C^{-1}(S_1-S_2{S}_3^{-1}{S}_2^T)a_1=\lambda a_1\text{\hspace{1em}(26)}$$
(10) 第二个条件 可改写为
$$a_1^T{C}_1{a}_1=1\text{\hspace{1em}(27)}$$

汇总
$$\begin{gathered} M{a}_1=\lambda a_1 \\ {a}_1^T{C}_1{a}_1=1 \\ {a}_2=-S_3^{-1}{S}_2^T{a}_1 \\ M=C^{-1}(S_1-S_2{S}_3^{-1}{S}_2^T)\text{\hspace{1em}(29)} \end{gathered}$$

现在我们可以回到通过一组点拟合ellipse的任务。如前所述，该任务可以表示为约束最小化问题(Eq. 7)，其最优解对应于Eq. 10的特征向量a，该特征向量产生最小非负值λ。Eq. 10与Eq. 28等效，因此求出矩阵 M的适当特征向量a1就足够了。

求解出M的特征向量，即为我们需要的a1

python 实现参考

pip install lsq-ellipse

cpp 实现

如有问题，需要帮助，欢迎留言、私信或加群 交流【群号：392784757】

#include <Eigen/Dense>

#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>

#include <math.h>
#include <vector>
#include <string>
#include <iostream>
#include <fstream>

// ref https://github.com/bdhammel/least-squares-ellipse-fitting/blob/master/ellipse.py

Eigen::MatrixXd coef_;

Eigen::MatrixXd X;

bool check_data()
{
  return X.rows() >= 5;
}

void fit(const Eigen::MatrixXd &X)
{
  if (!check_data())
  {
    std::cout << "points num < 5" << std::endl;
    return;
  }
  Eigen::VectorXd x = X.col(0);
  Eigen::VectorXd y = X.col(1);
  Eigen::MatrixXd D1(X.rows(), 3);

  D1.col(0) = x.array().square();    // x^2
  D1.col(1) = x.array() * y.array(); // xy
  D1.col(2) = y.array().square();    // y^2

  Eigen::MatrixXd D2(X.rows(), 3);
  D2.col(0) = x;
  D2.col(1) = y;
  D2.col(2) = Eigen::VectorXd::Ones(X.rows());

  Eigen::Matrix3d S1 = D1.transpose() * D1;
  Eigen::MatrixXd S2 = D1.transpose() * D2;
  Eigen::Matrix3d S3 = D2.transpose() * D2;

  // constraint matrix
  Eigen::Matrix3d C1;
  C1 << 0, 0, 2,
      0, -1, 0,
      2, 0, 0;

  // reduced scatter matrix
  Eigen::Matrix3d M = C1.inverse() * (S1 - S2 * S3.inverse() * S2.transpose());

  // svd
  Eigen::EigenSolver<Eigen::Matrix3d> eigensolver(M); // selfadjointEigenSolver error
  if (eigensolver.info() != Eigen::Success)
  {
    std::cerr << "Eigen decoposition failed" << std::endl;
    return;
  }

  // extract
  Eigen::Vector3d eigval = eigensolver.eigenvalues().real();
  Eigen::Matrix3d eigvec = eigensolver.eigenvectors().real();

  // eigenvector must meet the constratint 4ac-b^2 > 0
  Eigen::VectorXd cond = 4 * eigvec.row(0).array() * eigvec.row(2).array() - eigvec.row(1).array().square();
  int index = -1;
  for (size_t i = 0; i < cond.size(); i++)
  {
    if (cond(i) > 0)
    {
      index = i;
      break;
    }
  }

  if (index == -1)
  {
    std::cerr << "No valid solution found" << std::endl;
    return;
  }
  Eigen::Vector3d a1 = eigvec.col(index);
  if (a1(0) > 0) // 与numpy 保持一致 影响 后续偏转角计算 不影响其他指标
  {
      a1 = -1 * a1;
  }

  // |d f g> = -S3^(-1) * S2^(T)*|a b c>
  Eigen::Vector3d a2 = -S3.inverse() * S2.transpose() * a1;

  coef_.resize(6, 1);
  coef_ << a1, a2;
}

void parseCoeff2GeoFeatures()
{
  // a*x^2 +   b*x*y + c*y^2 +   d*x +   e*y + f = 0  (*)  Eqn 1
  // a*x^2 + 2*b*x*y + c*y^2 + 2*d*x + 2*f*y + g = 0  (**) Eqn 15
  double a = coef_(0, 0);
  double b = coef_(1, 0) * 0.5;
  double c = coef_(2, 0);
  double d = coef_(3, 0) * 0.5;
  double f = coef_(4, 0) * 0.5;
  double g = coef_(5, 0);

  double x0 = (c * d - b * f) / (b * b - a * c);
  double y0 = (a * f - b * d) / (b * b - a * c);

  double numerator = 2 * (a * f * f + c * d * d + g * b * b - 2 * b * d * f - a * c * g);
  double denominator1 = (b * b - a * c) * (std::sqrt((a - c) * (a - c) + 4 * b * b) - (c + a));
  double denominator2 = (b * b - a * c) * (-std::sqrt((a - c) * (a - c) + 4 * b * b) - (c + a));

  if (denominator1 <= 0 || denominator2 <= 0) // 负值正常 后续 sqrt 内 为正
  {
      //throw std::runtime_error("Negative denominator in semi-axes length calculation");
      std::cout<< "Negative denominator in semi-axes length calculation, Maybe lead to following error" <<std::endl;
  }

  double height = std::sqrt(numerator / denominator1);
  double width = std::sqrt(numerator / denominator2);

  double phi; // 偏转角 长轴与x轴夹角
  if (b == 0 && a > c)
  {
    phi = 0.0;
  }
  else if (b == 0 && a < c)
  {
    phi = M_PI * 0.5;
  }
  else if (b != 0 && a > c)
  {
    phi = 0.5 * std::atan(2 * b / (a - c));
  }
  else if (b != 0 && a < c)
  {
    phi = 0.5 * (M_PI + std::atan(2 * b / (a - c)));
  }
  else if (a == c)
  {
    std::cerr << "Ellipse is a perfect circle, the answer is degenerate." << std::endl;
    phi = 0.0;
  }
  else
  {
    throw std::runtime_error("Unreachable state in angle calculation");
  }

  std::cout << "Center: (" << x0 << ", " << y0 << ")" << std::endl;
  std::cout << "Width: " << width << std::endl;
  std::cout << "Height: " << height << std::endl;
  std::cout << "Rotation angle (phi): " << phi/M_PI << " PI" << std::endl;
}

void xy2ellipse(double cx, double cy, double width, double height, double phi, int n_points)
{
  std::vector<double> t_values(n_points);
  double step = 2.0 * M_PI / (n_points - 1);
  for (int i = 0; i < n_points; ++i)
  {
    t_values[i] = i * step;
  }

  // Prepare x and y arrays
  std::vector<double> x(n_points);
  std::vector<double> y(n_points);

  // Generate the points on the ellipse
  for (int i = 0; i < n_points; ++i)
  {
    double t = t_values[i];
    x[i] = cx + width * std::cos(t) * std::cos(phi) - height * std::sin(t) * std::sin(phi);
    y[i] = cy + width * std::cos(t) * std::sin(phi) + height * std::sin(t) * std::cos(phi);
  }
}

void readPCDToMatrix(std::string pcd_path, Eigen::MatrixXd &pointsxy)
{
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);

  if (pcl::io::loadPCDFile<pcl::PointXYZ>(pcd_path, *cloud) == -1)
  {
    PCL_ERROR("Couldn't read pcd file \n");
    return;
  }
  std::cout << "the num of points: " << cloud->points.size() << std::endl;
  // 处理点云应该是经过PCA 变换到标准坐标系下 主方向为Z轴, 投影到xy面上 为需要拟合的椭圆数据
  // Resize the matrix to hold all points
  pointsxy.resize(cloud->points.size(), 2); // Rows for points, 2 columns (x, y)
  for (size_t i = 0; i < cloud->points.size(); i++)
  {
    // pointsxy << cloud->points.at(i).x, cloud->points.at(i).y; // 固定大小可以这样初始化
    pointsxy(i, 0) = cloud->points[i].x; // x-coordinate
    pointsxy(i, 1) = cloud->points[i].y; // y-coordinate
  }
}

void readTxtToMatrix(const char *txt_path, const std::string &delimiter, Eigen::MatrixXd &pointsxy)
{
  std::ifstream infile(txt_path);
  std::string line;
  std::vector<std::string> lines;

  if (infile.is_open())
  {
    while (std::getline(infile, line))
    {
      lines.emplace_back(line);
    }
    infile.close();
  }
  else
  {
    std::cerr << "Unable to open file" << std::endl;
  }

  // parse
  std::cout << "the num of points: " << lines.size() << std::endl;
  pointsxy.resize(lines.size(), 2);
  int cnt = 0;
  size_t delim_len = delimiter.length();
  for (size_t i = 0; i < lines.size(); i++)
  {
    cnt = 0;
    size_t pos_start = 0, pos_end = 0;
    std::string line_ = lines[i];
    while ((pos_end = line_.find(delimiter, pos_start)) != std::string::npos)
    {
      pointsxy(i, cnt++) = atof(line_.substr(pos_start, pos_end - pos_start).c_str());
      pos_start = pos_end + delim_len;
      if (cnt == 2)
      {
        break;
      }
    }
    if (cnt == 2)
    {
      continue;
    }
    else
    {
      pointsxy(i, cnt++) = atof(line_.substr(pos_start).c_str());
    }
  }
}

// ref https://stackoverflow.com/questions/14265581/parse-split-a-string-in-c-using-string-delimiter-standard-c
std::vector<std::string> split(std::string s, std::string delimiter)
{
  size_t pos_start = 0, pos_end, delim_len = delimiter.length();
  std::string token;
  std::vector<std::string> res;

  while ((pos_end = s.find(delimiter, pos_start)) != std::string::npos)
  {
    token = s.substr(pos_start, pos_end - pos_start);
    pos_start = pos_end + delim_len;
    res.push_back(token);
  }

  res.push_back(s.substr(pos_start));
  return res;
}

int main(int argc, char **argv)
{
  std::string pcd_path = "in0003 - Cloud.pcd"; // 使用绝对路径
  std::string txt_path = "in_0003.txt";
  // readPCDToMatrix(pcd_path, X);
  readTxtToMatrix(txt_path.c_str(), ",", X);
  fit(X);
  parseCoeff2GeoFeatures();

  return 0;
}

精度对比

cpp实现

python 实现

精度可以做到 小数点后3位，且数值稳定，即使点分布准确分布在椭圆上

如有问题，需要帮助，欢迎留言、私信或加群 交流【群号：392784757】