diff --git a/cartographer/io/frame_id_filtering_points_processor.cc b/cartographer/io/frame_id_filtering_points_processor.cc index 089d453..56aea17 100644 --- a/cartographer/io/frame_id_filtering_points_processor.cc +++ b/cartographer/io/frame_id_filtering_points_processor.cc @@ -26,8 +26,7 @@ namespace io { std::unique_ptr FrameIdFilteringPointsProcessor::FromDictionary( - common::LuaParameterDictionary* dictionary, - PointsProcessor* next) { + common::LuaParameterDictionary* dictionary, PointsProcessor* next) { std::vector keep_frames, drop_frames; if (dictionary->HasKey("keep_frames")) { keep_frames = @@ -44,8 +43,7 @@ FrameIdFilteringPointsProcessor::FromDictionary( FrameIdFilteringPointsProcessor::FrameIdFilteringPointsProcessor( const std::unordered_set& keep_frame_ids, - const std::unordered_set& drop_frame_ids, - PointsProcessor* next) + const std::unordered_set& drop_frame_ids, PointsProcessor* next) : keep_frame_ids_(keep_frame_ids), drop_frame_ids_(drop_frame_ids), next_(next) { diff --git a/cartographer/io/frame_id_filtering_points_processor.h b/cartographer/io/frame_id_filtering_points_processor.h index 5e4f272..1563e60 100644 --- a/cartographer/io/frame_id_filtering_points_processor.h +++ b/cartographer/io/frame_id_filtering_points_processor.h @@ -33,11 +33,9 @@ class FrameIdFilteringPointsProcessor : public PointsProcessor { constexpr static const char* kConfigurationFileActionName = "frame_id_filter"; FrameIdFilteringPointsProcessor( const std::unordered_set& keep_frame_ids, - const std::unordered_set& drop_frame_ids, - PointsProcessor* next); + const std::unordered_set& drop_frame_ids, PointsProcessor* next); static std::unique_ptr FromDictionary( - common::LuaParameterDictionary* dictionary, - PointsProcessor* next); + common::LuaParameterDictionary* dictionary, PointsProcessor* next); ~FrameIdFilteringPointsProcessor() override {} FrameIdFilteringPointsProcessor(const FrameIdFilteringPointsProcessor&) = diff --git a/cartographer/io/points_processor_pipeline_builder.cc b/cartographer/io/points_processor_pipeline_builder.cc index bea92da..f7a4f31 100644 --- a/cartographer/io/points_processor_pipeline_builder.cc +++ b/cartographer/io/points_processor_pipeline_builder.cc @@ -19,8 +19,8 @@ #include "cartographer/common/make_unique.h" #include "cartographer/io/coloring_points_processor.h" #include "cartographer/io/counting_points_processor.h" -#include "cartographer/io/frame_id_filtering_points_processor.h" #include "cartographer/io/fixed_ratio_sampling_points_processor.h" +#include "cartographer/io/frame_id_filtering_points_processor.h" #include "cartographer/io/hybrid_grid_points_processor.h" #include "cartographer/io/intensity_to_color_points_processor.h" #include "cartographer/io/min_max_range_filtering_points_processor.h" diff --git a/cartographer/mapping/id.h b/cartographer/mapping/id.h index a9e10f4..ba7af52 100644 --- a/cartographer/mapping/id.h +++ b/cartographer/mapping/id.h @@ -33,7 +33,7 @@ struct NodeId { bool operator==(const NodeId& other) const { return std::forward_as_tuple(trajectory_id, node_index) == - std::forward_as_tuple(other.trajectory_id, other.node_index); + std::forward_as_tuple(other.trajectory_id, other.node_index); } bool operator!=(const NodeId& other) const { return !operator==(other); } diff --git a/cartographer/mapping_2d/sparse_pose_graph.cc b/cartographer/mapping_2d/sparse_pose_graph.cc index ee720eb..18d5801 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.cc +++ b/cartographer/mapping_2d/sparse_pose_graph.cc @@ -102,10 +102,10 @@ void SparsePoseGraph::AddScan( GetLocalToGlobalTransform(trajectory_id) * constant_data->initial_pose); common::MutexLocker locker(&mutex_); + AddTrajectoryIfNeeded(trajectory_id); trajectory_nodes_.Append( trajectory_id, mapping::TrajectoryNode{constant_data, optimized_pose}); ++num_trajectory_nodes_; - trajectory_connectivity_state_.Add(trajectory_id); // Test if the 'insertion_submap.back()' is one we never saw before. if (trajectory_id >= submap_data_.num_trajectories() || @@ -121,13 +121,6 @@ void SparsePoseGraph::AddScan( submap_data_.at(submap_id).submap = insertion_submaps.back(); } - // Make sure we have a sampler for this trajectory. - if (!global_localization_samplers_[trajectory_id]) { - global_localization_samplers_[trajectory_id] = - common::make_unique( - options_.global_sampling_ratio()); - } - // We have to check this here, because it might have changed by the time we // execute the lambda. const bool newly_finished_submap = insertion_submaps.front()->finished(); @@ -145,6 +138,16 @@ void SparsePoseGraph::AddWorkItem(const std::function& work_item) { } } +void SparsePoseGraph::AddTrajectoryIfNeeded(const int trajectory_id) { + trajectory_connectivity_state_.Add(trajectory_id); + // Make sure we have a sampler for this trajectory. + if (!global_localization_samplers_[trajectory_id]) { + global_localization_samplers_[trajectory_id] = + common::make_unique( + options_.global_sampling_ratio()); + } +} + void SparsePoseGraph::AddImuData(const int trajectory_id, const sensor::ImuData& imu_data) { common::MutexLocker locker(&mutex_); @@ -232,15 +235,15 @@ void SparsePoseGraph::ComputeConstraintsForScan( const mapping::NodeId node_id{ matching_id.trajectory_id, static_cast(matching_id.trajectory_id) < - optimization_problem_.node_data().size() && - !optimization_problem_.node_data()[matching_id.trajectory_id] - .empty() - ? static_cast(optimization_problem_.node_data() - .at(matching_id.trajectory_id) - .rbegin() - ->first + - 1) - : 0}; + optimization_problem_.node_data().size() && + !optimization_problem_.node_data()[matching_id.trajectory_id] + .empty() + ? static_cast(optimization_problem_.node_data() + .at(matching_id.trajectory_id) + .rbegin() + ->first + + 1) + : 0}; const auto& constant_data = trajectory_nodes_.at(node_id).constant_data; const transform::Rigid2d pose = transform::Project2D( constant_data->initial_pose * @@ -419,7 +422,7 @@ void SparsePoseGraph::AddSubmapFromProto(const int trajectory_id, const transform::Rigid2d initial_pose_2d = transform::Project2D(initial_pose); common::MutexLocker locker(&mutex_); - trajectory_connectivity_state_.Add(trajectory_id); + AddTrajectoryIfNeeded(trajectory_id); const mapping::SubmapId submap_id = submap_data_.Append(trajectory_id, SubmapData()); submap_data_.at(submap_id).submap = submap_ptr; @@ -447,7 +450,7 @@ void SparsePoseGraph::AddNodeFromProto(const int trajectory_id, mapping::FromProto(node.node_data())); common::MutexLocker locker(&mutex_); - trajectory_connectivity_state_.Add(trajectory_id); + AddTrajectoryIfNeeded(trajectory_id); const mapping::NodeId node_id = trajectory_nodes_.Append( trajectory_id, mapping::TrajectoryNode{constant_data, pose}); diff --git a/cartographer/mapping_2d/sparse_pose_graph.h b/cartographer/mapping_2d/sparse_pose_graph.h index 6ea1eae..a0bd973 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.h +++ b/cartographer/mapping_2d/sparse_pose_graph.h @@ -39,6 +39,7 @@ #include "cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h" #include "cartographer/mapping_2d/submaps.h" #include "cartographer/sensor/fixed_frame_pose_data.h" +#include "cartographer/sensor/odometry_data.h" #include "cartographer/sensor/point_cloud.h" #include "cartographer/transform/rigid_transform.h" #include "cartographer/transform/transform.h" @@ -121,6 +122,9 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { // Handles a new work item. void AddWorkItem(const std::function& work_item) REQUIRES(mutex_); + // Adds connectivity and sampler for a trajectory if it does not exist. + void AddTrajectoryIfNeeded(int trajectory_id) REQUIRES(mutex_); + // Grows the optimization problem to have an entry for every element of // 'insertion_submaps'. Returns the IDs for the 'insertion_submaps'. std::vector GrowSubmapTransformsAsNeeded( diff --git a/cartographer/mapping_3d/sparse_pose_graph.cc b/cartographer/mapping_3d/sparse_pose_graph.cc index f3eca30..169b4c4 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.cc +++ b/cartographer/mapping_3d/sparse_pose_graph.cc @@ -100,10 +100,10 @@ void SparsePoseGraph::AddScan( GetLocalToGlobalTransform(trajectory_id) * constant_data->initial_pose); common::MutexLocker locker(&mutex_); + AddTrajectoryIfNeeded(trajectory_id); trajectory_nodes_.Append( trajectory_id, mapping::TrajectoryNode{constant_data, optimized_pose}); ++num_trajectory_nodes_; - trajectory_connectivity_state_.Add(trajectory_id); // Test if the 'insertion_submap.back()' is one we never saw before. if (trajectory_id >= submap_data_.num_trajectories() || @@ -119,13 +119,6 @@ void SparsePoseGraph::AddScan( submap_data_.at(submap_id).submap = insertion_submaps.back(); } - // Make sure we have a sampler for this trajectory. - if (!global_localization_samplers_[trajectory_id]) { - global_localization_samplers_[trajectory_id] = - common::make_unique( - options_.global_sampling_ratio()); - } - // We have to check this here, because it might have changed by the time we // execute the lambda. const bool newly_finished_submap = insertion_submaps.front()->finished(); @@ -143,6 +136,16 @@ void SparsePoseGraph::AddWorkItem(const std::function& work_item) { } } +void SparsePoseGraph::AddTrajectoryIfNeeded(const int trajectory_id) { + trajectory_connectivity_state_.Add(trajectory_id); + // Make sure we have a sampler for this trajectory. + if (!global_localization_samplers_[trajectory_id]) { + global_localization_samplers_[trajectory_id] = + common::make_unique( + options_.global_sampling_ratio()); + } +} + void SparsePoseGraph::AddImuData(const int trajectory_id, const sensor::ImuData& imu_data) { common::MutexLocker locker(&mutex_); @@ -258,15 +261,15 @@ void SparsePoseGraph::ComputeConstraintsForScan( const mapping::NodeId node_id{ matching_id.trajectory_id, static_cast(matching_id.trajectory_id) < - optimization_problem_.node_data().size() && - !optimization_problem_.node_data()[matching_id.trajectory_id] - .empty() - ? static_cast(optimization_problem_.node_data() - .at(matching_id.trajectory_id) - .rbegin() - ->first + - 1) - : 0}; + optimization_problem_.node_data().size() && + !optimization_problem_.node_data()[matching_id.trajectory_id] + .empty() + ? static_cast(optimization_problem_.node_data() + .at(matching_id.trajectory_id) + .rbegin() + ->first + + 1) + : 0}; const auto& constant_data = trajectory_nodes_.at(node_id).constant_data; const transform::Rigid3d& pose = constant_data->initial_pose; const transform::Rigid3d optimized_pose = @@ -438,7 +441,7 @@ void SparsePoseGraph::AddSubmapFromProto(const int trajectory_id, std::make_shared(submap.submap_3d()); common::MutexLocker locker(&mutex_); - trajectory_connectivity_state_.Add(trajectory_id); + AddTrajectoryIfNeeded(trajectory_id); const mapping::SubmapId submap_id = submap_data_.Append(trajectory_id, SubmapData()); submap_data_.at(submap_id).submap = submap_ptr; @@ -466,7 +469,7 @@ void SparsePoseGraph::AddNodeFromProto(const int trajectory_id, mapping::FromProto(node.node_data())); common::MutexLocker locker(&mutex_); - trajectory_connectivity_state_.Add(trajectory_id); + AddTrajectoryIfNeeded(trajectory_id); const mapping::NodeId node_id = trajectory_nodes_.Append( trajectory_id, mapping::TrajectoryNode{constant_data, pose}); diff --git a/cartographer/mapping_3d/sparse_pose_graph.h b/cartographer/mapping_3d/sparse_pose_graph.h index 4442d57..b0a99ea 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.h +++ b/cartographer/mapping_3d/sparse_pose_graph.h @@ -122,6 +122,9 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { // Handles a new work item. void AddWorkItem(const std::function& work_item) REQUIRES(mutex_); + // Adds connectivity and sampler for a trajectory if it does not exist. + void AddTrajectoryIfNeeded(int trajectory_id) REQUIRES(mutex_); + // Grows the optimization problem to have an entry for every element of // 'insertion_submaps'. Returns the IDs for the 'insertion_submaps'. std::vector GrowSubmapTransformsAsNeeded(