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

Tiny sparse pose graph cleanup. (#169)

master
Wolfgang Hess 2016-12-20 16:43:58 +01:00 committed by GitHub
parent 628b9da6d2
commit 1477a81bfa
5 changed files with 24 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
cartographer/mapping_2d/sparse_pose_graph.cc
View File

@ -127,7 +127,7 @@ void SparsePoseGraph::AddScan(
} }
AddWorkItem([=]() REQUIRES(mutex_) { AddWorkItem([=]() REQUIRES(mutex_) {
ComputeConstraintsForScan(time, j, matching_submap, insertion_submaps, ComputeConstraintsForScan(j, matching_submap, insertion_submaps,
finished_submap, pose, covariance); finished_submap, pose, covariance);
}); });
} }
@ -199,8 +199,7 @@ void SparsePoseGraph::ComputeConstraintsForOldScans(
} }
void SparsePoseGraph::ComputeConstraintsForScan( void SparsePoseGraph::ComputeConstraintsForScan(
const common::Time time, const int scan_index, const int scan_index, const mapping::Submap* matching_submap,
const mapping::Submap* matching_submap,
std::vector<const mapping::Submap*> insertion_submaps, std::vector<const mapping::Submap*> insertion_submaps,
const mapping::Submap* finished_submap, const transform::Rigid2d& pose, const mapping::Submap* finished_submap, const transform::Rigid2d& pose,
const kalman_filter::Pose2DCovariance& covariance) { const kalman_filter::Pose2DCovariance& covariance) {
@ -210,10 +209,10 @@ void SparsePoseGraph::ComputeConstraintsForScan(
submap_transforms_[matching_index] * submap_transforms_[matching_index] *
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, optimization_problem_.node_data().size());
const mapping::Submaps* const scan_trajectory = const mapping::TrajectoryNode::ConstantData* const scan_data =
trajectory_nodes_[scan_index].constant_data->trajectory; trajectory_nodes_[scan_index].constant_data;
optimization_problem_.AddTrajectoryNode(scan_trajectory, time, pose, optimization_problem_.AddTrajectoryNode(
optimized_pose); scan_data->trajectory, scan_data->time, pose, optimized_pose);
for (const mapping::Submap* submap : insertion_submaps) { for (const mapping::Submap* submap : insertion_submaps) {
const int submap_index = GetSubmapIndex(submap); const int submap_index = GetSubmapIndex(submap);
CHECK(!submap_states_[submap_index].finished); CHECK(!submap_states_[submap_index].finished);

2
cartographer/mapping_2d/sparse_pose_graph.h
View File

@ -132,7 +132,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(
common::Time time, int scan_index, const mapping::Submap* matching_submap, int scan_index, const mapping::Submap* matching_submap,
std::vector<const mapping::Submap*> insertion_submaps, std::vector<const mapping::Submap*> insertion_submaps,
const mapping::Submap* finished_submap, const transform::Rigid2d& pose, const mapping::Submap* finished_submap, const transform::Rigid2d& pose,
const kalman_filter::Pose2DCovariance& covariance) REQUIRES(mutex_); const kalman_filter::Pose2DCovariance& covariance) REQUIRES(mutex_);

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

@ -107,24 +107,19 @@ void OptimizationProblem::Solve(
problem.SetParameterBlockConstant(C_submaps[0].data()); problem.SetParameterBlockConstant(C_submaps[0].data());
// Add cost functions for intra- and inter-submap constraints. // Add cost functions for intra- and inter-submap constraints.
std::vector<std::pair<Constraint::Tag, ceres::ResidualBlockId>>
constraints_residual_blocks;
for (const Constraint& constraint : constraints) { for (const Constraint& constraint : constraints) {
CHECK_GE(constraint.i, 0); CHECK_GE(constraint.i, 0);
CHECK_LT(constraint.i, submap_transforms->size()); CHECK_LT(constraint.i, submap_transforms->size());
CHECK_GE(constraint.j, 0); CHECK_GE(constraint.j, 0);
CHECK_LT(constraint.j, node_data_.size()); CHECK_LT(constraint.j, node_data_.size());
constraints_residual_blocks.emplace_back( problem.AddResidualBlock(
constraint.tag, new ceres::AutoDiffCostFunction<SpaCostFunction, 3, 3, 3>(
problem.AddResidualBlock( new SpaCostFunction(constraint.pose)),
new ceres::AutoDiffCostFunction<SpaCostFunction, 3, 3, 3>( // Only loop closure constraints should have a loss function.
new SpaCostFunction(constraint.pose)), constraint.tag == Constraint::INTER_SUBMAP
// Only loop closure constraints should have a loss function. ? new ceres::HuberLoss(options_.huber_scale())
constraint.tag == Constraint::INTER_SUBMAP : nullptr,
? new ceres::HuberLoss(options_.huber_scale()) C_submaps[constraint.i].data(), C_point_clouds[constraint.j].data());
: nullptr,
C_submaps[constraint.i].data(),
C_point_clouds[constraint.j].data()));
} }
// Add penalties for changes between consecutive scans. // Add penalties for changes between consecutive scans.

16
cartographer/mapping_3d/sparse_pose_graph.cc
View File

@ -123,7 +123,7 @@ int SparsePoseGraph::AddScan(
} }
AddWorkItem([=]() REQUIRES(mutex_) { AddWorkItem([=]() REQUIRES(mutex_) {
ComputeConstraintsForScan(time, j, matching_submap, insertion_submaps, ComputeConstraintsForScan(j, matching_submap, insertion_submaps,
finished_submap, pose, covariance); finished_submap, pose, covariance);
}); });
return j; return j;
@ -194,9 +194,9 @@ void SparsePoseGraph::ComputeConstraintsForOldScans(const Submap* submap) {
} }
void SparsePoseGraph::ComputeConstraintsForScan( void SparsePoseGraph::ComputeConstraintsForScan(
const common::Time time, const int scan_index, const int scan_index, const Submap* matching_submap,
const Submap* matching_submap, std::vector<const Submap*> insertion_submaps, std::vector<const Submap*> insertion_submaps, const Submap* finished_submap,
const Submap* finished_submap, const transform::Rigid3d& pose, const transform::Rigid3d& pose,
const kalman_filter::PoseCovariance& covariance) { const kalman_filter::PoseCovariance& covariance) {
GrowSubmapTransformsAsNeeded(insertion_submaps); GrowSubmapTransformsAsNeeded(insertion_submaps);
const int matching_index = GetSubmapIndex(matching_submap); const int matching_index = GetSubmapIndex(matching_submap);
@ -204,10 +204,10 @@ void SparsePoseGraph::ComputeConstraintsForScan(
submap_transforms_[matching_index] * submap_transforms_[matching_index] *
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, optimization_problem_.node_data().size());
const mapping::Submaps* const scan_trajectory = const mapping::TrajectoryNode::ConstantData* const scan_data =
trajectory_nodes_[scan_index].constant_data->trajectory; trajectory_nodes_[scan_index].constant_data;
optimization_problem_.AddTrajectoryNode(scan_trajectory, time, optimization_problem_.AddTrajectoryNode(scan_data->trajectory,
optimized_pose); scan_data->time, optimized_pose);
for (const Submap* submap : insertion_submaps) { for (const Submap* submap : insertion_submaps) {
const int submap_index = GetSubmapIndex(submap); const int submap_index = GetSubmapIndex(submap);
CHECK(!submap_states_[submap_index].finished); CHECK(!submap_states_[submap_index].finished);

2
cartographer/mapping_3d/sparse_pose_graph.h
View File

@ -130,7 +130,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(
common::Time time, int scan_index, const Submap* matching_submap, int scan_index, const Submap* matching_submap,
std::vector<const Submap*> insertion_submaps, std::vector<const Submap*> insertion_submaps,
const Submap* finished_submap, const transform::Rigid3d& pose, const Submap* finished_submap, const transform::Rigid3d& pose,
const kalman_filter::PoseCovariance& covariance) REQUIRES(mutex_); const kalman_filter::PoseCovariance& covariance) REQUIRES(mutex_);