diff --git a/cartographer/mapping/map_builder.cc b/cartographer/mapping/map_builder.cc index 1bbe606..0079bc3 100644 --- a/cartographer/mapping/map_builder.cc +++ b/cartographer/mapping/map_builder.cc @@ -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 ""; } diff --git a/cartographer/mapping/submaps.h b/cartographer/mapping/submaps.h index b207ccb..20794d5 100644 --- a/cartographer/mapping/submaps.h +++ b/cartographer/mapping/submaps.h @@ -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 diff --git a/cartographer/mapping_2d/local_trajectory_builder.cc b/cartographer/mapping_2d/local_trajectory_builder.cc index d953249..b76715b 100644 --- a/cartographer/mapping_2d/local_trajectory_builder.cc +++ b/cartographer/mapping_2d/local_trajectory_builder.cc @@ -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 diff --git a/cartographer/mapping_2d/sparse_pose_graph.cc b/cartographer/mapping_2d/sparse_pose_graph.cc index c87a43c..cc3c884 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.cc +++ b/cartographer/mapping_2d/sparse_pose_graph.cc @@ -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(extrapolated_submap_transforms.size()) - 1}) - .submap->local_pose.inverse(); + .submap->local_pose() + .inverse(); } std::vector> SparsePoseGraph::GetConnectedTrajectories() { @@ -474,8 +475,8 @@ std::vector SparsePoseGraph::ExtrapolateSubmapTransforms( trajectory_id, static_cast(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()); } } diff --git a/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc b/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc index 51f7f32..8e40572 100644 --- a/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc +++ b/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc @@ -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, diff --git a/cartographer/mapping_2d/submaps.cc b/cartographer/mapping_2d/submaps.cc index 1e4b831..bad9b09 100644 --- a/cartographer/mapping_2d/submaps.cc +++ b/cartographer/mapping_2d/submaps.cc @@ -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_); } } diff --git a/cartographer/mapping_2d/submaps.h b/cartographer/mapping_2d/submaps.h index 316218d..00ed6fc 100644 --- a/cartographer/mapping_2d/submaps.h +++ b/cartographer/mapping_2d/submaps.h @@ -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. diff --git a/cartographer/mapping_2d/submaps_test.cc b/cartographer/mapping_2d/submaps_test.cc index d56c87b..f70638f 100644 --- a/cartographer/mapping_2d/submaps_test.cc +++ b/cartographer/mapping_2d/submaps_test.cc @@ -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()); } } diff --git a/cartographer/mapping_3d/kalman_local_trajectory_builder.cc b/cartographer/mapping_3d/kalman_local_trajectory_builder.cc index 5a5b239..01c72e5 100644 --- a/cartographer/mapping_3d/kalman_local_trajectory_builder.cc +++ b/cartographer/mapping_3d/kalman_local_trajectory_builder.cc @@ -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() diff --git a/cartographer/mapping_3d/optimizing_local_trajectory_builder.cc b/cartographer/mapping_3d/optimizing_local_trajectory_builder.cc index 9e5ab2e..4bdfbcf 100644 --- a/cartographer/mapping_3d/optimizing_local_trajectory_builder.cc +++ b/cartographer/mapping_3d/optimizing_local_trajectory_builder.cc @@ -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( 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( 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; } diff --git a/cartographer/mapping_3d/sparse_pose_graph.cc b/cartographer/mapping_3d/sparse_pose_graph.cc index 717c4cc..1b73845 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.cc +++ b/cartographer/mapping_3d/sparse_pose_graph.cc @@ -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(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(extrapolated_submap_transforms.size()) - 1}) - .submap->local_pose.inverse(); + .submap->local_pose() + .inverse(); } std::vector> SparsePoseGraph::GetConnectedTrajectories() { @@ -476,8 +478,8 @@ std::vector SparsePoseGraph::ExtrapolateSubmapTransforms( trajectory_id, static_cast(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()); } } diff --git a/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc b/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc index 0ce6050..33b9506 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc +++ b/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc @@ -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 */ diff --git a/cartographer/mapping_3d/submaps.cc b/cartographer/mapping_3d/submaps.cc index cea5811..835919b 100644 --- a/cartographer/mapping_3d/submaps.cc +++ b/cartographer/mapping_3d/submaps.cc @@ -224,9 +224,9 @@ std::vector ExtractVoxelData( const HybridGrid& hybrid_grid, const transform::Rigid3f& transform, Eigen::Array2i* min_index, Eigen::Array2i* max_index) { std::vector 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 voxel_indices_and_probabilities = - ExtractVoxelData(high_resolution_hybrid_grid, + ExtractVoxelData(high_resolution_hybrid_grid_, global_submap_pose.cast(), &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()); + range_data, submap->local_pose().inverse().cast()); 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(), 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)); diff --git a/cartographer/mapping_3d/submaps.h b/cartographer/mapping_3d/submaps.h index 3d4a036..57c330c 100644 --- a/cartographer/mapping_3d/submaps.h +++ b/cartographer/mapping_3d/submaps.h @@ -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.