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

Move constraint_builders together. (#1103)

master
Alexander Belyaev 2018-04-23 13:49:25 +02:00 committed by GitHub
parent 3b5830745e
commit 3dd37da51b
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
16 changed files with 54 additions and 54 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -88,8 +88,8 @@ std::vector<SubmapId> PoseGraph2D::InitializeGlobalSubmapPoses(
optimization_problem_->AddSubmap(
trajectory_id,
first_submap_pose *
pose_graph::ComputeSubmapPose(*insertion_submaps[0]).inverse() *
pose_graph::ComputeSubmapPose(*insertion_submaps[1]));
constraints::ComputeSubmapPose(*insertion_submaps[0]).inverse() *
constraints::ComputeSubmapPose(*insertion_submaps[1]));
return {last_submap_id,
SubmapId{trajectory_id, last_submap_id.submap_index + 1}};
}
@ -246,7 +246,7 @@ void PoseGraph2D::ComputeConstraintsForNode(
transform::Rigid3d::Rotation(constant_data->gravity_alignment.inverse()));
const transform::Rigid2d global_pose_2d =
optimization_problem_->submap_data().at(matching_id).global_pose *
pose_graph::ComputeSubmapPose(*insertion_submaps.front()).inverse() *
constraints::ComputeSubmapPose(*insertion_submaps.front()).inverse() *
local_pose_2d;
optimization_problem_->AddTrajectoryNode(
matching_id.trajectory_id,
@ -260,7 +260,7 @@ void PoseGraph2D::ComputeConstraintsForNode(
CHECK(submap_data_.at(submap_id).state == SubmapState::kActive);
submap_data_.at(submap_id).node_ids.emplace(node_id);
const transform::Rigid2d constraint_transform =
pose_graph::ComputeSubmapPose(*insertion_submaps[i]).inverse() *
constraints::ComputeSubmapPose(*insertion_submaps[i]).inverse() *
local_pose_2d;
constraints_.push_back(Constraint{submap_id,
node_id,
@ -328,7 +328,7 @@ void PoseGraph2D::UpdateTrajectoryConnectivity(const Constraint& constraint) {
void PoseGraph2D::HandleWorkQueue() {
constraint_builder_.WhenDone(
[this](const pose_graph::ConstraintBuilder2D::Result& result) {
[this](const constraints::ConstraintBuilder2D::Result& result) {
{
common::MutexLocker locker(&mutex_);
constraints_.insert(constraints_.end(), result.begin(), result.end());
@ -390,7 +390,7 @@ void PoseGraph2D::WaitForAllComputations() {
std::cout << "\r\x1b[KOptimizing: Done. " << std::endl;
constraint_builder_.WhenDone(
[this,
&notification](const pose_graph::ConstraintBuilder2D::Result& result) {
&notification](const constraints::ConstraintBuilder2D::Result& result) {
common::MutexLocker locker(&mutex_);
constraints_.insert(constraints_.end(), result.begin(), result.end());
notification = true;

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

@ -33,7 +33,7 @@
#include "cartographer/common/thread_pool.h"
#include "cartographer/common/time.h"
#include "cartographer/mapping/2d/submap_2d.h"
#include "cartographer/mapping/internal/2d/pose_graph/constraint_builder_2d.h"
#include "cartographer/mapping/internal/constraints/constraint_builder_2d.h"
#include "cartographer/mapping/internal/optimization/optimization_problem_2d.h"
#include "cartographer/mapping/internal/trajectory_connectivity_state.h"
#include "cartographer/mapping/pose_graph.h"
@ -240,7 +240,7 @@ class PoseGraph2D : public PoseGraph {
// Current optimization problem.
std::unique_ptr<optimization::OptimizationProblem2D> optimization_problem_;
pose_graph::ConstraintBuilder2D constraint_builder_ GUARDED_BY(mutex_);
constraints::ConstraintBuilder2D 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

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

@ -343,7 +343,7 @@ void PoseGraph3D::UpdateTrajectoryConnectivity(const Constraint& constraint) {
void PoseGraph3D::HandleWorkQueue() {
constraint_builder_.WhenDone(
[this](const pose_graph::ConstraintBuilder3D::Result& result) {
[this](const constraints::ConstraintBuilder3D::Result& result) {
{
common::MutexLocker locker(&mutex_);
constraints_.insert(constraints_.end(), result.begin(), result.end());
@ -405,7 +405,7 @@ void PoseGraph3D::WaitForAllComputations() {
std::cout << "\r\x1b[KOptimizing: Done. " << std::endl;
constraint_builder_.WhenDone(
[this,
&notification](const pose_graph::ConstraintBuilder3D::Result& result) {
&notification](const constraints::ConstraintBuilder3D::Result& result) {
common::MutexLocker locker(&mutex_);
constraints_.insert(constraints_.end(), result.begin(), result.end());
notification = true;

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

@ -33,7 +33,7 @@
#include "cartographer/common/thread_pool.h"
#include "cartographer/common/time.h"
#include "cartographer/mapping/3d/submap_3d.h"
#include "cartographer/mapping/internal/3d/pose_graph/constraint_builder_3d.h"
#include "cartographer/mapping/internal/constraints/constraint_builder_3d.h"
#include "cartographer/mapping/internal/optimization/optimization_problem_3d.h"
#include "cartographer/mapping/internal/trajectory_connectivity_state.h"
#include "cartographer/mapping/pose_graph.h"
@ -244,7 +244,7 @@ class PoseGraph3D : public PoseGraph {
// Current optimization problem.
std::unique_ptr<optimization::OptimizationProblem3D> optimization_problem_;
pose_graph::ConstraintBuilder3D constraint_builder_ GUARDED_BY(mutex_);
constraints::ConstraintBuilder3D 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

6
cartographer/mapping/internal/pose_graph/constraint_builder.cc → cartographer/mapping/internal/constraints/constraint_builder.cc
View File

@ -14,7 +14,7 @@
* limitations under the License.
*/
#include "cartographer/mapping/internal/pose_graph/constraint_builder.h"
#include "cartographer/mapping/internal/constraints/constraint_builder.h"
#include "cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d.h"
#include "cartographer/mapping/internal/2d/scan_matching/fast_correlative_scan_matcher_2d.h"
@ -24,7 +24,7 @@
namespace cartographer {
namespace mapping {
namespace pose_graph {
namespace constraints {
proto::ConstraintBuilderOptions CreateConstraintBuilderOptions(
common::LuaParameterDictionary* const parameter_dictionary) {
@ -58,6 +58,6 @@ proto::ConstraintBuilderOptions CreateConstraintBuilderOptions(
return options;
}
} // namespace pose_graph
} // namespace constraints
} // namespace mapping
} // namespace cartographer

10
cartographer/mapping/internal/pose_graph/constraint_builder.h → cartographer/mapping/internal/constraints/constraint_builder.h
View File

@ -14,21 +14,21 @@
* limitations under the License.
*/
#ifndef CARTOGRAPHER_MAPPING_INTERNAL_POSE_GRAPH_CONSTRAINT_BUILDER_H_
#define CARTOGRAPHER_MAPPING_INTERNAL_POSE_GRAPH_CONSTRAINT_BUILDER_H_
#ifndef CARTOGRAPHER_MAPPING_INTERNAL_CONSTRAINTS_CONSTRAINT_BUILDER_H_
#define CARTOGRAPHER_MAPPING_INTERNAL_CONSTRAINTS_CONSTRAINT_BUILDER_H_
#include "cartographer/common/lua_parameter_dictionary.h"
#include "cartographer/mapping/pose_graph/proto/constraint_builder_options.pb.h"
namespace cartographer {
namespace mapping {
namespace pose_graph {
namespace constraints {
proto::ConstraintBuilderOptions CreateConstraintBuilderOptions(
common::LuaParameterDictionary* parameter_dictionary);
} // namespace pose_graph
} // namespace constraints
} // namespace mapping
} // namespace cartographer
#endif // CARTOGRAPHER_MAPPING_INTERNAL_POSE_GRAPH_CONSTRAINT_BUILDER_H_
#endif // CARTOGRAPHER_MAPPING_INTERNAL_CONSTRAINTS_CONSTRAINT_BUILDER_H_

8
cartographer/mapping/internal/2d/pose_graph/constraint_builder_2d.cc → cartographer/mapping/internal/constraints/constraint_builder_2d.cc
View File

@ -14,7 +14,7 @@
* limitations under the License.
*/
#include "cartographer/mapping/internal/2d/pose_graph/constraint_builder_2d.h"
#include "cartographer/mapping/internal/constraints/constraint_builder_2d.h"
#include <cmath>
#include <functional>
@ -39,7 +39,7 @@
namespace cartographer {
namespace mapping {
namespace pose_graph {
namespace constraints {
static auto* kConstraintsSearchedMetric = metrics::Counter::Null();
static auto* kConstraintsFoundMetric = metrics::Counter::Null();
@ -54,7 +54,7 @@ transform::Rigid2d ComputeSubmapPose(const Submap2D& submap) {
}
ConstraintBuilder2D::ConstraintBuilder2D(
const pose_graph::proto::ConstraintBuilderOptions& options,
const constraints::proto::ConstraintBuilderOptions& options,
common::ThreadPoolInterface* const thread_pool)
: options_(options),
thread_pool_(thread_pool),
@ -328,6 +328,6 @@ void ConstraintBuilder2D::RegisterMetrics(metrics::FamilyFactory* factory) {
kGlobalConstraintScoresMetric = scores->Add({{"search_region", "global"}});
}
} // namespace pose_graph
} // namespace constraints
} // namespace mapping
} // namespace cartographer

12
cartographer/mapping/internal/2d/pose_graph/constraint_builder_2d.h → cartographer/mapping/internal/constraints/constraint_builder_2d.h
View File

@ -14,8 +14,8 @@
* limitations under the License.
*/
#ifndef CARTOGRAPHER_MAPPING_INTERNAL_2D_POSE_GRAPH_CONSTRAINT_BUILDER_2D_H_
#define CARTOGRAPHER_MAPPING_INTERNAL_2D_POSE_GRAPH_CONSTRAINT_BUILDER_2D_H_
#ifndef CARTOGRAPHER_MAPPING_INTERNAL_CONSTRAINTS_CONSTRAINT_BUILDER_2D_H_
#define CARTOGRAPHER_MAPPING_INTERNAL_CONSTRAINTS_CONSTRAINT_BUILDER_2D_H_
#include <array>
#include <deque>
@ -41,7 +41,7 @@
namespace cartographer {
namespace mapping {
namespace pose_graph {
namespace constraints {
// Returns (map <- submap) where 'submap' is a coordinate system at the origin
// of the Submap.
@ -138,7 +138,7 @@ class ConstraintBuilder2D {
// runs the 'when_done_' callback and resets the state.
void FinishComputation(int computation_index) EXCLUDES(mutex_);
const pose_graph::proto::ConstraintBuilderOptions options_;
const constraints::proto::ConstraintBuilderOptions options_;
common::ThreadPoolInterface* thread_pool_;
common::Mutex mutex_;
@ -175,8 +175,8 @@ class ConstraintBuilder2D {
common::Histogram score_histogram_ GUARDED_BY(mutex_);
};
} // namespace pose_graph
} // namespace constraints
} // namespace mapping
} // namespace cartographer
#endif // CARTOGRAPHER_MAPPING_INTERNAL_2D_POSE_GRAPH_CONSTRAINT_BUILDER_2D_H_
#endif // CARTOGRAPHER_MAPPING_INTERNAL_CONSTRAINTS_CONSTRAINT_BUILDER_2D_H_

8
cartographer/mapping/internal/2d/pose_graph/constraint_builder_2d_test.cc → cartographer/mapping/internal/constraints/constraint_builder_2d_test.cc
View File

@ -14,20 +14,20 @@
* limitations under the License.
*/
#include "cartographer/mapping/internal/2d/pose_graph/constraint_builder_2d.h"
#include "cartographer/mapping/internal/constraints/constraint_builder_2d.h"
#include <functional>
#include "cartographer/common/internal/testing/thread_pool_for_testing.h"
#include "cartographer/mapping/2d/submap_2d.h"
#include "cartographer/mapping/internal/pose_graph/constraint_builder.h"
#include "cartographer/mapping/internal/constraints/constraint_builder.h"
#include "cartographer/mapping/internal/testing/test_helpers.h"
#include "gmock/gmock.h"
#include "gtest/gtest.h"
namespace cartographer {
namespace mapping {
namespace pose_graph {
namespace constraints {
namespace {
class MockCallback {
@ -103,6 +103,6 @@ TEST_F(ConstraintBuilder2DTest, FindsConstraints) {
}
} // namespace
} // namespace pose_graph
} // namespace constraints
} // namespace mapping
} // namespace cartographer

6
cartographer/mapping/internal/3d/pose_graph/constraint_builder_3d.cc → cartographer/mapping/internal/constraints/constraint_builder_3d.cc
View File

@ -14,7 +14,7 @@
* limitations under the License.
*/
#include "cartographer/mapping/internal/3d/pose_graph/constraint_builder_3d.h"
#include "cartographer/mapping/internal/constraints/constraint_builder_3d.h"
#include <cmath>
#include <functional>
@ -39,7 +39,7 @@
namespace cartographer {
namespace mapping {
namespace pose_graph {
namespace constraints {
static auto* kConstraintsSearchedMetric = metrics::Counter::Null();
static auto* kConstraintsFoundMetric = metrics::Counter::Null();
@ -371,6 +371,6 @@ void ConstraintBuilder3D::RegisterMetrics(metrics::FamilyFactory* factory) {
{{"search_region", "global"}, {"kind", "low_resolution_score"}});
}
} // namespace pose_graph
} // namespace constraints
} // namespace mapping
} // namespace cartographer

10
cartographer/mapping/internal/3d/pose_graph/constraint_builder_3d.h → cartographer/mapping/internal/constraints/constraint_builder_3d.h
View File

@ -14,8 +14,8 @@
* limitations under the License.
*/
#ifndef CARTOGRAPHER_MAPPING_INTERNAL_3D_POSE_GRAPH_CONSTRAINT_BUILDER_3D_H_
#define CARTOGRAPHER_MAPPING_INTERNAL_3D_POSE_GRAPH_CONSTRAINT_BUILDER_3D_H_
#ifndef CARTOGRAPHER_MAPPING_INTERNAL_CONSTRAINTS_CONSTRAINT_BUILDER_3D_H_
#define CARTOGRAPHER_MAPPING_INTERNAL_CONSTRAINTS_CONSTRAINT_BUILDER_3D_H_
#include <array>
#include <deque>
@ -44,7 +44,7 @@
namespace cartographer {
namespace mapping {
namespace pose_graph {
namespace constraints {
// Asynchronously computes constraints.
//
@ -191,8 +191,8 @@ class ConstraintBuilder3D {
common::Histogram low_resolution_score_histogram_ GUARDED_BY(mutex_);
};
} // namespace pose_graph
} // namespace constraints
} // namespace mapping
} // namespace cartographer
#endif // CARTOGRAPHER_MAPPING_INTERNAL_3D_POSE_GRAPH_CONSTRAINT_BUILDER_3D_H_
#endif // CARTOGRAPHER_MAPPING_INTERNAL_CONSTRAINTS_CONSTRAINT_BUILDER_3D_H_

8
cartographer/mapping/internal/3d/pose_graph/constraint_builder_3d_test.cc → cartographer/mapping/internal/constraints/constraint_builder_3d_test.cc
View File

@ -14,13 +14,13 @@
* limitations under the License.
*/
#include "cartographer/mapping/internal/3d/pose_graph/constraint_builder_3d.h"
#include "cartographer/mapping/internal/constraints/constraint_builder_3d.h"
#include <functional>
#include "cartographer/common/internal/testing/thread_pool_for_testing.h"
#include "cartographer/mapping/3d/submap_3d.h"
#include "cartographer/mapping/internal/pose_graph/constraint_builder.h"
#include "cartographer/mapping/internal/constraints/constraint_builder.h"
#include "cartographer/mapping/internal/testing/test_helpers.h"
#include "cartographer/mapping/pose_graph.h"
#include "cartographer/transform/rigid_transform.h"
@ -29,7 +29,7 @@
namespace cartographer {
namespace mapping {
namespace pose_graph {
namespace constraints {
namespace {
class MockCallback {
@ -113,6 +113,6 @@ TEST_F(ConstraintBuilder3DTest, FindsConstraints) {
}
} // namespace
} // namespace pose_graph
} // namespace constraints
} // namespace mapping
} // namespace cartographer

4
cartographer/mapping/pose_graph.cc
View File

@ -16,8 +16,8 @@
#include "cartographer/mapping/pose_graph.h"
#include "cartographer/mapping/internal/constraints/constraint_builder.h"
#include "cartographer/mapping/internal/optimization/optimization_problem_options.h"
#include "cartographer/mapping/internal/pose_graph/constraint_builder.h"
#include "cartographer/transform/transform.h"
#include "glog/logging.h"
@ -75,7 +75,7 @@ proto::PoseGraphOptions CreatePoseGraphOptions(
options.set_optimize_every_n_nodes(
parameter_dictionary->GetInt("optimize_every_n_nodes"));
*options.mutable_constraint_builder_options() =
pose_graph::CreateConstraintBuilderOptions(
constraints::CreateConstraintBuilderOptions(
parameter_dictionary->GetDictionary("constraint_builder").get());
options.set_matcher_translation_weight(
parameter_dictionary->GetDouble("matcher_translation_weight"));

2
cartographer/mapping/pose_graph/proto/constraint_builder_options.proto
View File

@ -14,7 +14,7 @@
syntax = "proto3";
package cartographer.mapping.pose_graph.proto;
package cartographer.mapping.constraints.proto;
import "cartographer/mapping/2d/scan_matching/proto/ceres_scan_matcher_options_2d.proto";
import "cartographer/mapping/2d/scan_matching/proto/fast_correlative_scan_matcher_options_2d.proto";

2
cartographer/mapping/proto/pose_graph_options.proto
View File

@ -25,7 +25,7 @@ message PoseGraphOptions {
int32 optimize_every_n_nodes = 1;
// Options for the constraint builder.
mapping.pose_graph.proto.ConstraintBuilderOptions
mapping.constraints.proto.ConstraintBuilderOptions
constraint_builder_options = 3;
// Weight used in the optimization problem for the translational component of

8
cartographer/metrics/register.cc
View File

@ -17,17 +17,17 @@
#include "cartographer/metrics/register.h"
#include "cartographer/mapping/internal/2d/local_trajectory_builder_2d.h"
#include "cartographer/mapping/internal/2d/pose_graph/constraint_builder_2d.h"
#include "cartographer/mapping/internal/3d/local_trajectory_builder_3d.h"
#include "cartographer/mapping/internal/3d/pose_graph/constraint_builder_3d.h"
#include "cartographer/mapping/internal/constraints/constraint_builder_2d.h"
#include "cartographer/mapping/internal/constraints/constraint_builder_3d.h"
#include "cartographer/mapping/internal/global_trajectory_builder.h"
namespace cartographer {
namespace metrics {
void RegisterAllMetrics(FamilyFactory* registry) {
mapping::pose_graph::ConstraintBuilder2D::RegisterMetrics(registry);
mapping::pose_graph::ConstraintBuilder3D::RegisterMetrics(registry);
mapping::constraints::ConstraintBuilder2D::RegisterMetrics(registry);
mapping::constraints::ConstraintBuilder3D::RegisterMetrics(registry);
mapping::GlobalTrajectoryBuilderRegisterMetrics(registry);
mapping::LocalTrajectoryBuilder2D::RegisterMetrics(registry);
mapping::LocalTrajectoryBuilder3D::RegisterMetrics(registry);