Cartographer向PoseGrapher里面添加用于控制分支的状态参数

先删除系统中安装的cartographer包含文件

cd  /usr/local/include

sudo rm -rf  cartographer

打开C++工程头文件

/cartographer/mapping/pose_graph.h

由于  class PoseGraph2D : public PoseGraph { ，所以我们需要在 class PoseGraph 里面添加

class PoseGraph : public PoseGraphInterface
{
  public:
     bool isInSlam;

在开启定位模式的时候，关闭掉一些优化选项，但是我们约定在 isInSlam模式的时候，需要开启相应的更多的优化。

通过配置文件trajectory_builder.lua写入配置参数

include "trajectory_builder_2d.lua"
include "trajectory_builder_3d.lua"
TRAJECTORY_BUILDER = {
  trajectory_builder_2d = TRAJECTORY_BUILDER_2D,
  trajectory_builder_3d = TRAJECTORY_BUILDER_3D,
  pure_localization = false,
  is_in_slam_mode = true,
}

这样在读取的时候，通过Ros版本的lua文件包含的trajectory_builder.lua 文件，则可以加载 is_in_slam_mode = true  控制参数

此外还需要在cartographer/mapping/trajectory_builder_interface.cc添加函数

options.set_is_in_slam_mode( parameter_dictionary->GetBool("is_in_slam_mode"));//

Ros端使用的配置 revo_lds_wish.lua

include "map_builder.lua"
include "trajectory_builder.lua"
options = {
  map_builder = MAP_BUILDER,
  trajectory_builder = TRAJECTORY_BUILDER,
.................
is_in_slam_mode = true,  --是否在slam模式，如果在slam模式，则解除全部限制
.................
}

luanch配置

  <node name="cartographer_node" pkg="cartographer_ros"
      type="cartographer_node" args="
          -configuration_directory $(find cartographer_ros)/configuration_files
          -configuration_basename revo_lds_wish.lua"
      output="screen">
    <remap from="scan" to="scan" />
  </node>

C++内部的参数传递

本应该取相同的名字的，但是为了避免歧义，还是通过赋值完成

cartographer/mapping/map_builder.cc

map_builder通过一个unique指针使用了 std::unique_ptr<PoseGraph> pose_graph_; 为成员pose_graph_。

在添加轨迹的时候，可以传入参数到PoseGraph2D，

int MapBuilder::AddTrajectoryBuilder(
    const std::set<SensorId>& expected_sensor_ids,
    const proto::TrajectoryBuilderOptions& trajectory_options,
    LocalSlamResultCallback local_slam_result_callback)
{
   //赋值指针的slam状态//wishchin!!!
    LOG(INFO) << "MapBuilder::AddTrajectoryBuilder -this->isInSlam:"<<pose_graph_.get()->isInSlam ;
    LOG(INFO) << "trajectory_options.is_in_slam_mode(): "<<trajectory_options.is_in_slam_mode() ;
    pose_graph_.get()->isInSlam  = trajectory_options.is_in_slam_mode();//wishchin 设定pose图的slam状态！
............................................

把 is_in_slam_mode 状态传递给PoseGraph2D->isInSlam

C++内部算法实现的逻辑控制

cartographer/mapping/internal/2d/pose_graph_2d.cc

void PoseGraph2D::ComputeConstraintsForNode(
    const NodeId& node_id,
    std::vector<std::shared_ptr<const Submap2D>> insertion_submaps,
    const bool newly_finished_submap)
{
 ........................................
  constraint_builder_.NotifyEndOfNode();
  ++num_nodes_since_last_loop_closure_;
  CHECK(!run_loop_closure_);

//如果在slam模式下，使用正常模式//这个成员在PoseGraph类里面声明的
 if( this->isInSlam )
 {
     LOG(INFO) << "PoseGraph2D::ComputeConstraintsForNode has use the opt model:  "<<this->isInSlam ;
     // 开启/关闭全局优化的参数//wishchin!!!
     if (options_.optimize_every_n_nodes() > 0 &&
         num_nodes_since_last_loop_closure_ > options_.optimize_every_n_nodes()) {
       DispatchOptimization();
     }
     return;
 }

   //若不是在slam模式下，即定位模式下，则使用时间控制！
  //--- *****开启/关闭全局优化的参数//wishchin!!!---------------//
  //若没有开启，则读取时间//添加标志符号，减少IO
  //const int secondConfine = 4*60;//设定10分钟
  int detaSecond =86400;//24*60*60= 144*6 = 864
  if( !this->isTime
......................................

可以通过这个逻辑链，使用配置参数控制全局优化的状态

 文章来源地址https://uudwc.com/A/9Yr5