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

写在前面

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

1. 椭圆 —— 从理论推导到最小二乘法拟合 https://blog.csdn.net/zh471021698/article/details/108812050
2. 椭圆拟合理论推导和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)=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.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】