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;
if (matching_result->insertion_result != nullptr) {
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->insertion_submaps));
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.
NodeId node_id; // 'j' in the paper.
// Pose of the scan 'j' relative to submap 'i'.
// Pose of the node 'j' relative to submap 'i'.
Pose pose;
// Differentiates between intra-submap (where scan 'j' was inserted into
// submap 'i') and inter-submap constraints (where scan 'j' was not inserted
// Differentiates between intra-submap (where node 'j' was inserted into
// submap 'i') and inter-submap constraints (where node 'j' was not inserted
// into submap 'i').
enum Tag { INTRA_SUBMAP, INTER_SUBMAP } tag;
};

1
cartographer/mapping/trajectory_builder.h
View File

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

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);
}
// Finally, compute and add empty rays based on misses in the scan.
// Finally, compute and add empty rays based on misses in the range data.
for (const Eigen::Vector3f& missing_echo : range_data.misses) {
CastRay(begin, superscaled_limits.GetCellIndex(missing_echo.head<2>()),
miss_table, probability_grid);

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

34
cartographer/mapping_2d/sparse_pose_graph.h
View File

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

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

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);
// Must be called after all computations related to one node have been added.
void NotifyEndOfScan();
void NotifyEndOfNode();
// Registers the 'callback' to be called with the results, after all
// computations triggered by MaybeAddConstraint() have finished.
void WhenDone(const std::function<void(const Result&)>& callback);
// Returns the number of consecutive finished scans.
int GetNumFinishedScans();
// Returns the number of consecutive finished nodes.
int GetNumFinishedNodes();
// Delete data related to 'submap_id'.
void DeleteScanMatcher(const mapping::SubmapId& submap_id);
@ -150,12 +150,12 @@ class ConstraintBuilder {
std::unique_ptr<std::function<void(const Result&)>> when_done_
GUARDED_BY(mutex_);
// Index of the scan in reaction to which computations are currently
// added. This is always the highest scan index seen so far, even when older
// scans are matched against a new submap.
// Index of the node in reaction to which computations are currently
// added. This is always the highest node index seen so far, even when older
// nodes are matched against a new submap.
int current_computation_ GUARDED_BY(mutex_) = 0;
// For each added scan, maps to the number of pending computations that were
// For each added node, maps to the number of pending computations that were
// added for it.
std::map<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());
}
// Add penalties for violating odometry or changes between consecutive scans
// Add penalties for violating odometry or changes between consecutive nodes
// if odometry is not available.
for (auto node_it = node_data_.begin(); node_it != node_data_.end();) {
const int trajectory_id = node_it->id.trajectory_id;

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) {}
// Computes the error between the scan-to-submap alignment 'zbar_ij' and the
// difference of submap pose 'c_i' and scan pose 'c_j' which are both in an
// Computes the error between the node-to-submap alignment 'zbar_ij' and the
// difference of submap pose 'c_i' and node pose 'c_j' which are both in an
// arbitrary common frame.
template <typename T>
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(
range_data, transform::Embed3D(pose_estimate.cast<float>())));
sparse_pose_graph_->AddScan(
sparse_pose_graph_->AddNode(
std::make_shared<const mapping::TrajectoryNode::Data>(
mapping::TrajectoryNode::Data{common::FromUniversal(0),
Eigen::Quaterniond::Identity(),
@ -208,7 +208,7 @@ TEST_F(SparsePoseGraphTest, NoMovement) {
transform::IsNearly(transform::Rigid3d::Identity(), 1e-2));
}
TEST_F(SparsePoseGraphTest, NoOverlappingScans) {
TEST_F(SparsePoseGraphTest, NoOverlappingNodes) {
std::mt19937 rng(0);
std::uniform_real_distribution<double> distribution(-1., 1.);
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::uniform_real_distribution<double> distribution(-1., 1.);
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::uniform_real_distribution<double> distribution(-1., 1.);
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
// always two submaps into which scans are inserted: an old submap that is used
// for matching, and a new one, which will be used for matching next, that is
// being initialized.
// always two submaps into which range data is inserted: an old submap that is
// used for matching, and a new one, which will be used for matching next, that
// is being initialized.
//
// Once a certain number of scans have been inserted, the new submap is
// Once a certain number of range data have been inserted, the new submap is
// considered initialized: the old submap is no longer changed, the "new" submap
// is now the "old" submap and is used for scan-to-map matching. Moreover, a
// "new" submap gets created. The "old" submap is forgotten by this object.

13
cartographer/mapping_2d/submaps_test.cc
View File

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

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,
const transform::Rigid3d& pose) {
LOG_IF_EVERY_N(INFO, num_total_ >= 500, 500)
<< "Motion filter reduced the number of scans to "
<< "Motion filter reduced the number of nodes to "
<< 100. * num_different_ / num_total_ << "%.";
++num_total_;
if (num_total_ > 1 &&

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

36
cartographer/mapping_3d/sparse_pose_graph.h
View File

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

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

14
cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h
View File

@ -100,14 +100,14 @@ class ConstraintBuilder {
const Eigen::Quaterniond& global_submap_rotation);
// Must be called after all computations related to one node have been added.
void NotifyEndOfScan();
void NotifyEndOfNode();
// Registers the 'callback' to be called with the results, after all
// computations triggered by MaybeAddConstraint() have finished.
void WhenDone(const std::function<void(const Result&)>& callback);
// Returns the number of consecutive finished scans.
int GetNumFinishedScans();
// Returns the number of consecutive finished nodes.
int GetNumFinishedNodes();
// Delete data related to 'submap_id'.
void DeleteScanMatcher(const mapping::SubmapId& submap_id);
@ -161,12 +161,12 @@ class ConstraintBuilder {
std::unique_ptr<std::function<void(const Result&)>> when_done_
GUARDED_BY(mutex_);
// Index of the scan in reaction to which computations are currently
// added. This is always the highest scan index seen so far, even when older
// scans are matched against a new submap.
// Index of the node in reaction to which computations are currently
// added. This is always the highest node index seen so far, even when older
// nodes are matched against a new submap.
int current_computation_ GUARDED_BY(mutex_) = 0;
// For each added scan, maps to the number of pending computations that were
// For each added node, maps to the number of pending computations that were
// added for it.
std::map<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) {
// Add penalties for violating odometry or changes between consecutive scans
// Add penalties for violating odometry or changes between consecutive nodes
// if odometry is not available.
for (auto node_it = node_data_.begin(); node_it != node_data_.end();) {
const int trajectory_id = node_it->id.trajectory_id;

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) {}
// Computes the error between the scan-to-submap alignment 'zbar_ij' and the
// difference of submap pose 'c_i' and scan pose 'c_j' which are both in an
// Computes the error between the node-to-submap alignment 'zbar_ij' and the
// difference of submap pose 'c_i' and node pose 'c_j' which are both in an
// arbitrary common frame.
template <typename T>
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
// always two submaps into which scans are inserted: an old submap that is used
// for matching, and a new one, which will be used for matching next, that is
// being initialized.
// always two submaps into which range data is inserted: an old submap that is
// used for matching, and a new one, which will be used for matching next, that
// is being initialized.
//
// Once a certain number of scans have been inserted, the new submap is
// Once a certain number of range data have been inserted, the new submap is
// considered initialized: the old submap is no longer changed, the "new" submap
// is now the "old" submap and is used for scan-to-map matching. Moreover, a
// "new" submap gets created. The "old" submap is forgotten by this object.