diff --git a/cartographer/mapping/internal/2d/pose_graph/landmark_cost_function_2d.h b/cartographer/mapping/internal/2d/pose_graph/landmark_cost_function_2d.h index 7b33751..1c6a47f 100644 --- a/cartographer/mapping/internal/2d/pose_graph/landmark_cost_function_2d.h +++ b/cartographer/mapping/internal/2d/pose_graph/landmark_cost_function_2d.h @@ -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), diff --git a/cartographer/mapping/internal/2d/pose_graph/landmark_cost_function_2d_test.cc b/cartographer/mapping/internal/2d/pose_graph/landmark_cost_function_2d_test.cc index b9b3706..9a0e505 100644 --- a/cartographer/mapping/internal/2d/pose_graph/landmark_cost_function_2d_test.cc +++ b/cartographer/mapping/internal/2d/pose_graph/landmark_cost_function_2d_test.cc @@ -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(); diff --git a/cartographer/mapping/internal/2d/pose_graph/optimization_problem_2d.cc b/cartographer/mapping/internal/2d/pose_graph/optimization_problem_2d.cc index 299a07f..954e813 100644 --- a/cartographer/mapping/internal/2d/pose_graph/optimization_problem_2d.cc +++ b/cartographer/mapping/internal/2d/pose_graph/optimization_problem_2d.cc @@ -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& 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& prev_node_pose, const std::array& next_node_pose) { const double interpolation_parameter = @@ -82,7 +80,7 @@ transform::Rigid3d GetInitialLandmarkPose( void AddLandmarkCostFunctions( const std::map& landmark_nodes, - bool freeze_landmarks, const MapById& node_data, + bool freeze_landmarks, const MapById& node_data, MapById>* C_nodes, std::map* 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> C_submaps; MapById> C_nodes; std::map 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 OptimizationProblem2D::InterpolateOdometry( std::unique_ptr 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 first_node_odometry = InterpolateOdometry(trajectory_id, first_node_data.time); diff --git a/cartographer/mapping/internal/2d/pose_graph/optimization_problem_2d.h b/cartographer/mapping/internal/2d/pose_graph/optimization_problem_2d.h index f15c111..5a56dd0 100644 --- a/cartographer/mapping/internal/2d/pose_graph/optimization_problem_2d.h +++ b/cartographer/mapping/internal/2d/pose_graph/optimization_problem_2d.h @@ -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 { 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& frozen_trajectories, const std::map& landmark_nodes) override; - const MapById& node_data() const override { + const MapById& node_data() const override { return node_data_; } - const MapById& submap_data() const override { + const MapById& submap_data() const override { return submap_data_; } const std::map& 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 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 node_data_; - MapById submap_data_; + MapById node_data_; + MapById submap_data_; std::map landmark_data_; sensor::MapByTime imu_data_; sensor::MapByTime odometry_data_; diff --git a/cartographer/mapping/internal/2d/pose_graph_2d.cc b/cartographer/mapping/internal/2d/pose_graph_2d.cc index a2c6870..234f800 100644 --- a/cartographer/mapping/internal/2d/pose_graph_2d.cc +++ b/cartographer/mapping/internal/2d/pose_graph_2d.cc @@ -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 PoseGraph2D::GetOdometryData() { return optimization_problem_->odometry_data(); } -std::map +std::map PoseGraph2D::GetLandmarkNodes() { common::MutexLocker locker(&mutex_); return landmark_nodes_; @@ -661,8 +663,8 @@ PoseGraph2D::GetFixedFramePoseData() { return {}; // Not implemented yet in 2D. } -std::vector PoseGraph2D::constraints() { - std::vector result; +std::vector PoseGraph2D::constraints() { + std::vector result; common::MutexLocker locker(&mutex_); for (const Constraint& constraint : constraints_) { result.push_back(Constraint{ @@ -717,7 +719,8 @@ std::vector> 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& global_submap_poses, + const MapById& 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()) { diff --git a/cartographer/mapping/internal/2d/pose_graph_2d.h b/cartographer/mapping/internal/2d/pose_graph_2d.h index 4c2c76e..a5ee8ad 100644 --- a/cartographer/mapping/internal/2d/pose_graph_2d.h +++ b/cartographer/mapping/internal/2d/pose_graph_2d.h @@ -108,7 +108,7 @@ class PoseGraph2D : public PoseGraph { void AddTrimmer(std::unique_ptr trimmer) override; void RunFinalOptimization() override; std::vector> GetConnectedTrajectories() override; - PoseGraph::SubmapData GetSubmapData(const SubmapId& submap_id) + PoseGraphInterface::SubmapData GetSubmapData(const SubmapId& submap_id) EXCLUDES(mutex_) override; MapById 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 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& global_submap_poses, + const MapById& 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 submap_data_ GUARDED_BY(mutex_); + MapById submap_data_ GUARDED_BY(mutex_); // Data that are currently being shown. MapById trajectory_nodes_ GUARDED_BY(mutex_); int num_trajectory_nodes_ GUARDED_BY(mutex_) = 0; // Global submap poses currently used for displaying data. - MapById global_submap_poses_ + MapById 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 GetSubmapIds(int trajectory_id) const override; - MapById GetAllSubmapData() - const override REQUIRES(parent_->mutex_); + MapById GetAllSubmapData() const override + REQUIRES(parent_->mutex_); const MapById& GetTrajectoryNodes() const override REQUIRES(parent_->mutex_); - const std::vector& GetConstraints() - const override REQUIRES(parent_->mutex_); + const std::vector& 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_); diff --git a/cartographer/mapping/internal/3d/pose_graph/landmark_cost_function_3d.h b/cartographer/mapping/internal/3d/pose_graph/landmark_cost_function_3d.h index e82b4ac..b871e48 100644 --- a/cartographer/mapping/internal/3d/pose_graph/landmark_cost_function_3d.h +++ b/cartographer/mapping/internal/3d/pose_graph/landmark_cost_function_3d.h @@ -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), diff --git a/cartographer/mapping/internal/3d/pose_graph/landmark_cost_function_3d_test.cc b/cartographer/mapping/internal/3d/pose_graph/landmark_cost_function_3d_test.cc index 0f4048a..a270d3b 100644 --- a/cartographer/mapping/internal/3d/pose_graph/landmark_cost_function_3d_test.cc +++ b/cartographer/mapping/internal/3d/pose_graph/landmark_cost_function_3d_test.cc @@ -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 cost_function( diff --git a/cartographer/mapping/internal/3d/pose_graph/optimization_problem_3d.cc b/cartographer/mapping/internal/3d/pose_graph/optimization_problem_3d.cc index 6daf799..baa9089 100644 --- a/cartographer/mapping/internal/3d/pose_graph/optimization_problem_3d.cc +++ b/cartographer/mapping/internal/3d/pose_graph/optimization_problem_3d.cc @@ -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 Interpolate( @@ -107,7 +105,7 @@ std::unique_ptr 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& landmark_nodes, - bool freeze_landmarks, const MapById& node_data, + bool freeze_landmarks, const MapById& node_data, MapById* C_nodes, std::map* 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 fixed_frame_pose = Interpolate(fixed_frame_pose_data_, trajectory_id, node_data.time); @@ -591,8 +589,8 @@ void OptimizationProblem3D::Solve( std::unique_ptr 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 first_node_odometry = Interpolate(odometry_data_, trajectory_id, first_node_data.time); diff --git a/cartographer/mapping/internal/3d/pose_graph/optimization_problem_3d.h b/cartographer/mapping/internal/3d/pose_graph/optimization_problem_3d.h index 044c4c5..6c545fa 100644 --- a/cartographer/mapping/internal/3d/pose_graph/optimization_problem_3d.h +++ b/cartographer/mapping/internal/3d/pose_graph/optimization_problem_3d.h @@ -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 { 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& frozen_trajectories, const std::map& landmark_nodes) override; - const MapById& node_data() const override { + const MapById& node_data() const override { return node_data_; } - const MapById& submap_data() const override { + const MapById& submap_data() const override { return submap_data_; } const std::map& landmark_data() @@ -118,12 +118,12 @@ class OptimizationProblem3D private: // Computes the relative pose between two nodes based on odometry data. std::unique_ptr 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 node_data_; - MapById submap_data_; + MapById node_data_; + MapById submap_data_; std::map landmark_data_; sensor::MapByTime imu_data_; sensor::MapByTime odometry_data_; diff --git a/cartographer/mapping/internal/3d/pose_graph/optimization_problem_3d_test.cc b/cartographer/mapping/internal/3d/pose_graph/optimization_problem_3d_test.cc index 4de86c4..963fd55 100644 --- a/cartographer/mapping/internal/3d/pose_graph/optimization_problem_3d_test.cc +++ b/cartographer/mapping/internal/3d/pose_graph/optimization_problem_3d_test.cc @@ -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); } diff --git a/cartographer/mapping/internal/3d/pose_graph_3d.cc b/cartographer/mapping/internal/3d/pose_graph_3d.cc index 99b4ba0..a19e446 100644 --- a/cartographer/mapping/internal/3d/pose_graph_3d.cc +++ b/cartographer/mapping/internal/3d/pose_graph_3d.cc @@ -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::map PoseGraph3D::GetLandmarkNodes() { common::MutexLocker locker(&mutex_); return landmark_nodes_; @@ -705,7 +707,7 @@ PoseGraph3D::GetTrajectoryData() { return optimization_problem_->trajectory_data(); } -std::vector PoseGraph3D::constraints() { +std::vector PoseGraph3D::constraints() { common::MutexLocker locker(&mutex_); return constraints_; } @@ -749,7 +751,8 @@ std::vector> 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(), - submap_data.pose}); + PoseGraphInterface::SubmapPose{submap_data.submap->num_range_data(), + submap_data.pose}); } return submap_poses; } transform::Rigid3d PoseGraph3D::ComputeLocalToGlobalTransform( - const MapById& global_submap_poses, + const MapById& 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()) { diff --git a/cartographer/mapping/internal/3d/pose_graph_3d.h b/cartographer/mapping/internal/3d/pose_graph_3d.h index 96f02a0..f0cba44 100644 --- a/cartographer/mapping/internal/3d/pose_graph_3d.h +++ b/cartographer/mapping/internal/3d/pose_graph_3d.h @@ -109,8 +109,7 @@ class PoseGraph3D : public PoseGraph { std::vector> GetConnectedTrajectories() override; PoseGraph::SubmapData GetSubmapData(const SubmapId& submap_id) EXCLUDES(mutex_) override; - MapById GetAllSubmapData() - EXCLUDES(mutex_) override; + MapById GetAllSubmapData() EXCLUDES(mutex_) override; MapById 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 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 GetSubmapDataUnderLock() - REQUIRES(mutex_); + MapById GetSubmapDataUnderLock() REQUIRES(mutex_); // Handles a new work item. void AddWorkItem(const std::function& 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& global_submap_poses, + const MapById& 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 submap_data_ GUARDED_BY(mutex_); + MapById submap_data_ GUARDED_BY(mutex_); // Data that are currently being shown. MapById trajectory_nodes_ GUARDED_BY(mutex_); int num_trajectory_nodes_ GUARDED_BY(mutex_) = 0; // Global submap poses currently used for displaying data. - MapById global_submap_poses_ + MapById 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 GetSubmapIds(int trajectory_id) const override; - MapById GetAllSubmapData() - const override REQUIRES(parent_->mutex_); + MapById GetAllSubmapData() const override + REQUIRES(parent_->mutex_); const MapById& GetTrajectoryNodes() const override REQUIRES(parent_->mutex_); - const std::vector& GetConstraints() - const override REQUIRES(parent_->mutex_); + const std::vector& 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_);