Improvement of 2D/3D consistency and cleanup. (#122)

master
Wolfgang Hess 2016-11-11 14:33:06 +01:00 committed by GitHub
parent 97d5cb2a8b
commit 1de387cba9
9 changed files with 44 additions and 40 deletions

View File

@ -118,6 +118,7 @@ google_library(mapping_2d_sparse_pose_graph
common_fixed_ratio_sampler
common_make_unique
common_math
common_mutex
common_thread_pool
common_time
kalman_filter_pose_tracker
@ -131,6 +132,7 @@ google_library(mapping_2d_sparse_pose_graph
sensor_compressed_point_cloud
sensor_point_cloud
sensor_voxel_filter
transform_rigid_transform
transform_transform
)

View File

@ -30,7 +30,6 @@
#include "Eigen/Eigenvalues"
#include "cartographer/common/make_unique.h"
#include "cartographer/common/math.h"
#include "cartographer/common/time.h"
#include "cartographer/mapping/proto/scan_matching_progress.pb.h"
#include "cartographer/mapping/sparse_pose_graph/proto/constraint_builder_options.pb.h"
#include "cartographer/sensor/compressed_point_cloud.h"
@ -101,7 +100,7 @@ void SparsePoseGraph::AddScan(
constant_node_data_->push_back(mapping::TrajectoryNode::ConstantData{
time, laser_fan_in_pose,
Compress(sensor::LaserFan{Eigen::Vector3f::Zero(), {}, {}, {}}), submaps,
transform::Rigid3d(tracking_to_pose)});
tracking_to_pose});
trajectory_nodes_.push_back(mapping::TrajectoryNode{
&constant_node_data_->back(), optimized_pose,
});
@ -126,10 +125,19 @@ void SparsePoseGraph::AddScan(
common::make_unique<common::FixedRatioSampler>(
options_.global_sampling_ratio());
}
AddWorkItem(
std::bind(std::mem_fn(&SparsePoseGraph::ComputeConstraintsForScan), this,
j, submaps, matching_submap, insertion_submaps, finished_submap,
pose, covariance));
AddWorkItem([=]() REQUIRES(mutex_) {
ComputeConstraintsForScan(j, submaps, matching_submap, insertion_submaps,
finished_submap, pose, covariance);
});
}
void SparsePoseGraph::AddWorkItem(std::function<void()> work_item) {
if (scan_queue_ == nullptr) {
work_item();
} else {
scan_queue_->push_back(work_item);
}
}
void SparsePoseGraph::ComputeConstraintsForOldScans(
@ -301,14 +309,6 @@ void SparsePoseGraph::WaitForAllComputations() {
locker.Await([&notification]() { return notification; });
}
void SparsePoseGraph::AddWorkItem(std::function<void()> work_item) {
if (scan_queue_ == nullptr) {
work_item();
} else {
scan_queue_->push_back(work_item);
}
}
void SparsePoseGraph::RunFinalOptimization() {
WaitForAllComputations();
optimization_problem_.SetMaxNumIterations(

View File

@ -37,6 +37,7 @@
#include "Eigen/Core"
#include "Eigen/Geometry"
#include "cartographer/common/fixed_ratio_sampler.h"
#include "cartographer/common/mutex.h"
#include "cartographer/common/thread_pool.h"
#include "cartographer/common/time.h"
#include "cartographer/kalman_filter/pose_tracker.h"
@ -47,6 +48,7 @@
#include "cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h"
#include "cartographer/mapping_2d/submaps.h"
#include "cartographer/sensor/point_cloud.h"
#include "cartographer/transform/rigid_transform.h"
#include "cartographer/transform/transform.h"
namespace cartographer {
@ -81,7 +83,6 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
void RunFinalOptimization() override;
bool HasNewOptimizedPoses() override;
mapping::proto::ScanMatchingProgress GetScanMatchingProgress() override;
std::vector<std::vector<const mapping::Submaps*>> GetConnectedTrajectories()
override;
std::vector<transform::Rigid3d> GetSubmapTransforms(
@ -111,6 +112,9 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
const mapping::Submaps* trajectory = nullptr;
};
// Handles a new work item.
void AddWorkItem(std::function<void()> work_item) REQUIRES(mutex_);
int GetSubmapIndex(const mapping::Submap* submap) const REQUIRES(mutex_) {
const auto iterator = submap_indices_.find(submap);
CHECK(iterator != submap_indices_.end());
@ -145,9 +149,6 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
// optimization being run at a time.
void RunOptimization() EXCLUDES(mutex_);
// Handles a new 'work_item'.
void AddWorkItem(std::function<void()> work_item) REQUIRES(mutex_);
// Adds extrapolated transforms, so that there are transforms for all submaps.
std::vector<transform::Rigid3d> ExtrapolateSubmapTransforms(
const std::vector<transform::Rigid2d>& submap_transforms,

View File

@ -244,6 +244,7 @@ google_library(mapping_3d_sparse_pose_graph
mapping_sparse_pose_graph
mapping_sparse_pose_graph_proto_constraint_builder_options
mapping_trajectory_connectivity
sensor_compressed_point_cloud
sensor_point_cloud
sensor_voxel_filter
transform_rigid_transform

View File

@ -30,9 +30,9 @@
#include "Eigen/Eigenvalues"
#include "cartographer/common/make_unique.h"
#include "cartographer/common/math.h"
#include "cartographer/common/mutex.h"
#include "cartographer/mapping/proto/scan_matching_progress.pb.h"
#include "cartographer/mapping/sparse_pose_graph/proto/constraint_builder_options.pb.h"
#include "cartographer/sensor/compressed_point_cloud.h"
#include "cartographer/sensor/voxel_filter.h"
#include "glog/logging.h"
@ -176,7 +176,7 @@ void SparsePoseGraph::ComputeConstraintsForScan(
submap_transforms_[matching_index] *
matching_submap->local_pose().inverse() * pose;
CHECK_EQ(scan_index, optimization_problem_.node_data().size());
optimization_problem_.AddTrajectoryNode(time, pose, optimized_pose);
optimization_problem_.AddTrajectoryNode(time, optimized_pose);
for (const Submap* submap : insertion_submaps) {
const int submap_index = GetSubmapIndex(submap);
CHECK(!submap_states_[submap_index].finished);
@ -247,7 +247,7 @@ void SparsePoseGraph::ComputeConstraintsForScan(
run_loop_closure_ = true;
// If there is a 'scan_queue_' already, some other thread will take care.
if (scan_queue_ == nullptr) {
scan_queue_.reset(new std::deque<std::function<void()>>);
scan_queue_ = common::make_unique<std::deque<std::function<void()>>>();
HandleScanQueue();
}
}
@ -338,8 +338,7 @@ void SparsePoseGraph::RunOptimization() {
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(node_data[i].point_cloud_pose);
trajectory_nodes_[i].pose = node_data[i].point_cloud_pose;
}
// Extrapolate all point cloud poses that were added later.
std::unordered_map<const mapping::Submaps*, transform::Rigid3d>

View File

@ -41,6 +41,7 @@
#include "cartographer/common/thread_pool.h"
#include "cartographer/common/time.h"
#include "cartographer/kalman_filter/pose_tracker.h"
#include "cartographer/mapping/proto/scan_matching_progress.pb.h"
#include "cartographer/mapping/sparse_pose_graph.h"
#include "cartographer/mapping/trajectory_connectivity.h"
#include "cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h"

View File

@ -84,10 +84,8 @@ void OptimizationProblem::AddImuData(common::Time time,
}
void OptimizationProblem::AddTrajectoryNode(
common::Time time, const transform::Rigid3d& initial_point_cloud_pose,
const transform::Rigid3d& point_cloud_pose) {
node_data_.push_back(
NodeData{time, initial_point_cloud_pose, point_cloud_pose});
common::Time time, const transform::Rigid3d& point_cloud_pose) {
node_data_.push_back(NodeData{time, point_cloud_pose});
}
void OptimizationProblem::SetMaxNumIterations(const int32 max_num_iterations) {

View File

@ -37,7 +37,6 @@ namespace sparse_pose_graph {
struct NodeData {
common::Time time;
transform::Rigid3d initial_point_cloud_pose;
transform::Rigid3d point_cloud_pose;
};
@ -57,7 +56,6 @@ class OptimizationProblem {
void AddImuData(common::Time time, const Eigen::Vector3d& linear_acceleration,
const Eigen::Vector3d& angular_velocity);
void AddTrajectoryNode(common::Time time,
const transform::Rigid3d& initial_point_cloud_pose,
const transform::Rigid3d& point_cloud_pose);
void SetMaxNumIterations(int32 max_num_iterations);

View File

@ -126,7 +126,7 @@ TEST_F(OptimizationProblemTest, ReducesNoise) {
AddNoise(node.ground_truth_pose, node.noise);
optimization_problem_.AddImuData(now, Eigen::Vector3d::UnitZ() * 9.81,
Eigen::Vector3d::Zero());
optimization_problem_.AddTrajectoryNode(now, pose, pose);
optimization_problem_.AddTrajectoryNode(now, pose);
now += common::FromSeconds(0.01);
}
@ -153,29 +153,33 @@ TEST_F(OptimizationProblemTest, ReducesNoise) {
std::vector<transform::Rigid3d> submap_transforms = {
kSubmap0Transform, kSubmap0Transform, kSubmap2Transform};
double translation_error_before = 0.;
double rotation_error_before = 0.;
const auto& node_data = optimization_problem_.node_data();
for (int j = 0; j != kNumNodes; ++j) {
translation_error_before += (test_data[j].ground_truth_pose.translation() -
node_data[j].point_cloud_pose.translation())
.norm();
rotation_error_before +=
transform::GetAngle(test_data[j].ground_truth_pose.inverse() *
node_data[j].point_cloud_pose);
}
optimization_problem_.Solve(constraints, kSubmap0Transform, trajectories,
&submap_transforms);
double translation_error_before = 0.;
double translation_error_after = 0.;
double rotation_error_before = 0.;
double rotation_error_after = 0.;
const auto& node_data = optimization_problem_.node_data();
for (int j = 0; j != kNumNodes; ++j) {
translation_error_before +=
(test_data[j].ground_truth_pose.translation() -
node_data[j].initial_point_cloud_pose.translation())
.norm();
translation_error_after += (test_data[j].ground_truth_pose.translation() -
node_data[j].point_cloud_pose.translation())
.norm();
rotation_error_before +=
transform::GetAngle(test_data[j].ground_truth_pose.inverse() *
node_data[j].initial_point_cloud_pose);
rotation_error_after +=
transform::GetAngle(test_data[j].ground_truth_pose.inverse() *
node_data[j].point_cloud_pose);
}
EXPECT_GT(0.8 * translation_error_before, translation_error_after);
EXPECT_GT(0.8 * rotation_error_before, rotation_error_after);
}