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

`submap_pose`s are now owned by the OptimizationProblem. (#269)

master
Holger Rapp 2017-05-09 17:24:48 +02:00 committed by GitHub
parent 20e9cde53d
commit 524b613f2c
9 changed files with 193 additions and 139 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

65
cartographer/mapping_2d/sparse_pose_graph.cc
View File

@ -52,31 +52,37 @@ SparsePoseGraph::~SparsePoseGraph() {
}
void SparsePoseGraph::GrowSubmapTransformsAsNeeded(
const std::vector<const mapping::Submap*>& submaps) {
CHECK(!submaps.empty());
CHECK_LT(submap_transforms_.size(), std::numeric_limits<int>::max());
const int next_transform_index = submap_transforms_.size();
const std::vector<const mapping::Submap*>& insertion_submaps) {
CHECK(!insertion_submaps.empty());
CHECK_LT(optimization_problem_.submap_data().size(),
std::numeric_limits<int>::max());
const int next_submap_index = optimization_problem_.submap_data().size();
// Verify that we have an index for the first submap.
const int first_transform_index = GetSubmapIndex(submaps[0]);
if (submaps.size() == 1) {
const int first_submap_index = GetSubmapIndex(insertion_submaps[0]);
CHECK_LE(first_submap_index, next_submap_index);
if (insertion_submaps.size() == 1) {
// If we don't already have an entry for this submap, add one.
if (first_transform_index == next_transform_index) {
submap_transforms_.push_back(transform::Rigid2d::Identity());
if (first_submap_index == next_submap_index) {
optimization_problem_.AddSubmap(
submap_states_[first_submap_index].trajectory,
transform::Rigid2d::Identity());
}
return;
}
CHECK_EQ(2, submaps.size());
CHECK_EQ(2, insertion_submaps.size());
// CHECK that we have a index for the second submap.
const int second_transform_index = GetSubmapIndex(submaps[1]);
CHECK_LE(second_transform_index, next_transform_index);
const int second_submap_index = GetSubmapIndex(insertion_submaps[1]);
CHECK_LE(second_submap_index, next_submap_index);
// Extrapolate if necessary.
if (second_transform_index == next_transform_index) {
const auto& first_submap_transform =
submap_transforms_[first_transform_index];
submap_transforms_.push_back(
first_submap_transform *
sparse_pose_graph::ComputeSubmapPose(*submaps[0]).inverse() *
sparse_pose_graph::ComputeSubmapPose(*submaps[1]));
if (second_submap_index == next_submap_index) {
const auto& first_submap_pose =
optimization_problem_.submap_data().at(first_submap_index).pose;
optimization_problem_.AddSubmap(
submap_states_[second_submap_index].trajectory,
first_submap_pose *
sparse_pose_graph::ComputeSubmapPose(*insertion_submaps[0])
.inverse() *
sparse_pose_graph::ComputeSubmapPose(*insertion_submaps[1]));
}
}
@ -151,7 +157,7 @@ void SparsePoseGraph::AddImuData(const mapping::Submaps* trajectory,
void SparsePoseGraph::ComputeConstraint(const int scan_index,
const int submap_index) {
const transform::Rigid2d relative_pose =
submap_transforms_[submap_index].inverse() *
optimization_problem_.submap_data().at(submap_index).pose.inverse() *
optimization_problem_.node_data().at(scan_index).point_cloud_pose;
const mapping::Submaps* const scan_trajectory =
@ -203,7 +209,7 @@ void SparsePoseGraph::ComputeConstraintsForScan(
GrowSubmapTransformsAsNeeded(insertion_submaps);
const int matching_index = GetSubmapIndex(matching_submap);
const transform::Rigid2d optimized_pose =
submap_transforms_[matching_index] *
optimization_problem_.submap_data().at(matching_index).pose *
sparse_pose_graph::ComputeSubmapPose(*matching_submap).inverse() * pose;
CHECK_EQ(scan_index, optimization_problem_.node_data().size());
const mapping::TrajectoryNode::ConstantData* const scan_data =
@ -330,8 +336,10 @@ void SparsePoseGraph::RunFinalOptimization() {
}
void SparsePoseGraph::RunOptimization() {
if (!submap_transforms_.empty()) {
optimization_problem_.Solve(constraints_, &submap_transforms_);
if (optimization_problem_.submap_data().empty()) {
return;
}
optimization_problem_.Solve(constraints_);
common::MutexLocker locker(&mutex_);
const auto& node_data = optimization_problem_.node_data();
@ -343,14 +351,18 @@ void SparsePoseGraph::RunOptimization() {
// Extrapolate all point cloud poses that were added later.
std::unordered_map<const mapping::Submaps*, transform::Rigid3d>
extrapolation_transforms;
std::vector<transform::Rigid2d> submap_transforms;
for (const auto& submap_data : optimization_problem_.submap_data()) {
submap_transforms.push_back(submap_data.pose);
}
for (size_t i = num_optimized_poses; i != trajectory_nodes_.size(); ++i) {
const mapping::Submaps* trajectory =
trajectory_nodes_[i].constant_data->trajectory;
if (extrapolation_transforms.count(trajectory) == 0) {
const auto new_submap_transforms =
ExtrapolateSubmapTransforms(submap_transforms_, trajectory);
const auto old_submap_transforms = ExtrapolateSubmapTransforms(
optimized_submap_transforms_, trajectory);
ExtrapolateSubmapTransforms(submap_transforms, trajectory);
const auto old_submap_transforms =
ExtrapolateSubmapTransforms(optimized_submap_transforms_, trajectory);
CHECK_EQ(new_submap_transforms.size(), old_submap_transforms.size());
extrapolation_transforms[trajectory] =
transform::Rigid3d(new_submap_transforms.back() *
@ -359,7 +371,7 @@ void SparsePoseGraph::RunOptimization() {
trajectory_nodes_[i].pose =
extrapolation_transforms[trajectory] * trajectory_nodes_[i].pose;
}
optimized_submap_transforms_ = submap_transforms_;
optimized_submap_transforms_ = submap_transforms;
connected_components_ = trajectory_connectivity_.ConnectedComponents();
reverse_connected_components_.clear();
for (size_t i = 0; i != connected_components_.size(); ++i) {
@ -367,7 +379,6 @@ void SparsePoseGraph::RunOptimization() {
reverse_connected_components_.emplace(trajectory, i);
}
}
}
}
std::vector<mapping::TrajectoryNode> SparsePoseGraph::GetTrajectoryNodes() {

7
cartographer/mapping_2d/sparse_pose_graph.h
View File

@ -105,9 +105,11 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
return iterator->second;
}
// Grows 'submap_transforms_' to have an entry for every element of 'submaps'.
// Grows the optimization problem to have an entry for every element of
// 'insertion_submaps'.
void GrowSubmapTransformsAsNeeded(
const std::vector<const mapping::Submap*>& submaps) REQUIRES(mutex_);
const std::vector<const mapping::Submap*>& insertion_submaps)
REQUIRES(mutex_);
// Adds constraints for a scan, and starts scan matching in the background.
void ComputeConstraintsForScan(
@ -167,7 +169,6 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
sparse_pose_graph::OptimizationProblem optimization_problem_;
sparse_pose_graph::ConstraintBuilder constraint_builder_ GUARDED_BY(mutex_);
std::vector<Constraint> constraints_;
std::vector<transform::Rigid2d> submap_transforms_; // (map <- submap)
// Submaps get assigned an index and state as soon as they are seen, even
// before they take part in the background computations.

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

@ -75,14 +75,17 @@ void OptimizationProblem::AddTrajectoryNode(
NodeData{trajectory, time, initial_point_cloud_pose, point_cloud_pose});
}
void OptimizationProblem::AddSubmap(const mapping::Submaps* const trajectory,
const transform::Rigid2d& submap_pose) {
submap_data_.push_back(SubmapData{trajectory, submap_pose});
}
void OptimizationProblem::SetMaxNumIterations(const int32 max_num_iterations) {
options_.mutable_ceres_solver_options()->set_max_num_iterations(
max_num_iterations);
}
void OptimizationProblem::Solve(
const std::vector<Constraint>& constraints,
std::vector<transform::Rigid2d>* const submap_transforms) {
void OptimizationProblem::Solve(const std::vector<Constraint>& constraints) {
if (node_data_.empty()) {
// Nothing to optimize.
return;
@ -92,10 +95,10 @@ void OptimizationProblem::Solve(
ceres::Problem problem(problem_options);
// Set the starting point.
std::vector<std::array<double, 3>> C_submaps(submap_transforms->size());
std::vector<std::array<double, 3>> C_submaps(submap_data_.size());
std::vector<std::array<double, 3>> C_point_clouds(node_data_.size());
for (size_t i = 0; i != submap_transforms->size(); ++i) {
C_submaps[i] = FromPose((*submap_transforms)[i]);
for (size_t i = 0; i != submap_data_.size(); ++i) {
C_submaps[i] = FromPose(submap_data_[i].pose);
problem.AddParameterBlock(C_submaps[i].data(), 3);
}
for (size_t j = 0; j != node_data_.size(); ++j) {
@ -109,7 +112,7 @@ void OptimizationProblem::Solve(
// Add cost functions for intra- and inter-submap constraints.
for (const Constraint& constraint : constraints) {
CHECK_GE(constraint.i, 0);
CHECK_LT(constraint.i, submap_transforms->size());
CHECK_LT(constraint.i, submap_data_.size());
CHECK_GE(constraint.j, 0);
CHECK_LT(constraint.j, node_data_.size());
problem.AddResidualBlock(
@ -167,8 +170,8 @@ void OptimizationProblem::Solve(
}
// Store the result.
for (size_t i = 0; i != submap_transforms->size(); ++i) {
(*submap_transforms)[i] = ToPose(C_submaps[i]);
for (size_t i = 0; i != submap_data_.size(); ++i) {
submap_data_[i].pose = ToPose(C_submaps[i]);
}
for (size_t j = 0; j != node_data_.size(); ++j) {
node_data_[j].point_cloud_pose = ToPose(C_point_clouds[j]);
@ -179,6 +182,10 @@ const std::vector<NodeData>& OptimizationProblem::node_data() const {
return node_data_;
}
const std::vector<SubmapData>& OptimizationProblem::submap_data() const {
return submap_data_;
}
} // namespace sparse_pose_graph
} // namespace mapping_2d
} // namespace cartographer

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

@ -42,6 +42,12 @@ struct NodeData {
transform::Rigid2d point_cloud_pose;
};
struct SubmapData {
// TODO(whess): Keep nodes per trajectory instead.
const mapping::Submaps* trajectory;
transform::Rigid2d pose;
};
// Implements the SPA loop closure method.
class OptimizationProblem {
public:
@ -61,19 +67,22 @@ class OptimizationProblem {
void AddTrajectoryNode(const mapping::Submaps* trajectory, common::Time time,
const transform::Rigid2d& initial_point_cloud_pose,
const transform::Rigid2d& point_cloud_pose);
void AddSubmap(const mapping::Submaps* trajectory,
const transform::Rigid2d& submap_pose);
void SetMaxNumIterations(int32 max_num_iterations);
// Computes the optimized poses.
void Solve(const std::vector<Constraint>& constraints,
std::vector<transform::Rigid2d>* submap_transforms);
void Solve(const std::vector<Constraint>& constraints);
const std::vector<NodeData>& node_data() const;
const std::vector<SubmapData>& submap_data() const;
private:
mapping::sparse_pose_graph::proto::OptimizationProblemOptions options_;
std::map<const mapping::Submaps*, std::deque<mapping_3d::ImuData>> imu_data_;
std::vector<NodeData> node_data_;
std::vector<SubmapData> submap_data_;
};
} // namespace sparse_pose_graph

62
cartographer/mapping_3d/sparse_pose_graph.cc
View File

@ -53,30 +53,35 @@ SparsePoseGraph::~SparsePoseGraph() {
}
void SparsePoseGraph::GrowSubmapTransformsAsNeeded(
const std::vector<const Submap*>& submaps) {
CHECK(!submaps.empty());
CHECK_LT(submap_transforms_.size(), std::numeric_limits<int>::max());
const int next_transform_index = submap_transforms_.size();
const std::vector<const Submap*>& insertion_submaps) {
CHECK(!insertion_submaps.empty());
CHECK_LT(optimization_problem_.submap_data().size(),
std::numeric_limits<int>::max());
const int next_submap_index = optimization_problem_.submap_data().size();
// Verify that we have an index for the first submap.
const int first_transform_index = GetSubmapIndex(submaps[0]);
if (submaps.size() == 1) {
const int first_submap_index = GetSubmapIndex(insertion_submaps[0]);
CHECK_LE(first_submap_index, next_submap_index);
if (insertion_submaps.size() == 1) {
// If we don't already have an entry for this submap, add one.
if (first_transform_index == next_transform_index) {
submap_transforms_.push_back(transform::Rigid3d::Identity());
if (first_submap_index == next_submap_index) {
optimization_problem_.AddSubmap(
submap_states_[first_submap_index].trajectory,
transform::Rigid3d::Identity());
}
return;
}
CHECK_EQ(2, submaps.size());
CHECK_EQ(2, insertion_submaps.size());
// CHECK that we have a index for the second submap.
const int second_transform_index = GetSubmapIndex(submaps[1]);
CHECK_LE(second_transform_index, next_transform_index);
const int second_submap_index = GetSubmapIndex(insertion_submaps[1]);
CHECK_LE(second_submap_index, next_submap_index);
// Extrapolate if necessary.
if (second_transform_index == next_transform_index) {
const auto& first_submap_transform =
submap_transforms_[first_transform_index];
submap_transforms_.push_back(first_submap_transform *
submaps[0]->local_pose().inverse() *
submaps[1]->local_pose());
if (second_submap_index == next_submap_index) {
const auto& first_submap_pose =
optimization_problem_.submap_data().at(first_submap_index).pose;
optimization_problem_.AddSubmap(
submap_states_[second_submap_index].trajectory,
first_submap_pose * insertion_submaps[0]->local_pose().inverse() *
insertion_submaps[1]->local_pose());
}
}
@ -152,7 +157,7 @@ void SparsePoseGraph::AddImuData(const mapping::Submaps* trajectory,
void SparsePoseGraph::ComputeConstraint(const int scan_index,
const int submap_index) {
const transform::Rigid3d relative_pose =
submap_transforms_[submap_index].inverse() *
optimization_problem_.submap_data().at(submap_index).pose.inverse() *
optimization_problem_.node_data().at(scan_index).point_cloud_pose;
const mapping::Submaps* const scan_trajectory =
@ -202,7 +207,7 @@ void SparsePoseGraph::ComputeConstraintsForScan(
GrowSubmapTransformsAsNeeded(insertion_submaps);
const int matching_index = GetSubmapIndex(matching_submap);
const transform::Rigid3d optimized_pose =
submap_transforms_[matching_index] *
optimization_problem_.submap_data().at(matching_index).pose *
matching_submap->local_pose().inverse() * pose;
CHECK_EQ(scan_index, optimization_problem_.node_data().size());
const mapping::TrajectoryNode::ConstantData* const scan_data =
@ -325,8 +330,10 @@ void SparsePoseGraph::RunFinalOptimization() {
}
void SparsePoseGraph::RunOptimization() {
if (!submap_transforms_.empty()) {
optimization_problem_.Solve(constraints_, &submap_transforms_);
if (optimization_problem_.submap_data().empty()) {
return;
}
optimization_problem_.Solve(constraints_);
common::MutexLocker locker(&mutex_);
const auto& node_data = optimization_problem_.node_data();
@ -337,14 +344,18 @@ void SparsePoseGraph::RunOptimization() {
// Extrapolate all point cloud poses that were added later.
std::unordered_map<const mapping::Submaps*, transform::Rigid3d>
extrapolation_transforms;
std::vector<transform::Rigid3d> submap_transforms;
for (const auto& submap_data : optimization_problem_.submap_data()) {
submap_transforms.push_back(submap_data.pose);
}
for (size_t i = num_optimized_poses; i != trajectory_nodes_.size(); ++i) {
const mapping::Submaps* trajectory =
trajectory_nodes_[i].constant_data->trajectory;
if (extrapolation_transforms.count(trajectory) == 0) {
const auto new_submap_transforms =
ExtrapolateSubmapTransforms(submap_transforms_, trajectory);
const auto old_submap_transforms = ExtrapolateSubmapTransforms(
optimized_submap_transforms_, trajectory);
ExtrapolateSubmapTransforms(submap_transforms, trajectory);
const auto old_submap_transforms =
ExtrapolateSubmapTransforms(optimized_submap_transforms_, trajectory);
CHECK_EQ(new_submap_transforms.size(), old_submap_transforms.size());
extrapolation_transforms[trajectory] =
transform::Rigid3d(new_submap_transforms.back() *
@ -353,7 +364,7 @@ void SparsePoseGraph::RunOptimization() {
trajectory_nodes_[i].pose =
extrapolation_transforms[trajectory] * trajectory_nodes_[i].pose;
}
optimized_submap_transforms_ = submap_transforms_;
optimized_submap_transforms_ = submap_transforms;
connected_components_ = trajectory_connectivity_.ConnectedComponents();
reverse_connected_components_.clear();
for (size_t i = 0; i != connected_components_.size(); ++i) {
@ -361,7 +372,6 @@ void SparsePoseGraph::RunOptimization() {
reverse_connected_components_.emplace(trajectory, i);
}
}
}
}
std::vector<mapping::TrajectoryNode> SparsePoseGraph::GetTrajectoryNodes() {

8
cartographer/mapping_3d/sparse_pose_graph.h
View File

@ -126,9 +126,10 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
return iterator->second;
}
// Grows 'submap_transforms_' to have an entry for every element of 'submaps'.
void GrowSubmapTransformsAsNeeded(const std::vector<const Submap*>& submaps)
REQUIRES(mutex_);
// Grows the optimization problem to have an entry for every element of
// 'insertion_submaps'.
void GrowSubmapTransformsAsNeeded(
const std::vector<const Submap*>& insertion_submaps) REQUIRES(mutex_);
// Adds constraints for a scan, and starts scan matching in the background.
void ComputeConstraintsForScan(
@ -187,7 +188,6 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
sparse_pose_graph::OptimizationProblem optimization_problem_;
sparse_pose_graph::ConstraintBuilder constraint_builder_ GUARDED_BY(mutex_);
std::vector<Constraint> constraints_;
std::vector<transform::Rigid3d> submap_transforms_; // (map <- submap)
// Submaps get assigned an index and state as soon as they are seen, even
// before they take part in the background computations.

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

@ -91,14 +91,17 @@ void OptimizationProblem::AddTrajectoryNode(
node_data_.push_back(NodeData{trajectory, time, point_cloud_pose});
}
void OptimizationProblem::AddSubmap(const mapping::Submaps* const trajectory,
const transform::Rigid3d& submap_pose) {
submap_data_.push_back(SubmapData{trajectory, submap_pose});
}
void OptimizationProblem::SetMaxNumIterations(const int32 max_num_iterations) {
options_.mutable_ceres_solver_options()->set_max_num_iterations(
max_num_iterations);
}
void OptimizationProblem::Solve(
const std::vector<Constraint>& constraints,
std::vector<transform::Rigid3d>* const submap_transforms) {
void OptimizationProblem::Solve(const std::vector<Constraint>& constraints) {
if (node_data_.empty()) {
// Nothing to optimize.
return;
@ -125,7 +128,7 @@ void OptimizationProblem::Solve(
std::deque<CeresPose> C_submaps;
std::deque<CeresPose> C_point_clouds;
// Tie the first submap to the origin.
CHECK(!submap_transforms->empty());
CHECK(!submap_data_.empty());
C_submaps.emplace_back(
transform::Rigid3d::Identity(), translation_parameterization(),
common::make_unique<ceres::AutoDiffLocalParameterization<
@ -133,9 +136,9 @@ void OptimizationProblem::Solve(
&problem);
problem.SetParameterBlockConstant(C_submaps.back().translation());
for (size_t i = 1; i != submap_transforms->size(); ++i) {
for (size_t i = 1; i != submap_data_.size(); ++i) {
C_submaps.emplace_back(
(*submap_transforms)[i], translation_parameterization(),
submap_data_[i].pose, translation_parameterization(),
common::make_unique<ceres::QuaternionParameterization>(), &problem);
}
for (size_t j = 0; j != node_data_.size(); ++j) {
@ -147,7 +150,7 @@ void OptimizationProblem::Solve(
// Add cost functions for intra- and inter-submap constraints.
for (const Constraint& constraint : constraints) {
CHECK_GE(constraint.i, 0);
CHECK_LT(constraint.i, submap_transforms->size());
CHECK_LT(constraint.i, submap_data_.size());
CHECK_GE(constraint.j, 0);
CHECK_LT(constraint.j, node_data_.size());
problem.AddResidualBlock(
@ -239,8 +242,8 @@ void OptimizationProblem::Solve(
}
// Store the result.
for (size_t i = 0; i != submap_transforms->size(); ++i) {
(*submap_transforms)[i] = C_submaps[i].ToRigid();
for (size_t i = 0; i != submap_data_.size(); ++i) {
submap_data_[i].pose = C_submaps[i].ToRigid();
}
for (size_t j = 0; j != node_data_.size(); ++j) {
node_data_[j].point_cloud_pose = C_point_clouds[j].ToRigid();
@ -251,6 +254,10 @@ const std::vector<NodeData>& OptimizationProblem::node_data() const {
return node_data_;
}
const std::vector<SubmapData>& OptimizationProblem::submap_data() const {
return submap_data_;
}
} // namespace sparse_pose_graph
} // namespace mapping_3d
} // namespace cartographer

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

@ -42,6 +42,12 @@ struct NodeData {
transform::Rigid3d point_cloud_pose;
};
struct SubmapData {
// TODO(whess): Keep nodes per trajectory instead.
const mapping::Submaps* trajectory;
transform::Rigid3d pose;
};
// Implements the SPA loop closure method.
class OptimizationProblem {
public:
@ -63,20 +69,23 @@ class OptimizationProblem {
const Eigen::Vector3d& angular_velocity);
void AddTrajectoryNode(const mapping::Submaps* trajectory, common::Time time,
const transform::Rigid3d& point_cloud_pose);
void AddSubmap(const mapping::Submaps* trajectory,
const transform::Rigid3d& submap_pose);
void SetMaxNumIterations(int32 max_num_iterations);
// Computes the optimized poses.
void Solve(const std::vector<Constraint>& constraints,
std::vector<transform::Rigid3d>* submap_transforms);
void Solve(const std::vector<Constraint>& constraints);
const std::vector<NodeData>& node_data() const;
const std::vector<SubmapData>& submap_data() const;
private:
mapping::sparse_pose_graph::proto::OptimizationProblemOptions options_;
FixZ fix_z_;
std::map<const mapping::Submaps*, std::deque<ImuData>> imu_data_;
std::vector<NodeData> node_data_;
std::vector<SubmapData> submap_data_;
double gravity_constant_ = 9.8;
};

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

@ -153,9 +153,6 @@ TEST_F(OptimizationProblemTest, ReducesNoise) {
1e-9 * Eigen::Matrix<double, 6, 6>::Identity()}});
}
std::vector<transform::Rigid3d> submap_transforms = {
kSubmap0Transform, kSubmap0Transform, kSubmap2Transform};
double translation_error_before = 0.;
double rotation_error_before = 0.;
const auto& node_data = optimization_problem_.node_data();
@ -168,7 +165,10 @@ TEST_F(OptimizationProblemTest, ReducesNoise) {
node_data[j].point_cloud_pose);
}
optimization_problem_.Solve(constraints, &submap_transforms);
optimization_problem_.AddSubmap(kTrajectory, kSubmap0Transform);
optimization_problem_.AddSubmap(kTrajectory, kSubmap0Transform);
optimization_problem_.AddSubmap(kTrajectory, kSubmap2Transform);
optimization_problem_.Solve(constraints);
double translation_error_after = 0.;
double rotation_error_after = 0.;