12345qiupeng
/
cartographer
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

feat:fix z to zero

master
邱棚 2024-07-12 15:20:35 +08:00
parent eb5d0b2218
commit fbd65fc5f3
2 changed files with 26 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
CMakeLists.txt
View File

@ -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()

28
cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc
View File

@ -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)});
}