Rename mapping_{2,3}d::SparsePoseGraph. (#678)
[RFC=0001](https://github.com/googlecartographer/rfcs/blob/master/text/0001-renaming-sparse-pose-graph.md)master
Wolfgang Hess
Wally B. Feed
parent
8c114d6eaf
commit
c25379cd20
|
|
@ -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()) {
|
||||
|
|
|
|||
|
|
@ -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_;
|
||||
|
|
|
|||
|
|
@ -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,
|
||||
common::ThreadPool* thread_pool)
|
||||
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,30 +150,30 @@ void SparsePoseGraph::AddTrajectoryIfNeeded(const int trajectory_id) {
|
|||
}
|
||||
}
|
||||
|
||||
void SparsePoseGraph::AddImuData(const int trajectory_id,
|
||||
const sensor::ImuData& imu_data) {
|
||||
void PoseGraph::AddImuData(const int trajectory_id,
|
||||
const sensor::ImuData& imu_data) {
|
||||
common::MutexLocker locker(&mutex_);
|
||||
AddWorkItem([=]() REQUIRES(mutex_) {
|
||||
optimization_problem_.AddImuData(trajectory_id, imu_data);
|
||||
});
|
||||
}
|
||||
|
||||
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,
|
||||
const mapping::SubmapId& submap_id) {
|
||||
void PoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
|
||||
const mapping::SubmapId& submap_id) {
|
||||
CHECK(submap_data_.at(submap_id).state == SubmapState::kFinished);
|
||||
|
||||
const common::Time node_time = GetLatestNodeTime(node_id, submap_id);
|
||||
|
|
@ -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([¬ification]() { 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,9 +382,8 @@ void SparsePoseGraph::FreezeTrajectory(const int trajectory_id) {
|
|||
});
|
||||
}
|
||||
|
||||
void SparsePoseGraph::AddSubmapFromProto(
|
||||
const transform::Rigid3d& global_submap_pose,
|
||||
const mapping::proto::Submap& submap) {
|
||||
void PoseGraph::AddSubmapFromProto(const transform::Rigid3d& global_submap_pose,
|
||||
const mapping::proto::Submap& submap) {
|
||||
if (!submap.has_submap_2d()) {
|
||||
return;
|
||||
}
|
||||
|
|
@ -412,8 +409,8 @@ void SparsePoseGraph::AddSubmapFromProto(
|
|||
});
|
||||
}
|
||||
|
||||
void SparsePoseGraph::AddNodeFromProto(const transform::Rigid3d& global_pose,
|
||||
const mapping::proto::Node& node) {
|
||||
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()};
|
||||
std::shared_ptr<const mapping::TrajectoryNode::Data> constant_data =
|
||||
|
|
@ -439,15 +436,15 @@ void SparsePoseGraph::AddNodeFromProto(const transform::Rigid3d& global_pose,
|
|||
});
|
||||
}
|
||||
|
||||
void SparsePoseGraph::AddNodeToSubmap(const mapping::NodeId& node_id,
|
||||
const mapping::SubmapId& submap_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_) {
|
||||
submap_data_.at(submap_id).node_ids.insert(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,16 +572,16 @@ std::vector<SparsePoseGraph::Constraint> SparsePoseGraph::constraints() {
|
|||
return result;
|
||||
}
|
||||
|
||||
void SparsePoseGraph::SetInitialTrajectoryPose(const int from_trajectory_id,
|
||||
const int to_trajectory_id,
|
||||
const transform::Rigid3d& pose,
|
||||
const common::Time time) {
|
||||
void PoseGraph::SetInitialTrajectoryPose(const int from_trajectory_id,
|
||||
const int to_trajectory_id,
|
||||
const transform::Rigid3d& pose,
|
||||
const common::Time time) {
|
||||
common::MutexLocker locker(&mutex_);
|
||||
initial_trajectory_poses_[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.
|
||||
|
|
@ -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,
|
||||
common::ThreadPool* thread_pool);
|
||||
~SparsePoseGraph() override;
|
||||
PoseGraph(const mapping::proto::SparsePoseGraphOptions& options,
|
||||
common::ThreadPool* thread_pool);
|
||||
~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_
|
||||
|
|
@ -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 =
|
||||
|
|
@ -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,
|
||||
common::ThreadPool* thread_pool)
|
||||
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,23 +148,23 @@ void SparsePoseGraph::AddTrajectoryIfNeeded(const int trajectory_id) {
|
|||
}
|
||||
}
|
||||
|
||||
void SparsePoseGraph::AddImuData(const int trajectory_id,
|
||||
const sensor::ImuData& imu_data) {
|
||||
void PoseGraph::AddImuData(const int trajectory_id,
|
||||
const sensor::ImuData& imu_data) {
|
||||
common::MutexLocker locker(&mutex_);
|
||||
AddWorkItem([=]() REQUIRES(mutex_) {
|
||||
optimization_problem_.AddImuData(trajectory_id, imu_data);
|
||||
});
|
||||
}
|
||||
|
||||
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,8 +174,8 @@ void SparsePoseGraph::AddFixedFramePoseData(
|
|||
});
|
||||
}
|
||||
|
||||
void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
|
||||
const mapping::SubmapId& submap_id) {
|
||||
void PoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
|
||||
const mapping::SubmapId& submap_id) {
|
||||
CHECK(submap_data_.at(submap_id).state == SubmapState::kFinished);
|
||||
|
||||
const transform::Rigid3d global_node_pose =
|
||||
|
|
@ -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([¬ification]() { 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,9 +399,8 @@ void SparsePoseGraph::FreezeTrajectory(const int trajectory_id) {
|
|||
});
|
||||
}
|
||||
|
||||
void SparsePoseGraph::AddSubmapFromProto(
|
||||
const transform::Rigid3d& global_submap_pose,
|
||||
const mapping::proto::Submap& submap) {
|
||||
void PoseGraph::AddSubmapFromProto(const transform::Rigid3d& global_submap_pose,
|
||||
const mapping::proto::Submap& submap) {
|
||||
if (!submap.has_submap_3d()) {
|
||||
return;
|
||||
}
|
||||
|
|
@ -427,8 +424,8 @@ void SparsePoseGraph::AddSubmapFromProto(
|
|||
});
|
||||
}
|
||||
|
||||
void SparsePoseGraph::AddNodeFromProto(const transform::Rigid3d& global_pose,
|
||||
const mapping::proto::Node& node) {
|
||||
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()};
|
||||
std::shared_ptr<const mapping::TrajectoryNode::Data> constant_data =
|
||||
|
|
@ -448,15 +445,15 @@ void SparsePoseGraph::AddNodeFromProto(const transform::Rigid3d& global_pose,
|
|||
});
|
||||
}
|
||||
|
||||
void SparsePoseGraph::AddNodeToSubmap(const mapping::NodeId& node_id,
|
||||
const mapping::SubmapId& submap_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_) {
|
||||
submap_data_.at(submap_id).node_ids.insert(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,36 +569,36 @@ 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,
|
||||
const int to_trajectory_id,
|
||||
const transform::Rigid3d& pose,
|
||||
const common::Time time) {
|
||||
void PoseGraph::SetInitialTrajectoryPose(const int from_trajectory_id,
|
||||
const int to_trajectory_id,
|
||||
const transform::Rigid3d& pose,
|
||||
const common::Time time) {
|
||||
common::MutexLocker locker(&mutex_);
|
||||
initial_trajectory_poses_[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.
|
||||
|
|
@ -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,
|
||||
common::ThreadPool* thread_pool);
|
||||
~SparsePoseGraph() override;
|
||||
PoseGraph(const mapping::proto::SparsePoseGraphOptions& options,
|
||||
common::ThreadPool* thread_pool);
|
||||
~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_
|
||||
Loading…
Reference in New Issue