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

View File

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

View File

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

View File

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

View File

@ -42,8 +42,7 @@ proto::SubmapsOptions CreateSubmapsOptions(
common::LuaParameterDictionary* parameter_dictionary); common::LuaParameterDictionary* parameter_dictionary);
struct Submap : public mapping::Submap { struct Submap : public mapping::Submap {
Submap(const MapLimits& limits, const Eigen::Vector2f& origin, Submap(const MapLimits& limits, const Eigen::Vector2f& origin);
int begin_range_data_index);
ProbabilityGrid probability_grid; ProbabilityGrid probability_grid;
}; };
@ -74,12 +73,6 @@ class Submaps : public mapping::Submaps {
std::vector<std::unique_ptr<Submap>> submaps_; std::vector<std::unique_ptr<Submap>> submaps_;
RangeDataInserter range_data_inserter_; 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 } // namespace mapping_2d

View File

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

View File

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

View File

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

View File

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

View File

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

View File

@ -55,14 +55,10 @@ class LocalTrajectoryBuilderInterface {
const Eigen::Vector3d& angular_velocity) = 0; const Eigen::Vector3d& angular_velocity) = 0;
virtual std::unique_ptr<InsertionResult> AddRangefinderData( virtual std::unique_ptr<InsertionResult> AddRangefinderData(
common::Time time, const Eigen::Vector3f& origin, 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, virtual void AddOdometerData(common::Time time,
const transform::Rigid3d& pose) = 0; 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 mapping_3d::Submaps* submaps() const = 0;
virtual const PoseEstimate& pose_estimate() const = 0; virtual const PoseEstimate& pose_estimate() const = 0;

View File

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

View File

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

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

View File

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

View File

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

View File

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