Add histograms for pose residuals (#478)

* WIP Add histograms for pose residuals

* Address reviewers comments

* Address more comments.
master
Christoph Schütte 2017-08-25 11:39:56 +02:00 committed by GitHub
parent 2fd2e86351
commit 96b71e227f
7 changed files with 47 additions and 0 deletions

View File

@ -47,4 +47,7 @@ message SparsePoseGraphOptions {
// Rate at which we sample a single trajectory's scans for global // Rate at which we sample a single trajectory's scans for global
// localization. // localization.
optional double global_sampling_ratio = 5; optional double global_sampling_ratio = 5;
// Whether to output histograms for the pose residuals.
optional bool log_residual_histograms = 9;
} }

View File

@ -55,6 +55,8 @@ proto::SparsePoseGraphOptions CreateSparsePoseGraphOptions(
CHECK_GT(options.max_num_final_iterations(), 0); CHECK_GT(options.max_num_final_iterations(), 0);
options.set_global_sampling_ratio( options.set_global_sampling_ratio(
parameter_dictionary->GetDouble("global_sampling_ratio")); parameter_dictionary->GetDouble("global_sampling_ratio"));
options.set_log_residual_histograms(
parameter_dictionary->GetBool("log_residual_histograms"));
return options; return options;
} }

View File

@ -145,6 +145,7 @@ class SparsePoseGraphTest : public ::testing::Test {
}, },
max_num_final_iterations = 200, max_num_final_iterations = 200,
global_sampling_ratio = 0.01, global_sampling_ratio = 0.01,
log_residual_histograms = true,
})text"); })text");
sparse_pose_graph_ = common::make_unique<SparsePoseGraph>( sparse_pose_graph_ = common::make_unique<SparsePoseGraph>(
mapping::CreateSparsePoseGraphOptions(parameter_dictionary.get()), mapping::CreateSparsePoseGraphOptions(parameter_dictionary.get()),

View File

@ -433,6 +433,34 @@ void SparsePoseGraph::RunFinalOptimization() {
.max_num_iterations()); .max_num_iterations());
} }
void SparsePoseGraph::LogResidualHistograms() {
common::Histogram rotational_residual;
common::Histogram translational_residual;
for (const Constraint& constraint : constraints_) {
if (constraint.tag == Constraint::Tag::INTRA_SUBMAP) {
const cartographer::transform::Rigid3d optimized_node_to_map =
trajectory_nodes_.at(constraint.node_id).pose;
const cartographer::transform::Rigid3d node_to_submap_constraint =
constraint.pose.zbar_ij;
const cartographer::transform::Rigid3d optimized_submap_to_map =
optimized_submap_transforms_.at(constraint.submap_id.trajectory_id)
.at(constraint.submap_id.submap_index)
.pose;
const cartographer::transform::Rigid3d optimized_node_to_submap =
optimized_submap_to_map.inverse() * optimized_node_to_map;
const cartographer::transform::Rigid3d residual =
node_to_submap_constraint.inverse() * optimized_node_to_submap;
rotational_residual.Add(
common::NormalizeAngleDifference(transform::GetAngle(residual)));
translational_residual.Add(residual.translation().norm());
}
}
LOG(INFO) << "Translational residuals histogram:\n"
<< translational_residual.ToString(10);
LOG(INFO) << "Rotational residuals histogram:\n"
<< rotational_residual.ToString(10);
}
void SparsePoseGraph::RunOptimization() { void SparsePoseGraph::RunOptimization() {
if (optimization_problem_.submap_data().empty()) { if (optimization_problem_.submap_data().empty()) {
return; return;
@ -481,6 +509,11 @@ void SparsePoseGraph::RunOptimization() {
for (auto& trimmer : trimmers_) { for (auto& trimmer : trimmers_) {
trimmer->Trim(&trimming_handle); trimmer->Trim(&trimming_handle);
} }
// Log the histograms for the pose residuals.
if (options_.log_residual_histograms()) {
LogResidualHistograms();
}
} }
std::vector<std::vector<mapping::TrajectoryNode>> std::vector<std::vector<mapping::TrajectoryNode>>

View File

@ -161,6 +161,10 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
mapping::SparsePoseGraph::SubmapData GetSubmapDataUnderLock( mapping::SparsePoseGraph::SubmapData GetSubmapDataUnderLock(
const mapping::SubmapId& submap_id) REQUIRES(mutex_); const mapping::SubmapId& submap_id) REQUIRES(mutex_);
// Logs histograms for the translational and rotational residual of scan
// poses.
void LogResidualHistograms();
const mapping::proto::SparsePoseGraphOptions options_; const mapping::proto::SparsePoseGraphOptions options_;
common::Mutex mutex_; common::Mutex mutex_;

View File

@ -94,4 +94,5 @@ SPARSE_POSE_GRAPH = {
}, },
max_num_final_iterations = 200, max_num_final_iterations = 200,
global_sampling_ratio = 0.003, global_sampling_ratio = 0.003,
log_residual_histograms = true,
} }

View File

@ -78,6 +78,9 @@ double global_sampling_ratio
Rate at which we sample a single trajectory's scans for global Rate at which we sample a single trajectory's scans for global
localization. localization.
bool log_residual_histograms
Whether to output histograms for the pose residuals.
cartographer.mapping.proto.TrajectoryBuilderOptions cartographer.mapping.proto.TrajectoryBuilderOptions
=================================================== ===================================================