diff --git a/cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc b/cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc index fce1348..17374d3 100644 --- a/cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc +++ b/cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc @@ -124,14 +124,14 @@ std::unique_ptr LocalTrajectoryBuilder3D::AddRangeData( const std::string& sensor_id, const sensor::TimedPointCloudData& unsynchronized_data) { - auto synchronized_data = + const auto synchronized_data = range_data_collator_.AddRangeData(sensor_id, unsynchronized_data); if (synchronized_data.ranges.empty()) { LOG(INFO) << "Range data collator filling buffer."; return nullptr; } - const common::Time& time = synchronized_data.time; + const common::Time& current_sensor_time = synchronized_data.time; if (extrapolator_ == nullptr) { // Until we've initialized the extrapolator with our first IMU message, we // cannot compute the orientation of the rangefinder. @@ -142,7 +142,7 @@ LocalTrajectoryBuilder3D::AddRangeData( CHECK(!synchronized_data.ranges.empty()); CHECK_LE(synchronized_data.ranges.back().point_time[3], 0.f); const common::Time time_first_point = - time + + current_sensor_time + common::FromSeconds(synchronized_data.ranges.front().point_time[3]); if (time_first_point < extrapolator_->GetLastPoseTime()) { LOG(INFO) << "Extrapolator is still initializing."; @@ -158,7 +158,8 @@ LocalTrajectoryBuilder3D::AddRangeData( bool warned = false; for (const auto& hit : hits) { - common::Time time_point = time + common::FromSeconds(hit.point_time[3]); + common::Time time_point = + current_sensor_time + common::FromSeconds(hit.point_time[3]); if (time_point < extrapolator_->GetLastExtrapolatedTime()) { if (!warned) { LOG(ERROR) @@ -199,7 +200,6 @@ LocalTrajectoryBuilder3D::AddRangeData( ++num_accumulated_; if (num_accumulated_ >= options_.num_accumulated_range_data()) { - const common::Time current_sensor_time = synchronized_data.time; common::optional sensor_duration; if (last_sensor_time_.has_value()) { sensor_duration = current_sensor_time - last_sensor_time_.value(); @@ -208,7 +208,7 @@ LocalTrajectoryBuilder3D::AddRangeData( num_accumulated_ = 0; transform::Rigid3f current_pose = - extrapolator_->ExtrapolatePose(time).cast(); + extrapolator_->ExtrapolatePose(current_sensor_time).cast(); const auto voxel_filter_start = std::chrono::steady_clock::now(); const sensor::RangeData filtered_range_data = { @@ -228,7 +228,7 @@ LocalTrajectoryBuilder3D::AddRangeData( } return AddAccumulatedRangeData( - time, + current_sensor_time, sensor::TransformRangeData(filtered_range_data, current_pose.inverse()), sensor_duration); }