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

Keep node data per trajectory in the optimization problem. (#282) 

Related to #256.

PAIR=SirVer
master
Wolfgang Hess 2017-05-15 14:09:45 +02:00 committed by GitHub
parent c084624958
commit 7d43eaa08c
7 changed files with 120 additions and 139 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

55
cartographer/mapping_2d/sparse_pose_graph.cc
View File

@ -102,9 +102,6 @@ void SparsePoseGraph::AddScan(
const int trajectory_id = trajectory_ids_.at(trajectory); const int trajectory_id = trajectory_ids_.at(trajectory);
const int flat_scan_index = trajectory_nodes_.size(); const int flat_scan_index = trajectory_nodes_.size();
CHECK_LT(flat_scan_index, std::numeric_limits<int>::max()); 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{ constant_node_data_.push_back(mapping::TrajectoryNode::ConstantData{
time, range_data_in_pose, time, range_data_in_pose,
@ -167,12 +164,15 @@ void SparsePoseGraph::AddImuData(const mapping::Submaps* trajectory,
void SparsePoseGraph::ComputeConstraint(const int scan_index, void SparsePoseGraph::ComputeConstraint(const int scan_index,
const mapping::SubmapId& submap_id) { const mapping::SubmapId& submap_id) {
const transform::Rigid2d relative_pose = const mapping::NodeId node_id = scan_index_to_node_id_.at(scan_index);
optimization_problem_.submap_data() const transform::Rigid2d relative_pose = optimization_problem_.submap_data()
.at(submap_id.trajectory_id) .at(submap_id.trajectory_id)
.at(submap_id.submap_index) .at(submap_id.submap_index)
.pose.inverse() * .pose.inverse() *
optimization_problem_.node_data().at(scan_index).point_cloud_pose; optimization_problem_.node_data()
.at(node_id.trajectory_id)
.at(node_id.node_index)
.point_cloud_pose;
const mapping::Submaps* const scan_trajectory = const mapping::Submaps* const scan_trajectory =
trajectory_nodes_[scan_index].constant_data->trajectory; trajectory_nodes_[scan_index].constant_data->trajectory;
@ -186,8 +186,7 @@ void SparsePoseGraph::ComputeConstraint(const int scan_index,
submap_states_.at(submap_id.trajectory_id) submap_states_.at(submap_id.trajectory_id)
.at(submap_id.submap_index) .at(submap_id.submap_index)
.submap, .submap,
scan_index_to_node_id_.at(scan_index), scan_index, node_id, scan_index, &trajectory_connectivity_,
&trajectory_connectivity_,
&trajectory_nodes_[scan_index].constant_data->range_data_2d.returns); &trajectory_nodes_[scan_index].constant_data->range_data_2d.returns);
} else { } else {
const bool scan_and_submap_trajectories_connected = const bool scan_and_submap_trajectories_connected =
@ -202,7 +201,7 @@ void SparsePoseGraph::ComputeConstraint(const int scan_index,
submap_states_.at(submap_id.trajectory_id) submap_states_.at(submap_id.trajectory_id)
.at(submap_id.submap_index) .at(submap_id.submap_index)
.submap, .submap,
scan_index_to_node_id_.at(scan_index), scan_index, node_id, scan_index,
&trajectory_nodes_[scan_index].constant_data->range_data_2d.returns, &trajectory_nodes_[scan_index].constant_data->range_data_2d.returns,
relative_pose); relative_pose);
} }
@ -212,14 +211,11 @@ void SparsePoseGraph::ComputeConstraint(const int scan_index,
void SparsePoseGraph::ComputeConstraintsForOldScans( void SparsePoseGraph::ComputeConstraintsForOldScans(
const mapping::Submap* submap) { const mapping::Submap* submap) {
const auto submap_id = GetSubmapId(submap); const auto submap_id = GetSubmapId(submap);
const auto& node_data = optimization_problem_.node_data(); const auto& submap_state =
CHECK_GT(node_data.size(), 0); submap_states_.at(submap_id.trajectory_id).at(submap_id.submap_index);
CHECK_LT(node_data.size(), std::numeric_limits<int>::max()); const int num_nodes = scan_index_to_node_id_.size();
const int num_nodes = node_data.size();
for (int scan_index = 0; scan_index < num_nodes; ++scan_index) { for (int scan_index = 0; scan_index < num_nodes; ++scan_index) {
if (submap_states_.at(submap_id.trajectory_id) if (submap_state.scan_indices.count(scan_index) == 0) {
.at(submap_id.submap_index)
.scan_indices.count(scan_index) == 0) {
ComputeConstraint(scan_index, submap_id); ComputeConstraint(scan_index, submap_id);
} }
} }
@ -238,12 +234,17 @@ void SparsePoseGraph::ComputeConstraintsForScan(
.at(matching_id.submap_index) .at(matching_id.submap_index)
.pose * .pose *
sparse_pose_graph::ComputeSubmapPose(*matching_submap).inverse() * pose; sparse_pose_graph::ComputeSubmapPose(*matching_submap).inverse() * pose;
CHECK_EQ(scan_index, optimization_problem_.node_data().size()); CHECK_EQ(scan_index, scan_index_to_node_id_.size());
scan_index_to_node_id_.push_back(
mapping::NodeId{matching_id.trajectory_id,
num_nodes_in_trajectory_[matching_id.trajectory_id]});
++num_nodes_in_trajectory_[matching_id.trajectory_id];
const mapping::TrajectoryNode::ConstantData* const scan_data = const mapping::TrajectoryNode::ConstantData* const scan_data =
trajectory_nodes_[scan_index].constant_data; trajectory_nodes_[scan_index].constant_data;
CHECK_EQ(trajectory_ids_.at(scan_data->trajectory),
matching_id.trajectory_id);
optimization_problem_.AddTrajectoryNode( optimization_problem_.AddTrajectoryNode(
trajectory_ids_.at(scan_data->trajectory), scan_data->time, pose, matching_id.trajectory_id, scan_data->time, pose, optimized_pose);
optimized_pose);
for (const mapping::Submap* submap : insertion_submaps) { for (const mapping::Submap* submap : insertion_submaps) {
const mapping::SubmapId submap_id = GetSubmapId(submap); const mapping::SubmapId submap_id = GetSubmapId(submap);
CHECK(!submap_states_.at(submap_id.trajectory_id) CHECK(!submap_states_.at(submap_id.trajectory_id)
@ -385,10 +386,12 @@ void SparsePoseGraph::RunOptimization() {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
const auto& node_data = optimization_problem_.node_data(); const auto& node_data = optimization_problem_.node_data();
const size_t num_optimized_poses = node_data.size(); const size_t num_optimized_poses = scan_index_to_node_id_.size();
for (size_t i = 0; i != num_optimized_poses; ++i) { for (size_t i = 0; i != num_optimized_poses; ++i) {
trajectory_nodes_[i].pose = trajectory_nodes_[i].pose =
transform::Rigid3d(transform::Embed3D(node_data[i].point_cloud_pose)); transform::Embed3D(node_data.at(scan_index_to_node_id_[i].trajectory_id)
.at(scan_index_to_node_id_[i].node_index)
.point_cloud_pose);
} }
// Extrapolate all point cloud poses that were added later. // Extrapolate all point cloud poses that were added later.
std::unordered_map<const mapping::Submaps*, transform::Rigid3d> std::unordered_map<const mapping::Submaps*, transform::Rigid3d>
@ -478,8 +481,8 @@ std::vector<transform::Rigid3d> SparsePoseGraph::ExtrapolateSubmapTransforms(
} else if (result.empty()) { } else if (result.empty()) {
result.push_back(transform::Rigid3d::Identity()); result.push_back(transform::Rigid3d::Identity());
} else { } else {
// Extrapolate to the remaining submaps. Accessing local_pose() in // Extrapolate to the remaining submaps. Accessing local_pose() in Submaps
// Submaps is okay, since the member is const. // is okay, since the member is const.
result.push_back(result.back() * result.push_back(result.back() *
submap_states_.at(trajectory_id) submap_states_.at(trajectory_id)
.at(result.size() - 1) .at(result.size() - 1)

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

@ -66,7 +66,7 @@ void OptimizationProblem::AddImuData(const int trajectory_id,
const Eigen::Vector3d& angular_velocity) { const Eigen::Vector3d& angular_velocity) {
CHECK_GE(trajectory_id, 0); CHECK_GE(trajectory_id, 0);
imu_data_.resize( imu_data_.resize(
std::max(submap_data_.size(), static_cast<size_t>(trajectory_id) + 1)); std::max(imu_data_.size(), static_cast<size_t>(trajectory_id) + 1));
imu_data_[trajectory_id].push_back( imu_data_[trajectory_id].push_back(
mapping_3d::ImuData{time, linear_acceleration, angular_velocity}); mapping_3d::ImuData{time, linear_acceleration, angular_velocity});
} }
@ -75,8 +75,11 @@ void OptimizationProblem::AddTrajectoryNode(
const int trajectory_id, const common::Time time, const int trajectory_id, const common::Time time,
const transform::Rigid2d& initial_point_cloud_pose, const transform::Rigid2d& initial_point_cloud_pose,
const transform::Rigid2d& point_cloud_pose) { const transform::Rigid2d& point_cloud_pose) {
node_data_.push_back(NodeData{trajectory_id, time, initial_point_cloud_pose, CHECK_GE(trajectory_id, 0);
point_cloud_pose}); node_data_.resize(
std::max(node_data_.size(), static_cast<size_t>(trajectory_id) + 1));
node_data_[trajectory_id].push_back(
NodeData{time, initial_point_cloud_pose, point_cloud_pose});
} }
void OptimizationProblem::AddSubmap(const int trajectory_id, void OptimizationProblem::AddSubmap(const int trajectory_id,
@ -98,20 +101,13 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints) {
return; 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::Options problem_options;
ceres::Problem problem(problem_options); ceres::Problem problem(problem_options);
// Set the starting point. // Set the starting point.
// TODO(hrapp): Move ceres data into SubmapData. // TODO(hrapp): Move ceres data into SubmapData.
std::vector<std::deque<std::array<double, 3>>> C_submaps(submap_data_.size()); std::vector<std::deque<std::array<double, 3>>> C_submaps(submap_data_.size());
std::vector<std::deque<std::array<double, 3>>> C_nodes( std::vector<std::deque<std::array<double, 3>>> C_nodes(node_data_.size());
nodes_per_trajectory.size());
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) {
for (size_t submap_index = 0; for (size_t submap_index = 0;
@ -130,15 +126,13 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints) {
} }
} }
} }
for (size_t trajectory_id = 0; trajectory_id != nodes_per_trajectory.size(); for (size_t trajectory_id = 0; trajectory_id != node_data_.size();
++trajectory_id) { ++trajectory_id) {
C_nodes[trajectory_id].resize(nodes_per_trajectory[trajectory_id].size()); C_nodes[trajectory_id].resize(node_data_[trajectory_id].size());
for (size_t node_index = 0; for (size_t node_index = 0; node_index != node_data_[trajectory_id].size();
node_index != nodes_per_trajectory[trajectory_id].size();
++node_index) { ++node_index) {
C_nodes[trajectory_id][node_index] = C_nodes[trajectory_id][node_index] =
FromPose(node_data_[nodes_per_trajectory[trajectory_id][node_index]] FromPose(node_data_[trajectory_id][node_index].point_cloud_pose);
.point_cloud_pose);
problem.AddParameterBlock(C_nodes[trajectory_id][node_index].data(), 3); problem.AddParameterBlock(C_nodes[trajectory_id][node_index].data(), 3);
} }
} }
@ -166,28 +160,22 @@ 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());
for (size_t trajectory_id = 0; trajectory_id != nodes_per_trajectory.size(); for (size_t trajectory_id = 0; trajectory_id != node_data_.size();
++trajectory_id) { ++trajectory_id) {
if (nodes_per_trajectory[trajectory_id].empty()) { for (size_t node_index = 1; node_index < node_data_[trajectory_id].size();
continue;
}
for (size_t node_index = 1;
node_index != nodes_per_trajectory[trajectory_id].size();
++node_index) { ++node_index) {
constexpr double kUnusedPositionPenalty = 1.; constexpr double kUnusedPositionPenalty = 1.;
constexpr double kUnusedOrientationPenalty = 1.; constexpr double kUnusedOrientationPenalty = 1.;
problem.AddResidualBlock( problem.AddResidualBlock(
new ceres::AutoDiffCostFunction< new ceres::AutoDiffCostFunction<SpaCostFunction, 3, 3, 3>(
SpaCostFunction, 3, 3, 3>(new SpaCostFunction(Constraint::Pose{ new SpaCostFunction(Constraint::Pose{
transform::Embed3D( transform::Embed3D(node_data_[trajectory_id][node_index - 1]
node_data_[nodes_per_trajectory[trajectory_id] .initial_point_cloud_pose.inverse() *
[node_index - 1]] node_data_[trajectory_id][node_index]
.initial_point_cloud_pose.inverse() * .initial_point_cloud_pose),
node_data_[nodes_per_trajectory[trajectory_id][node_index]] kalman_filter::Embed3D(consecutive_pose_change_penalty_matrix,
.initial_point_cloud_pose), kUnusedPositionPenalty,
kalman_filter::Embed3D(consecutive_pose_change_penalty_matrix, kUnusedOrientationPenalty)})),
kUnusedPositionPenalty,
kUnusedOrientationPenalty)})),
nullptr /* loss function */, nullptr /* loss function */,
C_nodes[trajectory_id][node_index - 1].data(), C_nodes[trajectory_id][node_index - 1].data(),
C_nodes[trajectory_id][node_index].data()); C_nodes[trajectory_id][node_index].data());
@ -213,18 +201,18 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints) {
ToPose(C_submaps[trajectory_id][submap_index]); ToPose(C_submaps[trajectory_id][submap_index]);
} }
} }
for (size_t trajectory_id = 0; trajectory_id != nodes_per_trajectory.size(); for (size_t trajectory_id = 0; trajectory_id != node_data_.size();
++trajectory_id) { ++trajectory_id) {
for (size_t node_index = 0; for (size_t node_index = 0; node_index != node_data_[trajectory_id].size();
node_index != nodes_per_trajectory[trajectory_id].size();
++node_index) { ++node_index) {
node_data_[nodes_per_trajectory[trajectory_id][node_index]] node_data_[trajectory_id][node_index].point_cloud_pose =
.point_cloud_pose = ToPose(C_nodes[trajectory_id][node_index]); ToPose(C_nodes[trajectory_id][node_index]);
} }
} }
} }
const std::vector<NodeData>& OptimizationProblem::node_data() const { const std::vector<std::vector<NodeData>>& OptimizationProblem::node_data()
const {
return node_data_; return node_data_;
} }

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

@ -18,6 +18,7 @@
#define CARTOGRAPHER_MAPPING_2D_SPARSE_POSE_GRAPH_OPTIMIZATION_PROBLEM_H_ #define CARTOGRAPHER_MAPPING_2D_SPARSE_POSE_GRAPH_OPTIMIZATION_PROBLEM_H_
#include <array> #include <array>
#include <deque>
#include <map> #include <map>
#include <vector> #include <vector>
@ -27,7 +28,6 @@
#include "cartographer/common/time.h" #include "cartographer/common/time.h"
#include "cartographer/mapping/sparse_pose_graph.h" #include "cartographer/mapping/sparse_pose_graph.h"
#include "cartographer/mapping/sparse_pose_graph/proto/optimization_problem_options.pb.h" #include "cartographer/mapping/sparse_pose_graph/proto/optimization_problem_options.pb.h"
#include "cartographer/mapping_2d/submaps.h"
#include "cartographer/mapping_3d/imu_integration.h" #include "cartographer/mapping_3d/imu_integration.h"
namespace cartographer { namespace cartographer {
@ -35,8 +35,6 @@ namespace mapping_2d {
namespace sparse_pose_graph { namespace sparse_pose_graph {
struct NodeData { struct NodeData {
// TODO(whess): Keep nodes per trajectory instead.
int trajectory_id;
common::Time time; common::Time time;
transform::Rigid2d initial_point_cloud_pose; transform::Rigid2d initial_point_cloud_pose;
transform::Rigid2d point_cloud_pose; transform::Rigid2d point_cloud_pose;
@ -72,13 +70,13 @@ class OptimizationProblem {
// Computes the optimized poses. // Computes the optimized poses.
void Solve(const std::vector<Constraint>& constraints); void Solve(const std::vector<Constraint>& constraints);
const std::vector<NodeData>& node_data() const; const std::vector<std::vector<NodeData>>& node_data() const;
const std::vector<std::vector<SubmapData>>& submap_data() const; const std::vector<std::vector<SubmapData>>& submap_data() const;
private: private:
mapping::sparse_pose_graph::proto::OptimizationProblemOptions options_; mapping::sparse_pose_graph::proto::OptimizationProblemOptions options_;
std::vector<std::deque<mapping_3d::ImuData>> imu_data_; std::vector<std::deque<mapping_3d::ImuData>> imu_data_;
std::vector<NodeData> node_data_; std::vector<std::vector<NodeData>> node_data_;
std::vector<std::vector<SubmapData>> submap_data_; std::vector<std::vector<SubmapData>> submap_data_;
}; };

56
cartographer/mapping_3d/sparse_pose_graph.cc
View File

@ -100,9 +100,6 @@ void SparsePoseGraph::AddScan(
const int trajectory_id = trajectory_ids_.at(trajectory); const int trajectory_id = trajectory_ids_.at(trajectory);
const int flat_scan_index = trajectory_nodes_.size(); const int flat_scan_index = trajectory_nodes_.size();
CHECK_LT(flat_scan_index, std::numeric_limits<int>::max()); 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{ constant_node_data_.push_back(mapping::TrajectoryNode::ConstantData{
time, sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}}, time, sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}},
@ -167,12 +164,15 @@ void SparsePoseGraph::AddImuData(const mapping::Submaps* trajectory,
void SparsePoseGraph::ComputeConstraint(const int scan_index, void SparsePoseGraph::ComputeConstraint(const int scan_index,
const mapping::SubmapId& submap_id) { const mapping::SubmapId& submap_id) {
const transform::Rigid3d relative_pose = const mapping::NodeId node_id = scan_index_to_node_id_.at(scan_index);
optimization_problem_.submap_data() const transform::Rigid3d relative_pose = optimization_problem_.submap_data()
.at(submap_id.trajectory_id) .at(submap_id.trajectory_id)
.at(submap_id.submap_index) .at(submap_id.submap_index)
.pose.inverse() * .pose.inverse() *
optimization_problem_.node_data().at(scan_index).point_cloud_pose; optimization_problem_.node_data()
.at(node_id.trajectory_id)
.at(node_id.node_index)
.point_cloud_pose;
const mapping::Submaps* const scan_trajectory = const mapping::Submaps* const scan_trajectory =
trajectory_nodes_[scan_index].constant_data->trajectory; trajectory_nodes_[scan_index].constant_data->trajectory;
@ -186,8 +186,7 @@ void SparsePoseGraph::ComputeConstraint(const int scan_index,
submap_states_.at(submap_id.trajectory_id) submap_states_.at(submap_id.trajectory_id)
.at(submap_id.submap_index) .at(submap_id.submap_index)
.submap, .submap,
scan_index_to_node_id_.at(scan_index), scan_index, node_id, scan_index, &trajectory_connectivity_, trajectory_nodes_);
&trajectory_connectivity_, trajectory_nodes_);
} else { } else {
const bool scan_and_submap_trajectories_connected = const bool scan_and_submap_trajectories_connected =
reverse_connected_components_.count(scan_trajectory_id) > 0 && reverse_connected_components_.count(scan_trajectory_id) > 0 &&
@ -201,22 +200,18 @@ void SparsePoseGraph::ComputeConstraint(const int scan_index,
submap_states_.at(submap_id.trajectory_id) submap_states_.at(submap_id.trajectory_id)
.at(submap_id.submap_index) .at(submap_id.submap_index)
.submap, .submap,
scan_index_to_node_id_.at(scan_index), scan_index, trajectory_nodes_, node_id, scan_index, trajectory_nodes_, relative_pose);
relative_pose);
} }
} }
} }
void SparsePoseGraph::ComputeConstraintsForOldScans(const Submap* submap) { void SparsePoseGraph::ComputeConstraintsForOldScans(const Submap* submap) {
const auto submap_id = GetSubmapId(submap); const auto submap_id = GetSubmapId(submap);
const auto& node_data = optimization_problem_.node_data(); const auto& submap_state =
CHECK_GT(node_data.size(), 0); submap_states_.at(submap_id.trajectory_id).at(submap_id.submap_index);
CHECK_LT(node_data.size(), std::numeric_limits<int>::max()); const int num_nodes = scan_index_to_node_id_.size();
const int num_nodes = node_data.size();
for (int scan_index = 0; scan_index < num_nodes; ++scan_index) { for (int scan_index = 0; scan_index < num_nodes; ++scan_index) {
if (submap_states_.at(submap_id.trajectory_id) if (submap_state.scan_indices.count(scan_index) == 0) {
.at(submap_id.submap_index)
.scan_indices.count(scan_index) == 0) {
ComputeConstraint(scan_index, submap_id); ComputeConstraint(scan_index, submap_id);
} }
} }
@ -235,12 +230,17 @@ void SparsePoseGraph::ComputeConstraintsForScan(
.at(matching_id.submap_index) .at(matching_id.submap_index)
.pose * .pose *
matching_submap->local_pose().inverse() * pose; matching_submap->local_pose().inverse() * pose;
CHECK_EQ(scan_index, optimization_problem_.node_data().size()); CHECK_EQ(scan_index, scan_index_to_node_id_.size());
scan_index_to_node_id_.push_back(
mapping::NodeId{matching_id.trajectory_id,
num_nodes_in_trajectory_[matching_id.trajectory_id]});
++num_nodes_in_trajectory_[matching_id.trajectory_id];
const mapping::TrajectoryNode::ConstantData* const scan_data = const mapping::TrajectoryNode::ConstantData* const scan_data =
trajectory_nodes_[scan_index].constant_data; trajectory_nodes_[scan_index].constant_data;
optimization_problem_.AddTrajectoryNode( CHECK_EQ(trajectory_ids_.at(scan_data->trajectory),
trajectory_ids_.at(scan_data->trajectory), scan_data->time, matching_id.trajectory_id);
optimized_pose); optimization_problem_.AddTrajectoryNode(matching_id.trajectory_id,
scan_data->time, optimized_pose);
for (const Submap* submap : insertion_submaps) { for (const Submap* submap : insertion_submaps) {
const mapping::SubmapId submap_id = GetSubmapId(submap); const mapping::SubmapId submap_id = GetSubmapId(submap);
CHECK(!submap_states_.at(submap_id.trajectory_id) CHECK(!submap_states_.at(submap_id.trajectory_id)
@ -378,9 +378,12 @@ void SparsePoseGraph::RunOptimization() {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
const auto& node_data = optimization_problem_.node_data(); const auto& node_data = optimization_problem_.node_data();
const size_t num_optimized_poses = node_data.size(); const size_t num_optimized_poses = scan_index_to_node_id_.size();
for (size_t i = 0; i != num_optimized_poses; ++i) { for (size_t i = 0; i != num_optimized_poses; ++i) {
trajectory_nodes_[i].pose = node_data[i].point_cloud_pose; trajectory_nodes_[i].pose =
node_data.at(scan_index_to_node_id_[i].trajectory_id)
.at(scan_index_to_node_id_[i].node_index)
.point_cloud_pose;
} }
// Extrapolate all point cloud poses that were added later. // Extrapolate all point cloud poses that were added later.
std::unordered_map<const mapping::Submaps*, transform::Rigid3d> std::unordered_map<const mapping::Submaps*, transform::Rigid3d>
@ -459,6 +462,7 @@ std::vector<transform::Rigid3d> SparsePoseGraph::ExtrapolateSubmapTransforms(
return {transform::Rigid3d::Identity()}; return {transform::Rigid3d::Identity()};
} }
// Submaps for which we have optimized poses.
std::vector<transform::Rigid3d> result; std::vector<transform::Rigid3d> result;
for (const auto& state : submap_states_.at(trajectory_id)) { for (const auto& state : submap_states_.at(trajectory_id)) {
if (trajectory_id < submap_transforms.size() && if (trajectory_id < submap_transforms.size() &&

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

@ -84,7 +84,7 @@ void OptimizationProblem::AddImuData(const int trajectory_id,
const Eigen::Vector3d& angular_velocity) { const Eigen::Vector3d& angular_velocity) {
CHECK_GE(trajectory_id, 0); CHECK_GE(trajectory_id, 0);
imu_data_.resize( imu_data_.resize(
std::max(submap_data_.size(), static_cast<size_t>(trajectory_id) + 1)); std::max(imu_data_.size(), static_cast<size_t>(trajectory_id) + 1));
imu_data_[trajectory_id].push_back( imu_data_[trajectory_id].push_back(
ImuData{time, linear_acceleration, angular_velocity}); ImuData{time, linear_acceleration, angular_velocity});
} }
@ -92,7 +92,10 @@ void OptimizationProblem::AddImuData(const int trajectory_id,
void OptimizationProblem::AddTrajectoryNode( void OptimizationProblem::AddTrajectoryNode(
const int trajectory_id, const common::Time time, const int trajectory_id, const common::Time time,
const transform::Rigid3d& point_cloud_pose) { const transform::Rigid3d& point_cloud_pose) {
node_data_.push_back(NodeData{trajectory_id, time, point_cloud_pose}); CHECK_GE(trajectory_id, 0);
node_data_.resize(
std::max(node_data_.size(), static_cast<size_t>(trajectory_id) + 1));
node_data_[trajectory_id].push_back(NodeData{time, point_cloud_pose});
} }
void OptimizationProblem::AddSubmap(const int trajectory_id, void OptimizationProblem::AddSubmap(const int trajectory_id,
@ -114,12 +117,6 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints) {
return; 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::Options problem_options;
ceres::Problem problem(problem_options); ceres::Problem problem(problem_options);
@ -136,7 +133,7 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints) {
CHECK(!submap_data_[0].empty()); CHECK(!submap_data_[0].empty());
// TODO(hrapp): Move ceres data into SubmapData. // TODO(hrapp): Move ceres data into SubmapData.
std::vector<std::deque<CeresPose>> C_submaps(submap_data_.size()); std::vector<std::deque<CeresPose>> C_submaps(submap_data_.size());
std::vector<std::deque<CeresPose>> C_nodes(nodes_per_trajectory.size()); std::vector<std::deque<CeresPose>> C_nodes(node_data_.size());
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) {
for (size_t submap_index = 0; for (size_t submap_index = 0;
@ -158,14 +155,12 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints) {
} }
} }
} }
for (size_t trajectory_id = 0; trajectory_id != nodes_per_trajectory.size(); for (size_t trajectory_id = 0; trajectory_id != node_data_.size();
++trajectory_id) { ++trajectory_id) {
for (size_t node_index = 0; for (size_t node_index = 0; node_index != node_data_[trajectory_id].size();
node_index != nodes_per_trajectory[trajectory_id].size();
++node_index) { ++node_index) {
C_nodes[trajectory_id].emplace_back( C_nodes[trajectory_id].emplace_back(
node_data_[nodes_per_trajectory[trajectory_id][node_index]] node_data_[trajectory_id][node_index].point_cloud_pose,
.point_cloud_pose,
translation_parameterization(), translation_parameterization(),
common::make_unique<ceres::QuaternionParameterization>(), &problem); common::make_unique<ceres::QuaternionParameterization>(), &problem);
} }
@ -196,34 +191,29 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints) {
// Add constraints based on IMU observations of angular velocities and // Add constraints based on IMU observations of angular velocities and
// linear acceleration. // linear acceleration.
for (size_t trajectory_id = 0; trajectory_id != nodes_per_trajectory.size(); for (size_t trajectory_id = 0; trajectory_id != node_data_.size();
++trajectory_id) { ++trajectory_id) {
const std::vector<size_t>& flat_indices =
nodes_per_trajectory[trajectory_id];
const std::deque<ImuData>& imu_data = imu_data_.at(trajectory_id); const std::deque<ImuData>& imu_data = imu_data_.at(trajectory_id);
CHECK(!flat_indices.empty());
CHECK(!imu_data.empty()); CHECK(!imu_data.empty());
// TODO(whess): Add support for empty trajectories.
const auto& node_data = node_data_[trajectory_id];
CHECK(!node_data.empty());
// Skip IMU data before the first node of this trajectory. // Skip IMU data before the first node of this trajectory.
auto it = imu_data.cbegin(); auto it = imu_data.cbegin();
while ((it + 1) != imu_data.cend() && while ((it + 1) != imu_data.cend() && (it + 1)->time <= node_data[0].time) {
(it + 1)->time <= node_data_[flat_indices[0]].time) {
++it; ++it;
} }
for (size_t node_index = 1; node_index < flat_indices.size(); for (size_t node_index = 1; node_index < node_data.size(); ++node_index) {
++node_index) {
auto it2 = it; auto it2 = it;
const IntegrateImuResult<double> result = const IntegrateImuResult<double> result =
IntegrateImu(imu_data, node_data_[flat_indices[node_index - 1]].time, IntegrateImu(imu_data, node_data[node_index - 1].time,
node_data_[flat_indices[node_index]].time, &it); node_data[node_index].time, &it);
if (node_index + 1 < flat_indices.size()) { if (node_index + 1 < node_data.size()) {
const common::Time first_time = const common::Time first_time = node_data[node_index - 1].time;
node_data_[flat_indices[node_index - 1]].time; const common::Time second_time = node_data[node_index].time;
const common::Time second_time = const common::Time third_time = node_data[node_index + 1].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 first_duration = second_time - first_time;
const common::Duration second_duration = third_time - second_time; const common::Duration second_duration = third_time - second_time;
const common::Time first_center = first_time + first_duration / 2; const common::Time first_center = first_time + first_duration / 2;
@ -283,18 +273,18 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints) {
C_submaps[trajectory_id][submap_index].ToRigid(); C_submaps[trajectory_id][submap_index].ToRigid();
} }
} }
for (size_t trajectory_id = 0; trajectory_id != nodes_per_trajectory.size(); for (size_t trajectory_id = 0; trajectory_id != node_data_.size();
++trajectory_id) { ++trajectory_id) {
for (size_t node_index = 0; for (size_t node_index = 0; node_index != node_data_[trajectory_id].size();
node_index != nodes_per_trajectory[trajectory_id].size();
++node_index) { ++node_index) {
node_data_[nodes_per_trajectory[trajectory_id][node_index]] node_data_[trajectory_id][node_index].point_cloud_pose =
.point_cloud_pose = C_nodes[trajectory_id][node_index].ToRigid(); C_nodes[trajectory_id][node_index].ToRigid();
} }
} }
} }
const std::vector<NodeData>& OptimizationProblem::node_data() const { const std::vector<std::vector<NodeData>>& OptimizationProblem::node_data()
const {
return node_data_; return node_data_;
} }

6
cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h
View File

@ -35,8 +35,6 @@ namespace mapping_3d {
namespace sparse_pose_graph { namespace sparse_pose_graph {
struct NodeData { struct NodeData {
// TODO(whess): Keep nodes per trajectory instead.
int trajectory_id;
common::Time time; common::Time time;
transform::Rigid3d point_cloud_pose; transform::Rigid3d point_cloud_pose;
}; };
@ -73,14 +71,14 @@ class OptimizationProblem {
// Computes the optimized poses. // Computes the optimized poses.
void Solve(const std::vector<Constraint>& constraints); void Solve(const std::vector<Constraint>& constraints);
const std::vector<NodeData>& node_data() const; const std::vector<std::vector<NodeData>>& node_data() const;
const std::vector<std::vector<SubmapData>>& submap_data() const; const std::vector<std::vector<SubmapData>>& submap_data() const;
private: private:
mapping::sparse_pose_graph::proto::OptimizationProblemOptions options_; mapping::sparse_pose_graph::proto::OptimizationProblemOptions options_;
FixZ fix_z_; FixZ fix_z_;
std::vector<std::deque<ImuData>> imu_data_; std::vector<std::deque<ImuData>> imu_data_;
std::vector<NodeData> node_data_; std::vector<std::vector<NodeData>> node_data_;
std::vector<std::vector<SubmapData>> submap_data_; std::vector<std::vector<SubmapData>> submap_data_;
double gravity_constant_ = 9.8; double gravity_constant_ = 9.8;
}; };

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

@ -155,7 +155,7 @@ TEST_F(OptimizationProblemTest, ReducesNoise) {
double translation_error_before = 0.; double translation_error_before = 0.;
double rotation_error_before = 0.; double rotation_error_before = 0.;
const auto& node_data = optimization_problem_.node_data(); const auto& node_data = optimization_problem_.node_data().at(0);
for (int j = 0; j != kNumNodes; ++j) { for (int j = 0; j != kNumNodes; ++j) {
translation_error_before += (test_data[j].ground_truth_pose.translation() - translation_error_before += (test_data[j].ground_truth_pose.translation() -
node_data[j].point_cloud_pose.translation()) node_data[j].point_cloud_pose.translation())