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;
static ceres::CostFunction* CreateAutoDiffCostFunction(
const LandmarkObservation& observation, const NodeData2D& prev_node,
const NodeData2D& next_node) {
const LandmarkObservation& observation, const NodeSpec2D& prev_node,
const NodeSpec2D& next_node) {
return new ceres::AutoDiffCostFunction<
LandmarkCostFunction2D, 6 /* residuals */,
3 /* previous node pose variables */, 3 /* next node pose variables */,
@ -71,8 +71,8 @@ class LandmarkCostFunction2D {
private:
LandmarkCostFunction2D(const LandmarkObservation& observation,
const NodeData2D& prev_node,
const NodeData2D& next_node)
const NodeSpec2D& prev_node,
const NodeSpec2D& next_node)
: landmark_to_tracking_transform_(
observation.landmark_to_tracking_transform),
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;
TEST(LandmarkCostFunctionTest, SmokeTest) {
NodeData2D prev_node;
NodeSpec2D prev_node;
prev_node.time = common::FromUniversal(0);
prev_node.gravity_alignment = Eigen::Quaterniond::Identity();
NodeData2D next_node;
NodeSpec2D next_node;
next_node.time = common::FromUniversal(10);
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 LandmarkNode = ::cartographer::mapping::PoseGraphInterface::LandmarkNode;
using NodeData = NodeData2D;
using SubmapData = SubmapData2D;
// Converts a pose into the 3 optimization variable format used for Ceres:
// 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.
transform::Rigid3d GetInitialLandmarkPose(
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>& next_node_pose) {
const double interpolation_parameter =
@ -82,7 +80,7 @@ transform::Rigid3d GetInitialLandmarkPose(
void AddLandmarkCostFunctions(
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,
std::map<std::string, CeresPose>* C_landmarks, ceres::Problem* problem) {
for (const auto& landmark_node : landmark_nodes) {
@ -145,7 +143,7 @@ void AddLandmarkCostFunctions(
} // namespace
OptimizationProblem2D::OptimizationProblem2D(
const pose_graph::proto::OptimizationProblemOptions& options)
const proto::OptimizationProblemOptions& options)
: options_(options) {}
OptimizationProblem2D::~OptimizationProblem2D() {}
@ -161,12 +159,12 @@ void OptimizationProblem2D::AddOdometryData(
}
void OptimizationProblem2D::AddTrajectoryNode(const int trajectory_id,
const NodeData& node_data) {
const NodeSpec2D& node_data) {
node_data_.Append(trajectory_id, node_data);
}
void OptimizationProblem2D::InsertTrajectoryNode(const NodeId& node_id,
const NodeData& node_data) {
const NodeSpec2D& node_data) {
node_data_.Insert(node_id, node_data);
}
@ -178,12 +176,12 @@ void OptimizationProblem2D::TrimTrajectoryNode(const NodeId& node_id) {
void OptimizationProblem2D::AddSubmap(
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(
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) {
@ -209,7 +207,7 @@ void OptimizationProblem2D::Solve(
ceres::Problem problem(problem_options);
// 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<NodeId, std::array<double, 3>> C_nodes;
std::map<std::string, CeresPose> C_landmarks;
@ -264,10 +262,10 @@ void OptimizationProblem2D::Solve(
auto prev_node_it = node_it;
for (++node_it; node_it != trajectory_end; ++node_it) {
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;
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) {
continue;
@ -344,8 +342,8 @@ std::unique_ptr<transform::Rigid3d> OptimizationProblem2D::InterpolateOdometry(
std::unique_ptr<transform::Rigid3d>
OptimizationProblem2D::CalculateOdometryBetweenNodes(
const int trajectory_id, const NodeData& first_node_data,
const NodeData& second_node_data) const {
const int trajectory_id, const NodeSpec2D& first_node_data,
const NodeSpec2D& second_node_data) const {
if (odometry_data_.HasTrajectory(trajectory_id)) {
const std::unique_ptr<transform::Rigid3d> first_node_odometry =
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 pose_graph {
struct NodeData2D {
struct NodeSpec2D {
common::Time time;
transform::Rigid2d local_pose_2d;
transform::Rigid2d global_pose_2d;
Eigen::Quaterniond gravity_alignment;
};
struct SubmapData2D {
struct SubmapSpec2D {
transform::Rigid2d global_pose;
};
class OptimizationProblem2D
: public OptimizationProblemInterface<NodeData2D, SubmapData2D,
: public OptimizationProblemInterface<NodeSpec2D, SubmapSpec2D,
transform::Rigid2d> {
public:
explicit OptimizationProblem2D(
@ -66,9 +66,9 @@ class OptimizationProblem2D
void AddOdometryData(int trajectory_id,
const sensor::OdometryData& odometry_data) override;
void AddTrajectoryNode(int trajectory_id,
const NodeData2D& node_data) override;
const NodeSpec2D& node_data) override;
void InsertTrajectoryNode(const NodeId& node_id,
const NodeData2D& node_data) override;
const NodeSpec2D& node_data) override;
void TrimTrajectoryNode(const NodeId& node_id) override;
void AddSubmap(int trajectory_id,
const transform::Rigid2d& global_submap_pose) override;
@ -82,10 +82,10 @@ class OptimizationProblem2D
const std::set<int>& frozen_trajectories,
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_;
}
const MapById<SubmapId, SubmapData2D>& submap_data() const override {
const MapById<SubmapId, SubmapSpec2D>& submap_data() const override {
return submap_data_;
}
const std::map<std::string, transform::Rigid3d>& landmark_data()
@ -105,12 +105,12 @@ class OptimizationProblem2D
int trajectory_id, common::Time time) const;
// Computes the relative pose between two nodes based on odometry data.
std::unique_ptr<transform::Rigid3d> CalculateOdometryBetweenNodes(
int trajectory_id, const NodeData2D& first_node_data,
const NodeData2D& second_node_data) const;
int trajectory_id, const NodeSpec2D& first_node_data,
const NodeSpec2D& second_node_data) const;
pose_graph::proto::OptimizationProblemOptions options_;
MapById<NodeId, NodeData2D> node_data_;
MapById<SubmapId, SubmapData2D> submap_data_;
MapById<NodeId, NodeSpec2D> node_data_;
MapById<SubmapId, SubmapSpec2D> submap_data_;
std::map<std::string, transform::Rigid3d> landmark_data_;
sensor::MapByTime<sensor::ImuData> imu_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()) {
// We grow 'submap_data_' as needed. This code assumes that the first
// 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();
}
@ -180,7 +181,7 @@ void PoseGraph2D::AddLandmarkData(int trajectory_id,
AddWorkItem([=]() REQUIRES(mutex_) {
for (const auto& observation : landmark_data.landmark_observations) {
landmark_nodes_[observation.id].landmark_observations.emplace_back(
PoseGraph::LandmarkNode::LandmarkObservation{
LandmarkNode::LandmarkObservation{
trajectory_id, landmark_data.time,
observation.landmark_to_tracking_transform,
observation.translation_weight, observation.rotation_weight});
@ -249,7 +250,7 @@ void PoseGraph2D::ComputeConstraintsForNode(
local_pose_2d;
optimization_problem_->AddTrajectoryNode(
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});
for (size_t i = 0; i < insertion_submaps.size(); ++i) {
const SubmapId submap_id = submap_ids[i];
@ -277,7 +278,8 @@ void PoseGraph2D::ComputeConstraintsForNode(
if (newly_finished_submap) {
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);
finished_submap_data.state = SubmapState::kFinished;
// 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,
const SubmapId& submap_id) const {
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()) {
const NodeId last_submap_node_id =
*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) {
CHECK_EQ(constraint.tag, PoseGraph::Constraint::INTER_SUBMAP);
CHECK_EQ(constraint.tag, Constraint::INTER_SUBMAP);
const common::Time time =
GetLatestNodeTime(constraint.node_id, constraint.submap_id);
trajectory_connectivity_state_.Connect(constraint.node_id.trajectory_id,
@ -441,11 +443,11 @@ void PoseGraph2D::AddSubmapFromProto(
common::MutexLocker locker(&mutex_);
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;
// Immediately show the submap at the 'global_submap_pose'.
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_) {
submap_data_.at(submap_id).state = SubmapState::kFinished;
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());
optimization_problem_->InsertTrajectoryNode(
node_id,
pose_graph::NodeData2D{
pose_graph::NodeSpec2D{
constant_data->time,
transform::Project2D(constant_data->local_pose *
gravity_alignment_inverse),
@ -645,7 +647,7 @@ sensor::MapByTime<sensor::OdometryData> PoseGraph2D::GetOdometryData() {
return optimization_problem_->odometry_data();
}
std::map<std::string /* landmark ID */, PoseGraph::LandmarkNode>
std::map<std::string /* landmark ID */, PoseGraphInterface::LandmarkNode>
PoseGraph2D::GetLandmarkNodes() {
common::MutexLocker locker(&mutex_);
return landmark_nodes_;
@ -661,8 +663,8 @@ PoseGraph2D::GetFixedFramePoseData() {
return {}; // Not implemented yet in 2D.
}
std::vector<PoseGraph2D::Constraint> PoseGraph2D::constraints() {
std::vector<Constraint> result;
std::vector<PoseGraphInterface::Constraint> PoseGraph2D::constraints() {
std::vector<PoseGraphInterface::Constraint> result;
common::MutexLocker locker(&mutex_);
for (const Constraint& constraint : constraints_) {
result.push_back(Constraint{
@ -717,7 +719,8 @@ std::vector<std::vector<int>> PoseGraph2D::GetConnectedTrajectories() {
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_);
return GetSubmapDataUnderLock(submap_id);
}
@ -743,7 +746,7 @@ PoseGraph2D::GetAllSubmapPoses() {
}
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 {
auto begin_it = global_submap_poses.BeginOfTrajectory(trajectory_id);
auto end_it = global_submap_poses.EndOfTrajectory(trajectory_id);
@ -766,7 +769,7 @@ transform::Rigid3d PoseGraph2D::ComputeLocalToGlobalTransform(
.inverse();
}
PoseGraph::SubmapData PoseGraph2D::GetSubmapDataUnderLock(
PoseGraphInterface::SubmapData PoseGraph2D::GetSubmapDataUnderLock(
const SubmapId& submap_id) {
const auto it = submap_data_.find(submap_id);
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 RunFinalOptimization() 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;
MapById<SubmapId, PoseGraphInterface::SubmapData> GetAllSubmapData()
EXCLUDES(mutex_) override;
@ -145,7 +145,7 @@ class PoseGraph2D : public PoseGraph {
// transitions to kFinished, all nodes are tried to match against this submap.
// Likewise, all new nodes are matched against submaps which are finished.
enum class SubmapState { kActive, kFinished };
struct SubmapData {
struct InternalSubmapData {
std::shared_ptr<const Submap2D> submap;
// 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
// 'global_submap_poses'.
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_);
PoseGraph::SubmapData GetSubmapDataUnderLock(const SubmapId& submap_id)
REQUIRES(mutex_);
SubmapData GetSubmapDataUnderLock(const SubmapId& submap_id) REQUIRES(mutex_);
common::Time GetLatestNodeTime(const NodeId& node_id,
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
// 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.
MapById<NodeId, TrajectoryNode> trajectory_nodes_ GUARDED_BY(mutex_);
int num_trajectory_nodes_ GUARDED_BY(mutex_) = 0;
// 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_);
// Global landmark poses with all observations.
@ -282,12 +281,12 @@ class PoseGraph2D : public PoseGraph {
int num_submaps(int trajectory_id) const override;
std::vector<SubmapId> GetSubmapIds(int trajectory_id) const override;
MapById<SubmapId, PoseGraphInterface::SubmapData> GetAllSubmapData()
const override REQUIRES(parent_->mutex_);
MapById<SubmapId, SubmapData> GetAllSubmapData() const override
REQUIRES(parent_->mutex_);
const MapById<NodeId, TrajectoryNode>& GetTrajectoryNodes() const override
REQUIRES(parent_->mutex_);
const std::vector<PoseGraphInterface::Constraint>& GetConstraints()
const override REQUIRES(parent_->mutex_);
const std::vector<Constraint>& GetConstraints() const override
REQUIRES(parent_->mutex_);
void MarkSubmapAsTrimmed(const SubmapId& submap_id)
REQUIRES(parent_->mutex_) override;
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;
static ceres::CostFunction* CreateAutoDiffCostFunction(
const LandmarkObservation& observation, const NodeData3D& prev_node,
const NodeData3D& next_node) {
const LandmarkObservation& observation, const NodeSpec3D& prev_node,
const NodeSpec3D& next_node) {
return new ceres::AutoDiffCostFunction<
LandmarkCostFunction3D, 6 /* residuals */,
4 /* previous node rotation variables */,
@ -76,8 +76,8 @@ class LandmarkCostFunction3D {
private:
LandmarkCostFunction3D(const LandmarkObservation& observation,
const NodeData3D& prev_node,
const NodeData3D& next_node)
const NodeSpec3D& prev_node,
const NodeSpec3D& next_node)
: landmark_to_tracking_transform_(
observation.landmark_to_tracking_transform),
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;
TEST(LandmarkCostFunction3DTest, SmokeTest) {
NodeData3D prev_node;
NodeSpec3D prev_node;
prev_node.time = common::FromUniversal(0);
NodeData3D next_node;
NodeSpec3D next_node;
next_node.time = common::FromUniversal(10);
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 TrajectoryData =
::cartographer::mapping::PoseGraphInterface::TrajectoryData;
using NodeData = NodeData3D;
using SubmapData = SubmapData3D;
// For odometry.
std::unique_ptr<transform::Rigid3d> Interpolate(
@ -107,7 +105,7 @@ std::unique_ptr<transform::Rigid3d> Interpolate(
// applies a relative transform from it.
transform::Rigid3d GetInitialLandmarkPose(
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 double interpolation_parameter =
common::ToSeconds(observation.time - prev_node.time) /
@ -125,7 +123,7 @@ transform::Rigid3d GetInitialLandmarkPose(
void AddLandmarkCostFunctions(
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,
std::map<std::string, CeresPose>* C_landmarks, ceres::Problem* problem) {
for (const auto& landmark_node : landmark_nodes) {
@ -212,7 +210,7 @@ void OptimizationProblem3D::AddFixedFramePoseData(
}
void OptimizationProblem3D::AddTrajectoryNode(const int trajectory_id,
const NodeData& node_data) {
const NodeSpec3D& node_data) {
node_data_.Append(trajectory_id, node_data);
trajectory_data_[trajectory_id];
}
@ -223,7 +221,7 @@ void OptimizationProblem3D::SetTrajectoryData(
}
void OptimizationProblem3D::InsertTrajectoryNode(const NodeId& node_id,
const NodeData& node_data) {
const NodeSpec3D& node_data) {
node_data_.Insert(node_id, node_data);
trajectory_data_[node_id.trajectory_id];
}
@ -240,12 +238,12 @@ void OptimizationProblem3D::TrimTrajectoryNode(const NodeId& node_id) {
void OptimizationProblem3D::AddSubmap(
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(
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) {
@ -370,10 +368,10 @@ void OptimizationProblem3D::Solve(
auto prev_node_it = node_it;
for (++node_it; node_it != trajectory_end; ++node_it) {
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;
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) {
continue;
@ -392,7 +390,7 @@ void OptimizationProblem3D::Solve(
if (next_node_it != trajectory_end &&
next_node_it->id.node_index == second_node_id.node_index + 1) {
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 second_time = second_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;
for (++node_it; node_it != trajectory_end; ++node_it) {
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;
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) {
continue;
@ -504,7 +502,7 @@ void OptimizationProblem3D::Solve(
bool fixed_frame_pose_initialized = false;
for (; node_it != trajectory_end; ++node_it) {
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 =
Interpolate(fixed_frame_pose_data_, trajectory_id, node_data.time);
@ -591,8 +589,8 @@ void OptimizationProblem3D::Solve(
std::unique_ptr<transform::Rigid3d>
OptimizationProblem3D::CalculateOdometryBetweenNodes(
const int trajectory_id, const NodeData& first_node_data,
const NodeData& second_node_data) const {
const int trajectory_id, const NodeSpec3D& first_node_data,
const NodeSpec3D& second_node_data) const {
if (odometry_data_.HasTrajectory(trajectory_id)) {
const std::unique_ptr<transform::Rigid3d> first_node_odometry =
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 pose_graph {
struct NodeData3D {
struct NodeSpec3D {
common::Time time;
transform::Rigid3d local_pose;
transform::Rigid3d global_pose;
};
struct SubmapData3D {
struct SubmapSpec3D {
transform::Rigid3d global_pose;
};
class OptimizationProblem3D
: public OptimizationProblemInterface<NodeData3D, SubmapData3D,
: public OptimizationProblemInterface<NodeSpec3D, SubmapSpec3D,
transform::Rigid3d> {
public:
explicit OptimizationProblem3D(
@ -66,9 +66,9 @@ class OptimizationProblem3D
void AddOdometryData(int trajectory_id,
const sensor::OdometryData& odometry_data) override;
void AddTrajectoryNode(int trajectory_id,
const NodeData3D& node_data) override;
const NodeSpec3D& node_data) override;
void InsertTrajectoryNode(const NodeId& node_id,
const NodeData3D& node_data) override;
const NodeSpec3D& node_data) override;
void TrimTrajectoryNode(const NodeId& node_id) override;
void AddSubmap(int trajectory_id,
const transform::Rigid3d& global_submap_pose) override;
@ -82,10 +82,10 @@ class OptimizationProblem3D
const std::set<int>& frozen_trajectories,
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_;
}
const MapById<SubmapId, SubmapData3D>& submap_data() const override {
const MapById<SubmapId, SubmapSpec3D>& submap_data() const override {
return submap_data_;
}
const std::map<std::string, transform::Rigid3d>& landmark_data()
@ -118,12 +118,12 @@ class OptimizationProblem3D
private:
// Computes the relative pose between two nodes based on odometry data.
std::unique_ptr<transform::Rigid3d> CalculateOdometryBetweenNodes(
int trajectory_id, const NodeData3D& first_node_data,
const NodeData3D& second_node_data) const;
int trajectory_id, const NodeSpec3D& first_node_data,
const NodeSpec3D& second_node_data) const;
pose_graph::proto::OptimizationProblemOptions options_;
MapById<NodeId, NodeData3D> node_data_;
MapById<SubmapId, SubmapData3D> submap_data_;
MapById<NodeId, NodeSpec3D> node_data_;
MapById<SubmapId, SubmapSpec3D> submap_data_;
std::map<std::string, transform::Rigid3d> landmark_data_;
sensor::MapByTime<sensor::ImuData> imu_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,
Eigen::Vector3d::Zero()});
optimization_problem_.AddTrajectoryNode(kTrajectoryId,
NodeData3D{now, pose, pose});
NodeSpec3D{now, pose, pose});
now += common::FromSeconds(0.01);
}

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

@ -117,7 +117,8 @@ NodeId PoseGraph3D::AddNode(
insertion_submaps.back()) {
// We grow 'submap_data_' as needed. This code assumes that the first
// 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();
}
@ -182,7 +183,7 @@ void PoseGraph3D::AddLandmarkData(int trajectory_id,
AddWorkItem([=]() REQUIRES(mutex_) {
for (const auto& observation : landmark_data.landmark_observations) {
landmark_nodes_[observation.id].landmark_observations.emplace_back(
PoseGraph::LandmarkNode::LandmarkObservation{
PoseGraphInterface::LandmarkNode::LandmarkObservation{
trajectory_id, landmark_data.time,
observation.landmark_to_tracking_transform,
observation.translation_weight, observation.rotation_weight});
@ -266,7 +267,7 @@ void PoseGraph3D::ComputeConstraintsForNode(
insertion_submaps.front()->local_pose().inverse() * local_pose;
optimization_problem_->AddTrajectoryNode(
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) {
const SubmapId submap_id = submap_ids[i];
// 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) {
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);
finished_submap_data.state = SubmapState::kFinished;
// 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,
const SubmapId& submap_id) const {
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()) {
const NodeId last_submap_node_id =
*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) {
CHECK_EQ(constraint.tag, PoseGraph::Constraint::INTER_SUBMAP);
CHECK_EQ(constraint.tag, PoseGraphInterface::Constraint::INTER_SUBMAP);
const common::Time time =
GetLatestNodeTime(constraint.node_id, constraint.submap_id);
trajectory_connectivity_state_.Connect(constraint.node_id.trajectory_id,
@ -455,11 +457,11 @@ void PoseGraph3D::AddSubmapFromProto(
common::MutexLocker locker(&mutex_);
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;
// Immediately show the submap at the 'global_submap_pose'.
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_) {
submap_data_.at(submap_id).state = SubmapState::kFinished;
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;
optimization_problem_->InsertTrajectoryNode(
node_id,
pose_graph::NodeData3D{constant_data->time, constant_data->local_pose,
pose_graph::NodeSpec3D{constant_data->time, constant_data->local_pose,
global_pose});
});
}
@ -693,7 +695,7 @@ PoseGraph3D::GetFixedFramePoseData() {
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() {
common::MutexLocker locker(&mutex_);
return landmark_nodes_;
@ -705,7 +707,7 @@ PoseGraph3D::GetTrajectoryData() {
return optimization_problem_->trajectory_data();
}
std::vector<PoseGraph::Constraint> PoseGraph3D::constraints() {
std::vector<PoseGraphInterface::Constraint> PoseGraph3D::constraints() {
common::MutexLocker locker(&mutex_);
return constraints_;
}
@ -749,7 +751,8 @@ std::vector<std::vector<int>> PoseGraph3D::GetConnectedTrajectories() {
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_);
return GetSubmapDataUnderLock(submap_id);
}
@ -768,14 +771,14 @@ PoseGraph3D::GetAllSubmapPoses() {
auto submap_data = GetSubmapDataUnderLock(submap_id_data.id);
submap_poses.Insert(
submap_id_data.id,
PoseGraph::SubmapPose{submap_data.submap->num_range_data(),
PoseGraphInterface::SubmapPose{submap_data.submap->num_range_data(),
submap_data.pose});
}
return submap_poses;
}
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 {
auto begin_it = global_submap_poses.BeginOfTrajectory(trajectory_id);
auto end_it = global_submap_poses.EndOfTrajectory(trajectory_id);
@ -797,7 +800,7 @@ transform::Rigid3d PoseGraph3D::ComputeLocalToGlobalTransform(
.inverse();
}
PoseGraph::SubmapData PoseGraph3D::GetSubmapDataUnderLock(
PoseGraphInterface::SubmapData PoseGraph3D::GetSubmapDataUnderLock(
const SubmapId& submap_id) {
const auto it = submap_data_.find(submap_id);
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;
PoseGraph::SubmapData GetSubmapData(const SubmapId& submap_id)
EXCLUDES(mutex_) override;
MapById<SubmapId, PoseGraphInterface::SubmapData> GetAllSubmapData()
EXCLUDES(mutex_) override;
MapById<SubmapId, SubmapData> GetAllSubmapData() EXCLUDES(mutex_) override;
MapById<SubmapId, SubmapPose> GetAllSubmapPoses() EXCLUDES(mutex_) override;
transform::Rigid3d GetLocalToGlobalTransform(int trajectory_id)
EXCLUDES(mutex_) override;
@ -150,7 +149,7 @@ class PoseGraph3D : public PoseGraph {
// transitions to kFinished, all nodes are tried to match against this submap.
// Likewise, all new nodes are matched against submaps which are finished.
enum class SubmapState { kActive, kFinished };
struct SubmapData {
struct InternalSubmapData {
std::shared_ptr<const Submap3D> submap;
// IDs of the nodes that were inserted into this map together with
@ -161,8 +160,7 @@ class PoseGraph3D : public PoseGraph {
SubmapState state = SubmapState::kActive;
};
MapById<SubmapId, PoseGraphInterface::SubmapData> GetSubmapDataUnderLock()
REQUIRES(mutex_);
MapById<SubmapId, SubmapData> GetSubmapDataUnderLock() REQUIRES(mutex_);
// Handles a new work item.
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
// 'global_submap_poses'.
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_);
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
// 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.
MapById<NodeId, TrajectoryNode> trajectory_nodes_ GUARDED_BY(mutex_);
int num_trajectory_nodes_ GUARDED_BY(mutex_) = 0;
// 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_);
// Global landmark poses with all observations.
@ -287,12 +285,12 @@ class PoseGraph3D : public PoseGraph {
int num_submaps(int trajectory_id) const override;
std::vector<SubmapId> GetSubmapIds(int trajectory_id) const override;
MapById<SubmapId, PoseGraphInterface::SubmapData> GetAllSubmapData()
const override REQUIRES(parent_->mutex_);
MapById<SubmapId, SubmapData> GetAllSubmapData() const override
REQUIRES(parent_->mutex_);
const MapById<NodeId, TrajectoryNode>& GetTrajectoryNodes() const override
REQUIRES(parent_->mutex_);
const std::vector<PoseGraphInterface::Constraint>& GetConstraints()
const override REQUIRES(parent_->mutex_);
const std::vector<Constraint>& GetConstraints() const override
REQUIRES(parent_->mutex_);
void MarkSubmapAsTrimmed(const SubmapId& submap_id)
REQUIRES(parent_->mutex_) override;
bool IsFinished(int trajectory_id) const override REQUIRES(parent_->mutex_);