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

Clean-up colliding 'SubmapData' names. (#1096)

master
Alexander Belyaev 2018-04-20 18:15:37 +02:00 committed by GitHub
parent 643bc825e9
commit eebced5e16
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
13 changed files with 118 additions and 119 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
cartographer/mapping/internal/2d/pose_graph/landmark_cost_function_2d.h
View File

@ -40,8 +40,8 @@ class LandmarkCostFunction2D {
PoseGraphInterface::LandmarkNode::LandmarkObservation; PoseGraphInterface::LandmarkNode::LandmarkObservation;
static ceres::CostFunction* CreateAutoDiffCostFunction( static ceres::CostFunction* CreateAutoDiffCostFunction(
const LandmarkObservation& observation, const NodeData2D& prev_node, const LandmarkObservation& observation, const NodeSpec2D& prev_node,
const NodeData2D& next_node) { const NodeSpec2D& next_node) {
return new ceres::AutoDiffCostFunction< return new ceres::AutoDiffCostFunction<
LandmarkCostFunction2D, 6 /* residuals */, LandmarkCostFunction2D, 6 /* residuals */,
3 /* previous node pose variables */, 3 /* next node pose variables */, 3 /* previous node pose variables */, 3 /* next node pose variables */,
@ -71,8 +71,8 @@ class LandmarkCostFunction2D {
private: private:
LandmarkCostFunction2D(const LandmarkObservation& observation, LandmarkCostFunction2D(const LandmarkObservation& observation,
const NodeData2D& prev_node, const NodeSpec2D& prev_node,
const NodeData2D& next_node) const NodeSpec2D& next_node)
: landmark_to_tracking_transform_( : landmark_to_tracking_transform_(
observation.landmark_to_tracking_transform), observation.landmark_to_tracking_transform),
prev_node_gravity_alignment_(prev_node.gravity_alignment), prev_node_gravity_alignment_(prev_node.gravity_alignment),

4
cartographer/mapping/internal/2d/pose_graph/landmark_cost_function_2d_test.cc
View File

@ -34,10 +34,10 @@ using LandmarkObservation =
PoseGraphInterface::LandmarkNode::LandmarkObservation; PoseGraphInterface::LandmarkNode::LandmarkObservation;
TEST(LandmarkCostFunctionTest, SmokeTest) { TEST(LandmarkCostFunctionTest, SmokeTest) {
NodeData2D prev_node; NodeSpec2D prev_node;
prev_node.time = common::FromUniversal(0); prev_node.time = common::FromUniversal(0);
prev_node.gravity_alignment = Eigen::Quaterniond::Identity(); prev_node.gravity_alignment = Eigen::Quaterniond::Identity();
NodeData2D next_node; NodeSpec2D next_node;
next_node.time = common::FromUniversal(10); next_node.time = common::FromUniversal(10);
next_node.gravity_alignment = Eigen::Quaterniond::Identity(); next_node.gravity_alignment = Eigen::Quaterniond::Identity();

26
cartographer/mapping/internal/2d/pose_graph/optimization_problem_2d.cc
View File

@ -43,8 +43,6 @@ namespace {
using ::cartographer::mapping::pose_graph::CeresPose; using ::cartographer::mapping::pose_graph::CeresPose;
using LandmarkNode = ::cartographer::mapping::PoseGraphInterface::LandmarkNode; using LandmarkNode = ::cartographer::mapping::PoseGraphInterface::LandmarkNode;
using NodeData = NodeData2D;
using SubmapData = SubmapData2D;
// Converts a pose into the 3 optimization variable format used for Ceres: // Converts a pose into the 3 optimization variable format used for Ceres:
// translation in x and y, followed by the rotation angle representing the // translation in x and y, followed by the rotation angle representing the
@ -63,7 +61,7 @@ transform::Rigid2d ToPose(const std::array<double, 3>& values) {
// applies a relative transform from it. // applies a relative transform from it.
transform::Rigid3d GetInitialLandmarkPose( transform::Rigid3d GetInitialLandmarkPose(
const LandmarkNode::LandmarkObservation& observation, const LandmarkNode::LandmarkObservation& observation,
const NodeData& prev_node, const NodeData& next_node, const NodeSpec2D& prev_node, const NodeSpec2D& next_node,
const std::array<double, 3>& prev_node_pose, const std::array<double, 3>& prev_node_pose,
const std::array<double, 3>& next_node_pose) { const std::array<double, 3>& next_node_pose) {
const double interpolation_parameter = const double interpolation_parameter =
@ -82,7 +80,7 @@ transform::Rigid3d GetInitialLandmarkPose(
void AddLandmarkCostFunctions( void AddLandmarkCostFunctions(
const std::map<std::string, LandmarkNode>& landmark_nodes, const std::map<std::string, LandmarkNode>& landmark_nodes,
bool freeze_landmarks, const MapById<NodeId, NodeData>& node_data, bool freeze_landmarks, const MapById<NodeId, NodeSpec2D>& node_data,
MapById<NodeId, std::array<double, 3>>* C_nodes, MapById<NodeId, std::array<double, 3>>* C_nodes,
std::map<std::string, CeresPose>* C_landmarks, ceres::Problem* problem) { std::map<std::string, CeresPose>* C_landmarks, ceres::Problem* problem) {
for (const auto& landmark_node : landmark_nodes) { for (const auto& landmark_node : landmark_nodes) {
@ -145,7 +143,7 @@ void AddLandmarkCostFunctions(
} // namespace } // namespace
OptimizationProblem2D::OptimizationProblem2D( OptimizationProblem2D::OptimizationProblem2D(
const pose_graph::proto::OptimizationProblemOptions& options) const proto::OptimizationProblemOptions& options)
: options_(options) {} : options_(options) {}
OptimizationProblem2D::~OptimizationProblem2D() {} OptimizationProblem2D::~OptimizationProblem2D() {}
@ -161,12 +159,12 @@ void OptimizationProblem2D::AddOdometryData(
} }
void OptimizationProblem2D::AddTrajectoryNode(const int trajectory_id, void OptimizationProblem2D::AddTrajectoryNode(const int trajectory_id,
const NodeData& node_data) { const NodeSpec2D& node_data) {
node_data_.Append(trajectory_id, node_data); node_data_.Append(trajectory_id, node_data);
} }
void OptimizationProblem2D::InsertTrajectoryNode(const NodeId& node_id, void OptimizationProblem2D::InsertTrajectoryNode(const NodeId& node_id,
const NodeData& node_data) { const NodeSpec2D& node_data) {
node_data_.Insert(node_id, node_data); node_data_.Insert(node_id, node_data);
} }
@ -178,12 +176,12 @@ void OptimizationProblem2D::TrimTrajectoryNode(const NodeId& node_id) {
void OptimizationProblem2D::AddSubmap( void OptimizationProblem2D::AddSubmap(
const int trajectory_id, const transform::Rigid2d& global_submap_pose) { const int trajectory_id, const transform::Rigid2d& global_submap_pose) {
submap_data_.Append(trajectory_id, SubmapData{global_submap_pose}); submap_data_.Append(trajectory_id, SubmapSpec2D{global_submap_pose});
} }
void OptimizationProblem2D::InsertSubmap( void OptimizationProblem2D::InsertSubmap(
const SubmapId& submap_id, const transform::Rigid2d& global_submap_pose) { const SubmapId& submap_id, const transform::Rigid2d& global_submap_pose) {
submap_data_.Insert(submap_id, SubmapData{global_submap_pose}); submap_data_.Insert(submap_id, SubmapSpec2D{global_submap_pose});
} }
void OptimizationProblem2D::TrimSubmap(const SubmapId& submap_id) { void OptimizationProblem2D::TrimSubmap(const SubmapId& submap_id) {
@ -209,7 +207,7 @@ void OptimizationProblem2D::Solve(
ceres::Problem problem(problem_options); ceres::Problem problem(problem_options);
// Set the starting point. // Set the starting point.
// TODO(hrapp): Move ceres data into SubmapData. // TODO(hrapp): Move ceres data into SubmapSpec.
MapById<SubmapId, std::array<double, 3>> C_submaps; MapById<SubmapId, std::array<double, 3>> C_submaps;
MapById<NodeId, std::array<double, 3>> C_nodes; MapById<NodeId, std::array<double, 3>> C_nodes;
std::map<std::string, CeresPose> C_landmarks; std::map<std::string, CeresPose> C_landmarks;
@ -264,10 +262,10 @@ void OptimizationProblem2D::Solve(
auto prev_node_it = node_it; auto prev_node_it = node_it;
for (++node_it; node_it != trajectory_end; ++node_it) { for (++node_it; node_it != trajectory_end; ++node_it) {
const NodeId first_node_id = prev_node_it->id; const NodeId first_node_id = prev_node_it->id;
const NodeData& first_node_data = prev_node_it->data; const NodeSpec2D& first_node_data = prev_node_it->data;
prev_node_it = node_it; prev_node_it = node_it;
const NodeId second_node_id = node_it->id; const NodeId second_node_id = node_it->id;
const NodeData& second_node_data = node_it->data; const NodeSpec2D& second_node_data = node_it->data;
if (second_node_id.node_index != first_node_id.node_index + 1) { if (second_node_id.node_index != first_node_id.node_index + 1) {
continue; continue;
@ -344,8 +342,8 @@ std::unique_ptr<transform::Rigid3d> OptimizationProblem2D::InterpolateOdometry(
std::unique_ptr<transform::Rigid3d> std::unique_ptr<transform::Rigid3d>
OptimizationProblem2D::CalculateOdometryBetweenNodes( OptimizationProblem2D::CalculateOdometryBetweenNodes(
const int trajectory_id, const NodeData& first_node_data, const int trajectory_id, const NodeSpec2D& first_node_data,
const NodeData& second_node_data) const { const NodeSpec2D& second_node_data) const {
if (odometry_data_.HasTrajectory(trajectory_id)) { if (odometry_data_.HasTrajectory(trajectory_id)) {
const std::unique_ptr<transform::Rigid3d> first_node_odometry = const std::unique_ptr<transform::Rigid3d> first_node_odometry =
InterpolateOdometry(trajectory_id, first_node_data.time); InterpolateOdometry(trajectory_id, first_node_data.time);

22
cartographer/mapping/internal/2d/pose_graph/optimization_problem_2d.h
View File

@ -40,19 +40,19 @@ namespace cartographer {
namespace mapping { namespace mapping {
namespace pose_graph { namespace pose_graph {
struct NodeData2D { struct NodeSpec2D {
common::Time time; common::Time time;
transform::Rigid2d local_pose_2d; transform::Rigid2d local_pose_2d;
transform::Rigid2d global_pose_2d; transform::Rigid2d global_pose_2d;
Eigen::Quaterniond gravity_alignment; Eigen::Quaterniond gravity_alignment;
}; };
struct SubmapData2D { struct SubmapSpec2D {
transform::Rigid2d global_pose; transform::Rigid2d global_pose;
}; };
class OptimizationProblem2D class OptimizationProblem2D
: public OptimizationProblemInterface<NodeData2D, SubmapData2D, : public OptimizationProblemInterface<NodeSpec2D, SubmapSpec2D,
transform::Rigid2d> { transform::Rigid2d> {
public: public:
explicit OptimizationProblem2D( explicit OptimizationProblem2D(
@ -66,9 +66,9 @@ class OptimizationProblem2D
void AddOdometryData(int trajectory_id, void AddOdometryData(int trajectory_id,
const sensor::OdometryData& odometry_data) override; const sensor::OdometryData& odometry_data) override;
void AddTrajectoryNode(int trajectory_id, void AddTrajectoryNode(int trajectory_id,
const NodeData2D& node_data) override; const NodeSpec2D& node_data) override;
void InsertTrajectoryNode(const NodeId& node_id, void InsertTrajectoryNode(const NodeId& node_id,
const NodeData2D& node_data) override; const NodeSpec2D& node_data) override;
void TrimTrajectoryNode(const NodeId& node_id) override; void TrimTrajectoryNode(const NodeId& node_id) override;
void AddSubmap(int trajectory_id, void AddSubmap(int trajectory_id,
const transform::Rigid2d& global_submap_pose) override; const transform::Rigid2d& global_submap_pose) override;
@ -82,10 +82,10 @@ class OptimizationProblem2D
const std::set<int>& frozen_trajectories, const std::set<int>& frozen_trajectories,
const std::map<std::string, LandmarkNode>& landmark_nodes) override; const std::map<std::string, LandmarkNode>& landmark_nodes) override;
const MapById<NodeId, NodeData2D>& node_data() const override { const MapById<NodeId, NodeSpec2D>& node_data() const override {
return node_data_; return node_data_;
} }
const MapById<SubmapId, SubmapData2D>& submap_data() const override { const MapById<SubmapId, SubmapSpec2D>& submap_data() const override {
return submap_data_; return submap_data_;
} }
const std::map<std::string, transform::Rigid3d>& landmark_data() const std::map<std::string, transform::Rigid3d>& landmark_data()
@ -105,12 +105,12 @@ class OptimizationProblem2D
int trajectory_id, common::Time time) const; int trajectory_id, common::Time time) const;
// Computes the relative pose between two nodes based on odometry data. // Computes the relative pose between two nodes based on odometry data.
std::unique_ptr<transform::Rigid3d> CalculateOdometryBetweenNodes( std::unique_ptr<transform::Rigid3d> CalculateOdometryBetweenNodes(
int trajectory_id, const NodeData2D& first_node_data, int trajectory_id, const NodeSpec2D& first_node_data,
const NodeData2D& second_node_data) const; const NodeSpec2D& second_node_data) const;
pose_graph::proto::OptimizationProblemOptions options_; pose_graph::proto::OptimizationProblemOptions options_;
MapById<NodeId, NodeData2D> node_data_; MapById<NodeId, NodeSpec2D> node_data_;
MapById<SubmapId, SubmapData2D> submap_data_; MapById<SubmapId, SubmapSpec2D> submap_data_;
std::map<std::string, transform::Rigid3d> landmark_data_; std::map<std::string, transform::Rigid3d> landmark_data_;
sensor::MapByTime<sensor::ImuData> imu_data_; sensor::MapByTime<sensor::ImuData> imu_data_;
sensor::MapByTime<sensor::OdometryData> odometry_data_; sensor::MapByTime<sensor::OdometryData> odometry_data_;

33
cartographer/mapping/internal/2d/pose_graph_2d.cc
View File

@ -119,7 +119,8 @@ NodeId PoseGraph2D::AddNode(
insertion_submaps.back()) { insertion_submaps.back()) {
// We grow 'submap_data_' as needed. This code assumes that the first // We grow 'submap_data_' as needed. This code assumes that the first
// time we see a new submap is as 'insertion_submaps.back()'. // time we see a new submap is as 'insertion_submaps.back()'.
const SubmapId submap_id = submap_data_.Append(trajectory_id, SubmapData()); const SubmapId submap_id =
submap_data_.Append(trajectory_id, InternalSubmapData());
submap_data_.at(submap_id).submap = insertion_submaps.back(); submap_data_.at(submap_id).submap = insertion_submaps.back();
} }
@ -180,7 +181,7 @@ void PoseGraph2D::AddLandmarkData(int trajectory_id,
AddWorkItem([=]() REQUIRES(mutex_) { AddWorkItem([=]() REQUIRES(mutex_) {
for (const auto& observation : landmark_data.landmark_observations) { for (const auto& observation : landmark_data.landmark_observations) {
landmark_nodes_[observation.id].landmark_observations.emplace_back( landmark_nodes_[observation.id].landmark_observations.emplace_back(
PoseGraph::LandmarkNode::LandmarkObservation{ LandmarkNode::LandmarkObservation{
trajectory_id, landmark_data.time, trajectory_id, landmark_data.time,
observation.landmark_to_tracking_transform, observation.landmark_to_tracking_transform,
observation.translation_weight, observation.rotation_weight}); observation.translation_weight, observation.rotation_weight});
@ -249,7 +250,7 @@ void PoseGraph2D::ComputeConstraintsForNode(
local_pose_2d; local_pose_2d;
optimization_problem_->AddTrajectoryNode( optimization_problem_->AddTrajectoryNode(
matching_id.trajectory_id, matching_id.trajectory_id,
pose_graph::NodeData2D{constant_data->time, local_pose_2d, global_pose_2d, pose_graph::NodeSpec2D{constant_data->time, local_pose_2d, global_pose_2d,
constant_data->gravity_alignment}); constant_data->gravity_alignment});
for (size_t i = 0; i < insertion_submaps.size(); ++i) { for (size_t i = 0; i < insertion_submaps.size(); ++i) {
const SubmapId submap_id = submap_ids[i]; const SubmapId submap_id = submap_ids[i];
@ -277,7 +278,8 @@ void PoseGraph2D::ComputeConstraintsForNode(
if (newly_finished_submap) { if (newly_finished_submap) {
const SubmapId finished_submap_id = submap_ids.front(); const SubmapId finished_submap_id = submap_ids.front();
SubmapData& finished_submap_data = submap_data_.at(finished_submap_id); InternalSubmapData& finished_submap_data =
submap_data_.at(finished_submap_id);
CHECK(finished_submap_data.state == SubmapState::kActive); CHECK(finished_submap_data.state == SubmapState::kActive);
finished_submap_data.state = SubmapState::kFinished; finished_submap_data.state = SubmapState::kFinished;
// We have a new completed submap, so we look into adding constraints for // We have a new completed submap, so we look into adding constraints for
@ -304,7 +306,7 @@ void PoseGraph2D::DispatchOptimization() {
common::Time PoseGraph2D::GetLatestNodeTime(const NodeId& node_id, common::Time PoseGraph2D::GetLatestNodeTime(const NodeId& node_id,
const SubmapId& submap_id) const { const SubmapId& submap_id) const {
common::Time time = trajectory_nodes_.at(node_id).constant_data->time; common::Time time = trajectory_nodes_.at(node_id).constant_data->time;
const SubmapData& submap_data = submap_data_.at(submap_id); const InternalSubmapData& submap_data = submap_data_.at(submap_id);
if (!submap_data.node_ids.empty()) { if (!submap_data.node_ids.empty()) {
const NodeId last_submap_node_id = const NodeId last_submap_node_id =
*submap_data_.at(submap_id).node_ids.rbegin(); *submap_data_.at(submap_id).node_ids.rbegin();
@ -315,7 +317,7 @@ common::Time PoseGraph2D::GetLatestNodeTime(const NodeId& node_id,
} }
void PoseGraph2D::UpdateTrajectoryConnectivity(const Constraint& constraint) { void PoseGraph2D::UpdateTrajectoryConnectivity(const Constraint& constraint) {
CHECK_EQ(constraint.tag, PoseGraph::Constraint::INTER_SUBMAP); CHECK_EQ(constraint.tag, Constraint::INTER_SUBMAP);
const common::Time time = const common::Time time =
GetLatestNodeTime(constraint.node_id, constraint.submap_id); GetLatestNodeTime(constraint.node_id, constraint.submap_id);
trajectory_connectivity_state_.Connect(constraint.node_id.trajectory_id, trajectory_connectivity_state_.Connect(constraint.node_id.trajectory_id,
@ -441,11 +443,11 @@ void PoseGraph2D::AddSubmapFromProto(
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
AddTrajectoryIfNeeded(submap_id.trajectory_id); AddTrajectoryIfNeeded(submap_id.trajectory_id);
submap_data_.Insert(submap_id, SubmapData()); submap_data_.Insert(submap_id, InternalSubmapData());
submap_data_.at(submap_id).submap = submap_ptr; submap_data_.at(submap_id).submap = submap_ptr;
// Immediately show the submap at the 'global_submap_pose'. // Immediately show the submap at the 'global_submap_pose'.
global_submap_poses_.Insert(submap_id, global_submap_poses_.Insert(submap_id,
pose_graph::SubmapData2D{global_submap_pose_2d}); pose_graph::SubmapSpec2D{global_submap_pose_2d});
AddWorkItem([this, submap_id, global_submap_pose_2d]() REQUIRES(mutex_) { AddWorkItem([this, submap_id, global_submap_pose_2d]() REQUIRES(mutex_) {
submap_data_.at(submap_id).state = SubmapState::kFinished; submap_data_.at(submap_id).state = SubmapState::kFinished;
optimization_problem_->InsertSubmap(submap_id, global_submap_pose_2d); optimization_problem_->InsertSubmap(submap_id, global_submap_pose_2d);
@ -469,7 +471,7 @@ void PoseGraph2D::AddNodeFromProto(const transform::Rigid3d& global_pose,
constant_data->gravity_alignment.inverse()); constant_data->gravity_alignment.inverse());
optimization_problem_->InsertTrajectoryNode( optimization_problem_->InsertTrajectoryNode(
node_id, node_id,
pose_graph::NodeData2D{ pose_graph::NodeSpec2D{
constant_data->time, constant_data->time,
transform::Project2D(constant_data->local_pose * transform::Project2D(constant_data->local_pose *
gravity_alignment_inverse), gravity_alignment_inverse),
@ -645,7 +647,7 @@ sensor::MapByTime<sensor::OdometryData> PoseGraph2D::GetOdometryData() {
return optimization_problem_->odometry_data(); return optimization_problem_->odometry_data();
} }
std::map<std::string /* landmark ID */, PoseGraph::LandmarkNode> std::map<std::string /* landmark ID */, PoseGraphInterface::LandmarkNode>
PoseGraph2D::GetLandmarkNodes() { PoseGraph2D::GetLandmarkNodes() {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
return landmark_nodes_; return landmark_nodes_;
@ -661,8 +663,8 @@ PoseGraph2D::GetFixedFramePoseData() {
return {}; // Not implemented yet in 2D. return {}; // Not implemented yet in 2D.
} }
std::vector<PoseGraph2D::Constraint> PoseGraph2D::constraints() { std::vector<PoseGraphInterface::Constraint> PoseGraph2D::constraints() {
std::vector<Constraint> result; std::vector<PoseGraphInterface::Constraint> result;
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
for (const Constraint& constraint : constraints_) { for (const Constraint& constraint : constraints_) {
result.push_back(Constraint{ result.push_back(Constraint{
@ -717,7 +719,8 @@ std::vector<std::vector<int>> PoseGraph2D::GetConnectedTrajectories() {
return trajectory_connectivity_state_.Components(); return trajectory_connectivity_state_.Components();
} }
PoseGraph::SubmapData PoseGraph2D::GetSubmapData(const SubmapId& submap_id) { PoseGraphInterface::SubmapData PoseGraph2D::GetSubmapData(
const SubmapId& submap_id) {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
return GetSubmapDataUnderLock(submap_id); return GetSubmapDataUnderLock(submap_id);
} }
@ -743,7 +746,7 @@ PoseGraph2D::GetAllSubmapPoses() {
} }
transform::Rigid3d PoseGraph2D::ComputeLocalToGlobalTransform( transform::Rigid3d PoseGraph2D::ComputeLocalToGlobalTransform(
const MapById<SubmapId, pose_graph::SubmapData2D>& global_submap_poses, const MapById<SubmapId, pose_graph::SubmapSpec2D>& global_submap_poses,
const int trajectory_id) const { const int trajectory_id) const {
auto begin_it = global_submap_poses.BeginOfTrajectory(trajectory_id); auto begin_it = global_submap_poses.BeginOfTrajectory(trajectory_id);
auto end_it = global_submap_poses.EndOfTrajectory(trajectory_id); auto end_it = global_submap_poses.EndOfTrajectory(trajectory_id);
@ -766,7 +769,7 @@ transform::Rigid3d PoseGraph2D::ComputeLocalToGlobalTransform(
.inverse(); .inverse();
} }
PoseGraph::SubmapData PoseGraph2D::GetSubmapDataUnderLock( PoseGraphInterface::SubmapData PoseGraph2D::GetSubmapDataUnderLock(
const SubmapId& submap_id) { const SubmapId& submap_id) {
const auto it = submap_data_.find(submap_id); const auto it = submap_data_.find(submap_id);
if (it == submap_data_.end()) { if (it == submap_data_.end()) {

21
cartographer/mapping/internal/2d/pose_graph_2d.h
View File

@ -108,7 +108,7 @@ class PoseGraph2D : public PoseGraph {
void AddTrimmer(std::unique_ptr<PoseGraphTrimmer> trimmer) override; void AddTrimmer(std::unique_ptr<PoseGraphTrimmer> trimmer) override;
void RunFinalOptimization() override; void RunFinalOptimization() override;
std::vector<std::vector<int>> GetConnectedTrajectories() override; std::vector<std::vector<int>> GetConnectedTrajectories() override;
PoseGraph::SubmapData GetSubmapData(const SubmapId& submap_id) PoseGraphInterface::SubmapData GetSubmapData(const SubmapId& submap_id)
EXCLUDES(mutex_) override; EXCLUDES(mutex_) override;
MapById<SubmapId, PoseGraphInterface::SubmapData> GetAllSubmapData() MapById<SubmapId, PoseGraphInterface::SubmapData> GetAllSubmapData()
EXCLUDES(mutex_) override; EXCLUDES(mutex_) override;
@ -145,7 +145,7 @@ class PoseGraph2D : public PoseGraph {
// transitions to kFinished, all nodes are tried to match against this submap. // transitions to kFinished, all nodes are tried to match against this submap.
// Likewise, all new nodes are matched against submaps which are finished. // Likewise, all new nodes are matched against submaps which are finished.
enum class SubmapState { kActive, kFinished }; enum class SubmapState { kActive, kFinished };
struct SubmapData { struct InternalSubmapData {
std::shared_ptr<const Submap2D> submap; std::shared_ptr<const Submap2D> submap;
// IDs of the nodes that were inserted into this map together with // IDs of the nodes that were inserted into this map together with
@ -201,11 +201,10 @@ class PoseGraph2D : public PoseGraph {
// Computes the local to global map frame transform based on the given // Computes the local to global map frame transform based on the given
// 'global_submap_poses'. // 'global_submap_poses'.
transform::Rigid3d ComputeLocalToGlobalTransform( transform::Rigid3d ComputeLocalToGlobalTransform(
const MapById<SubmapId, pose_graph::SubmapData2D>& global_submap_poses, const MapById<SubmapId, pose_graph::SubmapSpec2D>& global_submap_poses,
int trajectory_id) const REQUIRES(mutex_); int trajectory_id) const REQUIRES(mutex_);
PoseGraph::SubmapData GetSubmapDataUnderLock(const SubmapId& submap_id) SubmapData GetSubmapDataUnderLock(const SubmapId& submap_id) REQUIRES(mutex_);
REQUIRES(mutex_);
common::Time GetLatestNodeTime(const NodeId& node_id, common::Time GetLatestNodeTime(const NodeId& node_id,
const SubmapId& submap_id) const const SubmapId& submap_id) const
@ -246,14 +245,14 @@ class PoseGraph2D : public PoseGraph {
// Submaps get assigned an ID and state as soon as they are seen, even // Submaps get assigned an ID and state as soon as they are seen, even
// before they take part in the background computations. // before they take part in the background computations.
MapById<SubmapId, SubmapData> submap_data_ GUARDED_BY(mutex_); MapById<SubmapId, InternalSubmapData> submap_data_ GUARDED_BY(mutex_);
// Data that are currently being shown. // Data that are currently being shown.
MapById<NodeId, TrajectoryNode> trajectory_nodes_ GUARDED_BY(mutex_); MapById<NodeId, TrajectoryNode> trajectory_nodes_ GUARDED_BY(mutex_);
int num_trajectory_nodes_ GUARDED_BY(mutex_) = 0; int num_trajectory_nodes_ GUARDED_BY(mutex_) = 0;
// Global submap poses currently used for displaying data. // Global submap poses currently used for displaying data.
MapById<SubmapId, pose_graph::SubmapData2D> global_submap_poses_ MapById<SubmapId, pose_graph::SubmapSpec2D> global_submap_poses_
GUARDED_BY(mutex_); GUARDED_BY(mutex_);
// Global landmark poses with all observations. // Global landmark poses with all observations.
@ -282,12 +281,12 @@ class PoseGraph2D : public PoseGraph {
int num_submaps(int trajectory_id) const override; int num_submaps(int trajectory_id) const override;
std::vector<SubmapId> GetSubmapIds(int trajectory_id) const override; std::vector<SubmapId> GetSubmapIds(int trajectory_id) const override;
MapById<SubmapId, PoseGraphInterface::SubmapData> GetAllSubmapData() MapById<SubmapId, SubmapData> GetAllSubmapData() const override
const override REQUIRES(parent_->mutex_); REQUIRES(parent_->mutex_);
const MapById<NodeId, TrajectoryNode>& GetTrajectoryNodes() const override const MapById<NodeId, TrajectoryNode>& GetTrajectoryNodes() const override
REQUIRES(parent_->mutex_); REQUIRES(parent_->mutex_);
const std::vector<PoseGraphInterface::Constraint>& GetConstraints() const std::vector<Constraint>& GetConstraints() const override
const override REQUIRES(parent_->mutex_); REQUIRES(parent_->mutex_);
void MarkSubmapAsTrimmed(const SubmapId& submap_id) void MarkSubmapAsTrimmed(const SubmapId& submap_id)
REQUIRES(parent_->mutex_) override; REQUIRES(parent_->mutex_) override;
bool IsFinished(int trajectory_id) const override REQUIRES(parent_->mutex_); bool IsFinished(int trajectory_id) const override REQUIRES(parent_->mutex_);

8
cartographer/mapping/internal/3d/pose_graph/landmark_cost_function_3d.h
View File

@ -39,8 +39,8 @@ class LandmarkCostFunction3D {
PoseGraphInterface::LandmarkNode::LandmarkObservation; PoseGraphInterface::LandmarkNode::LandmarkObservation;
static ceres::CostFunction* CreateAutoDiffCostFunction( static ceres::CostFunction* CreateAutoDiffCostFunction(
const LandmarkObservation& observation, const NodeData3D& prev_node, const LandmarkObservation& observation, const NodeSpec3D& prev_node,
const NodeData3D& next_node) { const NodeSpec3D& next_node) {
return new ceres::AutoDiffCostFunction< return new ceres::AutoDiffCostFunction<
LandmarkCostFunction3D, 6 /* residuals */, LandmarkCostFunction3D, 6 /* residuals */,
4 /* previous node rotation variables */, 4 /* previous node rotation variables */,
@ -76,8 +76,8 @@ class LandmarkCostFunction3D {
private: private:
LandmarkCostFunction3D(const LandmarkObservation& observation, LandmarkCostFunction3D(const LandmarkObservation& observation,
const NodeData3D& prev_node, const NodeSpec3D& prev_node,
const NodeData3D& next_node) const NodeSpec3D& next_node)
: landmark_to_tracking_transform_( : landmark_to_tracking_transform_(
observation.landmark_to_tracking_transform), observation.landmark_to_tracking_transform),
translation_weight_(observation.translation_weight), translation_weight_(observation.translation_weight),

4
cartographer/mapping/internal/3d/pose_graph/landmark_cost_function_3d_test.cc
View File

@ -34,9 +34,9 @@ using LandmarkObservation =
PoseGraphInterface::LandmarkNode::LandmarkObservation; PoseGraphInterface::LandmarkNode::LandmarkObservation;
TEST(LandmarkCostFunction3DTest, SmokeTest) { TEST(LandmarkCostFunction3DTest, SmokeTest) {
NodeData3D prev_node; NodeSpec3D prev_node;
prev_node.time = common::FromUniversal(0); prev_node.time = common::FromUniversal(0);
NodeData3D next_node; NodeSpec3D next_node;
next_node.time = common::FromUniversal(10); next_node.time = common::FromUniversal(10);
std::unique_ptr<ceres::CostFunction> cost_function( std::unique_ptr<ceres::CostFunction> cost_function(

30
cartographer/mapping/internal/3d/pose_graph/optimization_problem_3d.cc
View File

@ -52,8 +52,6 @@ namespace {
using LandmarkNode = ::cartographer::mapping::PoseGraphInterface::LandmarkNode; using LandmarkNode = ::cartographer::mapping::PoseGraphInterface::LandmarkNode;
using TrajectoryData = using TrajectoryData =
::cartographer::mapping::PoseGraphInterface::TrajectoryData; ::cartographer::mapping::PoseGraphInterface::TrajectoryData;
using NodeData = NodeData3D;
using SubmapData = SubmapData3D;
// For odometry. // For odometry.
std::unique_ptr<transform::Rigid3d> Interpolate( std::unique_ptr<transform::Rigid3d> Interpolate(
@ -107,7 +105,7 @@ std::unique_ptr<transform::Rigid3d> Interpolate(
// applies a relative transform from it. // applies a relative transform from it.
transform::Rigid3d GetInitialLandmarkPose( transform::Rigid3d GetInitialLandmarkPose(
const LandmarkNode::LandmarkObservation& observation, const LandmarkNode::LandmarkObservation& observation,
const NodeData& prev_node, const NodeData& next_node, const NodeSpec3D& prev_node, const NodeSpec3D& next_node,
const CeresPose& prev_node_pose, const CeresPose& next_node_pose) { const CeresPose& prev_node_pose, const CeresPose& next_node_pose) {
const double interpolation_parameter = const double interpolation_parameter =
common::ToSeconds(observation.time - prev_node.time) / common::ToSeconds(observation.time - prev_node.time) /
@ -125,7 +123,7 @@ transform::Rigid3d GetInitialLandmarkPose(
void AddLandmarkCostFunctions( void AddLandmarkCostFunctions(
const std::map<std::string, LandmarkNode>& landmark_nodes, const std::map<std::string, LandmarkNode>& landmark_nodes,
bool freeze_landmarks, const MapById<NodeId, NodeData>& node_data, bool freeze_landmarks, const MapById<NodeId, NodeSpec3D>& node_data,
MapById<NodeId, CeresPose>* C_nodes, MapById<NodeId, CeresPose>* C_nodes,
std::map<std::string, CeresPose>* C_landmarks, ceres::Problem* problem) { std::map<std::string, CeresPose>* C_landmarks, ceres::Problem* problem) {
for (const auto& landmark_node : landmark_nodes) { for (const auto& landmark_node : landmark_nodes) {
@ -212,7 +210,7 @@ void OptimizationProblem3D::AddFixedFramePoseData(
} }
void OptimizationProblem3D::AddTrajectoryNode(const int trajectory_id, void OptimizationProblem3D::AddTrajectoryNode(const int trajectory_id,
const NodeData& node_data) { const NodeSpec3D& node_data) {
node_data_.Append(trajectory_id, node_data); node_data_.Append(trajectory_id, node_data);
trajectory_data_[trajectory_id]; trajectory_data_[trajectory_id];
} }
@ -223,7 +221,7 @@ void OptimizationProblem3D::SetTrajectoryData(
} }
void OptimizationProblem3D::InsertTrajectoryNode(const NodeId& node_id, void OptimizationProblem3D::InsertTrajectoryNode(const NodeId& node_id,
const NodeData& node_data) { const NodeSpec3D& node_data) {
node_data_.Insert(node_id, node_data); node_data_.Insert(node_id, node_data);
trajectory_data_[node_id.trajectory_id]; trajectory_data_[node_id.trajectory_id];
} }
@ -240,12 +238,12 @@ void OptimizationProblem3D::TrimTrajectoryNode(const NodeId& node_id) {
void OptimizationProblem3D::AddSubmap( void OptimizationProblem3D::AddSubmap(
const int trajectory_id, const transform::Rigid3d& global_submap_pose) { const int trajectory_id, const transform::Rigid3d& global_submap_pose) {
submap_data_.Append(trajectory_id, SubmapData{global_submap_pose}); submap_data_.Append(trajectory_id, SubmapSpec3D{global_submap_pose});
} }
void OptimizationProblem3D::InsertSubmap( void OptimizationProblem3D::InsertSubmap(
const SubmapId& submap_id, const transform::Rigid3d& global_submap_pose) { const SubmapId& submap_id, const transform::Rigid3d& global_submap_pose) {
submap_data_.Insert(submap_id, SubmapData{global_submap_pose}); submap_data_.Insert(submap_id, SubmapSpec3D{global_submap_pose});
} }
void OptimizationProblem3D::TrimSubmap(const SubmapId& submap_id) { void OptimizationProblem3D::TrimSubmap(const SubmapId& submap_id) {
@ -370,10 +368,10 @@ void OptimizationProblem3D::Solve(
auto prev_node_it = node_it; auto prev_node_it = node_it;
for (++node_it; node_it != trajectory_end; ++node_it) { for (++node_it; node_it != trajectory_end; ++node_it) {
const NodeId first_node_id = prev_node_it->id; const NodeId first_node_id = prev_node_it->id;
const NodeData& first_node_data = prev_node_it->data; const NodeSpec3D& first_node_data = prev_node_it->data;
prev_node_it = node_it; prev_node_it = node_it;
const NodeId second_node_id = node_it->id; const NodeId second_node_id = node_it->id;
const NodeData& second_node_data = node_it->data; const NodeSpec3D& second_node_data = node_it->data;
if (second_node_id.node_index != first_node_id.node_index + 1) { if (second_node_id.node_index != first_node_id.node_index + 1) {
continue; continue;
@ -392,7 +390,7 @@ void OptimizationProblem3D::Solve(
if (next_node_it != trajectory_end && if (next_node_it != trajectory_end &&
next_node_it->id.node_index == second_node_id.node_index + 1) { next_node_it->id.node_index == second_node_id.node_index + 1) {
const NodeId third_node_id = next_node_it->id; const NodeId third_node_id = next_node_it->id;
const NodeData& third_node_data = next_node_it->data; const NodeSpec3D& third_node_data = next_node_it->data;
const common::Time first_time = first_node_data.time; const common::Time first_time = first_node_data.time;
const common::Time second_time = second_node_data.time; const common::Time second_time = second_node_data.time;
const common::Time third_time = third_node_data.time; const common::Time third_time = third_node_data.time;
@ -450,10 +448,10 @@ void OptimizationProblem3D::Solve(
auto prev_node_it = node_it; auto prev_node_it = node_it;
for (++node_it; node_it != trajectory_end; ++node_it) { for (++node_it; node_it != trajectory_end; ++node_it) {
const NodeId first_node_id = prev_node_it->id; const NodeId first_node_id = prev_node_it->id;
const NodeData& first_node_data = prev_node_it->data; const NodeSpec3D& first_node_data = prev_node_it->data;
prev_node_it = node_it; prev_node_it = node_it;
const NodeId second_node_id = node_it->id; const NodeId second_node_id = node_it->id;
const NodeData& second_node_data = node_it->data; const NodeSpec3D& second_node_data = node_it->data;
if (second_node_id.node_index != first_node_id.node_index + 1) { if (second_node_id.node_index != first_node_id.node_index + 1) {
continue; continue;
@ -504,7 +502,7 @@ void OptimizationProblem3D::Solve(
bool fixed_frame_pose_initialized = false; bool fixed_frame_pose_initialized = false;
for (; node_it != trajectory_end; ++node_it) { for (; node_it != trajectory_end; ++node_it) {
const NodeId node_id = node_it->id; const NodeId node_id = node_it->id;
const NodeData& node_data = node_it->data; const NodeSpec3D& node_data = node_it->data;
const std::unique_ptr<transform::Rigid3d> fixed_frame_pose = const std::unique_ptr<transform::Rigid3d> fixed_frame_pose =
Interpolate(fixed_frame_pose_data_, trajectory_id, node_data.time); Interpolate(fixed_frame_pose_data_, trajectory_id, node_data.time);
@ -591,8 +589,8 @@ void OptimizationProblem3D::Solve(
std::unique_ptr<transform::Rigid3d> std::unique_ptr<transform::Rigid3d>
OptimizationProblem3D::CalculateOdometryBetweenNodes( OptimizationProblem3D::CalculateOdometryBetweenNodes(
const int trajectory_id, const NodeData& first_node_data, const int trajectory_id, const NodeSpec3D& first_node_data,
const NodeData& second_node_data) const { const NodeSpec3D& second_node_data) const {
if (odometry_data_.HasTrajectory(trajectory_id)) { if (odometry_data_.HasTrajectory(trajectory_id)) {
const std::unique_ptr<transform::Rigid3d> first_node_odometry = const std::unique_ptr<transform::Rigid3d> first_node_odometry =
Interpolate(odometry_data_, trajectory_id, first_node_data.time); Interpolate(odometry_data_, trajectory_id, first_node_data.time);

22
cartographer/mapping/internal/3d/pose_graph/optimization_problem_3d.h
View File

@ -41,18 +41,18 @@ namespace cartographer {
namespace mapping { namespace mapping {
namespace pose_graph { namespace pose_graph {
struct NodeData3D { struct NodeSpec3D {
common::Time time; common::Time time;
transform::Rigid3d local_pose; transform::Rigid3d local_pose;
transform::Rigid3d global_pose; transform::Rigid3d global_pose;
}; };
struct SubmapData3D { struct SubmapSpec3D {
transform::Rigid3d global_pose; transform::Rigid3d global_pose;
}; };
class OptimizationProblem3D class OptimizationProblem3D
: public OptimizationProblemInterface<NodeData3D, SubmapData3D, : public OptimizationProblemInterface<NodeSpec3D, SubmapSpec3D,
transform::Rigid3d> { transform::Rigid3d> {
public: public:
explicit OptimizationProblem3D( explicit OptimizationProblem3D(
@ -66,9 +66,9 @@ class OptimizationProblem3D
void AddOdometryData(int trajectory_id, void AddOdometryData(int trajectory_id,
const sensor::OdometryData& odometry_data) override; const sensor::OdometryData& odometry_data) override;
void AddTrajectoryNode(int trajectory_id, void AddTrajectoryNode(int trajectory_id,
const NodeData3D& node_data) override; const NodeSpec3D& node_data) override;
void InsertTrajectoryNode(const NodeId& node_id, void InsertTrajectoryNode(const NodeId& node_id,
const NodeData3D& node_data) override; const NodeSpec3D& node_data) override;
void TrimTrajectoryNode(const NodeId& node_id) override; void TrimTrajectoryNode(const NodeId& node_id) override;
void AddSubmap(int trajectory_id, void AddSubmap(int trajectory_id,
const transform::Rigid3d& global_submap_pose) override; const transform::Rigid3d& global_submap_pose) override;
@ -82,10 +82,10 @@ class OptimizationProblem3D
const std::set<int>& frozen_trajectories, const std::set<int>& frozen_trajectories,
const std::map<std::string, LandmarkNode>& landmark_nodes) override; const std::map<std::string, LandmarkNode>& landmark_nodes) override;
const MapById<NodeId, NodeData3D>& node_data() const override { const MapById<NodeId, NodeSpec3D>& node_data() const override {
return node_data_; return node_data_;
} }
const MapById<SubmapId, SubmapData3D>& submap_data() const override { const MapById<SubmapId, SubmapSpec3D>& submap_data() const override {
return submap_data_; return submap_data_;
} }
const std::map<std::string, transform::Rigid3d>& landmark_data() const std::map<std::string, transform::Rigid3d>& landmark_data()
@ -118,12 +118,12 @@ class OptimizationProblem3D
private: private:
// Computes the relative pose between two nodes based on odometry data. // Computes the relative pose between two nodes based on odometry data.
std::unique_ptr<transform::Rigid3d> CalculateOdometryBetweenNodes( std::unique_ptr<transform::Rigid3d> CalculateOdometryBetweenNodes(
int trajectory_id, const NodeData3D& first_node_data, int trajectory_id, const NodeSpec3D& first_node_data,
const NodeData3D& second_node_data) const; const NodeSpec3D& second_node_data) const;
pose_graph::proto::OptimizationProblemOptions options_; pose_graph::proto::OptimizationProblemOptions options_;
MapById<NodeId, NodeData3D> node_data_; MapById<NodeId, NodeSpec3D> node_data_;
MapById<SubmapId, SubmapData3D> submap_data_; MapById<SubmapId, SubmapSpec3D> submap_data_;
std::map<std::string, transform::Rigid3d> landmark_data_; std::map<std::string, transform::Rigid3d> landmark_data_;
sensor::MapByTime<sensor::ImuData> imu_data_; sensor::MapByTime<sensor::ImuData> imu_data_;
sensor::MapByTime<sensor::OdometryData> odometry_data_; sensor::MapByTime<sensor::OdometryData> odometry_data_;

2
cartographer/mapping/internal/3d/pose_graph/optimization_problem_3d_test.cc
View File

@ -128,7 +128,7 @@ TEST_F(OptimizationProblem3DTest, ReducesNoise) {
kTrajectoryId, sensor::ImuData{now, Eigen::Vector3d::UnitZ() * 9.81, kTrajectoryId, sensor::ImuData{now, Eigen::Vector3d::UnitZ() * 9.81,
Eigen::Vector3d::Zero()}); Eigen::Vector3d::Zero()});
optimization_problem_.AddTrajectoryNode(kTrajectoryId, optimization_problem_.AddTrajectoryNode(kTrajectoryId,
NodeData3D{now, pose, pose}); NodeSpec3D{now, pose, pose});
now += common::FromSeconds(0.01); now += common::FromSeconds(0.01);
} }

35
cartographer/mapping/internal/3d/pose_graph_3d.cc
View File

@ -117,7 +117,8 @@ NodeId PoseGraph3D::AddNode(
insertion_submaps.back()) { insertion_submaps.back()) {
// We grow 'submap_data_' as needed. This code assumes that the first // We grow 'submap_data_' as needed. This code assumes that the first
// time we see a new submap is as 'insertion_submaps.back()'. // time we see a new submap is as 'insertion_submaps.back()'.
const SubmapId submap_id = submap_data_.Append(trajectory_id, SubmapData()); const SubmapId submap_id =
submap_data_.Append(trajectory_id, InternalSubmapData());
submap_data_.at(submap_id).submap = insertion_submaps.back(); submap_data_.at(submap_id).submap = insertion_submaps.back();
} }
@ -182,7 +183,7 @@ void PoseGraph3D::AddLandmarkData(int trajectory_id,
AddWorkItem([=]() REQUIRES(mutex_) { AddWorkItem([=]() REQUIRES(mutex_) {
for (const auto& observation : landmark_data.landmark_observations) { for (const auto& observation : landmark_data.landmark_observations) {
landmark_nodes_[observation.id].landmark_observations.emplace_back( landmark_nodes_[observation.id].landmark_observations.emplace_back(
PoseGraph::LandmarkNode::LandmarkObservation{ PoseGraphInterface::LandmarkNode::LandmarkObservation{
trajectory_id, landmark_data.time, trajectory_id, landmark_data.time,
observation.landmark_to_tracking_transform, observation.landmark_to_tracking_transform,
observation.translation_weight, observation.rotation_weight}); observation.translation_weight, observation.rotation_weight});
@ -266,7 +267,7 @@ void PoseGraph3D::ComputeConstraintsForNode(
insertion_submaps.front()->local_pose().inverse() * local_pose; insertion_submaps.front()->local_pose().inverse() * local_pose;
optimization_problem_->AddTrajectoryNode( optimization_problem_->AddTrajectoryNode(
matching_id.trajectory_id, matching_id.trajectory_id,
pose_graph::NodeData3D{constant_data->time, local_pose, global_pose}); pose_graph::NodeSpec3D{constant_data->time, local_pose, global_pose});
for (size_t i = 0; i < insertion_submaps.size(); ++i) { for (size_t i = 0; i < insertion_submaps.size(); ++i) {
const SubmapId submap_id = submap_ids[i]; const SubmapId submap_id = submap_ids[i];
// Even if this was the last node added to 'submap_id', the submap will only // Even if this was the last node added to 'submap_id', the submap will only
@ -292,7 +293,8 @@ void PoseGraph3D::ComputeConstraintsForNode(
if (newly_finished_submap) { if (newly_finished_submap) {
const SubmapId finished_submap_id = submap_ids.front(); const SubmapId finished_submap_id = submap_ids.front();
SubmapData& finished_submap_data = submap_data_.at(finished_submap_id); InternalSubmapData& finished_submap_data =
submap_data_.at(finished_submap_id);
CHECK(finished_submap_data.state == SubmapState::kActive); CHECK(finished_submap_data.state == SubmapState::kActive);
finished_submap_data.state = SubmapState::kFinished; finished_submap_data.state = SubmapState::kFinished;
// We have a new completed submap, so we look into adding constraints for // We have a new completed submap, so we look into adding constraints for
@ -320,7 +322,7 @@ void PoseGraph3D::DispatchOptimization() {
common::Time PoseGraph3D::GetLatestNodeTime(const NodeId& node_id, common::Time PoseGraph3D::GetLatestNodeTime(const NodeId& node_id,
const SubmapId& submap_id) const { const SubmapId& submap_id) const {
common::Time time = trajectory_nodes_.at(node_id).constant_data->time; common::Time time = trajectory_nodes_.at(node_id).constant_data->time;
const SubmapData& submap_data = submap_data_.at(submap_id); const InternalSubmapData& submap_data = submap_data_.at(submap_id);
if (!submap_data.node_ids.empty()) { if (!submap_data.node_ids.empty()) {
const NodeId last_submap_node_id = const NodeId last_submap_node_id =
*submap_data_.at(submap_id).node_ids.rbegin(); *submap_data_.at(submap_id).node_ids.rbegin();
@ -331,7 +333,7 @@ common::Time PoseGraph3D::GetLatestNodeTime(const NodeId& node_id,
} }
void PoseGraph3D::UpdateTrajectoryConnectivity(const Constraint& constraint) { void PoseGraph3D::UpdateTrajectoryConnectivity(const Constraint& constraint) {
CHECK_EQ(constraint.tag, PoseGraph::Constraint::INTER_SUBMAP); CHECK_EQ(constraint.tag, PoseGraphInterface::Constraint::INTER_SUBMAP);
const common::Time time = const common::Time time =
GetLatestNodeTime(constraint.node_id, constraint.submap_id); GetLatestNodeTime(constraint.node_id, constraint.submap_id);
trajectory_connectivity_state_.Connect(constraint.node_id.trajectory_id, trajectory_connectivity_state_.Connect(constraint.node_id.trajectory_id,
@ -455,11 +457,11 @@ void PoseGraph3D::AddSubmapFromProto(
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
AddTrajectoryIfNeeded(submap_id.trajectory_id); AddTrajectoryIfNeeded(submap_id.trajectory_id);
submap_data_.Insert(submap_id, SubmapData()); submap_data_.Insert(submap_id, InternalSubmapData());
submap_data_.at(submap_id).submap = submap_ptr; submap_data_.at(submap_id).submap = submap_ptr;
// Immediately show the submap at the 'global_submap_pose'. // Immediately show the submap at the 'global_submap_pose'.
global_submap_poses_.Insert(submap_id, global_submap_poses_.Insert(submap_id,
pose_graph::SubmapData3D{global_submap_pose}); pose_graph::SubmapSpec3D{global_submap_pose});
AddWorkItem([this, submap_id, global_submap_pose]() REQUIRES(mutex_) { AddWorkItem([this, submap_id, global_submap_pose]() REQUIRES(mutex_) {
submap_data_.at(submap_id).state = SubmapState::kFinished; submap_data_.at(submap_id).state = SubmapState::kFinished;
optimization_problem_->InsertSubmap(submap_id, global_submap_pose); optimization_problem_->InsertSubmap(submap_id, global_submap_pose);
@ -481,7 +483,7 @@ void PoseGraph3D::AddNodeFromProto(const transform::Rigid3d& global_pose,
const auto& constant_data = trajectory_nodes_.at(node_id).constant_data; const auto& constant_data = trajectory_nodes_.at(node_id).constant_data;
optimization_problem_->InsertTrajectoryNode( optimization_problem_->InsertTrajectoryNode(
node_id, node_id,
pose_graph::NodeData3D{constant_data->time, constant_data->local_pose, pose_graph::NodeSpec3D{constant_data->time, constant_data->local_pose,
global_pose}); global_pose});
}); });
} }
@ -693,7 +695,7 @@ PoseGraph3D::GetFixedFramePoseData() {
return optimization_problem_->fixed_frame_pose_data(); return optimization_problem_->fixed_frame_pose_data();
} }
std::map<std::string /* landmark ID */, PoseGraph::LandmarkNode> std::map<std::string /* landmark ID */, PoseGraphInterface::LandmarkNode>
PoseGraph3D::GetLandmarkNodes() { PoseGraph3D::GetLandmarkNodes() {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
return landmark_nodes_; return landmark_nodes_;
@ -705,7 +707,7 @@ PoseGraph3D::GetTrajectoryData() {
return optimization_problem_->trajectory_data(); return optimization_problem_->trajectory_data();
} }
std::vector<PoseGraph::Constraint> PoseGraph3D::constraints() { std::vector<PoseGraphInterface::Constraint> PoseGraph3D::constraints() {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
return constraints_; return constraints_;
} }
@ -749,7 +751,8 @@ std::vector<std::vector<int>> PoseGraph3D::GetConnectedTrajectories() {
return trajectory_connectivity_state_.Components(); return trajectory_connectivity_state_.Components();
} }
PoseGraph::SubmapData PoseGraph3D::GetSubmapData(const SubmapId& submap_id) { PoseGraphInterface::SubmapData PoseGraph3D::GetSubmapData(
const SubmapId& submap_id) {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
return GetSubmapDataUnderLock(submap_id); return GetSubmapDataUnderLock(submap_id);
} }
@ -768,14 +771,14 @@ PoseGraph3D::GetAllSubmapPoses() {
auto submap_data = GetSubmapDataUnderLock(submap_id_data.id); auto submap_data = GetSubmapDataUnderLock(submap_id_data.id);
submap_poses.Insert( submap_poses.Insert(
submap_id_data.id, submap_id_data.id,
PoseGraph::SubmapPose{submap_data.submap->num_range_data(), PoseGraphInterface::SubmapPose{submap_data.submap->num_range_data(),
submap_data.pose}); submap_data.pose});
} }
return submap_poses; return submap_poses;
} }
transform::Rigid3d PoseGraph3D::ComputeLocalToGlobalTransform( transform::Rigid3d PoseGraph3D::ComputeLocalToGlobalTransform(
const MapById<SubmapId, pose_graph::SubmapData3D>& global_submap_poses, const MapById<SubmapId, pose_graph::SubmapSpec3D>& global_submap_poses,
const int trajectory_id) const { const int trajectory_id) const {
auto begin_it = global_submap_poses.BeginOfTrajectory(trajectory_id); auto begin_it = global_submap_poses.BeginOfTrajectory(trajectory_id);
auto end_it = global_submap_poses.EndOfTrajectory(trajectory_id); auto end_it = global_submap_poses.EndOfTrajectory(trajectory_id);
@ -797,7 +800,7 @@ transform::Rigid3d PoseGraph3D::ComputeLocalToGlobalTransform(
.inverse(); .inverse();
} }
PoseGraph::SubmapData PoseGraph3D::GetSubmapDataUnderLock( PoseGraphInterface::SubmapData PoseGraph3D::GetSubmapDataUnderLock(
const SubmapId& submap_id) { const SubmapId& submap_id) {
const auto it = submap_data_.find(submap_id); const auto it = submap_data_.find(submap_id);
if (it == submap_data_.end()) { if (it == submap_data_.end()) {

22
cartographer/mapping/internal/3d/pose_graph_3d.h
View File

@ -109,8 +109,7 @@ class PoseGraph3D : public PoseGraph {
std::vector<std::vector<int>> GetConnectedTrajectories() override; std::vector<std::vector<int>> GetConnectedTrajectories() override;
PoseGraph::SubmapData GetSubmapData(const SubmapId& submap_id) PoseGraph::SubmapData GetSubmapData(const SubmapId& submap_id)
EXCLUDES(mutex_) override; EXCLUDES(mutex_) override;
MapById<SubmapId, PoseGraphInterface::SubmapData> GetAllSubmapData() MapById<SubmapId, SubmapData> GetAllSubmapData() EXCLUDES(mutex_) override;
EXCLUDES(mutex_) override;
MapById<SubmapId, SubmapPose> GetAllSubmapPoses() EXCLUDES(mutex_) override; MapById<SubmapId, SubmapPose> GetAllSubmapPoses() EXCLUDES(mutex_) override;
transform::Rigid3d GetLocalToGlobalTransform(int trajectory_id) transform::Rigid3d GetLocalToGlobalTransform(int trajectory_id)
EXCLUDES(mutex_) override; EXCLUDES(mutex_) override;
@ -150,7 +149,7 @@ class PoseGraph3D : public PoseGraph {
// transitions to kFinished, all nodes are tried to match against this submap. // transitions to kFinished, all nodes are tried to match against this submap.
// Likewise, all new nodes are matched against submaps which are finished. // Likewise, all new nodes are matched against submaps which are finished.
enum class SubmapState { kActive, kFinished }; enum class SubmapState { kActive, kFinished };
struct SubmapData { struct InternalSubmapData {
std::shared_ptr<const Submap3D> submap; std::shared_ptr<const Submap3D> submap;
// IDs of the nodes that were inserted into this map together with // IDs of the nodes that were inserted into this map together with
@ -161,8 +160,7 @@ class PoseGraph3D : public PoseGraph {
SubmapState state = SubmapState::kActive; SubmapState state = SubmapState::kActive;
}; };
MapById<SubmapId, PoseGraphInterface::SubmapData> GetSubmapDataUnderLock() MapById<SubmapId, SubmapData> GetSubmapDataUnderLock() REQUIRES(mutex_);
REQUIRES(mutex_);
// Handles a new work item. // Handles a new work item.
void AddWorkItem(const std::function<void()>& work_item) REQUIRES(mutex_); void AddWorkItem(const std::function<void()>& work_item) REQUIRES(mutex_);
@ -202,7 +200,7 @@ class PoseGraph3D : public PoseGraph {
// Computes the local to global map frame transform based on the given // Computes the local to global map frame transform based on the given
// 'global_submap_poses'. // 'global_submap_poses'.
transform::Rigid3d ComputeLocalToGlobalTransform( transform::Rigid3d ComputeLocalToGlobalTransform(
const MapById<SubmapId, pose_graph::SubmapData3D>& global_submap_poses, const MapById<SubmapId, pose_graph::SubmapSpec3D>& global_submap_poses,
int trajectory_id) const REQUIRES(mutex_); int trajectory_id) const REQUIRES(mutex_);
PoseGraph::SubmapData GetSubmapDataUnderLock(const SubmapId& submap_id) PoseGraph::SubmapData GetSubmapDataUnderLock(const SubmapId& submap_id)
@ -251,14 +249,14 @@ class PoseGraph3D : public PoseGraph {
// Submaps get assigned an ID and state as soon as they are seen, even // Submaps get assigned an ID and state as soon as they are seen, even
// before they take part in the background computations. // before they take part in the background computations.
MapById<SubmapId, SubmapData> submap_data_ GUARDED_BY(mutex_); MapById<SubmapId, InternalSubmapData> submap_data_ GUARDED_BY(mutex_);
// Data that are currently being shown. // Data that are currently being shown.
MapById<NodeId, TrajectoryNode> trajectory_nodes_ GUARDED_BY(mutex_); MapById<NodeId, TrajectoryNode> trajectory_nodes_ GUARDED_BY(mutex_);
int num_trajectory_nodes_ GUARDED_BY(mutex_) = 0; int num_trajectory_nodes_ GUARDED_BY(mutex_) = 0;
// Global submap poses currently used for displaying data. // Global submap poses currently used for displaying data.
MapById<SubmapId, pose_graph::SubmapData3D> global_submap_poses_ MapById<SubmapId, pose_graph::SubmapSpec3D> global_submap_poses_
GUARDED_BY(mutex_); GUARDED_BY(mutex_);
// Global landmark poses with all observations. // Global landmark poses with all observations.
@ -287,12 +285,12 @@ class PoseGraph3D : public PoseGraph {
int num_submaps(int trajectory_id) const override; int num_submaps(int trajectory_id) const override;
std::vector<SubmapId> GetSubmapIds(int trajectory_id) const override; std::vector<SubmapId> GetSubmapIds(int trajectory_id) const override;
MapById<SubmapId, PoseGraphInterface::SubmapData> GetAllSubmapData() MapById<SubmapId, SubmapData> GetAllSubmapData() const override
const override REQUIRES(parent_->mutex_); REQUIRES(parent_->mutex_);
const MapById<NodeId, TrajectoryNode>& GetTrajectoryNodes() const override const MapById<NodeId, TrajectoryNode>& GetTrajectoryNodes() const override
REQUIRES(parent_->mutex_); REQUIRES(parent_->mutex_);
const std::vector<PoseGraphInterface::Constraint>& GetConstraints() const std::vector<Constraint>& GetConstraints() const override
const override REQUIRES(parent_->mutex_); REQUIRES(parent_->mutex_);
void MarkSubmapAsTrimmed(const SubmapId& submap_id) void MarkSubmapAsTrimmed(const SubmapId& submap_id)
REQUIRES(parent_->mutex_) override; REQUIRES(parent_->mutex_) override;
bool IsFinished(int trajectory_id) const override REQUIRES(parent_->mutex_); bool IsFinished(int trajectory_id) const override REQUIRES(parent_->mutex_);