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

Remove the 'log_residual_histograms' option. (#166) 

This option only existed in 2D. Also other refactoring to make the 2D
sparse pose graph optimization more similar to 3D.
master
Wolfgang Hess 2016-12-19 17:57:12 +01:00 committed by GitHub
parent 5af133c0dd
commit 71c951b370
13 changed files with 102 additions and 104 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
cartographer/mapping/sparse_pose_graph/optimization_problem_options.cc
View File

@ -38,8 +38,6 @@ proto::OptimizationProblemOptions CreateOptimizationProblemOptions(
"consecutive_scan_rotation_penalty_factor"));
options.set_log_solver_summary(
parameter_dictionary->GetBool("log_solver_summary"));
options.set_log_residual_histograms(
parameter_dictionary->GetBool("log_residual_histograms"));
*options.mutable_ceres_solver_options() =
common::CreateCeresSolverOptionsProto(
parameter_dictionary->GetDictionary("ceres_solver_options").get());

4
cartographer/mapping/sparse_pose_graph/proto/optimization_problem_options.proto
View File

@ -36,9 +36,5 @@ message OptimizationProblemOptions {
// If true, the Ceres solver summary will be logged for every optimization.
optional bool log_solver_summary = 5;
// If true, histograms of the final residual values after optimization will be
// logged for every optimization.
optional bool log_residual_histograms = 10;
optional common.proto.CeresSolverOptions ceres_solver_options = 7;
}

2
cartographer/mapping_2d/global_trajectory_builder.cc
View File

@ -54,6 +54,8 @@ void GlobalTrajectoryBuilder::AddImuData(
const Eigen::Vector3d& angular_velocity) {
local_trajectory_builder_.AddImuData(time, linear_acceleration,
angular_velocity);
sparse_pose_graph_->AddImuData(local_trajectory_builder_.submaps(), time,
linear_acceleration, angular_velocity);
}
void GlobalTrajectoryBuilder::AddOdometerData(const common::Time time,

56
cartographer/mapping_2d/sparse_pose_graph.cc
View File

@ -127,7 +127,7 @@ void SparsePoseGraph::AddScan(
}
AddWorkItem([=]() REQUIRES(mutex_) {
ComputeConstraintsForScan(j, matching_submap, insertion_submaps,
ComputeConstraintsForScan(time, j, matching_submap, insertion_submaps,
finished_submap, pose, covariance);
});
}
@ -140,11 +140,22 @@ void SparsePoseGraph::AddWorkItem(std::function<void()> work_item) {
}
}
void SparsePoseGraph::AddImuData(const mapping::Submaps* trajectory,
common::Time time,
const Eigen::Vector3d& linear_acceleration,
const Eigen::Vector3d& angular_velocity) {
common::MutexLocker locker(&mutex_);
AddWorkItem([=]() REQUIRES(mutex_) {
optimization_problem_.AddImuData(trajectory, time, linear_acceleration,
angular_velocity);
});
}
void SparsePoseGraph::ComputeConstraint(const int scan_index,
const int submap_index) {
const transform::Rigid2d relative_pose =
submap_transforms_[submap_index].inverse() *
point_cloud_poses_[scan_index];
optimization_problem_.node_data().at(scan_index).point_cloud_pose;
const mapping::Submaps* const scan_trajectory =
trajectory_nodes_[scan_index].constant_data->trajectory;
@ -176,9 +187,10 @@ void SparsePoseGraph::ComputeConstraint(const int scan_index,
void SparsePoseGraph::ComputeConstraintsForOldScans(
const mapping::Submap* submap) {
const int submap_index = GetSubmapIndex(submap);
CHECK_GT(point_cloud_poses_.size(), 0);
CHECK_LT(point_cloud_poses_.size(), std::numeric_limits<int>::max());
const int num_nodes = point_cloud_poses_.size();
const auto& node_data = optimization_problem_.node_data();
CHECK_GT(node_data.size(), 0);
CHECK_LT(node_data.size(), std::numeric_limits<int>::max());
const int num_nodes = node_data.size();
for (int scan_index = 0; scan_index < num_nodes; ++scan_index) {
if (submap_states_[submap_index].scan_indices.count(scan_index) == 0) {
ComputeConstraint(scan_index, submap_index);
@ -187,7 +199,8 @@ void SparsePoseGraph::ComputeConstraintsForOldScans(
}
void SparsePoseGraph::ComputeConstraintsForScan(
int scan_index, const mapping::Submap* matching_submap,
const common::Time time, 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) {
@ -196,9 +209,11 @@ void SparsePoseGraph::ComputeConstraintsForScan(
const transform::Rigid2d optimized_pose =
submap_transforms_[matching_index] *
sparse_pose_graph::ComputeSubmapPose(*matching_submap).inverse() * pose;
CHECK_EQ(scan_index, point_cloud_poses_.size());
initial_point_cloud_poses_.push_back(pose);
point_cloud_poses_.push_back(optimized_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);
for (const mapping::Submap* submap : insertion_submaps) {
const int submap_index = GetSubmapIndex(submap);
CHECK(!submap_states_[submap_index].finished);
@ -206,8 +221,8 @@ void SparsePoseGraph::ComputeConstraintsForScan(
// Unchanged covariance as (submap <- map) is a translation.
const transform::Rigid2d constraint_transform =
sparse_pose_graph::ComputeSubmapPose(*submap).inverse() * pose;
constexpr double kFakePositionCovariance = 1.;
constexpr double kFakeOrientationCovariance = 1.;
constexpr double kFakePositionCovariance = 1e-6;
constexpr double kFakeOrientationCovariance = 1e-6;
constraints_.push_back(Constraint{
submap_index,
scan_index,
@ -318,24 +333,15 @@ void SparsePoseGraph::RunFinalOptimization() {
void SparsePoseGraph::RunOptimization() {
if (!submap_transforms_.empty()) {
std::vector<const mapping::Submaps*> trajectories;
{
common::MutexLocker locker(&mutex_);
CHECK(!submap_states_.empty());
for (const auto& trajectory_node : trajectory_nodes_) {
trajectories.push_back(trajectory_node.constant_data->trajectory);
}
}
optimization_problem_.Solve(constraints_, trajectories,
initial_point_cloud_poses_, &point_cloud_poses_,
&submap_transforms_);
optimization_problem_.Solve(constraints_, &submap_transforms_);
common::MutexLocker locker(&mutex_);
has_new_optimized_poses_ = true;
const size_t num_optimized_poses = point_cloud_poses_.size();
const auto& node_data = optimization_problem_.node_data();
const size_t num_optimized_poses = node_data.size();
for (size_t i = 0; i != num_optimized_poses; ++i) {
trajectory_nodes_[i].pose =
transform::Rigid3d(transform::Embed3D(point_cloud_poses_[i]));
transform::Rigid3d(transform::Embed3D(node_data[i].point_cloud_pose));
}
// Extrapolate all point cloud poses that were added later.
std::unordered_map<const mapping::Submaps*, transform::Rigid3d>

9
cartographer/mapping_2d/sparse_pose_graph.h
View File

@ -80,6 +80,11 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
const std::vector<const mapping::Submap*>& insertion_submaps)
EXCLUDES(mutex_);
// Adds new IMU data to be used in the optimization.
void AddImuData(const mapping::Submaps* trajectory, common::Time time,
const Eigen::Vector3d& linear_acceleration,
const Eigen::Vector3d& angular_velocity);
void RunFinalOptimization() override;
bool HasNewOptimizedPoses() override;
mapping::proto::ScanMatchingProgress GetScanMatchingProgress() override;
@ -127,7 +132,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
// Adds constraints for a scan, and starts scan matching in the background.
void ComputeConstraintsForScan(
int scan_index, const mapping::Submap* matching_submap,
common::Time time, 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_);
@ -183,8 +188,6 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
sparse_pose_graph::OptimizationProblem optimization_problem_;
sparse_pose_graph::ConstraintBuilder constraint_builder_ GUARDED_BY(mutex_);
std::vector<Constraint> constraints_;
std::vector<transform::Rigid2d> initial_point_cloud_poses_;
std::vector<transform::Rigid2d> point_cloud_poses_; // (map <- point cloud)
std::vector<transform::Rigid2d> submap_transforms_; // (map <- submap)
// Submaps get assigned an index and state as soon as they are seen, even

2
cartographer/mapping_2d/sparse_pose_graph/CMakeLists.txt
View File

@ -55,8 +55,10 @@ google_library(mapping_2d_sparse_pose_graph_optimization_problem
common_histogram
common_math
common_port
common_time
mapping_2d_sparse_pose_graph_spa_cost_function
mapping_2d_submaps
mapping_3d_imu_integration
mapping_sparse_pose_graph
mapping_sparse_pose_graph_proto_optimization_problem_options
transform_transform

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

@ -228,8 +228,8 @@ void ConstraintBuilder::ComputeConstraint(
const transform::Rigid2d constraint_transform =
ComputeSubmapPose(*submap).inverse() * pose_estimate;
constexpr double kFakePositionCovariance = 1.;
constexpr double kFakeOrientationCovariance = 1.;
constexpr double kFakePositionCovariance = 1e-6;
constexpr double kFakeOrientationCovariance = 1e-6;
constraint->reset(new Constraint{
submap_index,
scan_index,

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

@ -59,6 +59,22 @@ OptimizationProblem::OptimizationProblem(
OptimizationProblem::~OptimizationProblem() {}
void OptimizationProblem::AddImuData(const mapping::Submaps* const trajectory,
const common::Time time,
const Eigen::Vector3d& linear_acceleration,
const Eigen::Vector3d& angular_velocity) {
imu_data_[trajectory].push_back(
mapping_3d::ImuData{time, linear_acceleration, angular_velocity});
}
void OptimizationProblem::AddTrajectoryNode(
const mapping::Submaps* const trajectory, const common::Time time,
const transform::Rigid2d& initial_point_cloud_pose,
const transform::Rigid2d& point_cloud_pose) {
node_data_.push_back(
NodeData{trajectory, time, initial_point_cloud_pose, point_cloud_pose});
}
void OptimizationProblem::SetMaxNumIterations(const int32 max_num_iterations) {
options_.mutable_ceres_solver_options()->set_max_num_iterations(
max_num_iterations);
@ -66,11 +82,8 @@ void OptimizationProblem::SetMaxNumIterations(const int32 max_num_iterations) {
void OptimizationProblem::Solve(
const std::vector<Constraint>& constraints,
const std::vector<const mapping::Submaps*>& trajectories,
const std::vector<transform::Rigid2d>& initial_point_cloud_poses,
std::vector<transform::Rigid2d>* point_cloud_poses,
std::vector<transform::Rigid2d>* submap_transforms) {
if (point_cloud_poses->empty()) {
std::vector<transform::Rigid2d>* const submap_transforms) {
if (node_data_.empty()) {
// Nothing to optimize.
return;
}
@ -80,13 +93,13 @@ void OptimizationProblem::Solve(
// Set the starting point.
std::vector<std::array<double, 3>> C_submaps(submap_transforms->size());
std::vector<std::array<double, 3>> C_point_clouds(point_cloud_poses->size());
std::vector<std::array<double, 3>> C_point_clouds(node_data_.size());
for (size_t i = 0; i != submap_transforms->size(); ++i) {
C_submaps[i] = FromPose((*submap_transforms)[i]);
problem.AddParameterBlock(C_submaps[i].data(), 3);
}
for (size_t j = 0; j != point_cloud_poses->size(); ++j) {
C_point_clouds[j] = FromPose((*point_cloud_poses)[j]);
for (size_t j = 0; j != node_data_.size(); ++j) {
C_point_clouds[j] = FromPose(node_data_[j].point_cloud_pose);
problem.AddParameterBlock(C_point_clouds[j].data(), 3);
}
@ -100,7 +113,7 @@ void OptimizationProblem::Solve(
CHECK_GE(constraint.i, 0);
CHECK_LT(constraint.i, submap_transforms->size());
CHECK_GE(constraint.j, 0);
CHECK_LT(constraint.j, point_cloud_poses->size());
CHECK_LT(constraint.j, node_data_.size());
constraints_residual_blocks.emplace_back(
constraint.tag,
problem.AddResidualBlock(
@ -115,22 +128,19 @@ void OptimizationProblem::Solve(
}
// Add penalties for changes between consecutive scans.
CHECK(!point_cloud_poses->empty());
const Eigen::DiagonalMatrix<double, 3> consecutive_pose_change_penalty_matrix(
options_.consecutive_scan_translation_penalty_factor(),
options_.consecutive_scan_translation_penalty_factor(),
options_.consecutive_scan_rotation_penalty_factor());
CHECK_GE(initial_point_cloud_poses.size(), point_cloud_poses->size());
CHECK_GE(trajectories.size(), point_cloud_poses->size());
// The poses in initial_point_cloud_poses and point_cloud_poses are
// interleaved from multiple trajectories (although the points from a given
// trajectory are in time order). 'last_pose_indices[trajectory]' is the index
// into 'initial_point_cloud_poses' of the most-recent pose on 'trajectory'.
// The poses in 'node_data_' are interleaved from multiple trajectories
// (although the points from a given trajectory are in time order).
// 'last_pose_indices[trajectory]' is the index of the most-recent pose on
// 'trajectory'.
std::map<const mapping::Submaps*, int> last_pose_indices;
for (size_t j = 0; j != point_cloud_poses->size(); ++j) {
const mapping::Submaps* trajectory = trajectories[j];
for (size_t j = 0; j != node_data_.size(); ++j) {
const mapping::Submaps* trajectory = node_data_[j].trajectory;
// This pose has a predecessor.
if (last_pose_indices.count(trajectory) != 0) {
const int last_pose_index = last_pose_indices[trajectory];
@ -139,9 +149,9 @@ void OptimizationProblem::Solve(
problem.AddResidualBlock(
new ceres::AutoDiffCostFunction<SpaCostFunction, 3, 3, 3>(
new SpaCostFunction(Constraint::Pose{
transform::Embed3D(
initial_point_cloud_poses[last_pose_index].inverse() *
initial_point_cloud_poses[j]),
transform::Embed3D(node_data_[last_pose_index]
.initial_point_cloud_pose.inverse() *
node_data_[j].initial_point_cloud_pose),
kalman_filter::Embed3D(consecutive_pose_change_penalty_matrix,
kUnusedPositionPenalty,
kUnusedOrientationPenalty)})),
@ -157,41 +167,6 @@ void OptimizationProblem::Solve(
common::CreateCeresSolverOptions(options_.ceres_solver_options()),
&problem, &summary);
if (options_.log_residual_histograms()) {
common::Histogram intra_submap_xy_residuals;
common::Histogram intra_submap_theta_residuals;
common::Histogram inter_submap_xy_residuals;
common::Histogram inter_submap_theta_residuals;
for (auto constraint_residual_block : constraints_residual_blocks) {
ceres::Problem::EvaluateOptions options;
options.apply_loss_function = false;
options.residual_blocks = {constraint_residual_block.second};
std::vector<double> residuals;
problem.Evaluate(options, nullptr, &residuals, nullptr, nullptr);
CHECK_EQ(3, residuals.size());
switch (constraint_residual_block.first) {
case Constraint::INTRA_SUBMAP:
intra_submap_xy_residuals.Add(common::Pow2(residuals[0]) +
common::Pow2(residuals[1]));
intra_submap_theta_residuals.Add(common::Pow2(residuals[2]));
break;
case Constraint::INTER_SUBMAP:
inter_submap_xy_residuals.Add(common::Pow2(residuals[0]) +
common::Pow2(residuals[1]));
inter_submap_theta_residuals.Add(common::Pow2(residuals[2]));
break;
}
}
LOG(INFO) << "Intra-submap x^2 + y^2 residual histogram:\n"
<< intra_submap_xy_residuals.ToString(10);
LOG(INFO) << "Intra-submap theta^2 residual histogram:\n"
<< intra_submap_theta_residuals.ToString(10);
LOG(INFO) << "Inter-submap x^2 + y^2 residual histogram:\n"
<< inter_submap_xy_residuals.ToString(10);
LOG(INFO) << "Inter-submap theta^2 residual histogram:\n"
<< inter_submap_theta_residuals.ToString(10);
}
if (options_.log_solver_summary()) {
LOG(INFO) << summary.FullReport();
}
@ -200,11 +175,15 @@ void OptimizationProblem::Solve(
for (size_t i = 0; i != submap_transforms->size(); ++i) {
(*submap_transforms)[i] = ToPose(C_submaps[i]);
}
for (size_t j = 0; j != point_cloud_poses->size(); ++j) {
(*point_cloud_poses)[j] = ToPose(C_point_clouds[j]);
for (size_t j = 0; j != node_data_.size(); ++j) {
node_data_[j].point_cloud_pose = ToPose(C_point_clouds[j]);
}
}
const std::vector<NodeData>& OptimizationProblem::node_data() const {
return node_data_;
}
} // namespace sparse_pose_graph
} // namespace mapping_2d
} // namespace cartographer

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

@ -24,14 +24,24 @@
#include "Eigen/Core"
#include "Eigen/Geometry"
#include "cartographer/common/port.h"
#include "cartographer/common/time.h"
#include "cartographer/mapping/sparse_pose_graph.h"
#include "cartographer/mapping/sparse_pose_graph/proto/optimization_problem_options.pb.h"
#include "cartographer/mapping_2d/submaps.h"
#include "cartographer/mapping_3d/imu_integration.h"
namespace cartographer {
namespace mapping_2d {
namespace sparse_pose_graph {
struct NodeData {
// TODO(whess): Keep nodes per trajectory instead.
const mapping::Submaps* trajectory;
common::Time time;
transform::Rigid2d initial_point_cloud_pose;
transform::Rigid2d point_cloud_pose;
};
// Implements the SPA loop closure method.
class OptimizationProblem {
public:
@ -45,19 +55,25 @@ class OptimizationProblem {
OptimizationProblem(const OptimizationProblem&) = delete;
OptimizationProblem& operator=(const OptimizationProblem&) = delete;
void AddImuData(const mapping::Submaps* trajectory, common::Time time,
const Eigen::Vector3d& linear_acceleration,
const Eigen::Vector3d& angular_velocity);
void AddTrajectoryNode(const mapping::Submaps* trajectory, common::Time time,
const transform::Rigid2d& initial_point_cloud_pose,
const transform::Rigid2d& point_cloud_pose);
void SetMaxNumIterations(int32 max_num_iterations);
// Computes the optimized poses. The point cloud at 'point_cloud_poses[i]'
// belongs to 'trajectories[i]'. Within a given trajectory, scans are expected
// to be contiguous.
// Computes the optimized poses.
void Solve(const std::vector<Constraint>& constraints,
const std::vector<const mapping::Submaps*>& trajectories,
const std::vector<transform::Rigid2d>& initial_point_cloud_poses,
std::vector<transform::Rigid2d>* point_cloud_poses,
std::vector<transform::Rigid2d>* submap_transforms);
const std::vector<NodeData>& node_data() const;
private:
mapping::sparse_pose_graph::proto::OptimizationProblemOptions options_;
std::map<const mapping::Submaps*, std::deque<mapping_3d::ImuData>> imu_data_;
std::vector<NodeData> node_data_;
};
} // namespace sparse_pose_graph

1
cartographer/mapping_2d/sparse_pose_graph_test.cc
View File

@ -125,7 +125,6 @@ class SparsePoseGraphTest : public ::testing::Test {
consecutive_scan_translation_penalty_factor = 0.,
consecutive_scan_rotation_penalty_factor = 0.,
log_solver_summary = true,
log_residual_histograms = true,
ceres_solver_options = {
use_nonmonotonic_steps = false,
max_num_iterations = 200,

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

@ -30,7 +30,6 @@
#include "cartographer/common/time.h"
#include "cartographer/mapping_3d/acceleration_cost_function.h"
#include "cartographer/mapping_3d/ceres_pose.h"
#include "cartographer/mapping_3d/imu_integration.h"
#include "cartographer/mapping_3d/rotation_cost_function.h"
#include "cartographer/mapping_3d/sparse_pose_graph/spa_cost_function.h"
#include "cartographer/transform/transform.h"

1
cartographer/mapping_3d/sparse_pose_graph/optimization_problem_test.cc
View File

@ -47,7 +47,6 @@ class OptimizationProblemTest : public ::testing::Test {
consecutive_scan_translation_penalty_factor = 1e-2,
consecutive_scan_rotation_penalty_factor = 1e-2,
log_solver_summary = true,
log_residual_histograms = true,
ceres_solver_options = {
use_nonmonotonic_steps = false,
max_num_iterations = 200,

1
configuration_files/sparse_pose_graph.lua
View File

@ -71,7 +71,6 @@ SPARSE_POSE_GRAPH = {
consecutive_scan_translation_penalty_factor = 1e5,
consecutive_scan_rotation_penalty_factor = 1e5,
log_solver_summary = false,
log_residual_histograms = false,
ceres_solver_options = {
use_nonmonotonic_steps = false,
max_num_iterations = 50,