Introduces mapping::MapById for nodes in 2D. (#581)

PAIR=cschuet
master
Wolfgang Hess 2017-10-13 14:41:54 +02:00 committed by GitHub
parent 2cca91d783
commit 464d770d48
4 changed files with 66 additions and 115 deletions

View File

@ -102,7 +102,7 @@ void SparsePoseGraph::AddScan(
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
AddTrajectoryIfNeeded(trajectory_id); AddTrajectoryIfNeeded(trajectory_id);
trajectory_nodes_.Append( const mapping::NodeId node_id = trajectory_nodes_.Append(
trajectory_id, mapping::TrajectoryNode{constant_data, optimized_pose}); trajectory_id, mapping::TrajectoryNode{constant_data, optimized_pose});
++num_trajectory_nodes_; ++num_trajectory_nodes_;
@ -124,7 +124,7 @@ void SparsePoseGraph::AddScan(
// execute the lambda. // execute the lambda.
const bool newly_finished_submap = insertion_submaps.front()->finished(); const bool newly_finished_submap = insertion_submaps.front()->finished();
AddWorkItem([=]() REQUIRES(mutex_) { AddWorkItem([=]() REQUIRES(mutex_) {
ComputeConstraintsForScan(trajectory_id, insertion_submaps, ComputeConstraintsForScan(node_id, insertion_submaps,
newly_finished_submap); newly_finished_submap);
}); });
} }
@ -188,10 +188,7 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
// search window. // search window.
const transform::Rigid2d initial_relative_pose = const transform::Rigid2d initial_relative_pose =
optimization_problem_.submap_data().at(submap_id).pose.inverse() * optimization_problem_.submap_data().at(submap_id).pose.inverse() *
optimization_problem_.node_data() optimization_problem_.node_data().at(node_id).pose;
.at(node_id.trajectory_id)
.at(node_id.node_index)
.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,
trajectory_nodes_.at(node_id).constant_data.get(), trajectory_nodes_.at(node_id).constant_data.get(),
@ -206,40 +203,23 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
void SparsePoseGraph::ComputeConstraintsForOldScans( void SparsePoseGraph::ComputeConstraintsForOldScans(
const mapping::SubmapId& submap_id) { const mapping::SubmapId& submap_id) {
const auto& submap_data = submap_data_.at(submap_id); const auto& submap_data = submap_data_.at(submap_id);
const auto& node_data = optimization_problem_.node_data(); for (const auto& node_id_data : optimization_problem_.node_data()) {
for (size_t trajectory_id = 0; trajectory_id != node_data.size(); const mapping::NodeId& node_id = node_id_data.id;
++trajectory_id) {
for (const auto& index_node_data : node_data[trajectory_id]) {
const mapping::NodeId node_id{static_cast<int>(trajectory_id),
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);
} }
} }
}
} }
void SparsePoseGraph::ComputeConstraintsForScan( void SparsePoseGraph::ComputeConstraintsForScan(
const int trajectory_id, const mapping::NodeId& node_id,
std::vector<std::shared_ptr<const Submap>> insertion_submaps, std::vector<std::shared_ptr<const Submap>> insertion_submaps,
const bool newly_finished_submap) { const bool newly_finished_submap) {
const std::vector<mapping::SubmapId> submap_ids = const std::vector<mapping::SubmapId> submap_ids =
GrowSubmapTransformsAsNeeded(trajectory_id, insertion_submaps); GrowSubmapTransformsAsNeeded(node_id.trajectory_id, insertion_submaps);
CHECK_EQ(submap_ids.size(), insertion_submaps.size()); CHECK_EQ(submap_ids.size(), insertion_submaps.size());
const mapping::SubmapId matching_id = submap_ids.front(); const mapping::SubmapId matching_id = submap_ids.front();
const mapping::NodeId node_id{
matching_id.trajectory_id,
static_cast<size_t>(matching_id.trajectory_id) <
optimization_problem_.node_data().size() &&
!optimization_problem_.node_data()[matching_id.trajectory_id]
.empty()
? static_cast<int>(optimization_problem_.node_data()
.at(matching_id.trajectory_id)
.rbegin()
->first +
1)
: 0};
const auto& constant_data = trajectory_nodes_.at(node_id).constant_data; const auto& constant_data = trajectory_nodes_.at(node_id).constant_data;
const transform::Rigid2d pose = transform::Project2D( const transform::Rigid2d pose = transform::Project2D(
constant_data->initial_pose * constant_data->initial_pose *
@ -489,14 +469,15 @@ void SparsePoseGraph::RunOptimization() {
const auto& submap_data = optimization_problem_.submap_data(); const auto& submap_data = optimization_problem_.submap_data();
const auto& node_data = optimization_problem_.node_data(); const auto& node_data = optimization_problem_.node_data();
for (int trajectory_id = 0; for (auto node_it = node_data.begin(); node_it != node_data.end();) {
trajectory_id != static_cast<int>(node_data.size()); ++trajectory_id) { const int trajectory_id = node_it->id.trajectory_id;
const auto trajectory_end = node_data.EndOfTrajectory(trajectory_id);
const int num_nodes = trajectory_nodes_.num_indices(trajectory_id); const int num_nodes = trajectory_nodes_.num_indices(trajectory_id);
for (const auto& node_data_index : node_data.at(trajectory_id)) { for (; node_it != trajectory_end; ++node_it) {
const mapping::NodeId node_id{trajectory_id, node_data_index.first}; const mapping::NodeId node_id = node_it->id;
auto& node = trajectory_nodes_.at(node_id); auto& node = trajectory_nodes_.at(node_id);
node.pose = node.pose =
transform::Embed3D(node_data_index.second.pose) * transform::Embed3D(node_it->data.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.
@ -506,10 +487,7 @@ 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();
int last_optimized_node_index = const int last_optimized_node_index = std::prev(node_it)->id.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; for (int node_index = last_optimized_node_index + 1; node_index < num_nodes;
++node_index) { ++node_index) {
const mapping::NodeId node_id{trajectory_id, node_index}; const mapping::NodeId node_id{trajectory_id, node_index};

View File

@ -134,7 +134,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
// Adds constraints for a scan, and starts scan matching in the background. // Adds constraints for a scan, and starts scan matching in the background.
void ComputeConstraintsForScan( void ComputeConstraintsForScan(
int trajectory_id, const mapping::NodeId& node_id,
std::vector<std::shared_ptr<const Submap>> insertion_submaps, std::vector<std::shared_ptr<const Submap>> insertion_submaps,
bool newly_finished_submap) REQUIRES(mutex_); bool newly_finished_submap) REQUIRES(mutex_);

View File

@ -82,25 +82,19 @@ void OptimizationProblem::AddTrajectoryNode(
const int trajectory_id, const common::Time time, const int trajectory_id, const common::Time time,
const transform::Rigid2d& initial_pose, const transform::Rigid2d& pose, const transform::Rigid2d& initial_pose, const transform::Rigid2d& pose,
const Eigen::Quaterniond& gravity_alignment) { const Eigen::Quaterniond& gravity_alignment) {
CHECK_GE(trajectory_id, 0); node_data_.Append(trajectory_id,
node_data_.resize(
std::max(node_data_.size(), static_cast<size_t>(trajectory_id) + 1));
trajectory_data_.resize(std::max(trajectory_data_.size(), node_data_.size()));
auto& trajectory_data = trajectory_data_[trajectory_id];
node_data_[trajectory_id].emplace(
trajectory_data.next_node_index,
NodeData{time, initial_pose, pose, gravity_alignment}); NodeData{time, initial_pose, pose, gravity_alignment});
++trajectory_data.next_node_index;
} }
void OptimizationProblem::TrimTrajectoryNode(const mapping::NodeId& node_id) { void OptimizationProblem::TrimTrajectoryNode(const mapping::NodeId& node_id) {
auto& node_data = node_data_.at(node_id.trajectory_id); node_data_.Trim(node_id);
CHECK(node_data.erase(node_id.node_index));
if (!node_data.empty() && const int trajectory_id = node_id.trajectory_id;
node_id.trajectory_id < static_cast<int>(imu_data_.size())) { if (node_data_.SizeOfTrajectoryOrZero(trajectory_id) == 0 &&
const common::Time node_time = node_data.begin()->second.time; trajectory_id < static_cast<int>(imu_data_.size())) {
auto& imu_data = imu_data_.at(node_id.trajectory_id); const common::Time node_time =
node_data_.BeginOfTrajectory(trajectory_id)->data.time;
auto& imu_data = imu_data_.at(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();
} }
@ -134,7 +128,7 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
// Set the starting point. // Set the starting point.
// TODO(hrapp): Move ceres data into SubmapData. // TODO(hrapp): Move ceres data into SubmapData.
mapping::MapById<mapping::SubmapId, std::array<double, 3>> C_submaps; mapping::MapById<mapping::SubmapId, std::array<double, 3>> C_submaps;
std::vector<std::map<int, std::array<double, 3>>> C_nodes(node_data_.size()); mapping::MapById<mapping::NodeId, std::array<double, 3>> C_nodes;
bool first_submap = true; bool first_submap = true;
for (const auto& submap_id_data : submap_data_) { for (const auto& submap_id_data : submap_data_) {
const bool frozen = const bool frozen =
@ -148,19 +142,13 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
problem.SetParameterBlockConstant(C_submaps.at(submap_id_data.id).data()); problem.SetParameterBlockConstant(C_submaps.at(submap_id_data.id).data());
} }
} }
for (size_t trajectory_id = 0; trajectory_id != node_data_.size(); for (const auto& node_id_data : node_data_) {
++trajectory_id) { const bool frozen =
const bool frozen = frozen_trajectories.count(trajectory_id) != 0; frozen_trajectories.count(node_id_data.id.trajectory_id) != 0;
for (const auto& index_node_data : node_data_[trajectory_id]) { C_nodes.Insert(node_id_data.id, FromPose(node_id_data.data.pose));
const int node_index = index_node_data.first; problem.AddParameterBlock(C_nodes.at(node_id_data.id).data(), 3);
const NodeData& node_data = index_node_data.second;
C_nodes[trajectory_id].emplace(node_index, FromPose(node_data.pose));
problem.AddParameterBlock(C_nodes[trajectory_id].at(node_index).data(),
3);
if (frozen) { if (frozen) {
problem.SetParameterBlockConstant( problem.SetParameterBlockConstant(C_nodes.at(node_id_data.id).data());
C_nodes[trajectory_id].at(node_index).data());
}
} }
} }
// Add cost functions for intra- and inter-submap constraints. // Add cost functions for intra- and inter-submap constraints.
@ -173,58 +161,52 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
? new ceres::HuberLoss(options_.huber_scale()) ? new ceres::HuberLoss(options_.huber_scale())
: nullptr, : nullptr,
C_submaps.at(constraint.submap_id).data(), C_submaps.at(constraint.submap_id).data(),
C_nodes.at(constraint.node_id.trajectory_id) C_nodes.at(constraint.node_id).data());
.at(constraint.node_id.node_index)
.data());
} }
// Add penalties for violating odometry or changes between consecutive scans // Add penalties for violating odometry or changes between consecutive scans
// if odometry is not available. // if odometry is not available.
for (size_t trajectory_id = 0; trajectory_id != node_data_.size(); for (auto node_it = node_data_.begin(); node_it != node_data_.end();) {
++trajectory_id) { const int trajectory_id = node_it->id.trajectory_id;
if (node_data_[trajectory_id].empty()) { const auto trajectory_end = node_data_.EndOfTrajectory(trajectory_id);
continue;
}
for (auto node_it = node_data_[trajectory_id].begin();;) { auto prev_node_it = node_it;
const int node_index = node_it->first; for (++node_it; node_it != trajectory_end; ++node_it) {
const NodeData& node_data = node_it->second; const mapping::NodeId first_node_id = prev_node_it->id;
++node_it; const NodeData& first_node_data = prev_node_it->data;
if (node_it == node_data_[trajectory_id].end()) { prev_node_it = node_it;
break; const mapping::NodeId second_node_id = node_it->id;
} const NodeData& second_node_data = node_it->data;
const int next_node_index = node_it->first; if (second_node_id.node_index != first_node_id.node_index + 1) {
const NodeData& next_node_data = node_it->second;
if (next_node_index != node_index + 1) {
continue; continue;
} }
const bool odometry_available = const bool odometry_available =
trajectory_id < odometry_data_.size() && trajectory_id < static_cast<int>(odometry_data_.size()) &&
odometry_data_[trajectory_id].Has(next_node_data.time) && odometry_data_[trajectory_id].Has(second_node_data.time) &&
odometry_data_[trajectory_id].Has(node_data.time); odometry_data_[trajectory_id].Has(first_node_data.time);
const transform::Rigid3d relative_pose = const transform::Rigid3d relative_pose =
odometry_available odometry_available
? transform::Rigid3d::Rotation(node_data.gravity_alignment) * ? transform::Rigid3d::Rotation(
first_node_data.gravity_alignment) *
odometry_data_[trajectory_id] odometry_data_[trajectory_id]
.Lookup(node_data.time) .Lookup(first_node_data.time)
.inverse() * .inverse() *
odometry_data_[trajectory_id].Lookup(next_node_data.time) * odometry_data_[trajectory_id].Lookup(
second_node_data.time) *
transform::Rigid3d::Rotation( transform::Rigid3d::Rotation(
next_node_data.gravity_alignment.inverse()) second_node_data.gravity_alignment.inverse())
: transform::Embed3D(node_data.initial_pose.inverse() * : transform::Embed3D(first_node_data.initial_pose.inverse() *
next_node_data.initial_pose); second_node_data.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{
relative_pose, relative_pose,
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.at(first_node_id).data(),
C_nodes[trajectory_id].at(node_index).data(), C_nodes.at(second_node_id).data());
C_nodes[trajectory_id].at(next_node_index).data());
} }
} }
@ -241,17 +223,13 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
for (const auto& C_submap_id_data : C_submaps) { for (const auto& C_submap_id_data : C_submaps) {
submap_data_.at(C_submap_id_data.id).pose = ToPose(C_submap_id_data.data); submap_data_.at(C_submap_id_data.id).pose = ToPose(C_submap_id_data.data);
} }
for (size_t trajectory_id = 0; trajectory_id != node_data_.size(); for (const auto& C_node_id_data : C_nodes) {
++trajectory_id) { node_data_.at(C_node_id_data.id).pose = ToPose(C_node_id_data.data);
for (auto& index_node_data : node_data_[trajectory_id]) {
index_node_data.second.pose =
ToPose(C_nodes[trajectory_id].at(index_node_data.first));
}
} }
} }
const std::vector<std::map<int, NodeData>>& OptimizationProblem::node_data() const mapping::MapById<mapping::NodeId, NodeData>&
const { OptimizationProblem::node_data() const {
return node_data_; return node_data_;
} }

View File

@ -79,20 +79,15 @@ 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::map<int, NodeData>>& node_data() const; const mapping::MapById<mapping::NodeId, NodeData>& node_data() const;
const mapping::MapById<mapping::SubmapId, SubmapData>& submap_data() const; const mapping::MapById<mapping::SubmapId, SubmapData>& submap_data() const;
private: private:
struct TrajectoryData {
// TODO(hrapp): Remove, once we can relabel constraints.
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::map<int, NodeData>> node_data_; mapping::MapById<mapping::NodeId, NodeData> node_data_;
std::vector<transform::TransformInterpolationBuffer> odometry_data_; std::vector<transform::TransformInterpolationBuffer> odometry_data_;
mapping::MapById<mapping::SubmapId, SubmapData> submap_data_; mapping::MapById<mapping::SubmapId, SubmapData> submap_data_;
std::vector<TrajectoryData> trajectory_data_;
}; };
} // namespace sparse_pose_graph } // namespace sparse_pose_graph