Move GetSubmapData from GlobalTrajectoryBuilder into SparsePoseGraph. (#352)
- Adds a function to query all SubmapData from the SparsePoseGraph. - Changes MapBuilder::SubmapToProto to take a SubmapId. - Remove num_submaps().master
parent
16636cd4e1
commit
9646b31145
|
@ -46,15 +46,6 @@ CollatedTrajectoryBuilder::CollatedTrajectoryBuilder(
|
||||||
|
|
||||||
CollatedTrajectoryBuilder::~CollatedTrajectoryBuilder() {}
|
CollatedTrajectoryBuilder::~CollatedTrajectoryBuilder() {}
|
||||||
|
|
||||||
int CollatedTrajectoryBuilder::num_submaps() {
|
|
||||||
return wrapped_trajectory_builder_->num_submaps();
|
|
||||||
}
|
|
||||||
|
|
||||||
TrajectoryBuilder::SubmapData CollatedTrajectoryBuilder::GetSubmapData(
|
|
||||||
const int submap_index) {
|
|
||||||
return wrapped_trajectory_builder_->GetSubmapData(submap_index);
|
|
||||||
}
|
|
||||||
|
|
||||||
const TrajectoryBuilder::PoseEstimate&
|
const TrajectoryBuilder::PoseEstimate&
|
||||||
CollatedTrajectoryBuilder::pose_estimate() const {
|
CollatedTrajectoryBuilder::pose_estimate() const {
|
||||||
return wrapped_trajectory_builder_->pose_estimate();
|
return wrapped_trajectory_builder_->pose_estimate();
|
||||||
|
|
|
@ -49,8 +49,6 @@ class CollatedTrajectoryBuilder : public TrajectoryBuilder {
|
||||||
CollatedTrajectoryBuilder& operator=(const CollatedTrajectoryBuilder&) =
|
CollatedTrajectoryBuilder& operator=(const CollatedTrajectoryBuilder&) =
|
||||||
delete;
|
delete;
|
||||||
|
|
||||||
int num_submaps() override;
|
|
||||||
SubmapData GetSubmapData(int submap_index) override;
|
|
||||||
const PoseEstimate& pose_estimate() const override;
|
const PoseEstimate& pose_estimate() const override;
|
||||||
|
|
||||||
void AddSensorData(const string& sensor_id,
|
void AddSensorData(const string& sensor_id,
|
||||||
|
|
|
@ -39,7 +39,6 @@ namespace mapping {
|
||||||
class GlobalTrajectoryBuilderInterface {
|
class GlobalTrajectoryBuilderInterface {
|
||||||
public:
|
public:
|
||||||
using PoseEstimate = TrajectoryBuilder::PoseEstimate;
|
using PoseEstimate = TrajectoryBuilder::PoseEstimate;
|
||||||
using SubmapData = TrajectoryBuilder::SubmapData;
|
|
||||||
|
|
||||||
GlobalTrajectoryBuilderInterface() {}
|
GlobalTrajectoryBuilderInterface() {}
|
||||||
virtual ~GlobalTrajectoryBuilderInterface() {}
|
virtual ~GlobalTrajectoryBuilderInterface() {}
|
||||||
|
@ -49,8 +48,6 @@ class GlobalTrajectoryBuilderInterface {
|
||||||
GlobalTrajectoryBuilderInterface& operator=(
|
GlobalTrajectoryBuilderInterface& operator=(
|
||||||
const GlobalTrajectoryBuilderInterface&) = delete;
|
const GlobalTrajectoryBuilderInterface&) = delete;
|
||||||
|
|
||||||
virtual int num_submaps() = 0;
|
|
||||||
virtual SubmapData GetSubmapData(int submap_index) = 0;
|
|
||||||
virtual const PoseEstimate& pose_estimate() const = 0;
|
virtual const PoseEstimate& pose_estimate() const = 0;
|
||||||
|
|
||||||
virtual void AddRangefinderData(common::Time time,
|
virtual void AddRangefinderData(common::Time time,
|
||||||
|
|
|
@ -104,25 +104,25 @@ int MapBuilder::GetBlockingTrajectoryId() const {
|
||||||
return sensor_collator_.GetBlockingTrajectoryId();
|
return sensor_collator_.GetBlockingTrajectoryId();
|
||||||
}
|
}
|
||||||
|
|
||||||
string MapBuilder::SubmapToProto(const int trajectory_id,
|
string MapBuilder::SubmapToProto(const mapping::SubmapId& submap_id,
|
||||||
const int submap_index,
|
|
||||||
proto::SubmapQuery::Response* const response) {
|
proto::SubmapQuery::Response* const response) {
|
||||||
if (trajectory_id < 0 || trajectory_id >= num_trajectory_builders()) {
|
if (submap_id.trajectory_id < 0 ||
|
||||||
return "Requested submap from trajectory " + std::to_string(trajectory_id) +
|
submap_id.trajectory_id >= num_trajectory_builders()) {
|
||||||
" but there are only " + std::to_string(num_trajectory_builders()) +
|
return "Requested submap from trajectory " +
|
||||||
" trajectories.";
|
std::to_string(submap_id.trajectory_id) + " but there are only " +
|
||||||
|
std::to_string(num_trajectory_builders()) + " trajectories.";
|
||||||
}
|
}
|
||||||
|
|
||||||
const int num_submaps = trajectory_builders_.at(trajectory_id)->num_submaps();
|
const int num_submaps =
|
||||||
if (submap_index < 0 || submap_index >= num_submaps) {
|
sparse_pose_graph_->num_submaps(submap_id.trajectory_id);
|
||||||
return "Requested submap " + std::to_string(submap_index) +
|
if (submap_id.submap_index < 0 || submap_id.submap_index >= num_submaps) {
|
||||||
" from trajectory " + std::to_string(trajectory_id) +
|
return "Requested submap " + std::to_string(submap_id.submap_index) +
|
||||||
|
" from trajectory " + std::to_string(submap_id.trajectory_id) +
|
||||||
" but there are only " + std::to_string(num_submaps) +
|
" but there are only " + std::to_string(num_submaps) +
|
||||||
" submaps in this trajectory.";
|
" submaps in this trajectory.";
|
||||||
}
|
}
|
||||||
|
|
||||||
const auto submap_data =
|
const auto submap_data = sparse_pose_graph_->GetSubmapData(submap_id);
|
||||||
trajectory_builders_.at(trajectory_id)->GetSubmapData(submap_index);
|
|
||||||
CHECK(submap_data.submap != nullptr);
|
CHECK(submap_data.submap != nullptr);
|
||||||
submap_data.submap->ToResponseProto(submap_data.pose, response);
|
submap_data.submap->ToResponseProto(submap_data.pose, response);
|
||||||
return "";
|
return "";
|
||||||
|
|
|
@ -27,6 +27,7 @@
|
||||||
#include "cartographer/common/lua_parameter_dictionary.h"
|
#include "cartographer/common/lua_parameter_dictionary.h"
|
||||||
#include "cartographer/common/port.h"
|
#include "cartographer/common/port.h"
|
||||||
#include "cartographer/common/thread_pool.h"
|
#include "cartographer/common/thread_pool.h"
|
||||||
|
#include "cartographer/mapping/id.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"
|
||||||
|
@ -71,10 +72,9 @@ class MapBuilder {
|
||||||
// unblocked.
|
// unblocked.
|
||||||
int GetBlockingTrajectoryId() const;
|
int GetBlockingTrajectoryId() const;
|
||||||
|
|
||||||
// Fills the SubmapQuery::Response corresponding to 'submap_index' from
|
// Fills the SubmapQuery::Response corresponding to 'submap_id'. Returns an
|
||||||
// 'trajectory_id'. Returns an error string on failure, or an empty string on
|
// error string on failure, or an empty string on success.
|
||||||
// success.
|
string SubmapToProto(const SubmapId& submap_id,
|
||||||
string SubmapToProto(int trajectory_id, int submap_index,
|
|
||||||
proto::SubmapQuery::Response* response);
|
proto::SubmapQuery::Response* response);
|
||||||
|
|
||||||
int num_trajectory_builders() const;
|
int num_trajectory_builders() const;
|
||||||
|
|
|
@ -64,6 +64,7 @@ proto::SparsePoseGraph SparsePoseGraph::ToProto() {
|
||||||
std::map<NodeId, NodeId> node_id_remapping; // Due to trimming.
|
std::map<NodeId, NodeId> node_id_remapping; // Due to trimming.
|
||||||
|
|
||||||
const auto all_trajectory_nodes = GetTrajectoryNodes();
|
const auto all_trajectory_nodes = GetTrajectoryNodes();
|
||||||
|
const auto all_submap_data = GetAllSubmapData();
|
||||||
for (size_t trajectory_id = 0; trajectory_id != all_trajectory_nodes.size();
|
for (size_t trajectory_id = 0; trajectory_id != all_trajectory_nodes.size();
|
||||||
++trajectory_id) {
|
++trajectory_id) {
|
||||||
const auto& single_trajectory_nodes = all_trajectory_nodes[trajectory_id];
|
const auto& single_trajectory_nodes = all_trajectory_nodes[trajectory_id];
|
||||||
|
@ -86,12 +87,9 @@ proto::SparsePoseGraph SparsePoseGraph::ToProto() {
|
||||||
}
|
}
|
||||||
|
|
||||||
if (!single_trajectory_nodes.empty()) {
|
if (!single_trajectory_nodes.empty()) {
|
||||||
const int num_submaps_in_trajectory = num_submaps(trajectory_id);
|
for (const auto& submap_data : all_submap_data[trajectory_id]) {
|
||||||
for (int submap_index = 0; submap_index != num_submaps_in_trajectory;
|
|
||||||
++submap_index) {
|
|
||||||
const SubmapId submap_id{static_cast<int>(trajectory_id), submap_index};
|
|
||||||
*trajectory_proto->add_submap()->mutable_pose() =
|
*trajectory_proto->add_submap()->mutable_pose() =
|
||||||
transform::ToProto(GetSubmapTransform(submap_id));
|
transform::ToProto(submap_data.pose);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
|
@ -28,6 +28,7 @@
|
||||||
#include "cartographer/mapping/pose_graph_trimmer.h"
|
#include "cartographer/mapping/pose_graph_trimmer.h"
|
||||||
#include "cartographer/mapping/proto/sparse_pose_graph.pb.h"
|
#include "cartographer/mapping/proto/sparse_pose_graph.pb.h"
|
||||||
#include "cartographer/mapping/proto/sparse_pose_graph_options.pb.h"
|
#include "cartographer/mapping/proto/sparse_pose_graph_options.pb.h"
|
||||||
|
#include "cartographer/mapping/submaps.h"
|
||||||
#include "cartographer/mapping/trajectory_node.h"
|
#include "cartographer/mapping/trajectory_node.h"
|
||||||
#include "cartographer/transform/rigid_transform.h"
|
#include "cartographer/transform/rigid_transform.h"
|
||||||
|
|
||||||
|
@ -61,6 +62,11 @@ class SparsePoseGraph {
|
||||||
enum Tag { INTRA_SUBMAP, INTER_SUBMAP } tag;
|
enum Tag { INTRA_SUBMAP, INTER_SUBMAP } tag;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
struct SubmapData {
|
||||||
|
std::shared_ptr<const Submap> submap;
|
||||||
|
transform::Rigid3d pose;
|
||||||
|
};
|
||||||
|
|
||||||
SparsePoseGraph() {}
|
SparsePoseGraph() {}
|
||||||
virtual ~SparsePoseGraph() {}
|
virtual ~SparsePoseGraph() {}
|
||||||
|
|
||||||
|
@ -80,8 +86,12 @@ class SparsePoseGraph {
|
||||||
// Return the number of submaps for the given 'trajectory_id'.
|
// Return the number of submaps for the given 'trajectory_id'.
|
||||||
virtual int num_submaps(int trajectory_id) = 0;
|
virtual int num_submaps(int trajectory_id) = 0;
|
||||||
|
|
||||||
// Returns the current optimized transform for the given 'submap_id'.
|
// Returns the current optimized transform and submap itself for the given
|
||||||
virtual transform::Rigid3d GetSubmapTransform(const SubmapId& submap_id) = 0;
|
// 'submap_id'.
|
||||||
|
virtual SubmapData GetSubmapData(const SubmapId& submap_id) = 0;
|
||||||
|
|
||||||
|
// Returns data for all Submaps by trajectory.
|
||||||
|
virtual std::vector<std::vector<SubmapData>> GetAllSubmapData() = 0;
|
||||||
|
|
||||||
// Returns the transform converting data in the local map frame (i.e. the
|
// Returns the transform converting data in the local map frame (i.e. the
|
||||||
// continuous, non-loop-closed frame) into the global map frame (i.e. the
|
// continuous, non-loop-closed frame) into the global map frame (i.e. the
|
||||||
|
|
|
@ -53,19 +53,12 @@ class TrajectoryBuilder {
|
||||||
sensor::PointCloud point_cloud;
|
sensor::PointCloud point_cloud;
|
||||||
};
|
};
|
||||||
|
|
||||||
struct SubmapData {
|
|
||||||
std::shared_ptr<const Submap> submap;
|
|
||||||
transform::Rigid3d pose;
|
|
||||||
};
|
|
||||||
|
|
||||||
TrajectoryBuilder() {}
|
TrajectoryBuilder() {}
|
||||||
virtual ~TrajectoryBuilder() {}
|
virtual ~TrajectoryBuilder() {}
|
||||||
|
|
||||||
TrajectoryBuilder(const TrajectoryBuilder&) = delete;
|
TrajectoryBuilder(const TrajectoryBuilder&) = delete;
|
||||||
TrajectoryBuilder& operator=(const TrajectoryBuilder&) = delete;
|
TrajectoryBuilder& operator=(const TrajectoryBuilder&) = delete;
|
||||||
|
|
||||||
virtual int num_submaps() = 0;
|
|
||||||
virtual SubmapData GetSubmapData(int submap_index) = 0;
|
|
||||||
virtual const PoseEstimate& pose_estimate() const = 0;
|
virtual const PoseEstimate& pose_estimate() const = 0;
|
||||||
|
|
||||||
virtual void AddSensorData(const string& sensor_id,
|
virtual void AddSensorData(const string& sensor_id,
|
||||||
|
|
|
@ -28,19 +28,6 @@ GlobalTrajectoryBuilder::GlobalTrajectoryBuilder(
|
||||||
|
|
||||||
GlobalTrajectoryBuilder::~GlobalTrajectoryBuilder() {}
|
GlobalTrajectoryBuilder::~GlobalTrajectoryBuilder() {}
|
||||||
|
|
||||||
int GlobalTrajectoryBuilder::num_submaps() {
|
|
||||||
return sparse_pose_graph_->num_submaps(trajectory_id_);
|
|
||||||
}
|
|
||||||
|
|
||||||
GlobalTrajectoryBuilder::SubmapData GlobalTrajectoryBuilder::GetSubmapData(
|
|
||||||
const int submap_index) {
|
|
||||||
// TODO(hrapp): Get rid of this function and query the sparse pose graph
|
|
||||||
// directly.
|
|
||||||
const mapping::SubmapId submap_id{trajectory_id_, submap_index};
|
|
||||||
return {sparse_pose_graph_->GetSubmap(submap_id),
|
|
||||||
sparse_pose_graph_->GetSubmapTransform(submap_id)};
|
|
||||||
}
|
|
||||||
|
|
||||||
void GlobalTrajectoryBuilder::AddRangefinderData(
|
void GlobalTrajectoryBuilder::AddRangefinderData(
|
||||||
const common::Time time, const Eigen::Vector3f& origin,
|
const common::Time time, const Eigen::Vector3f& origin,
|
||||||
const sensor::PointCloud& ranges) {
|
const sensor::PointCloud& ranges) {
|
||||||
|
|
|
@ -35,8 +35,6 @@ class GlobalTrajectoryBuilder
|
||||||
GlobalTrajectoryBuilder(const GlobalTrajectoryBuilder&) = delete;
|
GlobalTrajectoryBuilder(const GlobalTrajectoryBuilder&) = delete;
|
||||||
GlobalTrajectoryBuilder& operator=(const GlobalTrajectoryBuilder&) = delete;
|
GlobalTrajectoryBuilder& operator=(const GlobalTrajectoryBuilder&) = delete;
|
||||||
|
|
||||||
int num_submaps() override;
|
|
||||||
SubmapData GetSubmapData(int submap_index) override;
|
|
||||||
const mapping::GlobalTrajectoryBuilderInterface::PoseEstimate& pose_estimate()
|
const mapping::GlobalTrajectoryBuilderInterface::PoseEstimate& pose_estimate()
|
||||||
const override;
|
const override;
|
||||||
|
|
||||||
|
|
|
@ -424,12 +424,6 @@ SparsePoseGraph::GetTrajectoryNodes() {
|
||||||
return trajectory_nodes_.data();
|
return trajectory_nodes_.data();
|
||||||
}
|
}
|
||||||
|
|
||||||
std::shared_ptr<const Submap> SparsePoseGraph::GetSubmap(
|
|
||||||
const mapping::SubmapId& submap_id) {
|
|
||||||
common::MutexLocker locker(&mutex_);
|
|
||||||
return submap_data_.at(submap_id).submap;
|
|
||||||
}
|
|
||||||
|
|
||||||
std::vector<SparsePoseGraph::Constraint> SparsePoseGraph::constraints() {
|
std::vector<SparsePoseGraph::Constraint> SparsePoseGraph::constraints() {
|
||||||
common::MutexLocker locker(&mutex_);
|
common::MutexLocker locker(&mutex_);
|
||||||
return constraints_;
|
return constraints_;
|
||||||
|
@ -455,24 +449,29 @@ int SparsePoseGraph::num_submaps(const int trajectory_id) {
|
||||||
return submap_data_.num_indices(trajectory_id);
|
return submap_data_.num_indices(trajectory_id);
|
||||||
}
|
}
|
||||||
|
|
||||||
transform::Rigid3d SparsePoseGraph::GetSubmapTransform(
|
mapping::SparsePoseGraph::SubmapData SparsePoseGraph::GetSubmapData(
|
||||||
const mapping::SubmapId& submap_id) {
|
const mapping::SubmapId& submap_id) {
|
||||||
common::MutexLocker locker(&mutex_);
|
common::MutexLocker locker(&mutex_);
|
||||||
// We already have an optimized pose.
|
return GetSubmapDataUnderLock(submap_id);
|
||||||
if (submap_id.trajectory_id <
|
|
||||||
static_cast<int>(optimized_submap_transforms_.size()) &&
|
|
||||||
submap_id.submap_index < static_cast<int>(optimized_submap_transforms_
|
|
||||||
.at(submap_id.trajectory_id)
|
|
||||||
.size())) {
|
|
||||||
return transform::Embed3D(
|
|
||||||
optimized_submap_transforms_.at(submap_id.trajectory_id)
|
|
||||||
.at(submap_id.submap_index)
|
|
||||||
.pose);
|
|
||||||
}
|
}
|
||||||
// We have to extrapolate.
|
|
||||||
return ComputeLocalToGlobalTransform(optimized_submap_transforms_,
|
std::vector<std::vector<mapping::SparsePoseGraph::SubmapData>>
|
||||||
submap_id.trajectory_id) *
|
SparsePoseGraph::GetAllSubmapData() {
|
||||||
submap_data_.at(submap_id).submap->local_pose();
|
common::MutexLocker locker(&mutex_);
|
||||||
|
std::vector<std::vector<mapping::SparsePoseGraph::SubmapData>>
|
||||||
|
all_submap_data(submap_data_.num_trajectories());
|
||||||
|
for (int trajectory_id = 0; trajectory_id < submap_data_.num_trajectories();
|
||||||
|
++trajectory_id) {
|
||||||
|
all_submap_data[trajectory_id].reserve(
|
||||||
|
submap_data_.num_indices(trajectory_id));
|
||||||
|
for (int submap_index = 0;
|
||||||
|
submap_index < submap_data_.num_indices(trajectory_id);
|
||||||
|
++submap_index) {
|
||||||
|
all_submap_data[trajectory_id].emplace_back(GetSubmapDataUnderLock(
|
||||||
|
mapping::SubmapId{trajectory_id, submap_index}));
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return all_submap_data;
|
||||||
}
|
}
|
||||||
|
|
||||||
transform::Rigid3d SparsePoseGraph::ComputeLocalToGlobalTransform(
|
transform::Rigid3d SparsePoseGraph::ComputeLocalToGlobalTransform(
|
||||||
|
@ -493,6 +492,26 @@ transform::Rigid3d SparsePoseGraph::ComputeLocalToGlobalTransform(
|
||||||
.inverse();
|
.inverse();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
mapping::SparsePoseGraph::SubmapData SparsePoseGraph::GetSubmapDataUnderLock(
|
||||||
|
const mapping::SubmapId& submap_id) {
|
||||||
|
auto submap = submap_data_.at(submap_id).submap;
|
||||||
|
// We already have an optimized pose.
|
||||||
|
if (submap_id.trajectory_id <
|
||||||
|
static_cast<int>(optimized_submap_transforms_.size()) &&
|
||||||
|
submap_id.submap_index < static_cast<int>(optimized_submap_transforms_
|
||||||
|
.at(submap_id.trajectory_id)
|
||||||
|
.size())) {
|
||||||
|
return {submap, transform::Embed3D(
|
||||||
|
optimized_submap_transforms_.at(submap_id.trajectory_id)
|
||||||
|
.at(submap_id.submap_index)
|
||||||
|
.pose)};
|
||||||
|
}
|
||||||
|
// We have to extrapolate.
|
||||||
|
return {submap, ComputeLocalToGlobalTransform(optimized_submap_transforms_,
|
||||||
|
submap_id.trajectory_id) *
|
||||||
|
submap->local_pose()};
|
||||||
|
}
|
||||||
|
|
||||||
SparsePoseGraph::TrimmingHandle::TrimmingHandle(SparsePoseGraph* const parent)
|
SparsePoseGraph::TrimmingHandle::TrimmingHandle(SparsePoseGraph* const parent)
|
||||||
: parent_(parent) {}
|
: parent_(parent) {}
|
||||||
|
|
||||||
|
|
|
@ -84,14 +84,14 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
|
||||||
void RunFinalOptimization() override;
|
void RunFinalOptimization() override;
|
||||||
std::vector<std::vector<int>> GetConnectedTrajectories() override;
|
std::vector<std::vector<int>> GetConnectedTrajectories() override;
|
||||||
int num_submaps(int trajectory_id) EXCLUDES(mutex_) override;
|
int num_submaps(int trajectory_id) EXCLUDES(mutex_) override;
|
||||||
transform::Rigid3d GetSubmapTransform(const mapping::SubmapId& submap_id)
|
mapping::SparsePoseGraph::SubmapData GetSubmapData(
|
||||||
EXCLUDES(mutex_) override;
|
const mapping::SubmapId& submap_id) EXCLUDES(mutex_) override;
|
||||||
|
std::vector<std::vector<mapping::SparsePoseGraph::SubmapData>>
|
||||||
|
GetAllSubmapData() EXCLUDES(mutex_) override;
|
||||||
transform::Rigid3d GetLocalToGlobalTransform(int trajectory_id)
|
transform::Rigid3d GetLocalToGlobalTransform(int trajectory_id)
|
||||||
EXCLUDES(mutex_) override;
|
EXCLUDES(mutex_) override;
|
||||||
std::vector<std::vector<mapping::TrajectoryNode>> GetTrajectoryNodes()
|
std::vector<std::vector<mapping::TrajectoryNode>> GetTrajectoryNodes()
|
||||||
override EXCLUDES(mutex_);
|
override EXCLUDES(mutex_);
|
||||||
std::shared_ptr<const Submap> GetSubmap(const mapping::SubmapId& submap_id)
|
|
||||||
EXCLUDES(mutex_);
|
|
||||||
std::vector<Constraint> constraints() override EXCLUDES(mutex_);
|
std::vector<Constraint> constraints() override EXCLUDES(mutex_);
|
||||||
|
|
||||||
private:
|
private:
|
||||||
|
@ -154,11 +154,14 @@ 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(
|
||||||
|
const mapping::SubmapId& submap_id) REQUIRES(mutex_);
|
||||||
|
|
||||||
const mapping::proto::SparsePoseGraphOptions options_;
|
const mapping::proto::SparsePoseGraphOptions options_;
|
||||||
common::Mutex mutex_;
|
common::Mutex mutex_;
|
||||||
|
|
||||||
// If it exists, further scans must be added to this queue, and will be
|
// If it exists, further scans must be added to this queue, and will be
|
||||||
// considered later.
|
// considered later
|
||||||
std::unique_ptr<std::deque<std::function<void()>>> scan_queue_
|
std::unique_ptr<std::deque<std::function<void()>>> scan_queue_
|
||||||
GUARDED_BY(mutex_);
|
GUARDED_BY(mutex_);
|
||||||
|
|
||||||
|
|
|
@ -30,19 +30,6 @@ GlobalTrajectoryBuilder::GlobalTrajectoryBuilder(
|
||||||
|
|
||||||
GlobalTrajectoryBuilder::~GlobalTrajectoryBuilder() {}
|
GlobalTrajectoryBuilder::~GlobalTrajectoryBuilder() {}
|
||||||
|
|
||||||
int GlobalTrajectoryBuilder::num_submaps() {
|
|
||||||
return sparse_pose_graph_->num_submaps(trajectory_id_);
|
|
||||||
}
|
|
||||||
|
|
||||||
GlobalTrajectoryBuilder::SubmapData GlobalTrajectoryBuilder::GetSubmapData(
|
|
||||||
const int submap_index) {
|
|
||||||
// TODO(hrapp): Get rid of this function and query the sparse pose graph
|
|
||||||
// directly.
|
|
||||||
const mapping::SubmapId submap_id{trajectory_id_, submap_index};
|
|
||||||
return {sparse_pose_graph_->GetSubmap(submap_id),
|
|
||||||
sparse_pose_graph_->GetSubmapTransform(submap_id)};
|
|
||||||
}
|
|
||||||
|
|
||||||
void GlobalTrajectoryBuilder::AddImuData(
|
void GlobalTrajectoryBuilder::AddImuData(
|
||||||
const common::Time time, const Eigen::Vector3d& linear_acceleration,
|
const common::Time time, const Eigen::Vector3d& linear_acceleration,
|
||||||
const Eigen::Vector3d& angular_velocity) {
|
const Eigen::Vector3d& angular_velocity) {
|
||||||
|
|
|
@ -38,8 +38,6 @@ class GlobalTrajectoryBuilder
|
||||||
GlobalTrajectoryBuilder(const GlobalTrajectoryBuilder&) = delete;
|
GlobalTrajectoryBuilder(const GlobalTrajectoryBuilder&) = delete;
|
||||||
GlobalTrajectoryBuilder& operator=(const GlobalTrajectoryBuilder&) = delete;
|
GlobalTrajectoryBuilder& operator=(const GlobalTrajectoryBuilder&) = delete;
|
||||||
|
|
||||||
int num_submaps() override;
|
|
||||||
SubmapData GetSubmapData(int submap_index) override;
|
|
||||||
void AddImuData(common::Time time, const Eigen::Vector3d& linear_acceleration,
|
void AddImuData(common::Time time, const Eigen::Vector3d& linear_acceleration,
|
||||||
const Eigen::Vector3d& angular_velocity) override;
|
const Eigen::Vector3d& angular_velocity) override;
|
||||||
void AddRangefinderData(common::Time time, const Eigen::Vector3f& origin,
|
void AddRangefinderData(common::Time time, const Eigen::Vector3f& origin,
|
||||||
|
|
|
@ -424,12 +424,6 @@ SparsePoseGraph::GetTrajectoryNodes() {
|
||||||
return trajectory_nodes_.data();
|
return trajectory_nodes_.data();
|
||||||
}
|
}
|
||||||
|
|
||||||
std::shared_ptr<const Submap> SparsePoseGraph::GetSubmap(
|
|
||||||
const mapping::SubmapId& submap_id) {
|
|
||||||
common::MutexLocker locker(&mutex_);
|
|
||||||
return submap_data_.at(submap_id).submap;
|
|
||||||
}
|
|
||||||
|
|
||||||
std::vector<SparsePoseGraph::Constraint> SparsePoseGraph::constraints() {
|
std::vector<SparsePoseGraph::Constraint> SparsePoseGraph::constraints() {
|
||||||
common::MutexLocker locker(&mutex_);
|
common::MutexLocker locker(&mutex_);
|
||||||
return constraints_;
|
return constraints_;
|
||||||
|
@ -455,23 +449,29 @@ int SparsePoseGraph::num_submaps(const int trajectory_id) {
|
||||||
return submap_data_.num_indices(trajectory_id);
|
return submap_data_.num_indices(trajectory_id);
|
||||||
}
|
}
|
||||||
|
|
||||||
transform::Rigid3d SparsePoseGraph::GetSubmapTransform(
|
mapping::SparsePoseGraph::SubmapData SparsePoseGraph::GetSubmapData(
|
||||||
const mapping::SubmapId& submap_id) {
|
const mapping::SubmapId& submap_id) {
|
||||||
common::MutexLocker locker(&mutex_);
|
common::MutexLocker locker(&mutex_);
|
||||||
// We already have an optimized pose.
|
return GetSubmapDataUnderLock(submap_id);
|
||||||
if (submap_id.trajectory_id <
|
|
||||||
static_cast<int>(optimized_submap_transforms_.size()) &&
|
|
||||||
submap_id.submap_index < static_cast<int>(optimized_submap_transforms_
|
|
||||||
.at(submap_id.trajectory_id)
|
|
||||||
.size())) {
|
|
||||||
return optimized_submap_transforms_.at(submap_id.trajectory_id)
|
|
||||||
.at(submap_id.submap_index)
|
|
||||||
.pose;
|
|
||||||
}
|
}
|
||||||
// We have to extrapolate.
|
|
||||||
return ComputeLocalToGlobalTransform(optimized_submap_transforms_,
|
std::vector<std::vector<mapping::SparsePoseGraph::SubmapData>>
|
||||||
submap_id.trajectory_id) *
|
SparsePoseGraph::GetAllSubmapData() {
|
||||||
submap_data_.at(submap_id).submap->local_pose();
|
common::MutexLocker locker(&mutex_);
|
||||||
|
std::vector<std::vector<mapping::SparsePoseGraph::SubmapData>>
|
||||||
|
all_submap_data(submap_data_.num_trajectories());
|
||||||
|
for (int trajectory_id = 0; trajectory_id < submap_data_.num_trajectories();
|
||||||
|
++trajectory_id) {
|
||||||
|
all_submap_data[trajectory_id].reserve(
|
||||||
|
submap_data_.num_indices(trajectory_id));
|
||||||
|
for (int submap_index = 0;
|
||||||
|
submap_index < submap_data_.num_indices(trajectory_id);
|
||||||
|
++submap_index) {
|
||||||
|
all_submap_data[trajectory_id].emplace_back(GetSubmapDataUnderLock(
|
||||||
|
mapping::SubmapId{trajectory_id, submap_index}));
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return all_submap_data;
|
||||||
}
|
}
|
||||||
|
|
||||||
transform::Rigid3d SparsePoseGraph::ComputeLocalToGlobalTransform(
|
transform::Rigid3d SparsePoseGraph::ComputeLocalToGlobalTransform(
|
||||||
|
@ -492,5 +492,24 @@ transform::Rigid3d SparsePoseGraph::ComputeLocalToGlobalTransform(
|
||||||
.inverse();
|
.inverse();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
mapping::SparsePoseGraph::SubmapData SparsePoseGraph::GetSubmapDataUnderLock(
|
||||||
|
const mapping::SubmapId& submap_id) {
|
||||||
|
auto submap = submap_data_.at(submap_id).submap;
|
||||||
|
// We already have an optimized pose.
|
||||||
|
if (submap_id.trajectory_id <
|
||||||
|
static_cast<int>(optimized_submap_transforms_.size()) &&
|
||||||
|
submap_id.submap_index < static_cast<int>(optimized_submap_transforms_
|
||||||
|
.at(submap_id.trajectory_id)
|
||||||
|
.size())) {
|
||||||
|
return {submap, optimized_submap_transforms_.at(submap_id.trajectory_id)
|
||||||
|
.at(submap_id.submap_index)
|
||||||
|
.pose};
|
||||||
|
}
|
||||||
|
// We have to extrapolate.
|
||||||
|
return {submap, ComputeLocalToGlobalTransform(optimized_submap_transforms_,
|
||||||
|
submap_id.trajectory_id) *
|
||||||
|
submap->local_pose()};
|
||||||
|
}
|
||||||
|
|
||||||
} // namespace mapping_3d
|
} // namespace mapping_3d
|
||||||
} // namespace cartographer
|
} // namespace cartographer
|
||||||
|
|
|
@ -83,14 +83,14 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
|
||||||
void RunFinalOptimization() override;
|
void RunFinalOptimization() override;
|
||||||
std::vector<std::vector<int>> GetConnectedTrajectories() override;
|
std::vector<std::vector<int>> GetConnectedTrajectories() override;
|
||||||
int num_submaps(int trajectory_id) EXCLUDES(mutex_) override;
|
int num_submaps(int trajectory_id) EXCLUDES(mutex_) override;
|
||||||
transform::Rigid3d GetSubmapTransform(const mapping::SubmapId& submap_id)
|
mapping::SparsePoseGraph::SubmapData GetSubmapData(
|
||||||
EXCLUDES(mutex_) override;
|
const mapping::SubmapId& submap_id) EXCLUDES(mutex_) override;
|
||||||
|
std::vector<std::vector<mapping::SparsePoseGraph::SubmapData>>
|
||||||
|
GetAllSubmapData() EXCLUDES(mutex_) override;
|
||||||
transform::Rigid3d GetLocalToGlobalTransform(int trajectory_id)
|
transform::Rigid3d GetLocalToGlobalTransform(int trajectory_id)
|
||||||
EXCLUDES(mutex_) override;
|
EXCLUDES(mutex_) override;
|
||||||
std::vector<std::vector<mapping::TrajectoryNode>> GetTrajectoryNodes()
|
std::vector<std::vector<mapping::TrajectoryNode>> GetTrajectoryNodes()
|
||||||
override EXCLUDES(mutex_);
|
override EXCLUDES(mutex_);
|
||||||
std::shared_ptr<const Submap> GetSubmap(const mapping::SubmapId& submap_id)
|
|
||||||
EXCLUDES(mutex_);
|
|
||||||
std::vector<Constraint> constraints() override EXCLUDES(mutex_);
|
std::vector<Constraint> constraints() override EXCLUDES(mutex_);
|
||||||
|
|
||||||
private:
|
private:
|
||||||
|
@ -153,6 +153,9 @@ 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(
|
||||||
|
const mapping::SubmapId& submap_id) REQUIRES(mutex_);
|
||||||
|
|
||||||
const mapping::proto::SparsePoseGraphOptions options_;
|
const mapping::proto::SparsePoseGraphOptions options_;
|
||||||
common::Mutex mutex_;
|
common::Mutex mutex_;
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue