Wolfgang Hess 2017-11-15 09:39:45 +01:00 committed by Wally B. Feed
parent 7904808d40
commit 291c0f581b
19 changed files with 48 additions and 55 deletions

View File

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

View File

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

View File

@ -14,13 +14,13 @@
* limitations under the License. * limitations under the License.
*/ */
#include "cartographer/mapping/sparse_pose_graph/optimization_problem_options.h" #include "cartographer/mapping/pose_graph/optimization_problem_options.h"
#include "cartographer/common/ceres_solver_options.h" #include "cartographer/common/ceres_solver_options.h"
namespace cartographer { namespace cartographer {
namespace mapping { namespace mapping {
namespace sparse_pose_graph { namespace pose_graph {
proto::OptimizationProblemOptions CreateOptimizationProblemOptions( proto::OptimizationProblemOptions CreateOptimizationProblemOptions(
common::LuaParameterDictionary* const parameter_dictionary) { common::LuaParameterDictionary* const parameter_dictionary) {
@ -48,6 +48,6 @@ proto::OptimizationProblemOptions CreateOptimizationProblemOptions(
return options; return options;
} }
} // namespace sparse_pose_graph } // namespace pose_graph
} // namespace mapping } // namespace mapping
} // namespace cartographer } // namespace cartographer

View File

@ -14,21 +14,21 @@
* limitations under the License. * limitations under the License.
*/ */
#ifndef CARTOGRAPHER_MAPPING_SPARSE_POSE_GRAPH_OPTIMIZATION_PROBLEM_OPTIONS_H_ #ifndef CARTOGRAPHER_MAPPING_POSE_GRAPH_OPTIMIZATION_PROBLEM_OPTIONS_H_
#define CARTOGRAPHER_MAPPING_SPARSE_POSE_GRAPH_OPTIMIZATION_PROBLEM_OPTIONS_H_ #define CARTOGRAPHER_MAPPING_POSE_GRAPH_OPTIMIZATION_PROBLEM_OPTIONS_H_
#include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/common/lua_parameter_dictionary.h"
#include "cartographer/mapping/sparse_pose_graph/proto/optimization_problem_options.pb.h" #include "cartographer/mapping/pose_graph/proto/optimization_problem_options.pb.h"
namespace cartographer { namespace cartographer {
namespace mapping { namespace mapping {
namespace sparse_pose_graph { namespace pose_graph {
proto::OptimizationProblemOptions CreateOptimizationProblemOptions( proto::OptimizationProblemOptions CreateOptimizationProblemOptions(
common::LuaParameterDictionary* parameter_dictionary); common::LuaParameterDictionary* parameter_dictionary);
} // namespace sparse_pose_graph } // namespace pose_graph
} // namespace mapping } // namespace mapping
} // namespace cartographer } // namespace cartographer
#endif // CARTOGRAPHER_MAPPING_SPARSE_POSE_GRAPH_OPTIMIZATION_PROBLEM_OPTIONS_H_ #endif // CARTOGRAPHER_MAPPING_POSE_GRAPH_OPTIMIZATION_PROBLEM_OPTIONS_H_

View File

@ -14,7 +14,7 @@
syntax = "proto3"; syntax = "proto3";
package cartographer.mapping.sparse_pose_graph.proto; package cartographer.mapping.pose_graph.proto;
import "cartographer/mapping_2d/scan_matching/proto/ceres_scan_matcher_options.proto"; import "cartographer/mapping_2d/scan_matching/proto/ceres_scan_matcher_options.proto";
import "cartographer/mapping_2d/scan_matching/proto/fast_correlative_scan_matcher_options.proto"; import "cartographer/mapping_2d/scan_matching/proto/fast_correlative_scan_matcher_options.proto";

View File

@ -14,7 +14,7 @@
syntax = "proto3"; syntax = "proto3";
package cartographer.mapping.sparse_pose_graph.proto; package cartographer.mapping.pose_graph.proto;
import "cartographer/common/proto/ceres_solver_options.proto"; import "cartographer/common/proto/ceres_solver_options.proto";

View File

@ -16,8 +16,8 @@ syntax = "proto3";
package cartographer.mapping.proto; package cartographer.mapping.proto;
import "cartographer/mapping/sparse_pose_graph/proto/constraint_builder_options.proto"; import "cartographer/mapping/pose_graph/proto/constraint_builder_options.proto";
import "cartographer/mapping/sparse_pose_graph/proto/optimization_problem_options.proto"; import "cartographer/mapping/pose_graph/proto/optimization_problem_options.proto";
message SparsePoseGraphOptions { message SparsePoseGraphOptions {
// Online loop closure: If positive, will run the loop closure while the map // Online loop closure: If positive, will run the loop closure while the map
@ -25,7 +25,7 @@ message SparsePoseGraphOptions {
int32 optimize_every_n_scans = 1; int32 optimize_every_n_scans = 1;
// Options for the constraint builder. // Options for the constraint builder.
mapping.sparse_pose_graph.proto.ConstraintBuilderOptions mapping.pose_graph.proto.ConstraintBuilderOptions
constraint_builder_options = 3; constraint_builder_options = 3;
// Weight used in the optimization problem for the translational component of // Weight used in the optimization problem for the translational component of
@ -37,7 +37,7 @@ message SparsePoseGraphOptions {
double matcher_rotation_weight = 8; double matcher_rotation_weight = 8;
// Options for the optimization problem. // Options for the optimization problem.
mapping.sparse_pose_graph.proto.OptimizationProblemOptions mapping.pose_graph.proto.OptimizationProblemOptions
optimization_problem_options = 4; optimization_problem_options = 4;
// Number of iterations to use in 'optimization_problem_options' for the final // Number of iterations to use in 'optimization_problem_options' for the final

View File

@ -16,8 +16,8 @@
#include "cartographer/mapping/sparse_pose_graph.h" #include "cartographer/mapping/sparse_pose_graph.h"
#include "cartographer/mapping/sparse_pose_graph/constraint_builder.h" #include "cartographer/mapping/pose_graph/constraint_builder.h"
#include "cartographer/mapping/sparse_pose_graph/optimization_problem_options.h" #include "cartographer/mapping/pose_graph/optimization_problem_options.h"
#include "cartographer/transform/transform.h" #include "cartographer/transform/transform.h"
#include "glog/logging.h" #include "glog/logging.h"
@ -77,14 +77,14 @@ proto::SparsePoseGraphOptions CreateSparsePoseGraphOptions(
options.set_optimize_every_n_scans( options.set_optimize_every_n_scans(
parameter_dictionary->GetInt("optimize_every_n_scans")); parameter_dictionary->GetInt("optimize_every_n_scans"));
*options.mutable_constraint_builder_options() = *options.mutable_constraint_builder_options() =
sparse_pose_graph::CreateConstraintBuilderOptions( pose_graph::CreateConstraintBuilderOptions(
parameter_dictionary->GetDictionary("constraint_builder").get()); parameter_dictionary->GetDictionary("constraint_builder").get());
options.set_matcher_translation_weight( options.set_matcher_translation_weight(
parameter_dictionary->GetDouble("matcher_translation_weight")); parameter_dictionary->GetDouble("matcher_translation_weight"));
options.set_matcher_rotation_weight( options.set_matcher_rotation_weight(
parameter_dictionary->GetDouble("matcher_rotation_weight")); parameter_dictionary->GetDouble("matcher_rotation_weight"));
*options.mutable_optimization_problem_options() = *options.mutable_optimization_problem_options() =
sparse_pose_graph::CreateOptimizationProblemOptions( pose_graph::CreateOptimizationProblemOptions(
parameter_dictionary->GetDictionary("optimization_problem").get()); parameter_dictionary->GetDictionary("optimization_problem").get());
options.set_max_num_final_iterations( options.set_max_num_final_iterations(
parameter_dictionary->GetNonNegativeInt("max_num_final_iterations")); parameter_dictionary->GetNonNegativeInt("max_num_final_iterations"));

View File

@ -31,7 +31,7 @@
#include "Eigen/Eigenvalues" #include "Eigen/Eigenvalues"
#include "cartographer/common/make_unique.h" #include "cartographer/common/make_unique.h"
#include "cartographer/common/math.h" #include "cartographer/common/math.h"
#include "cartographer/mapping/sparse_pose_graph/proto/constraint_builder_options.pb.h" #include "cartographer/mapping/pose_graph/proto/constraint_builder_options.pb.h"
#include "cartographer/sensor/compressed_point_cloud.h" #include "cartographer/sensor/compressed_point_cloud.h"
#include "cartographer/sensor/voxel_filter.h" #include "cartographer/sensor/voxel_filter.h"
#include "glog/logging.h" #include "glog/logging.h"

View File

@ -43,7 +43,7 @@ transform::Rigid2d ComputeSubmapPose(const Submap& submap) {
} }
ConstraintBuilder::ConstraintBuilder( ConstraintBuilder::ConstraintBuilder(
const mapping::sparse_pose_graph::proto::ConstraintBuilderOptions& options, const mapping::pose_graph::proto::ConstraintBuilderOptions& options,
common::ThreadPool* const thread_pool) common::ThreadPool* const thread_pool)
: options_(options), : options_(options),
thread_pool_(thread_pool), thread_pool_(thread_pool),

View File

@ -30,8 +30,8 @@
#include "cartographer/common/math.h" #include "cartographer/common/math.h"
#include "cartographer/common/mutex.h" #include "cartographer/common/mutex.h"
#include "cartographer/common/thread_pool.h" #include "cartographer/common/thread_pool.h"
#include "cartographer/mapping/pose_graph/proto/constraint_builder_options.pb.h"
#include "cartographer/mapping/sparse_pose_graph.h" #include "cartographer/mapping/sparse_pose_graph.h"
#include "cartographer/mapping/sparse_pose_graph/proto/constraint_builder_options.pb.h"
#include "cartographer/mapping_2d/scan_matching/ceres_scan_matcher.h" #include "cartographer/mapping_2d/scan_matching/ceres_scan_matcher.h"
#include "cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.h" #include "cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.h"
#include "cartographer/mapping_2d/submaps.h" #include "cartographer/mapping_2d/submaps.h"
@ -62,8 +62,7 @@ class ConstraintBuilder {
using Result = std::vector<Constraint>; using Result = std::vector<Constraint>;
ConstraintBuilder( ConstraintBuilder(
const mapping::sparse_pose_graph::proto::ConstraintBuilderOptions& const mapping::pose_graph::proto::ConstraintBuilderOptions& options,
options,
common::ThreadPool* thread_pool); common::ThreadPool* thread_pool);
~ConstraintBuilder(); ~ConstraintBuilder();
@ -142,7 +141,7 @@ class ConstraintBuilder {
// runs the 'when_done_' callback and resets the state. // runs the 'when_done_' callback and resets the state.
void FinishComputation(int computation_index) EXCLUDES(mutex_); void FinishComputation(int computation_index) EXCLUDES(mutex_);
const mapping::sparse_pose_graph::proto::ConstraintBuilderOptions options_; const mapping::pose_graph::proto::ConstraintBuilderOptions options_;
common::ThreadPool* thread_pool_; common::ThreadPool* thread_pool_;
common::Mutex mutex_; common::Mutex mutex_;

View File

@ -55,8 +55,7 @@ transform::Rigid2d ToPose(const std::array<double, 3>& values) {
} // namespace } // namespace
OptimizationProblem::OptimizationProblem( OptimizationProblem::OptimizationProblem(
const mapping::sparse_pose_graph::proto::OptimizationProblemOptions& const mapping::pose_graph::proto::OptimizationProblemOptions& options)
options)
: options_(options) {} : options_(options) {}
OptimizationProblem::~OptimizationProblem() {} OptimizationProblem::~OptimizationProblem() {}

View File

@ -28,8 +28,8 @@
#include "cartographer/common/port.h" #include "cartographer/common/port.h"
#include "cartographer/common/time.h" #include "cartographer/common/time.h"
#include "cartographer/mapping/id.h" #include "cartographer/mapping/id.h"
#include "cartographer/mapping/pose_graph/proto/optimization_problem_options.pb.h"
#include "cartographer/mapping/sparse_pose_graph.h" #include "cartographer/mapping/sparse_pose_graph.h"
#include "cartographer/mapping/sparse_pose_graph/proto/optimization_problem_options.pb.h"
#include "cartographer/sensor/imu_data.h" #include "cartographer/sensor/imu_data.h"
#include "cartographer/sensor/map_by_time.h" #include "cartographer/sensor/map_by_time.h"
#include "cartographer/sensor/odometry_data.h" #include "cartographer/sensor/odometry_data.h"
@ -56,8 +56,7 @@ class OptimizationProblem {
using Constraint = mapping::SparsePoseGraph::Constraint; using Constraint = mapping::SparsePoseGraph::Constraint;
explicit OptimizationProblem( explicit OptimizationProblem(
const mapping::sparse_pose_graph::proto::OptimizationProblemOptions& const mapping::pose_graph::proto::OptimizationProblemOptions& options);
options);
~OptimizationProblem(); ~OptimizationProblem();
OptimizationProblem(const OptimizationProblem&) = delete; OptimizationProblem(const OptimizationProblem&) = delete;
@ -100,7 +99,7 @@ class OptimizationProblem {
int trajectory_id, const NodeData& first_node_data, int trajectory_id, const NodeData& first_node_data,
const NodeData& second_node_data) const; const NodeData& second_node_data) const;
mapping::sparse_pose_graph::proto::OptimizationProblemOptions options_; mapping::pose_graph::proto::OptimizationProblemOptions options_;
mapping::MapById<mapping::NodeId, NodeData> node_data_; mapping::MapById<mapping::NodeId, NodeData> node_data_;
mapping::MapById<mapping::SubmapId, SubmapData> submap_data_; mapping::MapById<mapping::SubmapId, SubmapData> submap_data_;
sensor::MapByTime<sensor::ImuData> imu_data_; sensor::MapByTime<sensor::ImuData> imu_data_;

View File

@ -30,7 +30,7 @@
#include "Eigen/Eigenvalues" #include "Eigen/Eigenvalues"
#include "cartographer/common/make_unique.h" #include "cartographer/common/make_unique.h"
#include "cartographer/common/math.h" #include "cartographer/common/math.h"
#include "cartographer/mapping/sparse_pose_graph/proto/constraint_builder_options.pb.h" #include "cartographer/mapping/pose_graph/proto/constraint_builder_options.pb.h"
#include "cartographer/sensor/compressed_point_cloud.h" #include "cartographer/sensor/compressed_point_cloud.h"
#include "cartographer/sensor/voxel_filter.h" #include "cartographer/sensor/voxel_filter.h"
#include "glog/logging.h" #include "glog/logging.h"

View File

@ -39,7 +39,7 @@ namespace mapping_3d {
namespace sparse_pose_graph { namespace sparse_pose_graph {
ConstraintBuilder::ConstraintBuilder( ConstraintBuilder::ConstraintBuilder(
const mapping::sparse_pose_graph::proto::ConstraintBuilderOptions& options, const mapping::pose_graph::proto::ConstraintBuilderOptions& options,
common::ThreadPool* const thread_pool) common::ThreadPool* const thread_pool)
: options_(options), : options_(options),
thread_pool_(thread_pool), thread_pool_(thread_pool),

View File

@ -58,8 +58,7 @@ class ConstraintBuilder {
using Result = std::vector<Constraint>; using Result = std::vector<Constraint>;
ConstraintBuilder( ConstraintBuilder(
const mapping::sparse_pose_graph::proto::ConstraintBuilderOptions& const mapping::pose_graph::proto::ConstraintBuilderOptions& options,
options,
common::ThreadPool* thread_pool); common::ThreadPool* thread_pool);
~ConstraintBuilder(); ~ConstraintBuilder();
@ -153,7 +152,7 @@ class ConstraintBuilder {
// runs the 'when_done_' callback and resets the state. // runs the 'when_done_' callback and resets the state.
void FinishComputation(int computation_index) EXCLUDES(mutex_); void FinishComputation(int computation_index) EXCLUDES(mutex_);
const mapping::sparse_pose_graph::proto::ConstraintBuilderOptions options_; const mapping::pose_graph::proto::ConstraintBuilderOptions options_;
common::ThreadPool* thread_pool_; common::ThreadPool* thread_pool_;
common::Mutex mutex_; common::Mutex mutex_;

View File

@ -74,8 +74,7 @@ std::unique_ptr<transform::Rigid3d> Interpolate(
} // namespace } // namespace
OptimizationProblem::OptimizationProblem( OptimizationProblem::OptimizationProblem(
const mapping::sparse_pose_graph::proto::OptimizationProblemOptions& const mapping::pose_graph::proto::OptimizationProblemOptions& options,
options,
FixZ fix_z) FixZ fix_z)
: options_(options), fix_z_(fix_z) {} : options_(options), fix_z_(fix_z) {}

View File

@ -28,8 +28,8 @@
#include "cartographer/common/port.h" #include "cartographer/common/port.h"
#include "cartographer/common/time.h" #include "cartographer/common/time.h"
#include "cartographer/mapping/id.h" #include "cartographer/mapping/id.h"
#include "cartographer/mapping/pose_graph/proto/optimization_problem_options.pb.h"
#include "cartographer/mapping/sparse_pose_graph.h" #include "cartographer/mapping/sparse_pose_graph.h"
#include "cartographer/mapping/sparse_pose_graph/proto/optimization_problem_options.pb.h"
#include "cartographer/sensor/fixed_frame_pose_data.h" #include "cartographer/sensor/fixed_frame_pose_data.h"
#include "cartographer/sensor/imu_data.h" #include "cartographer/sensor/imu_data.h"
#include "cartographer/sensor/map_by_time.h" #include "cartographer/sensor/map_by_time.h"
@ -58,8 +58,7 @@ class OptimizationProblem {
enum class FixZ { kYes, kNo }; enum class FixZ { kYes, kNo };
OptimizationProblem( OptimizationProblem(
const mapping::sparse_pose_graph::proto::OptimizationProblemOptions& const mapping::pose_graph::proto::OptimizationProblemOptions& options,
options,
FixZ fix_z); FixZ fix_z);
~OptimizationProblem(); ~OptimizationProblem();
@ -107,7 +106,7 @@ class OptimizationProblem {
std::array<double, 4> imu_calibration{{1., 0., 0., 0.}}; std::array<double, 4> imu_calibration{{1., 0., 0., 0.}};
}; };
mapping::sparse_pose_graph::proto::OptimizationProblemOptions options_; mapping::pose_graph::proto::OptimizationProblemOptions options_;
FixZ fix_z_; FixZ fix_z_;
mapping::MapById<mapping::NodeId, NodeData> node_data_; mapping::MapById<mapping::NodeId, NodeData> node_data_;
mapping::MapById<mapping::SubmapId, SubmapData> submap_data_; mapping::MapById<mapping::SubmapId, SubmapData> submap_data_;

View File

@ -21,7 +21,7 @@
#include "Eigen/Core" #include "Eigen/Core"
#include "cartographer/common/lua_parameter_dictionary_test_helpers.h" #include "cartographer/common/lua_parameter_dictionary_test_helpers.h"
#include "cartographer/common/time.h" #include "cartographer/common/time.h"
#include "cartographer/mapping/sparse_pose_graph/optimization_problem_options.h" #include "cartographer/mapping/pose_graph/optimization_problem_options.h"
#include "cartographer/transform/transform.h" #include "cartographer/transform/transform.h"
#include "glog/logging.h" #include "glog/logging.h"
#include "gmock/gmock.h" #include "gmock/gmock.h"
@ -37,8 +37,7 @@ class OptimizationProblemTest : public ::testing::Test {
: optimization_problem_(CreateOptions(), OptimizationProblem::FixZ::kNo), : optimization_problem_(CreateOptions(), OptimizationProblem::FixZ::kNo),
rng_(45387) {} rng_(45387) {}
mapping::sparse_pose_graph::proto::OptimizationProblemOptions mapping::pose_graph::proto::OptimizationProblemOptions CreateOptions() {
CreateOptions() {
auto parameter_dictionary = common::MakeDictionary(R"text( auto parameter_dictionary = common::MakeDictionary(R"text(
return { return {
acceleration_weight = 1e-4, acceleration_weight = 1e-4,
@ -55,7 +54,7 @@ class OptimizationProblemTest : public ::testing::Test {
num_threads = 4, num_threads = 4,
}, },
})text"); })text");
return mapping::sparse_pose_graph::CreateOptimizationProblemOptions( return mapping::pose_graph::CreateOptimizationProblemOptions(
parameter_dictionary.get()); parameter_dictionary.get());
} }