Ceres 获取雅可比及海塞矩阵

Reference：

1. 基于ceres-solver 的边缘化的实现
2. Ceres 官方文档
3. Ceres jacobian and hessian evaluation

相关文章：

1. Ceres-Solver 官方文档-其一
2. Ceres-Solver 官方文档-其二

在执行边缘化过程中，我们需要构建整个问题对应的 Hessian 矩阵，然后将要被边缘化的状态变量移动到 Hessian 矩阵的右下角或者左下角，执行舒尔补。因此我们可以将边缘化问题拆解成两个子问题：Hessian 矩阵的构建和被边缘化的状态变量移动到 Hessian 矩阵的右下或左下角。

而本文仅记录使用 Ceres 的接口获取优化问题中 Jacobian 和 Hessian 矩阵的过程。

1. Ceres 接口

Problem::Evaluate

Ceres 中提供了输出 cost、residual、gradient 以及 jacobian 的接口：

bool Problem::Evaluate(const Problem::EvaluateOptions &options, 
                       double* cost, 
                       vector<double>* residuals,
                       vector<double>* gradient,
                       CRSMatrix* jacobian)

该接口用于评估 Problem，任何的输出空指针都可以为 NULL。使用哪些残差块和参数块由的 Problem::EvaluateOptions 结构体控制。

1. Note 1
   计算将使用存储在构造问题时使用的参数块指针所指向的内存位置中的值，例如在以下代码中：

   Problem problem;
   double x = 1;
   problem.Add(new MyCostFunction, nullptr, &x);
   
   double cost = 0.0;
   problem.Evaluate(Problem::EvaluateOptions(), &cost, nullptr, nullptr, nullptr);
   

   cost 在 x=1 处评估。如果想求 x=2 时的值，那么：

   x = 2;
   problem.Evaluate(Problem::EvaluateOptions(), &cost, nullptr, nullptr, nullptr);
   

2. Note 2
   如果没有使用 Manifold(local parameterizations)，那么梯度向量(以及雅可比矩阵的列数)的大小是所有参数块大小的总和，参数块的排序由定义 problem 时状态变量的存储顺序决定。如果参数块具有流形，则它将 “TangentSize” 条目贡献给梯度向量(以及雅可比矩阵的列数)。

3. Note 3
   问题正在被解决时不能调用此函数，例如，在求解过程中，不能在一个迭代结束时从 IterationCallback 调用它。

CRSMatrix

struct CERES_EXPORT CRSMatrix {
  CRSMatrix() : num_rows(0), num_cols(0) {}

  int num_rows;
  int num_cols;

  std::vector<int> cols;
  std::vector<int> rows;
  std::vector<double> values;
};

CRSMatrix 是一种压缩的行稀疏矩阵，主要用于将雅可比矩阵传递给用户。

一个压缩的行矩阵将其内容存储在三个数组中：行、列和值。

• rows 是一个 num_rows+1 大小的数组，指向 cols 和 values 数组。对于每一行 i：
• cols[rows[i]] ... cols[rows[i + 1] - 1] 是第 i 行非零列的下标。
• values[rows[i]] .. values[rows[i + 1] - 1] 是对应条目的值。
• cols 和 values 包含的条目与矩阵中非零的条目一样多。

比方说有一个 3x4 的稀疏矩阵：

[ 0 10  0  4 ]
[ 0  2 -3  2 ]
[ 1  2  0  0 ]

那么三个数组会是：

          -row0-  ---row1---  -row2-
rows   = [ 0,      2,          5,     7]
cols   = [ 1,  3,  1,  2,  3,  0,  1]
values = [10,  4,  2, -3,  2,  1,  2]

这样做存储是因为这是稀疏矩阵，以这种方式存储相比于每一个值都存下来会更省空间。

• rows 以 0 开始，第 0 行有两个值，所以后面一个的值为 2，第 1 行有三个值，所以再后面一个值为 5；
• 与上面的 0、2、5、7 对应，cols 与 valus 第 0 个开始为第 0 行的数，第 2 个开始为第 1 行的数，…。

CRSMatrix 转 Eigen

ceres::CRSMatrix 转 Eigen 参考以下链接：https://github.com/vitalemonate/testCRSMatrix

Eigen::MatrixXd CRSMatrix2EigenMatrix(ceres::CRSMatrix *jacobian_crs_matrix)
{
    Eigen::MatrixXd J(jacobian_crs_matrix->num_rows, jacobian_crs_matrix->num_cols);
    J.setZero();

    std::vector<int> jacobian_crs_matrix_rows, jacobian_crs_matrix_cols;
    std::vector<double> jacobian_crs_matrix_values;
    jacobian_crs_matrix_rows = jacobian_crs_matrix->rows;
    jacobian_crs_matrix_cols = jacobian_crs_matrix->cols;
    jacobian_crs_matrix_values = jacobian_crs_matrix->values;

    int cur_index_in_cols_and_values = 0;
    // rows is a num_rows + 1 sized array
    int row_size = static_cast<int>(jacobian_crs_matrix_rows.size()) - 1;
    // outer loop traverse rows, inner loop traverse cols and values
    for (int row_index = 0; row_index < row_size; ++row_index)
    {
        while (cur_index_in_cols_and_values < jacobian_crs_matrix_rows[row_index + 1])
        {
            J(row_index, jacobian_crs_matrix_cols[cur_index_in_cols_and_values]) = jacobian_crs_matrix_values[cur_index_in_cols_and_values];
            cur_index_in_cols_and_values++;
        }
    }
    return J;
}

2. 雅可比和海塞矩阵示例

void Estimator::optimization(){
    // ... 
    TicToc t_solver;
    ceres::Solver::Summary summary;
    ceres::Solve(options, &problem, &summary);
    //cout << summary.BriefReport() << endl;
    ROS_DEBUG("Iterations : %d", static_cast<int>(summary.iterations.size()));
    ROS_DEBUG("solver costs: %f", t_solver.toc());

    evaluateBA(problem, summary);
    cout << "evaluateBA end" << endl;
}

void Estimator::evaluateBA(ceres::Problem& problem, const ceres::Solver::Summary& summary){
    cout << summary.FullReport() << endl;

    ceres::Problem::EvaluateOptions EvalOpts;
    ceres::CRSMatrix jacobian_crs_matrix;
    problem.Evaluate(EvalOpts, nullptr, nullptr, nullptr, &jacobian_crs_matrix);

    TicToc t_convert_J;
    Eigen::MatrixXd J = CRSMatrix2EigenMatrix(&jacobian_crs_matrix);
    cout << "convert sparse matrix cost " << t_convert_J.toc() << endl;

    TicToc t_construct_H;
    Eigen::MatrixXd H = J.transpose()*J;
    cout << "construct H cost " << t_construct_H.toc() << endl;
}

注意这里计算出来的 Hessian 矩阵是考虑了各代价函数的信息矩阵的，如果修改各代价函数信息矩阵代销，这里得到的 Hessian 也会相应发生变化。