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

Adds mapping:NodeId for Constraint. (#279) 

Also extracted mapping::SubmapId into an id.h file.

Related to #256.

PAIR=SirVer
master
Wolfgang Hess 2017-05-11 17:17:12 +02:00 committed by GitHub
parent 32583e0c5d
commit fde9272533
15 changed files with 297 additions and 193 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

61
cartographer/mapping/id.h Normal file
View File

@ -0,0 +1,61 @@
/*
* Copyright 2016 The Cartographer Authors
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#ifndef CARTOGRAPHER_MAPPING_ID_H_
#define CARTOGRAPHER_MAPPING_ID_H_
#include <ostream>
#include <tuple>
namespace cartographer {
namespace mapping {
// Uniquely identifies a trajectory node using a combination of a unique
// trajectory ID and a zero-based index of the node inside that trajectory.
struct NodeId {
int trajectory_id;
int node_index;
bool operator<(const NodeId& other) const {
return std::forward_as_tuple(trajectory_id, node_index) <
std::forward_as_tuple(other.trajectory_id, other.node_index);
}
};
inline std::ostream& operator<<(std::ostream& os, const NodeId& v) {
return os << "(" << v.trajectory_id << ", " << v.node_index << ")";
}
// Uniquely identifies a submap using a combination of a unique trajectory ID
// and a zero-based index of the submap inside that trajectory.
struct SubmapId {
int trajectory_id;
int submap_index;
bool operator<(const SubmapId& other) const {
return std::forward_as_tuple(trajectory_id, submap_index) <
std::forward_as_tuple(other.trajectory_id, other.submap_index);
}
};
inline std::ostream& operator<<(std::ostream& os, const SubmapId& v) {
return os << "(" << v.trajectory_id << ", " << v.submap_index << ")";
}
} // namespace mapping
} // namespace cartographer
#endif // CARTOGRAPHER_MAPPING_ID_H_

4
cartographer/mapping/sparse_pose_graph.cc
View File

@ -98,9 +98,9 @@ proto::SparsePoseGraph SparsePoseGraph::ToProto() {
constraint.submap_id.submap_index);
constraint_proto->mutable_scan_id()->set_trajectory_id(
grouped_node_indices[constraint.j].first);
constraint.node_id.trajectory_id);
constraint_proto->mutable_scan_id()->set_scan_index(
grouped_node_indices[constraint.j].second);
constraint.node_id.node_index);
constraint_proto->set_tag(mapping::ToProto(constraint.tag));
}

5
cartographer/mapping/sparse_pose_graph.h
View File

@ -23,6 +23,7 @@
#include <vector>
#include "cartographer/common/lua_parameter_dictionary.h"
#include "cartographer/mapping/id.h"
#include "cartographer/mapping/proto/sparse_pose_graph.pb.h"
#include "cartographer/mapping/proto/sparse_pose_graph_options.pb.h"
#include "cartographer/mapping/submaps.h"
@ -59,9 +60,7 @@ class SparsePoseGraph {
};
mapping::SubmapId submap_id; // 'i' in the paper.
// Scan index.
int j;
mapping::NodeId node_id; // 'j' in the paper.
// Pose of the scan 'j' relative to submap 'i'.
Pose pose;

42
cartographer/mapping/submaps.h
View File

@ -14,18 +14,6 @@
* limitations under the License.
*/
// Submaps is a sequence of maps to which scans are matched and into which scans
// are inserted.
//
// 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.
//
// Once a certain number of scans 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 inserted.
#ifndef CARTOGRAPHER_MAPPING_SUBMAPS_H_
#define CARTOGRAPHER_MAPPING_SUBMAPS_H_
@ -35,6 +23,7 @@
#include "Eigen/Geometry"
#include "cartographer/common/math.h"
#include "cartographer/common/port.h"
#include "cartographer/mapping/id.h"
#include "cartographer/mapping/probability_values.h"
#include "cartographer/mapping/proto/submap_visualization.pb.h"
#include "cartographer/mapping/trajectory_node.h"
@ -63,22 +52,6 @@ inline uint8 ProbabilityToLogOddsInteger(const float probability) {
return value;
}
// Uniquely identifies a submap using a combination of a unique trajectory ID
// and a zero-based index of the submap inside that trajectory.
struct SubmapId {
int trajectory_id;
int submap_index;
bool operator<(const SubmapId& other) const {
return std::forward_as_tuple(trajectory_id, submap_index) <
std::forward_as_tuple(other.trajectory_id, other.submap_index);
}
};
inline std::ostream& operator<<(std::ostream& os, const SubmapId& v) {
return os << "(" << v.trajectory_id << ", " << v.submap_index << ")";
}
// An individual submap, which has an initial position 'origin', keeps track of
// how many range data were inserted into it, and sets the
// 'finished_probability_grid' to be used for loop closing once the map no
@ -102,7 +75,18 @@ struct Submap {
const mapping_2d::ProbabilityGrid* finished_probability_grid = nullptr;
};
// A container of Submaps.
// Submaps is a sequence of maps to which scans are matched and into which scans
// are inserted.
//
// 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.
//
// Once a certain number of scans 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 inserted.
class Submaps {
public:
static constexpr uint8 kUnknownLogOdds = 0;

19
cartographer/mapping_2d/sparse_pose_graph.cc
View File

@ -100,8 +100,11 @@ void SparsePoseGraph::AddScan(
common::MutexLocker locker(&mutex_);
trajectory_ids_.emplace(trajectory, trajectory_ids_.size());
const int trajectory_id = trajectory_ids_.at(trajectory);
const int j = trajectory_nodes_.size();
CHECK_LT(j, std::numeric_limits<int>::max());
const int flat_scan_index = trajectory_nodes_.size();
CHECK_LT(flat_scan_index, std::numeric_limits<int>::max());
scan_index_to_node_id_.push_back(
mapping::NodeId{trajectory_id, num_nodes_in_trajectory_[trajectory_id]});
++num_nodes_in_trajectory_[trajectory_id];
constant_node_data_.push_back(mapping::TrajectoryNode::ConstantData{
time, range_data_in_pose,
@ -135,7 +138,7 @@ void SparsePoseGraph::AddScan(
}
AddWorkItem([=]() REQUIRES(mutex_) {
ComputeConstraintsForScan(j, matching_submap, insertion_submaps,
ComputeConstraintsForScan(flat_scan_index, matching_submap, insertion_submaps,
finished_submap, pose, covariance);
});
}
@ -180,8 +183,9 @@ void SparsePoseGraph::ComputeConstraint(const int scan_index,
if (scan_trajectory_id != submap_trajectory_id &&
global_localization_samplers_[scan_trajectory_id]->Pulse()) {
constraint_builder_.MaybeAddGlobalConstraint(
submap_id, submap_states_[submap_index].submap, scan_index,
scan_trajectory_id, &trajectory_connectivity_,
submap_id, submap_states_[submap_index].submap,
scan_index_to_node_id_.at(scan_index), scan_index,
&trajectory_connectivity_,
&trajectory_nodes_[scan_index].constant_data->range_data_2d.returns);
} else {
const bool scan_and_submap_trajectories_connected =
@ -192,7 +196,8 @@ void SparsePoseGraph::ComputeConstraint(const int scan_index,
if (scan_trajectory_id == submap_trajectory_id ||
scan_and_submap_trajectories_connected) {
constraint_builder_.MaybeAddConstraint(
submap_id, submap_states_[submap_index].submap, scan_index,
submap_id, submap_states_[submap_index].submap,
scan_index_to_node_id_.at(scan_index), scan_index,
&trajectory_nodes_[scan_index].constant_data->range_data_2d.returns,
relative_pose);
}
@ -243,7 +248,7 @@ void SparsePoseGraph::ComputeConstraintsForScan(
constexpr double kFakeOrientationCovariance = 1e-6;
constraints_.push_back(Constraint{
submap_states_[submap_index].id,
scan_index,
scan_index_to_node_id_.at(scan_index),
{transform::Embed3D(constraint_transform),
common::ComputeSpdMatrixSqrtInverse(
kalman_filter::Embed3D(covariance, kFakePositionCovariance,

25
cartographer/mapping_2d/sparse_pose_graph.h
View File

@ -14,16 +14,6 @@
* limitations under the License.
*/
// Implements the loop closure method called Sparse Pose Adjustment (SPA) from
// Konolige, Kurt, et al. "Efficient sparse pose adjustment for 2d mapping."
// Intelligent Robots and Systems (IROS), 2010 IEEE/RSJ International Conference
// 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.
#ifndef CARTOGRAPHER_MAPPING_2D_SPARSE_POSE_GRAPH_H_
#define CARTOGRAPHER_MAPPING_2D_SPARSE_POSE_GRAPH_H_
@ -53,7 +43,15 @@
namespace cartographer {
namespace mapping_2d {
// Implements the SPA loop closure method.
// Implements the loop closure method called Sparse Pose Adjustment (SPA) from
// Konolige, Kurt, et al. "Efficient sparse pose adjustment for 2d mapping."
// Intelligent Robots and Systems (IROS), 2010 IEEE/RSJ International Conference
// 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.
class SparsePoseGraph : public mapping::SparsePoseGraph {
public:
SparsePoseGraph(const mapping::proto::SparsePoseGraphOptions& options,
@ -199,6 +197,11 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
std::vector<SubmapState> submap_states_ GUARDED_BY(mutex_);
std::map<int, int> num_submaps_in_trajectory_ GUARDED_BY(mutex_);
// Mapping to flat indices to aid the transition to per-trajectory data
// structures.
std::map<int, int> num_nodes_in_trajectory_ GUARDED_BY(mutex_);
std::vector<mapping::NodeId> scan_index_to_node_id_ GUARDED_BY(mutex_);
// Connectivity structure of our trajectories by IDs.
std::vector<std::vector<int>> connected_components_;
// Trajectory ID to connected component ID.

32
cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc
View File

@ -62,7 +62,8 @@ ConstraintBuilder::~ConstraintBuilder() {
void ConstraintBuilder::MaybeAddConstraint(
const mapping::SubmapId& submap_id, const mapping::Submap* const submap,
const int scan_index, const sensor::PointCloud* const point_cloud,
const mapping::NodeId& node_id, const int flat_scan_index,
const sensor::PointCloud* const point_cloud,
const transform::Rigid2d& initial_relative_pose) {
if (initial_relative_pose.translation().norm() >
options_.max_constraint_distance()) {
@ -70,15 +71,14 @@ void ConstraintBuilder::MaybeAddConstraint(
}
if (sampler_.Pulse()) {
common::MutexLocker locker(&mutex_);
CHECK_LE(scan_index, current_computation_);
CHECK_LE(flat_scan_index, current_computation_);
constraints_.emplace_back();
auto* const constraint = &constraints_.back();
++pending_computations_[current_computation_];
const int current_computation = current_computation_;
ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
submap_id, submap->finished_probability_grid, [=]() EXCLUDES(mutex_) {
ComputeConstraint(submap_id, submap, scan_index,
-1, /* scan_trajectory_id */
ComputeConstraint(submap_id, submap, node_id,
false, /* match_full_submap */
nullptr, /* trajectory_connectivity */
point_cloud, initial_relative_pose, constraint);
@ -89,18 +89,18 @@ void ConstraintBuilder::MaybeAddConstraint(
void ConstraintBuilder::MaybeAddGlobalConstraint(
const mapping::SubmapId& submap_id, const mapping::Submap* const submap,
const int scan_index, const int scan_trajectory_id,
const mapping::NodeId& node_id, const int flat_scan_index,
mapping::TrajectoryConnectivity* trajectory_connectivity,
const sensor::PointCloud* const point_cloud) {
common::MutexLocker locker(&mutex_);
CHECK_LE(scan_index, current_computation_);
CHECK_LE(flat_scan_index, current_computation_);
constraints_.emplace_back();
auto* const constraint = &constraints_.back();
++pending_computations_[current_computation_];
const int current_computation = current_computation_;
ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
submap_id, submap->finished_probability_grid, [=]() EXCLUDES(mutex_) {
ComputeConstraint(submap_id, submap, scan_index, scan_trajectory_id,
ComputeConstraint(submap_id, submap, node_id,
true, /* match_full_submap */
trajectory_connectivity, point_cloud,
transform::Rigid2d::Identity(), constraint);
@ -108,9 +108,9 @@ void ConstraintBuilder::MaybeAddGlobalConstraint(
});
}
void ConstraintBuilder::NotifyEndOfScan(const int scan_index) {
void ConstraintBuilder::NotifyEndOfScan(const int flat_scan_index) {
common::MutexLocker locker(&mutex_);
CHECK_EQ(current_computation_, scan_index);
CHECK_EQ(current_computation_, flat_scan_index);
++current_computation_;
}
@ -166,7 +166,7 @@ ConstraintBuilder::GetSubmapScanMatcher(const mapping::SubmapId& submap_id) {
void ConstraintBuilder::ComputeConstraint(
const mapping::SubmapId& submap_id, const mapping::Submap* const submap,
const int scan_index, const int scan_trajectory_id, bool match_full_submap,
const mapping::NodeId& node_id, bool match_full_submap,
mapping::TrajectoryConnectivity* trajectory_connectivity,
const sensor::PointCloud* const point_cloud,
const transform::Rigid2d& initial_relative_pose,
@ -191,9 +191,9 @@ void ConstraintBuilder::ComputeConstraint(
filtered_point_cloud, options_.global_localization_min_score(),
&score, &pose_estimate)) {
CHECK_GT(score, options_.global_localization_min_score());
CHECK_GE(scan_trajectory_id, 0);
CHECK_GE(node_id.trajectory_id, 0);
CHECK_GE(submap_id.trajectory_id, 0);
trajectory_connectivity->Connect(scan_trajectory_id,
trajectory_connectivity->Connect(node_id.trajectory_id,
submap_id.trajectory_id);
} else {
return;
@ -229,7 +229,7 @@ void ConstraintBuilder::ComputeConstraint(
constexpr double kFakeOrientationCovariance = 1e-6;
constraint->reset(new Constraint{
submap_id,
scan_index,
node_id,
{transform::Embed3D(constraint_transform),
common::ComputeSpdMatrixSqrtInverse(
kalman_filter::Embed3D(covariance, kFakePositionCovariance,
@ -241,9 +241,9 @@ void ConstraintBuilder::ComputeConstraint(
const transform::Rigid2d difference =
initial_pose.inverse() * pose_estimate;
std::ostringstream info;
info << "Scan index " << scan_index << " with "
<< filtered_point_cloud.size() << " points on submap " << submap_id
<< " differs by translation " << std::fixed << std::setprecision(2)
info << "Node " << node_id << " with " << filtered_point_cloud.size()
<< " points on submap " << submap_id << " differs by translation "
<< std::fixed << std::setprecision(2)
<< difference.translation().norm() << " rotation "
<< std::setprecision(3) << std::abs(difference.normalized_angle())
<< " with score " << std::setprecision(1) << 100. * score

24
cartographer/mapping_2d/sparse_pose_graph/constraint_builder.h
View File

@ -72,33 +72,37 @@ class ConstraintBuilder {
ConstraintBuilder& operator=(const ConstraintBuilder&) = delete;
// Schedules exploring a new constraint between 'submap' identified by
// 'submap_id', and the 'point_cloud' for 'scan_index'.
// 'submap_id', and the 'point_cloud' for 'flat_scan_index'.
// The 'initial_pose' is relative to the 'submap'.
//
// The pointees of 'submap' and 'point_cloud' must stay valid until all
// computations are finished.
void MaybeAddConstraint(const mapping::SubmapId& submap_id,
const mapping::Submap* submap, int scan_index,
const mapping::Submap* submap,
const mapping::NodeId& node_id,
const int flat_scan_index,
const sensor::PointCloud* point_cloud,
const transform::Rigid2d& initial_relative_pose);
// Schedules exploring a new constraint between 'submap' identified by
// 'submap_id' and the 'point_cloud' for 'scan_index'. This performs
// 'submap_id' and the 'point_cloud' for 'flat_scan_index'. This performs
// full-submap matching.
//
// The scan at 'scan_index' should be from trajectory 'scan_trajectory_id'.
// The 'trajectory_connectivity' is updated if the full-submap match succeeds.
// The scan at 'flat_scan_index' should be from trajectory
// 'node_id.trajectory_id'. The 'trajectory_connectivity' is updated if the
// full-submap match succeeds.
//
// The pointees of 'submap' and 'point_cloud' must stay valid until all
// computations are finished.
void MaybeAddGlobalConstraint(
const mapping::SubmapId& submap_id, const mapping::Submap* submap,
int scan_index, int scan_trajectory_id,
const mapping::NodeId& node_id, const int flat_scan_index,
mapping::TrajectoryConnectivity* trajectory_connectivity,
const sensor::PointCloud* point_cloud);
// Must be called after all computations related to 'scan_index' are added.
void NotifyEndOfScan(const int scan_index);
// Must be called after all computations related to 'flat_scan_index' are
// added.
void NotifyEndOfScan(const int flat_scan_index);
// Registers the 'callback' to be called with the results, after all
// computations triggered by MaybeAddConstraint() have finished.
@ -132,12 +136,12 @@ class ConstraintBuilder {
// Runs in a background thread and does computations for an additional
// constraint, assuming 'submap' and 'point_cloud' do not change anymore.
// If 'match_full_submap' is true, and global localization succeeds, will
// connect 'scan_trajectory_id' and 'submap_id.trajectory_id' in
// connect 'node_id.trajectory_id' and 'submap_id.trajectory_id' in
// 'trajectory_connectivity'.
// As output, it may create a new Constraint in 'constraint'.
void ComputeConstraint(
const mapping::SubmapId& submap_id, const mapping::Submap* submap,
int scan_index, int scan_trajectory_id, bool match_full_submap,
const mapping::NodeId& node_id, bool match_full_submap,
mapping::TrajectoryConnectivity* trajectory_connectivity,
const sensor::PointCloud* point_cloud,
const transform::Rigid2d& initial_relative_pose,

84
cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc
View File

@ -98,13 +98,20 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints) {
return;
}
// Compute the indices of consecutive nodes for each trajectory.
std::vector<std::vector<size_t>> nodes_per_trajectory(submap_data_.size());
for (size_t j = 0; j != node_data_.size(); ++j) {
nodes_per_trajectory.at(node_data_[j].trajectory_id).push_back(j);
}
ceres::Problem::Options problem_options;
ceres::Problem problem(problem_options);
// Set the starting point.
// TODO(hrapp): Move ceres data into SubmapData.
std::vector<std::deque<std::array<double, 3>>> C_submaps(submap_data_.size());
std::vector<std::array<double, 3>> C_point_clouds(node_data_.size());
std::vector<std::deque<std::array<double, 3>>> C_nodes(
nodes_per_trajectory.size());
for (size_t trajectory_id = 0; trajectory_id != submap_data_.size();
++trajectory_id) {
for (size_t submap_index = 0;
@ -123,15 +130,21 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints) {
}
}
}
for (size_t j = 0; j != node_data_.size(); ++j) {
C_point_clouds[j] = FromPose(node_data_[j].point_cloud_pose);
problem.AddParameterBlock(C_point_clouds[j].data(), 3);
for (size_t trajectory_id = 0; trajectory_id != nodes_per_trajectory.size();
++trajectory_id) {
C_nodes[trajectory_id].resize(nodes_per_trajectory[trajectory_id].size());
for (size_t node_index = 0;
node_index != nodes_per_trajectory[trajectory_id].size();
++node_index) {
C_nodes[trajectory_id][node_index] =
FromPose(node_data_[nodes_per_trajectory[trajectory_id][node_index]]
.point_cloud_pose);
problem.AddParameterBlock(C_nodes[trajectory_id][node_index].data(), 3);
}
}
// Add cost functions for intra- and inter-submap constraints.
for (const Constraint& constraint : constraints) {
CHECK_GE(constraint.j, 0);
CHECK_LT(constraint.j, node_data_.size());
problem.AddResidualBlock(
new ceres::AutoDiffCostFunction<SpaCostFunction, 3, 3, 3>(
new SpaCostFunction(constraint.pose)),
@ -142,7 +155,9 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints) {
C_submaps.at(constraint.submap_id.trajectory_id)
.at(constraint.submap_id.submap_index)
.data(),
C_point_clouds[constraint.j].data());
C_nodes.at(constraint.node_id.trajectory_id)
.at(constraint.node_id.node_index)
.data());
}
// Add penalties for changes between consecutive scans.
@ -151,32 +166,32 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints) {
options_.consecutive_scan_translation_penalty_factor(),
options_.consecutive_scan_rotation_penalty_factor());
// The poses in 'node_data_' are interleaved from multiple trajectories
// (although the points from a given trajectory are in time order).
// 'last_pose_indices[trajectory_id]' is the index of the most-recent pose on
// 'trajectory_id'.
std::map<int, int> last_pose_indices;
for (size_t j = 0; j != node_data_.size(); ++j) {
const int trajectory_id = node_data_[j].trajectory_id;
// This pose has a predecessor.
if (last_pose_indices.count(trajectory_id) != 0) {
const int last_pose_index = last_pose_indices[trajectory_id];
for (size_t trajectory_id = 0; trajectory_id != nodes_per_trajectory.size();
++trajectory_id) {
if (nodes_per_trajectory[trajectory_id].empty()) {
continue;
}
for (size_t node_index = 1;
node_index != nodes_per_trajectory[trajectory_id].size();
++node_index) {
constexpr double kUnusedPositionPenalty = 1.;
constexpr double kUnusedOrientationPenalty = 1.;
problem.AddResidualBlock(
new ceres::AutoDiffCostFunction<SpaCostFunction, 3, 3, 3>(
new SpaCostFunction(Constraint::Pose{
transform::Embed3D(node_data_[last_pose_index]
.initial_point_cloud_pose.inverse() *
node_data_[j].initial_point_cloud_pose),
kalman_filter::Embed3D(consecutive_pose_change_penalty_matrix,
kUnusedPositionPenalty,
kUnusedOrientationPenalty)})),
nullptr /* loss function */, C_point_clouds[last_pose_index].data(),
C_point_clouds[j].data());
new ceres::AutoDiffCostFunction<
SpaCostFunction, 3, 3, 3>(new SpaCostFunction(Constraint::Pose{
transform::Embed3D(
node_data_[nodes_per_trajectory[trajectory_id]
[node_index - 1]]
.initial_point_cloud_pose.inverse() *
node_data_[nodes_per_trajectory[trajectory_id][node_index]]
.initial_point_cloud_pose),
kalman_filter::Embed3D(consecutive_pose_change_penalty_matrix,
kUnusedPositionPenalty,
kUnusedOrientationPenalty)})),
nullptr /* loss function */,
C_nodes[trajectory_id][node_index - 1].data(),
C_nodes[trajectory_id][node_index].data());
}
last_pose_indices[trajectory_id] = j;
}
// Solve.
@ -198,9 +213,14 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints) {
ToPose(C_submaps[trajectory_id][submap_index]);
}
}
for (size_t j = 0; j != node_data_.size(); ++j) {
node_data_[j].point_cloud_pose = ToPose(C_point_clouds[j]);
for (size_t trajectory_id = 0; trajectory_id != nodes_per_trajectory.size();
++trajectory_id) {
for (size_t node_index = 0;
node_index != nodes_per_trajectory[trajectory_id].size();
++node_index) {
node_data_[nodes_per_trajectory[trajectory_id][node_index]]
.point_cloud_pose = ToPose(C_nodes[trajectory_id][node_index]);
}
}
}

21
cartographer/mapping_3d/sparse_pose_graph.cc
View File

@ -98,8 +98,11 @@ void SparsePoseGraph::AddScan(
common::MutexLocker locker(&mutex_);
trajectory_ids_.emplace(trajectory, trajectory_ids_.size());
const int trajectory_id = trajectory_ids_.at(trajectory);
const int j = trajectory_nodes_.size();
CHECK_LT(j, std::numeric_limits<int>::max());
const int flat_scan_index = trajectory_nodes_.size();
CHECK_LT(flat_scan_index, std::numeric_limits<int>::max());
scan_index_to_node_id_.push_back(
mapping::NodeId{trajectory_id, num_nodes_in_trajectory_[trajectory_id]});
++num_nodes_in_trajectory_[trajectory_id];
constant_node_data_.push_back(mapping::TrajectoryNode::ConstantData{
time, sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}},
@ -130,7 +133,7 @@ void SparsePoseGraph::AddScan(
}
AddWorkItem([=]() REQUIRES(mutex_) {
ComputeConstraintsForScan(j, matching_submap, insertion_submaps,
ComputeConstraintsForScan(flat_scan_index, matching_submap, insertion_submaps,
finished_submap, pose, covariance);
});
}
@ -180,8 +183,9 @@ void SparsePoseGraph::ComputeConstraint(const int scan_index,
if (scan_trajectory_id != submap_trajectory_id &&
global_localization_samplers_[scan_trajectory_id]->Pulse()) {
constraint_builder_.MaybeAddGlobalConstraint(
submap_id, submap_states_[submap_index].submap, scan_index,
scan_trajectory_id, &trajectory_connectivity_, trajectory_nodes_);
submap_id, submap_states_[submap_index].submap,
scan_index_to_node_id_.at(scan_index), scan_index,
&trajectory_connectivity_, trajectory_nodes_);
} else {
const bool scan_and_submap_trajectories_connected =
reverse_connected_components_.count(scan_trajectory_id) > 0 &&
@ -191,8 +195,9 @@ void SparsePoseGraph::ComputeConstraint(const int scan_index,
if (scan_trajectory_id == submap_trajectory_id ||
scan_and_submap_trajectories_connected) {
constraint_builder_.MaybeAddConstraint(
submap_id, submap_states_[submap_index].submap, scan_index,
trajectory_nodes_, relative_pose);
submap_id, submap_states_[submap_index].submap,
scan_index_to_node_id_.at(scan_index), scan_index, trajectory_nodes_,
relative_pose);
}
}
}
@ -238,7 +243,7 @@ void SparsePoseGraph::ComputeConstraintsForScan(
submap->local_pose().inverse() * pose;
constraints_.push_back(
Constraint{submap_states_[submap_index].id,
scan_index,
scan_index_to_node_id_.at(scan_index),
{constraint_transform,
common::ComputeSpdMatrixSqrtInverse(
covariance, options_.constraint_builder_options()

25
cartographer/mapping_3d/sparse_pose_graph.h
View File

@ -14,16 +14,6 @@
* limitations under the License.
*/
// Implements the loop closure method called Sparse Pose Adjustment (SPA) from
// Konolige, Kurt, et al. "Efficient sparse pose adjustment for 2d mapping."
// Intelligent Robots and Systems (IROS), 2010 IEEE/RSJ International Conference
// 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.
#ifndef CARTOGRAPHER_MAPPING_3D_SPARSE_POSE_GRAPH_H_
#define CARTOGRAPHER_MAPPING_3D_SPARSE_POSE_GRAPH_H_
@ -53,7 +43,15 @@
namespace cartographer {
namespace mapping_3d {
// Implements the SPA loop closure method.
// Implements the loop closure method called Sparse Pose Adjustment (SPA) from
// Konolige, Kurt, et al. "Efficient sparse pose adjustment for 2d mapping."
// Intelligent Robots and Systems (IROS), 2010 IEEE/RSJ International Conference
// 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.
class SparsePoseGraph : public mapping::SparsePoseGraph {
public:
SparsePoseGraph(const mapping::proto::SparsePoseGraphOptions& options,
@ -199,6 +197,11 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
std::vector<SubmapState> submap_states_ GUARDED_BY(mutex_);
std::map<int, int> num_submaps_in_trajectory_ GUARDED_BY(mutex_);
// Mapping to flat indices to aid the transition to per-trajectory data
// structures.
std::map<int, int> num_nodes_in_trajectory_ GUARDED_BY(mutex_);
std::vector<mapping::NodeId> scan_index_to_node_id_ GUARDED_BY(mutex_);
// Connectivity structure of our trajectories by IDs.
std::vector<std::vector<int>> connected_components_;
// Trajectory ID to connected component ID.

46
cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc
View File

@ -43,14 +43,14 @@ namespace {
std::vector<mapping::TrajectoryNode> ComputeSubmapNodes(
const std::vector<mapping::TrajectoryNode>& trajectory_nodes,
const Submap* const submap, int scan_index,
const Submap* const submap, const int flat_scan_index,
const transform::Rigid3d& initial_relative_pose) {
std::vector<mapping::TrajectoryNode> submap_nodes;
for (const int node_index : submap->trajectory_node_indices) {
submap_nodes.push_back(mapping::TrajectoryNode{
trajectory_nodes[node_index].constant_data,
transform::Rigid3d(initial_relative_pose *
trajectory_nodes[scan_index].pose.inverse() *
trajectory_nodes[flat_scan_index].pose.inverse() *
trajectory_nodes[node_index].pose)});
}
return submap_nodes;
@ -77,7 +77,7 @@ ConstraintBuilder::~ConstraintBuilder() {
void ConstraintBuilder::MaybeAddConstraint(
const mapping::SubmapId& submap_id, const Submap* const submap,
const int scan_index,
const mapping::NodeId& node_id, const int flat_scan_index,
const std::vector<mapping::TrajectoryNode>& trajectory_nodes,
const transform::Rigid3d& initial_relative_pose) {
if (initial_relative_pose.translation().norm() >
@ -86,20 +86,19 @@ void ConstraintBuilder::MaybeAddConstraint(
}
if (sampler_.Pulse()) {
const auto submap_nodes = ComputeSubmapNodes(
trajectory_nodes, submap, scan_index, initial_relative_pose);
trajectory_nodes, submap, flat_scan_index, initial_relative_pose);
common::MutexLocker locker(&mutex_);
CHECK_LE(scan_index, current_computation_);
CHECK_LE(flat_scan_index, current_computation_);
constraints_.emplace_back();
auto* const constraint = &constraints_.back();
++pending_computations_[current_computation_];
const int current_computation = current_computation_;
const auto* const point_cloud =
&trajectory_nodes[scan_index].constant_data->range_data_3d.returns;
&trajectory_nodes[flat_scan_index].constant_data->range_data_3d.returns;
ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
submap_id, submap_nodes, &submap->high_resolution_hybrid_grid,
[=]() EXCLUDES(mutex_) {
ComputeConstraint(submap_id, submap, scan_index,
-1, /* scan_trajectory_id */
ComputeConstraint(submap_id, submap, node_id,
false, /* match_full_submap */
nullptr, /* trajectory_connectivity */
point_cloud, initial_relative_pose, constraint);
@ -110,23 +109,24 @@ void ConstraintBuilder::MaybeAddConstraint(
void ConstraintBuilder::MaybeAddGlobalConstraint(
const mapping::SubmapId& submap_id, const Submap* const submap,
const int scan_index, const int scan_trajectory_id,
const mapping::NodeId& node_id, const int flat_scan_index,
mapping::TrajectoryConnectivity* trajectory_connectivity,
const std::vector<mapping::TrajectoryNode>& trajectory_nodes) {
const auto submap_nodes = ComputeSubmapNodes(
trajectory_nodes, submap, scan_index, transform::Rigid3d::Identity());
const auto submap_nodes =
ComputeSubmapNodes(trajectory_nodes, submap, flat_scan_index,
transform::Rigid3d::Identity());
common::MutexLocker locker(&mutex_);
CHECK_LE(scan_index, current_computation_);
CHECK_LE(flat_scan_index, current_computation_);
constraints_.emplace_back();
auto* const constraint = &constraints_.back();
++pending_computations_[current_computation_];
const int current_computation = current_computation_;
const auto* const point_cloud =
&trajectory_nodes[scan_index].constant_data->range_data_3d.returns;
&trajectory_nodes[flat_scan_index].constant_data->range_data_3d.returns;
ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
submap_id, submap_nodes, &submap->high_resolution_hybrid_grid,
[=]() EXCLUDES(mutex_) {
ComputeConstraint(submap_id, submap, scan_index, scan_trajectory_id,
ComputeConstraint(submap_id, submap, node_id,
true, /* match_full_submap */
trajectory_connectivity, point_cloud,
transform::Rigid3d::Identity(), constraint);
@ -134,9 +134,9 @@ void ConstraintBuilder::MaybeAddGlobalConstraint(
});
}
void ConstraintBuilder::NotifyEndOfScan(const int scan_index) {
void ConstraintBuilder::NotifyEndOfScan(const int flat_scan_index) {
common::MutexLocker locker(&mutex_);
CHECK_EQ(current_computation_, scan_index);
CHECK_EQ(current_computation_, flat_scan_index);
++current_computation_;
}
@ -197,7 +197,7 @@ ConstraintBuilder::GetSubmapScanMatcher(const mapping::SubmapId& submap_id) {
void ConstraintBuilder::ComputeConstraint(
const mapping::SubmapId& submap_id, const Submap* const submap,
const int scan_index, const int scan_trajectory_id, bool match_full_submap,
const mapping::NodeId& node_id, bool match_full_submap,
mapping::TrajectoryConnectivity* trajectory_connectivity,
const sensor::CompressedPointCloud* const compressed_point_cloud,
const transform::Rigid3d& initial_relative_pose,
@ -223,9 +223,9 @@ void ConstraintBuilder::ComputeConstraint(
initial_pose.rotation(), filtered_point_cloud, point_cloud,
options_.global_localization_min_score(), &score, &pose_estimate)) {
CHECK_GT(score, options_.global_localization_min_score());
CHECK_GE(scan_trajectory_id, 0);
CHECK_GE(node_id.trajectory_id, 0);
CHECK_GE(submap_id.trajectory_id, 0);
trajectory_connectivity->Connect(scan_trajectory_id,
trajectory_connectivity->Connect(node_id.trajectory_id,
submap_id.trajectory_id);
} else {
return;
@ -258,7 +258,7 @@ void ConstraintBuilder::ComputeConstraint(
submap->local_pose().inverse() * pose_estimate;
constraint->reset(new OptimizationProblem::Constraint{
submap_id,
scan_index,
node_id,
{constraint_transform,
1. / std::sqrt(options_.lower_covariance_eigenvalue_bound()) *
kalman_filter::PoseCovariance::Identity()},
@ -268,9 +268,9 @@ void ConstraintBuilder::ComputeConstraint(
const transform::Rigid3d difference =
initial_pose.inverse() * pose_estimate;
std::ostringstream info;
info << "Scan index " << scan_index << " with "
<< filtered_point_cloud.size() << " points on submap " << submap_id
<< " differs by translation " << std::fixed << std::setprecision(2)
info << "Node " << node_id << " with " << filtered_point_cloud.size()
<< " points on submap " << submap_id << " differs by translation "
<< std::fixed << std::setprecision(2)
<< difference.translation().norm() << " rotation "
<< std::setprecision(3) << transform::GetAngle(difference)
<< " with score " << std::setprecision(1) << 100. * score << "%.";

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

@ -69,32 +69,34 @@ class ConstraintBuilder {
// Schedules exploring a new constraint between 'submap' identified by
// 'submap_id', and the 'range_data_3d.returns' in 'trajectory_nodes' for
// 'scan_index'. The 'initial_relative_pose' is relative to the 'submap'.
// 'flat_scan_index'. The 'initial_relative_pose' is relative to the 'submap'.
//
// The pointees of 'submap' and 'range_data_3d.returns' must stay valid until
// all computations are finished.
void MaybeAddConstraint(
const mapping::SubmapId& submap_id, const Submap* submap, int scan_index,
const mapping::SubmapId& submap_id, const Submap* submap,
const mapping::NodeId& node_id, int flat_scan_index,
const std::vector<mapping::TrajectoryNode>& trajectory_nodes,
const transform::Rigid3d& initial_relative_pose);
// Schedules exploring a new constraint between 'submap' identified by
// 'submap_id' and the 'range_data_3d.returns' in 'trajectory_nodes' for
// 'scan_index'. This performs full-submap matching.
// 'flat_scan_index'. This performs full-submap matching.
//
// The scan at 'scan_index' should be from trajectory 'scan_trajectory_id'.
// The scan at 'flat_scan_index' should be from 'node_id.trajectory_id'.
// The 'trajectory_connectivity' is updated if the full-submap match succeeds.
//
// The pointees of 'submap' and 'range_data_3d.returns' must stay valid until
// all computations are finished.
void MaybeAddGlobalConstraint(
const mapping::SubmapId& submap_id, const Submap* submap, int scan_index,
int scan_trajectory_id,
const mapping::SubmapId& submap_id, const Submap* submap,
const mapping::NodeId& node_id, int flat_scan_index,
mapping::TrajectoryConnectivity* trajectory_connectivity,
const std::vector<mapping::TrajectoryNode>& trajectory_nodes);
// Must be called after all computations related to 'scan_index' are added.
void NotifyEndOfScan(int scan_index);
// Must be called after all computations related to 'flat_scan_index' are
// added.
void NotifyEndOfScan(int flat_scan_index);
// Registers the 'callback' to be called with the results, after all
// computations triggered by MaybeAddConstraint() have finished.
@ -135,8 +137,8 @@ class ConstraintBuilder {
// 'trajectory_connectivity'.
// As output, it may create a new Constraint in 'constraint'.
void ComputeConstraint(
const mapping::SubmapId& submap_id, const Submap* submap, int scan_index,
int scan_trajectory_id, bool match_full_submap,
const mapping::SubmapId& submap_id, const Submap* submap,
const mapping::NodeId& node_id, bool match_full_submap,
mapping::TrajectoryConnectivity* trajectory_connectivity,
const sensor::CompressedPointCloud* const compressed_point_cloud,
const transform::Rigid3d& initial_relative_pose,

74
cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc
View File

@ -136,7 +136,7 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints) {
CHECK(!submap_data_[0].empty());
// TODO(hrapp): Move ceres data into SubmapData.
std::vector<std::deque<CeresPose>> C_submaps(submap_data_.size());
std::deque<CeresPose> C_point_clouds;
std::vector<std::deque<CeresPose>> C_nodes(nodes_per_trajectory.size());
for (size_t trajectory_id = 0; trajectory_id != submap_data_.size();
++trajectory_id) {
for (size_t submap_index = 0;
@ -158,16 +158,21 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints) {
}
}
}
for (size_t j = 0; j != node_data_.size(); ++j) {
C_point_clouds.emplace_back(
node_data_[j].point_cloud_pose, translation_parameterization(),
common::make_unique<ceres::QuaternionParameterization>(), &problem);
for (size_t trajectory_id = 0; trajectory_id != nodes_per_trajectory.size();
++trajectory_id) {
for (size_t node_index = 0;
node_index != nodes_per_trajectory[trajectory_id].size();
++node_index) {
C_nodes[trajectory_id].emplace_back(
node_data_[nodes_per_trajectory[trajectory_id][node_index]]
.point_cloud_pose,
translation_parameterization(),
common::make_unique<ceres::QuaternionParameterization>(), &problem);
}
}
// Add cost functions for intra- and inter-submap constraints.
for (const Constraint& constraint : constraints) {
CHECK_GE(constraint.j, 0);
CHECK_LT(constraint.j, node_data_.size());
problem.AddResidualBlock(
new ceres::AutoDiffCostFunction<SpaCostFunction, 6, 4, 3, 4, 3>(
new SpaCostFunction(constraint.pose)),
@ -181,36 +186,44 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints) {
C_submaps.at(constraint.submap_id.trajectory_id)
.at(constraint.submap_id.submap_index)
.translation(),
C_point_clouds[constraint.j].rotation(),
C_point_clouds[constraint.j].translation());
C_nodes.at(constraint.node_id.trajectory_id)
.at(constraint.node_id.node_index)
.rotation(),
C_nodes.at(constraint.node_id.trajectory_id)
.at(constraint.node_id.node_index)
.translation());
}
// Add constraints based on IMU observations of angular velocities and
// linear acceleration.
for (size_t trajectory_id = 0; trajectory_id != nodes_per_trajectory.size();
++trajectory_id) {
const std::vector<size_t>& node_indices =
const std::vector<size_t>& flat_indices =
nodes_per_trajectory[trajectory_id];
const std::deque<ImuData>& imu_data = imu_data_.at(trajectory_id);
CHECK(!node_indices.empty());
CHECK(!flat_indices.empty());
CHECK(!imu_data.empty());
// Skip IMU data before the first node of this trajectory.
auto it = imu_data.cbegin();
while ((it + 1) != imu_data.cend() &&
(it + 1)->time <= node_data_[node_indices[0]].time) {
(it + 1)->time <= node_data_[flat_indices[0]].time) {
++it;
}
for (size_t j = 1; j < node_indices.size(); ++j) {
for (size_t node_index = 1; node_index < flat_indices.size();
++node_index) {
auto it2 = it;
const IntegrateImuResult<double> result =
IntegrateImu(imu_data, node_data_[node_indices[j - 1]].time,
node_data_[node_indices[j]].time, &it);
if (j + 1 < node_indices.size()) {
const common::Time first_time = node_data_[node_indices[j - 1]].time;
const common::Time second_time = node_data_[node_indices[j]].time;
const common::Time third_time = node_data_[node_indices[j + 1]].time;
IntegrateImu(imu_data, node_data_[flat_indices[node_index - 1]].time,
node_data_[flat_indices[node_index]].time, &it);
if (node_index + 1 < flat_indices.size()) {
const common::Time first_time =
node_data_[flat_indices[node_index - 1]].time;
const common::Time second_time =
node_data_[flat_indices[node_index]].time;
const common::Time third_time =
node_data_[flat_indices[node_index + 1]].time;
const common::Duration first_duration = second_time - first_time;
const common::Duration second_duration = third_time - second_time;
const common::Time first_center = first_time + first_duration / 2;
@ -235,18 +248,18 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints) {
options_.acceleration_weight(), delta_velocity,
common::ToSeconds(first_duration),
common::ToSeconds(second_duration))),
nullptr, C_point_clouds[node_indices[j]].rotation(),
C_point_clouds[node_indices[j - 1]].translation(),
C_point_clouds[node_indices[j]].translation(),
C_point_clouds[node_indices[j + 1]].translation(),
nullptr, C_nodes[trajectory_id].at(node_index).rotation(),
C_nodes[trajectory_id].at(node_index - 1).translation(),
C_nodes[trajectory_id].at(node_index).translation(),
C_nodes[trajectory_id].at(node_index + 1).translation(),
&gravity_constant_);
}
problem.AddResidualBlock(
new ceres::AutoDiffCostFunction<RotationCostFunction, 3, 4, 4>(
new RotationCostFunction(options_.rotation_weight(),
result.delta_rotation)),
nullptr, C_point_clouds[node_indices[j - 1]].rotation(),
C_point_clouds[node_indices[j]].rotation());
nullptr, C_nodes[trajectory_id].at(node_index - 1).rotation(),
C_nodes[trajectory_id].at(node_index).rotation());
}
}
@ -270,9 +283,14 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints) {
C_submaps[trajectory_id][submap_index].ToRigid();
}
}
for (size_t j = 0; j != node_data_.size(); ++j) {
node_data_[j].point_cloud_pose = C_point_clouds[j].ToRigid();
for (size_t trajectory_id = 0; trajectory_id != nodes_per_trajectory.size();
++trajectory_id) {
for (size_t node_index = 0;
node_index != nodes_per_trajectory[trajectory_id].size();
++node_index) {
node_data_[nodes_per_trajectory[trajectory_id][node_index]]
.point_cloud_pose = C_nodes[trajectory_id][node_index].ToRigid();
}
}
}

6
cartographer/mapping_3d/sparse_pose_graph/optimization_problem_test.cc
View File

@ -132,13 +132,13 @@ TEST_F(OptimizationProblemTest, ReducesNoise) {
std::vector<OptimizationProblem::Constraint> constraints;
for (int j = 0; j != kNumNodes; ++j) {
constraints.push_back(OptimizationProblem::Constraint{
mapping::SubmapId{0, 0}, j,
mapping::SubmapId{0, 0}, mapping::NodeId{0, j},
OptimizationProblem::Constraint::Pose{
AddNoise(test_data[j].ground_truth_pose, test_data[j].noise),
Eigen::Matrix<double, 6, 6>::Identity()}});
// We add an additional independent, but equally noisy observation.
constraints.push_back(OptimizationProblem::Constraint{
mapping::SubmapId{0, 1}, j,
mapping::SubmapId{0, 1}, mapping::NodeId{0, j},
OptimizationProblem::Constraint::Pose{
AddNoise(test_data[j].ground_truth_pose,
RandomYawOnlyTransform(0.2, 0.3)),
@ -146,7 +146,7 @@ TEST_F(OptimizationProblemTest, ReducesNoise) {
// We add very noisy data with high covariance (i.e. small Lambda) to verify
// it is mostly ignored.
constraints.push_back(OptimizationProblem::Constraint{
mapping::SubmapId{0, 2}, j,
mapping::SubmapId{0, 2}, mapping::NodeId{0, j},
OptimizationProblem::Constraint::Pose{
kSubmap2Transform.inverse() * test_data[j].ground_truth_pose *
RandomTransform(1e3, 3.),