From a4c0e4754e99e461e2497b05f234839b181fc3cc Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Tue, 14 Nov 2017 16:53:31 +0100 Subject: [PATCH] Rename scan to node. (#667) Changes the naming from "scan" to "node" in the pose graph. AddNode() adds a new node to the graph which might contain data from multiple range sensors and not necessarily one scan. Configuration and documentation changes might follow in a separate PR. Related to #280. --- .../mapping/global_trajectory_builder.h | 2 +- cartographer/mapping/sparse_pose_graph.h | 6 +-- cartographer/mapping/trajectory_builder.h | 1 - cartographer/mapping_2d/ray_casting.cc | 2 +- cartographer/mapping_2d/sparse_pose_graph.cc | 46 +++++++++---------- cartographer/mapping_2d/sparse_pose_graph.h | 34 +++++++------- .../sparse_pose_graph/constraint_builder.cc | 12 ++--- .../sparse_pose_graph/constraint_builder.h | 14 +++--- .../sparse_pose_graph/optimization_problem.cc | 2 +- .../sparse_pose_graph/spa_cost_function.h | 4 +- .../mapping_2d/sparse_pose_graph_test.cc | 8 ++-- cartographer/mapping_2d/submaps.h | 8 ++-- cartographer/mapping_2d/submaps_test.cc | 13 +++--- cartographer/mapping_3d/motion_filter.cc | 2 +- cartographer/mapping_3d/sparse_pose_graph.cc | 46 +++++++++---------- cartographer/mapping_3d/sparse_pose_graph.h | 36 +++++++-------- .../sparse_pose_graph/constraint_builder.cc | 10 ++-- .../sparse_pose_graph/constraint_builder.h | 14 +++--- .../sparse_pose_graph/optimization_problem.cc | 2 +- .../sparse_pose_graph/spa_cost_function.h | 4 +- cartographer/mapping_3d/submaps.h | 8 ++-- 21 files changed, 137 insertions(+), 137 deletions(-) diff --git a/cartographer/mapping/global_trajectory_builder.h b/cartographer/mapping/global_trajectory_builder.h index f09f3b4..7031d15 100644 --- a/cartographer/mapping/global_trajectory_builder.h +++ b/cartographer/mapping/global_trajectory_builder.h @@ -57,7 +57,7 @@ class GlobalTrajectoryBuilder std::unique_ptr node_id; if (matching_result->insertion_result != nullptr) { node_id = ::cartographer::common::make_unique( - sparse_pose_graph_->AddScan( + sparse_pose_graph_->AddNode( matching_result->insertion_result->constant_data, trajectory_id_, matching_result->insertion_result->insertion_submaps)); CHECK_EQ(node_id->trajectory_id, trajectory_id_); diff --git a/cartographer/mapping/sparse_pose_graph.h b/cartographer/mapping/sparse_pose_graph.h index 274b72b..8b47bc6 100644 --- a/cartographer/mapping/sparse_pose_graph.h +++ b/cartographer/mapping/sparse_pose_graph.h @@ -57,11 +57,11 @@ class SparsePoseGraph { SubmapId submap_id; // 'i' in the paper. NodeId node_id; // 'j' in the paper. - // Pose of the scan 'j' relative to submap 'i'. + // Pose of the node 'j' relative to submap 'i'. Pose pose; - // Differentiates between intra-submap (where scan 'j' was inserted into - // submap 'i') and inter-submap constraints (where scan 'j' was not inserted + // Differentiates between intra-submap (where node 'j' was inserted into + // submap 'i') and inter-submap constraints (where node 'j' was not inserted // into submap 'i'). enum Tag { INTRA_SUBMAP, INTER_SUBMAP } tag; }; diff --git a/cartographer/mapping/trajectory_builder.h b/cartographer/mapping/trajectory_builder.h index 79f0155..0d7cb14 100644 --- a/cartographer/mapping/trajectory_builder.h +++ b/cartographer/mapping/trajectory_builder.h @@ -30,7 +30,6 @@ #include "cartographer/mapping/submaps.h" #include "cartographer/sensor/data.h" #include "cartographer/sensor/point_cloud.h" -#include "cartographer/sensor/range_data.h" namespace cartographer { namespace mapping { diff --git a/cartographer/mapping_2d/ray_casting.cc b/cartographer/mapping_2d/ray_casting.cc index 7a82929..b81a591 100644 --- a/cartographer/mapping_2d/ray_casting.cc +++ b/cartographer/mapping_2d/ray_casting.cc @@ -196,7 +196,7 @@ void CastRays(const sensor::RangeData& range_data, CastRay(begin, end, miss_table, probability_grid); } - // Finally, compute and add empty rays based on misses in the scan. + // Finally, compute and add empty rays based on misses in the range data. for (const Eigen::Vector3f& missing_echo : range_data.misses) { CastRay(begin, superscaled_limits.GetCellIndex(missing_echo.head<2>()), miss_table, probability_grid); diff --git a/cartographer/mapping_2d/sparse_pose_graph.cc b/cartographer/mapping_2d/sparse_pose_graph.cc index ea5f213..36de8f9 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.cc +++ b/cartographer/mapping_2d/sparse_pose_graph.cc @@ -100,7 +100,7 @@ std::vector SparsePoseGraph::InitializeGlobalSubmapPoses( return {front_submap_id, last_submap_id}; } -mapping::NodeId SparsePoseGraph::AddScan( +mapping::NodeId SparsePoseGraph::AddNode( std::shared_ptr constant_data, const int trajectory_id, const std::vector>& insertion_submaps) { @@ -128,7 +128,7 @@ mapping::NodeId SparsePoseGraph::AddScan( // execute the lambda. const bool newly_finished_submap = insertion_submaps.front()->finished(); AddWorkItem([=]() REQUIRES(mutex_) { - ComputeConstraintsForScan(node_id, insertion_submaps, + ComputeConstraintsForNode(node_id, insertion_submaps, newly_finished_submap); }); return node_id; @@ -178,17 +178,17 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id, const mapping::SubmapId& submap_id) { CHECK(submap_data_.at(submap_id).state == SubmapState::kFinished); - const common::Time scan_time = GetLatestScanTime(node_id, submap_id); + const common::Time node_time = GetLatestNodeTime(node_id, submap_id); const common::Time last_connection_time = trajectory_connectivity_state_.LastConnectionTime( node_id.trajectory_id, submap_id.trajectory_id); if (node_id.trajectory_id == submap_id.trajectory_id || - scan_time < + node_time < last_connection_time + common::FromSeconds( options_.global_constraint_search_after_n_seconds())) { - // If the scan and the submap belong to the same trajectory or if there has - // been a recent global constraint that ties that scan's trajectory to the + // If the node and the submap belong to the same trajectory or if there has + // been a recent global constraint that ties that node's trajectory to the // submap's trajectory, it suffices to do a match constrained to a local // search window. const transform::Rigid2d initial_relative_pose = @@ -207,7 +207,7 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id, } } -void SparsePoseGraph::ComputeConstraintsForOldScans( +void SparsePoseGraph::ComputeConstraintsForOldNodes( const mapping::SubmapId& submap_id) { const auto& submap_data = submap_data_.at(submap_id); for (const auto& node_id_data : optimization_problem_.node_data()) { @@ -218,7 +218,7 @@ void SparsePoseGraph::ComputeConstraintsForOldScans( } } -void SparsePoseGraph::ComputeConstraintsForScan( +void SparsePoseGraph::ComputeConstraintsForNode( const mapping::NodeId& node_id, std::vector> insertion_submaps, const bool newly_finished_submap) { @@ -240,7 +240,7 @@ void SparsePoseGraph::ComputeConstraintsForScan( constant_data->gravity_alignment); for (size_t i = 0; i < insertion_submaps.size(); ++i) { const mapping::SubmapId submap_id = submap_ids[i]; - // Even if this was the last scan added to 'submap_id', the submap will only + // Even if this was the last node added to 'submap_id', the submap will only // be marked as finished in 'submap_data_' further below. CHECK(submap_data_.at(submap_id).state == SubmapState::kActive); submap_data_.at(submap_id).node_ids.emplace(node_id); @@ -268,13 +268,13 @@ void SparsePoseGraph::ComputeConstraintsForScan( CHECK(finished_submap_data.state == SubmapState::kActive); finished_submap_data.state = SubmapState::kFinished; // We have a new completed submap, so we look into adding constraints for - // old scans. - ComputeConstraintsForOldScans(finished_submap_id); + // old nodes. + ComputeConstraintsForOldNodes(finished_submap_id); } - constraint_builder_.NotifyEndOfScan(); - ++num_scans_since_last_loop_closure_; + constraint_builder_.NotifyEndOfNode(); + ++num_nodes_since_last_loop_closure_; if (options_.optimize_every_n_scans() > 0 && - num_scans_since_last_loop_closure_ > options_.optimize_every_n_scans()) { + num_nodes_since_last_loop_closure_ > options_.optimize_every_n_scans()) { CHECK(!run_loop_closure_); run_loop_closure_ = true; // If there is a 'work_queue_' already, some other thread will take care. @@ -285,7 +285,7 @@ void SparsePoseGraph::ComputeConstraintsForScan( } } -common::Time SparsePoseGraph::GetLatestScanTime( +common::Time SparsePoseGraph::GetLatestNodeTime( const mapping::NodeId& node_id, const mapping::SubmapId& submap_id) const { common::Time time = trajectory_nodes_.at(node_id).constant_data->time; const SubmapData& submap_data = submap_data_.at(submap_id); @@ -302,7 +302,7 @@ void SparsePoseGraph::UpdateTrajectoryConnectivity( const Constraint& constraint) { CHECK_EQ(constraint.tag, mapping::SparsePoseGraph::Constraint::INTER_SUBMAP); const common::Time time = - GetLatestScanTime(constraint.node_id, constraint.submap_id); + GetLatestNodeTime(constraint.node_id, constraint.submap_id); trajectory_connectivity_state_.Connect(constraint.node_id.trajectory_id, constraint.submap_id.trajectory_id, time); @@ -326,7 +326,7 @@ void SparsePoseGraph::HandleWorkQueue() { trimmer->Trim(&trimming_handle); } - num_scans_since_last_loop_closure_ = 0; + num_nodes_since_last_loop_closure_ = 0; run_loop_closure_ = false; while (!run_loop_closure_) { if (work_queue_->empty()) { @@ -345,20 +345,20 @@ void SparsePoseGraph::HandleWorkQueue() { void SparsePoseGraph::WaitForAllComputations() { bool notification = false; common::MutexLocker locker(&mutex_); - const int num_finished_scans_at_start = - constraint_builder_.GetNumFinishedScans(); + const int num_finished_nodes_at_start = + constraint_builder_.GetNumFinishedNodes(); while (!locker.AwaitWithTimeout( [this]() REQUIRES(mutex_) { - return constraint_builder_.GetNumFinishedScans() == + return constraint_builder_.GetNumFinishedNodes() == num_trajectory_nodes_; }, common::FromSeconds(1.))) { std::ostringstream progress_info; progress_info << "Optimizing: " << std::fixed << std::setprecision(1) << 100. * - (constraint_builder_.GetNumFinishedScans() - - num_finished_scans_at_start) / - (num_trajectory_nodes_ - num_finished_scans_at_start) + (constraint_builder_.GetNumFinishedNodes() - + num_finished_nodes_at_start) / + (num_trajectory_nodes_ - num_finished_nodes_at_start) << "%..."; std::cout << "\r\x1b[K" << progress_info.str() << std::flush; } diff --git a/cartographer/mapping_2d/sparse_pose_graph.h b/cartographer/mapping_2d/sparse_pose_graph.h index f3ee710..98f84c7 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.h +++ b/cartographer/mapping_2d/sparse_pose_graph.h @@ -53,9 +53,9 @@ namespace mapping_2d { // on (pp. 22--29). IEEE, 2010. // // It is extended for submapping: -// Each scan has been matched against one or more submaps (adding a constraint -// for each match), both poses of scans and of submaps are to be optimized. -// All constraints are between a submap i and a scan j. +// Each node has been matched against one or more submaps (adding a constraint +// for each match), both poses of nodes and of submaps are to be optimized. +// All constraints are between a submap i and a node j. class SparsePoseGraph : public mapping::SparsePoseGraph { public: SparsePoseGraph(const mapping::proto::SparsePoseGraphOptions& options, @@ -67,10 +67,10 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { // Adds a new node with 'constant_data'. Its 'constant_data->local_pose' was // determined by scan matching against 'insertion_submaps.front()' and the - // scan was inserted into the 'insertion_submaps'. If + // node data was inserted into the 'insertion_submaps'. If // 'insertion_submaps.front().finished()' is 'true', data was inserted into // this submap for the last time. - mapping::NodeId AddScan( + mapping::NodeId AddNode( std::shared_ptr constant_data, int trajectory_id, const std::vector>& insertion_submaps) @@ -119,13 +119,13 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { private: // The current state of the submap in the background threads. When this - // transitions to kFinished, all scans are tried to match against this submap. - // Likewise, all new scans are matched against submaps which are finished. + // transitions to kFinished, all nodes are tried to match against this submap. + // Likewise, all new nodes are matched against submaps which are finished. enum class SubmapState { kActive, kFinished }; struct SubmapData { std::shared_ptr submap; - // IDs of the scans that were inserted into this map together with + // IDs of the nodes that were inserted into this map together with // constraints for them. They are not to be matched again when this submap // becomes 'finished'. std::set node_ids; @@ -146,18 +146,18 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { const std::vector>& insertion_submaps) REQUIRES(mutex_); - // Adds constraints for a scan, and starts scan matching in the background. - void ComputeConstraintsForScan( + // Adds constraints for a node, and starts scan matching in the background. + void ComputeConstraintsForNode( const mapping::NodeId& node_id, std::vector> insertion_submaps, bool newly_finished_submap) REQUIRES(mutex_); - // Computes constraints for a scan and submap pair. + // Computes constraints for a node and submap pair. void ComputeConstraint(const mapping::NodeId& node_id, const mapping::SubmapId& submap_id) REQUIRES(mutex_); - // Adds constraints for older scans whenever a new submap is finished. - void ComputeConstraintsForOldScans(const mapping::SubmapId& submap_id) + // Adds constraints for older nodes whenever a new submap is finished. + void ComputeConstraintsForOldNodes(const mapping::SubmapId& submap_id) REQUIRES(mutex_); // Registers the callback to run the optimization once all constraints have @@ -182,7 +182,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { mapping::SparsePoseGraph::SubmapData GetSubmapDataUnderLock( const mapping::SubmapId& submap_id) REQUIRES(mutex_); - common::Time GetLatestScanTime(const mapping::NodeId& node_id, + common::Time GetLatestNodeTime(const mapping::NodeId& node_id, const mapping::SubmapId& submap_id) const REQUIRES(mutex_); @@ -201,12 +201,12 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { // How our various trajectories are related. mapping::TrajectoryConnectivityState trajectory_connectivity_state_; - // We globally localize a fraction of the scans from each trajectory. + // We globally localize a fraction of the nodes from each trajectory. std::unordered_map> global_localization_samplers_ GUARDED_BY(mutex_); - // Number of scans added since last loop closure. - int num_scans_since_last_loop_closure_ GUARDED_BY(mutex_) = 0; + // Number of nodes added since last loop closure. + int num_nodes_since_last_loop_closure_ GUARDED_BY(mutex_) = 0; // Whether the optimization has to be run before more data is added. bool run_loop_closure_ GUARDED_BY(mutex_) = false; diff --git a/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc b/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc index 90b7034..ffd780d 100644 --- a/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc +++ b/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc @@ -101,7 +101,7 @@ void ConstraintBuilder::MaybeAddGlobalConstraint( }); } -void ConstraintBuilder::NotifyEndOfScan() { +void ConstraintBuilder::NotifyEndOfNode() { common::MutexLocker locker(&mutex_); ++current_computation_; } @@ -167,10 +167,10 @@ void ConstraintBuilder::ComputeConstraint( const SubmapScanMatcher* const submap_scan_matcher = GetSubmapScanMatcher(submap_id); - // The 'constraint_transform' (submap i <- scan j) is computed from: - // - a 'filtered_gravity_aligned_point_cloud' in scan j, - // - the initial guess 'initial_pose' for (map <- scan j), - // - the result 'pose_estimate' of Match() (map <- scan j). + // The 'constraint_transform' (submap i <- node j) is computed from: + // - a 'filtered_gravity_aligned_point_cloud' in node j, + // - the initial guess 'initial_pose' for (map <- node j), + // - the result 'pose_estimate' of Match() (map <- node j). // - the ComputeSubmapPose() (map <- submap i) float score = 0.; transform::Rigid2d pose_estimate = transform::Rigid2d::Identity(); @@ -273,7 +273,7 @@ void ConstraintBuilder::FinishComputation(const int computation_index) { } } -int ConstraintBuilder::GetNumFinishedScans() { +int ConstraintBuilder::GetNumFinishedNodes() { common::MutexLocker locker(&mutex_); if (pending_computations_.empty()) { return current_computation_; diff --git a/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.h b/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.h index 1505929..9dcf3fa 100644 --- a/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.h +++ b/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.h @@ -94,14 +94,14 @@ class ConstraintBuilder { const mapping::TrajectoryNode::Data* const constant_data); // Must be called after all computations related to one node have been added. - void NotifyEndOfScan(); + void NotifyEndOfNode(); // Registers the 'callback' to be called with the results, after all // computations triggered by MaybeAddConstraint() have finished. void WhenDone(const std::function& callback); - // Returns the number of consecutive finished scans. - int GetNumFinishedScans(); + // Returns the number of consecutive finished nodes. + int GetNumFinishedNodes(); // Delete data related to 'submap_id'. void DeleteScanMatcher(const mapping::SubmapId& submap_id); @@ -150,12 +150,12 @@ class ConstraintBuilder { std::unique_ptr> when_done_ GUARDED_BY(mutex_); - // Index of the scan in reaction to which computations are currently - // added. This is always the highest scan index seen so far, even when older - // scans are matched against a new submap. + // Index of the node in reaction to which computations are currently + // added. This is always the highest node index seen so far, even when older + // nodes are matched against a new submap. int current_computation_ GUARDED_BY(mutex_) = 0; - // For each added scan, maps to the number of pending computations that were + // For each added node, maps to the number of pending computations that were // added for it. std::map pending_computations_ GUARDED_BY(mutex_); diff --git a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc index 5faf24d..a1d76d0 100644 --- a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc +++ b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc @@ -163,7 +163,7 @@ void OptimizationProblem::Solve(const std::vector& constraints, C_nodes.at(constraint.node_id).data()); } - // Add penalties for violating odometry or changes between consecutive scans + // Add penalties for violating odometry or changes between consecutive nodes // if odometry is not available. for (auto node_it = node_data_.begin(); node_it != node_data_.end();) { const int trajectory_id = node_it->id.trajectory_id; diff --git a/cartographer/mapping_2d/sparse_pose_graph/spa_cost_function.h b/cartographer/mapping_2d/sparse_pose_graph/spa_cost_function.h index 3976925..ef113f7 100644 --- a/cartographer/mapping_2d/sparse_pose_graph/spa_cost_function.h +++ b/cartographer/mapping_2d/sparse_pose_graph/spa_cost_function.h @@ -38,8 +38,8 @@ class SpaCostFunction { explicit SpaCostFunction(const Constraint::Pose& pose) : pose_(pose) {} - // Computes the error between the scan-to-submap alignment 'zbar_ij' and the - // difference of submap pose 'c_i' and scan pose 'c_j' which are both in an + // Computes the error between the node-to-submap alignment 'zbar_ij' and the + // difference of submap pose 'c_i' and node pose 'c_j' which are both in an // arbitrary common frame. template static std::array ComputeUnscaledError( diff --git a/cartographer/mapping_2d/sparse_pose_graph_test.cc b/cartographer/mapping_2d/sparse_pose_graph_test.cc index 5d5f3eb..e117b5c 100644 --- a/cartographer/mapping_2d/sparse_pose_graph_test.cc +++ b/cartographer/mapping_2d/sparse_pose_graph_test.cc @@ -157,7 +157,7 @@ class SparsePoseGraphTest : public ::testing::Test { active_submaps_->InsertRangeData(TransformRangeData( range_data, transform::Embed3D(pose_estimate.cast()))); - sparse_pose_graph_->AddScan( + sparse_pose_graph_->AddNode( std::make_shared( mapping::TrajectoryNode::Data{common::FromUniversal(0), Eigen::Quaterniond::Identity(), @@ -208,7 +208,7 @@ TEST_F(SparsePoseGraphTest, NoMovement) { transform::IsNearly(transform::Rigid3d::Identity(), 1e-2)); } -TEST_F(SparsePoseGraphTest, NoOverlappingScans) { +TEST_F(SparsePoseGraphTest, NoOverlappingNodes) { std::mt19937 rng(0); std::uniform_real_distribution distribution(-1., 1.); std::vector poses; @@ -229,7 +229,7 @@ TEST_F(SparsePoseGraphTest, NoOverlappingScans) { } } -TEST_F(SparsePoseGraphTest, ConsecutivelyOverlappingScans) { +TEST_F(SparsePoseGraphTest, ConsecutivelyOverlappingNodes) { std::mt19937 rng(0); std::uniform_real_distribution distribution(-1., 1.); std::vector poses; @@ -250,7 +250,7 @@ TEST_F(SparsePoseGraphTest, ConsecutivelyOverlappingScans) { } } -TEST_F(SparsePoseGraphTest, OverlappingScans) { +TEST_F(SparsePoseGraphTest, OverlappingNodes) { std::mt19937 rng(0); std::uniform_real_distribution distribution(-1., 1.); std::vector ground_truth; diff --git a/cartographer/mapping_2d/submaps.h b/cartographer/mapping_2d/submaps.h index ab6f6b9..7d3f9a9 100644 --- a/cartographer/mapping_2d/submaps.h +++ b/cartographer/mapping_2d/submaps.h @@ -68,11 +68,11 @@ class Submap : public mapping::Submap { }; // Except during initialization when only a single submap exists, there are -// always two submaps into which scans are inserted: an old submap that is used -// for matching, and a new one, which will be used for matching next, that is -// being initialized. +// always two submaps into which range data is inserted: an old submap that is +// used for matching, and a new one, which will be used for matching next, that +// is being initialized. // -// Once a certain number of scans have been inserted, the new submap is +// Once a certain number of range data have been inserted, the new submap is // considered initialized: the old submap is no longer changed, the "new" submap // is now the "old" submap and is used for scan-to-map matching. Moreover, a // "new" submap gets created. The "old" submap is forgotten by this object. diff --git a/cartographer/mapping_2d/submaps_test.cc b/cartographer/mapping_2d/submaps_test.cc index b8468a2..a89d049 100644 --- a/cartographer/mapping_2d/submaps_test.cc +++ b/cartographer/mapping_2d/submaps_test.cc @@ -31,7 +31,7 @@ namespace cartographer { namespace mapping_2d { namespace { -TEST(SubmapsTest, TheRightNumberOfScansAreInserted) { +TEST(SubmapsTest, TheRightNumberOfRangeDataAreInserted) { constexpr int kNumRangeData = 10; auto parameter_dictionary = common::MakeDictionary( "return {" @@ -49,7 +49,8 @@ TEST(SubmapsTest, TheRightNumberOfScansAreInserted) { std::set> all_submaps; for (int i = 0; i != 1000; ++i) { submaps.InsertRangeData({Eigen::Vector3f::Zero(), {}, {}}); - // Except for the first, maps should only be returned after enough scans. + // Except for the first, maps should only be returned after enough range + // data. for (const auto& submap : submaps.submaps()) { all_submaps.insert(submap); } @@ -57,14 +58,14 @@ TEST(SubmapsTest, TheRightNumberOfScansAreInserted) { EXPECT_LE(kNumRangeData, submaps.submaps().front()->num_range_data()); } } - int correct_num_scans = 0; + int correct_num_range_data = 0; for (const auto& submap : all_submaps) { if (submap->num_range_data() == kNumRangeData * 2) { - ++correct_num_scans; + ++correct_num_range_data; } } - // Submaps should not be left without the right number of scans in them. - EXPECT_EQ(correct_num_scans, all_submaps.size() - 2); + // Submaps should not be left without the right number of range data in them. + EXPECT_EQ(correct_num_range_data, all_submaps.size() - 2); } TEST(SubmapsTest, ToFromProto) { diff --git a/cartographer/mapping_3d/motion_filter.cc b/cartographer/mapping_3d/motion_filter.cc index cf6803a..082ca71 100644 --- a/cartographer/mapping_3d/motion_filter.cc +++ b/cartographer/mapping_3d/motion_filter.cc @@ -40,7 +40,7 @@ MotionFilter::MotionFilter(const proto::MotionFilterOptions& options) bool MotionFilter::IsSimilar(const common::Time time, const transform::Rigid3d& pose) { LOG_IF_EVERY_N(INFO, num_total_ >= 500, 500) - << "Motion filter reduced the number of scans to " + << "Motion filter reduced the number of nodes to " << 100. * num_different_ / num_total_ << "%."; ++num_total_; if (num_total_ > 1 && diff --git a/cartographer/mapping_3d/sparse_pose_graph.cc b/cartographer/mapping_3d/sparse_pose_graph.cc index f401253..83301a8 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.cc +++ b/cartographer/mapping_3d/sparse_pose_graph.cc @@ -97,7 +97,7 @@ std::vector SparsePoseGraph::InitializeGlobalSubmapPoses( return {front_submap_id, last_submap_id}; } -mapping::NodeId SparsePoseGraph::AddScan( +mapping::NodeId SparsePoseGraph::AddNode( std::shared_ptr constant_data, const int trajectory_id, const std::vector>& insertion_submaps) { @@ -125,7 +125,7 @@ mapping::NodeId SparsePoseGraph::AddScan( // execute the lambda. const bool newly_finished_submap = insertion_submaps.front()->finished(); AddWorkItem([=]() REQUIRES(mutex_) { - ComputeConstraintsForScan(node_id, insertion_submaps, + ComputeConstraintsForNode(node_id, insertion_submaps, newly_finished_submap); }); return node_id; @@ -197,17 +197,17 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id, trajectory_nodes_.at(submap_node_id).global_pose}); } - const common::Time scan_time = GetLatestScanTime(node_id, submap_id); + const common::Time node_time = GetLatestNodeTime(node_id, submap_id); const common::Time last_connection_time = trajectory_connectivity_state_.LastConnectionTime( node_id.trajectory_id, submap_id.trajectory_id); if (node_id.trajectory_id == submap_id.trajectory_id || - scan_time < + node_time < last_connection_time + common::FromSeconds( options_.global_constraint_search_after_n_seconds())) { - // If the scan and the submap belong to the same trajectory or if there has - // been a recent global constraint that ties that scan's trajectory to the + // If the node and the submap belong to the same trajectory or if there has + // been a recent global constraint that ties that node's trajectory to the // submap's trajectory, it suffices to do a match constrained to a local // search window. constraint_builder_.MaybeAddConstraint( @@ -227,7 +227,7 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id, } } -void SparsePoseGraph::ComputeConstraintsForOldScans( +void SparsePoseGraph::ComputeConstraintsForOldNodes( const mapping::SubmapId& submap_id) { const auto& submap_data = submap_data_.at(submap_id); for (const auto& node_id_data : optimization_problem_.node_data()) { @@ -238,7 +238,7 @@ void SparsePoseGraph::ComputeConstraintsForOldScans( } } -void SparsePoseGraph::ComputeConstraintsForScan( +void SparsePoseGraph::ComputeConstraintsForNode( const mapping::NodeId& node_id, std::vector> insertion_submaps, const bool newly_finished_submap) { @@ -255,7 +255,7 @@ void SparsePoseGraph::ComputeConstraintsForScan( matching_id.trajectory_id, constant_data->time, local_pose, global_pose); for (size_t i = 0; i < insertion_submaps.size(); ++i) { const mapping::SubmapId submap_id = submap_ids[i]; - // Even if this was the last scan added to 'submap_id', the submap will only + // Even if this was the last node added to 'submap_id', the submap will only // be marked as finished in 'submap_data_' further below. CHECK(submap_data_.at(submap_id).state == SubmapState::kActive); submap_data_.at(submap_id).node_ids.emplace(node_id); @@ -282,13 +282,13 @@ void SparsePoseGraph::ComputeConstraintsForScan( CHECK(finished_submap_data.state == SubmapState::kActive); finished_submap_data.state = SubmapState::kFinished; // We have a new completed submap, so we look into adding constraints for - // old scans. - ComputeConstraintsForOldScans(finished_submap_id); + // old nodes. + ComputeConstraintsForOldNodes(finished_submap_id); } - constraint_builder_.NotifyEndOfScan(); - ++num_scans_since_last_loop_closure_; + constraint_builder_.NotifyEndOfNode(); + ++num_nodes_since_last_loop_closure_; if (options_.optimize_every_n_scans() > 0 && - num_scans_since_last_loop_closure_ > options_.optimize_every_n_scans()) { + num_nodes_since_last_loop_closure_ > options_.optimize_every_n_scans()) { CHECK(!run_loop_closure_); run_loop_closure_ = true; // If there is a 'work_queue_' already, some other thread will take care. @@ -299,7 +299,7 @@ void SparsePoseGraph::ComputeConstraintsForScan( } } -common::Time SparsePoseGraph::GetLatestScanTime( +common::Time SparsePoseGraph::GetLatestNodeTime( const mapping::NodeId& node_id, const mapping::SubmapId& submap_id) const { common::Time time = trajectory_nodes_.at(node_id).constant_data->time; const SubmapData& submap_data = submap_data_.at(submap_id); @@ -316,7 +316,7 @@ void SparsePoseGraph::UpdateTrajectoryConnectivity( const Constraint& constraint) { CHECK_EQ(constraint.tag, mapping::SparsePoseGraph::Constraint::INTER_SUBMAP); const common::Time time = - GetLatestScanTime(constraint.node_id, constraint.submap_id); + GetLatestNodeTime(constraint.node_id, constraint.submap_id); trajectory_connectivity_state_.Connect(constraint.node_id.trajectory_id, constraint.submap_id.trajectory_id, time); @@ -340,7 +340,7 @@ void SparsePoseGraph::HandleWorkQueue() { trimmer->Trim(&trimming_handle); } - num_scans_since_last_loop_closure_ = 0; + num_nodes_since_last_loop_closure_ = 0; run_loop_closure_ = false; while (!run_loop_closure_) { if (work_queue_->empty()) { @@ -359,20 +359,20 @@ void SparsePoseGraph::HandleWorkQueue() { void SparsePoseGraph::WaitForAllComputations() { bool notification = false; common::MutexLocker locker(&mutex_); - const int num_finished_scans_at_start = - constraint_builder_.GetNumFinishedScans(); + const int num_finished_nodes_at_start = + constraint_builder_.GetNumFinishedNodes(); while (!locker.AwaitWithTimeout( [this]() REQUIRES(mutex_) { - return constraint_builder_.GetNumFinishedScans() == + return constraint_builder_.GetNumFinishedNodes() == num_trajectory_nodes_; }, common::FromSeconds(1.))) { std::ostringstream progress_info; progress_info << "Optimizing: " << std::fixed << std::setprecision(1) << 100. * - (constraint_builder_.GetNumFinishedScans() - - num_finished_scans_at_start) / - (num_trajectory_nodes_ - num_finished_scans_at_start) + (constraint_builder_.GetNumFinishedNodes() - + num_finished_nodes_at_start) / + (num_trajectory_nodes_ - num_finished_nodes_at_start) << "%..."; std::cout << "\r\x1b[K" << progress_info.str() << std::flush; } diff --git a/cartographer/mapping_3d/sparse_pose_graph.h b/cartographer/mapping_3d/sparse_pose_graph.h index f5ae928..1920094 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.h +++ b/cartographer/mapping_3d/sparse_pose_graph.h @@ -53,9 +53,9 @@ namespace mapping_3d { // on (pp. 22--29). IEEE, 2010. // // It is extended for submapping in 3D: -// Each scan has been matched against one or more submaps (adding a constraint -// for each match), both poses of scans and of submaps are to be optimized. -// All constraints are between a submap i and a scan j. +// Each node has been matched against one or more submaps (adding a constraint +// for each match), both poses of nodes and of submaps are to be optimized. +// All constraints are between a submap i and a node j. class SparsePoseGraph : public mapping::SparsePoseGraph { public: SparsePoseGraph(const mapping::proto::SparsePoseGraphOptions& options, @@ -67,10 +67,10 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { // Adds a new node with 'constant_data'. Its 'constant_data->local_pose' was // determined by scan matching against 'insertion_submaps.front()' and the - // scan was inserted into the 'insertion_submaps'. If + // node data was inserted into the 'insertion_submaps'. If // 'insertion_submaps.front().finished()' is 'true', data was inserted into // this submap for the last time. - mapping::NodeId AddScan( + mapping::NodeId AddNode( std::shared_ptr constant_data, int trajectory_id, const std::vector>& insertion_submaps) @@ -119,13 +119,13 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { private: // The current state of the submap in the background threads. When this - // transitions to kFinished, all scans are tried to match against this submap. - // Likewise, all new scans are matched against submaps which are finished. + // transitions to kFinished, all nodes are tried to match against this submap. + // Likewise, all new nodes are matched against submaps which are finished. enum class SubmapState { kActive, kFinished }; struct SubmapData { std::shared_ptr submap; - // IDs of the scans that were inserted into this map together with + // IDs of the nodes that were inserted into this map together with // constraints for them. They are not to be matched again when this submap // becomes 'finished'. std::set node_ids; @@ -146,18 +146,18 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { const std::vector>& insertion_submaps) REQUIRES(mutex_); - // Adds constraints for a scan, and starts scan matching in the background. - void ComputeConstraintsForScan( + // Adds constraints for a node, and starts scan matching in the background. + void ComputeConstraintsForNode( const mapping::NodeId& node_id, std::vector> insertion_submaps, bool newly_finished_submap) REQUIRES(mutex_); - // Computes constraints for a scan and submap pair. + // Computes constraints for a node and submap pair. void ComputeConstraint(const mapping::NodeId& node_id, const mapping::SubmapId& submap_id) REQUIRES(mutex_); - // Adds constraints for older scans whenever a new submap is finished. - void ComputeConstraintsForOldScans(const mapping::SubmapId& submap_id) + // Adds constraints for older nodes whenever a new submap is finished. + void ComputeConstraintsForOldNodes(const mapping::SubmapId& submap_id) REQUIRES(mutex_); // Registers the callback to run the optimization once all constraints have @@ -182,11 +182,11 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { mapping::SparsePoseGraph::SubmapData GetSubmapDataUnderLock( const mapping::SubmapId& submap_id) REQUIRES(mutex_); - common::Time GetLatestScanTime(const mapping::NodeId& node_id, + common::Time GetLatestNodeTime(const mapping::NodeId& node_id, const mapping::SubmapId& submap_id) const REQUIRES(mutex_); - // Logs histograms for the translational and rotational residual of scan + // Logs histograms for the translational and rotational residual of node // poses. void LogResidualHistograms() REQUIRES(mutex_); @@ -205,12 +205,12 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { // How our various trajectories are related. mapping::TrajectoryConnectivityState trajectory_connectivity_state_; - // We globally localize a fraction of the scans from each trajectory. + // We globally localize a fraction of the nodes from each trajectory. std::unordered_map> global_localization_samplers_ GUARDED_BY(mutex_); - // Number of scans added since last loop closure. - int num_scans_since_last_loop_closure_ GUARDED_BY(mutex_) = 0; + // Number of nodes added since last loop closure. + int num_nodes_since_last_loop_closure_ GUARDED_BY(mutex_) = 0; // Whether the optimization has to be run before more data is added. bool run_loop_closure_ GUARDED_BY(mutex_) = false; diff --git a/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc b/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc index eea640f..244eee9 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc +++ b/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc @@ -103,7 +103,7 @@ void ConstraintBuilder::MaybeAddGlobalConstraint( }); } -void ConstraintBuilder::NotifyEndOfScan() { +void ConstraintBuilder::NotifyEndOfNode() { common::MutexLocker locker(&mutex_); ++current_computation_; } @@ -176,9 +176,9 @@ void ConstraintBuilder::ComputeConstraint( const SubmapScanMatcher* const submap_scan_matcher = GetSubmapScanMatcher(submap_id); - // The 'constraint_transform' (submap i <- scan j) is computed from: - // - a 'high_resolution_point_cloud' in scan j and - // - the initial guess 'initial_pose' (submap i <- scan j). + // The 'constraint_transform' (submap i <- node j) is computed from: + // - a 'high_resolution_point_cloud' in node j and + // - the initial guess 'initial_pose' (submap i <- node j). std::unique_ptr match_result; @@ -294,7 +294,7 @@ void ConstraintBuilder::FinishComputation(const int computation_index) { } } -int ConstraintBuilder::GetNumFinishedScans() { +int ConstraintBuilder::GetNumFinishedNodes() { common::MutexLocker locker(&mutex_); if (pending_computations_.empty()) { return current_computation_; diff --git a/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h b/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h index 649a386..4c64312 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h +++ b/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h @@ -100,14 +100,14 @@ class ConstraintBuilder { const Eigen::Quaterniond& global_submap_rotation); // Must be called after all computations related to one node have been added. - void NotifyEndOfScan(); + void NotifyEndOfNode(); // Registers the 'callback' to be called with the results, after all // computations triggered by MaybeAddConstraint() have finished. void WhenDone(const std::function& callback); - // Returns the number of consecutive finished scans. - int GetNumFinishedScans(); + // Returns the number of consecutive finished nodes. + int GetNumFinishedNodes(); // Delete data related to 'submap_id'. void DeleteScanMatcher(const mapping::SubmapId& submap_id); @@ -161,12 +161,12 @@ class ConstraintBuilder { std::unique_ptr> when_done_ GUARDED_BY(mutex_); - // Index of the scan in reaction to which computations are currently - // added. This is always the highest scan index seen so far, even when older - // scans are matched against a new submap. + // Index of the node in reaction to which computations are currently + // added. This is always the highest node index seen so far, even when older + // nodes are matched against a new submap. int current_computation_ GUARDED_BY(mutex_) = 0; - // For each added scan, maps to the number of pending computations that were + // For each added node, maps to the number of pending computations that were // added for it. std::map pending_computations_ GUARDED_BY(mutex_); diff --git a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc index 61f73bb..6de739d 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc +++ b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc @@ -327,7 +327,7 @@ void OptimizationProblem::Solve(const std::vector& constraints, } if (fix_z_ == FixZ::kYes) { - // Add penalties for violating odometry or changes between consecutive scans + // Add penalties for violating odometry or changes between consecutive nodes // if odometry is not available. for (auto node_it = node_data_.begin(); node_it != node_data_.end();) { const int trajectory_id = node_it->id.trajectory_id; diff --git a/cartographer/mapping_3d/sparse_pose_graph/spa_cost_function.h b/cartographer/mapping_3d/sparse_pose_graph/spa_cost_function.h index d1d2c7f..90c588b 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/spa_cost_function.h +++ b/cartographer/mapping_3d/sparse_pose_graph/spa_cost_function.h @@ -38,8 +38,8 @@ class SpaCostFunction { explicit SpaCostFunction(const Constraint::Pose& pose) : pose_(pose) {} - // Computes the error between the scan-to-submap alignment 'zbar_ij' and the - // difference of submap pose 'c_i' and scan pose 'c_j' which are both in an + // Computes the error between the node-to-submap alignment 'zbar_ij' and the + // difference of submap pose 'c_i' and node pose 'c_j' which are both in an // arbitrary common frame. template static std::array ComputeUnscaledError( diff --git a/cartographer/mapping_3d/submaps.h b/cartographer/mapping_3d/submaps.h index b9ed50d..d8ce26d 100644 --- a/cartographer/mapping_3d/submaps.h +++ b/cartographer/mapping_3d/submaps.h @@ -75,11 +75,11 @@ class Submap : public mapping::Submap { }; // Except during initialization when only a single submap exists, there are -// always two submaps into which scans are inserted: an old submap that is used -// for matching, and a new one, which will be used for matching next, that is -// being initialized. +// always two submaps into which range data is inserted: an old submap that is +// used for matching, and a new one, which will be used for matching next, that +// is being initialized. // -// Once a certain number of scans have been inserted, the new submap is +// Once a certain number of range data have been inserted, the new submap is // considered initialized: the old submap is no longer changed, the "new" submap // is now the "old" submap and is used for scan-to-map matching. Moreover, a // "new" submap gets created. The "old" submap is forgotten by this object.