update comments

.vscode/settings.json

@@ -13,10 +13,10 @@
   "todo-tree.general.tags": [
     "?",
     "!",
+    "param",
     "Step",
     "c++11",
     "note",
-    "done",
     "tag",
     "todo",
   ],
@@ -54,11 +54,11 @@
       "iconColour": "orange",
       "type": "text-and-comment",
     },
-    "done": {
+    "param": {
       "icon": "issue-closed",
-      "foreground": "#FF00FF",
-      "rulerColour": "#FF00FF",
-      "iconColour": "#FF00FF",
+      "foreground": "orange",
+      "rulerColour": "orange",
+      "iconColour": "orange",
       "type": "text-and-comment",
     },
     "tag": {

src/cartographer/cartographer/io/proto_stream_deserializer.cc

@@ -37,19 +37,22 @@ bool IsVersionSupported(const mapping::proto::SerializationHeader& header) {
 
 }  // namespace
 
+// 从文件中读取信息
 mapping::proto::PoseGraph DeserializePoseGraphFromFile(
     const std::string& file_name) {
   ProtoStreamReader reader(file_name);
   ProtoStreamDeserializer deserializer(&reader);
   return deserializer.pose_graph();
 }
 
+// 从ProtoStreamReader进行构造
 ProtoStreamDeserializer::ProtoStreamDeserializer(
     ProtoStreamReaderInterface* const reader)
     : reader_(reader), header_(ReadHeaderOrDie(reader)) {
   CHECK(IsVersionSupported(header_)) << "Unsupported serialization format \""
                                      << header_.format_version() << "\"";
 
+  // 向pose_graph_赋值
   CHECK(ReadNextSerializedData(&pose_graph_))
       << "Serialized stream misses PoseGraph.";
   CHECK(pose_graph_.has_pose_graph())

src/cartographer/cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc

@@ -124,17 +124,18 @@ std::unique_ptr<transform::Rigid2d> LocalTrajectoryBuilder2D::ScanMatch(
 
 
 /**
- * @brief 
+ * @brief 处理点云数据, 进行扫描匹配, 将点云写成地图
  * 
- * @param[in] sensor_id 
- * @param[in] unsynchronized_data 
- * @return std::unique_ptr<LocalTrajectoryBuilder2D::MatchingResult> 
+ * @param[in] sensor_id 点云数据对应的话题名称
+ * @param[in] unsynchronized_data 传入的点云数据
+ * @return std::unique_ptr<LocalTrajectoryBuilder2D::MatchingResult> 匹配后的结果
  */
 std::unique_ptr<LocalTrajectoryBuilder2D::MatchingResult>
 LocalTrajectoryBuilder2D::AddRangeData(
     const std::string& sensor_id,
     const sensor::TimedPointCloudData& unsynchronized_data) {
-  // Step: 1 进行多个传感器的数据同步, 只有一个传感器的时候, 可以直接忽略
+
+  // Step: 1 进行多个雷达点云数据的时间同步
   auto synchronized_data =
       range_data_collator_.AddRangeData(sensor_id, unsynchronized_data);
   if (synchronized_data.ranges.empty()) {
@@ -144,6 +145,7 @@ LocalTrajectoryBuilder2D::AddRangeData(
 
   const common::Time& time = synchronized_data.time;
   // Initialize extrapolator now if we do not ever use an IMU.
+  // 如果不用imu, 就在雷达这初始化位姿推测器
   if (!options_.use_imu_data()) {
     InitializeExtrapolator(time);
   }
@@ -159,7 +161,7 @@ LocalTrajectoryBuilder2D::AddRangeData(
   // TODO(gaschler): Check if this can strictly be 0.
   CHECK_LE(synchronized_data.ranges.back().point_time.time, 0.f);
 
-  // Step: 2 运动畸变的去除
+  // 计算第一个点的时间
   const common::Time time_first_point =
       time +
       common::FromSeconds(synchronized_data.ranges.front().point_time.time);
@@ -172,10 +174,12 @@ LocalTrajectoryBuilder2D::AddRangeData(
   range_data_poses.reserve(synchronized_data.ranges.size());
   bool warned = false;
 
-  // 插值得到每一个时间点的位姿
+  // 预测得到每一个时间点的位姿
   for (const auto& range : synchronized_data.ranges) {
     common::Time time_point = time + common::FromSeconds(range.point_time.time);
+    // 如果该时间比PoseExtrapolator的最新时间还要早,说明在第一个点被捕获时,PoseExtrapolator还没初始化
     if (time_point < extrapolator_->GetLastExtrapolatedTime()) {
+      // 一个循环只报一次错
       if (!warned) {
         LOG(ERROR)
             << "Timestamp of individual range data point jumps backwards from "
@@ -184,6 +188,8 @@ LocalTrajectoryBuilder2D::AddRangeData(
       }
       time_point = extrapolator_->GetLastExtrapolatedTime();
     }
+
+    // Step: 2 预测出 每个点的时间戳时刻, tracking frame 在 local slam 坐标系下的位姿
     range_data_poses.push_back(
         extrapolator_->ExtrapolatePose(time_point).cast<float>());
   }
@@ -196,39 +202,52 @@ LocalTrajectoryBuilder2D::AddRangeData(
 
   // Drop any returns below the minimum range and convert returns beyond the
   // maximum range into misses.
-  // Step: 3 无效数据的处理
-  // 得到所有的累计数据, 对范围的数据进行滤除
+  // 对每个数据点进行处理
   for (size_t i = 0; i < synchronized_data.ranges.size(); ++i) {
+    // 获取在tracking frame 下点的坐标
     const sensor::TimedRangefinderPoint& hit =
         synchronized_data.ranges[i].point_time;
+    // 将点云的origins坐标转到 local slam 坐标系下
     const Eigen::Vector3f origin_in_local =
         range_data_poses[i] *
         synchronized_data.origins.at(synchronized_data.ranges[i].origin_index);
+
+    // Step: 3 运动畸变的去除, 将hit坐标根据转到 local slam 坐标系下
     sensor::RangefinderPoint hit_in_local =
         range_data_poses[i] * sensor::ToRangefinderPoint(hit);
+
+    // 计算这个点的距离, 这里用的是去畸变之后的点的距离
     const Eigen::Vector3f delta = hit_in_local.position - origin_in_local;
-
     const float range = delta.norm();
+
+    // param: min_range max_range
     if (range >= options_.min_range()) {
       if (range <= options_.max_range()) {
+        // 在这里可以看到, returns里保存的是local slam下的去畸变之后的点的坐标
         accumulated_range_data_.returns.push_back(hit_in_local);
       } else {
+        // Step: 4 超过max_range时的处理: 用一个距离进行替代, 并放入misses里
         hit_in_local.position =
             origin_in_local +
+            // param: missing_data_ray_length, 是个比例, 不是距离
             options_.missing_data_ray_length() / range * delta;
         accumulated_range_data_.misses.push_back(hit_in_local);
       }
     }
-  }
+  } // end for
+
+  // 有一帧有效的数据了
   ++num_accumulated_;
 
+  // param: num_accumulated_range_data
   if (num_accumulated_ >= options_.num_accumulated_range_data()) {
     const common::Time current_sensor_time = synchronized_data.time;
     absl::optional<common::Duration> sensor_duration;
     if (last_sensor_time_.has_value()) {
       sensor_duration = current_sensor_time - last_sensor_time_.value();
     }
     last_sensor_time_ = current_sensor_time;
+
     num_accumulated_ = 0;
     const transform::Rigid3d gravity_alignment = transform::Rigid3d::Rotation(
         extrapolator_->EstimateGravityOrientation(time));
@@ -384,18 +403,22 @@ void LocalTrajectoryBuilder2D::AddOdometryData(
 
 // 如果Extrapolator没有初始化就进行初始化
 void LocalTrajectoryBuilder2D::InitializeExtrapolator(const common::Time time) {
+  // 如果已经初始化过了就直接返回
   if (extrapolator_ != nullptr) {
     return;
   }
   CHECK(!options_.pose_extrapolator_options().use_imu_based());
   // TODO(gaschler): Consider using InitializeWithImu as 3D does.
+
+  // 初始化位姿推测器
   extrapolator_ = absl::make_unique<PoseExtrapolator>(
       ::cartographer::common::FromSeconds(options_.pose_extrapolator_options()
                                               .constant_velocity()
                                               .pose_queue_duration()),
       options_.pose_extrapolator_options()
           .constant_velocity()
           .imu_gravity_time_constant());
+  // 添加初始位姿
   extrapolator_->AddPose(time, transform::Rigid3d::Identity());
 }
 

src/cartographer/cartographer/mapping/internal/range_data_collator.cc

@@ -79,18 +79,19 @@ sensor::TimedPointCloudOriginData RangeDataCollator::AddRangeData(
   return CropAndMerge();
 }
 
-// 对所有数据进行截取与合并
+// 对所有数据进行截取与合并, 返回匹配用的点云
 sensor::TimedPointCloudOriginData RangeDataCollator::CropAndMerge() {
   sensor::TimedPointCloudOriginData result{current_end_, {}, {}};
   bool warned_for_dropped_points = false;
   // 遍历所有的传感器话题
   for (auto it = id_to_pending_data_.begin();
        it != id_to_pending_data_.end();) {
+    // 获取数据的引用
     sensor::TimedPointCloudData& data = it->second;
     const sensor::TimedPointCloud& ranges = it->second.ranges;
     const std::vector<float>& intensities = it->second.intensities;
 
-    // 找到时间最接近current_start_的点的索引
+    // 找到点云中 时间最接近current_start_的点的索引
     auto overlap_begin = ranges.begin();
     while (overlap_begin < ranges.end() &&
            data.time + common::FromSeconds((*overlap_begin).time) <
@@ -106,7 +107,7 @@ sensor::TimedPointCloudOriginData RangeDataCollator::CropAndMerge() {
       ++overlap_end;
     }
 
-    // 丢弃点云中时间比起始时间早的点
+    // 丢弃点云中时间比起始时间早的点, 每执行一下CropAndMerge()打印一次log
     if (ranges.begin() < overlap_begin && !warned_for_dropped_points) {
       LOG(WARNING) << "Dropped " << std::distance(ranges.begin(), overlap_begin)
                    << " earlier points.";
@@ -125,6 +126,7 @@ sensor::TimedPointCloudOriginData RangeDataCollator::CropAndMerge() {
 
       auto intensities_overlap_it =
           intensities.begin() + (overlap_begin - ranges.begin());
+      // reserve() 在预留空间改变时, 会将之前的数据拷贝到新的内存中
       result.ranges.reserve(result.ranges.size() +
                             std::distance(overlap_begin, overlap_end));
 
@@ -138,8 +140,8 @@ sensor::TimedPointCloudOriginData RangeDataCollator::CropAndMerge() {
         // 针对每个点时间戳进行修正, 让最后一个点的时间为0
         point.point_time.time += time_correction;  
         result.ranges.push_back(point);
-      }
-    }
+      } // end for
+    } // end if
 
     // Drop buffered points until overlap_end.
     // 如果点云每个点都用了, 则可将这个数据进行删除
@@ -154,7 +156,7 @@ sensor::TimedPointCloudOriginData RangeDataCollator::CropAndMerge() {
     else {
       const auto intensities_overlap_end =
           intensities.begin() + (overlap_end - ranges.begin());
-      // 将用了的点删除
+      // 将用了的点删除, 这里的赋值是拷贝
       data = sensor::TimedPointCloudData{
           data.time, data.origin,
           sensor::TimedPointCloud(overlap_end, ranges.end()),
@@ -163,7 +165,7 @@ sensor::TimedPointCloudOriginData RangeDataCollator::CropAndMerge() {
     }
   } // end for
 
-  // 对各传感器的点云 按照最后一个点的时间进行排序
+  // 对各传感器的点云 按照每个点的时间从小到大进行排序
   std::sort(result.ranges.begin(), result.ranges.end(),
             [](const sensor::TimedPointCloudOriginData::RangeMeasurement& a,
                const sensor::TimedPointCloudOriginData::RangeMeasurement& b) {

src/cartographer/cartographer/mapping/internal/range_data_collator.h

@@ -48,7 +48,7 @@ class RangeDataCollator {
 
   const std::set<std::string> expected_sensor_ids_;
   // Store at most one message for each sensor.
-  std::map<std::string, sensor::TimedPointCloudData> id_to_pending_data_;
+  std::map<std::string, sensor::TimedPointCloudData> id_to_pending_data_; // 待处理的数据
   common::Time current_start_ = common::Time::min();
   common::Time current_end_ = common::Time::min();
 

src/cartographer/cartographer/mapping/map_builder.cc

@@ -380,6 +380,7 @@ std::map<int, int> MapBuilder::LoadState(
   }
 
   SerializedData proto;
+  // 向pose_graph_中添加信息
   while (deserializer.ReadNextSerializedData(&proto)) {
     switch (proto.data_case()) {
       case SerializedData::kPoseGraph:
@@ -421,20 +422,23 @@ std::map<int, int> MapBuilder::LoadState(
       }
       case SerializedData::kImuData: {
         if (load_frozen_state) break;
+        // 将IMU数据添加到位姿图中
         pose_graph_->AddImuData(
             trajectory_remapping.at(proto.imu_data().trajectory_id()),
             sensor::FromProto(proto.imu_data().imu_data()));
         break;
       }
       case SerializedData::kOdometryData: {
         if (load_frozen_state) break;
+        // 将Odom数据添加到位姿图中
         pose_graph_->AddOdometryData(
             trajectory_remapping.at(proto.odometry_data().trajectory_id()),
             sensor::FromProto(proto.odometry_data().odometry_data()));
         break;
       }
       case SerializedData::kFixedFramePoseData: {
         if (load_frozen_state) break;
+        // 将GPS数据添加到位姿图中
         pose_graph_->AddFixedFramePoseData(
             trajectory_remapping.at(
                 proto.fixed_frame_pose_data().trajectory_id()),
@@ -444,6 +448,7 @@ std::map<int, int> MapBuilder::LoadState(
       }
       case SerializedData::kLandmarkData: {
         if (load_frozen_state) break;
+        // 将landmark数据添加到位姿图中
         pose_graph_->AddLandmarkData(
             trajectory_remapping.at(proto.landmark_data().trajectory_id()),
             sensor::FromProto(proto.landmark_data().landmark_data()));
@@ -465,6 +470,7 @@ std::map<int, int> MapBuilder::LoadState(
           proto::PoseGraph::Constraint::INTRA_SUBMAP) {
         continue;
       }
+      // 添加子图的附属的节点
       pose_graph_->AddNodeToSubmap(
           NodeId{constraint_proto.node_id().trajectory_id(),
                  constraint_proto.node_id().node_index()},
@@ -498,7 +504,7 @@ std::map<int, int> MapBuilder::LoadStateFromFile(
   return LoadState(&stream, load_frozen_state);
 }
 
-// 接口
+// 工厂函数
 std::unique_ptr<MapBuilderInterface> CreateMapBuilder(
     const proto::MapBuilderOptions& options) {
   return absl::make_unique<MapBuilder>(options);

src/cartographer/cartographer/mapping/map_builder.h

@@ -96,7 +96,7 @@ class MapBuilder : public MapBuilderInterface {
       all_trajectory_builder_options_;
 };
 
-// 接口
+// 工厂函数
 std::unique_ptr<MapBuilderInterface> CreateMapBuilder(
     const proto::MapBuilderOptions& options);
 

src/cartographer/cartographer/sensor/range_data.h

@@ -33,9 +33,9 @@ namespace sensor {
 /**
  * @brief local_slam_data中存储所有雷达点云的数据结构
  * 
- * @param[] origin  当前时刻雷达的位姿, 也就是雷达坐标系的原点的坐标
- * @param[] returns 所有雷达数据点的坐标, 记为returns, 也就是hit
- * @param[] misses  是在光线方向上未检测到返回的点(nan, inf等等), 并以配置的距离代替, 将这段直线视为空闲区域
+ * @param origin  当前时刻雷达的位姿, 也就是雷达坐标系的原点的坐标
+ * @param returns 所有雷达数据点的坐标, 记为returns, 也就是hit
+ * @param misses  是在光线方向上未检测到返回的点(nan, inf等等), 并以配置的距离代替, 将这段直线视为空闲区域
  */
 struct RangeData {
   Eigen::Vector3f origin;

src/cartographer/cartographer/sensor/timed_point_cloud_data.h

@@ -24,23 +24,25 @@
 namespace cartographer {
 namespace sensor {
 
+// 时间同步前的点云
 struct TimedPointCloudData {
   common::Time time;        // 点云最后一个点的时间
-  Eigen::Vector3f origin;   // 从tracking_frame_到雷达坐标系的坐标变化
+  Eigen::Vector3f origin;   // 以tracking_frame_到雷达坐标系的坐标变换为原点
   TimedPointCloud ranges;   // 数据点的集合, 每个数据点包含xyz与time, time是负的
   // 'intensities' has to be same size as 'ranges', or empty.
   std::vector<float> intensities; // 空的
 };
 
+// 时间同步后的点云
 struct TimedPointCloudOriginData {
   struct RangeMeasurement {
-    TimedRangefinderPoint point_time;   // 带时间戳的单个数据点的坐标
-    float intensity;
-    size_t origin_index;
+    TimedRangefinderPoint point_time;   // 带时间戳的单个数据点的坐标 xyz
+    float intensity;                    // 强度值
+    size_t origin_index;                // 属于第几个origins的点
   };
-  common::Time time;
-  std::vector<Eigen::Vector3f> origins;
-  std::vector<RangeMeasurement> ranges;
+  common::Time time;                    // 点云的时间
+  std::vector<Eigen::Vector3f> origins; // 点云是由几个点云组成, 每个点云的原点
+  std::vector<RangeMeasurement> ranges; // 数据点的集合
 };
 
 // Converts 'timed_point_cloud_data' to a proto::TimedPointCloudData.

src/cartographer/configuration_files/trajectory_builder_2d.lua

@@ -19,7 +19,7 @@ TRAJECTORY_BUILDER_2D = {
   min_z = -0.8,                   -- 雷达数据的最高与最低的过滤, 保存中间值
   max_z = 2.,
   missing_data_ray_length = 5.,   -- 超过最大距离范围的数据点用这个距离代替
-  num_accumulated_range_data = 1, -- 
+  num_accumulated_range_data = 1, -- 几帧有效的点云数据进行一次扫描匹配
   voxel_filter_size = 0.025,      -- 体素滤波的立方体的边长
 
   -- 使用固定的voxel滤波之后, 再使用自适应体素滤波器