前言
本文是基于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 SolverProblem类行为的选项结构。- 下面的几行设置了 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_ 查找特定参数块的 IDif (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 blockif (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];// cleanupdelete[] 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 blockIdToResidualBlockMultimap::const_iterator it1 = id_to_residual_block_multimap_.find(parameter_block_id);//根据参数块id找到残差块if (it1 == id_to_residual_block_multimap_.end())//如果未找到与参数块相关联的残差块，则返回一个空的集合return Graph::ResidualBlockCollection(); // emptyResidualBlockCollection 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 inJ[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 Jacobiansconst 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 deltaEigen::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());// checkdouble 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已经存在，输出错误日志并返回falseLOG(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 将成本函数、损失函数和参数块指针向量传递给它，并将返回残差块的IDreturn_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-keepingstd::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_idresidual_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 ParameterBlocksfor (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, //需要重置的残差块IDstd::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();//获取当前参数块的idstd::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-keepingit->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 ceresfor (ParameterBlockCollection::iterator parameter_it = it->second.begin();parameter_it != it->second.end(); ++parameter_it)//遍历参数块{uint64_t parameter_id = parameter_it->second->id();//获取参数块的idstd::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 covarianceceres::Covariance::Options options;//配置ceres计算的参数// it can deal with rank deficient problem but very slowif (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 covariancecovariance.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);//协方差块置0covariance_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;
}
 | Scaffold - 脚手架)
