Move the mapping_3d/sparse_pose_graph directory. (#676)
[RFC=0001](https://github.com/googlecartographer/rfcs/blob/master/text/0001-renaming-sparse-pose-graph.md)master
parent
26db9d6210
commit
36b9cf7f9a
|
@ -14,7 +14,7 @@
|
|||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#include "cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h"
|
||||
#include "cartographer/mapping_3d/pose_graph/constraint_builder.h"
|
||||
|
||||
#include <cmath>
|
||||
#include <functional>
|
||||
|
@ -36,7 +36,7 @@
|
|||
|
||||
namespace cartographer {
|
||||
namespace mapping_3d {
|
||||
namespace sparse_pose_graph {
|
||||
namespace pose_graph {
|
||||
|
||||
ConstraintBuilder::ConstraintBuilder(
|
||||
const mapping::pose_graph::proto::ConstraintBuilderOptions& options,
|
||||
|
@ -308,6 +308,6 @@ void ConstraintBuilder::DeleteScanMatcher(const mapping::SubmapId& submap_id) {
|
|||
submap_scan_matchers_.erase(submap_id);
|
||||
}
|
||||
|
||||
} // namespace sparse_pose_graph
|
||||
} // namespace pose_graph
|
||||
} // namespace mapping_3d
|
||||
} // namespace cartographer
|
|
@ -14,8 +14,8 @@
|
|||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#ifndef CARTOGRAPHER_MAPPING_3D_SPARSE_POSE_GRAPH_CONSTRAINT_BUILDER_H_
|
||||
#define CARTOGRAPHER_MAPPING_3D_SPARSE_POSE_GRAPH_CONSTRAINT_BUILDER_H_
|
||||
#ifndef CARTOGRAPHER_MAPPING_3D_POSE_GRAPH_CONSTRAINT_BUILDER_H_
|
||||
#define CARTOGRAPHER_MAPPING_3D_POSE_GRAPH_CONSTRAINT_BUILDER_H_
|
||||
|
||||
#include <array>
|
||||
#include <deque>
|
||||
|
@ -32,9 +32,9 @@
|
|||
#include "cartographer/common/mutex.h"
|
||||
#include "cartographer/common/thread_pool.h"
|
||||
#include "cartographer/mapping/trajectory_node.h"
|
||||
#include "cartographer/mapping_3d/pose_graph/optimization_problem.h"
|
||||
#include "cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h"
|
||||
#include "cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.h"
|
||||
#include "cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h"
|
||||
#include "cartographer/mapping_3d/submaps.h"
|
||||
#include "cartographer/sensor/compressed_point_cloud.h"
|
||||
#include "cartographer/sensor/point_cloud.h"
|
||||
|
@ -42,7 +42,7 @@
|
|||
|
||||
namespace cartographer {
|
||||
namespace mapping_3d {
|
||||
namespace sparse_pose_graph {
|
||||
namespace pose_graph {
|
||||
|
||||
// Asynchronously computes constraints.
|
||||
//
|
||||
|
@ -191,8 +191,8 @@ class ConstraintBuilder {
|
|||
common::Histogram low_resolution_score_histogram_ GUARDED_BY(mutex_);
|
||||
};
|
||||
|
||||
} // namespace sparse_pose_graph
|
||||
} // namespace pose_graph
|
||||
} // namespace mapping_3d
|
||||
} // namespace cartographer
|
||||
|
||||
#endif // CARTOGRAPHER_MAPPING_3D_SPARSE_POSE_GRAPH_CONSTRAINT_BUILDER_H_
|
||||
#endif // CARTOGRAPHER_MAPPING_3D_POSE_GRAPH_CONSTRAINT_BUILDER_H_
|
|
@ -14,7 +14,7 @@
|
|||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#include "cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h"
|
||||
#include "cartographer/mapping_3d/pose_graph/optimization_problem.h"
|
||||
|
||||
#include <algorithm>
|
||||
#include <array>
|
||||
|
@ -33,9 +33,9 @@
|
|||
#include "cartographer/mapping_3d/acceleration_cost_function.h"
|
||||
#include "cartographer/mapping_3d/ceres_pose.h"
|
||||
#include "cartographer/mapping_3d/imu_integration.h"
|
||||
#include "cartographer/mapping_3d/pose_graph/spa_cost_function.h"
|
||||
#include "cartographer/mapping_3d/rotation_cost_function.h"
|
||||
#include "cartographer/mapping_3d/rotation_parameterization.h"
|
||||
#include "cartographer/mapping_3d/sparse_pose_graph/spa_cost_function.h"
|
||||
#include "cartographer/transform/timestamped_transform.h"
|
||||
#include "cartographer/transform/transform.h"
|
||||
#include "ceres/ceres.h"
|
||||
|
@ -45,7 +45,7 @@
|
|||
|
||||
namespace cartographer {
|
||||
namespace mapping_3d {
|
||||
namespace sparse_pose_graph {
|
||||
namespace pose_graph {
|
||||
|
||||
namespace {
|
||||
|
||||
|
@ -484,6 +484,6 @@ transform::Rigid3d OptimizationProblem::ComputeRelativePose(
|
|||
return first_node_data.local_pose.inverse() * second_node_data.local_pose;
|
||||
}
|
||||
|
||||
} // namespace sparse_pose_graph
|
||||
} // namespace pose_graph
|
||||
} // namespace mapping_3d
|
||||
} // namespace cartographer
|
|
@ -14,8 +14,8 @@
|
|||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#ifndef CARTOGRAPHER_MAPPING_3D_SPARSE_POSE_GRAPH_OPTIMIZATION_PROBLEM_H_
|
||||
#define CARTOGRAPHER_MAPPING_3D_SPARSE_POSE_GRAPH_OPTIMIZATION_PROBLEM_H_
|
||||
#ifndef CARTOGRAPHER_MAPPING_3D_POSE_GRAPH_OPTIMIZATION_PROBLEM_H_
|
||||
#define CARTOGRAPHER_MAPPING_3D_POSE_GRAPH_OPTIMIZATION_PROBLEM_H_
|
||||
|
||||
#include <array>
|
||||
#include <deque>
|
||||
|
@ -38,7 +38,7 @@
|
|||
|
||||
namespace cartographer {
|
||||
namespace mapping_3d {
|
||||
namespace sparse_pose_graph {
|
||||
namespace pose_graph {
|
||||
|
||||
struct NodeData {
|
||||
common::Time time;
|
||||
|
@ -116,8 +116,8 @@ class OptimizationProblem {
|
|||
std::vector<TrajectoryData> trajectory_data_;
|
||||
};
|
||||
|
||||
} // namespace sparse_pose_graph
|
||||
} // namespace pose_graph
|
||||
} // namespace mapping_3d
|
||||
} // namespace cartographer
|
||||
|
||||
#endif // CARTOGRAPHER_MAPPING_3D_SPARSE_POSE_GRAPH_OPTIMIZATION_PROBLEM_H_
|
||||
#endif // CARTOGRAPHER_MAPPING_3D_POSE_GRAPH_OPTIMIZATION_PROBLEM_H_
|
|
@ -14,7 +14,7 @@
|
|||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#include "cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h"
|
||||
#include "cartographer/mapping_3d/pose_graph/optimization_problem.h"
|
||||
|
||||
#include <random>
|
||||
|
||||
|
@ -28,7 +28,7 @@
|
|||
|
||||
namespace cartographer {
|
||||
namespace mapping_3d {
|
||||
namespace sparse_pose_graph {
|
||||
namespace pose_graph {
|
||||
namespace {
|
||||
|
||||
class OptimizationProblemTest : public ::testing::Test {
|
||||
|
@ -189,6 +189,6 @@ TEST_F(OptimizationProblemTest, ReducesNoise) {
|
|||
}
|
||||
|
||||
} // namespace
|
||||
} // namespace sparse_pose_graph
|
||||
} // namespace pose_graph
|
||||
} // namespace mapping_3d
|
||||
} // namespace cartographer
|
|
@ -14,8 +14,8 @@
|
|||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#ifndef CARTOGRAPHER_MAPPING_3D_SPARSE_POSE_GRAPH_SPA_COST_FUNCTION_H_
|
||||
#define CARTOGRAPHER_MAPPING_3D_SPARSE_POSE_GRAPH_SPA_COST_FUNCTION_H_
|
||||
#ifndef CARTOGRAPHER_MAPPING_3D_POSE_GRAPH_SPA_COST_FUNCTION_H_
|
||||
#define CARTOGRAPHER_MAPPING_3D_POSE_GRAPH_SPA_COST_FUNCTION_H_
|
||||
|
||||
#include <array>
|
||||
|
||||
|
@ -30,7 +30,7 @@
|
|||
|
||||
namespace cartographer {
|
||||
namespace mapping_3d {
|
||||
namespace sparse_pose_graph {
|
||||
namespace pose_graph {
|
||||
|
||||
class SpaCostFunction {
|
||||
public:
|
||||
|
@ -103,8 +103,8 @@ class SpaCostFunction {
|
|||
const Constraint::Pose pose_;
|
||||
};
|
||||
|
||||
} // namespace sparse_pose_graph
|
||||
} // namespace pose_graph
|
||||
} // namespace mapping_3d
|
||||
} // namespace cartographer
|
||||
|
||||
#endif // CARTOGRAPHER_MAPPING_3D_SPARSE_POSE_GRAPH_SPA_COST_FUNCTION_H_
|
||||
#endif // CARTOGRAPHER_MAPPING_3D_POSE_GRAPH_SPA_COST_FUNCTION_H_
|
|
@ -43,7 +43,7 @@ SparsePoseGraph::SparsePoseGraph(
|
|||
common::ThreadPool* thread_pool)
|
||||
: options_(options),
|
||||
optimization_problem_(options_.optimization_problem_options(),
|
||||
sparse_pose_graph::OptimizationProblem::FixZ::kNo),
|
||||
pose_graph::OptimizationProblem::FixZ::kNo),
|
||||
constraint_builder_(options_.constraint_builder_options(), thread_pool) {}
|
||||
|
||||
SparsePoseGraph::~SparsePoseGraph() {
|
||||
|
@ -324,7 +324,7 @@ void SparsePoseGraph::UpdateTrajectoryConnectivity(
|
|||
|
||||
void SparsePoseGraph::HandleWorkQueue() {
|
||||
constraint_builder_.WhenDone(
|
||||
[this](const sparse_pose_graph::ConstraintBuilder::Result& result) {
|
||||
[this](const pose_graph::ConstraintBuilder::Result& result) {
|
||||
{
|
||||
common::MutexLocker locker(&mutex_);
|
||||
constraints_.insert(constraints_.end(), result.begin(), result.end());
|
||||
|
@ -378,8 +378,8 @@ void SparsePoseGraph::WaitForAllComputations() {
|
|||
}
|
||||
std::cout << "\r\x1b[KOptimizing: Done. " << std::endl;
|
||||
constraint_builder_.WhenDone(
|
||||
[this, ¬ification](
|
||||
const sparse_pose_graph::ConstraintBuilder::Result& result) {
|
||||
[this,
|
||||
¬ification](const pose_graph::ConstraintBuilder::Result& result) {
|
||||
common::MutexLocker locker(&mutex_);
|
||||
constraints_.insert(constraints_.end(), result.begin(), result.end());
|
||||
notification = true;
|
||||
|
@ -419,7 +419,7 @@ void SparsePoseGraph::AddSubmapFromProto(
|
|||
submap_data_.at(submap_id).submap = submap_ptr;
|
||||
// Immediately show the submap at the 'global_submap_pose'.
|
||||
optimized_submap_transforms_.Insert(
|
||||
submap_id, sparse_pose_graph::SubmapData{global_submap_pose});
|
||||
submap_id, pose_graph::SubmapData{global_submap_pose});
|
||||
AddWorkItem([this, submap_id, global_submap_pose]() REQUIRES(mutex_) {
|
||||
CHECK_EQ(frozen_trajectories_.count(submap_id.trajectory_id), 1);
|
||||
submap_data_.at(submap_id).state = SubmapState::kFinished;
|
||||
|
@ -652,7 +652,7 @@ SparsePoseGraph::GetAllSubmapData() {
|
|||
}
|
||||
|
||||
transform::Rigid3d SparsePoseGraph::ComputeLocalToGlobalTransform(
|
||||
const mapping::MapById<mapping::SubmapId, sparse_pose_graph::SubmapData>&
|
||||
const mapping::MapById<mapping::SubmapId, pose_graph::SubmapData>&
|
||||
submap_transforms,
|
||||
const int trajectory_id) const {
|
||||
auto begin_it = submap_transforms.BeginOfTrajectory(trajectory_id);
|
||||
|
|
|
@ -35,8 +35,8 @@
|
|||
#include "cartographer/mapping/pose_graph_trimmer.h"
|
||||
#include "cartographer/mapping/sparse_pose_graph.h"
|
||||
#include "cartographer/mapping/trajectory_connectivity_state.h"
|
||||
#include "cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h"
|
||||
#include "cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h"
|
||||
#include "cartographer/mapping_3d/pose_graph/constraint_builder.h"
|
||||
#include "cartographer/mapping_3d/pose_graph/optimization_problem.h"
|
||||
#include "cartographer/mapping_3d/submaps.h"
|
||||
#include "cartographer/sensor/fixed_frame_pose_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
|
||||
// optimized 'submap_transforms'.
|
||||
transform::Rigid3d ComputeLocalToGlobalTransform(
|
||||
const mapping::MapById<mapping::SubmapId, sparse_pose_graph::SubmapData>&
|
||||
const mapping::MapById<mapping::SubmapId, pose_graph::SubmapData>&
|
||||
submap_transforms,
|
||||
int trajectory_id) const REQUIRES(mutex_);
|
||||
|
||||
|
@ -216,8 +216,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
|
|||
bool run_loop_closure_ GUARDED_BY(mutex_) = false;
|
||||
|
||||
// Current optimization problem.
|
||||
sparse_pose_graph::OptimizationProblem optimization_problem_;
|
||||
sparse_pose_graph::ConstraintBuilder constraint_builder_ GUARDED_BY(mutex_);
|
||||
pose_graph::OptimizationProblem optimization_problem_;
|
||||
pose_graph::ConstraintBuilder constraint_builder_ GUARDED_BY(mutex_);
|
||||
std::vector<Constraint> constraints_ GUARDED_BY(mutex_);
|
||||
|
||||
// Submaps get assigned an ID and state as soon as they are seen, even
|
||||
|
@ -231,7 +231,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
|
|||
int num_trajectory_nodes_ GUARDED_BY(mutex_) = 0;
|
||||
|
||||
// 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_);
|
||||
|
||||
// List of all trimmers to consult when optimizations finish.
|
||||
|
|
Loading…
Reference in New Issue