From f606d4b91c605b7b239655de703c36d76b2ca83a Mon Sep 17 00:00:00 2001 From: gaschler Date: Mon, 26 Feb 2018 18:38:00 +0100 Subject: [PATCH] Gracefully handle time-overlapping point clouds. (#936) Per-point relative times are usually computed by multiplying a per-point time increment by the number of points. So it is not uncommon for consecutive point clouds of a single sensor to overlap in time. When this happens, we act as if no backward jump in time happened and warn once per input point cloud. FIXES=#912 --- .../mapping/2d/local_trajectory_builder_2d.cc | 12 +++++++++++- .../mapping/3d/local_trajectory_builder_3d.cc | 12 +++++++++++- 2 files changed, 22 insertions(+), 2 deletions(-) diff --git a/cartographer/internal/mapping/2d/local_trajectory_builder_2d.cc b/cartographer/internal/mapping/2d/local_trajectory_builder_2d.cc index 6860e47..720dd65 100644 --- a/cartographer/internal/mapping/2d/local_trajectory_builder_2d.cc +++ b/cartographer/internal/mapping/2d/local_trajectory_builder_2d.cc @@ -106,8 +106,18 @@ LocalTrajectoryBuilder2D::AddRangeData( std::vector range_data_poses; range_data_poses.reserve(range_data.returns.size()); + bool warned = false; for (const Eigen::Vector4f& hit : range_data.returns) { - const common::Time time_point = time + common::FromSeconds(hit[3]); + common::Time time_point = time + common::FromSeconds(hit[3]); + if (time_point < extrapolator_->GetLastExtrapolatedTime()) { + if (!warned) { + LOG(ERROR) + << "Timestamp of individual range data point jumps backwards from " + << extrapolator_->GetLastExtrapolatedTime() << " to " << time_point; + warned = true; + } + time_point = extrapolator_->GetLastExtrapolatedTime(); + } range_data_poses.push_back( extrapolator_->ExtrapolatePose(time_point).cast()); } diff --git a/cartographer/internal/mapping/3d/local_trajectory_builder_3d.cc b/cartographer/internal/mapping/3d/local_trajectory_builder_3d.cc index b3e0bdb..f879a09 100644 --- a/cartographer/internal/mapping/3d/local_trajectory_builder_3d.cc +++ b/cartographer/internal/mapping/3d/local_trajectory_builder_3d.cc @@ -84,8 +84,18 @@ LocalTrajectoryBuilder3D::AddRangeData( std::vector hits_poses; hits_poses.reserve(hits.size()); + bool warned = false; for (const Eigen::Vector4f& hit : hits) { - const common::Time time_point = time + common::FromSeconds(hit[3]); + common::Time time_point = time + common::FromSeconds(hit[3]); + if (time_point < extrapolator_->GetLastExtrapolatedTime()) { + if (!warned) { + LOG(ERROR) + << "Timestamp of individual range data point jumps backwards from " + << extrapolator_->GetLastExtrapolatedTime() << " to " << time_point; + warned = true; + } + time_point = extrapolator_->GetLastExtrapolatedTime(); + } hits_poses.push_back( extrapolator_->ExtrapolatePose(time_point).cast()); }