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 = const Submap* const submap =
trajectory_builders_.at(trajectory_id)->submaps()->Get(submap_index); 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); submap->ToResponseProto(submap_transforms[submap_index], response);
return ""; 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 // 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 // 'finished_probability_grid' to be used for loop closing once the map no
// longer changes. // longer changes.
// TODO(hrapp): This should be a class now. class Submap {
struct Submap { public:
Submap(const transform::Rigid3d& local_pose) : local_pose(local_pose) {} Submap(const transform::Rigid3d& local_pose) : local_pose_(local_pose) {}
virtual ~Submap() {} virtual ~Submap() {}
// Local SLAM pose of this submap. // Local SLAM pose of this submap.
const transform::Rigid3d local_pose; transform::Rigid3d local_pose() const { return local_pose_; }
// Number of RangeData inserted. // 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 // 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
// InsertRangeData() will change the submap. // 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'. // Fills data into the 'response'.
virtual void ToResponseProto( virtual void ToResponseProto(
const transform::Rigid3d& global_submap_pose, const transform::Rigid3d& global_submap_pose,
proto::SubmapQuery::Response* response) const = 0; 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 // 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, const sensor::RangeData& range_data_in_tracking_2d,
transform::Rigid3d* pose_observation) { transform::Rigid3d* pose_observation) {
const ProbabilityGrid& probability_grid = const ProbabilityGrid& probability_grid =
submaps_.Get(submaps_.matching_index())->probability_grid; submaps_.Get(submaps_.matching_index())->probability_grid();
transform::Rigid2d pose_prediction_2d = transform::Rigid2d pose_prediction_2d =
transform::Project2D(pose_prediction * tracking_to_tracking_2d.inverse()); transform::Project2D(pose_prediction * tracking_to_tracking_2d.inverse());
// The online correlative scan matcher will refine the initial estimate for // 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(); submap_data_.at(submap_id).submap = insertion_submaps.back();
} }
const mapping::Submap* const finished_submap = const mapping::Submap* const finished_submap =
insertion_submaps.front()->finished_probability_grid != nullptr insertion_submaps.front()->finished_probability_grid() != nullptr
? insertion_submaps.front() ? insertion_submaps.front()
: nullptr; : nullptr;
@ -430,7 +430,8 @@ transform::Rigid3d SparsePoseGraph::GetLocalToGlobalTransform(
.at(mapping::SubmapId{ .at(mapping::SubmapId{
trajectory_id, trajectory_id,
static_cast<int>(extrapolated_submap_transforms.size()) - 1}) static_cast<int>(extrapolated_submap_transforms.size()) - 1})
.submap->local_pose.inverse(); .submap->local_pose()
.inverse();
} }
std::vector<std::vector<int>> SparsePoseGraph::GetConnectedTrajectories() { 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}; trajectory_id, static_cast<int>(result.size()) - 1};
result.push_back( result.push_back(
result.back() * result.back() *
submap_data_.at(previous_submap_id).submap->local_pose.inverse() * submap_data_.at(previous_submap_id).submap->local_pose().inverse() *
submap_data.submap->local_pose); 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 { namespace sparse_pose_graph {
transform::Rigid2d ComputeSubmapPose(const mapping::Submap& submap) { transform::Rigid2d ComputeSubmapPose(const mapping::Submap& submap) {
return transform::Project2D(submap.local_pose); return transform::Project2D(submap.local_pose());
} }
ConstraintBuilder::ConstraintBuilder( ConstraintBuilder::ConstraintBuilder(
@ -74,7 +74,7 @@ void ConstraintBuilder::MaybeAddConstraint(
++pending_computations_[current_computation_]; ++pending_computations_[current_computation_];
const int current_computation = current_computation_; const int current_computation = current_computation_;
ScheduleSubmapScanMatcherConstructionAndQueueWorkItem( ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
submap_id, submap->finished_probability_grid, [=]() EXCLUDES(mutex_) { submap_id, submap->finished_probability_grid(), [=]() EXCLUDES(mutex_) {
ComputeConstraint(submap_id, submap, node_id, ComputeConstraint(submap_id, submap, node_id,
false, /* match_full_submap */ false, /* match_full_submap */
nullptr, /* trajectory_connectivity */ nullptr, /* trajectory_connectivity */
@ -94,7 +94,7 @@ void ConstraintBuilder::MaybeAddGlobalConstraint(
++pending_computations_[current_computation_]; ++pending_computations_[current_computation_];
const int current_computation = current_computation_; const int current_computation = current_computation_;
ScheduleSubmapScanMatcherConstructionAndQueueWorkItem( ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
submap_id, submap->finished_probability_grid, [=]() EXCLUDES(mutex_) { submap_id, submap->finished_probability_grid(), [=]() EXCLUDES(mutex_) {
ComputeConstraint(submap_id, submap, node_id, ComputeConstraint(submap_id, submap, node_id,
true, /* match_full_submap */ true, /* match_full_submap */
trajectory_connectivity, point_cloud, 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) Submap::Submap(const MapLimits& limits, const Eigen::Vector2f& origin)
: mapping::Submap(transform::Rigid3d::Translation( : mapping::Submap(transform::Rigid3d::Translation(
Eigen::Vector3d(origin.x(), origin.y(), 0.))), Eigen::Vector3d(origin.x(), origin.y(), 0.))),
probability_grid(limits) {} probability_grid_(limits) {}
void Submap::ToResponseProto( void Submap::ToResponseProto(
const transform::Rigid3d&, const transform::Rigid3d&,
mapping::proto::SubmapQuery::Response* const response) const { mapping::proto::SubmapQuery::Response* const response) const {
Eigen::Array2i offset; Eigen::Array2i offset;
CellLimits limits; CellLimits limits;
probability_grid.ComputeCroppedLimits(&offset, &limits); probability_grid_.ComputeCroppedLimits(&offset, &limits);
string cells; string cells;
for (const Eigen::Array2i& xy_index : XYIndexRangeIterator(limits)) { 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 // 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 // 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 // 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. // detect visually for the user, though.
const int delta = const int delta =
128 - mapping::ProbabilityToLogOddsInteger( 128 - mapping::ProbabilityToLogOddsInteger(
probability_grid.GetProbability(xy_index + offset)); probability_grid_.GetProbability(xy_index + offset));
const uint8 alpha = delta > 0 ? 0 : -delta; const uint8 alpha = delta > 0 ? 0 : -delta;
const uint8 value = delta > 0 ? delta : 0; const uint8 value = delta > 0 ? delta : 0;
cells.push_back(value); cells.push_back(value);
@ -138,14 +138,14 @@ void Submap::ToResponseProto(
response->set_width(limits.num_x_cells); response->set_width(limits.num_x_cells);
response->set_height(limits.num_y_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); response->set_resolution(resolution);
const double max_x = const double max_x =
probability_grid.limits().max().x() - resolution * offset.y(); probability_grid_.limits().max().x() - resolution * offset.y();
const double max_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( *response->mutable_slice_pose() = transform::ToProto(
local_pose.inverse() * local_pose().inverse() *
transform::Rigid3d::Translation(Eigen::Vector3d(max_x, max_y, 0.))); 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) { void Submaps::InsertRangeData(const sensor::RangeData& 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->num_range_data; ++submap->num_range_data_;
} }
const Submap* const last_submap = Get(size() - 1); 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>()); 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 // Crop the finished Submap before inserting a new Submap to reduce peak
// memory usage a bit. // memory usage a bit.
Submap* submap = submaps_[index].get(); Submap* submap = submaps_[index].get();
CHECK(submap->finished_probability_grid == nullptr); CHECK(submap->finished_probability_grid_ == nullptr);
submap->probability_grid = submap->probability_grid_ =
ComputeCroppedProbabilityGrid(submap->probability_grid); ComputeCroppedProbabilityGrid(submap->probability_grid_);
submap->finished_probability_grid = &submap->probability_grid; submap->finished_probability_grid_ = &submap->probability_grid_;
if (options_.output_debug_images()) { if (options_.output_debug_images()) {
// Output the Submap that won't be changed from now on. // Output the Submap that won't be changed from now on.
WriteDebugImage("submap" + std::to_string(index) + ".webp", 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( proto::SubmapsOptions CreateSubmapsOptions(
common::LuaParameterDictionary* parameter_dictionary); common::LuaParameterDictionary* parameter_dictionary);
struct Submap : public mapping::Submap { class Submap : public mapping::Submap {
public:
Submap(const MapLimits& limits, const Eigen::Vector2f& origin); Submap(const MapLimits& limits, const Eigen::Vector2f& origin);
ProbabilityGrid probability_grid; const ProbabilityGrid& probability_grid() const { return probability_grid_; }
void ToResponseProto( void ToResponseProto(
const transform::Rigid3d& global_submap_pose, const transform::Rigid3d& global_submap_pose,
mapping::proto::SubmapQuery::Response* response) const override; mapping::proto::SubmapQuery::Response* response) const override;
private:
// TODO(hrapp): Remove friend declaration.
friend class Submaps;
ProbabilityGrid probability_grid_;
}; };
// A container of Submaps. // 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(); 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, submaps.Get(matching)->num_range_data); 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, 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 = const Submap* const matching_submap =
submaps_->Get(submaps_->matching_index()); submaps_->Get(submaps_->matching_index());
transform::Rigid3d initial_ceres_pose = transform::Rigid3d initial_ceres_pose =
matching_submap->local_pose.inverse() * pose_prediction; matching_submap->local_pose().inverse() * pose_prediction;
sensor::AdaptiveVoxelFilter adaptive_voxel_filter( sensor::AdaptiveVoxelFilter adaptive_voxel_filter(
options_.high_resolution_adaptive_voxel_filter_options()); options_.high_resolution_adaptive_voxel_filter_options());
const sensor::PointCloud filtered_point_cloud_in_tracking = const sensor::PointCloud filtered_point_cloud_in_tracking =
@ -154,7 +154,7 @@ KalmanLocalTrajectoryBuilder::AddAccumulatedRangeData(
const transform::Rigid3d initial_pose = initial_ceres_pose; const transform::Rigid3d initial_pose = initial_ceres_pose;
real_time_correlative_scan_matcher_->Match( real_time_correlative_scan_matcher_->Match(
initial_pose, filtered_point_cloud_in_tracking, 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; transform::Rigid3d pose_observation_in_submap;
@ -166,12 +166,12 @@ KalmanLocalTrajectoryBuilder::AddAccumulatedRangeData(
low_resolution_adaptive_voxel_filter.Filter(filtered_range_data.returns); low_resolution_adaptive_voxel_filter.Filter(filtered_range_data.returns);
ceres_scan_matcher_->Match(scan_matcher_pose_estimate_, initial_ceres_pose, ceres_scan_matcher_->Match(scan_matcher_pose_estimate_, initial_ceres_pose,
{{&filtered_point_cloud_in_tracking, {{&filtered_point_cloud_in_tracking,
&matching_submap->high_resolution_hybrid_grid}, &matching_submap->high_resolution_hybrid_grid()},
{&low_resolution_point_cloud_in_tracking, {&low_resolution_point_cloud_in_tracking,
&matching_submap->low_resolution_hybrid_grid}}, &matching_submap->low_resolution_hybrid_grid()}},
&pose_observation_in_submap, &summary); &pose_observation_in_submap, &summary);
const transform::Rigid3d pose_observation = const transform::Rigid3d pose_observation =
matching_submap->local_pose * pose_observation_in_submap; matching_submap->local_pose() * pose_observation_in_submap;
pose_tracker_->AddPoseObservation( pose_tracker_->AddPoseObservation(
time, pose_observation, time, pose_observation,
options_.kalman_local_trajectory_builder_options() 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 // We transform the states in 'batches_' in place to be in the submap frame as
// expected by the OccupiedSpaceCostFunctor. This is reverted after solving // expected by the OccupiedSpaceCostFunctor. This is reverted after solving
// the optimization problem. // the optimization problem.
TransformStates(matching_submap->local_pose.inverse()); TransformStates(matching_submap->local_pose().inverse());
for (size_t i = 0; i < batches_.size(); ++i) { for (size_t i = 0; i < batches_.size(); ++i) {
Batch& batch = batches_[i]; Batch& batch = batches_[i];
problem.AddResidualBlock( problem.AddResidualBlock(
@ -253,7 +253,7 @@ OptimizingLocalTrajectoryBuilder::MaybeOptimize(const common::Time time) {
std::sqrt(static_cast<double>( std::sqrt(static_cast<double>(
batch.high_resolution_filtered_points.size())), batch.high_resolution_filtered_points.size())),
batch.high_resolution_filtered_points, batch.high_resolution_filtered_points,
matching_submap->high_resolution_hybrid_grid), matching_submap->high_resolution_hybrid_grid()),
batch.high_resolution_filtered_points.size()), batch.high_resolution_filtered_points.size()),
nullptr, batch.state.translation.data(), batch.state.rotation.data()); nullptr, batch.state.translation.data(), batch.state.rotation.data());
problem.AddResidualBlock( problem.AddResidualBlock(
@ -265,7 +265,7 @@ OptimizingLocalTrajectoryBuilder::MaybeOptimize(const common::Time time) {
std::sqrt(static_cast<double>( std::sqrt(static_cast<double>(
batch.low_resolution_filtered_points.size())), batch.low_resolution_filtered_points.size())),
batch.low_resolution_filtered_points, batch.low_resolution_filtered_points,
matching_submap->low_resolution_hybrid_grid), matching_submap->low_resolution_hybrid_grid()),
batch.low_resolution_filtered_points.size()), batch.low_resolution_filtered_points.size()),
nullptr, batch.state.translation.data(), batch.state.rotation.data()); 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); ceres::Solve(ceres_solver_options_, &problem, &summary);
// The optimized states in 'batches_' are in the submap frame and we transform // The optimized states in 'batches_' are in the submap frame and we transform
// them in place to be in the local SLAM frame again. // 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()) { if (num_accumulated_ < options_.scans_per_accumulation()) {
return nullptr; 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; submap_data.at(trajectory_id).at(first_submap_id.submap_index).pose;
optimization_problem_.AddSubmap( optimization_problem_.AddSubmap(
trajectory_id, first_submap_pose * trajectory_id, first_submap_pose *
insertion_submaps[0]->local_pose.inverse() * insertion_submaps[0]->local_pose().inverse() *
insertion_submaps[1]->local_pose); insertion_submaps[1]->local_pose());
} }
} }
@ -113,8 +113,9 @@ void SparsePoseGraph::AddScan(
submap_ids_.emplace(insertion_submaps.back(), submap_id); submap_ids_.emplace(insertion_submaps.back(), submap_id);
submap_data_.at(submap_id).submap = insertion_submaps.back(); submap_data_.at(submap_id).submap = insertion_submaps.back();
} }
const Submap* const finished_submap = const Submap* const finished_submap = insertion_submaps.front()->finished()
insertion_submaps.front()->finished ? insertion_submaps.front() : nullptr; ? insertion_submaps.front()
: nullptr;
// Make sure we have a sampler for this trajectory. // Make sure we have a sampler for this trajectory.
if (!global_localization_samplers_[trajectory_id]) { if (!global_localization_samplers_[trajectory_id]) {
@ -235,7 +236,7 @@ void SparsePoseGraph::ComputeConstraintsForScan(
.at(matching_id.trajectory_id) .at(matching_id.trajectory_id)
.at(matching_id.submap_index) .at(matching_id.submap_index)
.pose * .pose *
matching_submap->local_pose.inverse() * pose; matching_submap->local_pose().inverse() * pose;
const mapping::NodeId node_id{ const mapping::NodeId node_id{
matching_id.trajectory_id, matching_id.trajectory_id,
static_cast<size_t>(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); CHECK(submap_data_.at(submap_id).state == SubmapState::kActive);
submap_data_.at(submap_id).node_ids.emplace(node_id); submap_data_.at(submap_id).node_ids.emplace(node_id);
const transform::Rigid3d constraint_transform = const transform::Rigid3d constraint_transform =
submap->local_pose.inverse() * pose; submap->local_pose().inverse() * pose;
constraints_.push_back( constraints_.push_back(
Constraint{submap_id, Constraint{submap_id,
node_id, node_id,
@ -432,7 +433,8 @@ transform::Rigid3d SparsePoseGraph::GetLocalToGlobalTransform(
.at(mapping::SubmapId{ .at(mapping::SubmapId{
trajectory_id, trajectory_id,
static_cast<int>(extrapolated_submap_transforms.size()) - 1}) static_cast<int>(extrapolated_submap_transforms.size()) - 1})
.submap->local_pose.inverse(); .submap->local_pose()
.inverse();
} }
std::vector<std::vector<int>> SparsePoseGraph::GetConnectedTrajectories() { 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}; trajectory_id, static_cast<int>(result.size()) - 1};
result.push_back( result.push_back(
result.back() * result.back() *
submap_data_.at(previous_submap_id).submap->local_pose.inverse() * submap_data_.at(previous_submap_id).submap->local_pose().inverse() *
submap_data.submap->local_pose); 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_]; ++pending_computations_[current_computation_];
const int current_computation = current_computation_; const int current_computation = current_computation_;
ScheduleSubmapScanMatcherConstructionAndQueueWorkItem( ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
submap_id, submap_nodes, &submap->high_resolution_hybrid_grid, submap_id, submap_nodes, &submap->high_resolution_hybrid_grid(),
[=]() EXCLUDES(mutex_) { [=]() EXCLUDES(mutex_) {
ComputeConstraint(submap_id, submap, node_id, ComputeConstraint(submap_id, submap, node_id,
false, /* match_full_submap */ false, /* match_full_submap */
@ -96,7 +96,7 @@ void ConstraintBuilder::MaybeAddGlobalConstraint(
++pending_computations_[current_computation_]; ++pending_computations_[current_computation_];
const int current_computation = current_computation_; const int current_computation = current_computation_;
ScheduleSubmapScanMatcherConstructionAndQueueWorkItem( ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
submap_id, submap_nodes, &submap->high_resolution_hybrid_grid, submap_id, submap_nodes, &submap->high_resolution_hybrid_grid(),
[=]() EXCLUDES(mutex_) { [=]() EXCLUDES(mutex_) {
ComputeConstraint( ComputeConstraint(
submap_id, submap, node_id, true, /* match_full_submap */ 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, const HybridGrid& hybrid_grid, const transform::Rigid3f& transform,
Eigen::Array2i* min_index, Eigen::Array2i* max_index) { Eigen::Array2i* min_index, Eigen::Array2i* max_index) {
std::vector<Eigen::Array4i> voxel_indices_and_probabilities; 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()) { for (auto it = HybridGrid::Iterator(hybrid_grid); !it.Done(); it.Next()) {
const uint16 probability_value = it.GetValue(); const uint16 probability_value = it.GetValue();
const float probability = mapping::ValueToProbability(probability_value); const float probability = mapping::ValueToProbability(probability_value);
@ -323,22 +323,22 @@ proto::SubmapsOptions CreateSubmapsOptions(
Submap::Submap(const float high_resolution, const float low_resolution, Submap::Submap(const float high_resolution, const float low_resolution,
const transform::Rigid3d& local_pose) const transform::Rigid3d& local_pose)
: mapping::Submap(local_pose), : mapping::Submap(local_pose),
high_resolution_hybrid_grid(high_resolution), high_resolution_hybrid_grid_(high_resolution),
low_resolution_hybrid_grid(low_resolution) {} low_resolution_hybrid_grid_(low_resolution) {}
void Submap::ToResponseProto( void Submap::ToResponseProto(
const transform::Rigid3d& global_submap_pose, const transform::Rigid3d& global_submap_pose,
mapping::proto::SubmapQuery::Response* const response) const { mapping::proto::SubmapQuery::Response* const response) const {
// Generate an X-ray view through the 'hybrid_grid', aligned to the xy-plane // Generate an X-ray view through the 'hybrid_grid', aligned to the xy-plane
// in the global map frame. // 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); response->set_resolution(resolution);
// Compute a bounding box for the texture. // Compute a bounding box for the texture.
Eigen::Array2i min_index(INT_MAX, INT_MAX); Eigen::Array2i min_index(INT_MAX, INT_MAX);
Eigen::Array2i max_index(INT_MIN, INT_MIN); Eigen::Array2i max_index(INT_MIN, INT_MIN);
const std::vector<Eigen::Array4i> voxel_indices_and_probabilities = 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, global_submap_pose.cast<float>(), &min_index,
&max_index); &max_index);
@ -383,17 +383,17 @@ void Submaps::InsertRangeData(const sensor::RangeData& range_data,
for (const int index : insertion_indices()) { for (const int index : insertion_indices()) {
Submap* submap = submaps_[index].get(); Submap* submap = submaps_[index].get();
const sensor::RangeData transformed_range_data = sensor::TransformRangeData( 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( range_data_inserter_.Insert(
FilterRangeDataByMaxRange(transformed_range_data, FilterRangeDataByMaxRange(transformed_range_data,
options_.high_resolution_max_range()), options_.high_resolution_max_range()),
&submap->high_resolution_hybrid_grid); &submap->high_resolution_hybrid_grid_);
range_data_inserter_.Insert(transformed_range_data, range_data_inserter_.Insert(transformed_range_data,
&submap->low_resolution_hybrid_grid); &submap->low_resolution_hybrid_grid_);
++submap->num_range_data; ++submap->num_range_data_;
} }
const Submap* const last_submap = Get(size() - 1); 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>(), AddSubmap(transform::Rigid3d(range_data.origin.cast<double>(),
gravity_alignment)); gravity_alignment));
} }
@ -402,8 +402,8 @@ void Submaps::InsertRangeData(const sensor::RangeData& range_data,
void Submaps::AddSubmap(const transform::Rigid3d& local_pose) { void Submaps::AddSubmap(const transform::Rigid3d& local_pose) {
if (size() > 1) { if (size() > 1) {
Submap* submap = submaps_[size() - 2].get(); Submap* submap = submaps_[size() - 2].get();
CHECK(!submap->finished); CHECK(!submap->finished_);
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(), local_pose)); options_.low_resolution(), local_pose));

21
cartographer/mapping_3d/submaps.h
View File

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