17 #ifndef CARTOGRAPHER_MAPPING_3D_SPARSE_POSE_GRAPH_H_ 18 #define CARTOGRAPHER_MAPPING_3D_SPARSE_POSE_GRAPH_H_ 24 #include <unordered_map> 28 #include "Eigen/Geometry" 43 namespace mapping_3d {
70 const Submap* matching_submap,
71 const std::vector<const Submap*>& insertion_submaps)
76 const Eigen::Vector3d& linear_acceleration,
77 const Eigen::Vector3d& angular_velocity);
79 void AddTrimmer(std::unique_ptr<mapping::PoseGraphTrimmer> trimmer)
override {
80 LOG(FATAL) <<
"Not yet implemented for 3D.";
114 const auto iterator = submap_ids_.find(submap);
115 CHECK(iterator != submap_ids_.end());
116 return iterator->second;
122 const std::vector<const Submap*>& insertion_submaps)
REQUIRES(
mutex_);
126 std::vector<const Submap*> insertion_submaps,
127 const Submap* finished_submap,
153 const std::vector<std::vector<sparse_pose_graph::SubmapData>>&
157 const mapping::proto::SparsePoseGraphOptions
options_;
162 std::unique_ptr<std::deque<std::function<void()>>> scan_queue_
169 std::unordered_map<int, std::unique_ptr<common::FixedRatioSampler>>
170 global_localization_samplers_
GUARDED_BY(mutex_);
173 int num_scans_since_last_loop_closure_
GUARDED_BY(mutex_) = 0;
176 bool run_loop_closure_
GUARDED_BY(mutex_) =
false;
181 std::vector<Constraint> constraints_
GUARDED_BY(mutex_);
185 std::map<const mapping::Submap*, mapping::SubmapId> submap_ids_
198 int num_trajectory_nodes_
GUARDED_BY(mutex_) = 0;
201 std::vector<std::vector<sparse_pose_graph::SubmapData>>
202 optimized_submap_transforms_
GUARDED_BY(mutex_);
208 #endif // CARTOGRAPHER_MAPPING_3D_SPARSE_POSE_GRAPH_H_ void GrowSubmapTransformsAsNeeded(const std::vector< const Submap * > &insertion_submaps) REQUIRES(mutex_)
void RunOptimization() EXCLUDES(mutex_)
const mapping::proto::SparsePoseGraphOptions options_
void AddWorkItem(std::function< void()> work_item) REQUIRES(mutex_)
void ComputeConstraintsForScan(const Submap *matching_submap, std::vector< const Submap * > insertion_submaps, const Submap *finished_submap, const transform::Rigid3d &pose) REQUIRES(mutex_)
std::vector< std::vector< mapping::TrajectoryNode > > GetTrajectoryNodes() override EXCLUDES(mutex_)
std::vector< Constraint > constraints() override EXCLUDES(mutex_)
void ComputeConstraint(const mapping::NodeId &node_id, const mapping::SubmapId &submap_id) REQUIRES(mutex_)
void HandleScanQueue() REQUIRES(mutex_)
std::map< int, size_t > reverse_connected_components_
SparsePoseGraph & operator=(const SparsePoseGraph &)=delete
UniversalTimeScaleClock::time_point Time
std::set< mapping::NodeId > node_ids
std::vector< std::vector< int > > connected_components_
~SparsePoseGraph() override
transform::Rigid3d GetLocalToGlobalTransform(int trajectory_id) EXCLUDES(mutex_) override
void AddImuData(int trajectory_id, common::Time time, const Eigen::Vector3d &linear_acceleration, const Eigen::Vector3d &angular_velocity)
std::unique_ptr< std::deque< std::function< void()> > > scan_queue_ GUARDED_BY(mutex_)
void WaitForAllComputations() EXCLUDES(mutex_)
transform::Rigid3d ComputeLocalToGlobalTransform(const std::vector< std::vector< sparse_pose_graph::SubmapData >> &submap_transforms, int trajectory_id) const REQUIRES(mutex_)
mapping::SubmapId GetSubmapId(const mapping::Submap *submap) const REQUIRES(mutex_)
transform::Rigid3d GetSubmapTransform(const mapping::SubmapId &submap_id) EXCLUDES(mutex_) override
void RunFinalOptimization() override
void ComputeConstraintsForOldScans(const Submap *submap) REQUIRES(mutex_)
int num_submaps(int trajectory_id) EXCLUDES(mutex_) override
sparse_pose_graph::OptimizationProblem optimization_problem_
void AddTrimmer(std::unique_ptr< mapping::PoseGraphTrimmer > trimmer) override
void AddScan(common::Time time, const sensor::RangeData &range_data_in_tracking, const transform::Rigid3d &pose, int trajectory_id, const Submap *matching_submap, const std::vector< const Submap * > &insertion_submaps) EXCLUDES(mutex_)
std::vector< std::vector< int > > GetConnectedTrajectories() override