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

Converted Submap from struct to class. (#333)

master
Holger Rapp 2017-06-12 17:28:38 +02:00 committed by Wolfgang Hess
parent 2d75f4ef56
commit cff0c73857
14 changed files with 106 additions and 73 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
cartographer/mapping/map_builder.cc
View File

@ -125,7 +125,7 @@ string MapBuilder::SubmapToProto(const int trajectory_id,
const Submap* const submap =
trajectory_builders_.at(trajectory_id)->submaps()->Get(submap_index);
response->set_submap_version(submap->num_range_data);
response->set_submap_version(submap->num_range_data());
submap->ToResponseProto(submap_transforms[submap_index], response);
return "";
}

22
cartographer/mapping/submaps.h
View File

@ -56,26 +56,36 @@ inline uint8 ProbabilityToLogOddsInteger(const float probability) {
// track of 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.
// TODO(hrapp): This should be a class now.
struct Submap {
Submap(const transform::Rigid3d& local_pose) : local_pose(local_pose) {}
class Submap {
public:
Submap(const transform::Rigid3d& local_pose) : local_pose_(local_pose) {}
virtual ~Submap() {}
// Local SLAM pose of this submap.
const transform::Rigid3d local_pose;
transform::Rigid3d local_pose() const { return local_pose_; }
// Number of RangeData inserted.
int num_range_data = 0;
size_t num_range_data() const { return num_range_data_; }
// The 'finished_probability_grid' when this submap is finished and will not
// change anymore. Otherwise, this is nullptr and the next call to
// InsertRangeData() will change the submap.
const mapping_2d::ProbabilityGrid* finished_probability_grid = nullptr;
const mapping_2d::ProbabilityGrid* finished_probability_grid() const {
return finished_probability_grid_;
}
// Fills data into the 'response'.
virtual void ToResponseProto(
const transform::Rigid3d& global_submap_pose,
proto::SubmapQuery::Response* response) const = 0;
private:
const transform::Rigid3d local_pose_;
protected:
// TODO(hrapp): All of this should be private.
int num_range_data_ = 0;
const mapping_2d::ProbabilityGrid* finished_probability_grid_ = nullptr;
};
// Submaps is a sequence of maps to which scans are matched and into which scans

2
cartographer/mapping_2d/local_trajectory_builder.cc
View File

@ -71,7 +71,7 @@ void LocalTrajectoryBuilder::ScanMatch(
const sensor::RangeData& range_data_in_tracking_2d,
transform::Rigid3d* pose_observation) {
const ProbabilityGrid& probability_grid =
submaps_.Get(submaps_.matching_index())->probability_grid;
submaps_.Get(submaps_.matching_index())->probability_grid();
transform::Rigid2d pose_prediction_2d =
transform::Project2D(pose_prediction * tracking_to_tracking_2d.inverse());
// The online correlative scan matcher will refine the initial estimate for

9
cartographer/mapping_2d/sparse_pose_graph.cc
View File

@ -116,7 +116,7 @@ void SparsePoseGraph::AddScan(
submap_data_.at(submap_id).submap = insertion_submaps.back();
}
const mapping::Submap* const finished_submap =
insertion_submaps.front()->finished_probability_grid != nullptr
insertion_submaps.front()->finished_probability_grid() != nullptr
? insertion_submaps.front()
: nullptr;
@ -430,7 +430,8 @@ transform::Rigid3d SparsePoseGraph::GetLocalToGlobalTransform(
.at(mapping::SubmapId{
trajectory_id,
static_cast<int>(extrapolated_submap_transforms.size()) - 1})
.submap->local_pose.inverse();
.submap->local_pose()
.inverse();
}
std::vector<std::vector<int>> SparsePoseGraph::GetConnectedTrajectories() {
@ -474,8 +475,8 @@ std::vector<transform::Rigid3d> SparsePoseGraph::ExtrapolateSubmapTransforms(
trajectory_id, static_cast<int>(result.size()) - 1};
result.push_back(
result.back() *
submap_data_.at(previous_submap_id).submap->local_pose.inverse() *
submap_data.submap->local_pose);
submap_data_.at(previous_submap_id).submap->local_pose().inverse() *
submap_data.submap->local_pose());
}
}

6
cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc
View File

@ -39,7 +39,7 @@ namespace mapping_2d {
namespace sparse_pose_graph {
transform::Rigid2d ComputeSubmapPose(const mapping::Submap& submap) {
return transform::Project2D(submap.local_pose);
return transform::Project2D(submap.local_pose());
}
ConstraintBuilder::ConstraintBuilder(
@ -74,7 +74,7 @@ void ConstraintBuilder::MaybeAddConstraint(
++pending_computations_[current_computation_];
const int current_computation = current_computation_;
ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
submap_id, submap->finished_probability_grid, [=]() EXCLUDES(mutex_) {
submap_id, submap->finished_probability_grid(), [=]() EXCLUDES(mutex_) {
ComputeConstraint(submap_id, submap, node_id,
false, /* match_full_submap */
nullptr, /* trajectory_connectivity */
@ -94,7 +94,7 @@ void ConstraintBuilder::MaybeAddGlobalConstraint(
++pending_computations_[current_computation_];
const int current_computation = current_computation_;
ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
submap_id, submap->finished_probability_grid, [=]() EXCLUDES(mutex_) {
submap_id, submap->finished_probability_grid(), [=]() EXCLUDES(mutex_) {
ComputeConstraint(submap_id, submap, node_id,
true, /* match_full_submap */
trajectory_connectivity, point_cloud,

34
cartographer/mapping_2d/submaps.cc
View File

@ -103,18 +103,18 @@ proto::SubmapsOptions CreateSubmapsOptions(
Submap::Submap(const MapLimits& limits, const Eigen::Vector2f& origin)
: mapping::Submap(transform::Rigid3d::Translation(
Eigen::Vector3d(origin.x(), origin.y(), 0.))),
probability_grid(limits) {}
probability_grid_(limits) {}
void Submap::ToResponseProto(
const transform::Rigid3d&,
mapping::proto::SubmapQuery::Response* const response) const {
Eigen::Array2i offset;
CellLimits limits;
probability_grid.ComputeCroppedLimits(&offset, &limits);
probability_grid_.ComputeCroppedLimits(&offset, &limits);
string cells;
for (const Eigen::Array2i& xy_index : XYIndexRangeIterator(limits)) {
if (probability_grid.IsKnown(xy_index + offset)) {
if (probability_grid_.IsKnown(xy_index + offset)) {
// We would like to add 'delta' but this is not possible using a value and
// alpha. We use premultiplied alpha, so when 'delta' is positive we can
// add it by setting 'alpha' to zero. If it is negative, we set 'value' to
@ -123,7 +123,7 @@ void Submap::ToResponseProto(
// detect visually for the user, though.
const int delta =
128 - mapping::ProbabilityToLogOddsInteger(
probability_grid.GetProbability(xy_index + offset));
probability_grid_.GetProbability(xy_index + offset));
const uint8 alpha = delta > 0 ? 0 : -delta;
const uint8 value = delta > 0 ? delta : 0;
cells.push_back(value);
@ -138,14 +138,14 @@ void Submap::ToResponseProto(
response->set_width(limits.num_x_cells);
response->set_height(limits.num_y_cells);
const double resolution = probability_grid.limits().resolution();
const double resolution = probability_grid_.limits().resolution();
response->set_resolution(resolution);
const double max_x =
probability_grid.limits().max().x() - resolution * offset.y();
probability_grid_.limits().max().x() - resolution * offset.y();
const double max_y =
probability_grid.limits().max().y() - resolution * offset.x();
probability_grid_.limits().max().y() - resolution * offset.x();
*response->mutable_slice_pose() = transform::ToProto(
local_pose.inverse() *
local_pose().inverse() *
transform::Rigid3d::Translation(Eigen::Vector3d(max_x, max_y, 0.)));
}
@ -160,12 +160,12 @@ Submaps::Submaps(const proto::SubmapsOptions& options)
void Submaps::InsertRangeData(const sensor::RangeData& 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->num_range_data;
CHECK(submap->finished_probability_grid_ == nullptr);
range_data_inserter_.Insert(range_data, &submap->probability_grid_);
++submap->num_range_data_;
}
const Submap* const last_submap = Get(size() - 1);
if (last_submap->num_range_data == options_.num_range_data()) {
if (last_submap->num_range_data_ == options_.num_range_data()) {
AddSubmap(range_data.origin.head<2>());
}
}
@ -182,14 +182,14 @@ void Submaps::FinishSubmap(int index) {
// Crop the finished Submap before inserting a new Submap to reduce peak
// memory usage a bit.
Submap* submap = submaps_[index].get();
CHECK(submap->finished_probability_grid == nullptr);
submap->probability_grid =
ComputeCroppedProbabilityGrid(submap->probability_grid);
submap->finished_probability_grid = &submap->probability_grid;
CHECK(submap->finished_probability_grid_ == nullptr);
submap->probability_grid_ =
ComputeCroppedProbabilityGrid(submap->probability_grid_);
submap->finished_probability_grid_ = &submap->probability_grid_;
if (options_.output_debug_images()) {
// Output the Submap that won't be changed from now on.
WriteDebugImage("submap" + std::to_string(index) + ".webp",
submap->probability_grid);
submap->probability_grid_);
}
}

11
cartographer/mapping_2d/submaps.h
View File

@ -41,14 +41,21 @@ ProbabilityGrid ComputeCroppedProbabilityGrid(
proto::SubmapsOptions CreateSubmapsOptions(
common::LuaParameterDictionary* parameter_dictionary);
struct Submap : public mapping::Submap {
class Submap : public mapping::Submap {
public:
Submap(const MapLimits& limits, const Eigen::Vector2f& origin);
ProbabilityGrid probability_grid;
const ProbabilityGrid& probability_grid() const { return probability_grid_; }
void ToResponseProto(
const transform::Rigid3d& global_submap_pose,
mapping::proto::SubmapQuery::Response* response) const override;
private:
// TODO(hrapp): Remove friend declaration.
friend class Submaps;
ProbabilityGrid probability_grid_;
};
// A container of Submaps.

4
cartographer/mapping_2d/submaps_test.cc
View File

@ -51,12 +51,12 @@ TEST(SubmapsTest, TheRightNumberOfScansAreInserted) {
const int matching = submaps.matching_index();
// Except for the first, maps should only be returned after enough scans.
if (matching != 0) {
EXPECT_LE(kNumRangeData, submaps.Get(matching)->num_range_data);
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, submaps.Get(i)->num_range_data);
EXPECT_EQ(kNumRangeData * 2, submaps.Get(i)->num_range_data());
}
}

10
cartographer/mapping_3d/kalman_local_trajectory_builder.cc
View File

@ -143,7 +143,7 @@ KalmanLocalTrajectoryBuilder::AddAccumulatedRangeData(
const Submap* const matching_submap =
submaps_->Get(submaps_->matching_index());
transform::Rigid3d initial_ceres_pose =
matching_submap->local_pose.inverse() * pose_prediction;
matching_submap->local_pose().inverse() * pose_prediction;
sensor::AdaptiveVoxelFilter adaptive_voxel_filter(
options_.high_resolution_adaptive_voxel_filter_options());
const sensor::PointCloud filtered_point_cloud_in_tracking =
@ -154,7 +154,7 @@ KalmanLocalTrajectoryBuilder::AddAccumulatedRangeData(
const transform::Rigid3d initial_pose = initial_ceres_pose;
real_time_correlative_scan_matcher_->Match(
initial_pose, filtered_point_cloud_in_tracking,
matching_submap->high_resolution_hybrid_grid, &initial_ceres_pose);
matching_submap->high_resolution_hybrid_grid(), &initial_ceres_pose);
}
transform::Rigid3d pose_observation_in_submap;
@ -166,12 +166,12 @@ KalmanLocalTrajectoryBuilder::AddAccumulatedRangeData(
low_resolution_adaptive_voxel_filter.Filter(filtered_range_data.returns);
ceres_scan_matcher_->Match(scan_matcher_pose_estimate_, initial_ceres_pose,
{{&filtered_point_cloud_in_tracking,
&matching_submap->high_resolution_hybrid_grid},
&matching_submap->high_resolution_hybrid_grid()},
{&low_resolution_point_cloud_in_tracking,
&matching_submap->low_resolution_hybrid_grid}},
&matching_submap->low_resolution_hybrid_grid()}},
&pose_observation_in_submap, &summary);
const transform::Rigid3d pose_observation =
matching_submap->local_pose * pose_observation_in_submap;
matching_submap->local_pose() * pose_observation_in_submap;
pose_tracker_->AddPoseObservation(
time, pose_observation,
options_.kalman_local_trajectory_builder_options()

8
cartographer/mapping_3d/optimizing_local_trajectory_builder.cc
View File

@ -241,7 +241,7 @@ OptimizingLocalTrajectoryBuilder::MaybeOptimize(const common::Time time) {
// We transform the states in 'batches_' in place to be in the submap frame as
// expected by the OccupiedSpaceCostFunctor. This is reverted after solving
// the optimization problem.
TransformStates(matching_submap->local_pose.inverse());
TransformStates(matching_submap->local_pose().inverse());
for (size_t i = 0; i < batches_.size(); ++i) {
Batch& batch = batches_[i];
problem.AddResidualBlock(
@ -253,7 +253,7 @@ OptimizingLocalTrajectoryBuilder::MaybeOptimize(const common::Time time) {
std::sqrt(static_cast<double>(
batch.high_resolution_filtered_points.size())),
batch.high_resolution_filtered_points,
matching_submap->high_resolution_hybrid_grid),
matching_submap->high_resolution_hybrid_grid()),
batch.high_resolution_filtered_points.size()),
nullptr, batch.state.translation.data(), batch.state.rotation.data());
problem.AddResidualBlock(
@ -265,7 +265,7 @@ OptimizingLocalTrajectoryBuilder::MaybeOptimize(const common::Time time) {
std::sqrt(static_cast<double>(
batch.low_resolution_filtered_points.size())),
batch.low_resolution_filtered_points,
matching_submap->low_resolution_hybrid_grid),
matching_submap->low_resolution_hybrid_grid()),
batch.low_resolution_filtered_points.size()),
nullptr, batch.state.translation.data(), batch.state.rotation.data());
@ -350,7 +350,7 @@ OptimizingLocalTrajectoryBuilder::MaybeOptimize(const common::Time time) {
ceres::Solve(ceres_solver_options_, &problem, &summary);
// The optimized states in 'batches_' are in the submap frame and we transform
// them in place to be in the local SLAM frame again.
TransformStates(matching_submap->local_pose);
TransformStates(matching_submap->local_pose());
if (num_accumulated_ < options_.scans_per_accumulation()) {
return nullptr;
}

20
cartographer/mapping_3d/sparse_pose_graph.cc
View File

@ -81,8 +81,8 @@ void SparsePoseGraph::GrowSubmapTransformsAsNeeded(
submap_data.at(trajectory_id).at(first_submap_id.submap_index).pose;
optimization_problem_.AddSubmap(
trajectory_id, first_submap_pose *
insertion_submaps[0]->local_pose.inverse() *
insertion_submaps[1]->local_pose);
insertion_submaps[0]->local_pose().inverse() *
insertion_submaps[1]->local_pose());
}
}
@ -113,8 +113,9 @@ void SparsePoseGraph::AddScan(
submap_ids_.emplace(insertion_submaps.back(), submap_id);
submap_data_.at(submap_id).submap = insertion_submaps.back();
}
const Submap* const finished_submap =
insertion_submaps.front()->finished ? insertion_submaps.front() : nullptr;
const Submap* const finished_submap = insertion_submaps.front()->finished()
? insertion_submaps.front()
: nullptr;
// Make sure we have a sampler for this trajectory.
if (!global_localization_samplers_[trajectory_id]) {
@ -235,7 +236,7 @@ void SparsePoseGraph::ComputeConstraintsForScan(
.at(matching_id.trajectory_id)
.at(matching_id.submap_index)
.pose *
matching_submap->local_pose.inverse() * pose;
matching_submap->local_pose().inverse() * pose;
const mapping::NodeId node_id{
matching_id.trajectory_id,
static_cast<size_t>(matching_id.trajectory_id) <
@ -252,7 +253,7 @@ void SparsePoseGraph::ComputeConstraintsForScan(
CHECK(submap_data_.at(submap_id).state == SubmapState::kActive);
submap_data_.at(submap_id).node_ids.emplace(node_id);
const transform::Rigid3d constraint_transform =
submap->local_pose.inverse() * pose;
submap->local_pose().inverse() * pose;
constraints_.push_back(
Constraint{submap_id,
node_id,
@ -432,7 +433,8 @@ transform::Rigid3d SparsePoseGraph::GetLocalToGlobalTransform(
.at(mapping::SubmapId{
trajectory_id,
static_cast<int>(extrapolated_submap_transforms.size()) - 1})
.submap->local_pose.inverse();
.submap->local_pose()
.inverse();
}
std::vector<std::vector<int>> SparsePoseGraph::GetConnectedTrajectories() {
@ -476,8 +478,8 @@ std::vector<transform::Rigid3d> SparsePoseGraph::ExtrapolateSubmapTransforms(
trajectory_id, static_cast<int>(result.size()) - 1};
result.push_back(
result.back() *
submap_data_.at(previous_submap_id).submap->local_pose.inverse() *
submap_data.submap->local_pose);
submap_data_.at(previous_submap_id).submap->local_pose().inverse() *
submap_data.submap->local_pose());
}
}

4
cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc
View File

@ -72,7 +72,7 @@ void ConstraintBuilder::MaybeAddConstraint(
++pending_computations_[current_computation_];
const int current_computation = current_computation_;
ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
submap_id, submap_nodes, &submap->high_resolution_hybrid_grid,
submap_id, submap_nodes, &submap->high_resolution_hybrid_grid(),
[=]() EXCLUDES(mutex_) {
ComputeConstraint(submap_id, submap, node_id,
false, /* match_full_submap */
@ -96,7 +96,7 @@ void ConstraintBuilder::MaybeAddGlobalConstraint(
++pending_computations_[current_computation_];
const int current_computation = current_computation_;
ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
submap_id, submap_nodes, &submap->high_resolution_hybrid_grid,
submap_id, submap_nodes, &submap->high_resolution_hybrid_grid(),
[=]() EXCLUDES(mutex_) {
ComputeConstraint(
submap_id, submap, node_id, true, /* match_full_submap */

26
cartographer/mapping_3d/submaps.cc
View File

@ -224,9 +224,9 @@ std::vector<Eigen::Array4i> ExtractVoxelData(
const HybridGrid& hybrid_grid, const transform::Rigid3f& transform,
Eigen::Array2i* min_index, Eigen::Array2i* max_index) {
std::vector<Eigen::Array4i> voxel_indices_and_probabilities;
const float resolution_inverse = 1. / hybrid_grid.resolution();
const float resolution_inverse = 1.f / hybrid_grid.resolution();
constexpr double kXrayObstructedCellProbabilityLimit = 0.501;
constexpr float kXrayObstructedCellProbabilityLimit = 0.501f;
for (auto it = HybridGrid::Iterator(hybrid_grid); !it.Done(); it.Next()) {
const uint16 probability_value = it.GetValue();
const float probability = mapping::ValueToProbability(probability_value);
@ -323,22 +323,22 @@ proto::SubmapsOptions CreateSubmapsOptions(
Submap::Submap(const float high_resolution, const float low_resolution,
const transform::Rigid3d& local_pose)
: mapping::Submap(local_pose),
high_resolution_hybrid_grid(high_resolution),
low_resolution_hybrid_grid(low_resolution) {}
high_resolution_hybrid_grid_(high_resolution),
low_resolution_hybrid_grid_(low_resolution) {}
void Submap::ToResponseProto(
const transform::Rigid3d& global_submap_pose,
mapping::proto::SubmapQuery::Response* const response) const {
// Generate an X-ray view through the 'hybrid_grid', aligned to the xy-plane
// in the global map frame.
const float resolution = high_resolution_hybrid_grid.resolution();
const float resolution = high_resolution_hybrid_grid_.resolution();
response->set_resolution(resolution);
// Compute a bounding box for the texture.
Eigen::Array2i min_index(INT_MAX, INT_MAX);
Eigen::Array2i max_index(INT_MIN, INT_MIN);
const std::vector<Eigen::Array4i> voxel_indices_and_probabilities =
ExtractVoxelData(high_resolution_hybrid_grid,
ExtractVoxelData(high_resolution_hybrid_grid_,
global_submap_pose.cast<float>(), &min_index,
&max_index);
@ -383,17 +383,17 @@ void Submaps::InsertRangeData(const sensor::RangeData& range_data,
for (const int index : insertion_indices()) {
Submap* submap = submaps_[index].get();
const sensor::RangeData transformed_range_data = sensor::TransformRangeData(
range_data, submap->local_pose.inverse().cast<float>());
range_data, submap->local_pose().inverse().cast<float>());
range_data_inserter_.Insert(
FilterRangeDataByMaxRange(transformed_range_data,
options_.high_resolution_max_range()),
&submap->high_resolution_hybrid_grid);
&submap->high_resolution_hybrid_grid_);
range_data_inserter_.Insert(transformed_range_data,
&submap->low_resolution_hybrid_grid);
++submap->num_range_data;
&submap->low_resolution_hybrid_grid_);
++submap->num_range_data_;
}
const Submap* const last_submap = Get(size() - 1);
if (last_submap->num_range_data == options_.num_range_data()) {
if (last_submap->num_range_data_ == options_.num_range_data()) {
AddSubmap(transform::Rigid3d(range_data.origin.cast<double>(),
gravity_alignment));
}
@ -402,8 +402,8 @@ void Submaps::InsertRangeData(const sensor::RangeData& range_data,
void Submaps::AddSubmap(const transform::Rigid3d& local_pose) {
if (size() > 1) {
Submap* submap = submaps_[size() - 2].get();
CHECK(!submap->finished);
submap->finished = true;
CHECK(!submap->finished_);
submap->finished_ = true;
}
submaps_.emplace_back(new Submap(options_.high_resolution(),
options_.low_resolution(), local_pose));

21
cartographer/mapping_3d/submaps.h
View File

@ -47,17 +47,30 @@ void InsertIntoProbabilityGrid(
proto::SubmapsOptions CreateSubmapsOptions(
common::LuaParameterDictionary* parameter_dictionary);
struct Submap : public mapping::Submap {
class Submap : public mapping::Submap {
public:
Submap(float high_resolution, float low_resolution,
const transform::Rigid3d& local_pose);
HybridGrid high_resolution_hybrid_grid;
HybridGrid low_resolution_hybrid_grid;
bool finished = false;
const HybridGrid& high_resolution_hybrid_grid() const {
return high_resolution_hybrid_grid_;
}
const HybridGrid& low_resolution_hybrid_grid() const {
return low_resolution_hybrid_grid_;
}
const bool finished() const { return finished_; }
void ToResponseProto(
const transform::Rigid3d& global_submap_pose,
mapping::proto::SubmapQuery::Response* response) const override;
private:
// TODO(hrapp): Remove friend declaration.
friend class Submaps;
HybridGrid high_resolution_hybrid_grid_;
HybridGrid low_resolution_hybrid_grid_;
bool finished_ = false;
};
// A container of Submaps.