沉默杀手HomeArchivesTagsCategories
Ceres(三) 解析求导
|SLAM工具|
Word count: 845|Reading time: 3 min

最常见的一个例子是 链接，不再复制粘贴

还是之前的曲线拟合问题： ，如果改用解析求导，需要构建一个继承CostFunction的类。核心函数是bool CostFunction: :Evaluate(double const *const *parameters, double *residuals, double **jacobians)，计算残差向量和雅可比矩阵

  • parameters: parameters是一个大小为CostFunction::parameter_block_sizes_.size()(输入参数块的数量和大小)的数组，parameters[i]是一个大小为parameter_block_sizes_[i]的数组，其中包含CostFunction所依赖的第i个参数块。parameters不会为nullptr

  • residuals: 是一个大小为num_residuals_(输出残差的个数)的数组。residuals也不会为nullptr

  • jacobians: 是一个大小为CostFunction::parameter_block_sizes_.size()的数组。如果jacobians是nullptr，用户只需要计算残差

  • jacobians[i]: 是一个大小为 num_residuals x parameter_block_sizes_[i] 的行数组，如果jacobians[i]不为nullptr，用户需要计算关于parameters[i]的残差向量的雅可比矩阵，并将其存储在这个数组中，即

如果jacobians为nullptr，通常我们只需要在Evaluate函数中实现residuals的计算，jacobians后面可以通过Ceres提供的自动求导（数值求导）替代，否则，还需要在Evaluate函数中实现jacobians的计算

如果parameters大小和residuals大小在编译时是已知的，就可以使用SizeCostFunction，该函数可以将paramters大小和residuals大小设置为模板参数，用户只需要在使用的时候给模板参数赋值就可以

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
// 模板参数依次为： number of residuals,  first parameter的维度
class MyCostFun : public SizedCostFunction<1, 1, 1, 1>
{
    public:
		MyCostFun(double x, double y):
			m_x(x), m_y(y){}
   virtual bool Evaluate(double const* const* parameters,
		      	 double* residuals,
			 double** jacobians) const
   {
       double a = parameters[0][0];
       double b = parameters[0][1];
       double c = parameters[0][2];
       residuals[0] = m_y - exp(a*m_x*m_x + b*m_x + c);

		if (jacobians != NULL && jacobians[0] != NULL)
		{
		    jacobians[0][0] = -exp(a*m_x*m_x + b*m_x + c)* m_x * m_x;
		    jacobians[0][1] = -exp(a*m_x*m_x + b*m_x + c)* m_x;
		    jacobians[0][2] = -exp(a*m_x*m_x + b*m_x + c);

		}
		return true;
   }
   protected:
	double m_x;
    double m_y;
};

拟合 时，使用SizedCostFunction<1, 2>，只计算雅格比jacobians[0][0] 和 jacobians[0][1]

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
int main(int argv, char** argc)
{
    ros::init(argv, argc, "test_ceres");
    ros::NodeHandle nh;
    // 真实值
	double a = 1.0, b = 2.0, c = 1.0;

	// 生成带有噪声的模拟数据
	vector<double> x_data, y_data;
	int N = 240;  // 数据数量
	double w_sigma = 1.0;  // 高斯标准差
	cv::RNG  rng;
	for(unsigned int i =0; i<N; i++)
	{
		double x = i/100.0;
		x_data.push_back(x);
		y_data.push_back(exp(a*x*x + b*x + c) + rng.gaussian(w_sigma)  );
	}
	// 作为初值
	a = 0.0;
	b = 0.0;
	c = 0.0;
	Problem problem;
	for(unsigned int i=0; i<N; i++)
	{
		CostFunction* cost_func = new MyCostFun(x_data[i], y_data[i]);
		problem.AddResidualBlock(cost_func, nullptr, &a, &b, &c);
	}
	Solver::Options option;
	option.minimizer_progress_to_stdout = true;

	Solver::Summary summary;
	Solve(option, &problem, &summary);
	cout << summary.BriefReport() << endl <<endl;
	cout << "a: "<< a <<"  b: "<< b << "  c: "<< c <<endl;

    return 0;
}

一开始我设置代价函数的模板为SizedCostFunction<1, 3>，结果运行报错： [problem_impl.cc:286] Check failed: num_parameter_blocks == cost_function->parameter_block_sizes().size() (3 vs. 1)

问题在于parameter_block_sizes只有1，而我们需要3，也就是对应abc三个参数。读SizedCostFunction源码发现，模板定义为template <int kNumResiduals, int... Ns>，构造函数只有两行

1
2
set_num_residuals(kNumResiduals);
*mutable_parameter_block_sizes() = std::vector<int32_t>{Ns...};

因此从第2个模板参数开始，有几个参数就对应parameter_block_sizes，改为SizedCostFunction<1, 1, 1, 1>