diff --git a/cartographer/mapping/global_trajectory_builder.h b/cartographer/mapping/global_trajectory_builder.h index ec04469..acc206d 100644 --- a/cartographer/mapping/global_trajectory_builder.h +++ b/cartographer/mapping/global_trajectory_builder.h @@ -25,16 +25,16 @@ namespace cartographer { namespace mapping { template + typename LocalTrajectoryBuilderOptions, typename PoseGraph> class GlobalTrajectoryBuilder : public mapping::GlobalTrajectoryBuilderInterface { public: GlobalTrajectoryBuilder( const LocalTrajectoryBuilderOptions& options, const int trajectory_id, - SparsePoseGraph* const sparse_pose_graph, + PoseGraph* const pose_graph, const LocalSlamResultCallback& local_slam_result_callback) : trajectory_id_(trajectory_id), - sparse_pose_graph_(sparse_pose_graph), + pose_graph_(pose_graph), local_trajectory_builder_(options), local_slam_result_callback_(local_slam_result_callback) {} ~GlobalTrajectoryBuilder() override {} @@ -59,7 +59,7 @@ class GlobalTrajectoryBuilder std::unique_ptr node_id; if (matching_result->insertion_result != nullptr) { node_id = ::cartographer::common::make_unique( - sparse_pose_graph_->AddNode( + pose_graph_->AddNode( matching_result->insertion_result->constant_data, trajectory_id_, matching_result->insertion_result->insertion_submaps)); CHECK_EQ(node_id->trajectory_id, trajectory_id_); @@ -73,24 +73,24 @@ class GlobalTrajectoryBuilder void AddSensorData(const sensor::ImuData& imu_data) override { local_trajectory_builder_.AddImuData(imu_data); - sparse_pose_graph_->AddImuData(trajectory_id_, imu_data); + pose_graph_->AddImuData(trajectory_id_, imu_data); } void AddSensorData(const sensor::OdometryData& odometry_data) override { CHECK(odometry_data.pose.IsValid()) << odometry_data.pose; local_trajectory_builder_.AddOdometryData(odometry_data); - sparse_pose_graph_->AddOdometryData(trajectory_id_, odometry_data); + pose_graph_->AddOdometryData(trajectory_id_, odometry_data); } void AddSensorData( const sensor::FixedFramePoseData& fixed_frame_pose) override { CHECK(fixed_frame_pose.pose.IsValid()) << fixed_frame_pose.pose; - sparse_pose_graph_->AddFixedFramePoseData(trajectory_id_, fixed_frame_pose); + pose_graph_->AddFixedFramePoseData(trajectory_id_, fixed_frame_pose); } private: const int trajectory_id_; - SparsePoseGraph* const sparse_pose_graph_; + PoseGraph* const pose_graph_; LocalTrajectoryBuilder local_trajectory_builder_; LocalSlamResultCallback local_slam_result_callback_; }; diff --git a/cartographer/mapping/map_builder.cc b/cartographer/mapping/map_builder.cc index 70860d6..f19bf1a 100644 --- a/cartographer/mapping/map_builder.cc +++ b/cartographer/mapping/map_builder.cc @@ -62,12 +62,12 @@ MapBuilder::MapBuilder( if (options.use_trajectory_builder_2d()) { sparse_pose_graph_2d_ = common::make_unique( options_.sparse_pose_graph_options(), &thread_pool_); - sparse_pose_graph_ = sparse_pose_graph_2d_.get(); + pose_graph_ = sparse_pose_graph_2d_.get(); } if (options.use_trajectory_builder_3d()) { sparse_pose_graph_3d_ = common::make_unique( options_.sparse_pose_graph_options(), &thread_pool_); - sparse_pose_graph_ = sparse_pose_graph_3d_.get(); + pose_graph_ = sparse_pose_graph_3d_.get(); } } @@ -104,13 +104,13 @@ int MapBuilder::AddTrajectoryBuilder( } if (trajectory_options.pure_localization()) { constexpr int kSubmapsToKeep = 3; - sparse_pose_graph_->AddTrimmer(common::make_unique( + pose_graph_->AddTrimmer(common::make_unique( trajectory_id, kSubmapsToKeep)); } if (trajectory_options.has_initial_trajectory_pose()) { const auto& initial_trajectory_pose = trajectory_options.initial_trajectory_pose(); - sparse_pose_graph_->SetInitialTrajectoryPose( + pose_graph_->SetInitialTrajectoryPose( trajectory_id, initial_trajectory_pose.to_trajectory_id(), transform::ToRigid3(initial_trajectory_pose.relative_pose()), common::FromUniversal(initial_trajectory_pose.timestamp())); @@ -131,7 +131,7 @@ TrajectoryBuilder* MapBuilder::GetTrajectoryBuilder( void MapBuilder::FinishTrajectory(const int trajectory_id) { sensor_collator_.FinishTrajectory(trajectory_id); - sparse_pose_graph_->FinishTrajectory(trajectory_id); + pose_graph_->FinishTrajectory(trajectory_id); } int MapBuilder::GetBlockingTrajectoryId() const { @@ -148,7 +148,7 @@ std::string MapBuilder::SubmapToProto( std::to_string(num_trajectory_builders()) + " trajectories."; } - const auto submap_data = sparse_pose_graph_->GetSubmapData(submap_id); + const auto submap_data = pose_graph_->GetSubmapData(submap_id); if (submap_data.submap == nullptr) { return "Requested submap " + std::to_string(submap_id.submap_index) + " from trajectory " + std::to_string(submap_id.trajectory_id) + @@ -160,10 +160,10 @@ std::string MapBuilder::SubmapToProto( void MapBuilder::SerializeState(io::ProtoStreamWriter* const writer) { // We serialize the pose graph followed by all the data referenced in it. - writer->WriteProto(sparse_pose_graph_->ToProto()); + writer->WriteProto(pose_graph_->ToProto()); // Next we serialize all submap data. { - for (const auto& submap_id_data : sparse_pose_graph_->GetAllSubmapData()) { + for (const auto& submap_id_data : pose_graph_->GetAllSubmapData()) { proto::SerializedData proto; auto* const submap_proto = proto.mutable_submap(); submap_proto->mutable_submap_id()->set_trajectory_id( @@ -176,7 +176,7 @@ void MapBuilder::SerializeState(io::ProtoStreamWriter* const writer) { } // Next we serialize all node data. { - for (const auto& node_id_data : sparse_pose_graph_->GetTrajectoryNodes()) { + for (const auto& node_id_data : pose_graph_->GetTrajectoryNodes()) { proto::SerializedData proto; auto* const node_proto = proto.mutable_node(); node_proto->mutable_node_id()->set_trajectory_id( @@ -189,7 +189,7 @@ void MapBuilder::SerializeState(io::ProtoStreamWriter* const writer) { } // Next we serialize IMU data from the pose graph. { - const auto all_imu_data = sparse_pose_graph_->GetImuData(); + const auto all_imu_data = pose_graph_->GetImuData(); for (const int trajectory_id : all_imu_data.trajectory_ids()) { for (const auto& imu_data : all_imu_data.trajectory(trajectory_id)) { proto::SerializedData proto; @@ -202,7 +202,7 @@ void MapBuilder::SerializeState(io::ProtoStreamWriter* const writer) { } // Next we serialize odometry data from the pose graph. { - const auto all_odometry_data = sparse_pose_graph_->GetOdometryData(); + const auto all_odometry_data = pose_graph_->GetOdometryData(); for (const int trajectory_id : all_odometry_data.trajectory_ids()) { for (const auto& odometry_data : all_odometry_data.trajectory(trajectory_id)) { @@ -230,7 +230,7 @@ void MapBuilder::LoadMap(io::ProtoStreamReader* const reader) { .second) << "Duplicate trajectory ID: " << trajectory_proto.trajectory_id(); trajectory_proto.set_trajectory_id(new_trajectory_id); - sparse_pose_graph_->FreezeTrajectory(new_trajectory_id); + pose_graph_->FreezeTrajectory(new_trajectory_id); } MapById submap_poses; @@ -263,7 +263,7 @@ void MapBuilder::LoadMap(io::ProtoStreamReader* const reader) { const transform::Rigid3d node_pose = node_poses.at(NodeId{proto.node().node_id().trajectory_id(), proto.node().node_id().node_index()}); - sparse_pose_graph_->AddNodeFromProto(node_pose, proto.node()); + pose_graph_->AddNodeFromProto(node_pose, proto.node()); } if (proto.has_submap()) { proto.mutable_submap()->mutable_submap_id()->set_trajectory_id( @@ -271,7 +271,7 @@ void MapBuilder::LoadMap(io::ProtoStreamReader* const reader) { const transform::Rigid3d submap_pose = submap_poses.at(SubmapId{proto.submap().submap_id().trajectory_id(), proto.submap().submap_id().submap_index()}); - sparse_pose_graph_->AddSubmapFromProto(submap_pose, proto.submap()); + pose_graph_->AddSubmapFromProto(submap_pose, proto.submap()); } // TODO(ojura): Deserialize IMU and odometry data when loading unfrozen // trajectories. @@ -290,7 +290,7 @@ void MapBuilder::LoadMap(io::ProtoStreamReader* const reader) { const SubmapId submap_id{ trajectory_remapping.at(constraint_proto.submap_id().trajectory_id()), constraint_proto.submap_id().submap_index()}; - sparse_pose_graph_->AddNodeToSubmap(node_id, submap_id); + pose_graph_->AddNodeToSubmap(node_id, submap_id); } CHECK(reader->eof()); } @@ -299,7 +299,7 @@ int MapBuilder::num_trajectory_builders() const { return trajectory_builders_.size(); } -SparsePoseGraph* MapBuilder::sparse_pose_graph() { return sparse_pose_graph_; } +PoseGraph* MapBuilder::pose_graph() { return pose_graph_; } } // namespace mapping } // namespace cartographer diff --git a/cartographer/mapping/map_builder.h b/cartographer/mapping/map_builder.h index a42b316..e5b5d35 100644 --- a/cartographer/mapping/map_builder.h +++ b/cartographer/mapping/map_builder.h @@ -29,10 +29,10 @@ #include "cartographer/common/thread_pool.h" #include "cartographer/io/proto_stream.h" #include "cartographer/mapping/id.h" +#include "cartographer/mapping/pose_graph.h" #include "cartographer/mapping/proto/map_builder_options.pb.h" #include "cartographer/mapping/proto/submap_visualization.pb.h" #include "cartographer/mapping/proto/trajectory_builder_options.pb.h" -#include "cartographer/mapping/sparse_pose_graph.h" #include "cartographer/mapping/submaps.h" #include "cartographer/mapping/trajectory_builder.h" #include "cartographer/mapping_2d/sparse_pose_graph.h" @@ -46,7 +46,7 @@ proto::MapBuilderOptions CreateMapBuilderOptions( common::LuaParameterDictionary* const parameter_dictionary); // Wires up the complete SLAM stack with TrajectoryBuilders (for local submaps) -// and a SparsePoseGraph for loop closure. +// and a PoseGraph for loop closure. class MapBuilder { public: using LocalSlamResultCallback = @@ -95,7 +95,7 @@ class MapBuilder { int num_trajectory_builders() const; - mapping::SparsePoseGraph* sparse_pose_graph(); + mapping::PoseGraph* pose_graph(); private: const proto::MapBuilderOptions options_; @@ -103,7 +103,7 @@ class MapBuilder { std::unique_ptr sparse_pose_graph_2d_; std::unique_ptr sparse_pose_graph_3d_; - mapping::SparsePoseGraph* sparse_pose_graph_; + mapping::PoseGraph* pose_graph_; LocalSlamResultCallback local_slam_result_callback_; diff --git a/cartographer/mapping/sparse_pose_graph.cc b/cartographer/mapping/pose_graph.cc similarity index 86% rename from cartographer/mapping/sparse_pose_graph.cc rename to cartographer/mapping/pose_graph.cc index 717df6e..1b08e5e 100644 --- a/cartographer/mapping/sparse_pose_graph.cc +++ b/cartographer/mapping/pose_graph.cc @@ -14,7 +14,7 @@ * limitations under the License. */ -#include "cartographer/mapping/sparse_pose_graph.h" +#include "cartographer/mapping/pose_graph.h" #include "cartographer/mapping/pose_graph/constraint_builder.h" #include "cartographer/mapping/pose_graph/optimization_problem_options.h" @@ -25,48 +25,45 @@ namespace cartographer { namespace mapping { proto::SparsePoseGraph::Constraint::Tag ToProto( - const SparsePoseGraph::Constraint::Tag& tag) { + const PoseGraph::Constraint::Tag& tag) { switch (tag) { - case SparsePoseGraph::Constraint::Tag::INTRA_SUBMAP: + case PoseGraph::Constraint::Tag::INTRA_SUBMAP: return proto::SparsePoseGraph::Constraint::INTRA_SUBMAP; - case SparsePoseGraph::Constraint::Tag::INTER_SUBMAP: + case PoseGraph::Constraint::Tag::INTER_SUBMAP: return proto::SparsePoseGraph::Constraint::INTER_SUBMAP; } LOG(FATAL) << "Unsupported tag."; } -SparsePoseGraph::Constraint::Tag FromProto( +PoseGraph::Constraint::Tag FromProto( const proto::SparsePoseGraph::Constraint::Tag& proto) { switch (proto) { case proto::SparsePoseGraph::Constraint::INTRA_SUBMAP: - return SparsePoseGraph::Constraint::Tag::INTRA_SUBMAP; + return PoseGraph::Constraint::Tag::INTRA_SUBMAP; case proto::SparsePoseGraph::Constraint::INTER_SUBMAP: - return SparsePoseGraph::Constraint::Tag::INTER_SUBMAP; + return PoseGraph::Constraint::Tag::INTER_SUBMAP; case ::google::protobuf::kint32max: case ::google::protobuf::kint32min:; } LOG(FATAL) << "Unsupported tag."; } -std::vector FromProto( +std::vector FromProto( const ::google::protobuf::RepeatedPtrField< - ::cartographer::mapping::proto::SparsePoseGraph::Constraint>& - constraint_protos) { - std::vector constraints; + proto::SparsePoseGraph::Constraint>& constraint_protos) { + std::vector constraints; for (const auto& constraint_proto : constraint_protos) { const mapping::SubmapId submap_id{ constraint_proto.submap_id().trajectory_id(), constraint_proto.submap_id().submap_index()}; const mapping::NodeId node_id{constraint_proto.node_id().trajectory_id(), constraint_proto.node_id().node_index()}; - const SparsePoseGraph::Constraint::Pose pose{ + const PoseGraph::Constraint::Pose pose{ transform::ToRigid3(constraint_proto.relative_pose()), constraint_proto.translation_weight(), constraint_proto.rotation_weight()}; - const SparsePoseGraph::Constraint::Tag tag = - FromProto(constraint_proto.tag()); - constraints.push_back( - SparsePoseGraph::Constraint{submap_id, node_id, pose, tag}); + const PoseGraph::Constraint::Tag tag = FromProto(constraint_proto.tag()); + constraints.push_back(PoseGraph::Constraint{submap_id, node_id, pose, tag}); } return constraints; } @@ -99,7 +96,7 @@ proto::SparsePoseGraphOptions CreateSparsePoseGraphOptions( return options; } -proto::SparsePoseGraph SparsePoseGraph::ToProto() { +proto::SparsePoseGraph PoseGraph::ToProto() { proto::SparsePoseGraph proto; std::map trajectory_protos; diff --git a/cartographer/mapping/sparse_pose_graph.h b/cartographer/mapping/pose_graph.h similarity index 93% rename from cartographer/mapping/sparse_pose_graph.h rename to cartographer/mapping/pose_graph.h index 8b47bc6..e8ba8d1 100644 --- a/cartographer/mapping/sparse_pose_graph.h +++ b/cartographer/mapping/pose_graph.h @@ -14,8 +14,8 @@ * limitations under the License. */ -#ifndef CARTOGRAPHER_MAPPING_SPARSE_POSE_GRAPH_H_ -#define CARTOGRAPHER_MAPPING_SPARSE_POSE_GRAPH_H_ +#ifndef CARTOGRAPHER_MAPPING_POSE_GRAPH_H_ +#define CARTOGRAPHER_MAPPING_POSE_GRAPH_H_ #include #include @@ -42,7 +42,7 @@ namespace mapping { proto::SparsePoseGraphOptions CreateSparsePoseGraphOptions( common::LuaParameterDictionary* const parameter_dictionary); -class SparsePoseGraph { +class PoseGraph { public: // A "constraint" as in the paper by Konolige, Kurt, et al. "Efficient sparse // pose adjustment for 2d mapping." Intelligent Robots and Systems (IROS), @@ -77,11 +77,11 @@ class SparsePoseGraph { common::Time time; }; - SparsePoseGraph() {} - virtual ~SparsePoseGraph() {} + PoseGraph() {} + virtual ~PoseGraph() {} - SparsePoseGraph(const SparsePoseGraph&) = delete; - SparsePoseGraph& operator=(const SparsePoseGraph&) = delete; + PoseGraph(const PoseGraph&) = delete; + PoseGraph& operator=(const PoseGraph&) = delete; // Inserts an IMU measurement. virtual void AddImuData(int trajectory_id, @@ -163,7 +163,7 @@ class SparsePoseGraph { const common::Time time) = 0; }; -std::vector FromProto( +std::vector FromProto( const ::google::protobuf::RepeatedPtrField< ::cartographer::mapping::proto::SparsePoseGraph::Constraint>& constraint_protos); @@ -171,4 +171,4 @@ std::vector FromProto( } // namespace mapping } // namespace cartographer -#endif // CARTOGRAPHER_MAPPING_SPARSE_POSE_GRAPH_H_ +#endif // CARTOGRAPHER_MAPPING_POSE_GRAPH_H_ diff --git a/cartographer/mapping_2d/pose_graph/constraint_builder.h b/cartographer/mapping_2d/pose_graph/constraint_builder.h index 3060abc..ab9096f 100644 --- a/cartographer/mapping_2d/pose_graph/constraint_builder.h +++ b/cartographer/mapping_2d/pose_graph/constraint_builder.h @@ -30,8 +30,8 @@ #include "cartographer/common/math.h" #include "cartographer/common/mutex.h" #include "cartographer/common/thread_pool.h" +#include "cartographer/mapping/pose_graph.h" #include "cartographer/mapping/pose_graph/proto/constraint_builder_options.pb.h" -#include "cartographer/mapping/sparse_pose_graph.h" #include "cartographer/mapping_2d/scan_matching/ceres_scan_matcher.h" #include "cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.h" #include "cartographer/mapping_2d/submaps.h" @@ -58,7 +58,7 @@ transform::Rigid2d ComputeSubmapPose(const Submap& submap); // This class is thread-safe. class ConstraintBuilder { public: - using Constraint = mapping::SparsePoseGraph::Constraint; + using Constraint = mapping::PoseGraph::Constraint; using Result = std::vector; ConstraintBuilder( diff --git a/cartographer/mapping_2d/pose_graph/optimization_problem.h b/cartographer/mapping_2d/pose_graph/optimization_problem.h index fd81636..d478a91 100644 --- a/cartographer/mapping_2d/pose_graph/optimization_problem.h +++ b/cartographer/mapping_2d/pose_graph/optimization_problem.h @@ -28,8 +28,8 @@ #include "cartographer/common/port.h" #include "cartographer/common/time.h" #include "cartographer/mapping/id.h" +#include "cartographer/mapping/pose_graph.h" #include "cartographer/mapping/pose_graph/proto/optimization_problem_options.pb.h" -#include "cartographer/mapping/sparse_pose_graph.h" #include "cartographer/sensor/imu_data.h" #include "cartographer/sensor/map_by_time.h" #include "cartographer/sensor/odometry_data.h" @@ -53,7 +53,7 @@ struct SubmapData { // Implements the SPA loop closure method. class OptimizationProblem { public: - using Constraint = mapping::SparsePoseGraph::Constraint; + using Constraint = mapping::PoseGraph::Constraint; explicit OptimizationProblem( const mapping::pose_graph::proto::OptimizationProblemOptions& options); diff --git a/cartographer/mapping_2d/pose_graph/spa_cost_function.h b/cartographer/mapping_2d/pose_graph/spa_cost_function.h index f770ace..b1c3b4a 100644 --- a/cartographer/mapping_2d/pose_graph/spa_cost_function.h +++ b/cartographer/mapping_2d/pose_graph/spa_cost_function.h @@ -22,7 +22,7 @@ #include "Eigen/Core" #include "Eigen/Geometry" #include "cartographer/common/math.h" -#include "cartographer/mapping/sparse_pose_graph.h" +#include "cartographer/mapping/pose_graph.h" #include "cartographer/transform/rigid_transform.h" #include "cartographer/transform/transform.h" #include "ceres/ceres.h" @@ -34,7 +34,7 @@ namespace pose_graph { class SpaCostFunction { public: - using Constraint = mapping::SparsePoseGraph::Constraint; + using Constraint = mapping::PoseGraph::Constraint; explicit SpaCostFunction(const Constraint::Pose& pose) : pose_(pose) {} diff --git a/cartographer/mapping_2d/sparse_pose_graph.cc b/cartographer/mapping_2d/sparse_pose_graph.cc index 370c4c9..e924c37 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.cc +++ b/cartographer/mapping_2d/sparse_pose_graph.cc @@ -297,7 +297,7 @@ common::Time SparsePoseGraph::GetLatestNodeTime( void SparsePoseGraph::UpdateTrajectoryConnectivity( const Constraint& constraint) { - CHECK_EQ(constraint.tag, mapping::SparsePoseGraph::Constraint::INTER_SUBMAP); + CHECK_EQ(constraint.tag, mapping::PoseGraph::Constraint::INTER_SUBMAP); const common::Time time = GetLatestNodeTime(constraint.node_id, constraint.submap_id); trajectory_connectivity_state_.Connect(constraint.node_id.trajectory_id, @@ -616,17 +616,16 @@ std::vector> SparsePoseGraph::GetConnectedTrajectories() { return trajectory_connectivity_state_.Components(); } -mapping::SparsePoseGraph::SubmapData SparsePoseGraph::GetSubmapData( +mapping::PoseGraph::SubmapData SparsePoseGraph::GetSubmapData( const mapping::SubmapId& submap_id) { common::MutexLocker locker(&mutex_); return GetSubmapDataUnderLock(submap_id); } -mapping::MapById +mapping::MapById SparsePoseGraph::GetAllSubmapData() { common::MutexLocker locker(&mutex_); - mapping::MapById - submaps; + mapping::MapById submaps; for (const auto& submap_id_data : submap_data_) { submaps.Insert(submap_id_data.id, GetSubmapDataUnderLock(submap_id_data.id)); @@ -659,7 +658,7 @@ transform::Rigid3d SparsePoseGraph::ComputeLocalToGlobalTransform( .inverse(); } -mapping::SparsePoseGraph::SubmapData SparsePoseGraph::GetSubmapDataUnderLock( +mapping::PoseGraph::SubmapData SparsePoseGraph::GetSubmapDataUnderLock( const mapping::SubmapId& submap_id) { const auto it = submap_data_.find(submap_id); if (it == submap_data_.end()) { @@ -737,8 +736,8 @@ void SparsePoseGraph::TrimmingHandle::MarkSubmapAsTrimmed( parent_->constraint_builder_.DeleteScanMatcher(submap_id); parent_->optimization_problem_.TrimSubmap(submap_id); - // Remove the 'nodes_to_remove' from the sparse pose graph and the - // optimization problem. + // Remove the 'nodes_to_remove' from the pose graph and the optimization + // problem. for (const mapping::NodeId& node_id : nodes_to_remove) { parent_->trajectory_nodes_.Trim(node_id); parent_->optimization_problem_.TrimTrajectoryNode(node_id); diff --git a/cartographer/mapping_2d/sparse_pose_graph.h b/cartographer/mapping_2d/sparse_pose_graph.h index 998847a..0769842 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.h +++ b/cartographer/mapping_2d/sparse_pose_graph.h @@ -32,8 +32,8 @@ #include "cartographer/common/mutex.h" #include "cartographer/common/thread_pool.h" #include "cartographer/common/time.h" +#include "cartographer/mapping/pose_graph.h" #include "cartographer/mapping/pose_graph_trimmer.h" -#include "cartographer/mapping/sparse_pose_graph.h" #include "cartographer/mapping/trajectory_connectivity_state.h" #include "cartographer/mapping_2d/pose_graph/constraint_builder.h" #include "cartographer/mapping_2d/pose_graph/optimization_problem.h" @@ -56,7 +56,7 @@ namespace mapping_2d { // Each node has been matched against one or more submaps (adding a constraint // for each match), both poses of nodes and of submaps are to be optimized. // All constraints are between a submap i and a node j. -class SparsePoseGraph : public mapping::SparsePoseGraph { +class SparsePoseGraph : public mapping::PoseGraph { public: SparsePoseGraph(const mapping::proto::SparsePoseGraphOptions& options, common::ThreadPool* thread_pool); @@ -98,9 +98,9 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { void AddTrimmer(std::unique_ptr trimmer) override; void RunFinalOptimization() override; std::vector> GetConnectedTrajectories() override; - mapping::SparsePoseGraph::SubmapData GetSubmapData( + mapping::PoseGraph::SubmapData GetSubmapData( const mapping::SubmapId& submap_id) EXCLUDES(mutex_) override; - mapping::MapById + mapping::MapById GetAllSubmapData() EXCLUDES(mutex_) override; transform::Rigid3d GetLocalToGlobalTransform(int trajectory_id) EXCLUDES(mutex_) override; @@ -179,7 +179,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { submap_transforms, int trajectory_id) const REQUIRES(mutex_); - mapping::SparsePoseGraph::SubmapData GetSubmapDataUnderLock( + mapping::PoseGraph::SubmapData GetSubmapDataUnderLock( const mapping::SubmapId& submap_id) REQUIRES(mutex_); common::Time GetLatestNodeTime(const mapping::NodeId& node_id, diff --git a/cartographer/mapping_3d/pose_graph/constraint_builder.h b/cartographer/mapping_3d/pose_graph/constraint_builder.h index 2285c02..e0d2e7d 100644 --- a/cartographer/mapping_3d/pose_graph/constraint_builder.h +++ b/cartographer/mapping_3d/pose_graph/constraint_builder.h @@ -54,7 +54,7 @@ namespace pose_graph { // This class is thread-safe. class ConstraintBuilder { public: - using Constraint = mapping::SparsePoseGraph::Constraint; + using Constraint = mapping::PoseGraph::Constraint; using Result = std::vector; ConstraintBuilder( diff --git a/cartographer/mapping_3d/pose_graph/optimization_problem.cc b/cartographer/mapping_3d/pose_graph/optimization_problem.cc index d0aa3ae..7fcf9c6 100644 --- a/cartographer/mapping_3d/pose_graph/optimization_problem.cc +++ b/cartographer/mapping_3d/pose_graph/optimization_problem.cc @@ -385,7 +385,7 @@ void OptimizationProblem::Solve(const std::vector& constraints, continue; } - const mapping::SparsePoseGraph::Constraint::Pose constraint_pose{ + const mapping::PoseGraph::Constraint::Pose constraint_pose{ *fixed_frame_pose, options_.fixed_frame_pose_translation_weight(), options_.fixed_frame_pose_rotation_weight()}; diff --git a/cartographer/mapping_3d/pose_graph/optimization_problem.h b/cartographer/mapping_3d/pose_graph/optimization_problem.h index 531c6f3..8ab9e6b 100644 --- a/cartographer/mapping_3d/pose_graph/optimization_problem.h +++ b/cartographer/mapping_3d/pose_graph/optimization_problem.h @@ -28,8 +28,8 @@ #include "cartographer/common/port.h" #include "cartographer/common/time.h" #include "cartographer/mapping/id.h" +#include "cartographer/mapping/pose_graph.h" #include "cartographer/mapping/pose_graph/proto/optimization_problem_options.pb.h" -#include "cartographer/mapping/sparse_pose_graph.h" #include "cartographer/sensor/fixed_frame_pose_data.h" #include "cartographer/sensor/imu_data.h" #include "cartographer/sensor/map_by_time.h" @@ -53,7 +53,7 @@ struct SubmapData { // Implements the SPA loop closure method. class OptimizationProblem { public: - using Constraint = mapping::SparsePoseGraph::Constraint; + using Constraint = mapping::PoseGraph::Constraint; enum class FixZ { kYes, kNo }; diff --git a/cartographer/mapping_3d/pose_graph/spa_cost_function.h b/cartographer/mapping_3d/pose_graph/spa_cost_function.h index 4e40dea..2638e95 100644 --- a/cartographer/mapping_3d/pose_graph/spa_cost_function.h +++ b/cartographer/mapping_3d/pose_graph/spa_cost_function.h @@ -22,7 +22,7 @@ #include "Eigen/Core" #include "Eigen/Geometry" #include "cartographer/common/math.h" -#include "cartographer/mapping/sparse_pose_graph.h" +#include "cartographer/mapping/pose_graph.h" #include "cartographer/transform/rigid_transform.h" #include "cartographer/transform/transform.h" #include "ceres/ceres.h" @@ -34,7 +34,7 @@ namespace pose_graph { class SpaCostFunction { public: - using Constraint = mapping::SparsePoseGraph::Constraint; + using Constraint = mapping::PoseGraph::Constraint; explicit SpaCostFunction(const Constraint::Pose& pose) : pose_(pose) {} diff --git a/cartographer/mapping_3d/sparse_pose_graph.cc b/cartographer/mapping_3d/sparse_pose_graph.cc index 3cdd598..0f12fa2 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.cc +++ b/cartographer/mapping_3d/sparse_pose_graph.cc @@ -314,7 +314,7 @@ common::Time SparsePoseGraph::GetLatestNodeTime( void SparsePoseGraph::UpdateTrajectoryConnectivity( const Constraint& constraint) { - CHECK_EQ(constraint.tag, mapping::SparsePoseGraph::Constraint::INTER_SUBMAP); + CHECK_EQ(constraint.tag, mapping::PoseGraph::Constraint::INTER_SUBMAP); const common::Time time = GetLatestNodeTime(constraint.node_id, constraint.submap_id); trajectory_connectivity_state_.Connect(constraint.node_id.trajectory_id, @@ -633,17 +633,16 @@ std::vector> SparsePoseGraph::GetConnectedTrajectories() { return trajectory_connectivity_state_.Components(); } -mapping::SparsePoseGraph::SubmapData SparsePoseGraph::GetSubmapData( +mapping::PoseGraph::SubmapData SparsePoseGraph::GetSubmapData( const mapping::SubmapId& submap_id) { common::MutexLocker locker(&mutex_); return GetSubmapDataUnderLock(submap_id); } -mapping::MapById +mapping::MapById SparsePoseGraph::GetAllSubmapData() { common::MutexLocker locker(&mutex_); - mapping::MapById - submaps; + mapping::MapById submaps; for (const auto& submap_id_data : submap_data_) { submaps.Insert(submap_id_data.id, GetSubmapDataUnderLock(submap_id_data.id)); @@ -675,7 +674,7 @@ transform::Rigid3d SparsePoseGraph::ComputeLocalToGlobalTransform( .inverse(); } -mapping::SparsePoseGraph::SubmapData SparsePoseGraph::GetSubmapDataUnderLock( +mapping::PoseGraph::SubmapData SparsePoseGraph::GetSubmapDataUnderLock( const mapping::SubmapId& submap_id) { const auto it = submap_data_.find(submap_id); if (it == submap_data_.end()) { @@ -751,8 +750,8 @@ void SparsePoseGraph::TrimmingHandle::MarkSubmapAsTrimmed( parent_->constraint_builder_.DeleteScanMatcher(submap_id); parent_->optimization_problem_.TrimSubmap(submap_id); - // Remove the 'nodes_to_remove' from the sparse pose graph and the - // optimization problem. + // Remove the 'nodes_to_remove' from the pose graph and the optimization + // problem. for (const mapping::NodeId& node_id : nodes_to_remove) { parent_->trajectory_nodes_.Trim(node_id); parent_->optimization_problem_.TrimTrajectoryNode(node_id); diff --git a/cartographer/mapping_3d/sparse_pose_graph.h b/cartographer/mapping_3d/sparse_pose_graph.h index 1ed97b0..482b1f4 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.h +++ b/cartographer/mapping_3d/sparse_pose_graph.h @@ -32,8 +32,8 @@ #include "cartographer/common/mutex.h" #include "cartographer/common/thread_pool.h" #include "cartographer/common/time.h" +#include "cartographer/mapping/pose_graph.h" #include "cartographer/mapping/pose_graph_trimmer.h" -#include "cartographer/mapping/sparse_pose_graph.h" #include "cartographer/mapping/trajectory_connectivity_state.h" #include "cartographer/mapping_3d/pose_graph/constraint_builder.h" #include "cartographer/mapping_3d/pose_graph/optimization_problem.h" @@ -56,7 +56,7 @@ namespace mapping_3d { // Each node has been matched against one or more submaps (adding a constraint // for each match), both poses of nodes and of submaps are to be optimized. // All constraints are between a submap i and a node j. -class SparsePoseGraph : public mapping::SparsePoseGraph { +class SparsePoseGraph : public mapping::PoseGraph { public: SparsePoseGraph(const mapping::proto::SparsePoseGraphOptions& options, common::ThreadPool* thread_pool); @@ -98,9 +98,9 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { void AddTrimmer(std::unique_ptr trimmer) override; void RunFinalOptimization() override; std::vector> GetConnectedTrajectories() override; - mapping::SparsePoseGraph::SubmapData GetSubmapData( + mapping::PoseGraph::SubmapData GetSubmapData( const mapping::SubmapId& submap_id) EXCLUDES(mutex_) override; - mapping::MapById + mapping::MapById GetAllSubmapData() EXCLUDES(mutex_) override; transform::Rigid3d GetLocalToGlobalTransform(int trajectory_id) EXCLUDES(mutex_) override; @@ -179,7 +179,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { submap_transforms, int trajectory_id) const REQUIRES(mutex_); - mapping::SparsePoseGraph::SubmapData GetSubmapDataUnderLock( + mapping::PoseGraph::SubmapData GetSubmapDataUnderLock( const mapping::SubmapId& submap_id) REQUIRES(mutex_); common::Time GetLatestNodeTime(const mapping::NodeId& node_id,