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.
master
Wolfgang Hess 2017-11-14 16:53:31 +01:00 committed by Wally B. Feed
parent 4b342eddd0
commit a4c0e4754e
21 changed files with 137 additions and 137 deletions

View File

@ -57,7 +57,7 @@ class GlobalTrajectoryBuilder
std::unique_ptr<mapping::NodeId> node_id; std::unique_ptr<mapping::NodeId> node_id;
if (matching_result->insertion_result != nullptr) { if (matching_result->insertion_result != nullptr) {
node_id = ::cartographer::common::make_unique<mapping::NodeId>( node_id = ::cartographer::common::make_unique<mapping::NodeId>(
sparse_pose_graph_->AddScan( sparse_pose_graph_->AddNode(
matching_result->insertion_result->constant_data, trajectory_id_, matching_result->insertion_result->constant_data, trajectory_id_,
matching_result->insertion_result->insertion_submaps)); matching_result->insertion_result->insertion_submaps));
CHECK_EQ(node_id->trajectory_id, trajectory_id_); CHECK_EQ(node_id->trajectory_id, trajectory_id_);

View File

@ -57,11 +57,11 @@ class SparsePoseGraph {
SubmapId submap_id; // 'i' in the paper. SubmapId submap_id; // 'i' in the paper.
NodeId node_id; // 'j' 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; Pose pose;
// Differentiates between intra-submap (where scan 'j' was inserted into // Differentiates between intra-submap (where node 'j' was inserted into
// submap 'i') and inter-submap constraints (where scan 'j' was not inserted // submap 'i') and inter-submap constraints (where node 'j' was not inserted
// into submap 'i'). // into submap 'i').
enum Tag { INTRA_SUBMAP, INTER_SUBMAP } tag; enum Tag { INTRA_SUBMAP, INTER_SUBMAP } tag;
}; };

View File

@ -30,7 +30,6 @@
#include "cartographer/mapping/submaps.h" #include "cartographer/mapping/submaps.h"
#include "cartographer/sensor/data.h" #include "cartographer/sensor/data.h"
#include "cartographer/sensor/point_cloud.h" #include "cartographer/sensor/point_cloud.h"
#include "cartographer/sensor/range_data.h"
namespace cartographer { namespace cartographer {
namespace mapping { namespace mapping {

View File

@ -196,7 +196,7 @@ void CastRays(const sensor::RangeData& range_data,
CastRay(begin, end, miss_table, probability_grid); 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) { for (const Eigen::Vector3f& missing_echo : range_data.misses) {
CastRay(begin, superscaled_limits.GetCellIndex(missing_echo.head<2>()), CastRay(begin, superscaled_limits.GetCellIndex(missing_echo.head<2>()),
miss_table, probability_grid); miss_table, probability_grid);

View File

@ -100,7 +100,7 @@ std::vector<mapping::SubmapId> SparsePoseGraph::InitializeGlobalSubmapPoses(
return {front_submap_id, last_submap_id}; return {front_submap_id, last_submap_id};
} }
mapping::NodeId SparsePoseGraph::AddScan( mapping::NodeId SparsePoseGraph::AddNode(
std::shared_ptr<const mapping::TrajectoryNode::Data> constant_data, std::shared_ptr<const mapping::TrajectoryNode::Data> constant_data,
const int trajectory_id, const int trajectory_id,
const std::vector<std::shared_ptr<const Submap>>& insertion_submaps) { const std::vector<std::shared_ptr<const Submap>>& insertion_submaps) {
@ -128,7 +128,7 @@ mapping::NodeId SparsePoseGraph::AddScan(
// execute the lambda. // execute the lambda.
const bool newly_finished_submap = insertion_submaps.front()->finished(); const bool newly_finished_submap = insertion_submaps.front()->finished();
AddWorkItem([=]() REQUIRES(mutex_) { AddWorkItem([=]() REQUIRES(mutex_) {
ComputeConstraintsForScan(node_id, insertion_submaps, ComputeConstraintsForNode(node_id, insertion_submaps,
newly_finished_submap); newly_finished_submap);
}); });
return node_id; return node_id;
@ -178,17 +178,17 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
const mapping::SubmapId& submap_id) { const mapping::SubmapId& submap_id) {
CHECK(submap_data_.at(submap_id).state == SubmapState::kFinished); 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 = const common::Time last_connection_time =
trajectory_connectivity_state_.LastConnectionTime( trajectory_connectivity_state_.LastConnectionTime(
node_id.trajectory_id, submap_id.trajectory_id); node_id.trajectory_id, submap_id.trajectory_id);
if (node_id.trajectory_id == submap_id.trajectory_id || if (node_id.trajectory_id == submap_id.trajectory_id ||
scan_time < node_time <
last_connection_time + last_connection_time +
common::FromSeconds( common::FromSeconds(
options_.global_constraint_search_after_n_seconds())) { options_.global_constraint_search_after_n_seconds())) {
// If the scan and the submap belong to the same trajectory or if there has // If the node 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 // 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 // submap's trajectory, it suffices to do a match constrained to a local
// search window. // search window.
const transform::Rigid2d initial_relative_pose = 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 mapping::SubmapId& submap_id) {
const auto& submap_data = submap_data_.at(submap_id); const auto& submap_data = submap_data_.at(submap_id);
for (const auto& node_id_data : optimization_problem_.node_data()) { 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, const mapping::NodeId& node_id,
std::vector<std::shared_ptr<const Submap>> insertion_submaps, std::vector<std::shared_ptr<const Submap>> insertion_submaps,
const bool newly_finished_submap) { const bool newly_finished_submap) {
@ -240,7 +240,7 @@ void SparsePoseGraph::ComputeConstraintsForScan(
constant_data->gravity_alignment); constant_data->gravity_alignment);
for (size_t i = 0; i < insertion_submaps.size(); ++i) { for (size_t i = 0; i < insertion_submaps.size(); ++i) {
const mapping::SubmapId submap_id = submap_ids[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. // be marked as finished in 'submap_data_' further below.
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);
@ -268,13 +268,13 @@ void SparsePoseGraph::ComputeConstraintsForScan(
CHECK(finished_submap_data.state == SubmapState::kActive); CHECK(finished_submap_data.state == SubmapState::kActive);
finished_submap_data.state = SubmapState::kFinished; finished_submap_data.state = SubmapState::kFinished;
// We have a new completed submap, so we look into adding constraints for // We have a new completed submap, so we look into adding constraints for
// old scans. // old nodes.
ComputeConstraintsForOldScans(finished_submap_id); ComputeConstraintsForOldNodes(finished_submap_id);
} }
constraint_builder_.NotifyEndOfScan(); constraint_builder_.NotifyEndOfNode();
++num_scans_since_last_loop_closure_; ++num_nodes_since_last_loop_closure_;
if (options_.optimize_every_n_scans() > 0 && 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_); CHECK(!run_loop_closure_);
run_loop_closure_ = true; run_loop_closure_ = true;
// If there is a 'work_queue_' already, some other thread will take care. // 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 { const mapping::NodeId& node_id, const mapping::SubmapId& submap_id) const {
common::Time time = trajectory_nodes_.at(node_id).constant_data->time; common::Time time = trajectory_nodes_.at(node_id).constant_data->time;
const SubmapData& submap_data = submap_data_.at(submap_id); const SubmapData& submap_data = submap_data_.at(submap_id);
@ -302,7 +302,7 @@ void SparsePoseGraph::UpdateTrajectoryConnectivity(
const Constraint& constraint) { const Constraint& constraint) {
CHECK_EQ(constraint.tag, mapping::SparsePoseGraph::Constraint::INTER_SUBMAP); CHECK_EQ(constraint.tag, mapping::SparsePoseGraph::Constraint::INTER_SUBMAP);
const common::Time time = 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, trajectory_connectivity_state_.Connect(constraint.node_id.trajectory_id,
constraint.submap_id.trajectory_id, constraint.submap_id.trajectory_id,
time); time);
@ -326,7 +326,7 @@ void SparsePoseGraph::HandleWorkQueue() {
trimmer->Trim(&trimming_handle); trimmer->Trim(&trimming_handle);
} }
num_scans_since_last_loop_closure_ = 0; num_nodes_since_last_loop_closure_ = 0;
run_loop_closure_ = false; run_loop_closure_ = false;
while (!run_loop_closure_) { while (!run_loop_closure_) {
if (work_queue_->empty()) { if (work_queue_->empty()) {
@ -345,20 +345,20 @@ void SparsePoseGraph::HandleWorkQueue() {
void SparsePoseGraph::WaitForAllComputations() { void SparsePoseGraph::WaitForAllComputations() {
bool notification = false; bool notification = false;
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
const int num_finished_scans_at_start = const int num_finished_nodes_at_start =
constraint_builder_.GetNumFinishedScans(); constraint_builder_.GetNumFinishedNodes();
while (!locker.AwaitWithTimeout( while (!locker.AwaitWithTimeout(
[this]() REQUIRES(mutex_) { [this]() REQUIRES(mutex_) {
return constraint_builder_.GetNumFinishedScans() == return constraint_builder_.GetNumFinishedNodes() ==
num_trajectory_nodes_; num_trajectory_nodes_;
}, },
common::FromSeconds(1.))) { common::FromSeconds(1.))) {
std::ostringstream progress_info; std::ostringstream progress_info;
progress_info << "Optimizing: " << std::fixed << std::setprecision(1) progress_info << "Optimizing: " << std::fixed << std::setprecision(1)
<< 100. * << 100. *
(constraint_builder_.GetNumFinishedScans() - (constraint_builder_.GetNumFinishedNodes() -
num_finished_scans_at_start) / num_finished_nodes_at_start) /
(num_trajectory_nodes_ - num_finished_scans_at_start) (num_trajectory_nodes_ - num_finished_nodes_at_start)
<< "%..."; << "%...";
std::cout << "\r\x1b[K" << progress_info.str() << std::flush; std::cout << "\r\x1b[K" << progress_info.str() << std::flush;
} }

View File

@ -53,9 +53,9 @@ namespace mapping_2d {
// on (pp. 22--29). IEEE, 2010. // on (pp. 22--29). IEEE, 2010.
// //
// It is extended for submapping: // It is extended for submapping:
// Each scan has been matched against one or more submaps (adding a constraint // Each node 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. // for each match), both poses of nodes and of submaps are to be optimized.
// All constraints are between a submap i and a scan j. // All constraints are between a submap i and a node j.
class SparsePoseGraph : public mapping::SparsePoseGraph { class SparsePoseGraph : public mapping::SparsePoseGraph {
public: public:
SparsePoseGraph(const mapping::proto::SparsePoseGraphOptions& options, 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 // Adds a new node with 'constant_data'. Its 'constant_data->local_pose' was
// determined by scan matching against 'insertion_submaps.front()' and the // 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 // 'insertion_submaps.front().finished()' is 'true', data was inserted into
// this submap for the last time. // this submap for the last time.
mapping::NodeId AddScan( mapping::NodeId AddNode(
std::shared_ptr<const mapping::TrajectoryNode::Data> constant_data, std::shared_ptr<const mapping::TrajectoryNode::Data> constant_data,
int trajectory_id, int trajectory_id,
const std::vector<std::shared_ptr<const Submap>>& insertion_submaps) const std::vector<std::shared_ptr<const Submap>>& insertion_submaps)
@ -119,13 +119,13 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
private: private:
// The current state of the submap in the background threads. When this // The current state of the submap in the background threads. When this
// transitions to kFinished, all scans are tried to match against this submap. // transitions to kFinished, all nodes are tried to match against this submap.
// Likewise, all new scans are matched against submaps which are finished. // Likewise, all new nodes are matched against submaps which are finished.
enum class SubmapState { kActive, kFinished }; enum class SubmapState { kActive, kFinished };
struct SubmapData { struct SubmapData {
std::shared_ptr<const Submap> submap; std::shared_ptr<const Submap> 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 // constraints for them. They are not to be matched again when this submap
// becomes 'finished'. // becomes 'finished'.
std::set<mapping::NodeId> node_ids; std::set<mapping::NodeId> node_ids;
@ -146,18 +146,18 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
const std::vector<std::shared_ptr<const Submap>>& insertion_submaps) const std::vector<std::shared_ptr<const Submap>>& insertion_submaps)
REQUIRES(mutex_); REQUIRES(mutex_);
// Adds constraints for a scan, and starts scan matching in the background. // Adds constraints for a node, and starts scan matching in the background.
void ComputeConstraintsForScan( void ComputeConstraintsForNode(
const mapping::NodeId& node_id, const mapping::NodeId& node_id,
std::vector<std::shared_ptr<const Submap>> insertion_submaps, std::vector<std::shared_ptr<const Submap>> insertion_submaps,
bool newly_finished_submap) REQUIRES(mutex_); 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, void ComputeConstraint(const mapping::NodeId& node_id,
const mapping::SubmapId& submap_id) REQUIRES(mutex_); const mapping::SubmapId& submap_id) REQUIRES(mutex_);
// Adds constraints for older scans whenever a new submap is finished. // Adds constraints for older nodes whenever a new submap is finished.
void ComputeConstraintsForOldScans(const mapping::SubmapId& submap_id) void ComputeConstraintsForOldNodes(const mapping::SubmapId& submap_id)
REQUIRES(mutex_); REQUIRES(mutex_);
// Registers the callback to run the optimization once all constraints have // Registers the callback to run the optimization once all constraints have
@ -182,7 +182,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
mapping::SparsePoseGraph::SubmapData GetSubmapDataUnderLock( mapping::SparsePoseGraph::SubmapData GetSubmapDataUnderLock(
const mapping::SubmapId& submap_id) REQUIRES(mutex_); 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 const mapping::SubmapId& submap_id) const
REQUIRES(mutex_); REQUIRES(mutex_);
@ -201,12 +201,12 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
// How our various trajectories are related. // How our various trajectories are related.
mapping::TrajectoryConnectivityState trajectory_connectivity_state_; 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<int, std::unique_ptr<common::FixedRatioSampler>> std::unordered_map<int, std::unique_ptr<common::FixedRatioSampler>>
global_localization_samplers_ GUARDED_BY(mutex_); global_localization_samplers_ GUARDED_BY(mutex_);
// Number of scans added since last loop closure. // Number of nodes added since last loop closure.
int num_scans_since_last_loop_closure_ GUARDED_BY(mutex_) = 0; int num_nodes_since_last_loop_closure_ GUARDED_BY(mutex_) = 0;
// Whether the optimization has to be run before more data is added. // Whether the optimization has to be run before more data is added.
bool run_loop_closure_ GUARDED_BY(mutex_) = false; bool run_loop_closure_ GUARDED_BY(mutex_) = false;

View File

@ -101,7 +101,7 @@ void ConstraintBuilder::MaybeAddGlobalConstraint(
}); });
} }
void ConstraintBuilder::NotifyEndOfScan() { void ConstraintBuilder::NotifyEndOfNode() {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
++current_computation_; ++current_computation_;
} }
@ -167,10 +167,10 @@ void ConstraintBuilder::ComputeConstraint(
const SubmapScanMatcher* const submap_scan_matcher = const SubmapScanMatcher* const submap_scan_matcher =
GetSubmapScanMatcher(submap_id); GetSubmapScanMatcher(submap_id);
// The 'constraint_transform' (submap i <- scan j) is computed from: // The 'constraint_transform' (submap i <- node j) is computed from:
// - a 'filtered_gravity_aligned_point_cloud' in scan j, // - a 'filtered_gravity_aligned_point_cloud' in node j,
// - the initial guess 'initial_pose' for (map <- scan j), // - the initial guess 'initial_pose' for (map <- node j),
// - the result 'pose_estimate' of Match() (map <- scan j). // - the result 'pose_estimate' of Match() (map <- node j).
// - the ComputeSubmapPose() (map <- submap i) // - the ComputeSubmapPose() (map <- submap i)
float score = 0.; float score = 0.;
transform::Rigid2d pose_estimate = transform::Rigid2d::Identity(); 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_); common::MutexLocker locker(&mutex_);
if (pending_computations_.empty()) { if (pending_computations_.empty()) {
return current_computation_; return current_computation_;

View File

@ -94,14 +94,14 @@ class ConstraintBuilder {
const mapping::TrajectoryNode::Data* const constant_data); const mapping::TrajectoryNode::Data* const constant_data);
// Must be called after all computations related to one node have been added. // 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 // Registers the 'callback' to be called with the results, after all
// computations triggered by MaybeAddConstraint() have finished. // computations triggered by MaybeAddConstraint() have finished.
void WhenDone(const std::function<void(const Result&)>& callback); void WhenDone(const std::function<void(const Result&)>& callback);
// Returns the number of consecutive finished scans. // Returns the number of consecutive finished nodes.
int GetNumFinishedScans(); int GetNumFinishedNodes();
// Delete data related to 'submap_id'. // Delete data related to 'submap_id'.
void DeleteScanMatcher(const mapping::SubmapId& submap_id); void DeleteScanMatcher(const mapping::SubmapId& submap_id);
@ -150,12 +150,12 @@ class ConstraintBuilder {
std::unique_ptr<std::function<void(const Result&)>> when_done_ std::unique_ptr<std::function<void(const Result&)>> when_done_
GUARDED_BY(mutex_); GUARDED_BY(mutex_);
// Index of the scan in reaction to which computations are currently // Index of the node in reaction to which computations are currently
// added. This is always the highest scan index seen so far, even when older // added. This is always the highest node index seen so far, even when older
// scans are matched against a new submap. // nodes are matched against a new submap.
int current_computation_ GUARDED_BY(mutex_) = 0; 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. // added for it.
std::map<int, int> pending_computations_ GUARDED_BY(mutex_); std::map<int, int> pending_computations_ GUARDED_BY(mutex_);

View File

@ -163,7 +163,7 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
C_nodes.at(constraint.node_id).data()); 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. // if odometry is not available.
for (auto node_it = node_data_.begin(); node_it != node_data_.end();) { for (auto node_it = node_data_.begin(); node_it != node_data_.end();) {
const int trajectory_id = node_it->id.trajectory_id; const int trajectory_id = node_it->id.trajectory_id;

View File

@ -38,8 +38,8 @@ class SpaCostFunction {
explicit SpaCostFunction(const Constraint::Pose& pose) : pose_(pose) {} explicit SpaCostFunction(const Constraint::Pose& pose) : pose_(pose) {}
// Computes the error between the scan-to-submap alignment 'zbar_ij' and the // Computes the error between the node-to-submap alignment 'zbar_ij' and the
// difference of submap pose 'c_i' and scan pose 'c_j' which are both in an // difference of submap pose 'c_i' and node pose 'c_j' which are both in an
// arbitrary common frame. // arbitrary common frame.
template <typename T> template <typename T>
static std::array<T, 3> ComputeUnscaledError( static std::array<T, 3> ComputeUnscaledError(

View File

@ -157,7 +157,7 @@ class SparsePoseGraphTest : public ::testing::Test {
active_submaps_->InsertRangeData(TransformRangeData( active_submaps_->InsertRangeData(TransformRangeData(
range_data, transform::Embed3D(pose_estimate.cast<float>()))); range_data, transform::Embed3D(pose_estimate.cast<float>())));
sparse_pose_graph_->AddScan( sparse_pose_graph_->AddNode(
std::make_shared<const mapping::TrajectoryNode::Data>( std::make_shared<const mapping::TrajectoryNode::Data>(
mapping::TrajectoryNode::Data{common::FromUniversal(0), mapping::TrajectoryNode::Data{common::FromUniversal(0),
Eigen::Quaterniond::Identity(), Eigen::Quaterniond::Identity(),
@ -208,7 +208,7 @@ TEST_F(SparsePoseGraphTest, NoMovement) {
transform::IsNearly(transform::Rigid3d::Identity(), 1e-2)); transform::IsNearly(transform::Rigid3d::Identity(), 1e-2));
} }
TEST_F(SparsePoseGraphTest, NoOverlappingScans) { TEST_F(SparsePoseGraphTest, NoOverlappingNodes) {
std::mt19937 rng(0); std::mt19937 rng(0);
std::uniform_real_distribution<double> distribution(-1., 1.); std::uniform_real_distribution<double> distribution(-1., 1.);
std::vector<transform::Rigid2d> poses; std::vector<transform::Rigid2d> poses;
@ -229,7 +229,7 @@ TEST_F(SparsePoseGraphTest, NoOverlappingScans) {
} }
} }
TEST_F(SparsePoseGraphTest, ConsecutivelyOverlappingScans) { TEST_F(SparsePoseGraphTest, ConsecutivelyOverlappingNodes) {
std::mt19937 rng(0); std::mt19937 rng(0);
std::uniform_real_distribution<double> distribution(-1., 1.); std::uniform_real_distribution<double> distribution(-1., 1.);
std::vector<transform::Rigid2d> poses; std::vector<transform::Rigid2d> poses;
@ -250,7 +250,7 @@ TEST_F(SparsePoseGraphTest, ConsecutivelyOverlappingScans) {
} }
} }
TEST_F(SparsePoseGraphTest, OverlappingScans) { TEST_F(SparsePoseGraphTest, OverlappingNodes) {
std::mt19937 rng(0); std::mt19937 rng(0);
std::uniform_real_distribution<double> distribution(-1., 1.); std::uniform_real_distribution<double> distribution(-1., 1.);
std::vector<transform::Rigid2d> ground_truth; std::vector<transform::Rigid2d> ground_truth;

View File

@ -68,11 +68,11 @@ class Submap : public mapping::Submap {
}; };
// Except during initialization when only a single submap exists, there are // 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 // always two submaps into which range data is inserted: an old submap that is
// for matching, and a new one, which will be used for matching next, that is // used for matching, and a new one, which will be used for matching next, that
// being initialized. // 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 // 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 // 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. // "new" submap gets created. The "old" submap is forgotten by this object.

View File

@ -31,7 +31,7 @@ namespace cartographer {
namespace mapping_2d { namespace mapping_2d {
namespace { namespace {
TEST(SubmapsTest, TheRightNumberOfScansAreInserted) { TEST(SubmapsTest, TheRightNumberOfRangeDataAreInserted) {
constexpr int kNumRangeData = 10; constexpr int kNumRangeData = 10;
auto parameter_dictionary = common::MakeDictionary( auto parameter_dictionary = common::MakeDictionary(
"return {" "return {"
@ -49,7 +49,8 @@ TEST(SubmapsTest, TheRightNumberOfScansAreInserted) {
std::set<std::shared_ptr<Submap>> all_submaps; std::set<std::shared_ptr<Submap>> all_submaps;
for (int i = 0; i != 1000; ++i) { for (int i = 0; i != 1000; ++i) {
submaps.InsertRangeData({Eigen::Vector3f::Zero(), {}, {}}); 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()) { for (const auto& submap : submaps.submaps()) {
all_submaps.insert(submap); all_submaps.insert(submap);
} }
@ -57,14 +58,14 @@ TEST(SubmapsTest, TheRightNumberOfScansAreInserted) {
EXPECT_LE(kNumRangeData, submaps.submaps().front()->num_range_data()); 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) { for (const auto& submap : all_submaps) {
if (submap->num_range_data() == kNumRangeData * 2) { 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. // Submaps should not be left without the right number of range data in them.
EXPECT_EQ(correct_num_scans, all_submaps.size() - 2); EXPECT_EQ(correct_num_range_data, all_submaps.size() - 2);
} }
TEST(SubmapsTest, ToFromProto) { TEST(SubmapsTest, ToFromProto) {

View File

@ -40,7 +40,7 @@ MotionFilter::MotionFilter(const proto::MotionFilterOptions& options)
bool MotionFilter::IsSimilar(const common::Time time, bool MotionFilter::IsSimilar(const common::Time time,
const transform::Rigid3d& pose) { const transform::Rigid3d& pose) {
LOG_IF_EVERY_N(INFO, num_total_ >= 500, 500) 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_ << "%."; << 100. * num_different_ / num_total_ << "%.";
++num_total_; ++num_total_;
if (num_total_ > 1 && if (num_total_ > 1 &&

View File

@ -97,7 +97,7 @@ std::vector<mapping::SubmapId> SparsePoseGraph::InitializeGlobalSubmapPoses(
return {front_submap_id, last_submap_id}; return {front_submap_id, last_submap_id};
} }
mapping::NodeId SparsePoseGraph::AddScan( mapping::NodeId SparsePoseGraph::AddNode(
std::shared_ptr<const mapping::TrajectoryNode::Data> constant_data, std::shared_ptr<const mapping::TrajectoryNode::Data> constant_data,
const int trajectory_id, const int trajectory_id,
const std::vector<std::shared_ptr<const Submap>>& insertion_submaps) { const std::vector<std::shared_ptr<const Submap>>& insertion_submaps) {
@ -125,7 +125,7 @@ mapping::NodeId SparsePoseGraph::AddScan(
// execute the lambda. // execute the lambda.
const bool newly_finished_submap = insertion_submaps.front()->finished(); const bool newly_finished_submap = insertion_submaps.front()->finished();
AddWorkItem([=]() REQUIRES(mutex_) { AddWorkItem([=]() REQUIRES(mutex_) {
ComputeConstraintsForScan(node_id, insertion_submaps, ComputeConstraintsForNode(node_id, insertion_submaps,
newly_finished_submap); newly_finished_submap);
}); });
return node_id; return node_id;
@ -197,17 +197,17 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
trajectory_nodes_.at(submap_node_id).global_pose}); 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 = const common::Time last_connection_time =
trajectory_connectivity_state_.LastConnectionTime( trajectory_connectivity_state_.LastConnectionTime(
node_id.trajectory_id, submap_id.trajectory_id); node_id.trajectory_id, submap_id.trajectory_id);
if (node_id.trajectory_id == submap_id.trajectory_id || if (node_id.trajectory_id == submap_id.trajectory_id ||
scan_time < node_time <
last_connection_time + last_connection_time +
common::FromSeconds( common::FromSeconds(
options_.global_constraint_search_after_n_seconds())) { options_.global_constraint_search_after_n_seconds())) {
// If the scan and the submap belong to the same trajectory or if there has // If the node 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 // 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 // submap's trajectory, it suffices to do a match constrained to a local
// search window. // search window.
constraint_builder_.MaybeAddConstraint( 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 mapping::SubmapId& submap_id) {
const auto& submap_data = submap_data_.at(submap_id); const auto& submap_data = submap_data_.at(submap_id);
for (const auto& node_id_data : optimization_problem_.node_data()) { 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, const mapping::NodeId& node_id,
std::vector<std::shared_ptr<const Submap>> insertion_submaps, std::vector<std::shared_ptr<const Submap>> insertion_submaps,
const bool newly_finished_submap) { const bool newly_finished_submap) {
@ -255,7 +255,7 @@ void SparsePoseGraph::ComputeConstraintsForScan(
matching_id.trajectory_id, constant_data->time, local_pose, global_pose); matching_id.trajectory_id, constant_data->time, local_pose, global_pose);
for (size_t i = 0; i < insertion_submaps.size(); ++i) { for (size_t i = 0; i < insertion_submaps.size(); ++i) {
const mapping::SubmapId submap_id = submap_ids[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. // be marked as finished in 'submap_data_' further below.
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);
@ -282,13 +282,13 @@ void SparsePoseGraph::ComputeConstraintsForScan(
CHECK(finished_submap_data.state == SubmapState::kActive); CHECK(finished_submap_data.state == SubmapState::kActive);
finished_submap_data.state = SubmapState::kFinished; finished_submap_data.state = SubmapState::kFinished;
// We have a new completed submap, so we look into adding constraints for // We have a new completed submap, so we look into adding constraints for
// old scans. // old nodes.
ComputeConstraintsForOldScans(finished_submap_id); ComputeConstraintsForOldNodes(finished_submap_id);
} }
constraint_builder_.NotifyEndOfScan(); constraint_builder_.NotifyEndOfNode();
++num_scans_since_last_loop_closure_; ++num_nodes_since_last_loop_closure_;
if (options_.optimize_every_n_scans() > 0 && 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_); CHECK(!run_loop_closure_);
run_loop_closure_ = true; run_loop_closure_ = true;
// If there is a 'work_queue_' already, some other thread will take care. // 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 { const mapping::NodeId& node_id, const mapping::SubmapId& submap_id) const {
common::Time time = trajectory_nodes_.at(node_id).constant_data->time; common::Time time = trajectory_nodes_.at(node_id).constant_data->time;
const SubmapData& submap_data = submap_data_.at(submap_id); const SubmapData& submap_data = submap_data_.at(submap_id);
@ -316,7 +316,7 @@ void SparsePoseGraph::UpdateTrajectoryConnectivity(
const Constraint& constraint) { const Constraint& constraint) {
CHECK_EQ(constraint.tag, mapping::SparsePoseGraph::Constraint::INTER_SUBMAP); CHECK_EQ(constraint.tag, mapping::SparsePoseGraph::Constraint::INTER_SUBMAP);
const common::Time time = 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, trajectory_connectivity_state_.Connect(constraint.node_id.trajectory_id,
constraint.submap_id.trajectory_id, constraint.submap_id.trajectory_id,
time); time);
@ -340,7 +340,7 @@ void SparsePoseGraph::HandleWorkQueue() {
trimmer->Trim(&trimming_handle); trimmer->Trim(&trimming_handle);
} }
num_scans_since_last_loop_closure_ = 0; num_nodes_since_last_loop_closure_ = 0;
run_loop_closure_ = false; run_loop_closure_ = false;
while (!run_loop_closure_) { while (!run_loop_closure_) {
if (work_queue_->empty()) { if (work_queue_->empty()) {
@ -359,20 +359,20 @@ void SparsePoseGraph::HandleWorkQueue() {
void SparsePoseGraph::WaitForAllComputations() { void SparsePoseGraph::WaitForAllComputations() {
bool notification = false; bool notification = false;
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
const int num_finished_scans_at_start = const int num_finished_nodes_at_start =
constraint_builder_.GetNumFinishedScans(); constraint_builder_.GetNumFinishedNodes();
while (!locker.AwaitWithTimeout( while (!locker.AwaitWithTimeout(
[this]() REQUIRES(mutex_) { [this]() REQUIRES(mutex_) {
return constraint_builder_.GetNumFinishedScans() == return constraint_builder_.GetNumFinishedNodes() ==
num_trajectory_nodes_; num_trajectory_nodes_;
}, },
common::FromSeconds(1.))) { common::FromSeconds(1.))) {
std::ostringstream progress_info; std::ostringstream progress_info;
progress_info << "Optimizing: " << std::fixed << std::setprecision(1) progress_info << "Optimizing: " << std::fixed << std::setprecision(1)
<< 100. * << 100. *
(constraint_builder_.GetNumFinishedScans() - (constraint_builder_.GetNumFinishedNodes() -
num_finished_scans_at_start) / num_finished_nodes_at_start) /
(num_trajectory_nodes_ - num_finished_scans_at_start) (num_trajectory_nodes_ - num_finished_nodes_at_start)
<< "%..."; << "%...";
std::cout << "\r\x1b[K" << progress_info.str() << std::flush; std::cout << "\r\x1b[K" << progress_info.str() << std::flush;
} }

View File

@ -53,9 +53,9 @@ namespace mapping_3d {
// on (pp. 22--29). IEEE, 2010. // on (pp. 22--29). IEEE, 2010.
// //
// It is extended for submapping in 3D: // It is extended for submapping in 3D:
// Each scan has been matched against one or more submaps (adding a constraint // Each node 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. // for each match), both poses of nodes and of submaps are to be optimized.
// All constraints are between a submap i and a scan j. // All constraints are between a submap i and a node j.
class SparsePoseGraph : public mapping::SparsePoseGraph { class SparsePoseGraph : public mapping::SparsePoseGraph {
public: public:
SparsePoseGraph(const mapping::proto::SparsePoseGraphOptions& options, 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 // Adds a new node with 'constant_data'. Its 'constant_data->local_pose' was
// determined by scan matching against 'insertion_submaps.front()' and the // 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 // 'insertion_submaps.front().finished()' is 'true', data was inserted into
// this submap for the last time. // this submap for the last time.
mapping::NodeId AddScan( mapping::NodeId AddNode(
std::shared_ptr<const mapping::TrajectoryNode::Data> constant_data, std::shared_ptr<const mapping::TrajectoryNode::Data> constant_data,
int trajectory_id, int trajectory_id,
const std::vector<std::shared_ptr<const Submap>>& insertion_submaps) const std::vector<std::shared_ptr<const Submap>>& insertion_submaps)
@ -119,13 +119,13 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
private: private:
// The current state of the submap in the background threads. When this // The current state of the submap in the background threads. When this
// transitions to kFinished, all scans are tried to match against this submap. // transitions to kFinished, all nodes are tried to match against this submap.
// Likewise, all new scans are matched against submaps which are finished. // Likewise, all new nodes are matched against submaps which are finished.
enum class SubmapState { kActive, kFinished }; enum class SubmapState { kActive, kFinished };
struct SubmapData { struct SubmapData {
std::shared_ptr<const Submap> submap; std::shared_ptr<const Submap> 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 // constraints for them. They are not to be matched again when this submap
// becomes 'finished'. // becomes 'finished'.
std::set<mapping::NodeId> node_ids; std::set<mapping::NodeId> node_ids;
@ -146,18 +146,18 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
const std::vector<std::shared_ptr<const Submap>>& insertion_submaps) const std::vector<std::shared_ptr<const Submap>>& insertion_submaps)
REQUIRES(mutex_); REQUIRES(mutex_);
// Adds constraints for a scan, and starts scan matching in the background. // Adds constraints for a node, and starts scan matching in the background.
void ComputeConstraintsForScan( void ComputeConstraintsForNode(
const mapping::NodeId& node_id, const mapping::NodeId& node_id,
std::vector<std::shared_ptr<const Submap>> insertion_submaps, std::vector<std::shared_ptr<const Submap>> insertion_submaps,
bool newly_finished_submap) REQUIRES(mutex_); 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, void ComputeConstraint(const mapping::NodeId& node_id,
const mapping::SubmapId& submap_id) REQUIRES(mutex_); const mapping::SubmapId& submap_id) REQUIRES(mutex_);
// Adds constraints for older scans whenever a new submap is finished. // Adds constraints for older nodes whenever a new submap is finished.
void ComputeConstraintsForOldScans(const mapping::SubmapId& submap_id) void ComputeConstraintsForOldNodes(const mapping::SubmapId& submap_id)
REQUIRES(mutex_); REQUIRES(mutex_);
// Registers the callback to run the optimization once all constraints have // Registers the callback to run the optimization once all constraints have
@ -182,11 +182,11 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
mapping::SparsePoseGraph::SubmapData GetSubmapDataUnderLock( mapping::SparsePoseGraph::SubmapData GetSubmapDataUnderLock(
const mapping::SubmapId& submap_id) REQUIRES(mutex_); 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 const mapping::SubmapId& submap_id) const
REQUIRES(mutex_); REQUIRES(mutex_);
// Logs histograms for the translational and rotational residual of scan // Logs histograms for the translational and rotational residual of node
// poses. // poses.
void LogResidualHistograms() REQUIRES(mutex_); void LogResidualHistograms() REQUIRES(mutex_);
@ -205,12 +205,12 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
// How our various trajectories are related. // How our various trajectories are related.
mapping::TrajectoryConnectivityState trajectory_connectivity_state_; 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<int, std::unique_ptr<common::FixedRatioSampler>> std::unordered_map<int, std::unique_ptr<common::FixedRatioSampler>>
global_localization_samplers_ GUARDED_BY(mutex_); global_localization_samplers_ GUARDED_BY(mutex_);
// Number of scans added since last loop closure. // Number of nodes added since last loop closure.
int num_scans_since_last_loop_closure_ GUARDED_BY(mutex_) = 0; int num_nodes_since_last_loop_closure_ GUARDED_BY(mutex_) = 0;
// Whether the optimization has to be run before more data is added. // Whether the optimization has to be run before more data is added.
bool run_loop_closure_ GUARDED_BY(mutex_) = false; bool run_loop_closure_ GUARDED_BY(mutex_) = false;

View File

@ -103,7 +103,7 @@ void ConstraintBuilder::MaybeAddGlobalConstraint(
}); });
} }
void ConstraintBuilder::NotifyEndOfScan() { void ConstraintBuilder::NotifyEndOfNode() {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
++current_computation_; ++current_computation_;
} }
@ -176,9 +176,9 @@ void ConstraintBuilder::ComputeConstraint(
const SubmapScanMatcher* const submap_scan_matcher = const SubmapScanMatcher* const submap_scan_matcher =
GetSubmapScanMatcher(submap_id); GetSubmapScanMatcher(submap_id);
// The 'constraint_transform' (submap i <- scan j) is computed from: // The 'constraint_transform' (submap i <- node j) is computed from:
// - a 'high_resolution_point_cloud' in scan j and // - a 'high_resolution_point_cloud' in node j and
// - the initial guess 'initial_pose' (submap i <- scan j). // - the initial guess 'initial_pose' (submap i <- node j).
std::unique_ptr<scan_matching::FastCorrelativeScanMatcher::Result> std::unique_ptr<scan_matching::FastCorrelativeScanMatcher::Result>
match_result; match_result;
@ -294,7 +294,7 @@ void ConstraintBuilder::FinishComputation(const int computation_index) {
} }
} }
int ConstraintBuilder::GetNumFinishedScans() { int ConstraintBuilder::GetNumFinishedNodes() {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
if (pending_computations_.empty()) { if (pending_computations_.empty()) {
return current_computation_; return current_computation_;

View File

@ -100,14 +100,14 @@ class ConstraintBuilder {
const Eigen::Quaterniond& global_submap_rotation); const Eigen::Quaterniond& global_submap_rotation);
// Must be called after all computations related to one node have been added. // 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 // Registers the 'callback' to be called with the results, after all
// computations triggered by MaybeAddConstraint() have finished. // computations triggered by MaybeAddConstraint() have finished.
void WhenDone(const std::function<void(const Result&)>& callback); void WhenDone(const std::function<void(const Result&)>& callback);
// Returns the number of consecutive finished scans. // Returns the number of consecutive finished nodes.
int GetNumFinishedScans(); int GetNumFinishedNodes();
// Delete data related to 'submap_id'. // Delete data related to 'submap_id'.
void DeleteScanMatcher(const mapping::SubmapId& submap_id); void DeleteScanMatcher(const mapping::SubmapId& submap_id);
@ -161,12 +161,12 @@ class ConstraintBuilder {
std::unique_ptr<std::function<void(const Result&)>> when_done_ std::unique_ptr<std::function<void(const Result&)>> when_done_
GUARDED_BY(mutex_); GUARDED_BY(mutex_);
// Index of the scan in reaction to which computations are currently // Index of the node in reaction to which computations are currently
// added. This is always the highest scan index seen so far, even when older // added. This is always the highest node index seen so far, even when older
// scans are matched against a new submap. // nodes are matched against a new submap.
int current_computation_ GUARDED_BY(mutex_) = 0; 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. // added for it.
std::map<int, int> pending_computations_ GUARDED_BY(mutex_); std::map<int, int> pending_computations_ GUARDED_BY(mutex_);

View File

@ -327,7 +327,7 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
} }
if (fix_z_ == FixZ::kYes) { 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. // if odometry is not available.
for (auto node_it = node_data_.begin(); node_it != node_data_.end();) { for (auto node_it = node_data_.begin(); node_it != node_data_.end();) {
const int trajectory_id = node_it->id.trajectory_id; const int trajectory_id = node_it->id.trajectory_id;

View File

@ -38,8 +38,8 @@ class SpaCostFunction {
explicit SpaCostFunction(const Constraint::Pose& pose) : pose_(pose) {} explicit SpaCostFunction(const Constraint::Pose& pose) : pose_(pose) {}
// Computes the error between the scan-to-submap alignment 'zbar_ij' and the // Computes the error between the node-to-submap alignment 'zbar_ij' and the
// difference of submap pose 'c_i' and scan pose 'c_j' which are both in an // difference of submap pose 'c_i' and node pose 'c_j' which are both in an
// arbitrary common frame. // arbitrary common frame.
template <typename T> template <typename T>
static std::array<T, 6> ComputeUnscaledError( static std::array<T, 6> ComputeUnscaledError(

View File

@ -75,11 +75,11 @@ class Submap : public mapping::Submap {
}; };
// Except during initialization when only a single submap exists, there are // 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 // always two submaps into which range data is inserted: an old submap that is
// for matching, and a new one, which will be used for matching next, that is // used for matching, and a new one, which will be used for matching next, that
// being initialized. // 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 // 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 // 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. // "new" submap gets created. The "old" submap is forgotten by this object.