沉默杀手HomeArchivesTagsCategories
Ceres(二) 自动求导
|SLAM工具|
Word count: 1.2k|Reading time: 5 min

头文件只有一个"ceres/ceres.h"

调试几个经典例子的过程

1
2
3
4
5
6
7
#include "ceres/ceres.h"

using ceres::AutoDiffCostFunction;
using ceres::CostFunction;
using ceres::Problem;
using ceres::Solver;
using ceres::Solve;


1
2
3
4
5
6
7
8
9
10
11
struct  Functor
{
	template <typename T> bool operator()(const T* const x, T* residual) const
	{
		residual[0] = 10.0 - x[0];
		residual[1] = 3.0 - x[1];
		// 直接让第2维度为0，优化后的x[1]还是初值2
		// residual[1] = T(0.0);
		return true;
	}
};

这里的变量为二维的，目标函数为10.0-x和3.0-x

如果这里不写10.0而是10，就会报错。 Ceres中没有int，只接受double

对于residual[1] = T(0.0);的情况，如果不加T，编译就会报错 error: no match for ‘operator=’ (operand types are ‘ceres::Jet’ and ‘double’) . Jet类型是ceres内置类型，我们要把double转成Jet类型，也就是T(0.0)

1
2
3
4
5
6
7
8
struct  TestLocalParam
{
	template<typename T> bool operator()(const T* x, const T* delta, T* x_plus_delta) const{
		x_plus_delta[0] = x[0] + 0.4 * delta[0];
		x_plus_delta[1] = x[1] + 0.4 * delta[1];
		return true;
	}
};

CostFunctor的operator就是计算残差，不带平方项，平方是ceres自动添加，相当于最小二乘的函数 f

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
// double x = 0.5;
double x[] = {0.5, 2.0};

Problem problem;

problem.AddParameterBlock(x, 2, 
  (new ceres::AutoDiffLocalParameterization
  	// 真正的维度是2， 参与优化的维度是1
  	<TestLocalParam, 2, 1> ())
	);
// AddParameterBlock 修改之后，无论下面的AutoDiffCostFunction参数是多少，只优化第1维度


// 残差的维度，参数的维度
// 如果此时是 2,1. 报错
CostFunction* cost_func = new AutoDiffCostFunction<Functor, 2, 2>(new Functor);
// 参数：CostFunction，可选的LossFunction，将CostFunction连接到一组参数块
problem.AddResidualBlock(cost_func, nullptr, x);

创建最小二乘问题，可以使用Problem::AddResidualBlock()和Problem::AddParameterBlock()。 可以使用Problem::AddParameterBlock()显式添加参数块，这将导致额外的正确性检查; 然而，如果参数块不存在，Problem::AddResidualBlock()会隐式添加参数块。

AddParameterBlock()显式地向Problem添加了一个参数块。它还允许用户将LocalParameterization对象与参数块关联起来。

AddResidualBlock默认会先调用AddParameterBlock，一般不用后者。

损失函数LossFunction，给异常值做限定，如果是nullptr，该项的代价就是残差的平方范数

代价函数携带关于它所期望的参数块大小的信息。函数检查这些参数是否与parameter_blocks中列出的参数块的大小匹配。如果检测到不匹配，程序将中止。

Solver

1
2
3
4
5
6
7
8
9
10
11
12
13
14
  Solver::Options  option;
  option.minimizer_progress_to_stdout = true;
  option.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;

  Solver::Summary summary;
  Solve(option, &problem, &summary);
  
  std::cout << summary.BriefReport() << std::endl<<std::endl;
  // std::cout << summary.FullReport() << std::endl<<std::endl;

  // std::cout << "initial x: " << initial_x <<"   x: "<< x << std::endl;
  std::cout <<"x[0]: "<< x[0] <<"   x[1]: "<< x[1]<< std::endl;
  return 0;
}

优先选用自动微分算法，某些情况可能需要用到解析微分算法，尽量避免数值微分算法。

曲线拟合

使用十四讲第六章的曲线拟合的例子，已知 N 个数据 ，它们符合曲线 ，通过曲线拟合求 a, b, c

跟上面的例子不同，这次目标函数为 ， 残差维度为3(未知a,b,c)

CMake配置如下

1
2
3
4
5
6
7
8
find_package(OpenCV REQUIRED)
INCLUDE_DIRECTORIES(${OpenCV_DIRS})

target_link_libraries(node_2
   ${catkin_LIBRARIES}
   ${CERES_LIBRARIES}
   ${OpenCV_LIBS}
)

先生成带有高斯噪声的数据

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
#include <opencv2/opencv.hpp>

// 真实值
double a =1.0, b=2.0, c=1.0;
// abc的初值
double abc[3]  = {0,0,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)  );
}

高斯噪声的产生： cv::RNG rng; rng.gaussian(w_sigma)

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
struct Functor
{
    Functor(double x, double y):
    m_x(x), m_y(y) {    }
    #if 0
    // 开始我是这么写的，没弄清残差的维度
    template<typename T> bool operator()( const T* const a, 
                                          const T* const b, 
                                          const T* const c, 
                                          T* residual)  const
    {
        residual[0] = m_y - exp(a[0] * m_x * m_x + b[0] * m_x + c[0]);
        return true;
    }
    #endif

    // 因为要传入数据(x,y)， 所以要定义两个成员变量
    template<typename T> bool operator()( const T* const abc, T* residual)  const
    {
        residual[0] = m_y - exp(abc[0] * m_x * m_x + abc[1] * m_x + abc[2]);
        return true;
    }
    const double m_x, m_y;
};

因为自变量x是一维的，而不是向量，所以残差只计算一维 residual[0]

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
Problem problem;
for(unsigned int i=0; i<N; i++)
{
	// 残差维度， 所估计变量的维度
  CostFunction* cost_func = new AutoDiffCostFunction<Functor,1,3>(new Functor(x_data[i], y_data[i]) );
  problem.AddResidualBlock(cost_func, nullptr, abc);
}
Solver::Options option;
option.minimizer_progress_to_stdout = true;
option.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;

Solver::Summary summary;
Solve(option, &problem, &summary);

cout << summary.BriefReport() << endl <<endl;
cout << "a: "<< abc[0] <<"  b: "<< abc[1]<< "  c: "<< abc[2] <<endl;

参考：
Curve Fitting(曲线拟合)
Problem类
资料