Cartographer learning records: 3D slam part of cartographer source code (I)

In the past Official data set and Self recording data set After verification , We can see clearly that cartographer Of 3D Drawing effect , In the next period of time, I will carry out 3D SLAM Read and comment the source code , Make a record here , If a friend finds something wrong , Hope you can tell me in time , So that I can correct it in time .

In terms of personal habits , My strategy for reading the code is to look at the header first , Be familiar with the functions and member variables included in the class , Then the depth first search is used for the implementation files + Breadth first search , Depth first search refers to starting from the main thread , Drill down into the calling function paragraph by paragraph ; Breadth first search mainly refers to presentation form , Put the call obtained by depth first search in this file for convenience of subsequent view , Now I will start with cartographer The upper ROS Encapsulation begins with a step-by-step reading .

One .node_main.cc file

The document is cartographer_ros Body file of , So choose to read the file step by step .

1. The beginning of the file is to call Google Open source command line tag processing library gflags,gflags The main supported parameter types include bool, int32, int64, uint64, double, string etc. , Define parameters through DEFINE_type Macro implementation , The parameters are parameter names , Parameter default values and prompt information , After the definition is completed, you can pass FLAGS_name Access the corresponding parameters , For example, the following “FLAGS_configuration_directory” and “FLAGS_configuration_basename” It corresponds to “configuration_directory” and “configuration_basename”.

DEFINE_bool(collect_metrics, false,
            "Activates the collection of runtime metrics. If activated, the "
            "metrics can be accessed via a ROS service.");
DEFINE_string(configuration_directory, "",
              "First directory in which configuration files are searched, "
              "second is always the Cartographer installation to allow "
              "including files from there.");
DEFINE_string(configuration_basename, "",
              "Basename, i.e. not containing any directory prefix, of the "
              "configuration file.");
DEFINE_string(load_state_filename, "",
              "If non-empty, filename of a .pbstream file to load, containing "
              "a saved SLAM state.");
DEFINE_bool(load_frozen_state, true,
            "Load the saved state as frozen (non-optimized) trajectories.");
DEFINE_bool(
    start_trajectory_with_default_topics, true,
    "Enable to immediately start the first trajectory with default topics.");
DEFINE_string(
    save_state_filename, "",
    "If non-empty, serialize state and write it to disk before shutting down.");

2. From the bottom main Function to read ,main The function mainly initializes , Run and end operations , Initialization includes InitGoogleLogging、ParseCommandLineFlags as well as ros Of init function , Operation is defined in this document run function , The end is ros Of shutdown function ,（ps：ros Front heel :: Represents a call under the global scope ）. The initialization and end operations can be realized by roughly understanding the following functions , The key is run function , It is directly related to how to get from ros The upper nodes enter the lower layer bit by bit cartographer Algorithmic level .

int main(int argc, char** argv) {

  //  initialization Google The logstore of glog
  google::InitGoogleLogging(argv[0]);
  
  //  call gflags Initialize parameters , The first and second parameters are main Parameter cardinality and parameter value of function , The third parameter is named remove_flags, If false, It means that argv The parameter value of is in accordance with flags separation 、 reorder , If true, gflags Will remove argv Parameters separated in 
  google::ParseCommandLineFlags(&argc, &argv, true);

  // Google The logstore of glog It's provided in CHECK Series of macros , Check whether a condition is true , In the form of CHECK(condition)
  // condition If it's not true , Then the following expression information and “check failed:" #condition "”
  //  And then exit the program , The processing process and after the error FATAL More like 
  CHECK(!FLAGS_configuration_directory.empty())
      << "-configuration_directory is missing.";
  CHECK(!FLAGS_configuration_basename.empty())
      << "-configuration_basename is missing.";

  // cartographer The Lord of ROS node "cartographer_node" The initialization 
  ::ros::init(argc, argv, "cartographer_node");


  //  start-up ROS, An explicit call to this function means that any NodeHandle Start before instance ROS Related threads ,  Network, etc 
  ::ros::start();

  //  Use ROS_INFO Conduct glog The output of the message 
  cartographer_ros::ScopedRosLogSink ros_log_sink;

  //  Began to run cartographer_ros
  cartographer_ros::Run();

  //  end ROS Related threads , Network, etc 
  ::ros::shutdown();
}

3.Run Function flow ： utilize LoadOptions Function load configuration -> Take the configuration as CreateMapBuilder Parameter creation map_builder object -> take map_builder And configuration node_options Construct as a parameter node Class object ->node Class object subscription topic Information begins to set the path ->node Class object ends all active tracks and performs global optimization .

Run The important calling functions in the function include CreateMapBuilder()、StartTrajectoryWithDefaultTopics()、FinishAllTrajectories()、RunFinalOptimization(), Some functions of the loading function will not be introduced too much .

// cartographer_ros Namespace 
namespace cartographer_ros {
namespace {

void Run() {
  //  Definition tf Time interval between releases 
  constexpr double kTfBufferCacheTimeInSeconds = 10.;
  //  Instantiation tf2_ros::Buffer object tf_buffer
  tf2_ros::Buffer tf_buffer{::ros::Duration(kTfBufferCacheTimeInSeconds)};
  //  Turn on monitoring tf Independent threads of 
  tf2_ros::TransformListener tf(tf_buffer);

  // node_options Structure , Contains map frame name settings 、 receive tf Of timeout Set the time 、 Subgraph publishing cycle settings 、 Pose release cycle settings 、 Path publishing cycle settings 
  NodeOptions node_options;
  // TrajectoryOptions Structure , Contains tracking frame name settings 、 Publish frame name settings 、 Odometer frame name setting 、 Radar scanning quantity setting 、 Point cloud quantity setting, etc 
  TrajectoryOptions trajectory_options;

  //  Add ——c++11 Introduced std::tie() Function can connect a variable to a given tuple On , Generate an element type that is all referenced tuple
  //  take LoadOptions  Acquired lua The parameter values in the file are assigned to  node_options  and  trajectory_options
  // LoadOptions  Function in node_options.h  In the definition of 
  std::tie(node_options, trajectory_options) =
      LoadOptions(FLAGS_configuration_directory, FLAGS_configuration_basename);

  // MapBuilder Class is cartographer Inside complete SLAM The algorithm class contains the front end （TrajectoryBuilders,scan to submap） With the back end （ Used to find loopback PoseGraph）
  // CreateMapBuilder Functions use lua The parameter values set in the file point to MapBuilder Class instantiates an object for subsequent use of related member functions 
  auto map_builder =
      cartographer::mapping::CreateMapBuilder(node_options.map_builder_options);
  
  //  Add ——c++11 Introduced std::move  Is to transfer the state or ownership of an object from one object to another ,
  //  It's just a transfer , There is no memory relocation or memory copy, so it can improve the utilization efficiency , improve performance 
  //  Right value references are used to support transfer semantics , Transferring semantics can make resources （ Pile up , System objects, etc ） Transfer from one object to another , This can reduce the creation of unnecessary temporary objects 、 Copy and destroy , Can greatly improve C++ Application performance 
  //  Maintenance of temporary objects in programs （ Create and destroy ） Have a serious impact on performance .

  // Node Class initialization , take ROS Of topic Pass in MapBuilder
  // Node  stay /cartographer_ros/cartographer_ros/cartographer/node.h  In the definition of 
  //  Many sensors are subscribed to this constructor topic, Collect sensor data 
  Node node(node_options, std::move(map_builder), &tf_buffer,
            FLAGS_collect_metrics);
  
  //  According to the front DEFINE_type Definition determines whether to load pbstream Map file 
  if (!FLAGS_load_state_filename.empty()) {
    node.LoadState(FLAGS_load_state_filename, FLAGS_load_frozen_state);
  }

  //  Use the default topic（ stay node_constants.h The file defines the names of related topics ） Start making tracks 
  if (FLAGS_start_trajectory_with_default_topics) {
    node.StartTrajectoryWithDefaultTopics(trajectory_options);
  }
  
  // ROS  Message callback handler ,ros::spinonce() Indicates that a subscription is called only once , Subsequent procedures can also be continued , If you want to keep subscribing, you need to add a loop , So relative spin() Have flexibility , Frequency and message pool size can be controlled ;
  ::ros::spin();

  //  End all active tracks 
  node.FinishAllTrajectories();

  //  When all the tracks are over , Perform another global optimization 
  node.RunFinalOptimization();

  //  If DEFINE_type Definition save_state_filename Non empty , Just keep it pbstream file 
  if (!FLAGS_save_state_filename.empty()) {
    node.SerializeState(FLAGS_save_state_filename,
                        true /* include_unfinished_submaps */);
  }
}

}  // namespace
}  // namespace cartographer_ros

The next article will first combine CreateMapBuilder() Functions and node The constructor of , Read it paragraph by paragraph , And then according to StartTrajectoryWithDefaultTopics()、FinishAllTrajectories()、RunFinalOptimization() It's in the right order node The member functions of the class are interpreted in turn , I will also point out some relevant places among the documents for your understanding , I hope that I can constantly correct and make progress in the process of recording .