Move the mapping/sparse_pose_graph directory. (#669)
[RFC=0001](https://github.com/googlecartographer/rfcs/blob/master/text/0001-renaming-sparse-pose-graph.md)master
parent
7904808d40
commit
291c0f581b
|
@ -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
|
|
@ -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_
|
|
@ -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
|
|
@ -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_
|
|
@ -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";
|
|
@ -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";
|
||||||
|
|
|
@ -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
|
||||||
|
|
|
@ -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"));
|
||||||
|
|
|
@ -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"
|
||||||
|
|
|
@ -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),
|
||||||
|
|
|
@ -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_;
|
||||||
|
|
||||||
|
|
|
@ -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() {}
|
||||||
|
|
|
@ -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_;
|
||||||
|
|
|
@ -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"
|
||||||
|
|
|
@ -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),
|
||||||
|
|
|
@ -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_;
|
||||||
|
|
||||||
|
|
|
@ -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) {}
|
||||||
|
|
||||||
|
|
|
@ -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_;
|
||||||
|
|
|
@ -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());
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue