12345qiupeng
/
cartographer
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Rename mapping::SparsePoseGraph. (#677) 

[RFC=0001](https://github.com/googlecartographer/rfcs/blob/master/text/0001-renaming-sparse-pose-graph.md)
master
Wolfgang Hess 2017-11-15 12:06:19 +01:00 committed by Wally B. Feed
parent 36b9cf7f9a
commit 8c114d6eaf
16 changed files with 87 additions and 92 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
cartographer/mapping/global_trajectory_builder.h
View File

@ -25,16 +25,16 @@ namespace cartographer {
namespace mapping {
template <typename LocalTrajectoryBuilder,
typename LocalTrajectoryBuilderOptions, typename SparsePoseGraph>
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<mapping::NodeId> node_id;
if (matching_result->insertion_result != nullptr) {
node_id = ::cartographer::common::make_unique<mapping::NodeId>(
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_;
};

32
cartographer/mapping/map_builder.cc
View File

@ -62,12 +62,12 @@ MapBuilder::MapBuilder(
if (options.use_trajectory_builder_2d()) {
sparse_pose_graph_2d_ = common::make_unique<mapping_2d::SparsePoseGraph>(
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<mapping_3d::SparsePoseGraph>(
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<PureLocalizationTrimmer>(
pose_graph_->AddTrimmer(common::make_unique<PureLocalizationTrimmer>(
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<SubmapId, transform::Rigid3d> 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

8
cartographer/mapping/map_builder.h
View File

@ -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<mapping_2d::SparsePoseGraph> sparse_pose_graph_2d_;
std::unique_ptr<mapping_3d::SparsePoseGraph> sparse_pose_graph_3d_;
mapping::SparsePoseGraph* sparse_pose_graph_;
mapping::PoseGraph* pose_graph_;
LocalSlamResultCallback local_slam_result_callback_;

31
cartographer/mapping/sparse_pose_graph.cc → cartographer/mapping/pose_graph.cc
View File

@ -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<SparsePoseGraph::Constraint> FromProto(
std::vector<PoseGraph::Constraint> FromProto(
const ::google::protobuf::RepeatedPtrField<
::cartographer::mapping::proto::SparsePoseGraph::Constraint>&
constraint_protos) {
std::vector<SparsePoseGraph::Constraint> constraints;
proto::SparsePoseGraph::Constraint>& constraint_protos) {
std::vector<PoseGraph::Constraint> 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<int, proto::Trajectory* const> trajectory_protos;

18
cartographer/mapping/sparse_pose_graph.h → cartographer/mapping/pose_graph.h
View File

@ -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 <memory>
#include <set>
@ -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<SparsePoseGraph::Constraint> FromProto(
std::vector<PoseGraph::Constraint> FromProto(
const ::google::protobuf::RepeatedPtrField<
::cartographer::mapping::proto::SparsePoseGraph::Constraint>&
constraint_protos);
@ -171,4 +171,4 @@ std::vector<SparsePoseGraph::Constraint> FromProto(
} // namespace mapping
} // namespace cartographer
#endif // CARTOGRAPHER_MAPPING_SPARSE_POSE_GRAPH_H_
#endif // CARTOGRAPHER_MAPPING_POSE_GRAPH_H_

4
cartographer/mapping_2d/pose_graph/constraint_builder.h
View File

@ -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<Constraint>;
ConstraintBuilder(

4
cartographer/mapping_2d/pose_graph/optimization_problem.h
View File

@ -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);

4
cartographer/mapping_2d/pose_graph/spa_cost_function.h
View File

@ -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) {}

15
cartographer/mapping_2d/sparse_pose_graph.cc
View File

@ -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<std::vector<int>> 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::SubmapId, mapping::SparsePoseGraph::SubmapData>
mapping::MapById<mapping::SubmapId, mapping::PoseGraph::SubmapData>
SparsePoseGraph::GetAllSubmapData() {
common::MutexLocker locker(&mutex_);
mapping::MapById<mapping::SubmapId, mapping::SparsePoseGraph::SubmapData>
submaps;
mapping::MapById<mapping::SubmapId, mapping::PoseGraph::SubmapData> 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);

10
cartographer/mapping_2d/sparse_pose_graph.h
View File

@ -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<mapping::PoseGraphTrimmer> trimmer) override;
void RunFinalOptimization() override;
std::vector<std::vector<int>> GetConnectedTrajectories() override;
mapping::SparsePoseGraph::SubmapData GetSubmapData(
mapping::PoseGraph::SubmapData GetSubmapData(
const mapping::SubmapId& submap_id) EXCLUDES(mutex_) override;
mapping::MapById<mapping::SubmapId, mapping::SparsePoseGraph::SubmapData>
mapping::MapById<mapping::SubmapId, mapping::PoseGraph::SubmapData>
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,

2
cartographer/mapping_3d/pose_graph/constraint_builder.h
View File

@ -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<Constraint>;
ConstraintBuilder(

2
cartographer/mapping_3d/pose_graph/optimization_problem.cc
View File

@ -385,7 +385,7 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& 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()};

4
cartographer/mapping_3d/pose_graph/optimization_problem.h
View File

@ -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 };

4
cartographer/mapping_3d/pose_graph/spa_cost_function.h
View File

@ -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) {}

15
cartographer/mapping_3d/sparse_pose_graph.cc
View File

@ -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<std::vector<int>> 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::SubmapId, mapping::SparsePoseGraph::SubmapData>
mapping::MapById<mapping::SubmapId, mapping::PoseGraph::SubmapData>
SparsePoseGraph::GetAllSubmapData() {
common::MutexLocker locker(&mutex_);
mapping::MapById<mapping::SubmapId, mapping::SparsePoseGraph::SubmapData>
submaps;
mapping::MapById<mapping::SubmapId, mapping::PoseGraph::SubmapData> 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);

10
cartographer/mapping_3d/sparse_pose_graph.h
View File

@ -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<mapping::PoseGraphTrimmer> trimmer) override;
void RunFinalOptimization() override;
std::vector<std::vector<int>> GetConnectedTrajectories() override;
mapping::SparsePoseGraph::SubmapData GetSubmapData(
mapping::PoseGraph::SubmapData GetSubmapData(
const mapping::SubmapId& submap_id) EXCLUDES(mutex_) override;
mapping::MapById<mapping::SubmapId, mapping::SparsePoseGraph::SubmapData>
mapping::MapById<mapping::SubmapId, mapping::PoseGraph::SubmapData>
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,