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

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
cartographer/mapping/global_trajectory_builder.h
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_);

6
cartographer/mapping/sparse_pose_graph.h
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;
}; };

1
cartographer/mapping/trajectory_builder.h
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 {

2
cartographer/mapping_2d/ray_casting.cc
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);

46
cartographer/mapping_2d/sparse_pose_graph.cc
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;
} }

34
cartographer/mapping_2d/sparse_pose_graph.h
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;

12
cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc
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_;

14
cartographer/mapping_2d/sparse_pose_graph/constraint_builder.h
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_);

2
cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc
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;

4
cartographer/mapping_2d/sparse_pose_graph/spa_cost_function.h
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(

8
cartographer/mapping_2d/sparse_pose_graph_test.cc
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;

8
cartographer/mapping_2d/submaps.h
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.

13
cartographer/mapping_2d/submaps_test.cc
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) {

2
cartographer/mapping_3d/motion_filter.cc
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 &&

46
cartographer/mapping_3d/sparse_pose_graph.cc
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;
} }

36
cartographer/mapping_3d/sparse_pose_graph.h
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;

10
cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc
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_;

14
cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h
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_);

2
cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc
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;

4
cartographer/mapping_3d/sparse_pose_graph/spa_cost_function.h
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(

8
cartographer/mapping_3d/submaps.h
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.