基于gici多传感器融合定位的图优化代码学习

最新推荐文章于 2024-07-08 17:49:05 发布

进击中的小龙

最新推荐文章于 2024-07-08 17:49:05 发布

阅读量1k

点赞数 16

分类专栏： C/C++ 组合导航文章标签：学习算法 c++

本文链接：https://blog.csdn.net/yitian_00/article/details/136082024

版权

组合导航同时被 2 个专栏收录

5 篇文章 4 订阅

订阅专栏

C/C++

2 篇文章 0 订阅

订阅专栏

前言

本文是基于gici-open项目对因子图优化GraphC++类 的学习，由于此项目的最小二乘估计部分采用了google的开源ceres库，可以从ceres的官方帮助文档处了解：Solving Non-linear Least Squares — Ceres Solver (ceres-solver.org)

在graph.h的406行处找到：

 /// @brief Solve the optimization problem.
  void solve()
  {
    Solve(options, problem_.get(), &summary);//调用ceres库的Solve函数进行融合参数估计
  }

**options ** 是ceres求解过程中的参数设置，比如设定迭代时间，迭代次数，使用什么样的方法

problem_ 其中定义了残差块即最小二乘估计的数据来源(参数块，残差块)以及描述参数块之间关系的代价函数(cost_function)、表达残差权值的损失函数(loss_function),虽然ceres开发者极力推荐自动求导求系数阵，但该项目开发者还是自己定义了方法去求导

&summary 是估计出来的结果

更多关于ceres的基础知识可以前往下面链接：

Ceres学习笔记建模篇001_代价函数基类CostFunction及其派生类SizedCostFunction介绍_ceres::sizedcostfunction-CSDN博客

1、构造函数Graph()

// 构造函数。
Graph::Graph()
    : residual_counter_(0)
{
  // 设置 Ceres Solver Problem 的选项。
  ceres::Problem::Options problemOptions;
  problemOptions.local_parameterization_ownership =
      ceres::Ownership::DO_NOT_TAKE_OWNERSHIP;
  problemOptions.loss_function_ownership =
      ceres::Ownership::DO_NOT_TAKE_OWNERSHIP;
  problemOptions.cost_function_ownership =
      ceres::Ownership::DO_NOT_TAKE_OWNERSHIP;

  // 创建一个新的 Ceres Solver Problem，并用指定的选项初始化它。
  problem_.reset(new ceres::Problem(problemOptions));

}

成员初始化列表：
- residual_counter_(0)：用值 0 初始化成员变量 residual_counter_。该变量是 Graph 类的成员。
Ceres Solver Problem 选项：
- ceres::Problem::Options：这是一个用于配置 Ceres Solver Problem 类行为的选项结构。
- 下面的几行设置了 Ceres Solver Problem 的一些选项：
  - problemOptions.local_parameterization_ownership：指定局部参数化的所有权策略。
  - problemOptions.loss_function_ownership：指定损失函数的所有权策略。
  - problemOptions.cost_function_ownership：指定代价函数的所有权策略。
Ceres Solver Problem 初始化：
- problem_.reset(new ceres::Problem(problemOptions))：这一行创建了一个新的 ceres::Problem 类的实例，并用指定的选项进行初始化。reset 函数用于管理 Problem 对象的所有权。

2、parameterBlockExists()

// Check whether a certain parameter block is part of the graph.
bool Graph::parameterBlockExists(uint64_t parameter_block_id) const
{  使用 id_to_parameter_block_map_ 查找特定参数块的 ID
  if (id_to_parameter_block_map_.find(parameter_block_id)
      == id_to_parameter_block_map_.end())
  {// 如果参数块的 ID 未找到，返回 false，表示参数块不存在于图中
    return false;
  }
  return true;
}

3、gttLhs()

//获取特定参数的海森矩阵块(即二阶导参数系数阵)
// Obtain the Hessian block for a specific parameter block.
void Graph::getLhs(uint64_t parameter_block_id, Eigen::MatrixXd& H)
{
  CHECK(parameterBlockExists(parameter_block_id))
      << "parameter block not in graph.";
  ResidualBlockCollection res = residuals(parameter_block_id);//根据参数块id获取残差块
  H.setZero();//初始化海森矩阵
  for (size_t i = 0; i < res.size(); ++i)//循环处理每个残差块
  {

    // parameters:
    ParameterBlockCollection pars = parameters(res[i].residual_block_id);//获取残差块相应的参数块
    //为参数块和残差块分配内存
    double** parameters_raw = new double*[pars.size()];
    Eigen::VectorXd residuals_eigen(res[i].error_interface_ptr->residualDim());
    double* residuals_raw = residuals_eigen.data();
    //为雅克比矩阵分配内存
    double** jacobians_raw = new double*[pars.size()];
    std::vector<
        Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>,
        Eigen::aligned_allocator<
            Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
                Eigen::RowMajor> > > jacobiansEigen(pars.size());

    double** jacobians_minimal_raw = new double*[pars.size()];
    std::vector<
        Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>,
        Eigen::aligned_allocator<
            Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
                Eigen::RowMajor> > > jacobians_minimal_eigen(pars.size());

    int J = -1;
    for (size_t j = 0; j < pars.size(); ++j)
    { //对应雅克比矩阵与相应参数块
      // determine which is the relevant block
      if (pars[j].second->id() == parameter_block_id)
        J = j;
      parameters_raw[j] = pars[j].second->parameters();
      jacobiansEigen[j].resize(res[i].error_interface_ptr->residualDim(),
                               pars[j].second->dimension());
      jacobians_raw[j] = jacobiansEigen[j].data();
      jacobians_minimal_eigen[j].resize(res[i].error_interface_ptr->residualDim(),
                                      pars[j].second->minimalDimension());
      jacobians_minimal_raw[j] = jacobians_minimal_eigen[j].data();
    }

    // evaluate residual block评价残差块
    res[i].error_interface_ptr->EvaluateWithMinimalJacobians(parameters_raw,
                                                           residuals_raw,
                                                           jacobians_raw,
                                                           jacobians_minimal_raw);

    // get block将雅克比矩阵J经过J'*J赋给海森矩阵
    H += jacobians_minimal_eigen[J].transpose() * jacobians_minimal_eigen[J];

    // cleanup
    delete[] parameters_raw;
    delete[] jacobians_raw;
    delete[] jacobians_minimal_raw;
  }
}

residual(parameter_block_id)

// Get the residual blocks of a parameter block.
Graph::ResidualBlockCollection Graph::residuals(uint64_t parameter_block_id) const
{
  // get the residual blocks of a parameter block
  IdToResidualBlockMultimap::const_iterator it1 = id_to_residual_block_multimap_
      .find(parameter_block_id);//根据参数块id找到残差块
  if (it1 == id_to_residual_block_multimap_.end())//如果未找到与参数块相关联的残差块，则返回一个空的集合
    return Graph::ResidualBlockCollection();  // empty
  ResidualBlockCollection returnResiduals;
  std::pair<IdToResidualBlockMultimap::const_iterator,
      IdToResidualBlockMultimap::const_iterator> range =
      id_to_residual_block_multimap_.equal_range(parameter_block_id);//获取与参数块ID相关联的残差块范围
  for (IdToResidualBlockMultimap::const_iterator it = range.first;
      it != range.second; ++it)
  {//遍历残差块，并将每个残差块添加到returnResiduals集合中
    returnResiduals.push_back(it->second);
  }
  return returnResiduals;//返回包含与给定参数块ID相关联的所有残差块的集合
}

4、isMinimalJacobianCorrect()

// Check a Jacobian with numeric differences.通过数值差分检查雅可比矩阵是否正确
bool Graph::isMinimalJacobianCorrect(ceres::ResidualBlockId residual_block_id,
                                   double relTol) const
{ // 获取残差块的错误接口指针和参数块集合
  std::shared_ptr<const ErrorInterface> error_interface_ptr =
      errorInterfacePtr(residual_block_id);
  ParameterBlockCollection parameter_blocks = parameters(residual_block_id);

  // set up data structures for storage 设置用于存储雅克比矩阵的数据结构
  std::vector<
      Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>,
      Eigen::aligned_allocator<
          Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> > >
      J(parameter_blocks.size());//J存储完整的雅克比矩阵
  std::vector<
      Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>,
      Eigen::aligned_allocator<
          Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> > >
      J_min(parameter_blocks.size());//J_min存储简化的雅克比矩阵
  std::vector<
      Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>,
      Eigen::aligned_allocator<Eigen::Matrix<double, Eigen::Dynamic,
      Eigen::Dynamic, Eigen::RowMajor> > > J_min_numDiff(//存储简化的雅可比矩阵通过数值差分得到的结果
      parameter_blocks.size());
  std::vector<double*> parameters(parameter_blocks.size());
  std::vector<double*> jacobians(parameter_blocks.size());
  std::vector<double*> jacobians_minimal(parameter_blocks.size());
  for (size_t i = 0; i < parameter_blocks.size(); ++i)
  {
    // fill in
    J[i].resize(error_interface_ptr->residualDim(), //对第i个雅可比矩阵进行重新调整大小，使其与相应的参数块维度匹配
                parameter_blocks[i].second->dimension());
    J_min[i].resize(error_interface_ptr->residualDim(),//调整为与参数块的最小维度匹配
                    parameter_blocks[i].second->minimalDimension());
    J_min_numDiff[i].resize(error_interface_ptr->residualDim(),//为了存储数值差分得到的雅可比矩阵
                            parameter_blocks[i].second->minimalDimension());
    parameters[i] = parameter_blocks[i].second->parameters();//将第i个参数块的参数指针存储到 parameters 数组中
    jacobians[i] = J[i].data();//将第i个雅可比矩阵的数据指针存储到 jacobians 数组中
    jacobians_minimal[i] = J_min[i].data();
  }

  // calculate num diff Jacobians
  const double delta = 1e-8;//用于表示微小的变化量
  for (size_t i = 0; i < parameter_blocks.size(); ++i)//循环遍历每一个参数块
  { //循环遍历当前参数块的最小维度
    for (size_t j = 0; j < parameter_blocks[i].second->minimalDimension(); ++j)
    {
      Eigen::VectorXd residuals_p(error_interface_ptr->residualDim());//存储正变化量后的残差值
      Eigen::VectorXd residuals_m(error_interface_ptr->residualDim());//存储负变化量的残差值

      // apply positive delta
      Eigen::VectorXd parameters_p(parameter_blocks[i].second->dimension());//应用正变化量的参数向量
      Eigen::VectorXd parameters_m(parameter_blocks[i].second->dimension());//应用负变化量的参数向量
      Eigen::VectorXd plus(parameter_blocks[i].second->minimalDimension());//通过plus函数实现参数的变化
      plus.setZero();
      plus[j] = delta;
      parameter_blocks[i].second->plus(parameters[i], plus.data(),//将当前参数向量与给定的变化量相加得到新的参数向量
                                       parameters_p.data());
      parameters[i] = parameters_p.data();
      error_interface_ptr->EvaluateWithMinimalJacobians(parameters.data(), //计算应用了正变化量后的残差值
                                                       residuals_p.data(),
                                                       nullptr,
                                                       nullptr);
      parameters[i] = parameter_blocks[i].second->parameters();  // reset 将参数向量重置回原始值
      // apply negative delta 
      plus.setZero();
      plus[j] = -delta;
      parameter_blocks[i].second->plus(parameters[i], plus.data(),
                                       parameters_m.data());
      parameters[i] = parameters_m.data();
      error_interface_ptr->EvaluateWithMinimalJacobians(parameters.data(), //计算应用了负变化量后的残差值
                                                       residuals_m.data(),
                                                       nullptr,
                                                       nullptr);
      parameters[i] = parameter_blocks[i].second->parameters();  // reset
      // calculate numeric difference 利用数值差分的定义(f(x+δ) - f(x-δ))/2δ计算得到当前参数块在当前方向上的数值差分雅可比矩阵
      J_min_numDiff[i].col(j) = (residuals_p - residuals_m) * 1.0 / (2.0 * delta);//存储数值差分
    }
  }

  // calculate analytic Jacobians and compare 计算解析雅可比矩阵并进行比较
  bool isCorrect = true; //表示解析雅可比矩阵是否正确
  Eigen::VectorXd residuals(error_interface_ptr->residualDim());//用于存储残差的值
  for (size_t i = 0; i < parameter_blocks.size(); ++i)//循环遍历参数块每一个参数块
  {
    // calc 计算当前参数块的解析雅可比矩阵，并存储在 jacobians 和 jacobians_minimal 中
    error_interface_ptr->EvaluateWithMinimalJacobians(parameters.data(),
                                                     residuals.data(),
                                                     jacobians.data(),
                                                     jacobians_minimal.data());
    // check
    double norm_minimal = J_min_numDiff[i].norm();//计算数值差分雅可比矩阵的范数
    Eigen::MatrixXd J_diff_minimal = J_min_numDiff[i] - J_min[i];//计算数值差分雅可比矩阵和解析雅可比矩阵之间的差异
    double max_diff_minimal =
        std::max(-J_diff_minimal.minCoeff(), J_diff_minimal.maxCoeff());//找出差异的最大值

    if (max_diff_minimal / norm_minimal > relTol)//如果相对差异超过了预先设定的相对容差 relTol
    { //输出相关的信息，包括残差类型、数值差分雅可比矩阵、解析雅可比矩阵、相对误差和相对容差
      LOG(INFO) << "Minimal Jacobian inconsistent: "
                << kErrorToStr.at(error_interface_ptr->typeInfo());
      LOG(INFO) << "num diff Jacobian[" << i << "]:\n" << J_min_numDiff[i];
      LOG(INFO) << "provided Jacobian[" << i << "]:\n" << J_min[i];
      LOG(INFO) << "relative error: " << max_diff_minimal / norm_minimal
                << ", relative tolerance: " << relTol;
      isCorrect = false;//则将 isCorrect 设置为 false
    }
  }

  return isCorrect;
}

5、addParameterBlock

// Add a parameter block to the map 向图(ceres problem)中添加参数块
bool Graph::addParameterBlock(
    std::shared_ptr<ParameterBlock> parameter_block,
    int parameterization, const int /*group*/)
{

  CHECK(parameter_block != nullptr);
  VLOG(200) << "Adding parameter block with parameterization "
            << parameterization << " and id " << BackendId(parameter_block->id());

  // check Id availability // 检查ID是否已经存在
  if (parameterBlockExists(parameter_block->id()))
  { // 如果ID已经存在，输出错误日志并返回false
    LOG(ERROR) << "Parameter block with id " << BackendId(parameter_block->id())
               << " exists already!";
    return false;
  }
  // 将参数块的ID和智能指针添加到地图中
  id_to_parameter_block_map_.insert(
      std::pair<uint64_t, std::shared_ptr<ParameterBlock> >(
          parameter_block->id(), parameter_block));

  // also add to ceres problem  将参数块也添加到ceres problem中
  switch (parameterization)
  {
    case Parameterization::Trivial:
    { // 如果参数化方式是Trivial，则将参数块添加到ceres problem中
      problem_->AddParameterBlock(parameter_block->parameters(),
                                  parameter_block->dimension());
      break;
    }
    case Parameterization::HomogeneousPoint:
    { // 如果参数化方式是HomogeneousPoint，则将参数块添加到ceres problem中，并设置局部参数化方式
      problem_->AddParameterBlock(parameter_block->parameters(),
                                  parameter_block->dimension(),
                                  &homogeneous_point_local_parameterization_);
      parameter_block->setLocalParameterizationPtr(
          &homogeneous_point_local_parameterization_);
      break;
    }
    case Parameterization::Pose6d:
    { // 如果参数化方式是Pose6d，则将参数块添加到ceres problem中，并设置局部参数化方式
      problem_->AddParameterBlock(parameter_block->parameters(),
                                  parameter_block->dimension(),
                                  &pose_local_parameterization_);
      parameter_block->setLocalParameterizationPtr(&pose_local_parameterization_);
      break;
    }
    default:
    {
      LOG(ERROR) << "Unknown parameterization!";
      return false;
      break;  // just for consistency...
    }
  }

  /*const LocalParamizationAdditionalInterfaces* ptr =
      dynamic_cast<const LocalParamizationAdditionalInterfaces*>(
      parameter_block->localParameterizationPtr());
  if(ptr)
    std::cout<<"verify local size "<< parameter_block->localParameterizationPtr()->LocalSize() << " = "<<
            int(ptr->verify(parameter_block->parameters()))<<
            std::endl;*/

  return true;
}

6、addResidualBlock

  // add in ceres 将成本函数、损失函数和参数块指针向量传递给它，并将返回残差块的ID
  return_id = problem_->AddResidualBlock(cost_function.get(), loss_function,
                                         parameter_blocks);

  if (FLAGS_v >=200)//如果日志级别大于等于200
  {
    std::stringstream s;
    s << "Adding residual block: "
      << kErrorToStr.at(std::dynamic_pointer_cast<ErrorInterface>(cost_function)->typeInfo())
      << " with id " << return_id
      << " connected to the following parameter blocks:\n";
    for (auto block : parameter_block_ptrs)
    {
      s << BackendId(block->id()) << "\n";
    }
    VLOG(200) << s.str();
  }

  // add in book-keeping
  std::shared_ptr<ErrorInterface> error_interface_ptr = //存储cost_function的ErrorInterface接口的指针
      std::dynamic_pointer_cast<ErrorInterface>(cost_function);//将cost_function强制转换为ErrorInterface指针类型
  CHECK(error_interface_ptr!=0)
      << "Supplied a cost function without ErrorInterface";
  residual_block_id_to_residual_block_spec_map_.insert( //关联了残差块的ID和一个ResidualBlockSpec对象
      std::pair< ceres::ResidualBlockId, ResidualBlockSpec>(//存储了残差块的ID、损失函数以及ErrorInterface指针
          return_id,
          ResidualBlockSpec(return_id, loss_function, error_interface_ptr)));

  // update book-keeping 更新参数块集合的映射
  bool insertion_success;
  std::tie(std::ignore, insertion_success) = //如果插入成功，则返回return_id
      residual_block_id_to_parameter_block_collection_map_.insert(
          std::make_pair(return_id, parameter_block_collection));
  if (insertion_success == false)
  { //如果插入失败，则返回一个值为0的ceres::ResidualBlockId对象
    return ceres::ResidualBlockId(0);
  }
  //更新了涉及的参数块上的ResidualBlock指针的映射
  // update ResidualBlock pointers on involved ParameterBlocks
  for (uint64_t parameter_id = 0; //遍历参数块
      parameter_id < parameter_block_collection.size(); ++parameter_id)
  {
    id_to_residual_block_multimap_.insert(
        std::pair<uint64_t, ResidualBlockSpec>( //参数块的ID与对应的ResidualBlockSpec对象关联起来
            parameter_block_collection[parameter_id].first,
            ResidualBlockSpec(return_id, loss_function, error_interface_ptr)));
  }

  return return_id;
}

重载版本

//这个重载版本的函数使得向图中添加残差块时更加方便，可以直接传递多个参数块的智能指针，而不必手动创建参数块的指针向量
// Add a residual block. See respective ceres docu. If more are needed, see other interface.
ceres::ResidualBlockId Graph::addResidualBlock(
    std::shared_ptr< ceres::CostFunction> cost_function,
    ceres::LossFunction* loss_function,
    std::shared_ptr<ParameterBlock> x0, //x0到x9是参数块的智能指针，表示与该残差相关联的参数块
    std::shared_ptr<ParameterBlock> x1,
    std::shared_ptr<ParameterBlock> x2,
    std::shared_ptr<ParameterBlock> x3,
    std::shared_ptr<ParameterBlock> x4,
    std::shared_ptr<ParameterBlock> x5,
    std::shared_ptr<ParameterBlock> x6,
    std::shared_ptr<ParameterBlock> x7,
    std::shared_ptr<ParameterBlock> x8,
    std::shared_ptr<ParameterBlock> x9)
{

  CHECK(cost_function != nullptr);
  std::vector<std::shared_ptr<ParameterBlock> > parameter_block_ptrs;
  if (x0 != 0) //检查每个参数块是否为非空指针，如果是非空指针，则将其添加到parameter_block_ptrs向量中
  {
    parameter_block_ptrs.push_back(x0);
  }
  if (x1 != 0)
  {
    parameter_block_ptrs.push_back(x1);
  }
  if (x2 != 0)
  {
    parameter_block_ptrs.push_back(x2);
  }
  if (x3 != 0)
  {
    parameter_block_ptrs.push_back(x3);
  }
  if (x4 != 0)
  {
    parameter_block_ptrs.push_back(x4);
  }
  if (x5 != 0)
  {
    parameter_block_ptrs.push_back(x5);
  }
  if (x6 != 0)
  {
    parameter_block_ptrs.push_back(x6);
  }
  if (x7 != 0)
  {
    parameter_block_ptrs.push_back(x7);
  }
  if (x8 != 0)
  {
    parameter_block_ptrs.push_back(x8);
  }
  if (x9 != 0)
  {
    parameter_block_ptrs.push_back(x9);
  }

  return Graph::addResidualBlock(cost_function, loss_function, parameter_block_ptrs);

}

7、resetResidualBlock

// Replace the parameters connected to a residual block ID.
void Graph::resetResidualBlock(
    ceres::ResidualBlockId residual_block_id, //需要重置的残差块ID
    std::vector<std::shared_ptr<ParameterBlock> >& parameter_block_ptrs)//参数块指针
{
  // remember the residual block spec:
  ResidualBlockSpec spec =//通过residual_block_id从映射中获取相应的残差块规范，将其存储在spec中
      residual_block_id_to_residual_block_spec_map_[residual_block_id];
  // remove residual from old parameter set 删除旧参数集中的残差
  ResidualBlockIdToParameterBlockCollectionMap::iterator it =//在参数块映射中查找给定残差块，并将其迭代器存储在it中
      residual_block_id_to_parameter_block_collection_map_.find(residual_block_id);
  CHECK(it!=residual_block_id_to_parameter_block_collection_map_.end())
      << "residual block not in graph.";
  for (ParameterBlockCollection::iterator parameter_it = it->second.begin();//循环遍历参数块
      parameter_it != it->second.end(); ++parameter_it)
  {
    uint64_t parameter_id = parameter_it->second->id();//获取当前参数块的id
    std::pair<IdToResidualBlockMultimap::iterator,//在一个多重映射中查找具有给定参数ID的残差块，并将其范围存储在range中
        IdToResidualBlockMultimap::iterator> range = id_to_residual_block_multimap_
        .equal_range(parameter_id);
    CHECK(range.first!=id_to_residual_block_multimap_.end())
        << "book-keeping is broken";
    for (IdToResidualBlockMultimap::iterator it2 = range.first;//遍历range范围内的残差块
        it2 != range.second;)
    {
      if (residual_block_id == it2->second.residual_block_id)
      {//将其中与给定residual_block_id相同的残差块从多重映射中移除，从而将残差块与参数的关联删除
        it2 = id_to_residual_block_multimap_.erase(it2);  // remove book-keeping
      }
      else
      {
        it2++;
      }
    }
  }
//更新与残差块相关联的参数块集合，并确保与这些参数块相关联的残差块指针也被更新
  ParameterBlockCollection parameter_block_collection;
  for (size_t i = 0; i < parameter_block_ptrs.size(); ++i)//遍历parameter_block_ptrs中的参数块指针
  {
    parameter_block_collection.push_back(
        ParameterBlockSpec(parameter_block_ptrs.at(i)->id(),//为每个参数块创建一个ParameterBlockSpec对象
                           parameter_block_ptrs.at(i)));
  }

  // update book-keeping
  it->second = parameter_block_collection;//更新参数块

  // update ResidualBlock pointers on involved ParameterBlocks更新与参数块关联的残差块指针
  for (uint64_t parameter_id = 0;//循环遍历参数块
      parameter_id < parameter_block_collection.size(); ++parameter_id)
  { //将参数块与当前的残差块规范spec相关联，并将该关联插入到id_to_residual_block_multimap_中
    id_to_residual_block_multimap_.insert(
        std::pair<uint64_t, ResidualBlockSpec>(
            parameter_block_collection[parameter_id].first, spec));
  }
}

8、removeResidualBlock

// Remove a residual block.从图中移除一个残差块
bool Graph::removeResidualBlock(ceres::ResidualBlockId residual_block_id)
{
  VLOG(200) << "Removing residual block with ID " << residual_block_id;
  //通过给定的残差块id在映射中查找对应的参数块，并将其迭代器存储在it中
  ResidualBlockIdToParameterBlockCollectionMap::iterator it =
      residual_block_id_to_parameter_block_collection_map_.find(residual_block_id);
  if (it == residual_block_id_to_parameter_block_collection_map_.end())
  {
    LOG(ERROR) << "Residual block " << residual_block_id << " not in graph!";
    return false;
  }
  //使用Ceres Solver提供的RemoveResidualBlock函数，从Ceres中移除给定的残差块
  problem_->RemoveResidualBlock(residual_block_id);  // remove in ceres

  for (ParameterBlockCollection::iterator parameter_it = it->second.begin();
      parameter_it != it->second.end(); ++parameter_it)//遍历参数块
  {
    uint64_t parameter_id = parameter_it->second->id();//获取参数块的id
    std::pair<IdToResidualBlockMultimap::iterator,//在多重映射中查找与当前参数块ID相关联的所有残差块，并将范围存储在range中
        IdToResidualBlockMultimap::iterator> range = id_to_residual_block_multimap_
        .equal_range(parameter_id);
    CHECK(range.first!=id_to_residual_block_multimap_.end())
        << "book-keeping is broken";

    for (IdToResidualBlockMultimap::iterator it2 = range.first;
        it2 != range.second;)//遍历与当前参数块关联的所有残差块，并将其中与给定残差块ID相同的残差块从多重映射中移除
    {
      if (residual_block_id == it2->second.residual_block_id)
      {
        it2 = id_to_residual_block_multimap_.erase(it2);  // remove book-keeping
      }
      else
      {
        it2++;
      }
    }
  }
  residual_block_id_to_parameter_block_collection_map_.erase(it);  // remove book-keeping移除残差块的记录
  residual_block_id_to_residual_block_spec_map_.erase(residual_block_id);  // remove book-keeping移除残差块规范
  return true;
}

9、computeCovariance

计算给定参数块的协方差矩阵

// Get covariance estimation of given parameter blocks
bool Graph::computeCovariance(
    const std::vector<uint64_t>& parameter_block_ids,
    Eigen::MatrixXd& covariance,
    bool use_dense_svd)
{
  // Get parameters 获取参数信息
  std::vector<const double*> parameters;//参数指针
  std::vector<size_t> parameter_block_sizes;//参数维度
  std::vector<size_t> parameter_block_starts;//存储每个参数块在总参数向量中的起始位置
  size_t parameter_size = 0;//存储总参数向量的大小
  for (size_t i = 0; i < parameter_block_ids.size(); i++) {//遍历参数块
    auto it = id_to_parameter_block_map_.find(parameter_block_ids[i]);//获取对应的参数块
    if (it == id_to_parameter_block_map_.end()) {
      LOG(ERROR) << "Parameter block does not exist!";
      return false;
    }
    auto& parameter_block = it->second;
    parameters.push_back(parameter_block->parameters());// 将参数信息存储到向量中
    parameter_block_sizes.push_back(parameter_block->dimension());
    parameter_block_starts.push_back(parameter_size);
    parameter_size += parameter_block->dimension();
  }

  // Make pairs 使用了嵌套的循环来生成所有可能的参数块对，以便后续计算协方差
  std::vector<std::pair<const double*, const double*> > covariance_blocks;
  for (size_t i = 0; i < parameters.size(); i++) {
    for (size_t j = i; j < parameters.size(); j++) {
      covariance_blocks.push_back(std::make_pair(parameters[i], parameters[j]));//可以看做是上三角矩阵
    }
  }

  // Compute covariance
  ceres::Covariance::Options options;//配置ceres计算的参数
  // it can deal with rank deficient problem but very slow
  if (use_dense_svd) {//使用密集SVD算法进行计算
    options.algorithm_type = ceres::DENSE_SVD;
    options.null_space_rank = -1; //处理秩亏问题
  }
  ceres::Covariance covariance_handle(options);//创建一个Ceres协方差计算器对象
  if (!covariance_handle.Compute(covariance_blocks, problem_.get())) {//进行协方差计算，如果计算失败，则记录警告并退出
    LOG(WARNING) << "Failed to compute covariance!";
    return false;
  }
//根据参数块的维度和起始位置，从协方差计算器对象中获取对应的协方差块，并将其填充到最终的协方差矩阵中
  // Get covariance
  covariance.resize(parameter_size, parameter_size);
  for (size_t i = 0; i < parameters.size(); i++) {
    for (size_t j = i; j < parameters.size(); j++) {
      size_t size_i = parameter_block_sizes[i];//参数块大小
      size_t size_j = parameter_block_sizes[j];
      size_t start_i = parameter_block_starts[i];//参数块起始指针
      size_t start_j = parameter_block_starts[j];
      const double* para_i = parameters[i];
      const double* para_j = parameters[j];
      Eigen::MatrixXd cov_i_j = Eigen::MatrixXd::Zero(size_i, size_j);//协方差块置0
      covariance_handle.GetCovarianceBlock(para_i, para_j, cov_i_j.data());//从解算器中获取指定协方差块
      covariance.block(start_i, start_j, size_i, size_j) = cov_i_j;//上三角矩阵
      covariance.block(start_j, start_i, size_j, size_i) = cov_i_j.transpose();//协方差矩阵转置对称
    }
  }

  return true;
}

10、computeTotalCost

// Evaluate all residual blocks and get total cost评估所有残差块和获取总的成本
double Graph::computeTotalCost(bool apply_loss_function)
{
  ResidualBlockCollection residual_collection = residuals();//获取图中的所有残差块
  double total_cost_squared = 0.0;//用于累积所有残差块的成本的平方和
  for (auto& residual : residual_collection)//遍历所有残差块
  {//调用Ceres Solver提供的EvaluateResidualBlock函数，评估当前残差块的残差，并将结果存储在cost向量中
    Eigen::VectorXd cost = 
      Eigen::VectorXd::Zero(residual.error_interface_ptr->residualDim());
    problem_->EvaluateResidualBlock(residual.residual_block_id, 
      apply_loss_function, cost.data(), nullptr, nullptr);
    const double cost_norm = cost.norm();//计算当前残差块的代价的范数
    total_cost_squared += cost_norm * cost_norm;//将范数的平方累积到 total_cost_squared
  }
  return sqrt(total_cost_squared);//返回总代价的平方根
}

resdiuals

// Get all the residual blocks
Graph::ResidualBlockCollection Graph::residuals() const
{
  ResidualBlockCollection returnResiduals;
  for (auto it : residual_block_id_to_residual_block_spec_map_) {
    returnResiduals.push_back(it.second);
  }
  return returnResiduals;
}