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

Small readability improvement (#1304)

master
Juraj Oršulić 2018-07-23 16:21:45 +02:00 committed by Wally B. Feed
parent 29420b1912
commit 5b35641881
1 changed files with 7 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

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