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 { namespace mapping {
template <typename LocalTrajectoryBuilder, template <typename LocalTrajectoryBuilder,
typename LocalTrajectoryBuilderOptions, typename SparsePoseGraph> typename LocalTrajectoryBuilderOptions, typename PoseGraph>
class GlobalTrajectoryBuilder class GlobalTrajectoryBuilder
: public mapping::GlobalTrajectoryBuilderInterface { : public mapping::GlobalTrajectoryBuilderInterface {
public: public:
GlobalTrajectoryBuilder( GlobalTrajectoryBuilder(
const LocalTrajectoryBuilderOptions& options, const int trajectory_id, const LocalTrajectoryBuilderOptions& options, const int trajectory_id,
SparsePoseGraph* const sparse_pose_graph, PoseGraph* const pose_graph,
const LocalSlamResultCallback& local_slam_result_callback) const LocalSlamResultCallback& local_slam_result_callback)
: trajectory_id_(trajectory_id), : trajectory_id_(trajectory_id),
sparse_pose_graph_(sparse_pose_graph), pose_graph_(pose_graph),
local_trajectory_builder_(options), local_trajectory_builder_(options),
local_slam_result_callback_(local_slam_result_callback) {} local_slam_result_callback_(local_slam_result_callback) {}
~GlobalTrajectoryBuilder() override {} ~GlobalTrajectoryBuilder() override {}
@ -59,7 +59,7 @@ class GlobalTrajectoryBuilder
std::unique_ptr<mapping::NodeId> node_id; std::unique_ptr<mapping::NodeId> node_id;
if (matching_result->insertion_result != nullptr) { if (matching_result->insertion_result != nullptr) {
node_id = ::cartographer::common::make_unique<mapping::NodeId>( 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->constant_data, trajectory_id_,
matching_result->insertion_result->insertion_submaps)); matching_result->insertion_result->insertion_submaps));
CHECK_EQ(node_id->trajectory_id, trajectory_id_); CHECK_EQ(node_id->trajectory_id, trajectory_id_);
@ -73,24 +73,24 @@ class GlobalTrajectoryBuilder
void AddSensorData(const sensor::ImuData& imu_data) override { void AddSensorData(const sensor::ImuData& imu_data) override {
local_trajectory_builder_.AddImuData(imu_data); 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 { void AddSensorData(const sensor::OdometryData& odometry_data) override {
CHECK(odometry_data.pose.IsValid()) << odometry_data.pose; CHECK(odometry_data.pose.IsValid()) << odometry_data.pose;
local_trajectory_builder_.AddOdometryData(odometry_data); local_trajectory_builder_.AddOdometryData(odometry_data);
sparse_pose_graph_->AddOdometryData(trajectory_id_, odometry_data); pose_graph_->AddOdometryData(trajectory_id_, odometry_data);
} }
void AddSensorData( void AddSensorData(
const sensor::FixedFramePoseData& fixed_frame_pose) override { const sensor::FixedFramePoseData& fixed_frame_pose) override {
CHECK(fixed_frame_pose.pose.IsValid()) << fixed_frame_pose.pose; 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: private:
const int trajectory_id_; const int trajectory_id_;
SparsePoseGraph* const sparse_pose_graph_; PoseGraph* const pose_graph_;
LocalTrajectoryBuilder local_trajectory_builder_; LocalTrajectoryBuilder local_trajectory_builder_;
LocalSlamResultCallback local_slam_result_callback_; 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()) { if (options.use_trajectory_builder_2d()) {
sparse_pose_graph_2d_ = common::make_unique<mapping_2d::SparsePoseGraph>( sparse_pose_graph_2d_ = common::make_unique<mapping_2d::SparsePoseGraph>(
options_.sparse_pose_graph_options(), &thread_pool_); 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()) { if (options.use_trajectory_builder_3d()) {
sparse_pose_graph_3d_ = common::make_unique<mapping_3d::SparsePoseGraph>( sparse_pose_graph_3d_ = common::make_unique<mapping_3d::SparsePoseGraph>(
options_.sparse_pose_graph_options(), &thread_pool_); 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()) { if (trajectory_options.pure_localization()) {
constexpr int kSubmapsToKeep = 3; constexpr int kSubmapsToKeep = 3;
sparse_pose_graph_->AddTrimmer(common::make_unique<PureLocalizationTrimmer>( pose_graph_->AddTrimmer(common::make_unique<PureLocalizationTrimmer>(
trajectory_id, kSubmapsToKeep)); trajectory_id, kSubmapsToKeep));
} }
if (trajectory_options.has_initial_trajectory_pose()) { if (trajectory_options.has_initial_trajectory_pose()) {
const auto& initial_trajectory_pose = const auto& initial_trajectory_pose =
trajectory_options.initial_trajectory_pose(); trajectory_options.initial_trajectory_pose();
sparse_pose_graph_->SetInitialTrajectoryPose( pose_graph_->SetInitialTrajectoryPose(
trajectory_id, initial_trajectory_pose.to_trajectory_id(), trajectory_id, initial_trajectory_pose.to_trajectory_id(),
transform::ToRigid3(initial_trajectory_pose.relative_pose()), transform::ToRigid3(initial_trajectory_pose.relative_pose()),
common::FromUniversal(initial_trajectory_pose.timestamp())); common::FromUniversal(initial_trajectory_pose.timestamp()));
@ -131,7 +131,7 @@ TrajectoryBuilder* MapBuilder::GetTrajectoryBuilder(
void MapBuilder::FinishTrajectory(const int trajectory_id) { void MapBuilder::FinishTrajectory(const int trajectory_id) {
sensor_collator_.FinishTrajectory(trajectory_id); sensor_collator_.FinishTrajectory(trajectory_id);
sparse_pose_graph_->FinishTrajectory(trajectory_id); pose_graph_->FinishTrajectory(trajectory_id);
} }
int MapBuilder::GetBlockingTrajectoryId() const { int MapBuilder::GetBlockingTrajectoryId() const {
@ -148,7 +148,7 @@ std::string MapBuilder::SubmapToProto(
std::to_string(num_trajectory_builders()) + " trajectories."; 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) { if (submap_data.submap == nullptr) {
return "Requested submap " + std::to_string(submap_id.submap_index) + return "Requested submap " + std::to_string(submap_id.submap_index) +
" from trajectory " + std::to_string(submap_id.trajectory_id) + " from trajectory " + std::to_string(submap_id.trajectory_id) +
@ -160,10 +160,10 @@ std::string MapBuilder::SubmapToProto(
void MapBuilder::SerializeState(io::ProtoStreamWriter* const writer) { void MapBuilder::SerializeState(io::ProtoStreamWriter* const writer) {
// We serialize the pose graph followed by all the data referenced in it. // 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. // 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; proto::SerializedData proto;
auto* const submap_proto = proto.mutable_submap(); auto* const submap_proto = proto.mutable_submap();
submap_proto->mutable_submap_id()->set_trajectory_id( 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. // 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; proto::SerializedData proto;
auto* const node_proto = proto.mutable_node(); auto* const node_proto = proto.mutable_node();
node_proto->mutable_node_id()->set_trajectory_id( 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. // 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 int trajectory_id : all_imu_data.trajectory_ids()) {
for (const auto& imu_data : all_imu_data.trajectory(trajectory_id)) { for (const auto& imu_data : all_imu_data.trajectory(trajectory_id)) {
proto::SerializedData proto; proto::SerializedData proto;
@ -202,7 +202,7 @@ void MapBuilder::SerializeState(io::ProtoStreamWriter* const writer) {
} }
// Next we serialize odometry data from the pose graph. // 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 int trajectory_id : all_odometry_data.trajectory_ids()) {
for (const auto& odometry_data : for (const auto& odometry_data :
all_odometry_data.trajectory(trajectory_id)) { all_odometry_data.trajectory(trajectory_id)) {
@ -230,7 +230,7 @@ void MapBuilder::LoadMap(io::ProtoStreamReader* const reader) {
.second) .second)
<< "Duplicate trajectory ID: " << trajectory_proto.trajectory_id(); << "Duplicate trajectory ID: " << trajectory_proto.trajectory_id();
trajectory_proto.set_trajectory_id(new_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; MapById<SubmapId, transform::Rigid3d> submap_poses;
@ -263,7 +263,7 @@ void MapBuilder::LoadMap(io::ProtoStreamReader* const reader) {
const transform::Rigid3d node_pose = const transform::Rigid3d node_pose =
node_poses.at(NodeId{proto.node().node_id().trajectory_id(), node_poses.at(NodeId{proto.node().node_id().trajectory_id(),
proto.node().node_id().node_index()}); 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()) { if (proto.has_submap()) {
proto.mutable_submap()->mutable_submap_id()->set_trajectory_id( 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 = const transform::Rigid3d submap_pose =
submap_poses.at(SubmapId{proto.submap().submap_id().trajectory_id(), submap_poses.at(SubmapId{proto.submap().submap_id().trajectory_id(),
proto.submap().submap_id().submap_index()}); 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 // TODO(ojura): Deserialize IMU and odometry data when loading unfrozen
// trajectories. // trajectories.
@ -290,7 +290,7 @@ void MapBuilder::LoadMap(io::ProtoStreamReader* const reader) {
const SubmapId submap_id{ const SubmapId submap_id{
trajectory_remapping.at(constraint_proto.submap_id().trajectory_id()), trajectory_remapping.at(constraint_proto.submap_id().trajectory_id()),
constraint_proto.submap_id().submap_index()}; constraint_proto.submap_id().submap_index()};
sparse_pose_graph_->AddNodeToSubmap(node_id, submap_id); pose_graph_->AddNodeToSubmap(node_id, submap_id);
} }
CHECK(reader->eof()); CHECK(reader->eof());
} }
@ -299,7 +299,7 @@ int MapBuilder::num_trajectory_builders() const {
return trajectory_builders_.size(); return trajectory_builders_.size();
} }
SparsePoseGraph* MapBuilder::sparse_pose_graph() { return sparse_pose_graph_; } PoseGraph* MapBuilder::pose_graph() { return pose_graph_; }
} // namespace mapping } // namespace mapping
} // namespace cartographer } // namespace cartographer

8
cartographer/mapping/map_builder.h
View File

@ -29,10 +29,10 @@
#include "cartographer/common/thread_pool.h" #include "cartographer/common/thread_pool.h"
#include "cartographer/io/proto_stream.h" #include "cartographer/io/proto_stream.h"
#include "cartographer/mapping/id.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/map_builder_options.pb.h"
#include "cartographer/mapping/proto/submap_visualization.pb.h" #include "cartographer/mapping/proto/submap_visualization.pb.h"
#include "cartographer/mapping/proto/trajectory_builder_options.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/submaps.h"
#include "cartographer/mapping/trajectory_builder.h" #include "cartographer/mapping/trajectory_builder.h"
#include "cartographer/mapping_2d/sparse_pose_graph.h" #include "cartographer/mapping_2d/sparse_pose_graph.h"
@ -46,7 +46,7 @@ proto::MapBuilderOptions CreateMapBuilderOptions(
common::LuaParameterDictionary* const parameter_dictionary); common::LuaParameterDictionary* const parameter_dictionary);
// Wires up the complete SLAM stack with TrajectoryBuilders (for local submaps) // 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 { class MapBuilder {
public: public:
using LocalSlamResultCallback = using LocalSlamResultCallback =
@ -95,7 +95,7 @@ class MapBuilder {
int num_trajectory_builders() const; int num_trajectory_builders() const;
mapping::SparsePoseGraph* sparse_pose_graph(); mapping::PoseGraph* pose_graph();
private: private:
const proto::MapBuilderOptions options_; const proto::MapBuilderOptions options_;
@ -103,7 +103,7 @@ class MapBuilder {
std::unique_ptr<mapping_2d::SparsePoseGraph> sparse_pose_graph_2d_; std::unique_ptr<mapping_2d::SparsePoseGraph> sparse_pose_graph_2d_;
std::unique_ptr<mapping_3d::SparsePoseGraph> sparse_pose_graph_3d_; std::unique_ptr<mapping_3d::SparsePoseGraph> sparse_pose_graph_3d_;
mapping::SparsePoseGraph* sparse_pose_graph_; mapping::PoseGraph* pose_graph_;
LocalSlamResultCallback local_slam_result_callback_; 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. * 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/constraint_builder.h"
#include "cartographer/mapping/pose_graph/optimization_problem_options.h" #include "cartographer/mapping/pose_graph/optimization_problem_options.h"
@ -25,48 +25,45 @@ namespace cartographer {
namespace mapping { namespace mapping {
proto::SparsePoseGraph::Constraint::Tag ToProto( proto::SparsePoseGraph::Constraint::Tag ToProto(
const SparsePoseGraph::Constraint::Tag& tag) { const PoseGraph::Constraint::Tag& tag) {
switch (tag) { switch (tag) {
case SparsePoseGraph::Constraint::Tag::INTRA_SUBMAP: case PoseGraph::Constraint::Tag::INTRA_SUBMAP:
return proto::SparsePoseGraph::Constraint::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; return proto::SparsePoseGraph::Constraint::INTER_SUBMAP;
} }
LOG(FATAL) << "Unsupported tag."; LOG(FATAL) << "Unsupported tag.";
} }
SparsePoseGraph::Constraint::Tag FromProto( PoseGraph::Constraint::Tag FromProto(
const proto::SparsePoseGraph::Constraint::Tag& proto) { const proto::SparsePoseGraph::Constraint::Tag& proto) {
switch (proto) { switch (proto) {
case proto::SparsePoseGraph::Constraint::INTRA_SUBMAP: case proto::SparsePoseGraph::Constraint::INTRA_SUBMAP:
return SparsePoseGraph::Constraint::Tag::INTRA_SUBMAP; return PoseGraph::Constraint::Tag::INTRA_SUBMAP;
case proto::SparsePoseGraph::Constraint::INTER_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::kint32max:
case ::google::protobuf::kint32min:; case ::google::protobuf::kint32min:;
} }
LOG(FATAL) << "Unsupported tag."; LOG(FATAL) << "Unsupported tag.";
} }
std::vector<SparsePoseGraph::Constraint> FromProto( std::vector<PoseGraph::Constraint> FromProto(
const ::google::protobuf::RepeatedPtrField< const ::google::protobuf::RepeatedPtrField<
::cartographer::mapping::proto::SparsePoseGraph::Constraint>& proto::SparsePoseGraph::Constraint>& constraint_protos) {
constraint_protos) { std::vector<PoseGraph::Constraint> constraints;
std::vector<SparsePoseGraph::Constraint> constraints;
for (const auto& constraint_proto : constraint_protos) { for (const auto& constraint_proto : constraint_protos) {
const mapping::SubmapId submap_id{ const mapping::SubmapId submap_id{
constraint_proto.submap_id().trajectory_id(), constraint_proto.submap_id().trajectory_id(),
constraint_proto.submap_id().submap_index()}; constraint_proto.submap_id().submap_index()};
const mapping::NodeId node_id{constraint_proto.node_id().trajectory_id(), const mapping::NodeId node_id{constraint_proto.node_id().trajectory_id(),
constraint_proto.node_id().node_index()}; constraint_proto.node_id().node_index()};
const SparsePoseGraph::Constraint::Pose pose{ const PoseGraph::Constraint::Pose pose{
transform::ToRigid3(constraint_proto.relative_pose()), transform::ToRigid3(constraint_proto.relative_pose()),
constraint_proto.translation_weight(), constraint_proto.translation_weight(),
constraint_proto.rotation_weight()}; constraint_proto.rotation_weight()};
const SparsePoseGraph::Constraint::Tag tag = const PoseGraph::Constraint::Tag tag = FromProto(constraint_proto.tag());
FromProto(constraint_proto.tag()); constraints.push_back(PoseGraph::Constraint{submap_id, node_id, pose, tag});
constraints.push_back(
SparsePoseGraph::Constraint{submap_id, node_id, pose, tag});
} }
return constraints; return constraints;
} }
@ -99,7 +96,7 @@ proto::SparsePoseGraphOptions CreateSparsePoseGraphOptions(
return options; return options;
} }
proto::SparsePoseGraph SparsePoseGraph::ToProto() { proto::SparsePoseGraph PoseGraph::ToProto() {
proto::SparsePoseGraph proto; proto::SparsePoseGraph proto;
std::map<int, proto::Trajectory* const> trajectory_protos; 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. * limitations under the License.
*/ */
#ifndef CARTOGRAPHER_MAPPING_SPARSE_POSE_GRAPH_H_ #ifndef CARTOGRAPHER_MAPPING_POSE_GRAPH_H_
#define CARTOGRAPHER_MAPPING_SPARSE_POSE_GRAPH_H_ #define CARTOGRAPHER_MAPPING_POSE_GRAPH_H_
#include <memory> #include <memory>
#include <set> #include <set>
@ -42,7 +42,7 @@ namespace mapping {
proto::SparsePoseGraphOptions CreateSparsePoseGraphOptions( proto::SparsePoseGraphOptions CreateSparsePoseGraphOptions(
common::LuaParameterDictionary* const parameter_dictionary); common::LuaParameterDictionary* const parameter_dictionary);
class SparsePoseGraph { class PoseGraph {
public: public:
// A "constraint" as in the paper by Konolige, Kurt, et al. "Efficient sparse // A "constraint" as in the paper by Konolige, Kurt, et al. "Efficient sparse
// pose adjustment for 2d mapping." Intelligent Robots and Systems (IROS), // pose adjustment for 2d mapping." Intelligent Robots and Systems (IROS),
@ -77,11 +77,11 @@ class SparsePoseGraph {
common::Time time; common::Time time;
}; };
SparsePoseGraph() {} PoseGraph() {}
virtual ~SparsePoseGraph() {} virtual ~PoseGraph() {}
SparsePoseGraph(const SparsePoseGraph&) = delete; PoseGraph(const PoseGraph&) = delete;
SparsePoseGraph& operator=(const SparsePoseGraph&) = delete; PoseGraph& operator=(const PoseGraph&) = delete;
// Inserts an IMU measurement. // Inserts an IMU measurement.
virtual void AddImuData(int trajectory_id, virtual void AddImuData(int trajectory_id,
@ -163,7 +163,7 @@ class SparsePoseGraph {
const common::Time time) = 0; const common::Time time) = 0;
}; };
std::vector<SparsePoseGraph::Constraint> FromProto( std::vector<PoseGraph::Constraint> FromProto(
const ::google::protobuf::RepeatedPtrField< const ::google::protobuf::RepeatedPtrField<
::cartographer::mapping::proto::SparsePoseGraph::Constraint>& ::cartographer::mapping::proto::SparsePoseGraph::Constraint>&
constraint_protos); constraint_protos);
@ -171,4 +171,4 @@ std::vector<SparsePoseGraph::Constraint> FromProto(
} // namespace mapping } // namespace mapping
} // namespace cartographer } // 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/math.h"
#include "cartographer/common/mutex.h" #include "cartographer/common/mutex.h"
#include "cartographer/common/thread_pool.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/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/ceres_scan_matcher.h"
#include "cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.h" #include "cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.h"
#include "cartographer/mapping_2d/submaps.h" #include "cartographer/mapping_2d/submaps.h"
@ -58,7 +58,7 @@ transform::Rigid2d ComputeSubmapPose(const Submap& submap);
// This class is thread-safe. // This class is thread-safe.
class ConstraintBuilder { class ConstraintBuilder {
public: public:
using Constraint = mapping::SparsePoseGraph::Constraint; using Constraint = mapping::PoseGraph::Constraint;
using Result = std::vector<Constraint>; using Result = std::vector<Constraint>;
ConstraintBuilder( ConstraintBuilder(

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

@ -28,8 +28,8 @@
#include "cartographer/common/port.h" #include "cartographer/common/port.h"
#include "cartographer/common/time.h" #include "cartographer/common/time.h"
#include "cartographer/mapping/id.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/pose_graph/proto/optimization_problem_options.pb.h"
#include "cartographer/mapping/sparse_pose_graph.h"
#include "cartographer/sensor/imu_data.h" #include "cartographer/sensor/imu_data.h"
#include "cartographer/sensor/map_by_time.h" #include "cartographer/sensor/map_by_time.h"
#include "cartographer/sensor/odometry_data.h" #include "cartographer/sensor/odometry_data.h"
@ -53,7 +53,7 @@ struct SubmapData {
// Implements the SPA loop closure method. // Implements the SPA loop closure method.
class OptimizationProblem { class OptimizationProblem {
public: public:
using Constraint = mapping::SparsePoseGraph::Constraint; using Constraint = mapping::PoseGraph::Constraint;
explicit OptimizationProblem( explicit OptimizationProblem(
const mapping::pose_graph::proto::OptimizationProblemOptions& options); 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/Core"
#include "Eigen/Geometry" #include "Eigen/Geometry"
#include "cartographer/common/math.h" #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/rigid_transform.h"
#include "cartographer/transform/transform.h" #include "cartographer/transform/transform.h"
#include "ceres/ceres.h" #include "ceres/ceres.h"
@ -34,7 +34,7 @@ namespace pose_graph {
class SpaCostFunction { class SpaCostFunction {
public: public:
using Constraint = mapping::SparsePoseGraph::Constraint; using Constraint = mapping::PoseGraph::Constraint;
explicit SpaCostFunction(const Constraint::Pose& pose) : pose_(pose) {} 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( void SparsePoseGraph::UpdateTrajectoryConnectivity(
const Constraint& constraint) { 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 = const common::Time time =
GetLatestNodeTime(constraint.node_id, constraint.submap_id); GetLatestNodeTime(constraint.node_id, constraint.submap_id);
trajectory_connectivity_state_.Connect(constraint.node_id.trajectory_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(); return trajectory_connectivity_state_.Components();
} }
mapping::SparsePoseGraph::SubmapData SparsePoseGraph::GetSubmapData( mapping::PoseGraph::SubmapData SparsePoseGraph::GetSubmapData(
const mapping::SubmapId& submap_id) { const mapping::SubmapId& submap_id) {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
return GetSubmapDataUnderLock(submap_id); return GetSubmapDataUnderLock(submap_id);
} }
mapping::MapById<mapping::SubmapId, mapping::SparsePoseGraph::SubmapData> mapping::MapById<mapping::SubmapId, mapping::PoseGraph::SubmapData>
SparsePoseGraph::GetAllSubmapData() { SparsePoseGraph::GetAllSubmapData() {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
mapping::MapById<mapping::SubmapId, mapping::SparsePoseGraph::SubmapData> mapping::MapById<mapping::SubmapId, mapping::PoseGraph::SubmapData> submaps;
submaps;
for (const auto& submap_id_data : submap_data_) { for (const auto& submap_id_data : submap_data_) {
submaps.Insert(submap_id_data.id, submaps.Insert(submap_id_data.id,
GetSubmapDataUnderLock(submap_id_data.id)); GetSubmapDataUnderLock(submap_id_data.id));
@ -659,7 +658,7 @@ transform::Rigid3d SparsePoseGraph::ComputeLocalToGlobalTransform(
.inverse(); .inverse();
} }
mapping::SparsePoseGraph::SubmapData SparsePoseGraph::GetSubmapDataUnderLock( mapping::PoseGraph::SubmapData SparsePoseGraph::GetSubmapDataUnderLock(
const mapping::SubmapId& submap_id) { const mapping::SubmapId& submap_id) {
const auto it = submap_data_.find(submap_id); const auto it = submap_data_.find(submap_id);
if (it == submap_data_.end()) { if (it == submap_data_.end()) {
@ -737,8 +736,8 @@ void SparsePoseGraph::TrimmingHandle::MarkSubmapAsTrimmed(
parent_->constraint_builder_.DeleteScanMatcher(submap_id); parent_->constraint_builder_.DeleteScanMatcher(submap_id);
parent_->optimization_problem_.TrimSubmap(submap_id); parent_->optimization_problem_.TrimSubmap(submap_id);
// Remove the 'nodes_to_remove' from the sparse pose graph and the // Remove the 'nodes_to_remove' from the pose graph and the optimization
// optimization problem. // problem.
for (const mapping::NodeId& node_id : nodes_to_remove) { for (const mapping::NodeId& node_id : nodes_to_remove) {
parent_->trajectory_nodes_.Trim(node_id); parent_->trajectory_nodes_.Trim(node_id);
parent_->optimization_problem_.TrimTrajectoryNode(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/mutex.h"
#include "cartographer/common/thread_pool.h" #include "cartographer/common/thread_pool.h"
#include "cartographer/common/time.h" #include "cartographer/common/time.h"
#include "cartographer/mapping/pose_graph.h"
#include "cartographer/mapping/pose_graph_trimmer.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/trajectory_connectivity_state.h"
#include "cartographer/mapping_2d/pose_graph/constraint_builder.h" #include "cartographer/mapping_2d/pose_graph/constraint_builder.h"
#include "cartographer/mapping_2d/pose_graph/optimization_problem.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 // 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. // 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. // All constraints are between a submap i and a node j.
class SparsePoseGraph : public mapping::SparsePoseGraph { class SparsePoseGraph : public mapping::PoseGraph {
public: public:
SparsePoseGraph(const mapping::proto::SparsePoseGraphOptions& options, SparsePoseGraph(const mapping::proto::SparsePoseGraphOptions& options,
common::ThreadPool* thread_pool); common::ThreadPool* thread_pool);
@ -98,9 +98,9 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
void AddTrimmer(std::unique_ptr<mapping::PoseGraphTrimmer> trimmer) override; void AddTrimmer(std::unique_ptr<mapping::PoseGraphTrimmer> trimmer) override;
void RunFinalOptimization() override; void RunFinalOptimization() override;
std::vector<std::vector<int>> GetConnectedTrajectories() override; std::vector<std::vector<int>> GetConnectedTrajectories() override;
mapping::SparsePoseGraph::SubmapData GetSubmapData( mapping::PoseGraph::SubmapData GetSubmapData(
const mapping::SubmapId& submap_id) EXCLUDES(mutex_) override; 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; GetAllSubmapData() EXCLUDES(mutex_) override;
transform::Rigid3d GetLocalToGlobalTransform(int trajectory_id) transform::Rigid3d GetLocalToGlobalTransform(int trajectory_id)
EXCLUDES(mutex_) override; EXCLUDES(mutex_) override;
@ -179,7 +179,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
submap_transforms, submap_transforms,
int trajectory_id) const REQUIRES(mutex_); int trajectory_id) const REQUIRES(mutex_);
mapping::SparsePoseGraph::SubmapData GetSubmapDataUnderLock( mapping::PoseGraph::SubmapData GetSubmapDataUnderLock(
const mapping::SubmapId& submap_id) REQUIRES(mutex_); const mapping::SubmapId& submap_id) REQUIRES(mutex_);
common::Time GetLatestNodeTime(const mapping::NodeId& node_id, 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. // This class is thread-safe.
class ConstraintBuilder { class ConstraintBuilder {
public: public:
using Constraint = mapping::SparsePoseGraph::Constraint; using Constraint = mapping::PoseGraph::Constraint;
using Result = std::vector<Constraint>; using Result = std::vector<Constraint>;
ConstraintBuilder( 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; continue;
} }
const mapping::SparsePoseGraph::Constraint::Pose constraint_pose{ const mapping::PoseGraph::Constraint::Pose constraint_pose{
*fixed_frame_pose, options_.fixed_frame_pose_translation_weight(), *fixed_frame_pose, options_.fixed_frame_pose_translation_weight(),
options_.fixed_frame_pose_rotation_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/port.h"
#include "cartographer/common/time.h" #include "cartographer/common/time.h"
#include "cartographer/mapping/id.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/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/fixed_frame_pose_data.h"
#include "cartographer/sensor/imu_data.h" #include "cartographer/sensor/imu_data.h"
#include "cartographer/sensor/map_by_time.h" #include "cartographer/sensor/map_by_time.h"
@ -53,7 +53,7 @@ struct SubmapData {
// Implements the SPA loop closure method. // Implements the SPA loop closure method.
class OptimizationProblem { class OptimizationProblem {
public: public:
using Constraint = mapping::SparsePoseGraph::Constraint; using Constraint = mapping::PoseGraph::Constraint;
enum class FixZ { kYes, kNo }; 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/Core"
#include "Eigen/Geometry" #include "Eigen/Geometry"
#include "cartographer/common/math.h" #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/rigid_transform.h"
#include "cartographer/transform/transform.h" #include "cartographer/transform/transform.h"
#include "ceres/ceres.h" #include "ceres/ceres.h"
@ -34,7 +34,7 @@ namespace pose_graph {
class SpaCostFunction { class SpaCostFunction {
public: public:
using Constraint = mapping::SparsePoseGraph::Constraint; using Constraint = mapping::PoseGraph::Constraint;
explicit SpaCostFunction(const Constraint::Pose& pose) : pose_(pose) {} 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( void SparsePoseGraph::UpdateTrajectoryConnectivity(
const Constraint& constraint) { 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 = const common::Time time =
GetLatestNodeTime(constraint.node_id, constraint.submap_id); GetLatestNodeTime(constraint.node_id, constraint.submap_id);
trajectory_connectivity_state_.Connect(constraint.node_id.trajectory_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(); return trajectory_connectivity_state_.Components();
} }
mapping::SparsePoseGraph::SubmapData SparsePoseGraph::GetSubmapData( mapping::PoseGraph::SubmapData SparsePoseGraph::GetSubmapData(
const mapping::SubmapId& submap_id) { const mapping::SubmapId& submap_id) {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
return GetSubmapDataUnderLock(submap_id); return GetSubmapDataUnderLock(submap_id);
} }
mapping::MapById<mapping::SubmapId, mapping::SparsePoseGraph::SubmapData> mapping::MapById<mapping::SubmapId, mapping::PoseGraph::SubmapData>
SparsePoseGraph::GetAllSubmapData() { SparsePoseGraph::GetAllSubmapData() {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
mapping::MapById<mapping::SubmapId, mapping::SparsePoseGraph::SubmapData> mapping::MapById<mapping::SubmapId, mapping::PoseGraph::SubmapData> submaps;
submaps;
for (const auto& submap_id_data : submap_data_) { for (const auto& submap_id_data : submap_data_) {
submaps.Insert(submap_id_data.id, submaps.Insert(submap_id_data.id,
GetSubmapDataUnderLock(submap_id_data.id)); GetSubmapDataUnderLock(submap_id_data.id));
@ -675,7 +674,7 @@ transform::Rigid3d SparsePoseGraph::ComputeLocalToGlobalTransform(
.inverse(); .inverse();
} }
mapping::SparsePoseGraph::SubmapData SparsePoseGraph::GetSubmapDataUnderLock( mapping::PoseGraph::SubmapData SparsePoseGraph::GetSubmapDataUnderLock(
const mapping::SubmapId& submap_id) { const mapping::SubmapId& submap_id) {
const auto it = submap_data_.find(submap_id); const auto it = submap_data_.find(submap_id);
if (it == submap_data_.end()) { if (it == submap_data_.end()) {
@ -751,8 +750,8 @@ void SparsePoseGraph::TrimmingHandle::MarkSubmapAsTrimmed(
parent_->constraint_builder_.DeleteScanMatcher(submap_id); parent_->constraint_builder_.DeleteScanMatcher(submap_id);
parent_->optimization_problem_.TrimSubmap(submap_id); parent_->optimization_problem_.TrimSubmap(submap_id);
// Remove the 'nodes_to_remove' from the sparse pose graph and the // Remove the 'nodes_to_remove' from the pose graph and the optimization
// optimization problem. // problem.
for (const mapping::NodeId& node_id : nodes_to_remove) { for (const mapping::NodeId& node_id : nodes_to_remove) {
parent_->trajectory_nodes_.Trim(node_id); parent_->trajectory_nodes_.Trim(node_id);
parent_->optimization_problem_.TrimTrajectoryNode(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/mutex.h"
#include "cartographer/common/thread_pool.h" #include "cartographer/common/thread_pool.h"
#include "cartographer/common/time.h" #include "cartographer/common/time.h"
#include "cartographer/mapping/pose_graph.h"
#include "cartographer/mapping/pose_graph_trimmer.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/trajectory_connectivity_state.h"
#include "cartographer/mapping_3d/pose_graph/constraint_builder.h" #include "cartographer/mapping_3d/pose_graph/constraint_builder.h"
#include "cartographer/mapping_3d/pose_graph/optimization_problem.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 // 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. // 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. // All constraints are between a submap i and a node j.
class SparsePoseGraph : public mapping::SparsePoseGraph { class SparsePoseGraph : public mapping::PoseGraph {
public: public:
SparsePoseGraph(const mapping::proto::SparsePoseGraphOptions& options, SparsePoseGraph(const mapping::proto::SparsePoseGraphOptions& options,
common::ThreadPool* thread_pool); common::ThreadPool* thread_pool);
@ -98,9 +98,9 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
void AddTrimmer(std::unique_ptr<mapping::PoseGraphTrimmer> trimmer) override; void AddTrimmer(std::unique_ptr<mapping::PoseGraphTrimmer> trimmer) override;
void RunFinalOptimization() override; void RunFinalOptimization() override;
std::vector<std::vector<int>> GetConnectedTrajectories() override; std::vector<std::vector<int>> GetConnectedTrajectories() override;
mapping::SparsePoseGraph::SubmapData GetSubmapData( mapping::PoseGraph::SubmapData GetSubmapData(
const mapping::SubmapId& submap_id) EXCLUDES(mutex_) override; 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; GetAllSubmapData() EXCLUDES(mutex_) override;
transform::Rigid3d GetLocalToGlobalTransform(int trajectory_id) transform::Rigid3d GetLocalToGlobalTransform(int trajectory_id)
EXCLUDES(mutex_) override; EXCLUDES(mutex_) override;
@ -179,7 +179,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
submap_transforms, submap_transforms,
int trajectory_id) const REQUIRES(mutex_); int trajectory_id) const REQUIRES(mutex_);
mapping::SparsePoseGraph::SubmapData GetSubmapDataUnderLock( mapping::PoseGraph::SubmapData GetSubmapDataUnderLock(
const mapping::SubmapId& submap_id) REQUIRES(mutex_); const mapping::SubmapId& submap_id) REQUIRES(mutex_);
common::Time GetLatestNodeTime(const mapping::NodeId& node_id, common::Time GetLatestNodeTime(const mapping::NodeId& node_id,