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

Omit odometry penalty within frozen trajectory. (#661) 

In the case of pure localization, this reduces the size of the optimization problem by a large factor.
master
gaschler 2017-11-13 14:19:15 +01:00 committed by Wally B. Feed
parent 4a8607810e
commit fd5003b69b
2 changed files with 8 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -168,6 +168,10 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
for (auto node_it = node_data_.begin(); node_it != node_data_.end();) {
const int trajectory_id = node_it->id.trajectory_id;
const auto trajectory_end = node_data_.EndOfTrajectory(trajectory_id);
if (frozen_trajectories.count(trajectory_id) != 0) {
node_it = trajectory_end;
continue;
}
auto prev_node_it = node_it;
for (++node_it; node_it != trajectory_end; ++node_it) {

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

@ -309,6 +309,10 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
for (auto node_it = node_data_.begin(); node_it != node_data_.end();) {
const int trajectory_id = node_it->id.trajectory_id;
const auto trajectory_end = node_data_.EndOfTrajectory(trajectory_id);
if (frozen_trajectories.count(trajectory_id) != 0) {
node_it = trajectory_end;
continue;
}
auto prev_node_it = node_it;
for (++node_it; node_it != trajectory_end; ++node_it) {