ceres求解非线性最小二乘问题入门-CSDN博客

ceres是一个用于解决有约束非线性最小二乘问题的库，涉及定义代价函数、求导方法。文章通过拟合平面圆的示例，介绍了数值求导、自动求导和解析求导的使用，并比较了它们的效率和适用场景。

ceres用于求解有约束的非线性最小二乘问题，如下式：
$\begin{array}{ll} \min _{\mathbf{x}} & \frac{1}{2} \sum_{i} \rho_{i}\left(\left\|f_{i}\left(x_{i_{1}}, \ldots, x_{i_{k}}\right)\right\|^{2}\right) \\ \text { s.t. } & l_{j} \leq x_{j} \leq u_{j} \end{array}$
其中：

$ρi(∥fi(xi1,…,xik)∥2)\rho_{i}\left(\left\|f_{i}\left(x_{i_{1}}, \ldots, x_{i_{k}}\right)\right\|^{2}\right)$ 为残差块(residualBlock)；

$fi(.)f_{i}\left(.\right)$ 为代价函数，也称残差；

$ρi\rho_{i}$ 为损失函数，用于较少异常值对非线性最小二乘问题求解的影响，通常为标量函数；

$[xi1,…,xik][x_{i_{1}}, \ldots, x_{i_{k}}]$ 为参数块，是最小二乘问题待求的参数；

s.t.意思是subject to （受限于），也就是参数的优化所在范围。当限制条件变成负无穷到正无穷，就是非线性最小二乘问题。
$\frac{1}{2} \sum_{i}\left\|f_{i}\left(x_{i_{1}}, \ldots, x_{i_{k}}\right)\right\|^{2}$
使用ceres解决上述问题，关键在于：

定义代价函数
定义求导方法

定义代价函数

定义代价函数即对问题进行数学建模的过程，比如拟合问题，即寻找拟合点见下面几个问题的代价函数定义：

对n个点拟合一个空间球，由球面上的点到圆心的距离都等于球的半径，容易可得球拟合的代价函数如下：
$f(x,y,z)=(x-x_0)^2+(y-y_0)^2+(z-z_0)^2-r^2$
其中： $x_0,y_0,z_0,r$ 分别表示待求参数球心坐标和球的半径。

在ceres中定义如下：

struct SphereitCostFunctor{
	SphereitCostFunctor(double x, double y, double z){
        _x = x;
        _y = y;
        _z = z;
    };
    // ct 球中心
    // r 球半径
    template <typename T>
    bool operator()(const T* const ct, const T* const r, T *residual) const { // 末尾的const声明不可少
        residual[0] = pow(_x-ct[0],2) + pow(_y-ct[1],2) + pow(_z-ct[2],2) - r*r;
        return true;
    }
	private:
    double _x,_y,_z;	// 用于拟合的点坐标
}
// 每个代价函数对象对应一个拟合点

定义求导方法

ceres中提供了三种求导的方法：

用户自己提供导数的解析式
数值求导
自动求导

解析式方法 主要用于目标函数形式比较简单可直接给出目标函数的导数形式的情况或者对速度要求比较高的场合；

数值求导 用于代价函数中无法给出显式的计算表达式，比如调用了第三方库的函数；

自动求导 用于无法或者自行求导太过于复杂的情况，通过自动求导，ceres可自动对目标函数进行求导，而不需要用户显式提供雅克比矩阵。但前提是必须能够提供代价函数的表达，且表达式中的所使用的基本函数必须是ceres中的函数，而不能用标准的c++函数。比如sqrt, pow等。如果使用c++中的函数会报错。

同时对于求解如下的代价函数：
$\begin{aligned} y &=1-\sqrt{x_{0}^{2}+x_{1}^{2}} \\ D_{1} y &=-\frac{x_{0}}{\sqrt{x_{0}^{2}+x_{1}^{2}}}, D_{2} y=-\frac{x_{1}}{\sqrt{x_{0}^{2}+x_{1}^{2}}} \end{aligned}$
其中：下面一行为其偏导形式，显然在 $x_0=0,x_1=0$ 的情况下，是无法计算的。

自动求导的实现方法很巧妙，有兴趣的可以移步到http://www.ceres-solver.org/automatic_derivatives.html进行详细了解。

代价函数中调用了其他函数，可否用自动求导的方式？？？

运行效率：解析式 > 自动求导 > 数值求导；

下表是一个案例的速度对比：

CostFunction	Time (ns)
Rat43Analytic	255
Rat43AnalyticOptimized	92
Rat43NumericDiffForward	262
Rat43NumericDiffCentral	517
Rat43NumericDiffRidders	3760
Rat43AutomaticDiff	129

官方推荐用优先用自动求导的方式，其次是解析式，最后才是数值求导。

其中数值求导又包括：前向差分、中心差分：前向差分求导误差为O(h)而中心差分的求导误差为O(h^2)，h为求导的步长（通常取很小的值），可见中心差分的误差比前向差分误差小；但中心差分每次求导时要计算两次函数值，而前向差分只要计算一次，速度更快。

上述三种求导方法可单独使用也可结合起来使用。

demo 1:拟合平面圆

已知若干数据点，下面演示如果用不同的求导方法获得这些数据点的拟合圆。

首先需写出圆的代价函数，由圆的方程容易可得圆的代价函数：
$f(x,y,r)=(x-x_0)^2+(y-y_0)^2-r^2$

数值求导的方法实例代码

因为是第一个例子，所以对代码进行了详细的注释说明，请留意。

// 定义代价函数，已仿函数的形式进行定义
struct CircleFitCostFunctor_numDif {
   	// 构造函数，其参数视具体的问题而定，没有固定形式，因每个数据点对应一个残差，所以
    // 这里的输入为数据点的坐标
	CircleFitCostFunctor_numDif(double x, double y)
		:_x(x), _y(y) {}
    /*
	重载()操作，前面的参数为输入参数，为指向参数块的指针，用const double*或
	const double* const。最后一个参数为输出参数，为残差的指针。输入参数的个
	数（即残差块的个数）可根据用户的需要进行组织，但最多不超过10个。
	此例中，ct为第一个参数块即圆心，r为第二个参数块即半径，当然也可以将圆心
	的两个参数各作为一个参数块。
	*/
	bool operator()(const double*  ct,
		const double* r, double* residual) const {
		residual[0] = pow(_x - ct[0], 2) + pow(_y - ct[1], 2) - r[0] * r[0];
		return true;
	}
private:
	const double _x, _y;
};

// 测试用数值求导的方式
void fit_circle_num_dif_test()
{
	// 圆的参数
	double ct[2] = { 0 };
	double r = 0; 
    
    // 生成拟合点
	int n_pt = 1000;			// 拟合点的数量
	double noice_std = 0.01;	// 噪声标准差
	double math_ct[2] = {10,10};
	double math_r = 5;
	double step = M_PI*2 / n_pt;
	vector<double> x(n_pt), y(n_pt);

	if (noice_std == 0.0) {
		for (int i = 0; i < n_pt; ++i) {
			x[i] = math_ct[0] + math_r * cos(step * i);
			y[i] = math_ct[1] + math_r * sin(step * i);
			of << x[i] << " " << y[i] << " " << 0 << endl;
		}
	}
	else {
		std::random_device rd;
		std::mt19937 gen(rd());
		std::normal_distribution<double>disr(0, noice_std);
		for (int i = 0; i < n_pt; ++i) {
			x[i] = math_ct[0] + math_r * cos(step * i) + disr(gen);
			y[i] = math_ct[1] + math_r * sin(step * i) + disr(gen);
			of << x[i] << " " << y[i] << " " << 0 << endl;
		}
	}

    // 构建优化目标
	ceres::Problem problem;
	for (int i = 0; i < x.size(); ++i) {
		ceres::CostFunction *costFun = new ceres::NumericDiffCostFunction<CircleFitCostFunctor_numDif,  // 代价函数类型
			ceres::CENTRAL,	// 差分方式：中心差分，较为常用
			1,	// 残差个数
			2,	// 第一个参数指针指向的参数个数，这里特指圆心
			1>	// 第二个参数指针指向的参数个数，这里特指半径
			(new CircleFitCostFunctor_numDif(x[i], y[i]));
		problem.AddResidualBlock(costFun, // 代价函数对象
			NULL,	// 损失函数，可不指定,即NULL
			ct,		// 第一个参数指针
			&r);	// 第二个参数指针
	}

	ceres::Solver::Options options;
	options.max_num_iterations = 50;
	options.minimizer_progress_to_stdout = true;

	ceres::Solver::Summary summary;
	ceres::Solve(options, &problem, &summary);
	std::cout << summary.BriefReport() << "\n";
	return;
}

自动求导的方法实例代码

和数值求导的差别如下1.2：

// 1. 代价函数的定义
struct CircleFitCostFunctor_autoDif {
	CircleFitCostFunctor_autoDif(double x, double y)
		:_x(x), _y(y) {}
	/*
	重载()操作，注意需定义成模板函数，且函数中实现的所用到的数学计算需用ceres的
	版本，而不能用c++中。
	*/
	template <typename T> bool operator()(const T* const ct,
		const T* const r, T* residual) const {
		residual[0] = ceres::sqrt(ceres::pow(_x - ct[0], 2) + ceres::pow(_y - ct[1], 2)) - r[0]; // 需要用ceres中的定义的基本计算
		return true;
	}
private:
	const double _x, _y;
};

// 2.残差对象的定义
ceres::CostFunction *costFun = new ceres::AutoDiffCostFunction<CircleFitCostFunctor_autoDif,  // 代价函数类型
	1,	// 残差个数
	2,	// 第一个参数指针指向的参数个数，这里特指圆心
	1>	// 第二个参数指针指向的参数个数，这里特指半径
	(new CircleFitCostFunctor_autoDif(x[i], y[i]));

解析求导方法

不需要定义代价函数的仿函数，而是直接继承自SizedCostFunction或CostFunction重写其中的Evaluate函数，示例代码如下：

// 解析式求导代价函数
// 模板参数说明：<残差块个数，第一个参数块参数数量，第二个参数块参数数量>
class CircleFitCostFunction : public ceres::SizedCostFunction<1, 2, 1> { 
public:
	CircleFitCostFunction(double x, double y)
		: _x(x), _y(y) {}
    /*
    parameters 为参数的指针，parameters[n]表示第n个参数块，parameters[n][m]
    表示第n个参数块的第m个参数。
    jacobians 为雅克比矩阵的指针，在函数内部需对其进行判断，判断是否空，如果不为空
    则需要用户提供雅克比矩阵计算方法。其大小和paramters对应。
    */
	virtual bool Evaluate(double const* const* parameters,
						  double* residuals,
						  double** jacobians) const {
		const double x = parameters[0][0];
		const double y = parameters[0][1];
		const double r = parameters[1][0];
		residuals[0] = pow(_x - x, 2) + pow(_y - y,2) - r*r;

		// Compute the Jacobian if asked for.
		if (jacobians != NULL && jacobians[0] != NULL) {
			jacobians[0][0] = -2 * (_x - x);
			jacobians[0][1] = -2 * (_y - y);
			jacobians[1][0] = -2 * r;
		}
		return true;
	}
private:
	double _x, _y;
};