#include <pose_graph_3d.h>
Classes | |
struct | InternalSubmapData |
class | TrimmingHandle |
Public Member Functions | |
void | AddFixedFramePoseData (int trajectory_id, const sensor::FixedFramePoseData &fixed_frame_pose_data) override EXCLUDES(mutex_) |
void | AddImuData (int trajectory_id, const sensor::ImuData &imu_data) override EXCLUDES(mutex_) |
void | AddLandmarkData (int trajectory_id, const sensor::LandmarkData &landmark_data) override EXCLUDES(mutex_) |
NodeId | AddNode (std::shared_ptr< const TrajectoryNode::Data > constant_data, int trajectory_id, const std::vector< std::shared_ptr< const Submap3D >> &insertion_submaps) EXCLUDES(mutex_) |
void | AddNodeFromProto (const transform::Rigid3d &global_pose, const proto::Node &node) override |
void | AddNodeToSubmap (const NodeId &node_id, const SubmapId &submap_id) override |
void | AddOdometryData (int trajectory_id, const sensor::OdometryData &odometry_data) override EXCLUDES(mutex_) |
void | AddSerializedConstraints (const std::vector< Constraint > &constraints) override |
void | AddSubmapFromProto (const transform::Rigid3d &global_submap_pose, const proto::Submap &submap) override |
void | AddTrimmer (std::unique_ptr< PoseGraphTrimmer > trimmer) override |
std::vector< Constraint > | constraints () const override EXCLUDES(mutex_) |
void | FinishTrajectory (int trajectory_id) override |
void | FreezeTrajectory (int trajectory_id) override |
MapById< SubmapId, SubmapData > | GetAllSubmapData () const EXCLUDES(mutex_) override |
MapById< SubmapId, SubmapPose > | GetAllSubmapPoses () const EXCLUDES(mutex_) override |
std::vector< std::vector< int > > | GetConnectedTrajectories () const override |
sensor::MapByTime< sensor::FixedFramePoseData > | GetFixedFramePoseData () const override EXCLUDES(mutex_) |
sensor::MapByTime< sensor::ImuData > | GetImuData () const override EXCLUDES(mutex_) |
transform::Rigid3d | GetInterpolatedGlobalTrajectoryPose (int trajectory_id, const common::Time time) const REQUIRES(mutex_) |
std::map< std::string, PoseGraph::LandmarkNode > | GetLandmarkNodes () const override EXCLUDES(mutex_) |
std::map< std::string, transform::Rigid3d > | GetLandmarkPoses () const override EXCLUDES(mutex_) |
transform::Rigid3d | GetLocalToGlobalTransform (int trajectory_id) const EXCLUDES(mutex_) override |
sensor::MapByTime< sensor::OdometryData > | GetOdometryData () const override EXCLUDES(mutex_) |
PoseGraph::SubmapData | GetSubmapData (const SubmapId &submap_id) const EXCLUDES(mutex_) override |
std::map< int, TrajectoryData > | GetTrajectoryData () const override |
MapById< NodeId, TrajectoryNodePose > | GetTrajectoryNodePoses () const override EXCLUDES(mutex_) |
MapById< NodeId, TrajectoryNode > | GetTrajectoryNodes () const override EXCLUDES(mutex_) |
bool | IsTrajectoryFinished (int trajectory_id) const override REQUIRES(mutex_) |
bool | IsTrajectoryFrozen (int trajectory_id) const override REQUIRES(mutex_) |
PoseGraph3D & | operator= (const PoseGraph3D &)=delete |
PoseGraph3D (const proto::PoseGraphOptions &options, std::unique_ptr< optimization::OptimizationProblem3D > optimization_problem, common::ThreadPool *thread_pool) | |
PoseGraph3D (const PoseGraph3D &)=delete | |
void | RunFinalOptimization () override |
void | SetGlobalSlamOptimizationCallback (PoseGraphInterface::GlobalSlamOptimizationCallback callback) override |
void | SetInitialTrajectoryPose (int from_trajectory_id, int to_trajectory_id, const transform::Rigid3d &pose, const common::Time time) override EXCLUDES(mutex_) |
void | SetLandmarkPose (const std::string &landmark_id, const transform::Rigid3d &global_pose) override EXCLUDES(mutex_) |
void | SetTrajectoryDataFromProto (const proto::TrajectoryData &data) override |
~PoseGraph3D () override | |
Public Member Functions inherited from cartographer::mapping::PoseGraph | |
PoseGraph & | operator= (const PoseGraph &)=delete |
PoseGraph () | |
PoseGraph (const PoseGraph &)=delete | |
virtual void | SetTrajectoryDataFromProto (const mapping::proto::TrajectoryData &data)=0 |
proto::PoseGraph | ToProto () const override |
~PoseGraph () override | |
Public Member Functions inherited from cartographer::mapping::PoseGraphInterface | |
PoseGraphInterface & | operator= (const PoseGraphInterface &)=delete |
PoseGraphInterface () | |
PoseGraphInterface (const PoseGraphInterface &)=delete | |
virtual | ~PoseGraphInterface () |
Protected Member Functions | |
void | WaitForAllComputations () EXCLUDES(mutex_) |
Private Types | |
enum | SubmapState { SubmapState::kActive, SubmapState::kFinished } |
Private Member Functions | |
void | AddTrajectoryIfNeeded (int trajectory_id) REQUIRES(mutex_) |
void | AddWorkItem (const std::function< void()> &work_item) REQUIRES(mutex_) |
void | ComputeConstraint (const NodeId &node_id, const SubmapId &submap_id) REQUIRES(mutex_) |
void | ComputeConstraintsForNode (const NodeId &node_id, std::vector< std::shared_ptr< const Submap3D >> insertion_submaps, bool newly_finished_submap) REQUIRES(mutex_) |
void | ComputeConstraintsForOldNodes (const SubmapId &submap_id) REQUIRES(mutex_) |
transform::Rigid3d | ComputeLocalToGlobalTransform (const MapById< SubmapId, optimization::SubmapSpec3D > &global_submap_poses, int trajectory_id) const REQUIRES(mutex_) |
void | DispatchOptimization () REQUIRES(mutex_) |
common::Time | GetLatestNodeTime (const NodeId &node_id, const SubmapId &submap_id) const REQUIRES(mutex_) |
MapById< SubmapId, SubmapData > | GetSubmapDataUnderLock () const REQUIRES(mutex_) |
PoseGraph::SubmapData | GetSubmapDataUnderLock (const SubmapId &submap_id) const REQUIRES(mutex_) |
std::unique_ptr< std::deque< std::function< void()> > > work_queue_ | GUARDED_BY (mutex_) |
std::unordered_map< int, std::unique_ptr< common::FixedRatioSampler > > global_localization_samplers_ | GUARDED_BY (mutex_) |
int num_nodes_since_last_loop_closure_ | GUARDED_BY (mutex_)=0 |
bool run_loop_closure_ | GUARDED_BY (mutex_) |
constraints::ConstraintBuilder3D constraint_builder_ | GUARDED_BY (mutex_) |
std::vector< Constraint > constraints_ | GUARDED_BY (mutex_) |
MapById< SubmapId, InternalSubmapData > submap_data_ | GUARDED_BY (mutex_) |
MapById< NodeId, TrajectoryNode > trajectory_nodes_ | GUARDED_BY (mutex_) |
int num_trajectory_nodes_ | GUARDED_BY (mutex_)=0 |
MapById< SubmapId, optimization::SubmapSpec3D > global_submap_poses_ | GUARDED_BY (mutex_) |
std::map< std::string, PoseGraph::LandmarkNode > landmark_nodes_ | GUARDED_BY (mutex_) |
std::vector< std::unique_ptr< PoseGraphTrimmer > > trimmers_ | GUARDED_BY (mutex_) |
std::set< int > frozen_trajectories_ | GUARDED_BY (mutex_) |
std::set< int > finished_trajectories_ | GUARDED_BY (mutex_) |
std::map< int, InitialTrajectoryPose > initial_trajectory_poses_ | GUARDED_BY (mutex_) |
void | HandleWorkQueue (const constraints::ConstraintBuilder3D::Result &result) REQUIRES(mutex_) |
std::vector< SubmapId > | InitializeGlobalSubmapPoses (int trajectory_id, const common::Time time, const std::vector< std::shared_ptr< const Submap3D >> &insertion_submaps) REQUIRES(mutex_) |
void | LogResidualHistograms () const REQUIRES(mutex_) |
void | RunOptimization () EXCLUDES(mutex_) |
void | UpdateTrajectoryConnectivity (const Constraint &constraint) REQUIRES(mutex_) |
Private Attributes | |
GlobalSlamOptimizationCallback | global_slam_optimization_callback_ |
common::Mutex | mutex_ |
std::unique_ptr< optimization::OptimizationProblem3D > | optimization_problem_ |
const proto::PoseGraphOptions | options_ |
TrajectoryConnectivityState | trajectory_connectivity_state_ |
Additional Inherited Members | |
Public Types inherited from cartographer::mapping::PoseGraphInterface | |
using | GlobalSlamOptimizationCallback = std::function< void(const std::map< int, SubmapId > &, const std::map< int, NodeId > &)> |
Definition at line 59 of file pose_graph_3d.h.
|
strongprivate |
Enumerator | |
---|---|
kActive | |
kFinished |
Definition at line 156 of file pose_graph_3d.h.
cartographer::mapping::PoseGraph3D::PoseGraph3D | ( | const proto::PoseGraphOptions & | options, |
std::unique_ptr< optimization::OptimizationProblem3D > | optimization_problem, | ||
common::ThreadPool * | thread_pool | ||
) |
Definition at line 42 of file pose_graph_3d.cc.
|
override |
Definition at line 50 of file pose_graph_3d.cc.
|
delete |
|
overridevirtual |
Implements cartographer::mapping::PoseGraph.
Definition at line 169 of file pose_graph_3d.cc.
|
overridevirtual |
Implements cartographer::mapping::PoseGraph.
Definition at line 153 of file pose_graph_3d.cc.
|
overridevirtual |
Implements cartographer::mapping::PoseGraph.
Definition at line 179 of file pose_graph_3d.cc.
NodeId cartographer::mapping::PoseGraph3D::AddNode | ( | std::shared_ptr< const TrajectoryNode::Data > | constant_data, |
int | trajectory_id, | ||
const std::vector< std::shared_ptr< const Submap3D >> & | insertion_submaps | ||
) |
Definition at line 101 of file pose_graph_3d.cc.
|
overridevirtual |
Implements cartographer::mapping::PoseGraph.
Definition at line 490 of file pose_graph_3d.cc.
|
overridevirtual |
Implements cartographer::mapping::PoseGraph.
Definition at line 529 of file pose_graph_3d.cc.
|
overridevirtual |
Implements cartographer::mapping::PoseGraph.
Definition at line 161 of file pose_graph_3d.cc.
|
overridevirtual |
Implements cartographer::mapping::PoseGraph.
Definition at line 537 of file pose_graph_3d.cc.
|
overridevirtual |
Implements cartographer::mapping::PoseGraph.
Definition at line 466 of file pose_graph_3d.cc.
|
private |
Definition at line 143 of file pose_graph_3d.cc.
|
overridevirtual |
Implements cartographer::mapping::PoseGraph.
Definition at line 562 of file pose_graph_3d.cc.
|
private |
Definition at line 135 of file pose_graph_3d.cc.
|
private |
Definition at line 194 of file pose_graph_3d.cc.
|
private |
Definition at line 255 of file pose_graph_3d.cc.
|
private |
Definition at line 245 of file pose_graph_3d.cc.
|
private |
Definition at line 806 of file pose_graph_3d.cc.
|
overridevirtual |
Implements cartographer::mapping::PoseGraphInterface.
Definition at line 736 of file pose_graph_3d.cc.
|
private |
Definition at line 313 of file pose_graph_3d.cc.
|
overridevirtual |
Implements cartographer::mapping::PoseGraph.
Definition at line 435 of file pose_graph_3d.cc.
|
overridevirtual |
Implements cartographer::mapping::PoseGraph.
Definition at line 453 of file pose_graph_3d.cc.
|
overridevirtual |
Implements cartographer::mapping::PoseGraphInterface.
Definition at line 787 of file pose_graph_3d.cc.
|
overridevirtual |
Implements cartographer::mapping::PoseGraphInterface.
Definition at line 793 of file pose_graph_3d.cc.
|
overridevirtual |
Implements cartographer::mapping::PoseGraph.
Definition at line 776 of file pose_graph_3d.cc.
|
overridevirtual |
Implements cartographer::mapping::PoseGraph.
Definition at line 719 of file pose_graph_3d.cc.
|
overridevirtual |
Implements cartographer::mapping::PoseGraph.
Definition at line 708 of file pose_graph_3d.cc.
transform::Rigid3d cartographer::mapping::PoseGraph3D::GetInterpolatedGlobalTrajectoryPose | ( | int | trajectory_id, |
const common::Time | time | ||
) | const |
Definition at line 750 of file pose_graph_3d.cc.
|
overridevirtual |
Implements cartographer::mapping::PoseGraph.
Definition at line 725 of file pose_graph_3d.cc.
|
overridevirtual |
Implements cartographer::mapping::PoseGraphInterface.
Definition at line 687 of file pose_graph_3d.cc.
|
private |
Definition at line 323 of file pose_graph_3d.cc.
|
overridevirtual |
Implements cartographer::mapping::PoseGraphInterface.
Definition at line 770 of file pose_graph_3d.cc.
|
overridevirtual |
Implements cartographer::mapping::PoseGraph.
Definition at line 713 of file pose_graph_3d.cc.
|
overridevirtual |
Implements cartographer::mapping::PoseGraph.
Definition at line 780 of file pose_graph_3d.cc.
|
private |
Definition at line 953 of file pose_graph_3d.cc.
|
private |
Definition at line 829 of file pose_graph_3d.cc.
|
overridevirtual |
Implements cartographer::mapping::PoseGraphInterface.
Definition at line 731 of file pose_graph_3d.cc.
|
overridevirtual |
Implements cartographer::mapping::PoseGraphInterface.
Definition at line 669 of file pose_graph_3d.cc.
|
overridevirtual |
Implements cartographer::mapping::PoseGraphInterface.
Definition at line 664 of file pose_graph_3d.cc.
|
private |
|
private |
|
privatepure virtual |
|
private |
|
private |
|
private |
|
private |
|
private |
|
privatepure virtual |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
Definition at line 345 of file pose_graph_3d.cc.
|
private |
Definition at line 56 of file pose_graph_3d.cc.
|
overridevirtual |
Implements cartographer::mapping::PoseGraphInterface.
Definition at line 449 of file pose_graph_3d.cc.
|
overridevirtual |
Implements cartographer::mapping::PoseGraphInterface.
Definition at line 462 of file pose_graph_3d.cc.
|
private |
Definition at line 588 of file pose_graph_3d.cc.
|
delete |
|
overridevirtual |
Implements cartographer::mapping::PoseGraphInterface.
Definition at line 570 of file pose_graph_3d.cc.
|
private |
Definition at line 614 of file pose_graph_3d.cc.
|
overridevirtual |
Implements cartographer::mapping::PoseGraphInterface.
Definition at line 962 of file pose_graph_3d.cc.
|
overridevirtual |
Implements cartographer::mapping::PoseGraph.
Definition at line 741 of file pose_graph_3d.cc.
|
overridevirtual |
Implements cartographer::mapping::PoseGraphInterface.
Definition at line 700 of file pose_graph_3d.cc.
|
override |
Definition at line 510 of file pose_graph_3d.cc.
|
private |
Definition at line 336 of file pose_graph_3d.cc.
|
protected |
Definition at line 403 of file pose_graph_3d.cc.
|
private |
Definition at line 230 of file pose_graph_3d.h.
|
mutableprivate |
Definition at line 231 of file pose_graph_3d.h.
|
private |
Definition at line 252 of file pose_graph_3d.h.
|
private |
Definition at line 229 of file pose_graph_3d.h.
|
private |
Definition at line 239 of file pose_graph_3d.h.