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

Use vector<map<>> for node_data. (#472)

master
Jihoon Lee 2017-09-04 18:01:44 +02:00 committed by Wolfgang Hess
parent c8de50bd2b
commit 18d8ea75fa
3 changed files with 82 additions and 82 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

44
cartographer/mapping_2d/sparse_pose_graph.cc
View File

@ -195,8 +195,7 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
.pose.inverse() * .pose.inverse() *
optimization_problem_.node_data() optimization_problem_.node_data()
.at(node_id.trajectory_id) .at(node_id.trajectory_id)
.at(node_id.node_index - optimization_problem_.num_trimmed_nodes( .at(node_id.node_index)
node_id.trajectory_id))
.pose; .pose;
constraint_builder_.MaybeAddConstraint( constraint_builder_.MaybeAddConstraint(
submap_id, submap_data_.at(submap_id).submap.get(), node_id, submap_id, submap_data_.at(submap_id).submap.get(), node_id,
@ -212,14 +211,9 @@ void SparsePoseGraph::ComputeConstraintsForOldScans(
const auto& node_data = optimization_problem_.node_data(); const auto& node_data = optimization_problem_.node_data();
for (size_t trajectory_id = 0; trajectory_id != node_data.size(); for (size_t trajectory_id = 0; trajectory_id != node_data.size();
++trajectory_id) { ++trajectory_id) {
for (size_t node_data_index = 0; for (const auto& index_node_data : node_data[trajectory_id]) {
node_data_index != node_data[trajectory_id].size();
++node_data_index) {
const int node_index =
node_data_index +
optimization_problem_.num_trimmed_nodes(trajectory_id);
const mapping::NodeId node_id{static_cast<int>(trajectory_id), const mapping::NodeId node_id{static_cast<int>(trajectory_id),
static_cast<int>(node_index)}; index_node_data.first};
CHECK(!trajectory_nodes_.at(node_id).trimmed()); CHECK(!trajectory_nodes_.at(node_id).trimmed());
if (submap_data.node_ids.count(node_id) == 0) { if (submap_data.node_ids.count(node_id) == 0) {
ComputeConstraint(node_id, submap_id); ComputeConstraint(node_id, submap_id);
@ -247,12 +241,14 @@ void SparsePoseGraph::ComputeConstraintsForScan(
const mapping::NodeId node_id{ const mapping::NodeId node_id{
matching_id.trajectory_id, matching_id.trajectory_id,
static_cast<size_t>(matching_id.trajectory_id) < static_cast<size_t>(matching_id.trajectory_id) <
optimization_problem_.node_data().size() optimization_problem_.node_data().size() &&
!optimization_problem_.node_data()[matching_id.trajectory_id]
.empty()
? static_cast<int>(optimization_problem_.node_data() ? static_cast<int>(optimization_problem_.node_data()
.at(matching_id.trajectory_id) .at(matching_id.trajectory_id)
.size()) + .rbegin()
optimization_problem_.num_trimmed_nodes( ->first +
matching_id.trajectory_id) 1)
: 0}; : 0};
const auto& scan_data = trajectory_nodes_.at(node_id).constant_data; const auto& scan_data = trajectory_nodes_.at(node_id).constant_data;
optimization_problem_.AddTrajectoryNode( optimization_problem_.AddTrajectoryNode(
@ -349,16 +345,14 @@ void SparsePoseGraph::WaitForAllComputations() {
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_.GetNumFinishedScans() -
num_finished_scans_at_start) / num_finished_scans_at_start) /
(num_trajectory_nodes_ - num_finished_scans_at_start) (num_trajectory_nodes_ - num_finished_scans_at_start)
<< "%..."; << "%...";
std::cout << "\r\x1b[K" << progress_info.str() << std::flush; std::cout << "\r\x1b[K" << progress_info.str() << std::flush;
} }
std::cout << "\r\x1b[KOptimizing: Done. " << std::endl; std::cout << "\r\x1b[KOptimizing: Done. " << std::endl;
constraint_builder_.WhenDone( constraint_builder_.WhenDone([this, &notification](
[this, &notification](
const sparse_pose_graph::ConstraintBuilder::Result& result) { const sparse_pose_graph::ConstraintBuilder::Result& result) {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
constraints_.insert(constraints_.end(), result.begin(), result.end()); constraints_.insert(constraints_.end(), result.begin(), result.end());
@ -441,15 +435,12 @@ void SparsePoseGraph::RunOptimization() {
const auto& node_data = optimization_problem_.node_data(); const auto& node_data = optimization_problem_.node_data();
for (int trajectory_id = 0; for (int trajectory_id = 0;
trajectory_id != static_cast<int>(node_data.size()); ++trajectory_id) { trajectory_id != static_cast<int>(node_data.size()); ++trajectory_id) {
int node_data_index = 0;
const int num_nodes = trajectory_nodes_.num_indices(trajectory_id); const int num_nodes = trajectory_nodes_.num_indices(trajectory_id);
int node_index = optimization_problem_.num_trimmed_nodes(trajectory_id); for (const auto& node_data_index : node_data.at(trajectory_id)) {
for (; node_data_index != static_cast<int>(node_data[trajectory_id].size()); const mapping::NodeId node_id{trajectory_id, node_data_index.first};
++node_data_index, ++node_index) {
const mapping::NodeId node_id{trajectory_id, node_index};
auto& node = trajectory_nodes_.at(node_id); auto& node = trajectory_nodes_.at(node_id);
node.pose = node.pose =
transform::Embed3D(node_data[trajectory_id][node_data_index].pose) * transform::Embed3D(node_data_index.second.pose) *
transform::Rigid3d::Rotation(node.constant_data->gravity_alignment); transform::Rigid3d::Rotation(node.constant_data->gravity_alignment);
} }
// Extrapolate all point cloud poses that were added later. // Extrapolate all point cloud poses that were added later.
@ -459,7 +450,12 @@ void SparsePoseGraph::RunOptimization() {
optimized_submap_transforms_, trajectory_id); optimized_submap_transforms_, trajectory_id);
const transform::Rigid3d old_global_to_new_global = const transform::Rigid3d old_global_to_new_global =
local_to_new_global * local_to_old_global.inverse(); local_to_new_global * local_to_old_global.inverse();
for (; node_index < num_nodes; ++node_index) { int last_optimized_node_index =
node_data.at(trajectory_id).empty()
? 0
: node_data.at(trajectory_id).rbegin()->first;
for (int node_index = last_optimized_node_index + 1; node_index < num_nodes;
++node_index) {
const mapping::NodeId node_id{trajectory_id, node_index}; const mapping::NodeId node_id{trajectory_id, node_index};
auto& node_pose = trajectory_nodes_.at(node_id).pose; auto& node_pose = trajectory_nodes_.at(node_id).pose;
node_pose = old_global_to_new_global * node_pose; node_pose = old_global_to_new_global * node_pose;

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

@ -84,25 +84,28 @@ void OptimizationProblem::AddTrajectoryNode(
CHECK_GE(trajectory_id, 0); CHECK_GE(trajectory_id, 0);
node_data_.resize( node_data_.resize(
std::max(node_data_.size(), static_cast<size_t>(trajectory_id) + 1)); std::max(node_data_.size(), static_cast<size_t>(trajectory_id) + 1));
node_data_[trajectory_id].push_back(NodeData{time, initial_pose, pose});
trajectory_data_.resize(std::max(trajectory_data_.size(), node_data_.size())); trajectory_data_.resize(std::max(trajectory_data_.size(), node_data_.size()));
auto& trajectory_data = trajectory_data_.at(trajectory_id);
node_data_[trajectory_id].emplace(trajectory_data.next_node_index,
NodeData{time, initial_pose, pose});
++trajectory_data.next_node_index;
} }
void OptimizationProblem::TrimTrajectoryNode(const mapping::NodeId& node_id) { void OptimizationProblem::TrimTrajectoryNode(const mapping::NodeId& node_id) {
auto& trajectory_data = trajectory_data_.at(node_id.trajectory_id);
// We only allow trimming from the start.
CHECK_EQ(trajectory_data.num_trimmed_nodes, node_id.node_index);
auto& node_data = node_data_.at(node_id.trajectory_id); auto& node_data = node_data_.at(node_id.trajectory_id);
CHECK(!node_data.empty()); CHECK(node_data.erase(node_id.node_index));
if (node_id.trajectory_id < static_cast<int>(imu_data_.size())) {
const common::Time node_time = node_data.front().time; if (!node_data.empty() &&
node_id.trajectory_id < static_cast<int>(imu_data_.size())) {
auto node_it = node_data.begin();
const common::Time node_time = node_it->second.time;
auto& imu_data = imu_data_.at(node_id.trajectory_id); auto& imu_data = imu_data_.at(node_id.trajectory_id);
while (imu_data.size() > 1 && imu_data[1].time <= node_time) { while (imu_data.size() > 1 && imu_data[1].time <= node_time) {
imu_data.pop_front(); imu_data.pop_front();
} }
} }
node_data.pop_front();
++trajectory_data.num_trimmed_nodes;
} }
void OptimizationProblem::AddSubmap(const int trajectory_id, void OptimizationProblem::AddSubmap(const int trajectory_id,
@ -143,7 +146,7 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
// TODO(hrapp): Move ceres data into SubmapData. // TODO(hrapp): Move ceres data into SubmapData.
std::vector<std::map<int, std::array<double, 3>>> C_submaps( std::vector<std::map<int, std::array<double, 3>>> C_submaps(
submap_data_.size()); submap_data_.size());
std::vector<std::vector<std::array<double, 3>>> C_nodes(node_data_.size()); std::vector<std::map<int, std::array<double, 3>>> C_nodes(node_data_.size());
bool first_submap = true; bool first_submap = true;
for (size_t trajectory_id = 0; trajectory_id != submap_data_.size(); for (size_t trajectory_id = 0; trajectory_id != submap_data_.size();
++trajectory_id) { ++trajectory_id) {
@ -168,14 +171,15 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
for (size_t trajectory_id = 0; trajectory_id != node_data_.size(); for (size_t trajectory_id = 0; trajectory_id != node_data_.size();
++trajectory_id) { ++trajectory_id) {
const bool frozen = frozen_trajectories.count(trajectory_id); const bool frozen = frozen_trajectories.count(trajectory_id);
// Reserve guarantees that data does not move, so the pointers for Ceres for (const auto& index_node_data : node_data_[trajectory_id]) {
// stay valid. const int node_index = index_node_data.first;
C_nodes[trajectory_id].reserve(node_data_[trajectory_id].size()); const NodeData& node_data = index_node_data.second;
for (const NodeData& node_data : node_data_[trajectory_id]) { C_nodes[trajectory_id].emplace(node_index, FromPose(node_data.pose));
C_nodes[trajectory_id].push_back(FromPose(node_data.pose)); problem.AddParameterBlock(C_nodes[trajectory_id].at(node_index).data(),
problem.AddParameterBlock(C_nodes[trajectory_id].back().data(), 3); 3);
if (frozen) { if (frozen) {
problem.SetParameterBlockConstant(C_nodes[trajectory_id].back().data()); problem.SetParameterBlockConstant(
C_nodes[trajectory_id].at(node_index).data());
} }
} }
} }
@ -192,9 +196,7 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
.at(constraint.submap_id.submap_index) .at(constraint.submap_id.submap_index)
.data(), .data(),
C_nodes.at(constraint.node_id.trajectory_id) C_nodes.at(constraint.node_id.trajectory_id)
.at(constraint.node_id.node_index - .at(constraint.node_id.node_index)
trajectory_data_.at(constraint.node_id.trajectory_id)
.num_trimmed_nodes)
.data()); .data());
} }
@ -202,27 +204,37 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
// if odometry is not available. // if odometry is not available.
for (size_t trajectory_id = 0; trajectory_id != node_data_.size(); for (size_t trajectory_id = 0; trajectory_id != node_data_.size();
++trajectory_id) { ++trajectory_id) {
for (size_t node_data_index = 1; if (node_data_[trajectory_id].empty()) {
node_data_index < node_data_[trajectory_id].size(); continue;
++node_data_index) { }
for (auto node_it = node_data_[trajectory_id].begin();;) {
const int node_index = node_it->first;
const NodeData& node_data = node_it->second;
++node_it;
if (node_it == node_data_[trajectory_id].end()) {
break;
}
const int next_node_index = node_it->first;
const NodeData& next_node_data = node_it->second;
if (next_node_index != node_index + 1) {
continue;
}
const bool odometry_available = const bool odometry_available =
trajectory_id < odometry_data_.size() && trajectory_id < odometry_data_.size() &&
odometry_data_[trajectory_id].Has( odometry_data_[trajectory_id].Has(
node_data_[trajectory_id][node_data_index].time) && node_data_[trajectory_id][next_node_index].time) &&
odometry_data_[trajectory_id].Has( odometry_data_[trajectory_id].Has(
node_data_[trajectory_id][node_data_index - 1].time); node_data_[trajectory_id][node_index].time);
const transform::Rigid3d relative_pose = const transform::Rigid3d relative_pose =
odometry_available odometry_available
? odometry_data_[trajectory_id] ? odometry_data_[trajectory_id].Lookup(node_data.time).inverse() *
.Lookup( odometry_data_[trajectory_id].Lookup(next_node_data.time)
node_data_[trajectory_id][node_data_index - 1].time) : transform::Embed3D(node_data.initial_pose.inverse() *
.inverse() * next_node_data.initial_pose);
odometry_data_[trajectory_id].Lookup(
node_data_[trajectory_id][node_data_index].time)
: transform::Embed3D(
node_data_[trajectory_id][node_data_index - 1]
.initial_pose.inverse() *
node_data_[trajectory_id][node_data_index].initial_pose);
problem.AddResidualBlock( problem.AddResidualBlock(
new ceres::AutoDiffCostFunction<SpaCostFunction, 3, 3, 3>( new ceres::AutoDiffCostFunction<SpaCostFunction, 3, 3, 3>(
new SpaCostFunction(Constraint::Pose{ new SpaCostFunction(Constraint::Pose{
@ -230,8 +242,8 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
options_.consecutive_scan_translation_penalty_factor(), options_.consecutive_scan_translation_penalty_factor(),
options_.consecutive_scan_rotation_penalty_factor()})), options_.consecutive_scan_rotation_penalty_factor()})),
nullptr /* loss function */, nullptr /* loss function */,
C_nodes[trajectory_id][node_data_index - 1].data(), C_nodes[trajectory_id][node_index].data(),
C_nodes[trajectory_id][node_data_index].data()); C_nodes[trajectory_id][next_node_index].data());
} }
} }
@ -254,16 +266,14 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
} }
for (size_t trajectory_id = 0; trajectory_id != node_data_.size(); for (size_t trajectory_id = 0; trajectory_id != node_data_.size();
++trajectory_id) { ++trajectory_id) {
for (size_t node_data_index = 0; for (auto& index_node_data : node_data_[trajectory_id]) {
node_data_index != node_data_[trajectory_id].size(); index_node_data.second.pose =
++node_data_index) { ToPose(C_nodes[trajectory_id].at(index_node_data.first));
node_data_[trajectory_id][node_data_index].pose =
ToPose(C_nodes[trajectory_id][node_data_index]);
} }
} }
} }
const std::vector<std::deque<NodeData>>& OptimizationProblem::node_data() const std::vector<std::map<int, NodeData>>& OptimizationProblem::node_data()
const { const {
return node_data_; return node_data_;
} }
@ -273,10 +283,6 @@ const std::vector<std::map<int, SubmapData>>& OptimizationProblem::submap_data()
return submap_data_; return submap_data_;
} }
int OptimizationProblem::num_trimmed_nodes(int trajectory_id) const {
return trajectory_data_.at(trajectory_id).num_trimmed_nodes;
}
} // namespace sparse_pose_graph } // namespace sparse_pose_graph
} // namespace mapping_2d } // namespace mapping_2d
} // namespace cartographer } // namespace cartographer

8
cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h
View File

@ -76,20 +76,18 @@ class OptimizationProblem {
void Solve(const std::vector<Constraint>& constraints, void Solve(const std::vector<Constraint>& constraints,
const std::set<int>& frozen_trajectories); const std::set<int>& frozen_trajectories);
const std::vector<std::deque<NodeData>>& node_data() const; const std::vector<std::map<int, NodeData>>& node_data() const;
const std::vector<std::map<int, SubmapData>>& submap_data() const; const std::vector<std::map<int, SubmapData>>& submap_data() const;
int num_trimmed_nodes(int trajectory_id) const;
private: private:
struct TrajectoryData { struct TrajectoryData {
// TODO(hrapp): Remove, once we can relabel constraints. // TODO(hrapp): Remove, once we can relabel constraints.
int next_submap_index = 0; int next_submap_index = 0;
int num_trimmed_nodes = 0; int next_node_index = 0;
}; };
mapping::sparse_pose_graph::proto::OptimizationProblemOptions options_; mapping::sparse_pose_graph::proto::OptimizationProblemOptions options_;
std::vector<std::deque<sensor::ImuData>> imu_data_; std::vector<std::deque<sensor::ImuData>> imu_data_;
std::vector<std::deque<NodeData>> node_data_; std::vector<std::map<int, NodeData>> node_data_;
std::vector<transform::TransformInterpolationBuffer> odometry_data_; std::vector<transform::TransformInterpolationBuffer> odometry_data_;
std::vector<std::map<int, SubmapData>> submap_data_; std::vector<std::map<int, SubmapData>> submap_data_;
std::vector<TrajectoryData> trajectory_data_; std::vector<TrajectoryData> trajectory_data_;