Wolfgang Hess 2017-11-15 13:50:18 +01:00 committed by Wally B. Feed
parent 8c114d6eaf
commit c25379cd20
7 changed files with 158 additions and 168 deletions

View File

@ -60,14 +60,14 @@ MapBuilder::MapBuilder(
thread_pool_(options.num_background_threads()),
local_slam_result_callback_(local_slam_result_callback) {
if (options.use_trajectory_builder_2d()) {
sparse_pose_graph_2d_ = common::make_unique<mapping_2d::SparsePoseGraph>(
pose_graph_2d_ = common::make_unique<mapping_2d::PoseGraph>(
options_.sparse_pose_graph_options(), &thread_pool_);
pose_graph_ = sparse_pose_graph_2d_.get();
pose_graph_ = pose_graph_2d_.get();
}
if (options.use_trajectory_builder_3d()) {
sparse_pose_graph_3d_ = common::make_unique<mapping_3d::SparsePoseGraph>(
pose_graph_3d_ = common::make_unique<mapping_3d::PoseGraph>(
options_.sparse_pose_graph_options(), &thread_pool_);
pose_graph_ = sparse_pose_graph_3d_.get();
pose_graph_ = pose_graph_3d_.get();
}
}
@ -85,9 +85,9 @@ int MapBuilder::AddTrajectoryBuilder(
common::make_unique<mapping::GlobalTrajectoryBuilder<
mapping_3d::LocalTrajectoryBuilder,
mapping_3d::proto::LocalTrajectoryBuilderOptions,
mapping_3d::SparsePoseGraph>>(
mapping_3d::PoseGraph>>(
trajectory_options.trajectory_builder_3d_options(),
trajectory_id, sparse_pose_graph_3d_.get(),
trajectory_id, pose_graph_3d_.get(),
local_slam_result_callback_)));
} else {
CHECK(trajectory_options.has_trajectory_builder_2d_options());
@ -97,9 +97,9 @@ int MapBuilder::AddTrajectoryBuilder(
common::make_unique<mapping::GlobalTrajectoryBuilder<
mapping_2d::LocalTrajectoryBuilder,
mapping_2d::proto::LocalTrajectoryBuilderOptions,
mapping_2d::SparsePoseGraph>>(
mapping_2d::PoseGraph>>(
trajectory_options.trajectory_builder_2d_options(),
trajectory_id, sparse_pose_graph_2d_.get(),
trajectory_id, pose_graph_2d_.get(),
local_slam_result_callback_)));
}
if (trajectory_options.pure_localization()) {

View File

@ -35,8 +35,8 @@
#include "cartographer/mapping/proto/trajectory_builder_options.pb.h"
#include "cartographer/mapping/submaps.h"
#include "cartographer/mapping/trajectory_builder.h"
#include "cartographer/mapping_2d/sparse_pose_graph.h"
#include "cartographer/mapping_3d/sparse_pose_graph.h"
#include "cartographer/mapping_2d/pose_graph.h"
#include "cartographer/mapping_3d/pose_graph.h"
#include "cartographer/sensor/collator.h"
namespace cartographer {
@ -101,8 +101,8 @@ class MapBuilder {
const proto::MapBuilderOptions options_;
common::ThreadPool thread_pool_;
std::unique_ptr<mapping_2d::SparsePoseGraph> sparse_pose_graph_2d_;
std::unique_ptr<mapping_3d::SparsePoseGraph> sparse_pose_graph_3d_;
std::unique_ptr<mapping_2d::PoseGraph> pose_graph_2d_;
std::unique_ptr<mapping_3d::PoseGraph> pose_graph_3d_;
mapping::PoseGraph* pose_graph_;
LocalSlamResultCallback local_slam_result_callback_;

View File

@ -14,7 +14,7 @@
* limitations under the License.
*/
#include "cartographer/mapping_2d/sparse_pose_graph.h"
#include "cartographer/mapping_2d/pose_graph.h"
#include <algorithm>
#include <cmath>
@ -39,20 +39,19 @@
namespace cartographer {
namespace mapping_2d {
SparsePoseGraph::SparsePoseGraph(
const mapping::proto::SparsePoseGraphOptions& options,
PoseGraph::PoseGraph(const mapping::proto::SparsePoseGraphOptions& options,
common::ThreadPool* thread_pool)
: options_(options),
optimization_problem_(options_.optimization_problem_options()),
constraint_builder_(options_.constraint_builder_options(), thread_pool) {}
SparsePoseGraph::~SparsePoseGraph() {
PoseGraph::~PoseGraph() {
WaitForAllComputations();
common::MutexLocker locker(&mutex_);
CHECK(work_queue_ == nullptr);
}
std::vector<mapping::SubmapId> SparsePoseGraph::InitializeGlobalSubmapPoses(
std::vector<mapping::SubmapId> PoseGraph::InitializeGlobalSubmapPoses(
const int trajectory_id, const common::Time time,
const std::vector<std::shared_ptr<const Submap>>& insertion_submaps) {
CHECK(!insertion_submaps.empty());
@ -99,7 +98,7 @@ std::vector<mapping::SubmapId> SparsePoseGraph::InitializeGlobalSubmapPoses(
return {front_submap_id, last_submap_id};
}
mapping::NodeId SparsePoseGraph::AddNode(
mapping::NodeId PoseGraph::AddNode(
std::shared_ptr<const mapping::TrajectoryNode::Data> constant_data,
const int trajectory_id,
const std::vector<std::shared_ptr<const Submap>>& insertion_submaps) {
@ -133,7 +132,7 @@ mapping::NodeId SparsePoseGraph::AddNode(
return node_id;
}
void SparsePoseGraph::AddWorkItem(const std::function<void()>& work_item) {
void PoseGraph::AddWorkItem(const std::function<void()>& work_item) {
if (work_queue_ == nullptr) {
work_item();
} else {
@ -141,7 +140,7 @@ void SparsePoseGraph::AddWorkItem(const std::function<void()>& work_item) {
}
}
void SparsePoseGraph::AddTrajectoryIfNeeded(const int trajectory_id) {
void PoseGraph::AddTrajectoryIfNeeded(const int trajectory_id) {
trajectory_connectivity_state_.Add(trajectory_id);
// Make sure we have a sampler for this trajectory.
if (!global_localization_samplers_[trajectory_id]) {
@ -151,7 +150,7 @@ void SparsePoseGraph::AddTrajectoryIfNeeded(const int trajectory_id) {
}
}
void SparsePoseGraph::AddImuData(const int trajectory_id,
void PoseGraph::AddImuData(const int trajectory_id,
const sensor::ImuData& imu_data) {
common::MutexLocker locker(&mutex_);
AddWorkItem([=]() REQUIRES(mutex_) {
@ -159,21 +158,21 @@ void SparsePoseGraph::AddImuData(const int trajectory_id,
});
}
void SparsePoseGraph::AddOdometryData(
const int trajectory_id, const sensor::OdometryData& odometry_data) {
void PoseGraph::AddOdometryData(const int trajectory_id,
const sensor::OdometryData& odometry_data) {
common::MutexLocker locker(&mutex_);
AddWorkItem([=]() REQUIRES(mutex_) {
optimization_problem_.AddOdometryData(trajectory_id, odometry_data);
});
}
void SparsePoseGraph::AddFixedFramePoseData(
void PoseGraph::AddFixedFramePoseData(
const int trajectory_id,
const sensor::FixedFramePoseData& fixed_frame_pose_data) {
LOG(FATAL) << "Not yet implemented for 2D.";
}
void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
void PoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
const mapping::SubmapId& submap_id) {
CHECK(submap_data_.at(submap_id).state == SubmapState::kFinished);
@ -206,7 +205,7 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
}
}
void SparsePoseGraph::ComputeConstraintsForOldNodes(
void PoseGraph::ComputeConstraintsForOldNodes(
const mapping::SubmapId& submap_id) {
const auto& submap_data = submap_data_.at(submap_id);
for (const auto& node_id_data : optimization_problem_.node_data()) {
@ -217,7 +216,7 @@ void SparsePoseGraph::ComputeConstraintsForOldNodes(
}
}
void SparsePoseGraph::ComputeConstraintsForNode(
void PoseGraph::ComputeConstraintsForNode(
const mapping::NodeId& node_id,
std::vector<std::shared_ptr<const Submap>> insertion_submaps,
const bool newly_finished_submap) {
@ -282,7 +281,7 @@ void SparsePoseGraph::ComputeConstraintsForNode(
}
}
common::Time SparsePoseGraph::GetLatestNodeTime(
common::Time PoseGraph::GetLatestNodeTime(
const mapping::NodeId& node_id, const mapping::SubmapId& submap_id) const {
common::Time time = trajectory_nodes_.at(node_id).constant_data->time;
const SubmapData& submap_data = submap_data_.at(submap_id);
@ -295,8 +294,7 @@ common::Time SparsePoseGraph::GetLatestNodeTime(
return time;
}
void SparsePoseGraph::UpdateTrajectoryConnectivity(
const Constraint& constraint) {
void PoseGraph::UpdateTrajectoryConnectivity(const Constraint& constraint) {
CHECK_EQ(constraint.tag, mapping::PoseGraph::Constraint::INTER_SUBMAP);
const common::Time time =
GetLatestNodeTime(constraint.node_id, constraint.submap_id);
@ -305,7 +303,7 @@ void SparsePoseGraph::UpdateTrajectoryConnectivity(
time);
}
void SparsePoseGraph::HandleWorkQueue() {
void PoseGraph::HandleWorkQueue() {
constraint_builder_.WhenDone(
[this](const pose_graph::ConstraintBuilder::Result& result) {
{
@ -339,7 +337,7 @@ void SparsePoseGraph::HandleWorkQueue() {
});
}
void SparsePoseGraph::WaitForAllComputations() {
void PoseGraph::WaitForAllComputations() {
bool notification = false;
common::MutexLocker locker(&mutex_);
const int num_finished_nodes_at_start =
@ -370,12 +368,12 @@ void SparsePoseGraph::WaitForAllComputations() {
locker.Await([&notification]() { return notification; });
}
void SparsePoseGraph::FinishTrajectory(const int trajectory_id) {
void PoseGraph::FinishTrajectory(const int trajectory_id) {
// TODO(jihoonl): Add a logic to notify trimmers to finish the given
// trajectory.
}
void SparsePoseGraph::FreezeTrajectory(const int trajectory_id) {
void PoseGraph::FreezeTrajectory(const int trajectory_id) {
common::MutexLocker locker(&mutex_);
trajectory_connectivity_state_.Add(trajectory_id);
AddWorkItem([this, trajectory_id]() REQUIRES(mutex_) {
@ -384,8 +382,7 @@ void SparsePoseGraph::FreezeTrajectory(const int trajectory_id) {
});
}
void SparsePoseGraph::AddSubmapFromProto(
const transform::Rigid3d& global_submap_pose,
void PoseGraph::AddSubmapFromProto(const transform::Rigid3d& global_submap_pose,
const mapping::proto::Submap& submap) {
if (!submap.has_submap_2d()) {
return;
@ -412,7 +409,7 @@ void SparsePoseGraph::AddSubmapFromProto(
});
}
void SparsePoseGraph::AddNodeFromProto(const transform::Rigid3d& global_pose,
void PoseGraph::AddNodeFromProto(const transform::Rigid3d& global_pose,
const mapping::proto::Node& node) {
const mapping::NodeId node_id = {node.node_id().trajectory_id(),
node.node_id().node_index()};
@ -439,7 +436,7 @@ void SparsePoseGraph::AddNodeFromProto(const transform::Rigid3d& global_pose,
});
}
void SparsePoseGraph::AddNodeToSubmap(const mapping::NodeId& node_id,
void PoseGraph::AddNodeToSubmap(const mapping::NodeId& node_id,
const mapping::SubmapId& submap_id) {
common::MutexLocker locker(&mutex_);
AddWorkItem([this, node_id, submap_id]() REQUIRES(mutex_) {
@ -447,7 +444,7 @@ void SparsePoseGraph::AddNodeToSubmap(const mapping::NodeId& node_id,
});
}
void SparsePoseGraph::AddSerializedConstraints(
void PoseGraph::AddSerializedConstraints(
const std::vector<Constraint>& constraints) {
common::MutexLocker locker(&mutex_);
AddWorkItem([this, constraints]() REQUIRES(mutex_) {
@ -479,8 +476,7 @@ void SparsePoseGraph::AddSerializedConstraints(
});
}
void SparsePoseGraph::AddTrimmer(
std::unique_ptr<mapping::PoseGraphTrimmer> trimmer) {
void PoseGraph::AddTrimmer(std::unique_ptr<mapping::PoseGraphTrimmer> trimmer) {
common::MutexLocker locker(&mutex_);
// C++11 does not allow us to move a unique_ptr into a lambda.
mapping::PoseGraphTrimmer* const trimmer_ptr = trimmer.release();
@ -488,7 +484,7 @@ void SparsePoseGraph::AddTrimmer(
REQUIRES(mutex_) { trimmers_.emplace_back(trimmer_ptr); });
}
void SparsePoseGraph::RunFinalOptimization() {
void PoseGraph::RunFinalOptimization() {
WaitForAllComputations();
optimization_problem_.SetMaxNumIterations(
options_.max_num_final_iterations());
@ -499,7 +495,7 @@ void SparsePoseGraph::RunFinalOptimization() {
.max_num_iterations());
}
void SparsePoseGraph::RunOptimization() {
void PoseGraph::RunOptimization() {
if (optimization_problem_.submap_data().empty()) {
return;
}
@ -544,22 +540,22 @@ void SparsePoseGraph::RunOptimization() {
}
mapping::MapById<mapping::NodeId, mapping::TrajectoryNode>
SparsePoseGraph::GetTrajectoryNodes() {
PoseGraph::GetTrajectoryNodes() {
common::MutexLocker locker(&mutex_);
return trajectory_nodes_;
}
sensor::MapByTime<sensor::ImuData> SparsePoseGraph::GetImuData() {
sensor::MapByTime<sensor::ImuData> PoseGraph::GetImuData() {
common::MutexLocker locker(&mutex_);
return optimization_problem_.imu_data();
}
sensor::MapByTime<sensor::OdometryData> SparsePoseGraph::GetOdometryData() {
sensor::MapByTime<sensor::OdometryData> PoseGraph::GetOdometryData() {
common::MutexLocker locker(&mutex_);
return optimization_problem_.odometry_data();
}
std::vector<SparsePoseGraph::Constraint> SparsePoseGraph::constraints() {
std::vector<PoseGraph::Constraint> PoseGraph::constraints() {
std::vector<Constraint> result;
common::MutexLocker locker(&mutex_);
for (const Constraint& constraint : constraints_) {
@ -576,7 +572,7 @@ std::vector<SparsePoseGraph::Constraint> SparsePoseGraph::constraints() {
return result;
}
void SparsePoseGraph::SetInitialTrajectoryPose(const int from_trajectory_id,
void PoseGraph::SetInitialTrajectoryPose(const int from_trajectory_id,
const int to_trajectory_id,
const transform::Rigid3d& pose,
const common::Time time) {
@ -585,7 +581,7 @@ void SparsePoseGraph::SetInitialTrajectoryPose(const int from_trajectory_id,
InitialTrajectoryPose{to_trajectory_id, pose, time};
}
transform::Rigid3d SparsePoseGraph::GetInterpolatedGlobalTrajectoryPose(
transform::Rigid3d PoseGraph::GetInterpolatedGlobalTrajectoryPose(
const int trajectory_id, const common::Time time) const {
CHECK(trajectory_nodes_.SizeOfTrajectoryOrZero(trajectory_id) > 0);
const auto it = trajectory_nodes_.lower_bound(trajectory_id, time);
@ -605,25 +601,25 @@ transform::Rigid3d SparsePoseGraph::GetInterpolatedGlobalTrajectoryPose(
.transform;
}
transform::Rigid3d SparsePoseGraph::GetLocalToGlobalTransform(
transform::Rigid3d PoseGraph::GetLocalToGlobalTransform(
const int trajectory_id) {
common::MutexLocker locker(&mutex_);
return ComputeLocalToGlobalTransform(optimized_submap_transforms_,
trajectory_id);
}
std::vector<std::vector<int>> SparsePoseGraph::GetConnectedTrajectories() {
std::vector<std::vector<int>> PoseGraph::GetConnectedTrajectories() {
return trajectory_connectivity_state_.Components();
}
mapping::PoseGraph::SubmapData SparsePoseGraph::GetSubmapData(
mapping::PoseGraph::SubmapData PoseGraph::GetSubmapData(
const mapping::SubmapId& submap_id) {
common::MutexLocker locker(&mutex_);
return GetSubmapDataUnderLock(submap_id);
}
mapping::MapById<mapping::SubmapId, mapping::PoseGraph::SubmapData>
SparsePoseGraph::GetAllSubmapData() {
PoseGraph::GetAllSubmapData() {
common::MutexLocker locker(&mutex_);
mapping::MapById<mapping::SubmapId, mapping::PoseGraph::SubmapData> submaps;
for (const auto& submap_id_data : submap_data_) {
@ -633,7 +629,7 @@ SparsePoseGraph::GetAllSubmapData() {
return submaps;
}
transform::Rigid3d SparsePoseGraph::ComputeLocalToGlobalTransform(
transform::Rigid3d PoseGraph::ComputeLocalToGlobalTransform(
const mapping::MapById<mapping::SubmapId, pose_graph::SubmapData>&
submap_transforms,
const int trajectory_id) const {
@ -658,7 +654,7 @@ transform::Rigid3d SparsePoseGraph::ComputeLocalToGlobalTransform(
.inverse();
}
mapping::PoseGraph::SubmapData SparsePoseGraph::GetSubmapDataUnderLock(
mapping::PoseGraph::SubmapData PoseGraph::GetSubmapDataUnderLock(
const mapping::SubmapId& submap_id) {
const auto it = submap_data_.find(submap_id);
if (it == submap_data_.end()) {
@ -677,16 +673,15 @@ mapping::PoseGraph::SubmapData SparsePoseGraph::GetSubmapDataUnderLock(
submap->local_pose()};
}
SparsePoseGraph::TrimmingHandle::TrimmingHandle(SparsePoseGraph* const parent)
PoseGraph::TrimmingHandle::TrimmingHandle(PoseGraph* const parent)
: parent_(parent) {}
int SparsePoseGraph::TrimmingHandle::num_submaps(
const int trajectory_id) const {
int PoseGraph::TrimmingHandle::num_submaps(const int trajectory_id) const {
const auto& submap_data = parent_->optimization_problem_.submap_data();
return submap_data.SizeOfTrajectoryOrZero(trajectory_id);
}
void SparsePoseGraph::TrimmingHandle::MarkSubmapAsTrimmed(
void PoseGraph::TrimmingHandle::MarkSubmapAsTrimmed(
const mapping::SubmapId& submap_id) {
// TODO(hrapp): We have to make sure that the trajectory has been finished
// if we want to delete the last submaps.

View File

@ -14,8 +14,8 @@
* limitations under the License.
*/
#ifndef CARTOGRAPHER_MAPPING_2D_SPARSE_POSE_GRAPH_H_
#define CARTOGRAPHER_MAPPING_2D_SPARSE_POSE_GRAPH_H_
#ifndef CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_H_
#define CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_H_
#include <deque>
#include <functional>
@ -56,14 +56,14 @@ 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::PoseGraph {
class PoseGraph : public mapping::PoseGraph {
public:
SparsePoseGraph(const mapping::proto::SparsePoseGraphOptions& options,
PoseGraph(const mapping::proto::SparsePoseGraphOptions& options,
common::ThreadPool* thread_pool);
~SparsePoseGraph() override;
~PoseGraph() override;
SparsePoseGraph(const SparsePoseGraph&) = delete;
SparsePoseGraph& operator=(const SparsePoseGraph&) = delete;
PoseGraph(const PoseGraph&) = delete;
PoseGraph& operator=(const PoseGraph&) = delete;
// Adds a new node with 'constant_data'. Its 'constant_data->local_pose' was
// determined by scan matching against 'insertion_submaps.front()' and the
@ -245,7 +245,7 @@ class SparsePoseGraph : public mapping::PoseGraph {
// 'mutex_' of the pose graph is held while this class is used.
class TrimmingHandle : public mapping::Trimmable {
public:
TrimmingHandle(SparsePoseGraph* parent);
TrimmingHandle(PoseGraph* parent);
~TrimmingHandle() override {}
int num_submaps(int trajectory_id) const override;
@ -253,11 +253,11 @@ class SparsePoseGraph : public mapping::PoseGraph {
REQUIRES(parent_->mutex_) override;
private:
SparsePoseGraph* const parent_;
PoseGraph* const parent_;
};
};
} // namespace mapping_2d
} // namespace cartographer
#endif // CARTOGRAPHER_MAPPING_2D_SPARSE_POSE_GRAPH_H_
#endif // CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_H_

View File

@ -14,7 +14,7 @@
* limitations under the License.
*/
#include "cartographer/mapping_2d/sparse_pose_graph.h"
#include "cartographer/mapping_2d/pose_graph.h"
#include <cmath>
#include <memory>
@ -35,9 +35,9 @@ namespace cartographer {
namespace mapping_2d {
namespace {
class SparsePoseGraphTest : public ::testing::Test {
class PoseGraphTest : public ::testing::Test {
protected:
SparsePoseGraphTest() : thread_pool_(1) {
PoseGraphTest() : thread_pool_(1) {
// Builds a wavy, irregularly circular point cloud that is unique
// rotationally. This gives us good rotational texture and avoids any
// possibility of the CeresScanMatcher preferring free space (>
@ -132,7 +132,7 @@ class SparsePoseGraphTest : public ::testing::Test {
log_residual_histograms = true,
global_constraint_search_after_n_seconds = 10.0,
})text");
sparse_pose_graph_ = common::make_unique<SparsePoseGraph>(
pose_graph_ = common::make_unique<PoseGraph>(
mapping::CreateSparsePoseGraphOptions(parameter_dictionary.get()),
&thread_pool_);
}
@ -157,7 +157,7 @@ class SparsePoseGraphTest : public ::testing::Test {
active_submaps_->InsertRangeData(TransformRangeData(
range_data, transform::Embed3D(pose_estimate.cast<float>())));
sparse_pose_graph_->AddNode(
pose_graph_->AddNode(
std::make_shared<const mapping::TrajectoryNode::Data>(
mapping::TrajectoryNode::Data{common::FromUniversal(0),
Eigen::Quaterniond::Identity(),
@ -181,22 +181,22 @@ class SparsePoseGraphTest : public ::testing::Test {
sensor::PointCloud point_cloud_;
std::unique_ptr<ActiveSubmaps> active_submaps_;
common::ThreadPool thread_pool_;
std::unique_ptr<SparsePoseGraph> sparse_pose_graph_;
std::unique_ptr<PoseGraph> pose_graph_;
transform::Rigid2d current_pose_;
};
TEST_F(SparsePoseGraphTest, EmptyMap) {
sparse_pose_graph_->RunFinalOptimization();
const auto nodes = sparse_pose_graph_->GetTrajectoryNodes();
TEST_F(PoseGraphTest, EmptyMap) {
pose_graph_->RunFinalOptimization();
const auto nodes = pose_graph_->GetTrajectoryNodes();
EXPECT_TRUE(nodes.empty());
}
TEST_F(SparsePoseGraphTest, NoMovement) {
TEST_F(PoseGraphTest, NoMovement) {
MoveRelative(transform::Rigid2d::Identity());
MoveRelative(transform::Rigid2d::Identity());
MoveRelative(transform::Rigid2d::Identity());
sparse_pose_graph_->RunFinalOptimization();
const auto nodes = sparse_pose_graph_->GetTrajectoryNodes();
pose_graph_->RunFinalOptimization();
const auto nodes = pose_graph_->GetTrajectoryNodes();
ASSERT_THAT(ToVectorInt(nodes.trajectory_ids()),
::testing::ContainerEq(std::vector<int>{0}));
EXPECT_THAT(nodes.SizeOfTrajectoryOrZero(0), ::testing::Eq(3u));
@ -208,7 +208,7 @@ TEST_F(SparsePoseGraphTest, NoMovement) {
transform::IsNearly(transform::Rigid3d::Identity(), 1e-2));
}
TEST_F(SparsePoseGraphTest, NoOverlappingNodes) {
TEST_F(PoseGraphTest, NoOverlappingNodes) {
std::mt19937 rng(0);
std::uniform_real_distribution<double> distribution(-1., 1.);
std::vector<transform::Rigid2d> poses;
@ -216,8 +216,8 @@ TEST_F(SparsePoseGraphTest, NoOverlappingNodes) {
MoveRelative(transform::Rigid2d({0.25 * distribution(rng), 5.}, 0.));
poses.emplace_back(current_pose_);
}
sparse_pose_graph_->RunFinalOptimization();
const auto nodes = sparse_pose_graph_->GetTrajectoryNodes();
pose_graph_->RunFinalOptimization();
const auto nodes = pose_graph_->GetTrajectoryNodes();
ASSERT_THAT(ToVectorInt(nodes.trajectory_ids()),
::testing::ContainerEq(std::vector<int>{0}));
for (int i = 0; i != 4; ++i) {
@ -229,7 +229,7 @@ TEST_F(SparsePoseGraphTest, NoOverlappingNodes) {
}
}
TEST_F(SparsePoseGraphTest, ConsecutivelyOverlappingNodes) {
TEST_F(PoseGraphTest, ConsecutivelyOverlappingNodes) {
std::mt19937 rng(0);
std::uniform_real_distribution<double> distribution(-1., 1.);
std::vector<transform::Rigid2d> poses;
@ -237,8 +237,8 @@ TEST_F(SparsePoseGraphTest, ConsecutivelyOverlappingNodes) {
MoveRelative(transform::Rigid2d({0.25 * distribution(rng), 2.}, 0.));
poses.emplace_back(current_pose_);
}
sparse_pose_graph_->RunFinalOptimization();
const auto nodes = sparse_pose_graph_->GetTrajectoryNodes();
pose_graph_->RunFinalOptimization();
const auto nodes = pose_graph_->GetTrajectoryNodes();
ASSERT_THAT(ToVectorInt(nodes.trajectory_ids()),
::testing::ContainerEq(std::vector<int>{0}));
for (int i = 0; i != 5; ++i) {
@ -250,7 +250,7 @@ TEST_F(SparsePoseGraphTest, ConsecutivelyOverlappingNodes) {
}
}
TEST_F(SparsePoseGraphTest, OverlappingNodes) {
TEST_F(PoseGraphTest, OverlappingNodes) {
std::mt19937 rng(0);
std::uniform_real_distribution<double> distribution(-1., 1.);
std::vector<transform::Rigid2d> ground_truth;
@ -265,8 +265,8 @@ TEST_F(SparsePoseGraphTest, OverlappingNodes) {
ground_truth.emplace_back(current_pose_);
poses.emplace_back(noise * current_pose_);
}
sparse_pose_graph_->RunFinalOptimization();
const auto nodes = sparse_pose_graph_->GetTrajectoryNodes();
pose_graph_->RunFinalOptimization();
const auto nodes = pose_graph_->GetTrajectoryNodes();
ASSERT_THAT(ToVectorInt(nodes.trajectory_ids()),
::testing::ContainerEq(std::vector<int>{0}));
transform::Rigid2d true_movement =

View File

@ -14,7 +14,7 @@
* limitations under the License.
*/
#include "cartographer/mapping_3d/sparse_pose_graph.h"
#include "cartographer/mapping_3d/pose_graph.h"
#include <algorithm>
#include <cmath>
@ -38,21 +38,20 @@
namespace cartographer {
namespace mapping_3d {
SparsePoseGraph::SparsePoseGraph(
const mapping::proto::SparsePoseGraphOptions& options,
PoseGraph::PoseGraph(const mapping::proto::SparsePoseGraphOptions& options,
common::ThreadPool* thread_pool)
: options_(options),
optimization_problem_(options_.optimization_problem_options(),
pose_graph::OptimizationProblem::FixZ::kNo),
constraint_builder_(options_.constraint_builder_options(), thread_pool) {}
SparsePoseGraph::~SparsePoseGraph() {
PoseGraph::~PoseGraph() {
WaitForAllComputations();
common::MutexLocker locker(&mutex_);
CHECK(work_queue_ == nullptr);
}
std::vector<mapping::SubmapId> SparsePoseGraph::InitializeGlobalSubmapPoses(
std::vector<mapping::SubmapId> PoseGraph::InitializeGlobalSubmapPoses(
const int trajectory_id, const common::Time time,
const std::vector<std::shared_ptr<const Submap>>& insertion_submaps) {
CHECK(!insertion_submaps.empty());
@ -97,7 +96,7 @@ std::vector<mapping::SubmapId> SparsePoseGraph::InitializeGlobalSubmapPoses(
return {front_submap_id, last_submap_id};
}
mapping::NodeId SparsePoseGraph::AddNode(
mapping::NodeId PoseGraph::AddNode(
std::shared_ptr<const mapping::TrajectoryNode::Data> constant_data,
const int trajectory_id,
const std::vector<std::shared_ptr<const Submap>>& insertion_submaps) {
@ -131,7 +130,7 @@ mapping::NodeId SparsePoseGraph::AddNode(
return node_id;
}
void SparsePoseGraph::AddWorkItem(const std::function<void()>& work_item) {
void PoseGraph::AddWorkItem(const std::function<void()>& work_item) {
if (work_queue_ == nullptr) {
work_item();
} else {
@ -139,7 +138,7 @@ void SparsePoseGraph::AddWorkItem(const std::function<void()>& work_item) {
}
}
void SparsePoseGraph::AddTrajectoryIfNeeded(const int trajectory_id) {
void PoseGraph::AddTrajectoryIfNeeded(const int trajectory_id) {
trajectory_connectivity_state_.Add(trajectory_id);
// Make sure we have a sampler for this trajectory.
if (!global_localization_samplers_[trajectory_id]) {
@ -149,7 +148,7 @@ void SparsePoseGraph::AddTrajectoryIfNeeded(const int trajectory_id) {
}
}
void SparsePoseGraph::AddImuData(const int trajectory_id,
void PoseGraph::AddImuData(const int trajectory_id,
const sensor::ImuData& imu_data) {
common::MutexLocker locker(&mutex_);
AddWorkItem([=]() REQUIRES(mutex_) {
@ -157,15 +156,15 @@ void SparsePoseGraph::AddImuData(const int trajectory_id,
});
}
void SparsePoseGraph::AddOdometryData(
const int trajectory_id, const sensor::OdometryData& odometry_data) {
void PoseGraph::AddOdometryData(const int trajectory_id,
const sensor::OdometryData& odometry_data) {
common::MutexLocker locker(&mutex_);
AddWorkItem([=]() REQUIRES(mutex_) {
optimization_problem_.AddOdometryData(trajectory_id, odometry_data);
});
}
void SparsePoseGraph::AddFixedFramePoseData(
void PoseGraph::AddFixedFramePoseData(
const int trajectory_id,
const sensor::FixedFramePoseData& fixed_frame_pose_data) {
common::MutexLocker locker(&mutex_);
@ -175,7 +174,7 @@ void SparsePoseGraph::AddFixedFramePoseData(
});
}
void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
void PoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
const mapping::SubmapId& submap_id) {
CHECK(submap_data_.at(submap_id).state == SubmapState::kFinished);
@ -227,7 +226,7 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
}
}
void SparsePoseGraph::ComputeConstraintsForOldNodes(
void PoseGraph::ComputeConstraintsForOldNodes(
const mapping::SubmapId& submap_id) {
const auto& submap_data = submap_data_.at(submap_id);
for (const auto& node_id_data : optimization_problem_.node_data()) {
@ -238,7 +237,7 @@ void SparsePoseGraph::ComputeConstraintsForOldNodes(
}
}
void SparsePoseGraph::ComputeConstraintsForNode(
void PoseGraph::ComputeConstraintsForNode(
const mapping::NodeId& node_id,
std::vector<std::shared_ptr<const Submap>> insertion_submaps,
const bool newly_finished_submap) {
@ -299,7 +298,7 @@ void SparsePoseGraph::ComputeConstraintsForNode(
}
}
common::Time SparsePoseGraph::GetLatestNodeTime(
common::Time PoseGraph::GetLatestNodeTime(
const mapping::NodeId& node_id, const mapping::SubmapId& submap_id) const {
common::Time time = trajectory_nodes_.at(node_id).constant_data->time;
const SubmapData& submap_data = submap_data_.at(submap_id);
@ -312,8 +311,7 @@ common::Time SparsePoseGraph::GetLatestNodeTime(
return time;
}
void SparsePoseGraph::UpdateTrajectoryConnectivity(
const Constraint& constraint) {
void PoseGraph::UpdateTrajectoryConnectivity(const Constraint& constraint) {
CHECK_EQ(constraint.tag, mapping::PoseGraph::Constraint::INTER_SUBMAP);
const common::Time time =
GetLatestNodeTime(constraint.node_id, constraint.submap_id);
@ -322,7 +320,7 @@ void SparsePoseGraph::UpdateTrajectoryConnectivity(
time);
}
void SparsePoseGraph::HandleWorkQueue() {
void PoseGraph::HandleWorkQueue() {
constraint_builder_.WhenDone(
[this](const pose_graph::ConstraintBuilder::Result& result) {
{
@ -356,7 +354,7 @@ void SparsePoseGraph::HandleWorkQueue() {
});
}
void SparsePoseGraph::WaitForAllComputations() {
void PoseGraph::WaitForAllComputations() {
bool notification = false;
common::MutexLocker locker(&mutex_);
const int num_finished_nodes_at_start =
@ -387,12 +385,12 @@ void SparsePoseGraph::WaitForAllComputations() {
locker.Await([&notification]() { return notification; });
}
void SparsePoseGraph::FinishTrajectory(const int trajectory_id) {
void PoseGraph::FinishTrajectory(const int trajectory_id) {
// TODO(jihoonl): Add a logic to notify trimmers to finish the given
// trajectory.
}
void SparsePoseGraph::FreezeTrajectory(const int trajectory_id) {
void PoseGraph::FreezeTrajectory(const int trajectory_id) {
common::MutexLocker locker(&mutex_);
trajectory_connectivity_state_.Add(trajectory_id);
AddWorkItem([this, trajectory_id]() REQUIRES(mutex_) {
@ -401,8 +399,7 @@ void SparsePoseGraph::FreezeTrajectory(const int trajectory_id) {
});
}
void SparsePoseGraph::AddSubmapFromProto(
const transform::Rigid3d& global_submap_pose,
void PoseGraph::AddSubmapFromProto(const transform::Rigid3d& global_submap_pose,
const mapping::proto::Submap& submap) {
if (!submap.has_submap_3d()) {
return;
@ -427,7 +424,7 @@ void SparsePoseGraph::AddSubmapFromProto(
});
}
void SparsePoseGraph::AddNodeFromProto(const transform::Rigid3d& global_pose,
void PoseGraph::AddNodeFromProto(const transform::Rigid3d& global_pose,
const mapping::proto::Node& node) {
const mapping::NodeId node_id = {node.node_id().trajectory_id(),
node.node_id().node_index()};
@ -448,7 +445,7 @@ void SparsePoseGraph::AddNodeFromProto(const transform::Rigid3d& global_pose,
});
}
void SparsePoseGraph::AddNodeToSubmap(const mapping::NodeId& node_id,
void PoseGraph::AddNodeToSubmap(const mapping::NodeId& node_id,
const mapping::SubmapId& submap_id) {
common::MutexLocker locker(&mutex_);
AddWorkItem([this, node_id, submap_id]() REQUIRES(mutex_) {
@ -456,7 +453,7 @@ void SparsePoseGraph::AddNodeToSubmap(const mapping::NodeId& node_id,
});
}
void SparsePoseGraph::AddSerializedConstraints(
void PoseGraph::AddSerializedConstraints(
const std::vector<Constraint>& constraints) {
common::MutexLocker locker(&mutex_);
AddWorkItem([this, constraints]() REQUIRES(mutex_) {
@ -481,8 +478,7 @@ void SparsePoseGraph::AddSerializedConstraints(
});
}
void SparsePoseGraph::AddTrimmer(
std::unique_ptr<mapping::PoseGraphTrimmer> trimmer) {
void PoseGraph::AddTrimmer(std::unique_ptr<mapping::PoseGraphTrimmer> trimmer) {
common::MutexLocker locker(&mutex_);
// C++11 does not allow us to move a unique_ptr into a lambda.
mapping::PoseGraphTrimmer* const trimmer_ptr = trimmer.release();
@ -490,7 +486,7 @@ void SparsePoseGraph::AddTrimmer(
REQUIRES(mutex_) { trimmers_.emplace_back(trimmer_ptr); });
}
void SparsePoseGraph::RunFinalOptimization() {
void PoseGraph::RunFinalOptimization() {
WaitForAllComputations();
optimization_problem_.SetMaxNumIterations(
options_.max_num_final_iterations());
@ -501,7 +497,7 @@ void SparsePoseGraph::RunFinalOptimization() {
.max_num_iterations());
}
void SparsePoseGraph::LogResidualHistograms() {
void PoseGraph::LogResidualHistograms() {
common::Histogram rotational_residual;
common::Histogram translational_residual;
for (const Constraint& constraint : constraints_) {
@ -527,7 +523,7 @@ void SparsePoseGraph::LogResidualHistograms() {
<< rotational_residual.ToString(10);
}
void SparsePoseGraph::RunOptimization() {
void PoseGraph::RunOptimization() {
if (optimization_problem_.submap_data().empty()) {
return;
}
@ -573,27 +569,27 @@ void SparsePoseGraph::RunOptimization() {
}
mapping::MapById<mapping::NodeId, mapping::TrajectoryNode>
SparsePoseGraph::GetTrajectoryNodes() {
PoseGraph::GetTrajectoryNodes() {
common::MutexLocker locker(&mutex_);
return trajectory_nodes_;
}
sensor::MapByTime<sensor::ImuData> SparsePoseGraph::GetImuData() {
sensor::MapByTime<sensor::ImuData> PoseGraph::GetImuData() {
common::MutexLocker locker(&mutex_);
return optimization_problem_.imu_data();
}
sensor::MapByTime<sensor::OdometryData> SparsePoseGraph::GetOdometryData() {
sensor::MapByTime<sensor::OdometryData> PoseGraph::GetOdometryData() {
common::MutexLocker locker(&mutex_);
return optimization_problem_.odometry_data();
}
std::vector<SparsePoseGraph::Constraint> SparsePoseGraph::constraints() {
std::vector<PoseGraph::Constraint> PoseGraph::constraints() {
common::MutexLocker locker(&mutex_);
return constraints_;
}
void SparsePoseGraph::SetInitialTrajectoryPose(const int from_trajectory_id,
void PoseGraph::SetInitialTrajectoryPose(const int from_trajectory_id,
const int to_trajectory_id,
const transform::Rigid3d& pose,
const common::Time time) {
@ -602,7 +598,7 @@ void SparsePoseGraph::SetInitialTrajectoryPose(const int from_trajectory_id,
InitialTrajectoryPose{to_trajectory_id, pose, time};
}
transform::Rigid3d SparsePoseGraph::GetInterpolatedGlobalTrajectoryPose(
transform::Rigid3d PoseGraph::GetInterpolatedGlobalTrajectoryPose(
const int trajectory_id, const common::Time time) const {
CHECK(trajectory_nodes_.SizeOfTrajectoryOrZero(trajectory_id) > 0);
const auto it = trajectory_nodes_.lower_bound(trajectory_id, time);
@ -622,25 +618,25 @@ transform::Rigid3d SparsePoseGraph::GetInterpolatedGlobalTrajectoryPose(
.transform;
}
transform::Rigid3d SparsePoseGraph::GetLocalToGlobalTransform(
transform::Rigid3d PoseGraph::GetLocalToGlobalTransform(
const int trajectory_id) {
common::MutexLocker locker(&mutex_);
return ComputeLocalToGlobalTransform(optimized_submap_transforms_,
trajectory_id);
}
std::vector<std::vector<int>> SparsePoseGraph::GetConnectedTrajectories() {
std::vector<std::vector<int>> PoseGraph::GetConnectedTrajectories() {
return trajectory_connectivity_state_.Components();
}
mapping::PoseGraph::SubmapData SparsePoseGraph::GetSubmapData(
mapping::PoseGraph::SubmapData PoseGraph::GetSubmapData(
const mapping::SubmapId& submap_id) {
common::MutexLocker locker(&mutex_);
return GetSubmapDataUnderLock(submap_id);
}
mapping::MapById<mapping::SubmapId, mapping::PoseGraph::SubmapData>
SparsePoseGraph::GetAllSubmapData() {
PoseGraph::GetAllSubmapData() {
common::MutexLocker locker(&mutex_);
mapping::MapById<mapping::SubmapId, mapping::PoseGraph::SubmapData> submaps;
for (const auto& submap_id_data : submap_data_) {
@ -650,7 +646,7 @@ SparsePoseGraph::GetAllSubmapData() {
return submaps;
}
transform::Rigid3d SparsePoseGraph::ComputeLocalToGlobalTransform(
transform::Rigid3d PoseGraph::ComputeLocalToGlobalTransform(
const mapping::MapById<mapping::SubmapId, pose_graph::SubmapData>&
submap_transforms,
const int trajectory_id) const {
@ -674,7 +670,7 @@ transform::Rigid3d SparsePoseGraph::ComputeLocalToGlobalTransform(
.inverse();
}
mapping::PoseGraph::SubmapData SparsePoseGraph::GetSubmapDataUnderLock(
mapping::PoseGraph::SubmapData PoseGraph::GetSubmapDataUnderLock(
const mapping::SubmapId& submap_id) {
const auto it = submap_data_.find(submap_id);
if (it == submap_data_.end()) {
@ -691,16 +687,15 @@ mapping::PoseGraph::SubmapData SparsePoseGraph::GetSubmapDataUnderLock(
submap->local_pose()};
}
SparsePoseGraph::TrimmingHandle::TrimmingHandle(SparsePoseGraph* const parent)
PoseGraph::TrimmingHandle::TrimmingHandle(PoseGraph* const parent)
: parent_(parent) {}
int SparsePoseGraph::TrimmingHandle::num_submaps(
const int trajectory_id) const {
int PoseGraph::TrimmingHandle::num_submaps(const int trajectory_id) const {
const auto& submap_data = parent_->optimization_problem_.submap_data();
return submap_data.SizeOfTrajectoryOrZero(trajectory_id);
}
void SparsePoseGraph::TrimmingHandle::MarkSubmapAsTrimmed(
void PoseGraph::TrimmingHandle::MarkSubmapAsTrimmed(
const mapping::SubmapId& submap_id) {
// TODO(hrapp): We have to make sure that the trajectory has been finished
// if we want to delete the last submaps.

View File

@ -14,8 +14,8 @@
* limitations under the License.
*/
#ifndef CARTOGRAPHER_MAPPING_3D_SPARSE_POSE_GRAPH_H_
#define CARTOGRAPHER_MAPPING_3D_SPARSE_POSE_GRAPH_H_
#ifndef CARTOGRAPHER_MAPPING_3D_POSE_GRAPH_H_
#define CARTOGRAPHER_MAPPING_3D_POSE_GRAPH_H_
#include <deque>
#include <functional>
@ -56,14 +56,14 @@ 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::PoseGraph {
class PoseGraph : public mapping::PoseGraph {
public:
SparsePoseGraph(const mapping::proto::SparsePoseGraphOptions& options,
PoseGraph(const mapping::proto::SparsePoseGraphOptions& options,
common::ThreadPool* thread_pool);
~SparsePoseGraph() override;
~PoseGraph() override;
SparsePoseGraph(const SparsePoseGraph&) = delete;
SparsePoseGraph& operator=(const SparsePoseGraph&) = delete;
PoseGraph(const PoseGraph&) = delete;
PoseGraph& operator=(const PoseGraph&) = delete;
// Adds a new node with 'constant_data'. Its 'constant_data->local_pose' was
// determined by scan matching against 'insertion_submaps.front()' and the
@ -249,7 +249,7 @@ class SparsePoseGraph : public mapping::PoseGraph {
// 'mutex_' of the pose graph is held while this class is used.
class TrimmingHandle : public mapping::Trimmable {
public:
TrimmingHandle(SparsePoseGraph* parent);
TrimmingHandle(PoseGraph* parent);
~TrimmingHandle() override {}
int num_submaps(int trajectory_id) const override;
@ -257,11 +257,11 @@ class SparsePoseGraph : public mapping::PoseGraph {
REQUIRES(parent_->mutex_) override;
private:
SparsePoseGraph* const parent_;
PoseGraph* const parent_;
};
};
} // namespace mapping_3d
} // namespace cartographer
#endif // CARTOGRAPHER_MAPPING_3D_SPARSE_POSE_GRAPH_H_
#endif // CARTOGRAPHER_MAPPING_3D_POSE_GRAPH_H_