#include "cartographer/mapping/internal/2d/pose_graph_2d.h"#include <algorithm>#include <cmath>#include <cstdio>#include <functional>#include <iomanip>#include <iostream>#include <iterator>#include <limits>#include <memory>#include <sstream>#include <string>#include "Eigen/Eigenvalues"#include "absl/memory/memory.h"#include "cartographer/common/math.h"#include "cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d.h"#include "cartographer/mapping/proto/pose_graph/constraint_builder_options.pb.h"#include "cartographer/sensor/compressed_point_cloud.h"#include "cartographer/sensor/internal/voxel_filter.h"#include "cartographer/transform/transform.h"#include "glog/logging.h"
Include dependency graph for pose_graph_2d.cc:
Go to the source code of this file.
Namespaces | |
| namespace | cartographer |
| namespace | cartographer::mapping |
Variables | |
| static auto * | cartographer::mapping::kActiveSubmapsMetric = metrics::Gauge::Null() |
| static auto * | cartographer::mapping::kConstraintsDifferentTrajectoryMetric = metrics::Gauge::Null() |
| static auto * | cartographer::mapping::kConstraintsSameTrajectoryMetric = metrics::Gauge::Null() |
| static auto * | cartographer::mapping::kDeletedSubmapsMetric = metrics::Gauge::Null() |
| static auto * | cartographer::mapping::kFrozenSubmapsMetric = metrics::Gauge::Null() |
| static auto * | cartographer::mapping::kWorkQueueDelayMetric = metrics::Gauge::Null() |
| std::vector< std::vector< int > > PoseGraph2D::GetConnectedTrajectories() const {absl::MutexLock locker(&mutex_);return data_.trajectory_connectivity_state.Components();}PoseGraphInterface::SubmapData PoseGraph2D::GetSubmapData(const SubmapId &submap_id) const {absl::MutexLock locker(&mutex_);return GetSubmapDataUnderLock(submap_id);}MapById < SubmapId, PoseGraphInterface::SubmapData > PoseGraph2D::GetAllSubmapData() const {absl::MutexLock locker(&mutex_);return GetSubmapDataUnderLock();}MapById < SubmapId, PoseGraphInterface::SubmapPose > PoseGraph2D::GetAllSubmapPoses() const {absl::MutexLock locker(&mutex_);MapById < SubmapId, SubmapPose > submap_poses;for(const auto &submap_id_data:data_.submap_data){auto submap_data=GetSubmapDataUnderLock(submap_id_data.id);submap_poses.Insert(submap_id_data.id, PoseGraph::SubmapPose{submap_data.submap-> num_range_data(), submap_data.pose});}return submap_poses;}transform::Rigid3d PoseGraph2D::ComputeLocalToGlobalTransform(const MapById< SubmapId, optimization::SubmapSpec2D > &global_submap_poses, const int trajectory_id) const {auto begin_it=global_submap_poses.BeginOfTrajectory(trajectory_id);auto end_it=global_submap_poses.EndOfTrajectory(trajectory_id);if(begin_it==end_it){const auto it=data_.initial_trajectory_poses.find(trajectory_id);if(it!=data_.initial_trajectory_poses.end()){return GetInterpolatedGlobalTrajectoryPose(it-> second it second time *it second | cartographer::mapping::relative_pose |
| std::vector< std::vector< int > > PoseGraph2D::GetConnectedTrajectories() const {absl::MutexLock locker(&mutex_);return data_.trajectory_connectivity_state.Components();}PoseGraphInterface::SubmapData PoseGraph2D::GetSubmapData(const SubmapId &submap_id) const {absl::MutexLock locker(&mutex_);return GetSubmapDataUnderLock(submap_id);}MapById < SubmapId, PoseGraphInterface::SubmapData > PoseGraph2D::GetAllSubmapData() const {absl::MutexLock locker(&mutex_);return GetSubmapDataUnderLock();}MapById < SubmapId, PoseGraphInterface::SubmapPose > PoseGraph2D::GetAllSubmapPoses() const {absl::MutexLock locker(&mutex_);MapById < SubmapId, SubmapPose > submap_poses;for(const auto &submap_id_data:data_.submap_data){auto submap_data=GetSubmapDataUnderLock(submap_id_data.id);submap_poses.Insert(submap_id_data.id, PoseGraph::SubmapPose{submap_data.submap-> num_range_data(), submap_data.pose});}return submap_poses;}transform::Rigid3d PoseGraph2D::ComputeLocalToGlobalTransform(const MapById< SubmapId, optimization::SubmapSpec2D > &global_submap_poses, const int trajectory_id) const {auto begin_it=global_submap_poses.BeginOfTrajectory(trajectory_id);auto end_it=global_submap_poses.EndOfTrajectory(trajectory_id);if(begin_it==end_it){const auto it=data_.initial_trajectory_poses.find(trajectory_id);if(it!=data_.initial_trajectory_poses.end()){return GetInterpolatedGlobalTrajectoryPose(it-> second | cartographer::mapping::to_trajectory_id |