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

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
master
gaschler 2018-02-26 18:38:00 +01:00 committed by Wally B. Feed
parent 3ebfa757ef
commit f606d4b91c
2 changed files with 22 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
cartographer/internal/mapping/2d/local_trajectory_builder_2d.cc
View File

@ -106,8 +106,18 @@ LocalTrajectoryBuilder2D::AddRangeData(
std::vector<transform::Rigid3f> range_data_poses; std::vector<transform::Rigid3f> range_data_poses;
range_data_poses.reserve(range_data.returns.size()); range_data_poses.reserve(range_data.returns.size());
bool warned = false;
for (const Eigen::Vector4f& hit : range_data.returns) { 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( range_data_poses.push_back(
extrapolator_->ExtrapolatePose(time_point).cast<float>()); extrapolator_->ExtrapolatePose(time_point).cast<float>());
} }

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

@ -84,8 +84,18 @@ LocalTrajectoryBuilder3D::AddRangeData(
std::vector<transform::Rigid3f> hits_poses; std::vector<transform::Rigid3f> hits_poses;
hits_poses.reserve(hits.size()); hits_poses.reserve(hits.size());
bool warned = false;
for (const Eigen::Vector4f& hit : hits) { 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( hits_poses.push_back(
extrapolator_->ExtrapolatePose(time_point).cast<float>()); extrapolator_->ExtrapolatePose(time_point).cast<float>());
} }