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_) {
ComputeConstraintsForScan(time, j, matching_submap, insertion_submaps,
ComputeConstraintsForScan(j, matching_submap, insertion_submaps,
finished_submap, pose, covariance);
});
}
@ -199,8 +199,7 @@ void SparsePoseGraph::ComputeConstraintsForOldScans(
}
void SparsePoseGraph::ComputeConstraintsForScan(
const common::Time time, const int scan_index,
const mapping::Submap* matching_submap,
const int scan_index, const mapping::Submap* matching_submap,
std::vector<const mapping::Submap*> insertion_submaps,
const mapping::Submap* finished_submap, const transform::Rigid2d& pose,
const kalman_filter::Pose2DCovariance& covariance) {
@ -210,10 +209,10 @@ void SparsePoseGraph::ComputeConstraintsForScan(
submap_transforms_[matching_index] *
sparse_pose_graph::ComputeSubmapPose(*matching_submap).inverse() * pose;
CHECK_EQ(scan_index, optimization_problem_.node_data().size());
const mapping::Submaps* const scan_trajectory =
trajectory_nodes_[scan_index].constant_data->trajectory;
optimization_problem_.AddTrajectoryNode(scan_trajectory, time, pose,
optimized_pose);
const mapping::TrajectoryNode::ConstantData* const scan_data =
trajectory_nodes_[scan_index].constant_data;
optimization_problem_.AddTrajectoryNode(
scan_data->trajectory, scan_data->time, pose, optimized_pose);
for (const mapping::Submap* submap : insertion_submaps) {
const int submap_index = GetSubmapIndex(submap);
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.
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,
const mapping::Submap* finished_submap, const transform::Rigid2d& pose,
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());
// 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) {
CHECK_GE(constraint.i, 0);
CHECK_LT(constraint.i, submap_transforms->size());
CHECK_GE(constraint.j, 0);
CHECK_LT(constraint.j, node_data_.size());
constraints_residual_blocks.emplace_back(
constraint.tag,
problem.AddResidualBlock(
new ceres::AutoDiffCostFunction<SpaCostFunction, 3, 3, 3>(
new SpaCostFunction(constraint.pose)),
// Only loop closure constraints should have a loss function.
constraint.tag == Constraint::INTER_SUBMAP
? new ceres::HuberLoss(options_.huber_scale())
: nullptr,
C_submaps[constraint.i].data(),
C_point_clouds[constraint.j].data()));
problem.AddResidualBlock(
new ceres::AutoDiffCostFunction<SpaCostFunction, 3, 3, 3>(
new SpaCostFunction(constraint.pose)),
// Only loop closure constraints should have a loss function.
constraint.tag == Constraint::INTER_SUBMAP
? new ceres::HuberLoss(options_.huber_scale())
: nullptr,
C_submaps[constraint.i].data(), C_point_clouds[constraint.j].data());
}
// 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_) {
ComputeConstraintsForScan(time, j, matching_submap, insertion_submaps,
ComputeConstraintsForScan(j, matching_submap, insertion_submaps,
finished_submap, pose, covariance);
});
return j;
@ -194,9 +194,9 @@ void SparsePoseGraph::ComputeConstraintsForOldScans(const Submap* submap) {
}
void SparsePoseGraph::ComputeConstraintsForScan(
const common::Time time, const int scan_index,
const Submap* matching_submap, std::vector<const Submap*> insertion_submaps,
const Submap* finished_submap, const transform::Rigid3d& pose,
const int scan_index, const Submap* matching_submap,
std::vector<const Submap*> insertion_submaps, const Submap* finished_submap,
const transform::Rigid3d& pose,
const kalman_filter::PoseCovariance& covariance) {
GrowSubmapTransformsAsNeeded(insertion_submaps);
const int matching_index = GetSubmapIndex(matching_submap);
@ -204,10 +204,10 @@ void SparsePoseGraph::ComputeConstraintsForScan(
submap_transforms_[matching_index] *
matching_submap->local_pose().inverse() * pose;
CHECK_EQ(scan_index, optimization_problem_.node_data().size());
const mapping::Submaps* const scan_trajectory =
trajectory_nodes_[scan_index].constant_data->trajectory;
optimization_problem_.AddTrajectoryNode(scan_trajectory, time,
optimized_pose);
const mapping::TrajectoryNode::ConstantData* const scan_data =
trajectory_nodes_[scan_index].constant_data;
optimization_problem_.AddTrajectoryNode(scan_data->trajectory,
scan_data->time, optimized_pose);
for (const Submap* submap : insertion_submaps) {
const int submap_index = GetSubmapIndex(submap);
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.
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,
const Submap* finished_submap, const transform::Rigid3d& pose,
const kalman_filter::PoseCovariance& covariance) REQUIRES(mutex_);