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

Delete NodeData and ImuData from trimmed submaps in the OptimizationProblem (#364) 

Related to #283.
PAIR=wohe
master
Holger Rapp 2017-06-26 16:38:08 +02:00 committed by GitHub
parent 4c3059e088
commit d8eb94b4a9
3 changed files with 76 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

35
cartographer/mapping_2d/sparse_pose_graph.cc
View File

@ -190,9 +190,9 @@ 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) .at(node_id.node_index - optimization_problem_.num_trimmed_nodes(
node_id.trajectory_id))
.point_cloud_pose; .point_cloud_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->range_data_2d.returns, &trajectory_nodes_.at(node_id).constant_data->range_data_2d.returns,
@ -207,12 +207,16 @@ 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_index = 0; node_index != node_data[trajectory_id].size(); for (size_t node_data_index = 0;
++node_index) { 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)}; static_cast<int>(node_index)};
if (!trajectory_nodes_.at(node_id).trimmed() && CHECK(!trajectory_nodes_.at(node_id).trimmed());
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);
} }
} }
@ -241,7 +245,9 @@ void SparsePoseGraph::ComputeConstraintsForScan(
optimization_problem_.node_data().size() optimization_problem_.node_data().size()
? 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()) .size()) +
optimization_problem_.num_trimmed_nodes(
matching_id.trajectory_id)
: 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(
@ -386,13 +392,14 @@ 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_index = 0; 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);
for (; node_index != static_cast<int>(node_data[trajectory_id].size()); int node_index = optimization_problem_.num_trimmed_nodes(trajectory_id);
++node_index) { for (; node_data_index != static_cast<int>(node_data[trajectory_id].size());
++node_data_index, ++node_index) {
const mapping::NodeId node_id{trajectory_id, node_index}; const mapping::NodeId node_id{trajectory_id, node_index};
trajectory_nodes_.at(node_id).pose = transform::Embed3D( trajectory_nodes_.at(node_id).pose = transform::Embed3D(
node_data[trajectory_id][node_index].point_cloud_pose); node_data[trajectory_id][node_data_index].point_cloud_pose);
} }
// Extrapolate all point cloud poses that were added later. // Extrapolate all point cloud poses that were added later.
const auto local_to_new_global = ComputeLocalToGlobalTransform( const auto local_to_new_global = ComputeLocalToGlobalTransform(
@ -583,15 +590,13 @@ void SparsePoseGraph::TrimmingHandle::MarkSubmapAsTrimmed(
for (const mapping::NodeId& node_id : nodes_to_remove) { for (const mapping::NodeId& node_id : nodes_to_remove) {
CHECK(!parent_->trajectory_nodes_.at(node_id).trimmed()); CHECK(!parent_->trajectory_nodes_.at(node_id).trimmed());
parent_->trajectory_nodes_.at(node_id).constant_data.reset(); parent_->trajectory_nodes_.at(node_id).constant_data.reset();
parent_->optimization_problem_.TrimTrajectoryNode(node_id);
} }
// TODO(whess): The optimization problem should no longer include the submap // TODO(whess): The optimization problem should no longer include the submap.
// and the removed nodes.
// TODO(whess): If the first submap is gone, we want to tie the first not // TODO(whess): If the first submap is gone, we want to tie the first not
// yet trimmed submap to be set fixed to its current pose. // yet trimmed submap to be set fixed to its current pose.
// TODO(hrapp): Delete related IMU data.
} }
} // namespace mapping_2d } // namespace mapping_2d

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

@ -80,6 +80,22 @@ void OptimizationProblem::AddTrajectoryNode(
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( node_data_[trajectory_id].push_back(
NodeData{time, initial_point_cloud_pose, point_cloud_pose}); NodeData{time, initial_point_cloud_pose, point_cloud_pose});
trajectory_data_.resize(std::max(trajectory_data_.size(), node_data_.size()));
}
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);
CHECK(!node_data.empty());
const common::Time node_time = node_data.front().time;
auto& imu_data = imu_data_.at(node_id.trajectory_id);
while (imu_data.size() > 1 && imu_data[1].time <= node_time) {
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,
@ -107,7 +123,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.
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(node_data_.size()); std::vector<std::vector<std::array<double, 3>>> 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;
@ -128,12 +144,12 @@ 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) {
C_nodes[trajectory_id].resize(node_data_[trajectory_id].size()); // Reserve guarantees that data does not move, so the pointers for Ceres
for (size_t node_index = 0; node_index != node_data_[trajectory_id].size(); // stay valid.
++node_index) { C_nodes[trajectory_id].reserve(node_data_[trajectory_id].size());
C_nodes[trajectory_id][node_index] = for (const NodeData& node_data : node_data_[trajectory_id]) {
FromPose(node_data_[trajectory_id][node_index].point_cloud_pose); C_nodes[trajectory_id].push_back(FromPose(node_data.point_cloud_pose));
problem.AddParameterBlock(C_nodes[trajectory_id][node_index].data(), 3); problem.AddParameterBlock(C_nodes[trajectory_id].back().data(), 3);
} }
} }
@ -150,27 +166,31 @@ 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());
} }
// Add penalties for changes between consecutive scans. // Add penalties for changes between consecutive scans.
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_index = 1; node_index < node_data_[trajectory_id].size(); for (size_t node_data_index = 1;
++node_index) { node_data_index < node_data_[trajectory_id].size();
++node_data_index) {
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{
transform::Embed3D(node_data_[trajectory_id][node_index - 1] transform::Embed3D(
.initial_point_cloud_pose.inverse() * node_data_[trajectory_id][node_data_index - 1]
node_data_[trajectory_id][node_index] .initial_point_cloud_pose.inverse() *
.initial_point_cloud_pose), node_data_[trajectory_id][node_data_index]
.initial_point_cloud_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[trajectory_id][node_index - 1].data(), C_nodes[trajectory_id][node_data_index - 1].data(),
C_nodes[trajectory_id][node_index].data()); C_nodes[trajectory_id][node_data_index].data());
} }
} }
@ -195,15 +215,16 @@ 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_index = 0; node_index != node_data_[trajectory_id].size(); for (size_t node_data_index = 0;
++node_index) { node_data_index != node_data_[trajectory_id].size();
node_data_[trajectory_id][node_index].point_cloud_pose = ++node_data_index) {
ToPose(C_nodes[trajectory_id][node_index]); node_data_[trajectory_id][node_data_index].point_cloud_pose =
ToPose(C_nodes[trajectory_id][node_data_index]);
} }
} }
} }
const std::vector<std::vector<NodeData>>& OptimizationProblem::node_data() const std::vector<std::deque<NodeData>>& OptimizationProblem::node_data()
const { const {
return node_data_; return node_data_;
} }
@ -213,6 +234,10 @@ const std::vector<std::vector<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

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

@ -63,6 +63,7 @@ class OptimizationProblem {
void AddTrajectoryNode(int trajectory_id, common::Time time, void AddTrajectoryNode(int trajectory_id, 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);
void TrimTrajectoryNode(const mapping::NodeId& node_id);
void AddSubmap(int trajectory_id, const transform::Rigid2d& submap_pose); void AddSubmap(int trajectory_id, const transform::Rigid2d& submap_pose);
void SetMaxNumIterations(int32 max_num_iterations); void SetMaxNumIterations(int32 max_num_iterations);
@ -70,14 +71,21 @@ 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<std::vector<NodeData>>& node_data() const; const std::vector<std::deque<NodeData>>& node_data() const;
const std::vector<std::vector<SubmapData>>& submap_data() const; const std::vector<std::vector<SubmapData>>& submap_data() const;
int num_trimmed_nodes(int trajectory_id) const;
private: private:
struct TrajectoryData {
// TODO(hrapp): Remove, once we can relabel constraints.
int num_trimmed_nodes = 0;
};
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<std::vector<NodeData>> node_data_; std::vector<std::deque<NodeData>> node_data_;
std::vector<std::vector<SubmapData>> submap_data_; std::vector<std::vector<SubmapData>> submap_data_;
std::vector<TrajectoryData> trajectory_data_;
}; };
} // namespace sparse_pose_graph } // namespace sparse_pose_graph