Simplify submap versions. (#235)
We now only keep track of the number of inserted range data.master
parent
92d360a8f2
commit
af22dc6fe1
|
@ -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());
|
||||||
|
|
|
@ -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
|
||||||
|
|
|
@ -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
|
||||||
|
|
|
@ -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
|
||||||
|
|
|
@ -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);
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -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,
|
||||||
|
|
|
@ -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});
|
||||||
|
|
|
@ -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_;
|
||||||
|
|
|
@ -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;
|
||||||
|
|
|
@ -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;
|
||||||
|
|
||||||
|
|
|
@ -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();
|
||||||
|
|
|
@ -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_;
|
||||||
|
|
|
@ -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, ¬ification](
|
constraint_builder_.WhenDone(
|
||||||
|
[this, ¬ification](
|
||||||
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());
|
||||||
}
|
}
|
||||||
|
|
|
@ -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,
|
||||||
|
|
|
@ -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(
|
||||||
|
|
|
@ -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
|
||||||
|
|
Loading…
Reference in New Issue