系统开发总结_可以做100张照片的软件_产品推广找哪家公司_百度快照功能

写在前面

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

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

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

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

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

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

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

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

椭圆拟合

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

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

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

$\min_a \sum_{i=1}^N F(x_i,y_i)^2 = \min_a \sum_{i=1}^N (F_a(x_i))^2 \\ = \min_a \sum_{i=1}^N (F_a(x_i \cdot a))^2$

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

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

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

$B^2 = 1 \tag{6}$

针对椭圆拟合问题重新公式化
$\min_a || D \textbf{a} ||^2 \\ \textbf{a}^T \textbf{C}\textbf{a} = 1 \tag{7}$

$\begin{bmatrix} x_1^2 & x_1y_1 & y_1^2 & x_1 & y_1 & 1 \\ \vdots & \vdots & \vdots & \vdots & \vdots & \vdots \\ x_i^2 & x_iy_i & y_i^2 & x_i & y_i & 1 \\ \vdots & \vdots & \vdots & \vdots & \vdots & \vdots \\ x_N^2 & x_Ny_N & y_N^2 & x_N & y_N & 1 \end{bmatrix} \tag{8}$

$\begin{bmatrix} 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{bmatrix} \tag{9}$

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

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

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

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

得到： $D^TD\textbf{a}-\lambda C\textbf{a} = 0$

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

$a^TCa=1 \\ S\textbf{a} = \lambda C \textbf{a} \tag{10}$

$\begin{bmatrix} S_{x^4} & S_{x^3 y} & S_{x^2y^2} & S_{x^3} & S_{x^2 y} & S_{x^2} \\ S_{x^3y} & S_{x^2 y^2} & S_{xy^3} & S_{x^2y} & S_{x y^2} & S_{xy} \\ S_{x^2y^2} & S_{x y^3} & S_{y^4} & S_{xy^2} & S_{y^3} & S_{y^2} \\ S_{x^3} & S_{x^2 y} & S_{xy^2} & S_{x^2} & S_{x y} & S_{x} \\ S_{x^2y} & S_{x y^2} & S_{y^3} & S_{xy} & S_{y^2} & S_{y} \\ S_{x^2} & S_{x y} & S_{y^2} & S_{x} & S_{y} & S_{1} \\ \end{bmatrix} \tag{11}$

再简化为求取特征值形式 $S^{-1} C \textbf{a} = \frac 1 \lambda \textbf{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 \tag{21}$

$S_2^T a_1 + S_3 a_2 = 0 \tag{22}$

在这里插入图片描述

$a_2 = -S_3^{-1} S_2^T a_1 \tag{24}$
(24) 带入（21）
$(S_1 - S_2 S_3^{-1} S_2^T) a_1 = \lambda C_1 a_1 \tag{25}$
C1是正则的

$C^{-1}(S_1 - S_2 S_3^{-1} S_2^T) a_1 = \lambda a_1 \tag{26}$
(10) 第二个条件可改写为
$a_1^T C_1 a_1 = 1 \tag{27}$

汇总
$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) \tag {29}$

现在我们可以回到通过一组点拟合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.pyEigen::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^2D1.col(1) = x.array() * y.array(); // xyD1.col(2) = y.array().square();    // y^2Eigen::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 matrixEigen::Matrix3d C1;C1 << 0, 0, 2,0, -1, 0,2, 0, 0;// reduced scatter matrixEigen::Matrix3d M = C1.inverse() * (S1 - S2 * S3.inverse() * S2.transpose());// svdEigen::EigenSolver<Eigen::Matrix3d> eigensolver(M); // selfadjointEigenSolver errorif (eigensolver.info() != Eigen::Success){std::cerr << "Eigen decoposition failed" << std::endl;return;}// extractEigen::Vector3d eigval = eigensolver.eigenvalues().real();Eigen::Matrix3d eigvec = eigensolver.eigenvectors().real();// eigenvector must meet the constratint 4ac-b^2 > 0Eigen::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 15double 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 arraysstd::vector<double> x(n_points);std::vector<double> y(n_points);// Generate the points on the ellipsefor (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 pointspointsxy.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-coordinatepointsxy(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;}// parsestd::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】