feat:fix z to zero

CMakeLists.txt

@@ -25,6 +25,9 @@ option(BUILD_GRPC "build Cartographer gRPC support" false)
 set(CARTOGRAPHER_HAS_GRPC ${BUILD_GRPC})
 option(BUILD_PROMETHEUS "build Prometheus monitoring support" false)
 
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+
 include("${PROJECT_SOURCE_DIR}/cmake/functions.cmake")
 google_initialize_cartographer_project()
 google_enable_testing()

cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc

@@ -136,6 +136,7 @@ LocalTrajectoryBuilder3D::AddRangeData(
         << "Passed point cloud has inconsistent number of intensities and "
            "ranges.";
   }
+    // 1. 进行多个雷达点云的时间同步，点云坐标相对于tracking
   auto synchronized_data =
       range_data_collator_.AddRangeData(sensor_id, unsynchronized_data);
   if (synchronized_data.ranges.empty()) {
@@ -152,6 +153,7 @@ LocalTrajectoryBuilder3D::AddRangeData(
 
   CHECK(!synchronized_data.ranges.empty());
   CHECK_LE(synchronized_data.ranges.back().point_time.time, 0.f);
+    // 计算地一个点云的时间
   const common::Time time_first_point =
       synchronized_data.time +
       common::FromSeconds(synchronized_data.ranges.front().point_time.time);
@@ -178,6 +180,7 @@ LocalTrajectoryBuilder3D::AddRangeData(
   bool warned = false;
   std::vector<common::Time> hit_times;
   common::Time prev_time_point = extrapolator_->GetLastExtrapolatedTime();
+    // 预测得到每一个时间点的位姿
   for (const auto& point_cloud_origin_data :
        accumulated_point_cloud_origin_data_) {
     for (const auto& hit : point_cloud_origin_data.ranges) {
@@ -310,7 +313,6 @@ LocalTrajectoryBuilder3D::AddAccumulatedRangeData(
     LOG(WARNING) << "Dropped empty low resolution point cloud data.";
     return nullptr;
   }
-
   std::unique_ptr<transform::Rigid3d> pose_estimate =
       ScanMatch(pose_prediction, low_resolution_point_cloud_in_tracking,
                 high_resolution_point_cloud_in_tracking);
@@ -318,7 +320,21 @@ LocalTrajectoryBuilder3D::AddAccumulatedRangeData(
     LOG(WARNING) << "Scan matching failed.";
     return nullptr;
   }
-  extrapolator_->AddPose(time, *pose_estimate);
+
+    // 获取当前的 translation 和 rotation
+    auto translation = pose_estimate->translation();
+    auto rotation = pose_estimate->rotation();
+
+    // 将 translation 的 z 值设置为 0
+    translation.z() = 0;
+
+    // 创建一个新的 Rigid3d 对象
+    std::unique_ptr<transform::Rigid3d> modified_pose_estimate =
+        std::make_unique<transform::Rigid3d>(translation, rotation);
+
+
+    // 插入位姿推测器中
+  extrapolator_->AddPose(time, *modified_pose_estimate);
 
   const auto scan_matcher_stop = std::chrono::steady_clock::now();
   const auto scan_matcher_duration = scan_matcher_stop - scan_matcher_start;
@@ -330,13 +346,14 @@ LocalTrajectoryBuilder3D::AddAccumulatedRangeData(
   }
 
   sensor::RangeData filtered_range_data_in_local = sensor::TransformRangeData(
-      filtered_range_data_in_tracking, pose_estimate->cast<float>());
+      filtered_range_data_in_tracking, modified_pose_estimate->cast<float>());
 
   const auto insert_into_submap_start = std::chrono::steady_clock::now();
+    // 插入到后端的地图中
   std::unique_ptr<InsertionResult> insertion_result = InsertIntoSubmap(
       time, filtered_range_data_in_local, filtered_range_data_in_tracking,
       high_resolution_point_cloud_in_tracking,
-      low_resolution_point_cloud_in_tracking, *pose_estimate,
+      low_resolution_point_cloud_in_tracking, *modified_pose_estimate,
       gravity_alignment);
   const auto insert_into_submap_stop = std::chrono::steady_clock::now();
 
@@ -369,8 +386,9 @@ LocalTrajectoryBuilder3D::AddAccumulatedRangeData(
   }
   last_wall_time_ = wall_time;
   last_thread_cpu_time_seconds_ = thread_cpu_time_seconds;
+    // 返回匹配结果
   return absl::make_unique<MatchingResult>(MatchingResult{
-      time, *pose_estimate, std::move(filtered_range_data_in_local),
+      time, *modified_pose_estimate, std::move(filtered_range_data_in_local),
       std::move(insertion_result)});
 }