最小二乘法实现椭圆拟合

本文仅使用Eigen库，使用C++手推最小二乘法实现椭圆拟合

---

首先定义平面点的结构体：

struct Point2d {
	double u;
	double v;
	Point2d() {
		u = -1;
		v = -1;
	}
	Point2d(double u, double v) :u(u), v(v) {};
};

---

定义导入外部数据的导入函数：

bool ImportPointCloud(char& filename, std::vector<Point2d> &mdata) {
    Point2d temp;
    std::ifstream import;
    import.open(&filename);
	if (!import) return false;
    while (import.peek() != EOF) {
        import >> temp.u >> temp.v;
        mdata.push_back(temp);
    }
    import.close();
    return true;
}

---

定义椭圆目标方程

//目标方程
void getElipseObjFun(const Eigen::MatrixXd& mPc, const Eigen::RowVectorXd& vInitPara, Eigen::VectorXd& vObjFun)
{
	double dx = 0, dy = 0;
	vObjFun.resize(mPc.rows());
	for (int i = 0; i < mPc.rows(); i++)
	{
		//边缘点的二维坐标
		dx = mPc.row(i)(0);
		dy = mPc.row(i)(1);
		// 椭圆方程 Ax^2 + Bxy + Cy^2 + Dx + Ey + 1 = 0  ； 将实数参数归一化
		vObjFun(i) = vInitPara(0) * pow(dx, 2) + vInitPara(1) * dx * dy + vInitPara(2) * pow(dy, 2) + vInitPara(3) * dx + vInitPara(4) * dy + 1;
	}
}

---

定义目标方程的雅可比矩阵

//目标方程的雅可比矩阵 ，对A、B、C、D、E求导
void getEllipseJacobi(const Eigen::MatrixXd& mPc, const Eigen::RowVectorXd& vInitPara, Eigen::MatrixXd& mJacobi)
{
	double df[5] = { 0, 0, 0, 0, 0 };
	double dx = 0, dy = 0;
	mJacobi.resize(mPc.rows(), 5);
	for (int i = 0; i < mPc.rows(); i++)
	{
		dx = mPc.row(i)(0);
		dy = mPc.row(i)(1);
		df[0] = pow(dx, 2);
		df[1] = dx * dy;
		df[2] = pow(dy, 2);
		df[3] = dx;
		df[4] = dy;
		mJacobi.row(i) << df[0], df[1], df[2], df[3], df[4];
	}
}

---

定义高斯牛顿迭代的迭代函数

void ellipseGaussNewtonIterate(const Eigen::MatrixXd& mPc, const double* pInitPara, double* pNewPara, double* ellipsePara)
{
	double dA, dB, dC, dD, dE;
	Eigen::VectorXd vObjFun;  //目标函数
	Eigen::MatrixXd mJacobi;  //雅克比矩阵
	Eigen::RowVectorXd vPara = Eigen::RowVectorXd::Zero(5); //全局变量
	Eigen::RowVectorXd vNewPara = Eigen::RowVectorXd::Zero(5); //更新后的全局变量
	vPara << pInitPara[0], pInitPara[1], pInitPara[2], pInitPara[3], pInitPara[4];


	int i = 0;
	double dDelta = vPara.lpNorm<1>();  //求初值的1范数
	//迭代次数为200
	while (dDelta > residual && i < Iteration)
	{
		getElipseObjFun(mPc, vPara, vObjFun);
		getEllipseJacobi(mPc, vPara, mJacobi);
		vNewPara = vPara - ((mJacobi.transpose() * mJacobi).inverse() * mJacobi.transpose() * vObjFun).transpose();  //由于计算为行向量，因此需要转置
		dDelta = ((vPara - vNewPara)).lpNorm<1>(); //求残差的1范数
		vPara = vNewPara;
		i += 1;
	}
	//将新的全局变量赋值
	for (int i = 0; i < 5; i++)
	{
		*(pNewPara + i) = vPara[i];
	}

	dA = pNewPara[0];
	dB = pNewPara[1];
	dC = pNewPara[2];
	dD = pNewPara[3];
	dE = pNewPara[4];

	//输出椭圆参数
	double dXc = (dB * dE - 2 * dC * dD) / (4 * dA * dC - pow(dB, 2));  // 椭圆几何中心x坐标
	double dYc = (dB * dD - 2 * dA * dE) / (4 * dA * dC - pow(dB, 2));  // 椭圆几何中心y坐标
	double dLa = sqrt((2 * (dA * pow(dXc, 2) + dC * pow(dYc, 2) + dB * dXc * dYc - 1)) / (dA + dC - sqrt(pow(dA - dC, 2) + pow(dB, 2))));  // 长轴长度
	double dLb = sqrt((2 * (dA * pow(dXc, 2) + dC * pow(dYc, 2) + dB * dXc * dYc - 1)) / (dA + dC + sqrt(pow(dA - dC, 2) + pow(dB, 2))));  // 短轴长度
	double dTheta = 0.5 * atan(dB / (dA - dC));
	if (dA > dC) dTheta = dTheta - 3.1415926 / 2; //当为钝角时，转换为小于90度的角

}

---

最终调用函数的主函数为：

int main()
{
	char filename[255];
	sprintf_s(filename, 255, ".//data//data1.txt");
	std::vector<Point2d> mdata;
	ImportPointCloud(*filename, mdata);
	Eigen::MatrixXd mPc = Eigen::MatrixXd::Zero(mdata.size(),2);
	for (int i = 0; i < mdata.size(); i++) 
	{
		mPc(i, 0) = mdata[i].u;
		mPc(i, 1) = mdata[i].v;
	}

	double* pInitPara = new double[5]{ 0.1 ,0.1 ,0.1,0.1,0.1 };
	double* pNewPara = new double[5]{ 0 };
	double* ellipsePara = new double[5]{ 0 };
	ellipseGaussNewtonIterate(mPc, pInitPara, pNewPara, ellipsePara);
	std::cout << pNewPara[0] << std::endl;
}

读者将参考该文进行椭圆参数求解