0%

Ceres 学习记录(一)

本文主要参考 Ceres 官方文档,学习了一下非线性优化库 Ceres Solver 的使用,包括如何使用 Ceres 的自动求导以及解析求导还有它们的对比。使用的例子在 LidarSLAM - 第六次作业 中有介绍。

待求解问题

这里用的例子是我在学习深蓝学院的激光slam课程第六次作业中提供的例子,这里简单介绍一下。主要是给定一系列节点和边然后进行图优化。这里的节点是机器人一系列位置(x, y, yaw),边则是前后两帧相对位姿观测值以及信息矩阵。优化过程主要是降低相对位姿观测值和由实际前后位姿计算出的预测值之间产生的误差。问题中节点的边的结构如下:

1
2
3
4
5
6
7
8
9
10
11
/**
* Custom Edge and Vertex
*/
struct myEdge
{
int xi,xj; // 对应节点的索引
Eigen::Vector3d measurement; // 两个节点之间的相对位姿的观测值,分别为:dx, dy, dtheta
Eigen::Matrix3d infoMatrix; // 本次观测的信息矩阵
};

typedef Eigen::Vector3d myVertex; // 机器人位姿,x, y, theta

Ceres 使用

Ceres 基本过程

使用 Ceres 的基本过程比较简单,如下所示:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
/// 获取我们需要优化的节点和边
std::vector<myVertex> Vertexs;
std::vector<myEdge> Edges;


/// 声明一个问题
ceres::Problem problem;

/// 核心部分:
for (const auto& edge: Edges) {
// 对每一次观测,生成一个 Cost Function,也就是误差函数
ceres::CostFunction* cost_function = AutoDiffFunctor::create(edge);
// 将这一次观测涉及到的节点以残差块的形式添加进问题中
problem.AddResidualBlock(cost_function, nullptr, Vertexs[edge.xi].data(), Vertexs[edge.xj].data());
}

/// 节点和边添加完成后就可以求解
ceres::Solver::Options options; // 不同求解选项
options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY; // 这里使用稀疏 Cholesky 求解线性方程组
options.minimizer_progress_to_stdout = true; // 输出求解过程
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary); // 求解问题
std::cout << summary.FullReport() << "\n"; // 输出结果

使用基本 Ceres 求解优化问题的步骤大致如上,基本上就是:

  1. 获取节点和边
  2. 声明一个 ceres::Problem 对象
  3. 利用边(观测)生成 ceres::CostFunction 对象,这里生成的方式很大程度上会根据我们的选择而使用解析求导或者数值求导
  4. 将边中涉及到的节点添加进问题中
  5. 设置求解器选项
  6. 求解

这其中,1, 2, 5, 6 都比较固定,主要值得我们关注的是 3 和 4 接下来详细介绍一下:

Ceres 实现自动求导

要实现自动数值求导,首先需要定义一个仿函数类,之所以说是仿函数,是因为在这个类中重载了 operator() 使得我们可以像使用函数一样使用这个类。另外这里官方用的是一个 struct,但是大多数情况下 structclass 没区别,所以这里统一用 class

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
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
class AutoDiffFunctor {
public:
AutoDiffFunctor(const myEdge& edge): measurement(edge.measurement), sqrt_info_matrix(edge.infoMatrix.array().sqrt()) {}

template <typename T>
bool operator()(const T* const v1, const T* const v2, T* residual) const {


Eigen::Matrix<T, 3, 1> v1_mat{ v1[0], v1[1], v1[2] };
Eigen::Matrix<T, 3, 1> v2_mat{ v2[0], v2[1], v2[2] };
Eigen::Matrix<T, 3, 1> m = measurement.template cast<T>();
Eigen::Map<Eigen::Matrix<T, 3, 1>> error{ residual };

// calculate error from translation and rotation respectively and combine them together
Eigen::Matrix<T, 3, 3> X1 = PoseToTrans(v1_mat);
Eigen::Matrix<T, 3, 3> X2 = PoseToTrans(v2_mat);
Eigen::Matrix<T, 3, 3> Z = PoseToTrans(m);

Eigen::Matrix<T, 2, 2> Ri = X1.block(0, 0, 2, 2);
Eigen::Matrix<T, 2, 2> Rj = X2.block(0, 0, 2, 2);
Eigen::Matrix<T, 2, 2> Rij = Z.block(0, 0, 2, 2);

Eigen::Matrix<T, 2, 1> ti{ v1_mat(0), v1_mat(1) };
Eigen::Matrix<T, 2, 1> tj{ v2_mat(0), v2_mat(1) };
Eigen::Matrix<T, 2, 1> tij{ m(0), m(1) };

Eigen::Matrix<T, 2, 2> dRiT_dtheta; // derivative of Ri^T over theta
dRiT_dtheta(0, 0) = T(-1) * Ri(1, 0); // cosX -> -sinX
dRiT_dtheta(0, 1) = T( 1) * Ri(0, 0); // sinX -> cosX
dRiT_dtheta(1, 0) = T(-1) * Ri(0, 0); // -sinX -> -cosX
dRiT_dtheta(1, 1) = T(-1) * Ri(1, 0); // cosX -> -sinX

// calcuate error & normalize error on theta
error.template segment<2>(0) = Rij.transpose() * (Ri.transpose() * (tj - ti) - tij);
error(2) = v2_mat(2) - v1_mat(2) - m(2);
if (error(2) > T(M_PI)) {
error(2) -= T(2 * M_PI);
} else if (error(2) < T(-1 * M_PI)) {
error(2) += T(2 * M_PI);
}

error = sqrt_info_matrix.template cast<T>() * error;

return true;
}

static ceres::CostFunction* create(const myEdge& edge) {
return (new ceres::AutoDiffCostFunction<AutoDiffFunctor, 3, 3, 3>(new AutoDiffFunctor(edge)));
}

private:
Eigen::Vector3d measurement;
Eigen::Matrix3d sqrt_info_matrix;
};

先看一下仿函数,首先针对每一条边 (Edge) 我们需要生成一个 AutoDiffFunctor 对象,因此每个对象对应一条边,成员变量中包含边的相关信息。在这个问题中,观测涉及到主要是信息矩阵和相对位姿,而我们在计算过程中需要的是信息矩阵的平方根,所以成员变量分别为:

  • measurement (相对位姿)
  • sqrt_info_matrix (信息矩阵的平方根)。

对于 operator(),我们需要将其中定义误差函数,先看一下函数声明:

1
2
template <typename T>
bool operator()(const T* const v1, const T* const v2, T* residual) const;
  • template <typename T>: 首先这必须是一个模板函数,因为我们在使用 Ceres 的数值求导中,只需要提供误差函数而不需要手动计算 Jacobian。Ceres 使用数值求导的方法中会涉及到另一个变量类型 ceres::Jet 同样会使用这个操作。因此需要将这个函数声明为模板函数;
  • v1v2 :待求解的节点,这里一条边涉及到两个节点因此需要两个参数,如果是一元边则去掉 v2 因此类推。
  • residual:残差,也就是待求解误差,是我们需要在这个函数中计算的变量

接下来看内部实现:

1
2
3
4
Eigen::Matrix<T, 3, 1> v1_mat{ v1[0], v1[1], v1[2] };      /// 第一个节点的位姿
Eigen::Matrix<T, 3, 1> v2_mat{ v2[0], v2[1], v2[2] }; /// 第二个节点的位姿
Eigen::Matrix<T, 3, 1> m = measurement.template cast<T>(); /// 相对位姿的观测值
Eigen::Map<Eigen::Matrix<T, 3, 1>> error{ residual }; /// 误差

先获取我们要用的主要参数。这里注意一下:

  • 大部分需要计算的变量都要声明为 T 类型的,而因为 AutoDiffFunctor::measurement 本身的类型是 Eigen::Vector3d 为了可以在模板函数中进行计算也需要手动转为 T
  • Eigen::Map<Eigen::Matrix<T, 3, 1>> error{ residual }: 这里主要是利用 Eigen::Mapresidual 包裹起来,大致上就是将 residual 映射成 Eigen::Matrix<T, 3, 1>,我们在操作的 error 的时候 residual 也会随之更新。
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
// calculate error from translation and rotation respectively and combine them together
Eigen::Matrix<T, 3, 3> X1 = PoseToTrans(v1_mat);
Eigen::Matrix<T, 3, 3> X2 = PoseToTrans(v2_mat);
Eigen::Matrix<T, 3, 3> Z = PoseToTrans(m);

Eigen::Matrix<T, 2, 2> Ri = X1.block(0, 0, 2, 2);
Eigen::Matrix<T, 2, 2> Rj = X2.block(0, 0, 2, 2);
Eigen::Matrix<T, 2, 2> Rij = Z.block(0, 0, 2, 2);

Eigen::Matrix<T, 2, 1> ti{ v1_mat(0), v1_mat(1) };
Eigen::Matrix<T, 2, 1> tj{ v2_mat(0), v2_mat(1) };
Eigen::Matrix<T, 2, 1> tij{ m(0), m(1) };

Eigen::Matrix<T, 2, 2> dRiT_dtheta; // derivative of Ri^T over theta
dRiT_dtheta(0, 0) = T(-1) * Ri(1, 0); // cosX -> -sinX
dRiT_dtheta(0, 1) = T( 1) * Ri(0, 0); // sinX -> cosX
dRiT_dtheta(1, 0) = T(-1) * Ri(0, 0); // -sinX -> -cosX
dRiT_dtheta(1, 1) = T(-1) * Ri(1, 0); // cosX -> -sinX

// calcuate error & normalize error on theta
error.template segment<2>(0) = Rij.transpose() * (Ri.transpose() * (tj - ti) - tij);
error(2) = v2_mat(2) - v1_mat(2) - m(2);
if (error(2) > T(M_PI)) {
error(2) -= T(2 * M_PI);
} else if (error(2) < T(-1 * M_PI)) {
error(2) += T(2 * M_PI);
}
error = sqrt_info_matrix.template cast<T>() * error;

这一部分主要就是根据 v1v2 的位姿计算出预测的相对位姿,再和实际观测到的相对位姿 measurement 作比较,数学原理可以参考:基于图优化的激光 SLAM 方法,这里不再赘述。最后让函数返回 true 即可。

给定一条边 Edge,生成 CostFunction 的方法为:
new ceres::AutoDiffCostFunction<AutoDiffFunctor, 3, 3, 3>(new AutoDiffFunctor(edge))

1
2
3
static ceres::CostFunction* create(const myEdge& edge) {
return (new ceres::AutoDiffCostFunction<AutoDiffFunctor, 3, 3, 3>(new AutoDiffFunctor(edge)));
}

模板的维度由节点和误差的维度决定:第一个 3 表示误差的维度是 3 (dx, dy, dtheta),第二个和第三个 3 表示节点维度为 3 ( x, y, theta )。将生成 Cost Function 的操作包装在这个类的一个工厂函数中,因此在主函数中生成一个 CostFunction 只需要调用工厂函数即可,比较简洁:

1
ceres::CostFunction* cost_function = AutoDiffFunctor::create(edge);

在添加了边之后还需要添加待优化节点,方法是:

1
problem.AddResidualBlock(cost_function, nullptr, Vertexs[edge.xi].data(), Vertexs[edge.xj].data());

AddResidualBlock 的各项参数含义为:

  • cost_function: 如上定义,主要定义了待求解非线性最小二乘问题
  • loss_function:损失函数,本篇先不介绍,不特意指定时传入 nullptr 即可
  • Vertexs[edge.xi].data(), Vertexs[edge.xj].data(): 待优化节点的地址,主要由于我们需要该节点被优化,因此一定要通过指针传递,且传递的必须是待优化节点的地址。像下面一样的操作是不行的:
1
2
3
4
// 错误!此时优化是 v1 和 v2 (副本)而不是 Vertexs[edge.xi], Vertexs[edge.xi]
auto v1 = Vertexs[edge.xi];
auto v2 = Vertexs[edge.xi];
problem.AddResidualBlock(cost_function, nullptr, v1.data(), v2.data());

Ceres 实现解析求导

这个问题由于不是很复杂,所以除了使用自动求导以外我们还可以使用解析求导,即除了手动定义误差函数以外,还定义了误差函数针对各个待优化变量的 Jacobian。定义的方法和上面差不多,也是需要先声明一个类,如下所示:

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
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
class AnalyticDiffFunction : public ceres::SizedCostFunction<3, 3, 3> {
public:
virtual ~AnalyticDiffFunction() {}

AnalyticDiffFunction(const myEdge& edge): measurement(edge.measurement), sqrt_info_matrix(edge.infoMatrix.array().sqrt()) {}

virtual bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const {

Eigen::Vector3d xi{ parameters[0][0], parameters[0][1], parameters[0][2] };
Eigen::Vector3d xj{ parameters[1][0], parameters[1][1], parameters[1][2] };

Eigen::Map<Eigen::Vector3d> error_ij{ residuals };
Eigen::Matrix3d Ai;
Eigen::Matrix3d Bi;

Eigen::Matrix3d Xi = PoseToTrans(xi);
Eigen::Matrix2d Ri = Xi.block(0, 0, 2, 2);
Eigen::Vector2d ti{ xi(0), xi(1) };

Eigen::Matrix3d Xj = PoseToTrans(xj);
Eigen::Matrix2d Rj = Xj.block(0, 0, 2, 2);
Eigen::Vector2d tj{ xj(0), xj(1) };

Eigen::Matrix3d Z = PoseToTrans(measurement);
Eigen::Matrix2d Rij = Z.block(0, 0, 2, 2);
Eigen::Vector2d tij{ measurement(0), measurement(1) };

Eigen::Matrix2d dRiT_dtheta; // derivative of Ri^T over theta
dRiT_dtheta(0, 0) = -1 * Ri(1, 0); // cosX -> -sinX
dRiT_dtheta(0, 1) = 1 * Ri(0, 0); // sinX -> cosX
dRiT_dtheta(1, 0) = -1 * Ri(0, 0); // -sinX -> -cosX
dRiT_dtheta(1, 1) = -1 * Ri(1, 0); // cosX -> -sinX

// calcuate error & normalize error on theta
error_ij.segment<2>(0) = Rij.transpose() * (Ri.transpose() * (tj - ti) - tij);
error_ij(2) = xj(2) - xi(2) - measurement(2);
if (error_ij(2) > M_PI) {
error_ij(2) -= 2 * M_PI;
} else if (error_ij(2) < -1 * M_PI) {
error_ij(2) += 2 * M_PI;
}

Ai.setZero();
Ai.block(0, 0, 2, 2) = -Rij.transpose() * Ri.transpose();
Ai.block(0, 2, 2, 1) = Rij.transpose() * dRiT_dtheta * (tj - ti);
Ai(2, 2) = -1.0;

Bi.setIdentity();
Bi.block(0, 0, 2, 2) = Rij.transpose() * Ri.transpose();

error_ij = sqrt_info_matrix * error_ij;

if(jacobians){
if(jacobians[0])
{
Eigen::Map<Eigen::Matrix<double,3,3,Eigen::RowMajor> > jacobian_xi(jacobians[0]);
jacobian_xi = sqrt_info_matrix * Ai;
}
if(jacobians[1])
{
Eigen::Map<Eigen::Matrix<double,3,3,Eigen::RowMajor> > jacobian_xj(jacobians[1]);
jacobian_xj = sqrt_info_matrix * Bi;
}
}

return true;
}

private:
Eigen::Vector3d measurement;
Eigen::Matrix3d sqrt_info_matrix;
};

可以看出来基本的思路都是差不多的,区别在于这个类就可以作为 CostFunction 使用。我们需要继承 ceres::SizedCostFunction 来定义一个针对我们这个问题特定的子类,并且数据类型固定为 doubleceres::SizedCostFunction 的维度 <n1, n2, n3, ...> 同上分别为误差的维度以及每一个节点的维度。接下来分别看每个函数:

  • AnalyticDiffFunction(const myEdge& edge) 和上述仿函数类似,每一个 AnalyticDiffFunction 关联一条边,因此生成过程中传入边的相关信息并储存。
  • virtual bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const;:核心函数,跟自动求导中用仿函数中 operator() 类似,这里重写了基类中的 evaluate 函数,并且需要在其中定义误差和雅克比矩阵的计算方式:
    • double const* const* parameters:参数块,主要是传入的各个节点,第一维度为节点的个数(这里是 3),第二维度则是对应节点的维度(这里两个节点的维度都是 3)
    • double* residuals:残差,维度取决于误差的维度,这里是 3
    • double** jacobians:雅克比矩阵,第一个维度为节点的个数(也就是要求解的雅克比矩阵的数量),第二个维度为对应雅克比矩阵的元素个数(这里两个雅克比矩阵的维度都是 3x3,因此维度是 9)

下面看一下内部具体实现:

1
2
3
Eigen::Vector3d xi{ parameters[0][0], parameters[0][1], parameters[0][2] };
Eigen::Vector3d xj{ parameters[1][0], parameters[1][1], parameters[1][2] };
Eigen::Map<Eigen::Vector3d> error_ij{ residuals };

和自动求导差不多,将要使用的变量转为 Eigen 矩阵方便使用。

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
Eigen::Matrix3d Xi = PoseToTrans(xi);
Eigen::Matrix2d Ri = Xi.block(0, 0, 2, 2);
Eigen::Vector2d ti{ xi(0), xi(1) };

Eigen::Matrix3d Xj = PoseToTrans(xj);
Eigen::Matrix2d Rj = Xj.block(0, 0, 2, 2);
Eigen::Vector2d tj{ xj(0), xj(1) };

Eigen::Matrix3d Z = PoseToTrans(measurement);
Eigen::Matrix2d Rij = Z.block(0, 0, 2, 2);
Eigen::Vector2d tij{ measurement(0), measurement(1) };

Eigen::Matrix2d dRiT_dtheta; // derivative of Ri^T over theta
dRiT_dtheta(0, 0) = -1 * Ri(1, 0); // cosX -> -sinX
dRiT_dtheta(0, 1) = 1 * Ri(0, 0); // sinX -> cosX
dRiT_dtheta(1, 0) = -1 * Ri(0, 0); // -sinX -> -cosX
dRiT_dtheta(1, 1) = -1 * Ri(1, 0); // cosX -> -sinX

// calcuate error & normalize error on theta
error_ij.segment<2>(0) = Rij.transpose() * (Ri.transpose() * (tj - ti) - tij);
error_ij(2) = xj(2) - xi(2) - measurement(2);
if (error_ij(2) > M_PI) {
error_ij(2) -= 2 * M_PI;
} else if (error_ij(2) < -1 * M_PI) {
error_ij(2) += 2 * M_PI;
}

error_ij = sqrt_info_matrix * error_ij;

定义残差的计算方式,和自动求导的过程一样

1
2
3
4
5
6
7
8
9
Eigen::Matrix3d Ai;
Eigen::Matrix3d Bi;
Ai.setZero();
Ai.block(0, 0, 2, 2) = -Rij.transpose() * Ri.transpose();
Ai.block(0, 2, 2, 1) = Rij.transpose() * dRiT_dtheta * (tj - ti);
Ai(2, 2) = -1.0;

Bi.setIdentity();
Bi.block(0, 0, 2, 2) = Rij.transpose() * Ri.transpose();

定义雅克比矩阵的计算方式,具体数学原理参考:基于图优化的激光 SLAM 方法

1
2
3
4
5
6
7
8
9
10
11
12
if(jacobians){
if(jacobians[0])
{
Eigen::Map<Eigen::Matrix<double,3,3,Eigen::RowMajor> > jacobian_xi(jacobians[0]);
jacobian_xi = sqrt_info_matrix * Ai;
}
if(jacobians[1])
{
Eigen::Map<Eigen::Matrix<double,3,3,Eigen::RowMajor> > jacobian_xj(jacobians[1]);
jacobian_xj = sqrt_info_matrix * Bi;
}
}

更新雅克比矩阵,这里和残差的不同,jacobians 是可选项,也就是传入的参数可能会存在 nullptr 的情况。这里附上官方的说明:

If jacobians is nullptr, the user is only expected to compute the residuals.

jacobians[i] is a row-major array of size num_residuals x parameter_block_sizes_[i].

If jacobians[i] is not nullptr, the user is required to compute the Jacobian of the residual vector with respect to parameters[i] and store it in this array

大致意思就是我们需要检查 jacobians 是不是空指针,只有当不是空指针我们才需要计算。

解析求导的过程基本就是这样,添加边和节点的过程也是类似的,区别就是我们生成的 AnalyticDiffFunction 本身就可以作为 ceres::CostFunction (因为是它的子类):

1
2
ceres::CostFunction* cost_function = new AnalyticDiffFunction(edge);
problem.AddResidualBlock(cost_function, nullptr, Vertexs[edge.xi].data(), Vertexs[edge.xj].data());

性能比较

最后比较了一下两者的性能,数据集中包含的边 3995个,结果分别如下:

自动求导

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
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
Edges:3995
initError: 3.08592e+08
Use ceres
iter cost cost_change |gradient| |step| tr_ratio tr_radius ls_iter iter_time total_time
0 1.542960e+08 0.00e+00 3.20e+06 0.00e+00 0.00e+00 1.00e+04 0 3.67e-01 3.69e-01
1 7.897132e+06 1.46e+08 3.38e+04 6.78e+02 9.50e-01 3.00e+04 1 4.16e-01 7.85e-01
2 4.043697e+04 7.86e+06 1.82e+03 3.05e+02 9.96e-01 9.00e+04 1 4.14e-01 1.20e+00
3 5.285684e+03 3.52e+04 7.78e+01 2.41e+02 9.99e-01 2.70e+05 1 4.17e-01 1.62e+00
4 5.174879e+03 1.11e+02 2.88e+00 1.40e+02 9.99e-01 8.10e+05 1 4.20e-01 2.04e+00
5 5.172348e+03 2.53e+00 9.94e-02 3.48e+01 1.00e+00 2.43e+06 1 4.22e-01 2.46e+00
6 5.172333e+03 1.58e-02 2.80e-03 3.24e+00 1.00e+00 7.29e+06 1 4.18e-01 2.88e+00

Solver Summary (v 1.13.0-eigen-(3.3.4)-lapack-suitesparse-(5.1.2)-cxsparse-(3.1.9)-openmp)

Original Reduced
Parameter blocks 1941 1941
Parameters 5823 5823
Residual blocks 3995 3995
Residual 11985 11985

Minimizer TRUST_REGION

Sparse linear algebra library SUITE_SPARSE
Trust region strategy LEVENBERG_MARQUARDT

Given Used
Linear solver SPARSE_NORMAL_CHOLESKY SPARSE_NORMAL_CHOLESKY
Threads 1 1
Linear solver threads 1 1
Linear solver ordering AUTOMATIC 1941

Cost:
Initial 1.542960e+08
Final 5.172333e+03
Change 1.542909e+08

Minimizer iterations 7
Successful steps 7
Unsuccessful steps 0

Time (in seconds):
Preprocessor 0.0024

Residual evaluation 0.2763
Jacobian evaluation 2.6158
Linear solver 0.0191
Minimizer 2.9155

Postprocessor 0.0001
Total 2.9180

Termination: CONVERGENCE (Function tolerance reached. |cost_change|/cost: 2.813346e-09 <= 1.000000e-06)

Takes :2.97861 Seconds.
FinalError: 10344.7

解析求导

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
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
Edges:3995
initError: 3.08592e+08
Use ceres
iter cost cost_change |gradient| |step| tr_ratio tr_radius ls_iter iter_time total_time
0 1.542960e+08 0.00e+00 3.20e+06 0.00e+00 0.00e+00 1.00e+04 0 1.14e-01 1.16e-01
1 7.897132e+06 1.46e+08 3.38e+04 6.78e+02 9.50e-01 3.00e+04 1 2.06e-01 3.23e-01
2 4.043697e+04 7.86e+06 1.82e+03 3.05e+02 9.96e-01 9.00e+04 1 2.08e-01 5.30e-01
3 5.285684e+03 3.52e+04 7.78e+01 2.41e+02 9.99e-01 2.70e+05 1 2.14e-01 7.44e-01
4 5.174879e+03 1.11e+02 2.88e+00 1.40e+02 9.99e-01 8.10e+05 1 2.09e-01 9.53e-01
5 5.172348e+03 2.53e+00 9.94e-02 3.48e+01 1.00e+00 2.43e+06 1 2.10e-01 1.16e+00
6 5.172333e+03 1.58e-02 2.80e-03 3.24e+00 1.00e+00 7.29e+06 1 2.15e-01 1.38e+00

Solver Summary (v 1.13.0-eigen-(3.3.4)-lapack-suitesparse-(5.1.2)-cxsparse-(3.1.9)-openmp)

Original Reduced
Parameter blocks 1941 1941
Parameters 5823 5823
Residual blocks 3995 3995
Residual 11985 11985

Minimizer TRUST_REGION

Sparse linear algebra library SUITE_SPARSE
Trust region strategy LEVENBERG_MARQUARDT

Given Used
Linear solver SPARSE_NORMAL_CHOLESKY SPARSE_NORMAL_CHOLESKY
Threads 1 1
Linear solver threads 1 1
Linear solver ordering AUTOMATIC 1941

Cost:
Initial 1.542960e+08
Final 5.172333e+03
Change 1.542909e+08

Minimizer iterations 7
Successful steps 7
Unsuccessful steps 0

Time (in seconds):
Preprocessor 0.0020

Residual evaluation 0.6382
Jacobian evaluation 0.8106
Linear solver 0.0187
Minimizer 1.4716

Postprocessor 0.0001
Total 1.4738

Termination: CONVERGENCE (Function tolerance reached. |cost_change|/cost: 2.813348e-09 <= 1.000000e-06)

Takes :1.53347 Seconds.
FinalError: 10344.7

比较一下可以看到,两种方法最后精度差不多,解析求导的性能略优于数值求导(时间较快)。不过在实际过程中大部分的问题求解雅克比矩阵都比较麻烦,所以通常建议使用自动求导。