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

Move the mapping_2d/sparse_pose_graph directory. (#675) 

[RFC=0001](https://github.com/googlecartographer/rfcs/blob/master/text/0001-renaming-sparse-pose-graph.md)
master
Wolfgang Hess 2017-11-15 10:52:06 +01:00 committed by Wally B. Feed
parent f6192e4735
commit 26db9d6210
7 changed files with 37 additions and 40 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc → cartographer/mapping_2d/pose_graph/constraint_builder.cc
View File

@ -14,7 +14,7 @@
* limitations under the License. * limitations under the License.
*/ */
#include "cartographer/mapping_2d/sparse_pose_graph/constraint_builder.h" #include "cartographer/mapping_2d/pose_graph/constraint_builder.h"
#include <cmath> #include <cmath>
#include <functional> #include <functional>
@ -36,7 +36,7 @@
namespace cartographer { namespace cartographer {
namespace mapping_2d { namespace mapping_2d {
namespace sparse_pose_graph { namespace pose_graph {
transform::Rigid2d ComputeSubmapPose(const Submap& submap) { transform::Rigid2d ComputeSubmapPose(const Submap& submap) {
return transform::Project2D(submap.local_pose()); return transform::Project2D(submap.local_pose());
@ -287,6 +287,6 @@ void ConstraintBuilder::DeleteScanMatcher(const mapping::SubmapId& submap_id) {
submap_scan_matchers_.erase(submap_id); submap_scan_matchers_.erase(submap_id);
} }
} // namespace sparse_pose_graph } // namespace pose_graph
} // namespace mapping_2d } // namespace mapping_2d
} // namespace cartographer } // namespace cartographer

10
cartographer/mapping_2d/sparse_pose_graph/constraint_builder.h → cartographer/mapping_2d/pose_graph/constraint_builder.h
View File

@ -14,8 +14,8 @@
* limitations under the License. * limitations under the License.
*/ */
#ifndef CARTOGRAPHER_MAPPING_2D_SPARSE_POSE_GRAPH_CONSTRAINT_BUILDER_H_ #ifndef CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_CONSTRAINT_BUILDER_H_
#define CARTOGRAPHER_MAPPING_2D_SPARSE_POSE_GRAPH_CONSTRAINT_BUILDER_H_ #define CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_CONSTRAINT_BUILDER_H_
#include <array> #include <array>
#include <deque> #include <deque>
@ -42,7 +42,7 @@
namespace cartographer { namespace cartographer {
namespace mapping_2d { namespace mapping_2d {
namespace sparse_pose_graph { namespace pose_graph {
// Returns (map <- submap) where 'submap' is a coordinate system at the origin // Returns (map <- submap) where 'submap' is a coordinate system at the origin
// of the Submap. // of the Submap.
@ -178,8 +178,8 @@ class ConstraintBuilder {
common::Histogram score_histogram_ GUARDED_BY(mutex_); common::Histogram score_histogram_ GUARDED_BY(mutex_);
}; };
} // namespace sparse_pose_graph } // namespace pose_graph
} // namespace mapping_2d } // namespace mapping_2d
} // namespace cartographer } // namespace cartographer
#endif // CARTOGRAPHER_MAPPING_2D_SPARSE_POSE_GRAPH_CONSTRAINT_BUILDER_H_ #endif // CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_CONSTRAINT_BUILDER_H_

8
cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc → cartographer/mapping_2d/pose_graph/optimization_problem.cc
View File

@ -14,7 +14,7 @@
* limitations under the License. * limitations under the License.
*/ */
#include "cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h" #include "cartographer/mapping_2d/pose_graph/optimization_problem.h"
#include <algorithm> #include <algorithm>
#include <array> #include <array>
@ -27,7 +27,7 @@
#include "cartographer/common/ceres_solver_options.h" #include "cartographer/common/ceres_solver_options.h"
#include "cartographer/common/histogram.h" #include "cartographer/common/histogram.h"
#include "cartographer/common/math.h" #include "cartographer/common/math.h"
#include "cartographer/mapping_2d/sparse_pose_graph/spa_cost_function.h" #include "cartographer/mapping_2d/pose_graph/spa_cost_function.h"
#include "cartographer/sensor/odometry_data.h" #include "cartographer/sensor/odometry_data.h"
#include "cartographer/transform/transform.h" #include "cartographer/transform/transform.h"
#include "ceres/ceres.h" #include "ceres/ceres.h"
@ -35,7 +35,7 @@
namespace cartographer { namespace cartographer {
namespace mapping_2d { namespace mapping_2d {
namespace sparse_pose_graph { namespace pose_graph {
namespace { namespace {
@ -274,6 +274,6 @@ transform::Rigid3d OptimizationProblem::ComputeRelativePose(
second_node_data.initial_pose); second_node_data.initial_pose);
} }
} // namespace sparse_pose_graph } // namespace pose_graph
} // namespace mapping_2d } // namespace mapping_2d
} // namespace cartographer } // namespace cartographer

10
cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h → cartographer/mapping_2d/pose_graph/optimization_problem.h
View File

@ -14,8 +14,8 @@
* limitations under the License. * limitations under the License.
*/ */
#ifndef CARTOGRAPHER_MAPPING_2D_SPARSE_POSE_GRAPH_OPTIMIZATION_PROBLEM_H_ #ifndef CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_OPTIMIZATION_PROBLEM_H_
#define CARTOGRAPHER_MAPPING_2D_SPARSE_POSE_GRAPH_OPTIMIZATION_PROBLEM_H_ #define CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_OPTIMIZATION_PROBLEM_H_
#include <array> #include <array>
#include <deque> #include <deque>
@ -37,7 +37,7 @@
namespace cartographer { namespace cartographer {
namespace mapping_2d { namespace mapping_2d {
namespace sparse_pose_graph { namespace pose_graph {
struct NodeData { struct NodeData {
common::Time time; common::Time time;
@ -106,8 +106,8 @@ class OptimizationProblem {
sensor::MapByTime<sensor::OdometryData> odometry_data_; sensor::MapByTime<sensor::OdometryData> odometry_data_;
}; };
} // namespace sparse_pose_graph } // namespace pose_graph
} // namespace mapping_2d } // namespace mapping_2d
} // namespace cartographer } // namespace cartographer
#endif // CARTOGRAPHER_MAPPING_2D_SPARSE_POSE_GRAPH_OPTIMIZATION_PROBLEM_H_ #endif // CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_OPTIMIZATION_PROBLEM_H_

10
cartographer/mapping_2d/sparse_pose_graph/spa_cost_function.h → cartographer/mapping_2d/pose_graph/spa_cost_function.h
View File

@ -14,8 +14,8 @@
* limitations under the License. * limitations under the License.
*/ */
#ifndef CARTOGRAPHER_MAPPING_2D_SPARSE_POSE_GRAPH_SPA_COST_FUNCTION_H_ #ifndef CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_SPA_COST_FUNCTION_H_
#define CARTOGRAPHER_MAPPING_2D_SPARSE_POSE_GRAPH_SPA_COST_FUNCTION_H_ #define CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_SPA_COST_FUNCTION_H_
#include <array> #include <array>
@ -30,7 +30,7 @@
namespace cartographer { namespace cartographer {
namespace mapping_2d { namespace mapping_2d {
namespace sparse_pose_graph { namespace pose_graph {
class SpaCostFunction { class SpaCostFunction {
public: public:
@ -81,8 +81,8 @@ class SpaCostFunction {
const Constraint::Pose pose_; const Constraint::Pose pose_;
}; };
} // namespace sparse_pose_graph } // namespace pose_graph
} // namespace mapping_2d } // namespace mapping_2d
} // namespace cartographer } // namespace cartographer
#endif // CARTOGRAPHER_MAPPING_2D_SPARSE_POSE_GRAPH_SPA_COST_FUNCTION_H_ #endif // CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_SPA_COST_FUNCTION_H_

21
cartographer/mapping_2d/sparse_pose_graph.cc
View File

@ -87,9 +87,8 @@ std::vector<mapping::SubmapId> SparsePoseGraph::InitializeGlobalSubmapPoses(
optimization_problem_.AddSubmap( optimization_problem_.AddSubmap(
trajectory_id, trajectory_id,
first_submap_pose * first_submap_pose *
sparse_pose_graph::ComputeSubmapPose(*insertion_submaps[0]) pose_graph::ComputeSubmapPose(*insertion_submaps[0]).inverse() *
.inverse() * pose_graph::ComputeSubmapPose(*insertion_submaps[1]));
sparse_pose_graph::ComputeSubmapPose(*insertion_submaps[1]));
return {last_submap_id, return {last_submap_id,
mapping::SubmapId{trajectory_id, last_submap_id.submap_index + 1}}; mapping::SubmapId{trajectory_id, last_submap_id.submap_index + 1}};
} }
@ -232,8 +231,7 @@ void SparsePoseGraph::ComputeConstraintsForNode(
transform::Rigid3d::Rotation(constant_data->gravity_alignment.inverse())); transform::Rigid3d::Rotation(constant_data->gravity_alignment.inverse()));
const transform::Rigid2d optimized_pose = const transform::Rigid2d optimized_pose =
optimization_problem_.submap_data().at(matching_id).global_pose * optimization_problem_.submap_data().at(matching_id).global_pose *
sparse_pose_graph::ComputeSubmapPose(*insertion_submaps.front()) pose_graph::ComputeSubmapPose(*insertion_submaps.front()).inverse() *
.inverse() *
pose; pose;
optimization_problem_.AddTrajectoryNode( optimization_problem_.AddTrajectoryNode(
matching_id.trajectory_id, constant_data->time, pose, optimized_pose, matching_id.trajectory_id, constant_data->time, pose, optimized_pose,
@ -245,8 +243,7 @@ void SparsePoseGraph::ComputeConstraintsForNode(
CHECK(submap_data_.at(submap_id).state == SubmapState::kActive); CHECK(submap_data_.at(submap_id).state == SubmapState::kActive);
submap_data_.at(submap_id).node_ids.emplace(node_id); submap_data_.at(submap_id).node_ids.emplace(node_id);
const transform::Rigid2d constraint_transform = const transform::Rigid2d constraint_transform =
sparse_pose_graph::ComputeSubmapPose(*insertion_submaps[i]).inverse() * pose_graph::ComputeSubmapPose(*insertion_submaps[i]).inverse() * pose;
pose;
constraints_.push_back(Constraint{submap_id, constraints_.push_back(Constraint{submap_id,
node_id, node_id,
{transform::Embed3D(constraint_transform), {transform::Embed3D(constraint_transform),
@ -310,7 +307,7 @@ void SparsePoseGraph::UpdateTrajectoryConnectivity(
void SparsePoseGraph::HandleWorkQueue() { void SparsePoseGraph::HandleWorkQueue() {
constraint_builder_.WhenDone( constraint_builder_.WhenDone(
[this](const sparse_pose_graph::ConstraintBuilder::Result& result) { [this](const pose_graph::ConstraintBuilder::Result& result) {
{ {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
constraints_.insert(constraints_.end(), result.begin(), result.end()); constraints_.insert(constraints_.end(), result.begin(), result.end());
@ -364,8 +361,8 @@ void SparsePoseGraph::WaitForAllComputations() {
} }
std::cout << "\r\x1b[KOptimizing: Done. " << std::endl; std::cout << "\r\x1b[KOptimizing: Done. " << std::endl;
constraint_builder_.WhenDone( constraint_builder_.WhenDone(
[this, &notification]( [this,
const sparse_pose_graph::ConstraintBuilder::Result& result) { &notification](const pose_graph::ConstraintBuilder::Result& result) {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
constraints_.insert(constraints_.end(), result.begin(), result.end()); constraints_.insert(constraints_.end(), result.begin(), result.end());
notification = true; notification = true;
@ -407,7 +404,7 @@ void SparsePoseGraph::AddSubmapFromProto(
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'.
optimized_submap_transforms_.Insert( optimized_submap_transforms_.Insert(
submap_id, sparse_pose_graph::SubmapData{global_submap_pose_2d}); submap_id, pose_graph::SubmapData{global_submap_pose_2d});
AddWorkItem([this, submap_id, global_submap_pose_2d]() REQUIRES(mutex_) { AddWorkItem([this, submap_id, global_submap_pose_2d]() REQUIRES(mutex_) {
CHECK_EQ(frozen_trajectories_.count(submap_id.trajectory_id), 1); CHECK_EQ(frozen_trajectories_.count(submap_id.trajectory_id), 1);
submap_data_.at(submap_id).state = SubmapState::kFinished; submap_data_.at(submap_id).state = SubmapState::kFinished;
@ -638,7 +635,7 @@ SparsePoseGraph::GetAllSubmapData() {
} }
transform::Rigid3d SparsePoseGraph::ComputeLocalToGlobalTransform( transform::Rigid3d SparsePoseGraph::ComputeLocalToGlobalTransform(
const mapping::MapById<mapping::SubmapId, sparse_pose_graph::SubmapData>& const mapping::MapById<mapping::SubmapId, pose_graph::SubmapData>&
submap_transforms, submap_transforms,
const int trajectory_id) const { const int trajectory_id) const {
auto begin_it = submap_transforms.BeginOfTrajectory(trajectory_id); auto begin_it = submap_transforms.BeginOfTrajectory(trajectory_id);

12
cartographer/mapping_2d/sparse_pose_graph.h
View File

@ -35,8 +35,8 @@
#include "cartographer/mapping/pose_graph_trimmer.h" #include "cartographer/mapping/pose_graph_trimmer.h"
#include "cartographer/mapping/sparse_pose_graph.h" #include "cartographer/mapping/sparse_pose_graph.h"
#include "cartographer/mapping/trajectory_connectivity_state.h" #include "cartographer/mapping/trajectory_connectivity_state.h"
#include "cartographer/mapping_2d/sparse_pose_graph/constraint_builder.h" #include "cartographer/mapping_2d/pose_graph/constraint_builder.h"
#include "cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h" #include "cartographer/mapping_2d/pose_graph/optimization_problem.h"
#include "cartographer/mapping_2d/submaps.h" #include "cartographer/mapping_2d/submaps.h"
#include "cartographer/sensor/fixed_frame_pose_data.h" #include "cartographer/sensor/fixed_frame_pose_data.h"
#include "cartographer/sensor/odometry_data.h" #include "cartographer/sensor/odometry_data.h"
@ -175,7 +175,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
// Computes the local to global map frame transform based on the given // Computes the local to global map frame transform based on the given
// optimized 'submap_transforms'. // optimized 'submap_transforms'.
transform::Rigid3d ComputeLocalToGlobalTransform( transform::Rigid3d ComputeLocalToGlobalTransform(
const mapping::MapById<mapping::SubmapId, sparse_pose_graph::SubmapData>& const mapping::MapById<mapping::SubmapId, pose_graph::SubmapData>&
submap_transforms, submap_transforms,
int trajectory_id) const REQUIRES(mutex_); int trajectory_id) const REQUIRES(mutex_);
@ -212,8 +212,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
bool run_loop_closure_ GUARDED_BY(mutex_) = false; bool run_loop_closure_ GUARDED_BY(mutex_) = false;
// Current optimization problem. // Current optimization problem.
sparse_pose_graph::OptimizationProblem optimization_problem_; pose_graph::OptimizationProblem optimization_problem_;
sparse_pose_graph::ConstraintBuilder constraint_builder_ GUARDED_BY(mutex_); pose_graph::ConstraintBuilder constraint_builder_ GUARDED_BY(mutex_);
std::vector<Constraint> constraints_ GUARDED_BY(mutex_); std::vector<Constraint> constraints_ GUARDED_BY(mutex_);
// 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
@ -227,7 +227,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
int num_trajectory_nodes_ GUARDED_BY(mutex_) = 0; int num_trajectory_nodes_ GUARDED_BY(mutex_) = 0;
// Current submap transforms used for displaying data. // Current submap transforms used for displaying data.
mapping::MapById<mapping::SubmapId, sparse_pose_graph::SubmapData> mapping::MapById<mapping::SubmapId, pose_graph::SubmapData>
optimized_submap_transforms_ GUARDED_BY(mutex_); optimized_submap_transforms_ GUARDED_BY(mutex_);
// List of all trimmers to consult when optimizations finish. // List of all trimmers to consult when optimizations finish.