diff --git a/cartographer/mapping_2d/CMakeLists.txt b/cartographer/mapping_2d/CMakeLists.txt index 342c453..5070487 100644 --- a/cartographer/mapping_2d/CMakeLists.txt +++ b/cartographer/mapping_2d/CMakeLists.txt @@ -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 ) diff --git a/cartographer/mapping_2d/sparse_pose_graph.cc b/cartographer/mapping_2d/sparse_pose_graph.cc index dad8d7a..f72eaa6 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.cc +++ b/cartographer/mapping_2d/sparse_pose_graph.cc @@ -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( 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 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([¬ification]() { return notification; }); } -void SparsePoseGraph::AddWorkItem(std::function work_item) { - if (scan_queue_ == nullptr) { - work_item(); - } else { - scan_queue_->push_back(work_item); - } -} - void SparsePoseGraph::RunFinalOptimization() { WaitForAllComputations(); optimization_problem_.SetMaxNumIterations( diff --git a/cartographer/mapping_2d/sparse_pose_graph.h b/cartographer/mapping_2d/sparse_pose_graph.h index 990d7c7..90ed4a4 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.h +++ b/cartographer/mapping_2d/sparse_pose_graph.h @@ -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> GetConnectedTrajectories() override; std::vector GetSubmapTransforms( @@ -111,6 +112,9 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { const mapping::Submaps* trajectory = nullptr; }; + // Handles a new work item. + void AddWorkItem(std::function 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 work_item) REQUIRES(mutex_); - // Adds extrapolated transforms, so that there are transforms for all submaps. std::vector ExtrapolateSubmapTransforms( const std::vector& submap_transforms, diff --git a/cartographer/mapping_3d/CMakeLists.txt b/cartographer/mapping_3d/CMakeLists.txt index b624377..257a608 100644 --- a/cartographer/mapping_3d/CMakeLists.txt +++ b/cartographer/mapping_3d/CMakeLists.txt @@ -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 diff --git a/cartographer/mapping_3d/sparse_pose_graph.cc b/cartographer/mapping_3d/sparse_pose_graph.cc index 19d409c..7b1ca57 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.cc +++ b/cartographer/mapping_3d/sparse_pose_graph.cc @@ -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>); + scan_queue_ = common::make_unique>>(); 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 diff --git a/cartographer/mapping_3d/sparse_pose_graph.h b/cartographer/mapping_3d/sparse_pose_graph.h index ef7b912..8383644 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.h +++ b/cartographer/mapping_3d/sparse_pose_graph.h @@ -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" diff --git a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc index 99b34b9..cef16d0 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc +++ b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc @@ -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) { diff --git a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h index b28776b..b97d67d 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h +++ b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h @@ -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); diff --git a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem_test.cc b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem_test.cc index 0c3e843..fd476b6 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem_test.cc +++ b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem_test.cc @@ -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 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); }