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

Simplify submap versions. (#235) 

We now only keep track of the number of inserted range data.
master
Wolfgang Hess 2017-04-21 14:12:08 +02:00 committed by GitHub
parent 92d360a8f2
commit af22dc6fe1
16 changed files with 121 additions and 148 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
cartographer/mapping/map_builder.cc
View File

@ -141,8 +141,7 @@ string MapBuilder::SubmapToProto(const int trajectory_id,
" submaps in this trajectory.";
}
response->set_submap_version(
submaps->Get(submap_index)->end_range_data_index);
response->set_submap_version(submaps->Get(submap_index)->num_range_data);
const std::vector<transform::Rigid3d> submap_transforms =
sparse_pose_graph_->GetSubmapTransforms(*submaps);
CHECK_EQ(submap_transforms.size(), submaps->size());

13
cartographer/mapping/submaps.h
View File

@ -64,14 +64,11 @@ inline uint8 ProbabilityToLogOddsInteger(const float probability) {
}
// An individual submap, which has an initial position 'origin', keeps track of
// which range data were inserted into it, and sets the
// how many range data were inserted into it, and sets the
// 'finished_probability_grid' to be used for loop closing once the map no
// longer changes.
struct Submap {
Submap(const Eigen::Vector3f& origin, int begin_range_data_index)
: origin(origin),
begin_range_data_index(begin_range_data_index),
end_range_data_index(begin_range_data_index) {}
Submap(const Eigen::Vector3f& origin) : origin(origin) {}
transform::Rigid3d local_pose() const {
return transform::Rigid3d::Translation(origin.cast<double>());
@ -80,10 +77,8 @@ struct Submap {
// Origin of this submap.
Eigen::Vector3f origin;
// This Submap contains RangeData with indices in the range
// ['begin_range_data_index', 'end_range_data_index').
int begin_range_data_index;
int end_range_data_index;
// Number of RangeData inserted.
int num_range_data = 0;
// The 'finished_probability_grid' when this submap is finished and will not
// change anymore. Otherwise, this is nullptr and the next call to

17
cartographer/mapping_2d/submaps.cc
View File

@ -101,10 +101,8 @@ proto::SubmapsOptions CreateSubmapsOptions(
return options;
}
Submap::Submap(const MapLimits& limits, const Eigen::Vector2f& origin,
const int begin_range_data_index)
: mapping::Submap(Eigen::Vector3f(origin.x(), origin.y(), 0.),
begin_range_data_index),
Submap::Submap(const MapLimits& limits, const Eigen::Vector2f& origin)
: mapping::Submap(Eigen::Vector3f(origin.x(), origin.y(), 0.)),
probability_grid(limits) {}
Submaps::Submaps(const proto::SubmapsOptions& options)
@ -116,16 +114,14 @@ Submaps::Submaps(const proto::SubmapsOptions& options)
}
void Submaps::InsertRangeData(const sensor::RangeData& range_data) {
CHECK_LT(num_range_data_, std::numeric_limits<int>::max());
++num_range_data_;
for (const int index : insertion_indices()) {
Submap* submap = submaps_[index].get();
CHECK(submap->finished_probability_grid == nullptr);
range_data_inserter_.Insert(range_data, &submap->probability_grid);
submap->end_range_data_index = num_range_data_;
++submap->num_range_data;
}
++num_range_data_in_last_submap_;
if (num_range_data_in_last_submap_ == options_.num_range_data()) {
const Submap* const last_submap = Get(size() - 1);
if (last_submap->num_range_data == options_.num_range_data()) {
AddSubmap(range_data.origin.head<2>());
}
}
@ -173,9 +169,8 @@ void Submaps::AddSubmap(const Eigen::Vector2f& origin) {
origin.cast<double>() +
options_.half_length() * Eigen::Vector2d::Ones(),
CellLimits(num_cells_per_dimension, num_cells_per_dimension)),
origin, num_range_data_));
origin));
LOG(INFO) << "Added submap " << size();
num_range_data_in_last_submap_ = 0;
}
} // namespace mapping_2d

9
cartographer/mapping_2d/submaps.h
View File

@ -42,8 +42,7 @@ proto::SubmapsOptions CreateSubmapsOptions(
common::LuaParameterDictionary* parameter_dictionary);
struct Submap : public mapping::Submap {
Submap(const MapLimits& limits, const Eigen::Vector2f& origin,
int begin_range_data_index);
Submap(const MapLimits& limits, const Eigen::Vector2f& origin);
ProbabilityGrid probability_grid;
};
@ -74,12 +73,6 @@ class Submaps : public mapping::Submaps {
std::vector<std::unique_ptr<Submap>> submaps_;
RangeDataInserter range_data_inserter_;
// Number of RangeData inserted.
int num_range_data_ = 0;
// Number of RangeData inserted since the last Submap was added.
int num_range_data_in_last_submap_ = 0;
};
} // namespace mapping_2d

10
cartographer/mapping_2d/submaps_test.cc
View File

@ -46,23 +46,17 @@ TEST(SubmapsTest, TheRightNumberOfScansAreInserted) {
"},"
"}");
Submaps submaps{CreateSubmapsOptions(parameter_dictionary.get())};
auto num_inserted = [&submaps](const int i) {
return submaps.Get(i)->end_range_data_index -
submaps.Get(i)->begin_range_data_index;
};
for (int i = 0; i != 1000; ++i) {
submaps.InsertRangeData({Eigen::Vector3f::Zero(), {}, {}});
const int matching = submaps.matching_index();
// Except for the first, maps should only be returned after enough scans.
if (matching != 0) {
EXPECT_LE(kNumRangeData, num_inserted(matching));
EXPECT_LE(kNumRangeData, submaps.Get(matching)->num_range_data);
}
}
for (int i = 0; i != submaps.size() - 2; ++i) {
// Submaps should not be left without the right number of scans in them.
EXPECT_EQ(kNumRangeData * 2, num_inserted(i));
EXPECT_EQ(i * kNumRangeData, submaps.Get(i)->begin_range_data_index);
EXPECT_EQ((i + 2) * kNumRangeData, submaps.Get(i)->end_range_data_index);
EXPECT_EQ(kNumRangeData * 2, submaps.Get(i)->num_range_data);
}
}

7
cartographer/mapping_3d/global_trajectory_builder.cc
View File

@ -45,19 +45,18 @@ void GlobalTrajectoryBuilder::AddImuData(
void GlobalTrajectoryBuilder::AddRangefinderData(
const common::Time time, const Eigen::Vector3f& origin,
const sensor::PointCloud& ranges) {
auto insertion_result =
local_trajectory_builder_->AddRangefinderData(time, origin, ranges);
auto insertion_result = local_trajectory_builder_->AddRangefinderData(
time, origin, ranges, sparse_pose_graph_->GetNextTrajectoryNodeIndex());
if (insertion_result == nullptr) {
return;
}
const int trajectory_node_index = sparse_pose_graph_->AddScan(
sparse_pose_graph_->AddScan(
insertion_result->time, insertion_result->range_data_in_tracking,
insertion_result->pose_observation, insertion_result->covariance_estimate,
insertion_result->submaps, insertion_result->matching_submap,
insertion_result->insertion_submaps);
local_trajectory_builder_->AddTrajectoryNodeIndex(trajectory_node_index);
}
void GlobalTrajectoryBuilder::AddOdometerData(const common::Time time,

27
cartographer/mapping_3d/kalman_local_trajectory_builder.cc
View File

@ -72,7 +72,7 @@ void KalmanLocalTrajectoryBuilder::AddImuData(
std::unique_ptr<KalmanLocalTrajectoryBuilder::InsertionResult>
KalmanLocalTrajectoryBuilder::AddRangefinderData(
const common::Time time, const Eigen::Vector3f& origin,
const sensor::PointCloud& ranges) {
const sensor::PointCloud& ranges, int next_trajectory_node_index) {
if (!pose_tracker_) {
LOG(INFO) << "PoseTracker not yet initialized.";
return nullptr;
@ -114,15 +114,18 @@ KalmanLocalTrajectoryBuilder::AddRangefinderData(
if (num_accumulated_ >= options_.scans_per_accumulation()) {
num_accumulated_ = 0;
return AddAccumulatedRangeData(
time, sensor::TransformRangeData(accumulated_range_data_,
tracking_delta.inverse()));
time,
sensor::TransformRangeData(accumulated_range_data_,
tracking_delta.inverse()),
next_trajectory_node_index);
}
return nullptr;
}
std::unique_ptr<KalmanLocalTrajectoryBuilder::InsertionResult>
KalmanLocalTrajectoryBuilder::AddAccumulatedRangeData(
const common::Time time, const sensor::RangeData& range_data_in_tracking) {
const common::Time time, const sensor::RangeData& range_data_in_tracking,
const int trajectory_node_index) {
const sensor::RangeData filtered_range_data = {
range_data_in_tracking.origin,
sensor::VoxelFiltered(range_data_in_tracking.returns,
@ -181,7 +184,7 @@ KalmanLocalTrajectoryBuilder::AddAccumulatedRangeData(
pose_observation.cast<float>())};
return InsertIntoSubmap(time, filtered_range_data, pose_observation,
covariance_estimate);
covariance_estimate, trajectory_node_index);
}
void KalmanLocalTrajectoryBuilder::AddOdometerData(
@ -206,16 +209,12 @@ KalmanLocalTrajectoryBuilder::pose_estimate() const {
return last_pose_estimate_;
}
void KalmanLocalTrajectoryBuilder::AddTrajectoryNodeIndex(
int trajectory_node_index) {
submaps_->AddTrajectoryNodeIndex(trajectory_node_index);
}
std::unique_ptr<KalmanLocalTrajectoryBuilder::InsertionResult>
KalmanLocalTrajectoryBuilder::InsertIntoSubmap(
const common::Time time, const sensor::RangeData& range_data_in_tracking,
const transform::Rigid3d& pose_observation,
const kalman_filter::PoseCovariance& covariance_estimate) {
const kalman_filter::PoseCovariance& covariance_estimate,
const int trajectory_node_index) {
if (motion_filter_.IsSimilar(time, pose_observation)) {
return nullptr;
}
@ -225,8 +224,10 @@ KalmanLocalTrajectoryBuilder::InsertIntoSubmap(
for (int insertion_index : submaps_->insertion_indices()) {
insertion_submaps.push_back(submaps_->Get(insertion_index));
}
submaps_->InsertRangeData(sensor::TransformRangeData(
range_data_in_tracking, pose_observation.cast<float>()));
submaps_->InsertRangeData(
sensor::TransformRangeData(range_data_in_tracking,
pose_observation.cast<float>()),
trajectory_node_index);
return std::unique_ptr<InsertionResult>(new InsertionResult{
time, range_data_in_tracking, pose_observation, covariance_estimate,
submaps_.get(), matching_submap, insertion_submaps});

12
cartographer/mapping_3d/kalman_local_trajectory_builder.h
View File

@ -50,21 +50,23 @@ class KalmanLocalTrajectoryBuilder : public LocalTrajectoryBuilderInterface {
const Eigen::Vector3d& angular_velocity) override;
std::unique_ptr<InsertionResult> AddRangefinderData(
common::Time time, const Eigen::Vector3f& origin,
const sensor::PointCloud& ranges) override;
const sensor::PointCloud& ranges,
int next_trajectory_node_index) override;
void AddOdometerData(common::Time time,
const transform::Rigid3d& pose) override;
void AddTrajectoryNodeIndex(int trajectory_node_index) override;
const mapping_3d::Submaps* submaps() const override;
const PoseEstimate& pose_estimate() const override;
private:
std::unique_ptr<InsertionResult> AddAccumulatedRangeData(
common::Time time, const sensor::RangeData& range_data_in_tracking);
common::Time time, const sensor::RangeData& range_data_in_tracking,
int trajectory_node_index);
std::unique_ptr<InsertionResult> InsertIntoSubmap(
const common::Time time, const sensor::RangeData& range_data_in_tracking,
common::Time time, const sensor::RangeData& range_data_in_tracking,
const transform::Rigid3d& pose_observation,
const kalman_filter::PoseCovariance& covariance_estimate);
const kalman_filter::PoseCovariance& covariance_estimate,
int trajectory_node_index);
const proto::LocalTrajectoryBuilderOptions options_;
std::unique_ptr<mapping_3d::Submaps> submaps_;

3
cartographer/mapping_3d/kalman_local_trajectory_builder_test.cc
View File

@ -261,7 +261,8 @@ class KalmanLocalTrajectoryBuilderTest : public ::testing::Test {
AddLinearOnlyImuObservation(node.time, node.pose);
const auto range_data = GenerateRangeData(node.pose);
if (local_trajectory_builder_->AddRangefinderData(
node.time, range_data.origin, range_data.returns) != nullptr) {
node.time, range_data.origin, range_data.returns, num_poses) !=
nullptr) {
const auto pose_estimate = local_trajectory_builder_->pose_estimate();
EXPECT_THAT(pose_estimate.pose, transform::IsNearly(node.pose, 1e-1));
++num_poses;

6
cartographer/mapping_3d/local_trajectory_builder_interface.h
View File

@ -55,14 +55,10 @@ class LocalTrajectoryBuilderInterface {
const Eigen::Vector3d& angular_velocity) = 0;
virtual std::unique_ptr<InsertionResult> AddRangefinderData(
common::Time time, const Eigen::Vector3f& origin,
const sensor::PointCloud& ranges) = 0;
const sensor::PointCloud& ranges, int next_trajectory_node_index) = 0;
virtual void AddOdometerData(common::Time time,
const transform::Rigid3d& pose) = 0;
// Register a 'trajectory_node_index' from the SparsePoseGraph corresponding
// to the latest inserted laser scan. This is used to remember which
// trajectory node should be used to visualize a Submap.
virtual void AddTrajectoryNodeIndex(int trajectory_node_index) = 0;
virtual const mapping_3d::Submaps* submaps() const = 0;
virtual const PoseEstimate& pose_estimate() const = 0;

30
cartographer/mapping_3d/optimizing_local_trajectory_builder.cc
View File

@ -135,7 +135,7 @@ void OptimizingLocalTrajectoryBuilder::AddOdometerData(
std::unique_ptr<OptimizingLocalTrajectoryBuilder::InsertionResult>
OptimizingLocalTrajectoryBuilder::AddRangefinderData(
const common::Time time, const Eigen::Vector3f& origin,
const sensor::PointCloud& ranges) {
const sensor::PointCloud& ranges, const int next_trajectory_node_index) {
CHECK_GT(ranges.size(), 0);
// TODO(hrapp): Handle misses.
@ -188,7 +188,7 @@ OptimizingLocalTrajectoryBuilder::AddRangefinderData(
++num_accumulated_;
RemoveObsoleteSensorData();
return MaybeOptimize(time);
return MaybeOptimize(time, next_trajectory_node_index);
}
void OptimizingLocalTrajectoryBuilder::RemoveObsoleteSensorData() {
@ -215,7 +215,8 @@ void OptimizingLocalTrajectoryBuilder::RemoveObsoleteSensorData() {
}
std::unique_ptr<OptimizingLocalTrajectoryBuilder::InsertionResult>
OptimizingLocalTrajectoryBuilder::MaybeOptimize(const common::Time time) {
OptimizingLocalTrajectoryBuilder::MaybeOptimize(
const common::Time time, const int trajectory_node_index) {
// TODO(hrapp): Make the number of optimizations configurable.
if (num_accumulated_ < options_.scans_per_accumulation() &&
num_accumulated_ % 10 != 0) {
@ -348,13 +349,15 @@ OptimizingLocalTrajectoryBuilder::MaybeOptimize(const common::Time time) {
}
return AddAccumulatedRangeData(time, optimized_pose,
accumulated_range_data_in_tracking);
accumulated_range_data_in_tracking,
trajectory_node_index);
}
std::unique_ptr<OptimizingLocalTrajectoryBuilder::InsertionResult>
OptimizingLocalTrajectoryBuilder::AddAccumulatedRangeData(
const common::Time time, const transform::Rigid3d& optimized_pose,
const sensor::RangeData& range_data_in_tracking) {
const sensor::RangeData& range_data_in_tracking,
const int trajectory_node_index) {
const sensor::RangeData filtered_range_data = {
range_data_in_tracking.origin,
sensor::VoxelFiltered(range_data_in_tracking.returns,
@ -372,7 +375,8 @@ OptimizingLocalTrajectoryBuilder::AddAccumulatedRangeData(
sensor::TransformPointCloud(filtered_range_data.returns,
optimized_pose.cast<float>())};
return InsertIntoSubmap(time, filtered_range_data, optimized_pose);
return InsertIntoSubmap(time, filtered_range_data, optimized_pose,
trajectory_node_index);
}
const OptimizingLocalTrajectoryBuilder::PoseEstimate&
@ -380,15 +384,11 @@ OptimizingLocalTrajectoryBuilder::pose_estimate() const {
return last_pose_estimate_;
}
void OptimizingLocalTrajectoryBuilder::AddTrajectoryNodeIndex(
int trajectory_node_index) {
submaps_->AddTrajectoryNodeIndex(trajectory_node_index);
}
std::unique_ptr<OptimizingLocalTrajectoryBuilder::InsertionResult>
OptimizingLocalTrajectoryBuilder::InsertIntoSubmap(
const common::Time time, const sensor::RangeData& range_data_in_tracking,
const transform::Rigid3d& pose_observation) {
const transform::Rigid3d& pose_observation,
const int trajectory_node_index) {
if (motion_filter_.IsSimilar(time, pose_observation)) {
return nullptr;
}
@ -398,8 +398,10 @@ OptimizingLocalTrajectoryBuilder::InsertIntoSubmap(
for (int insertion_index : submaps_->insertion_indices()) {
insertion_submaps.push_back(submaps_->Get(insertion_index));
}
submaps_->InsertRangeData(sensor::TransformRangeData(
range_data_in_tracking, pose_observation.cast<float>()));
submaps_->InsertRangeData(
sensor::TransformRangeData(range_data_in_tracking,
pose_observation.cast<float>()),
trajectory_node_index);
const kalman_filter::PoseCovariance kCovariance =
1e-7 * kalman_filter::PoseCovariance::Identity();

15
cartographer/mapping_3d/optimizing_local_trajectory_builder.h
View File

@ -53,10 +53,10 @@ class OptimizingLocalTrajectoryBuilder
const Eigen::Vector3d& angular_velocity) override;
std::unique_ptr<InsertionResult> AddRangefinderData(
common::Time time, const Eigen::Vector3f& origin,
const sensor::PointCloud& ranges) override;
void AddOdometerData(const common::Time time,
const sensor::PointCloud& ranges,
int next_trajectory_node_index) override;
void AddOdometerData(common::Time time,
const transform::Rigid3d& pose) override;
void AddTrajectoryNodeIndex(int trajectory_node_index) override;
const mapping_3d::Submaps* submaps() const override;
const PoseEstimate& pose_estimate() const override;
@ -102,13 +102,16 @@ class OptimizingLocalTrajectoryBuilder
std::unique_ptr<InsertionResult> AddAccumulatedRangeData(
common::Time time, const transform::Rigid3d& pose_observation,
const sensor::RangeData& range_data_in_tracking);
const sensor::RangeData& range_data_in_tracking,
const int next_trajectory_node_index);
std::unique_ptr<InsertionResult> InsertIntoSubmap(
const common::Time time, const sensor::RangeData& range_data_in_tracking,
const transform::Rigid3d& pose_observation);
const transform::Rigid3d& pose_observation,
const int next_trajectory_node_index);
std::unique_ptr<InsertionResult> MaybeOptimize(common::Time time);
std::unique_ptr<InsertionResult> MaybeOptimize(
common::Time time, const int next_trajectory_node_index);
const proto::LocalTrajectoryBuilderOptions options_;
const ceres::Solver::Options ceres_solver_options_;

53
cartographer/mapping_3d/sparse_pose_graph.cc
View File

@ -83,7 +83,7 @@ void SparsePoseGraph::GrowSubmapTransformsAsNeeded(
}
}
int SparsePoseGraph::AddScan(
void SparsePoseGraph::AddScan(
common::Time time, const sensor::RangeData& range_data_in_tracking,
const transform::Rigid3d& pose,
const kalman_filter::PoseCovariance& covariance, const Submaps* submaps,
@ -126,7 +126,11 @@ int SparsePoseGraph::AddScan(
ComputeConstraintsForScan(j, matching_submap, insertion_submaps,
finished_submap, pose, covariance);
});
return j;
}
int SparsePoseGraph::GetNextTrajectoryNodeIndex() {
common::MutexLocker locker(&mutex_);
return trajectory_nodes_.size();
}
void SparsePoseGraph::AddWorkItem(std::function<void()> work_item) {
@ -285,25 +289,30 @@ void SparsePoseGraph::WaitForAllComputations() {
common::MutexLocker locker(&mutex_);
const int num_finished_scans_at_start =
constraint_builder_.GetNumFinishedScans();
while (!locker.AwaitWithTimeout([this]() REQUIRES(mutex_) {
return static_cast<size_t>(constraint_builder_.GetNumFinishedScans()) ==
trajectory_nodes_.size();
}, common::FromSeconds(1.))) {
while (!locker.AwaitWithTimeout(
[this]() REQUIRES(mutex_) {
return static_cast<size_t>(constraint_builder_.GetNumFinishedScans()) ==
trajectory_nodes_.size();
},
common::FromSeconds(1.))) {
std::ostringstream progress_info;
progress_info << "Optimizing: " << std::fixed << std::setprecision(1)
<< 100. * (constraint_builder_.GetNumFinishedScans() -
num_finished_scans_at_start) /
<< 100. *
(constraint_builder_.GetNumFinishedScans() -
num_finished_scans_at_start) /
(trajectory_nodes_.size() -
num_finished_scans_at_start) << "%...";
num_finished_scans_at_start)
<< "%...";
std::cout << "\r\x1b[K" << progress_info.str() << std::flush;
}
std::cout << "\r\x1b[KOptimizing: Done. " << std::endl;
constraint_builder_.WhenDone([this, &notification](
const sparse_pose_graph::ConstraintBuilder::Result& result) {
constraints_.insert(constraints_.end(), result.begin(), result.end());
common::MutexLocker locker(&mutex_);
notification = true;
});
constraint_builder_.WhenDone(
[this, &notification](
const sparse_pose_graph::ConstraintBuilder::Result& result) {
constraints_.insert(constraints_.end(), result.begin(), result.end());
common::MutexLocker locker(&mutex_);
notification = true;
});
locker.Await([&notification]() { return notification; });
}
@ -336,13 +345,13 @@ void SparsePoseGraph::RunOptimization() {
const mapping::Submaps* trajectory =
trajectory_nodes_[i].constant_data->trajectory;
if (extrapolation_transforms.count(trajectory) == 0) {
extrapolation_transforms[trajectory] =
transform::Rigid3d(ExtrapolateSubmapTransforms(submap_transforms_,
*trajectory).back() *
ExtrapolateSubmapTransforms(
optimized_submap_transforms_, *trajectory)
.back()
.inverse());
extrapolation_transforms[trajectory] = transform::Rigid3d(
ExtrapolateSubmapTransforms(submap_transforms_, *trajectory)
.back() *
ExtrapolateSubmapTransforms(optimized_submap_transforms_,
*trajectory)
.back()
.inverse());
}
trajectory_nodes_[i].pose =
extrapolation_transforms[trajectory] * trajectory_nodes_[i].pose;

19
cartographer/mapping_3d/sparse_pose_graph.h
View File

@ -69,16 +69,19 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
// Adds a new 'range_data_in_tracking' observation at 'time', and a 'pose'
// that will later be optimized. The 'pose' was determined by scan matching
// against the 'matching_submap' and the scan was inserted into the
// 'insertion_submaps'. The index into the vector of trajectory nodes as
// used with GetTrajectoryNodes() is returned.
int AddScan(common::Time time,
const sensor::RangeData& range_data_in_tracking,
const transform::Rigid3d& pose,
const kalman_filter::PoseCovariance& pose_covariance,
const Submaps* submaps, const Submap* matching_submap,
const std::vector<const Submap*>& insertion_submaps)
// 'insertion_submaps'.
void AddScan(common::Time time,
const sensor::RangeData& range_data_in_tracking,
const transform::Rigid3d& pose,
const kalman_filter::PoseCovariance& pose_covariance,
const Submaps* submaps, const Submap* matching_submap,
const std::vector<const Submap*>& insertion_submaps)
EXCLUDES(mutex_);
// The index into the vector of trajectory nodes as used with
// GetTrajectoryNodes() for the next node added with AddScan() is returned.
int GetNextTrajectoryNodeIndex() EXCLUDES(mutex_);
// Adds new IMU data to be used in the optimization.
void AddImuData(const mapping::Submaps* trajectory, common::Time time,
const Eigen::Vector3d& linear_acceleration,

30
cartographer/mapping_3d/submaps.cc
View File

@ -217,8 +217,8 @@ proto::SubmapsOptions CreateSubmapsOptions(
}
Submap::Submap(const float high_resolution, const float low_resolution,
const Eigen::Vector3f& origin, const int begin_range_data_index)
: mapping::Submap(origin, begin_range_data_index),
const Eigen::Vector3f& origin)
: mapping::Submap(origin),
high_resolution_hybrid_grid(high_resolution, origin),
low_resolution_hybrid_grid(low_resolution, origin) {}
@ -274,9 +274,8 @@ void Submaps::SubmapToProto(
global_submap_pose.translation().z())));
}
void Submaps::InsertRangeData(const sensor::RangeData& range_data) {
CHECK_LT(num_range_data_, std::numeric_limits<int>::max());
++num_range_data_;
void Submaps::InsertRangeData(const sensor::RangeData& range_data,
const int trajectory_node_index) {
for (const int index : insertion_indices()) {
Submap* submap = submaps_[index].get();
range_data_inserter_.Insert(
@ -285,10 +284,11 @@ void Submaps::InsertRangeData(const sensor::RangeData& range_data) {
&submap->high_resolution_hybrid_grid);
range_data_inserter_.Insert(range_data,
&submap->low_resolution_hybrid_grid);
submap->end_range_data_index = num_range_data_;
++submap->num_range_data;
submap->trajectory_node_indices.push_back(trajectory_node_index);
}
++num_range_data_in_last_submap_;
if (num_range_data_in_last_submap_ == options_.num_range_data()) {
const Submap* const last_submap = Get(size() - 1);
if (last_submap->num_range_data == options_.num_range_data()) {
AddSubmap(range_data.origin);
}
}
@ -301,16 +301,6 @@ const HybridGrid& Submaps::low_resolution_matching_grid() const {
return submaps_[matching_index()]->low_resolution_hybrid_grid;
}
void Submaps::AddTrajectoryNodeIndex(const int trajectory_node_index) {
for (int i = 0; i != size(); ++i) {
Submap& submap = *submaps_[i];
if (submap.end_range_data_index == num_range_data_ &&
submap.begin_range_data_index <= num_range_data_ - 1) {
submap.trajectory_node_indices.push_back(trajectory_node_index);
}
}
}
void Submaps::AddSubmap(const Eigen::Vector3f& origin) {
if (size() > 1) {
Submap* submap = submaps_[size() - 2].get();
@ -318,10 +308,8 @@ void Submaps::AddSubmap(const Eigen::Vector3f& origin) {
submap->finished = true;
}
submaps_.emplace_back(new Submap(options_.high_resolution(),
options_.low_resolution(), origin,
num_range_data_));
options_.low_resolution(), origin));
LOG(INFO) << "Added submap " << size();
num_range_data_in_last_submap_ = 0;
}
std::vector<Submaps::PixelData> Submaps::AccumulatePixelData(

15
cartographer/mapping_3d/submaps.h
View File

@ -47,11 +47,12 @@ proto::SubmapsOptions CreateSubmapsOptions(
struct Submap : public mapping::Submap {
Submap(float high_resolution, float low_resolution,
const Eigen::Vector3f& origin, int begin_range_data_index);
const Eigen::Vector3f& origin);
HybridGrid high_resolution_hybrid_grid;
HybridGrid low_resolution_hybrid_grid;
bool finished = false;
// Indices into the nodes of the SparsePoseGraph used to visualize the submap.
std::vector<int> trajectory_node_indices;
};
@ -71,7 +72,8 @@ class Submaps : public mapping::Submaps {
mapping::proto::SubmapQuery::Response* response) const override;
// Inserts 'range_data' into the Submap collection.
void InsertRangeData(const sensor::RangeData& range_data);
void InsertRangeData(const sensor::RangeData& range_data,
int trajectory_node_index);
// Returns the 'high_resolution' HybridGrid to be used for matching.
const HybridGrid& high_resolution_matching_grid() const;
@ -79,9 +81,6 @@ class Submaps : public mapping::Submaps {
// Returns the 'low_resolution' HybridGrid to be used for matching.
const HybridGrid& low_resolution_matching_grid() const;
// Adds a node to be used when visualizing the submap.
void AddTrajectoryNodeIndex(int trajectory_node_index);
private:
struct PixelData {
int min_z = INT_MAX;
@ -112,12 +111,6 @@ class Submaps : public mapping::Submaps {
std::vector<std::unique_ptr<Submap>> submaps_;
RangeDataInserter range_data_inserter_;
// Number of RangeData inserted.
int num_range_data_ = 0;
// Number of RangeData inserted since the last Submap was added.
int num_range_data_in_last_submap_ = 0;
};
} // namespace mapping_3d