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

Sparse pose graph no longer uses Submaps* for trajectory identification. (#290) 

Related to #256.

PAIR=wohe
master
Holger Rapp 2017-05-16 14:24:49 +02:00 committed by GitHub
parent 8612f5e6e5
commit 5aa968748e
17 changed files with 74 additions and 119 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
cartographer/mapping/map_builder.cc
View File

@ -78,7 +78,7 @@ int MapBuilder::AddTrajectoryBuilder(
&sensor_collator_, trajectory_id, expected_sensor_ids,
common::make_unique<mapping_3d::GlobalTrajectoryBuilder>(
trajectory_options.trajectory_builder_3d_options(),
sparse_pose_graph_3d_.get())));
trajectory_id, sparse_pose_graph_3d_.get())));
} else {
CHECK(trajectory_options.has_trajectory_builder_2d_options());
trajectory_builders_.push_back(
@ -86,10 +86,8 @@ int MapBuilder::AddTrajectoryBuilder(
&sensor_collator_, trajectory_id, expected_sensor_ids,
common::make_unique<mapping_2d::GlobalTrajectoryBuilder>(
trajectory_options.trajectory_builder_2d_options(),
sparse_pose_graph_2d_.get())));
trajectory_id, sparse_pose_graph_2d_.get())));
}
trajectory_ids_.emplace(trajectory_builders_.back()->submaps(),
trajectory_id);
return trajectory_id;
}
@ -106,12 +104,6 @@ int MapBuilder::GetBlockingTrajectoryId() const {
return sensor_collator_.GetBlockingTrajectoryId();
}
int MapBuilder::GetTrajectoryId(const Submaps* const trajectory) const {
const auto trajectory_id = trajectory_ids_.find(trajectory);
CHECK(trajectory_id != trajectory_ids_.end());
return trajectory_id->second;
}
proto::TrajectoryConnectivity MapBuilder::GetTrajectoryConnectivity() {
return ToProto(sparse_pose_graph_->GetConnectedTrajectories());
}
@ -125,10 +117,8 @@ string MapBuilder::SubmapToProto(const int trajectory_id,
" trajectories.";
}
const Submaps* const submaps =
trajectory_builders_.at(trajectory_id)->submaps();
const std::vector<transform::Rigid3d> submap_transforms =
sparse_pose_graph_->GetSubmapTransforms(submaps);
sparse_pose_graph_->GetSubmapTransforms(trajectory_id);
if (submap_index < 0 ||
static_cast<size_t>(submap_index) >= submap_transforms.size()) {
return "Requested submap " + std::to_string(submap_index) +
@ -137,6 +127,8 @@ string MapBuilder::SubmapToProto(const int trajectory_id,
" submaps in this trajectory.";
}
const Submaps* const submaps =
trajectory_builders_.at(trajectory_id)->submaps();
response->set_submap_version(submaps->Get(submap_index)->num_range_data);
submaps->SubmapToProto(submap_index, submap_transforms[submap_index],
response);

4
cartographer/mapping/map_builder.h
View File

@ -71,9 +71,6 @@ class MapBuilder {
// unblocked.
int GetBlockingTrajectoryId() const;
// Returns the trajectory ID for 'trajectory'.
int GetTrajectoryId(const mapping::Submaps* trajectory) const;
// Returns the trajectory connectivity.
proto::TrajectoryConnectivity GetTrajectoryConnectivity();
@ -97,7 +94,6 @@ class MapBuilder {
sensor::Collator sensor_collator_;
std::vector<std::unique_ptr<mapping::TrajectoryBuilder>> trajectory_builders_;
std::unordered_map<const mapping::Submaps*, int> trajectory_ids_;
};
} // namespace mapping

9
cartographer/mapping/sparse_pose_graph.h
View File

@ -26,7 +26,6 @@
#include "cartographer/mapping/id.h"
#include "cartographer/mapping/proto/sparse_pose_graph.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/transform/rigid_transform.h"
@ -71,11 +70,6 @@ class SparsePoseGraph {
// Get the current trajectory clusters.
virtual std::vector<std::vector<int>> GetConnectedTrajectories() = 0;
// Returns the current optimized transforms for the given 'trajectory'.
// TODO(hrapp): Remove this version.
virtual std::vector<transform::Rigid3d> GetSubmapTransforms(
const Submaps* trajectory) = 0;
// Returns the current optimized transforms for the given 'trajectory_id'.
virtual std::vector<transform::Rigid3d> GetSubmapTransforms(
int trajectory_id) = 0;
@ -83,8 +77,7 @@ class SparsePoseGraph {
// 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
// discontinuous, loop-closed frame).
virtual transform::Rigid3d GetLocalToGlobalTransform(
const Submaps* submaps) = 0;
virtual transform::Rigid3d GetLocalToGlobalTransform(int trajectory_id) = 0;
// Returns the current optimized trajectories.
virtual std::vector<std::vector<TrajectoryNode>> GetTrajectoryNodes() = 0;

10
cartographer/mapping_2d/global_trajectory_builder.cc
View File

@ -21,8 +21,8 @@ namespace mapping_2d {
GlobalTrajectoryBuilder::GlobalTrajectoryBuilder(
const proto::LocalTrajectoryBuilderOptions& options,
SparsePoseGraph* sparse_pose_graph)
: options_(options),
const int trajectory_id, SparsePoseGraph* sparse_pose_graph)
: trajectory_id_(trajectory_id),
sparse_pose_graph_(sparse_pose_graph),
local_trajectory_builder_(options) {}
@ -44,7 +44,7 @@ void GlobalTrajectoryBuilder::AddRangefinderData(
insertion_result->range_data_in_tracking_2d,
insertion_result->pose_estimate_2d,
kalman_filter::Project2D(insertion_result->covariance_estimate),
insertion_result->submaps, insertion_result->matching_submap,
trajectory_id_, insertion_result->matching_submap,
insertion_result->insertion_submaps);
}
}
@ -54,8 +54,8 @@ void GlobalTrajectoryBuilder::AddImuData(
const Eigen::Vector3d& angular_velocity) {
local_trajectory_builder_.AddImuData(time, linear_acceleration,
angular_velocity);
sparse_pose_graph_->AddImuData(local_trajectory_builder_.submaps(), time,
linear_acceleration, angular_velocity);
sparse_pose_graph_->AddImuData(trajectory_id_, time, linear_acceleration,
angular_velocity);
}
void GlobalTrajectoryBuilder::AddOdometerData(const common::Time time,

3
cartographer/mapping_2d/global_trajectory_builder.h
View File

@ -28,6 +28,7 @@ class GlobalTrajectoryBuilder
: public mapping::GlobalTrajectoryBuilderInterface {
public:
GlobalTrajectoryBuilder(const proto::LocalTrajectoryBuilderOptions& options,
int trajectory_id,
SparsePoseGraph* sparse_pose_graph);
~GlobalTrajectoryBuilder() override;
@ -48,7 +49,7 @@ class GlobalTrajectoryBuilder
const transform::Rigid3d& pose) override;
private:
const proto::LocalTrajectoryBuilderOptions options_;
const int trajectory_id_;
SparsePoseGraph* const sparse_pose_graph_;
LocalTrajectoryBuilder local_trajectory_builder_;
};

5
cartographer/mapping_2d/local_trajectory_builder.cc
View File

@ -236,9 +236,8 @@ LocalTrajectoryBuilder::AddHorizontalRangeData(
transform::Embed3D(pose_estimate_2d.cast<float>())));
return common::make_unique<InsertionResult>(InsertionResult{
time, &submaps_, matching_submap, insertion_submaps,
tracking_to_tracking_2d, tracking_2d_to_map, range_data_in_tracking_2d,
pose_estimate_2d, covariance_observation});
time, matching_submap, insertion_submaps, tracking_to_tracking_2d,
range_data_in_tracking_2d, pose_estimate_2d, covariance_observation});
}
const mapping::GlobalTrajectoryBuilderInterface::PoseEstimate&

2
cartographer/mapping_2d/local_trajectory_builder.h
View File

@ -45,11 +45,9 @@ class LocalTrajectoryBuilder {
public:
struct InsertionResult {
common::Time time;
const mapping::Submaps* submaps;
const mapping::Submap* matching_submap;
std::vector<const mapping::Submap*> insertion_submaps;
transform::Rigid3d tracking_to_tracking_2d;
transform::Rigid3d tracking_2d_to_map;
sensor::RangeData range_data_in_tracking_2d;
transform::Rigid2d pose_estimate_2d;
kalman_filter::PoseCovariance covariance_estimate;

46
cartographer/mapping_2d/sparse_pose_graph.cc
View File

@ -90,16 +90,13 @@ void SparsePoseGraph::GrowSubmapTransformsAsNeeded(
void SparsePoseGraph::AddScan(
common::Time time, const transform::Rigid3d& tracking_to_pose,
const sensor::RangeData& range_data_in_pose, const transform::Rigid2d& pose,
const kalman_filter::Pose2DCovariance& covariance,
const mapping::Submaps* const trajectory,
const kalman_filter::Pose2DCovariance& covariance, const int trajectory_id,
const mapping::Submap* const matching_submap,
const std::vector<const mapping::Submap*>& insertion_submaps) {
const transform::Rigid3d optimized_pose(
GetLocalToGlobalTransform(trajectory) * transform::Embed3D(pose));
GetLocalToGlobalTransform(trajectory_id) * transform::Embed3D(pose));
common::MutexLocker locker(&mutex_);
trajectory_ids_.emplace(trajectory, trajectory_ids_.size());
const int trajectory_id = trajectory_ids_.at(trajectory);
const int flat_scan_index = trajectory_nodes_.size();
CHECK_LT(flat_scan_index, std::numeric_limits<int>::max());
@ -150,15 +147,13 @@ void SparsePoseGraph::AddWorkItem(std::function<void()> work_item) {
}
}
void SparsePoseGraph::AddImuData(const mapping::Submaps* trajectory,
common::Time time,
void SparsePoseGraph::AddImuData(const int trajectory_id, common::Time time,
const Eigen::Vector3d& linear_acceleration,
const Eigen::Vector3d& angular_velocity) {
common::MutexLocker locker(&mutex_);
trajectory_ids_.emplace(trajectory, trajectory_ids_.size());
AddWorkItem([=]() REQUIRES(mutex_) {
optimization_problem_.AddImuData(trajectory_ids_.at(trajectory), time,
linear_acceleration, angular_velocity);
optimization_problem_.AddImuData(trajectory_id, time, linear_acceleration,
angular_velocity);
});
}
@ -173,7 +168,6 @@ void SparsePoseGraph::ComputeConstraint(const int scan_index,
.at(node_id.trajectory_id)
.at(node_id.node_index)
.point_cloud_pose;
const int scan_trajectory_id =
trajectory_nodes_[scan_index].constant_data->trajectory_id;
@ -423,9 +417,10 @@ void SparsePoseGraph::RunOptimization() {
std::vector<std::vector<mapping::TrajectoryNode>>
SparsePoseGraph::GetTrajectoryNodes() {
common::MutexLocker locker(&mutex_);
std::vector<std::vector<mapping::TrajectoryNode>> result(
trajectory_ids_.size());
std::vector<std::vector<mapping::TrajectoryNode>> result;
for (const auto& node : trajectory_nodes_) {
result.resize(
std::max<size_t>(result.size(), node.constant_data->trajectory_id + 1));
result.at(node.constant_data->trajectory_id).push_back(node);
}
return result;
@ -437,11 +432,18 @@ std::vector<SparsePoseGraph::Constraint> SparsePoseGraph::constraints() {
}
transform::Rigid3d SparsePoseGraph::GetLocalToGlobalTransform(
const mapping::Submaps* const submaps) {
const auto extrapolated_submap_transforms = GetSubmapTransforms(submaps);
const int trajectory_id) {
common::MutexLocker locker(&mutex_);
if (submap_states_.size() <= static_cast<size_t>(trajectory_id) ||
submap_states_.at(trajectory_id).empty()) {
return transform::Rigid3d::Identity();
}
const auto extrapolated_submap_transforms =
ExtrapolateSubmapTransforms(optimized_submap_transforms_, trajectory_id);
return extrapolated_submap_transforms.back() *
submaps->Get(extrapolated_submap_transforms.size() - 1)
->local_pose()
submap_states_.at(trajectory_id)
.at(extrapolated_submap_transforms.size() - 1)
.submap->local_pose()
.inverse();
}
@ -450,16 +452,6 @@ std::vector<std::vector<int>> SparsePoseGraph::GetConnectedTrajectories() {
return connected_components_;
}
std::vector<transform::Rigid3d> SparsePoseGraph::GetSubmapTransforms(
const mapping::Submaps* const trajectory) {
common::MutexLocker locker(&mutex_);
if (trajectory_ids_.count(trajectory) == 0) {
return {transform::Rigid3d::Identity()};
}
return ExtrapolateSubmapTransforms(optimized_submap_transforms_,
trajectory_ids_.at(trajectory));
}
std::vector<transform::Rigid3d> SparsePoseGraph::GetSubmapTransforms(
const int trajectory_id) {
common::MutexLocker locker(&mutex_);

13
cartographer/mapping_2d/sparse_pose_graph.h
View File

@ -70,23 +70,20 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
const sensor::RangeData& range_data_in_pose,
const transform::Rigid2d& pose,
const kalman_filter::Pose2DCovariance& pose_covariance,
const mapping::Submaps* trajectory,
const mapping::Submap* matching_submap,
int trajectory_id, const mapping::Submap* matching_submap,
const std::vector<const mapping::Submap*>& insertion_submaps)
EXCLUDES(mutex_);
// Adds new IMU data to be used in the optimization.
void AddImuData(const mapping::Submaps* trajectory, common::Time time,
void AddImuData(int trajectory_id, common::Time time,
const Eigen::Vector3d& linear_acceleration,
const Eigen::Vector3d& angular_velocity);
void RunFinalOptimization() override;
std::vector<std::vector<int>> GetConnectedTrajectories() override;
std::vector<transform::Rigid3d> GetSubmapTransforms(
const mapping::Submaps* trajectory) EXCLUDES(mutex_) override;
std::vector<transform::Rigid3d> GetSubmapTransforms(int trajectory_id)
EXCLUDES(mutex_) override;
transform::Rigid3d GetLocalToGlobalTransform(const mapping::Submaps* submaps)
transform::Rigid3d GetLocalToGlobalTransform(int trajectory_id)
EXCLUDES(mutex_) override;
std::vector<std::vector<mapping::TrajectoryNode>> GetTrajectoryNodes()
override EXCLUDES(mutex_);
@ -209,10 +206,6 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
// Current submap transforms used for displaying data.
std::vector<std::vector<sparse_pose_graph::SubmapData>>
optimized_submap_transforms_ GUARDED_BY(mutex_);
// Map from submap pointers to trajectory IDs.
std::unordered_map<const mapping::Submaps*, int> trajectory_ids_
GUARDED_BY(mutex_);
};
} // namespace mapping_2d

3
cartographer/mapping_2d/sparse_pose_graph_test.cc
View File

@ -158,11 +158,12 @@ class SparsePoseGraphTest : public ::testing::Test {
const sensor::RangeData range_data{
Eigen::Vector3f::Zero(), new_point_cloud, {}};
const transform::Rigid2d pose_estimate = noise * current_pose_;
constexpr int kTrajectoryId = 0;
submaps_->InsertRangeData(TransformRangeData(
range_data, transform::Embed3D(pose_estimate.cast<float>())));
sparse_pose_graph_->AddScan(common::FromUniversal(0),
transform::Rigid3d::Identity(), range_data,
pose_estimate, covariance, submaps_.get(),
pose_estimate, covariance, kTrajectoryId,
matching_submap, insertion_submaps);
}

11
cartographer/mapping_3d/global_trajectory_builder.cc
View File

@ -23,8 +23,9 @@ namespace mapping_3d {
GlobalTrajectoryBuilder::GlobalTrajectoryBuilder(
const proto::LocalTrajectoryBuilderOptions& options,
SparsePoseGraph* sparse_pose_graph)
: sparse_pose_graph_(sparse_pose_graph),
const int trajectory_id, SparsePoseGraph* sparse_pose_graph)
: trajectory_id_(trajectory_id),
sparse_pose_graph_(sparse_pose_graph),
local_trajectory_builder_(CreateLocalTrajectoryBuilder(options)) {}
GlobalTrajectoryBuilder::~GlobalTrajectoryBuilder() {}
@ -38,8 +39,8 @@ void GlobalTrajectoryBuilder::AddImuData(
const Eigen::Vector3d& angular_velocity) {
local_trajectory_builder_->AddImuData(time, linear_acceleration,
angular_velocity);
sparse_pose_graph_->AddImuData(local_trajectory_builder_->submaps(), time,
linear_acceleration, angular_velocity);
sparse_pose_graph_->AddImuData(trajectory_id_, time, linear_acceleration,
angular_velocity);
}
void GlobalTrajectoryBuilder::AddRangefinderData(
@ -55,7 +56,7 @@ void GlobalTrajectoryBuilder::AddRangefinderData(
sparse_pose_graph_->AddScan(
insertion_result->time, insertion_result->range_data_in_tracking,
insertion_result->pose_observation, insertion_result->covariance_estimate,
insertion_result->submaps, insertion_result->matching_submap,
trajectory_id_, insertion_result->matching_submap,
insertion_result->insertion_submaps);
}

2
cartographer/mapping_3d/global_trajectory_builder.h
View File

@ -31,6 +31,7 @@ class GlobalTrajectoryBuilder
: public mapping::GlobalTrajectoryBuilderInterface {
public:
GlobalTrajectoryBuilder(const proto::LocalTrajectoryBuilderOptions& options,
int trajectory_id,
mapping_3d::SparsePoseGraph* sparse_pose_graph);
~GlobalTrajectoryBuilder() override;
@ -47,6 +48,7 @@ class GlobalTrajectoryBuilder
const PoseEstimate& pose_estimate() const override;
private:
const int trajectory_id_;
mapping_3d::SparsePoseGraph* const sparse_pose_graph_;
std::unique_ptr<LocalTrajectoryBuilderInterface> local_trajectory_builder_;
};

2
cartographer/mapping_3d/kalman_local_trajectory_builder.cc
View File

@ -230,7 +230,7 @@ KalmanLocalTrajectoryBuilder::InsertIntoSubmap(
trajectory_node_index);
return std::unique_ptr<InsertionResult>(new InsertionResult{
time, range_data_in_tracking, pose_observation, covariance_estimate,
submaps_.get(), matching_submap, insertion_submaps});
matching_submap, insertion_submaps});
}
} // namespace mapping_3d

1
cartographer/mapping_3d/local_trajectory_builder_interface.h
View File

@ -38,7 +38,6 @@ class LocalTrajectoryBuilderInterface {
sensor::RangeData range_data_in_tracking;
transform::Rigid3d pose_observation;
kalman_filter::PoseCovariance covariance_estimate;
const Submaps* submaps;
const Submap* matching_submap;
std::vector<const Submap*> insertion_submaps;
};

6
cartographer/mapping_3d/optimizing_local_trajectory_builder.cc
View File

@ -406,9 +406,9 @@ OptimizingLocalTrajectoryBuilder::InsertIntoSubmap(
const kalman_filter::PoseCovariance kCovariance =
1e-7 * kalman_filter::PoseCovariance::Identity();
return std::unique_ptr<InsertionResult>(new InsertionResult{
time, range_data_in_tracking, pose_observation, kCovariance,
submaps_.get(), matching_submap, insertion_submaps});
return std::unique_ptr<InsertionResult>(
new InsertionResult{time, range_data_in_tracking, pose_observation,
kCovariance, matching_submap, insertion_submaps});
}
OptimizingLocalTrajectoryBuilder::State

46
cartographer/mapping_3d/sparse_pose_graph.cc
View File

@ -89,15 +89,13 @@ void SparsePoseGraph::GrowSubmapTransformsAsNeeded(
void SparsePoseGraph::AddScan(
common::Time time, const sensor::RangeData& range_data_in_tracking,
const transform::Rigid3d& pose,
const kalman_filter::PoseCovariance& covariance,
const Submaps* const trajectory, const Submap* const matching_submap,
const kalman_filter::PoseCovariance& covariance, const int trajectory_id,
const Submap* const matching_submap,
const std::vector<const Submap*>& insertion_submaps) {
const transform::Rigid3d optimized_pose(
GetLocalToGlobalTransform(trajectory) * pose);
GetLocalToGlobalTransform(trajectory_id) * pose);
common::MutexLocker locker(&mutex_);
trajectory_ids_.emplace(trajectory, trajectory_ids_.size());
const int trajectory_id = trajectory_ids_.at(trajectory);
const int flat_scan_index = trajectory_nodes_.size();
CHECK_LT(flat_scan_index, std::numeric_limits<int>::max());
@ -150,15 +148,13 @@ void SparsePoseGraph::AddWorkItem(std::function<void()> work_item) {
}
}
void SparsePoseGraph::AddImuData(const mapping::Submaps* trajectory,
common::Time time,
void SparsePoseGraph::AddImuData(const int trajectory_id, common::Time time,
const Eigen::Vector3d& linear_acceleration,
const Eigen::Vector3d& angular_velocity) {
common::MutexLocker locker(&mutex_);
trajectory_ids_.emplace(trajectory, trajectory_ids_.size());
AddWorkItem([=]() REQUIRES(mutex_) {
optimization_problem_.AddImuData(trajectory_ids_.at(trajectory), time,
linear_acceleration, angular_velocity);
optimization_problem_.AddImuData(trajectory_id, time, linear_acceleration,
angular_velocity);
});
}
@ -413,9 +409,10 @@ void SparsePoseGraph::RunOptimization() {
std::vector<std::vector<mapping::TrajectoryNode>>
SparsePoseGraph::GetTrajectoryNodes() {
common::MutexLocker locker(&mutex_);
std::vector<std::vector<mapping::TrajectoryNode>> result(
trajectory_ids_.size());
std::vector<std::vector<mapping::TrajectoryNode>> result;
for (const auto& node : trajectory_nodes_) {
result.resize(
std::max<size_t>(result.size(), node.constant_data->trajectory_id + 1));
result.at(node.constant_data->trajectory_id).push_back(node);
}
return result;
@ -427,11 +424,18 @@ std::vector<SparsePoseGraph::Constraint> SparsePoseGraph::constraints() {
}
transform::Rigid3d SparsePoseGraph::GetLocalToGlobalTransform(
const mapping::Submaps* const submaps) {
const auto extrapolated_submap_transforms = GetSubmapTransforms(submaps);
const int trajectory_id) {
common::MutexLocker locker(&mutex_);
if (submap_states_.size() <= static_cast<size_t>(trajectory_id) ||
submap_states_.at(trajectory_id).empty()) {
return transform::Rigid3d::Identity();
}
const auto extrapolated_submap_transforms =
ExtrapolateSubmapTransforms(optimized_submap_transforms_, trajectory_id);
return extrapolated_submap_transforms.back() *
submaps->Get(extrapolated_submap_transforms.size() - 1)
->local_pose()
submap_states_.at(trajectory_id)
.at(extrapolated_submap_transforms.size() - 1)
.submap->local_pose()
.inverse();
}
@ -440,16 +444,6 @@ std::vector<std::vector<int>> SparsePoseGraph::GetConnectedTrajectories() {
return connected_components_;
}
std::vector<transform::Rigid3d> SparsePoseGraph::GetSubmapTransforms(
const mapping::Submaps* const trajectory) {
common::MutexLocker locker(&mutex_);
if (trajectory_ids_.count(trajectory) == 0) {
return {transform::Rigid3d::Identity()};
}
return ExtrapolateSubmapTransforms(optimized_submap_transforms_,
trajectory_ids_.at(trajectory));
}
std::vector<transform::Rigid3d> SparsePoseGraph::GetSubmapTransforms(
const int trajectory_id) {
common::MutexLocker locker(&mutex_);

12
cartographer/mapping_3d/sparse_pose_graph.h
View File

@ -69,7 +69,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
const sensor::RangeData& range_data_in_tracking,
const transform::Rigid3d& pose,
const kalman_filter::PoseCovariance& pose_covariance,
const Submaps* trajectory, const Submap* matching_submap,
int trajectory_id, const Submap* matching_submap,
const std::vector<const Submap*>& insertion_submaps)
EXCLUDES(mutex_);
@ -78,17 +78,15 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
int GetNextTrajectoryNodeIndex() EXCLUDES(mutex_);
// Adds new IMU data to be used in the optimization.
void AddImuData(const mapping::Submaps* trajectory, common::Time time,
void AddImuData(int trajectory_id, common::Time time,
const Eigen::Vector3d& linear_acceleration,
const Eigen::Vector3d& angular_velocity);
void RunFinalOptimization() override;
std::vector<std::vector<int>> GetConnectedTrajectories() override;
std::vector<transform::Rigid3d> GetSubmapTransforms(
const mapping::Submaps* trajectory) EXCLUDES(mutex_) override;
std::vector<transform::Rigid3d> GetSubmapTransforms(int trajectory_id)
EXCLUDES(mutex_) override;
transform::Rigid3d GetLocalToGlobalTransform(const mapping::Submaps* submaps)
transform::Rigid3d GetLocalToGlobalTransform(int trajectory_id)
EXCLUDES(mutex_) override;
std::vector<std::vector<mapping::TrajectoryNode>> GetTrajectoryNodes()
override EXCLUDES(mutex_);
@ -209,10 +207,6 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
// Current submap transforms used for displaying data.
std::vector<std::vector<sparse_pose_graph::SubmapData>>
optimized_submap_transforms_ GUARDED_BY(mutex_);
// Map from submap pointers to trajectory IDs.
std::unordered_map<const mapping::Submaps*, int> trajectory_ids_
GUARDED_BY(mutex_);
};
} // namespace mapping_3d