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

Move files related to optimization to optimization/. (#1102)

master
Alexander Belyaev 2018-04-20 20:52:17 +02:00 committed by Christoph Schütte
parent eebced5e16
commit 4aa2af23de
31 changed files with 155 additions and 155 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -41,7 +41,7 @@ namespace mapping {
PoseGraph2D::PoseGraph2D( PoseGraph2D::PoseGraph2D(
const proto::PoseGraphOptions& options, const proto::PoseGraphOptions& options,
std::unique_ptr<pose_graph::OptimizationProblem2D> optimization_problem, std::unique_ptr<optimization::OptimizationProblem2D> optimization_problem,
common::ThreadPool* thread_pool) common::ThreadPool* thread_pool)
: options_(options), : options_(options),
optimization_problem_(std::move(optimization_problem)), optimization_problem_(std::move(optimization_problem)),
@ -250,7 +250,8 @@ void PoseGraph2D::ComputeConstraintsForNode(
local_pose_2d; local_pose_2d;
optimization_problem_->AddTrajectoryNode( optimization_problem_->AddTrajectoryNode(
matching_id.trajectory_id, matching_id.trajectory_id,
pose_graph::NodeSpec2D{constant_data->time, local_pose_2d, global_pose_2d, optimization::NodeSpec2D{constant_data->time, local_pose_2d,
global_pose_2d,
constant_data->gravity_alignment}); constant_data->gravity_alignment});
for (size_t i = 0; i < insertion_submaps.size(); ++i) { for (size_t i = 0; i < insertion_submaps.size(); ++i) {
const SubmapId submap_id = submap_ids[i]; const SubmapId submap_id = submap_ids[i];
@ -446,8 +447,8 @@ void PoseGraph2D::AddSubmapFromProto(
submap_data_.Insert(submap_id, InternalSubmapData()); submap_data_.Insert(submap_id, InternalSubmapData());
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'.
global_submap_poses_.Insert(submap_id, global_submap_poses_.Insert(
pose_graph::SubmapSpec2D{global_submap_pose_2d}); submap_id, optimization::SubmapSpec2D{global_submap_pose_2d});
AddWorkItem([this, submap_id, global_submap_pose_2d]() REQUIRES(mutex_) { AddWorkItem([this, submap_id, global_submap_pose_2d]() REQUIRES(mutex_) {
submap_data_.at(submap_id).state = SubmapState::kFinished; submap_data_.at(submap_id).state = SubmapState::kFinished;
optimization_problem_->InsertSubmap(submap_id, global_submap_pose_2d); optimization_problem_->InsertSubmap(submap_id, global_submap_pose_2d);
@ -471,7 +472,7 @@ void PoseGraph2D::AddNodeFromProto(const transform::Rigid3d& global_pose,
constant_data->gravity_alignment.inverse()); constant_data->gravity_alignment.inverse());
optimization_problem_->InsertTrajectoryNode( optimization_problem_->InsertTrajectoryNode(
node_id, node_id,
pose_graph::NodeSpec2D{ optimization::NodeSpec2D{
constant_data->time, constant_data->time,
transform::Project2D(constant_data->local_pose * transform::Project2D(constant_data->local_pose *
gravity_alignment_inverse), gravity_alignment_inverse),
@ -746,7 +747,7 @@ PoseGraph2D::GetAllSubmapPoses() {
} }
transform::Rigid3d PoseGraph2D::ComputeLocalToGlobalTransform( transform::Rigid3d PoseGraph2D::ComputeLocalToGlobalTransform(
const MapById<SubmapId, pose_graph::SubmapSpec2D>& global_submap_poses, const MapById<SubmapId, optimization::SubmapSpec2D>& global_submap_poses,
const int trajectory_id) const { const int trajectory_id) const {
auto begin_it = global_submap_poses.BeginOfTrajectory(trajectory_id); auto begin_it = global_submap_poses.BeginOfTrajectory(trajectory_id);
auto end_it = global_submap_poses.EndOfTrajectory(trajectory_id); auto end_it = global_submap_poses.EndOfTrajectory(trajectory_id);

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

@ -34,7 +34,7 @@
#include "cartographer/common/time.h" #include "cartographer/common/time.h"
#include "cartographer/mapping/2d/submap_2d.h" #include "cartographer/mapping/2d/submap_2d.h"
#include "cartographer/mapping/internal/2d/pose_graph/constraint_builder_2d.h" #include "cartographer/mapping/internal/2d/pose_graph/constraint_builder_2d.h"
#include "cartographer/mapping/internal/2d/pose_graph/optimization_problem_2d.h" #include "cartographer/mapping/internal/optimization/optimization_problem_2d.h"
#include "cartographer/mapping/internal/trajectory_connectivity_state.h" #include "cartographer/mapping/internal/trajectory_connectivity_state.h"
#include "cartographer/mapping/pose_graph.h" #include "cartographer/mapping/pose_graph.h"
#include "cartographer/mapping/pose_graph_trimmer.h" #include "cartographer/mapping/pose_graph_trimmer.h"
@ -61,7 +61,7 @@ class PoseGraph2D : public PoseGraph {
public: public:
PoseGraph2D( PoseGraph2D(
const proto::PoseGraphOptions& options, const proto::PoseGraphOptions& options,
std::unique_ptr<pose_graph::OptimizationProblem2D> optimization_problem, std::unique_ptr<optimization::OptimizationProblem2D> optimization_problem,
common::ThreadPool* thread_pool); common::ThreadPool* thread_pool);
~PoseGraph2D() override; ~PoseGraph2D() override;
@ -201,7 +201,7 @@ class PoseGraph2D : public PoseGraph {
// Computes the local to global map frame transform based on the given // Computes the local to global map frame transform based on the given
// 'global_submap_poses'. // 'global_submap_poses'.
transform::Rigid3d ComputeLocalToGlobalTransform( transform::Rigid3d ComputeLocalToGlobalTransform(
const MapById<SubmapId, pose_graph::SubmapSpec2D>& global_submap_poses, const MapById<SubmapId, optimization::SubmapSpec2D>& global_submap_poses,
int trajectory_id) const REQUIRES(mutex_); int trajectory_id) const REQUIRES(mutex_);
SubmapData GetSubmapDataUnderLock(const SubmapId& submap_id) REQUIRES(mutex_); SubmapData GetSubmapDataUnderLock(const SubmapId& submap_id) REQUIRES(mutex_);
@ -239,7 +239,7 @@ class PoseGraph2D : public PoseGraph {
void DispatchOptimization() REQUIRES(mutex_); void DispatchOptimization() REQUIRES(mutex_);
// Current optimization problem. // Current optimization problem.
std::unique_ptr<pose_graph::OptimizationProblem2D> optimization_problem_; std::unique_ptr<optimization::OptimizationProblem2D> optimization_problem_;
pose_graph::ConstraintBuilder2D constraint_builder_ GUARDED_BY(mutex_); pose_graph::ConstraintBuilder2D constraint_builder_ GUARDED_BY(mutex_);
std::vector<Constraint> constraints_ GUARDED_BY(mutex_); std::vector<Constraint> constraints_ GUARDED_BY(mutex_);
@ -252,7 +252,7 @@ class PoseGraph2D : public PoseGraph {
int num_trajectory_nodes_ GUARDED_BY(mutex_) = 0; int num_trajectory_nodes_ GUARDED_BY(mutex_) = 0;
// Global submap poses currently used for displaying data. // Global submap poses currently used for displaying data.
MapById<SubmapId, pose_graph::SubmapSpec2D> global_submap_poses_ MapById<SubmapId, optimization::SubmapSpec2D> global_submap_poses_
GUARDED_BY(mutex_); GUARDED_BY(mutex_);
// Global landmark poses with all observations. // Global landmark poses with all observations.

2
cartographer/mapping/internal/2d/pose_graph_2d_test.cc
View File

@ -137,7 +137,7 @@ class PoseGraph2DTest : public ::testing::Test {
auto options = CreatePoseGraphOptions(parameter_dictionary.get()); auto options = CreatePoseGraphOptions(parameter_dictionary.get());
pose_graph_ = common::make_unique<PoseGraph2D>( pose_graph_ = common::make_unique<PoseGraph2D>(
options, options,
common::make_unique<pose_graph::OptimizationProblem2D>( common::make_unique<optimization::OptimizationProblem2D>(
options.optimization_problem_options()), options.optimization_problem_options()),
&thread_pool_); &thread_pool_);
} }

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

@ -41,7 +41,7 @@ namespace mapping {
PoseGraph3D::PoseGraph3D( PoseGraph3D::PoseGraph3D(
const proto::PoseGraphOptions& options, const proto::PoseGraphOptions& options,
std::unique_ptr<pose_graph::OptimizationProblem3D> optimization_problem, std::unique_ptr<optimization::OptimizationProblem3D> optimization_problem,
common::ThreadPool* thread_pool) common::ThreadPool* thread_pool)
: options_(options), : options_(options),
optimization_problem_(std::move(optimization_problem)), optimization_problem_(std::move(optimization_problem)),
@ -267,7 +267,7 @@ void PoseGraph3D::ComputeConstraintsForNode(
insertion_submaps.front()->local_pose().inverse() * local_pose; insertion_submaps.front()->local_pose().inverse() * local_pose;
optimization_problem_->AddTrajectoryNode( optimization_problem_->AddTrajectoryNode(
matching_id.trajectory_id, matching_id.trajectory_id,
pose_graph::NodeSpec3D{constant_data->time, local_pose, global_pose}); optimization::NodeSpec3D{constant_data->time, local_pose, global_pose});
for (size_t i = 0; i < insertion_submaps.size(); ++i) { for (size_t i = 0; i < insertion_submaps.size(); ++i) {
const SubmapId submap_id = submap_ids[i]; const SubmapId submap_id = submap_ids[i];
// Even if this was the last node added to 'submap_id', the submap will only // Even if this was the last node added to 'submap_id', the submap will only
@ -461,7 +461,7 @@ void PoseGraph3D::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'.
global_submap_poses_.Insert(submap_id, global_submap_poses_.Insert(submap_id,
pose_graph::SubmapSpec3D{global_submap_pose}); optimization::SubmapSpec3D{global_submap_pose});
AddWorkItem([this, submap_id, global_submap_pose]() REQUIRES(mutex_) { AddWorkItem([this, submap_id, global_submap_pose]() REQUIRES(mutex_) {
submap_data_.at(submap_id).state = SubmapState::kFinished; submap_data_.at(submap_id).state = SubmapState::kFinished;
optimization_problem_->InsertSubmap(submap_id, global_submap_pose); optimization_problem_->InsertSubmap(submap_id, global_submap_pose);
@ -483,7 +483,7 @@ void PoseGraph3D::AddNodeFromProto(const transform::Rigid3d& global_pose,
const auto& constant_data = trajectory_nodes_.at(node_id).constant_data; const auto& constant_data = trajectory_nodes_.at(node_id).constant_data;
optimization_problem_->InsertTrajectoryNode( optimization_problem_->InsertTrajectoryNode(
node_id, node_id,
pose_graph::NodeSpec3D{constant_data->time, constant_data->local_pose, optimization::NodeSpec3D{constant_data->time, constant_data->local_pose,
global_pose}); global_pose});
}); });
} }
@ -778,7 +778,7 @@ PoseGraph3D::GetAllSubmapPoses() {
} }
transform::Rigid3d PoseGraph3D::ComputeLocalToGlobalTransform( transform::Rigid3d PoseGraph3D::ComputeLocalToGlobalTransform(
const MapById<SubmapId, pose_graph::SubmapSpec3D>& global_submap_poses, const MapById<SubmapId, optimization::SubmapSpec3D>& global_submap_poses,
const int trajectory_id) const { const int trajectory_id) const {
auto begin_it = global_submap_poses.BeginOfTrajectory(trajectory_id); auto begin_it = global_submap_poses.BeginOfTrajectory(trajectory_id);
auto end_it = global_submap_poses.EndOfTrajectory(trajectory_id); auto end_it = global_submap_poses.EndOfTrajectory(trajectory_id);

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

@ -34,7 +34,7 @@
#include "cartographer/common/time.h" #include "cartographer/common/time.h"
#include "cartographer/mapping/3d/submap_3d.h" #include "cartographer/mapping/3d/submap_3d.h"
#include "cartographer/mapping/internal/3d/pose_graph/constraint_builder_3d.h" #include "cartographer/mapping/internal/3d/pose_graph/constraint_builder_3d.h"
#include "cartographer/mapping/internal/3d/pose_graph/optimization_problem_3d.h" #include "cartographer/mapping/internal/optimization/optimization_problem_3d.h"
#include "cartographer/mapping/internal/trajectory_connectivity_state.h" #include "cartographer/mapping/internal/trajectory_connectivity_state.h"
#include "cartographer/mapping/pose_graph.h" #include "cartographer/mapping/pose_graph.h"
#include "cartographer/mapping/pose_graph_trimmer.h" #include "cartographer/mapping/pose_graph_trimmer.h"
@ -60,7 +60,7 @@ class PoseGraph3D : public PoseGraph {
public: public:
PoseGraph3D( PoseGraph3D(
const proto::PoseGraphOptions& options, const proto::PoseGraphOptions& options,
std::unique_ptr<pose_graph::OptimizationProblem3D> optimization_problem, std::unique_ptr<optimization::OptimizationProblem3D> optimization_problem,
common::ThreadPool* thread_pool); common::ThreadPool* thread_pool);
~PoseGraph3D() override; ~PoseGraph3D() override;
@ -200,7 +200,7 @@ class PoseGraph3D : public PoseGraph {
// Computes the local to global map frame transform based on the given // Computes the local to global map frame transform based on the given
// 'global_submap_poses'. // 'global_submap_poses'.
transform::Rigid3d ComputeLocalToGlobalTransform( transform::Rigid3d ComputeLocalToGlobalTransform(
const MapById<SubmapId, pose_graph::SubmapSpec3D>& global_submap_poses, const MapById<SubmapId, optimization::SubmapSpec3D>& global_submap_poses,
int trajectory_id) const REQUIRES(mutex_); int trajectory_id) const REQUIRES(mutex_);
PoseGraph::SubmapData GetSubmapDataUnderLock(const SubmapId& submap_id) PoseGraph::SubmapData GetSubmapDataUnderLock(const SubmapId& submap_id)
@ -243,7 +243,7 @@ class PoseGraph3D : public PoseGraph {
void DispatchOptimization() REQUIRES(mutex_); void DispatchOptimization() REQUIRES(mutex_);
// Current optimization problem. // Current optimization problem.
std::unique_ptr<pose_graph::OptimizationProblem3D> optimization_problem_; std::unique_ptr<optimization::OptimizationProblem3D> optimization_problem_;
pose_graph::ConstraintBuilder3D constraint_builder_ GUARDED_BY(mutex_); pose_graph::ConstraintBuilder3D constraint_builder_ GUARDED_BY(mutex_);
std::vector<Constraint> constraints_ GUARDED_BY(mutex_); std::vector<Constraint> constraints_ GUARDED_BY(mutex_);
@ -256,7 +256,7 @@ class PoseGraph3D : public PoseGraph {
int num_trajectory_nodes_ GUARDED_BY(mutex_) = 0; int num_trajectory_nodes_ GUARDED_BY(mutex_) = 0;
// Global submap poses currently used for displaying data. // Global submap poses currently used for displaying data.
MapById<SubmapId, pose_graph::SubmapSpec3D> global_submap_poses_ MapById<SubmapId, optimization::SubmapSpec3D> global_submap_poses_
GUARDED_BY(mutex_); GUARDED_BY(mutex_);
// Global landmark poses with all observations. // Global landmark poses with all observations.

8
cartographer/mapping/internal/3d/pose_graph_3d_test.cc
View File

@ -28,8 +28,8 @@ namespace cartographer {
namespace mapping { namespace mapping {
namespace { namespace {
using ::cartographer::mapping::pose_graph::OptimizationProblem3D; using ::cartographer::mapping::optimization::OptimizationProblem3D;
using ::cartographer::mapping::pose_graph::proto::OptimizationProblemOptions; using ::cartographer::mapping::optimization::proto::OptimizationProblemOptions;
using ::cartographer::transform::Rigid3d; using ::cartographer::transform::Rigid3d;
class MockOptimizationProblem3D : public OptimizationProblem3D { class MockOptimizationProblem3D : public OptimizationProblem3D {
@ -46,7 +46,7 @@ class PoseGraph3DForTesting : public PoseGraph3D {
public: public:
PoseGraph3DForTesting( PoseGraph3DForTesting(
const proto::PoseGraphOptions& options, const proto::PoseGraphOptions& options,
std::unique_ptr<pose_graph::OptimizationProblem3D> optimization_problem, std::unique_ptr<optimization::OptimizationProblem3D> optimization_problem,
common::ThreadPool* thread_pool) common::ThreadPool* thread_pool)
: PoseGraph3D(options, std::move(optimization_problem), thread_pool) {} : PoseGraph3D(options, std::move(optimization_problem), thread_pool) {}
@ -68,7 +68,7 @@ class PoseGraph3DTest : public ::testing::Test {
void BuildPoseGraph() { void BuildPoseGraph() {
auto optimization_problem = auto optimization_problem =
common::make_unique<pose_graph::OptimizationProblem3D>( common::make_unique<optimization::OptimizationProblem3D>(
pose_graph_options_.optimization_problem_options()); pose_graph_options_.optimization_problem_options());
pose_graph_ = common::make_unique<PoseGraph3DForTesting>( pose_graph_ = common::make_unique<PoseGraph3DForTesting>(
pose_graph_options_, std::move(optimization_problem), pose_graph_options_, std::move(optimization_problem),

4
cartographer/mapping/internal/3d/scan_matching/ceres_scan_matcher_3d.cc
View File

@ -26,7 +26,7 @@
#include "cartographer/mapping/internal/3d/scan_matching/occupied_space_cost_function_3d.h" #include "cartographer/mapping/internal/3d/scan_matching/occupied_space_cost_function_3d.h"
#include "cartographer/mapping/internal/3d/scan_matching/rotation_delta_cost_functor_3d.h" #include "cartographer/mapping/internal/3d/scan_matching/rotation_delta_cost_functor_3d.h"
#include "cartographer/mapping/internal/3d/scan_matching/translation_delta_cost_functor_3d.h" #include "cartographer/mapping/internal/3d/scan_matching/translation_delta_cost_functor_3d.h"
#include "cartographer/mapping/internal/pose_graph/ceres_pose.h" #include "cartographer/mapping/internal/optimization/ceres_pose.h"
#include "cartographer/transform/rigid_transform.h" #include "cartographer/transform/rigid_transform.h"
#include "cartographer/transform/transform.h" #include "cartographer/transform/transform.h"
#include "ceres/ceres.h" #include "ceres/ceres.h"
@ -76,7 +76,7 @@ void CeresScanMatcher3D::Match(
transform::Rigid3d* const pose_estimate, transform::Rigid3d* const pose_estimate,
ceres::Solver::Summary* const summary) { ceres::Solver::Summary* const summary) {
ceres::Problem problem; ceres::Problem problem;
pose_graph::CeresPose ceres_pose( optimization::CeresPose ceres_pose(
initial_pose_estimate, nullptr /* translation_parameterization */, initial_pose_estimate, nullptr /* translation_parameterization */,
options_.only_optimize_yaw() options_.only_optimize_yaw()
? std::unique_ptr<ceres::LocalParameterization>( ? std::unique_ptr<ceres::LocalParameterization>(

6
cartographer/mapping/internal/pose_graph/ceres_pose.cc → cartographer/mapping/internal/optimization/ceres_pose.cc
View File

@ -14,11 +14,11 @@
* limitations under the License. * limitations under the License.
*/ */
#include "cartographer/mapping/internal/pose_graph/ceres_pose.h" #include "cartographer/mapping/internal/optimization/ceres_pose.h"
namespace cartographer { namespace cartographer {
namespace mapping { namespace mapping {
namespace pose_graph { namespace optimization {
CeresPose::CeresPose( CeresPose::CeresPose(
const transform::Rigid3d& rigid, const transform::Rigid3d& rigid,
@ -40,6 +40,6 @@ const transform::Rigid3d CeresPose::ToRigid() const {
return transform::Rigid3d::FromArrays(data_->rotation, data_->translation); return transform::Rigid3d::FromArrays(data_->rotation, data_->translation);
} }
} // namespace pose_graph } // namespace optimization
} // namespace mapping } // namespace mapping
} // namespace cartographer } // namespace cartographer

10
cartographer/mapping/internal/pose_graph/ceres_pose.h → cartographer/mapping/internal/optimization/ceres_pose.h
View File

@ -14,8 +14,8 @@
* limitations under the License. * limitations under the License.
*/ */
#ifndef CARTOGRAPHER_MAPPING_INTERNAL_POSE_GRAPH_CERES_POSE_H_ #ifndef CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_CERES_POSE_H_
#define CARTOGRAPHER_MAPPING_INTERNAL_POSE_GRAPH_CERES_POSE_H_ #define CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_CERES_POSE_H_
#include <array> #include <array>
#include <memory> #include <memory>
@ -26,7 +26,7 @@
namespace cartographer { namespace cartographer {
namespace mapping { namespace mapping {
namespace pose_graph { namespace optimization {
class CeresPose { class CeresPose {
public: public:
@ -53,8 +53,8 @@ class CeresPose {
std::shared_ptr<Data> data_; std::shared_ptr<Data> data_;
}; };
} // namespace pose_graph } // namespace optimization
} // namespace mapping } // namespace mapping
} // namespace cartographer } // namespace cartographer
#endif // CARTOGRAPHER_MAPPING_INTERNAL_POSE_GRAPH_CERES_POSE_H_ #endif // CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_CERES_POSE_H_

6
cartographer/mapping/internal/3d/acceleration_cost_function_3d.h → cartographer/mapping/internal/optimization/cost_functions/acceleration_cost_function_3d.h
View File

@ -14,8 +14,8 @@
* limitations under the License. * limitations under the License.
*/ */
#ifndef CARTOGRAPHER_MAPPING_INTERNAL_3D_ACCELERATION_COST_FUNCTION_3D_H_ #ifndef CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_COST_FUNCTIONS_ACCELERATION_COST_FUNCTION_3D_H_
#define CARTOGRAPHER_MAPPING_INTERNAL_3D_ACCELERATION_COST_FUNCTION_3D_H_ #define CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_COST_FUNCTIONS_ACCELERATION_COST_FUNCTION_3D_H_
#include "Eigen/Core" #include "Eigen/Core"
#include "Eigen/Geometry" #include "Eigen/Geometry"
@ -101,4 +101,4 @@ class AccelerationCostFunction3D {
} // namespace mapping } // namespace mapping
} // namespace cartographer } // namespace cartographer
#endif // CARTOGRAPHER_MAPPING_INTERNAL_3D_ACCELERATION_COST_FUNCTION_3D_H_ #endif // CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_COST_FUNCTIONS_ACCELERATION_COST_FUNCTION_3D_H_

12
cartographer/mapping/internal/pose_graph/cost_helpers.h → cartographer/mapping/internal/optimization/cost_functions/cost_helpers.h
View File

@ -14,8 +14,8 @@
* limitations under the License. * limitations under the License.
*/ */
#ifndef CARTOGRAPHER_MAPPING_INTERNAL_POSE_GRAPH_COST_HELPERS_H_ #ifndef CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_COST_FUNCTIONS_COST_HELPERS_H_
#define CARTOGRAPHER_MAPPING_INTERNAL_POSE_GRAPH_COST_HELPERS_H_ #define CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_COST_FUNCTIONS_COST_HELPERS_H_
#include "Eigen/Core" #include "Eigen/Core"
#include "Eigen/Geometry" #include "Eigen/Geometry"
@ -24,7 +24,7 @@
namespace cartographer { namespace cartographer {
namespace mapping { namespace mapping {
namespace pose_graph { namespace optimization {
// Computes the error between the given relative pose and the difference of // Computes the error between the given relative pose and the difference of
// poses 'start' and 'end' which are both in an arbitrary common frame. // poses 'start' and 'end' which are both in an arbitrary common frame.
@ -80,10 +80,10 @@ InterpolateNodes2D(const T* const prev_node_pose,
const Eigen::Quaterniond& next_node_gravity_alignment, const Eigen::Quaterniond& next_node_gravity_alignment,
const double interpolation_parameter); const double interpolation_parameter);
} // namespace pose_graph } // namespace optimization
} // namespace mapping } // namespace mapping
} // namespace cartographer } // namespace cartographer
#include "cartographer/mapping/internal/pose_graph/cost_helpers_impl.h" #include "cartographer/mapping/internal/optimization/cost_functions/cost_helpers_impl.h"
#endif // CARTOGRAPHER_MAPPING_INTERNAL_POSE_GRAPH_COST_HELPERS_H_ #endif // CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_COST_FUNCTIONS_COST_HELPERS_H_

10
cartographer/mapping/internal/pose_graph/cost_helpers_impl.h → cartographer/mapping/internal/optimization/cost_functions/cost_helpers_impl.h
View File

@ -14,8 +14,8 @@
* limitations under the License. * limitations under the License.
*/ */
#ifndef CARTOGRAPHER_MAPPING_INTERNAL_POSE_GRAPH_COST_HELPERS_IMPL_H_ #ifndef CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_COST_FUNCTIONS_COST_HELPERS_IMPL_H_
#define CARTOGRAPHER_MAPPING_INTERNAL_POSE_GRAPH_COST_HELPERS_IMPL_H_ #define CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_COST_FUNCTIONS_COST_HELPERS_IMPL_H_
#include "Eigen/Core" #include "Eigen/Core"
#include "cartographer/transform/rigid_transform.h" #include "cartographer/transform/rigid_transform.h"
@ -23,7 +23,7 @@
namespace cartographer { namespace cartographer {
namespace mapping { namespace mapping {
namespace pose_graph { namespace optimization {
template <typename T> template <typename T>
static std::array<T, 3> ComputeUnscaledError( static std::array<T, 3> ComputeUnscaledError(
@ -190,8 +190,8 @@ InterpolateNodes2D(const T* const prev_node_pose,
T(0)}}); T(0)}});
} }
} // namespace pose_graph } // namespace optimization
} // namespace mapping } // namespace mapping
} // namespace cartographer } // namespace cartographer
#endif // CARTOGRAPHER_MAPPING_INTERNAL_POSE_GRAPH_COST_HELPERS_IMPL_H_ #endif // CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_COST_FUNCTIONS_COST_HELPERS_IMPL_H_

14
cartographer/mapping/internal/2d/pose_graph/landmark_cost_function_2d.h → cartographer/mapping/internal/optimization/cost_functions/landmark_cost_function_2d.h
View File

@ -14,13 +14,13 @@
* limitations under the License. * limitations under the License.
*/ */
#ifndef CARTOGRAPHER_MAPPING_INTERNAL_2D_POSE_GRAPH_LANDMARK_COST_FUNCTION_2D_H_ #ifndef CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_COST_FUNCTIONS_LANDMARK_COST_FUNCTION_2D_H_
#define CARTOGRAPHER_MAPPING_INTERNAL_2D_POSE_GRAPH_LANDMARK_COST_FUNCTION_2D_H_ #define CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_COST_FUNCTIONS_LANDMARK_COST_FUNCTION_2D_H_
#include "Eigen/Core" #include "Eigen/Core"
#include "Eigen/Geometry" #include "Eigen/Geometry"
#include "cartographer/mapping/internal/2d/pose_graph/optimization_problem_2d.h" #include "cartographer/mapping/internal/optimization/cost_functions/cost_helpers.h"
#include "cartographer/mapping/internal/pose_graph/cost_helpers.h" #include "cartographer/mapping/internal/optimization/optimization_problem_2d.h"
#include "cartographer/mapping/pose_graph_interface.h" #include "cartographer/mapping/pose_graph_interface.h"
#include "cartographer/transform/rigid_transform.h" #include "cartographer/transform/rigid_transform.h"
#include "cartographer/transform/transform.h" #include "cartographer/transform/transform.h"
@ -29,7 +29,7 @@
namespace cartographer { namespace cartographer {
namespace mapping { namespace mapping {
namespace pose_graph { namespace optimization {
// Cost function measuring the weighted error between the observed pose given by // Cost function measuring the weighted error between the observed pose given by
// the landmark measurement and the linearly interpolated pose of embedded in 3D // the landmark measurement and the linearly interpolated pose of embedded in 3D
@ -91,8 +91,8 @@ class LandmarkCostFunction2D {
const double interpolation_parameter_; const double interpolation_parameter_;
}; };
} // namespace pose_graph } // namespace optimization
} // namespace mapping } // namespace mapping
} // namespace cartographer } // namespace cartographer
#endif // CARTOGRAPHER_MAPPING_INTERNAL_2D_POSE_GRAPH_LANDMARK_COST_FUNCTION_2D_H_ #endif // CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_COST_FUNCTIONS_LANDMARK_COST_FUNCTION_2D_H_

6
cartographer/mapping/internal/2d/pose_graph/landmark_cost_function_2d_test.cc → cartographer/mapping/internal/optimization/cost_functions/landmark_cost_function_2d_test.cc
View File

@ -14,7 +14,7 @@
* limitations under the License. * limitations under the License.
*/ */
#include "cartographer/mapping/internal/2d/pose_graph/landmark_cost_function_2d.h" #include "cartographer/mapping/internal/optimization/cost_functions/landmark_cost_function_2d.h"
#include <memory> #include <memory>
@ -24,7 +24,7 @@
namespace cartographer { namespace cartographer {
namespace mapping { namespace mapping {
namespace pose_graph { namespace optimization {
namespace { namespace {
using ::testing::DoubleEq; using ::testing::DoubleEq;
@ -72,6 +72,6 @@ TEST(LandmarkCostFunctionTest, SmokeTest) {
} }
} // namespace } // namespace
} // namespace pose_graph } // namespace optimization
} // namespace mapping } // namespace mapping
} // namespace cartographer } // namespace cartographer

14
cartographer/mapping/internal/3d/pose_graph/landmark_cost_function_3d.h → cartographer/mapping/internal/optimization/cost_functions/landmark_cost_function_3d.h
View File

@ -14,13 +14,13 @@
* limitations under the License. * limitations under the License.
*/ */
#ifndef CARTOGRAPHER_MAPPING_INTERNAL_3D_POSE_GRAPH_LANDMARK_COST_FUNCTION_3D_H_ #ifndef CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_COST_FUNCTIONS_LANDMARK_COST_FUNCTION_3D_H_
#define CARTOGRAPHER_MAPPING_INTERNAL_3D_POSE_GRAPH_LANDMARK_COST_FUNCTION_3D_H_ #define CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_COST_FUNCTIONS_LANDMARK_COST_FUNCTION_3D_H_
#include "Eigen/Core" #include "Eigen/Core"
#include "Eigen/Geometry" #include "Eigen/Geometry"
#include "cartographer/mapping/internal/3d/pose_graph/optimization_problem_3d.h" #include "cartographer/mapping/internal/optimization/cost_functions/cost_helpers.h"
#include "cartographer/mapping/internal/pose_graph/cost_helpers.h" #include "cartographer/mapping/internal/optimization/optimization_problem_3d.h"
#include "cartographer/mapping/pose_graph_interface.h" #include "cartographer/mapping/pose_graph_interface.h"
#include "cartographer/transform/rigid_transform.h" #include "cartographer/transform/rigid_transform.h"
#include "cartographer/transform/transform.h" #include "cartographer/transform/transform.h"
@ -29,7 +29,7 @@
namespace cartographer { namespace cartographer {
namespace mapping { namespace mapping {
namespace pose_graph { namespace optimization {
// Cost function measuring the weighted error between the observed pose given by // Cost function measuring the weighted error between the observed pose given by
// the landmark measurement and the linearly interpolated pose. // the landmark measurement and the linearly interpolated pose.
@ -92,8 +92,8 @@ class LandmarkCostFunction3D {
const double interpolation_parameter_; const double interpolation_parameter_;
}; };
} // namespace pose_graph } // namespace optimization
} // namespace mapping } // namespace mapping
} // namespace cartographer } // namespace cartographer
#endif // CARTOGRAPHER_MAPPING_INTERNAL_3D_POSE_GRAPH_LANDMARK_COST_FUNCTION_3D_H_ #endif // CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_COST_FUNCTIONS_LANDMARK_COST_FUNCTION_3D_H_

6
cartographer/mapping/internal/3d/pose_graph/landmark_cost_function_3d_test.cc → cartographer/mapping/internal/optimization/cost_functions/landmark_cost_function_3d_test.cc
View File

@ -14,7 +14,7 @@
* limitations under the License. * limitations under the License.
*/ */
#include "cartographer/mapping/internal/3d/pose_graph/landmark_cost_function_3d.h" #include "cartographer/mapping/internal/optimization/cost_functions/landmark_cost_function_3d.h"
#include <memory> #include <memory>
@ -24,7 +24,7 @@
namespace cartographer { namespace cartographer {
namespace mapping { namespace mapping {
namespace pose_graph { namespace optimization {
namespace { namespace {
using ::testing::DoubleEq; using ::testing::DoubleEq;
@ -72,6 +72,6 @@ TEST(LandmarkCostFunction3DTest, SmokeTest) {
} }
} // namespace } // namespace
} // namespace pose_graph } // namespace optimization
} // namespace mapping } // namespace mapping
} // namespace cartographer } // namespace cartographer

18
cartographer/mapping/internal/3d/rotation_cost_function_3d.h → cartographer/mapping/internal/optimization/cost_functions/rotation_cost_function_3d.h
View File

@ -14,8 +14,8 @@
* limitations under the License. * limitations under the License.
*/ */
#ifndef CARTOGRAPHER_MAPPING_INTERNAL_3D_ROTATION_COST_FUNCTION_3D_H_ #ifndef CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_COST_FUNCTIONS_ROTATION_COST_FUNCTION_3D_H_
#define CARTOGRAPHER_MAPPING_INTERNAL_3D_ROTATION_COST_FUNCTION_3D_H_ #define CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_COST_FUNCTIONS_ROTATION_COST_FUNCTION_3D_H_
#include "Eigen/Core" #include "Eigen/Core"
#include "Eigen/Geometry" #include "Eigen/Geometry"
@ -25,15 +25,15 @@ namespace cartographer {
namespace mapping { namespace mapping {
// Penalizes differences between IMU data and optimized orientations. // Penalizes differences between IMU data and optimized orientations.
class RotationCostFunction { class RotationCostFunction3D {
public: public:
static ceres::CostFunction* CreateAutoDiffCostFunction( static ceres::CostFunction* CreateAutoDiffCostFunction(
const double scaling_factor, const double scaling_factor,
const Eigen::Quaterniond& delta_rotation_imu_frame) { const Eigen::Quaterniond& delta_rotation_imu_frame) {
return new ceres::AutoDiffCostFunction< return new ceres::AutoDiffCostFunction<
RotationCostFunction, 3 /* residuals */, 4 /* rotation variables */, RotationCostFunction3D, 3 /* residuals */, 4 /* rotation variables */,
4 /* rotation variables */, 4 /* rotation variables */ 4 /* rotation variables */, 4 /* rotation variables */
>(new RotationCostFunction(scaling_factor, delta_rotation_imu_frame)); >(new RotationCostFunction3D(scaling_factor, delta_rotation_imu_frame));
} }
template <typename T> template <typename T>
@ -56,13 +56,13 @@ class RotationCostFunction {
} }
private: private:
RotationCostFunction(const double scaling_factor, RotationCostFunction3D(const double scaling_factor,
const Eigen::Quaterniond& delta_rotation_imu_frame) const Eigen::Quaterniond& delta_rotation_imu_frame)
: scaling_factor_(scaling_factor), : scaling_factor_(scaling_factor),
delta_rotation_imu_frame_(delta_rotation_imu_frame) {} delta_rotation_imu_frame_(delta_rotation_imu_frame) {}
RotationCostFunction(const RotationCostFunction&) = delete; RotationCostFunction3D(const RotationCostFunction3D&) = delete;
RotationCostFunction& operator=(const RotationCostFunction&) = delete; RotationCostFunction3D& operator=(const RotationCostFunction3D&) = delete;
const double scaling_factor_; const double scaling_factor_;
const Eigen::Quaterniond delta_rotation_imu_frame_; const Eigen::Quaterniond delta_rotation_imu_frame_;
@ -71,4 +71,4 @@ class RotationCostFunction {
} // namespace mapping } // namespace mapping
} // namespace cartographer } // namespace cartographer
#endif // CARTOGRAPHER_MAPPING_INTERNAL_3D_ROTATION_COST_FUNCTION_3D_H_ #endif // CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_COST_FUNCTIONS_ROTATION_COST_FUNCTION_3D_H_

16
cartographer/mapping/internal/2d/pose_graph/spa_cost_function_2d.h → cartographer/mapping/internal/optimization/cost_functions/spa_cost_function_2d.h
View File

@ -14,15 +14,15 @@
* limitations under the License. * limitations under the License.
*/ */
#ifndef CARTOGRAPHER_MAPPING_INTERNAL_2D_POSE_GRAPH_SPA_COST_FUNCTION_2D_H_ #ifndef CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_COST_FUNCTIONS_SPA_COST_FUNCTION_2D_H_
#define CARTOGRAPHER_MAPPING_INTERNAL_2D_POSE_GRAPH_SPA_COST_FUNCTION_2D_H_ #define CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_COST_FUNCTIONS_SPA_COST_FUNCTION_2D_H_
#include <array> #include <array>
#include "Eigen/Core" #include "Eigen/Core"
#include "Eigen/Geometry" #include "Eigen/Geometry"
#include "cartographer/common/math.h" #include "cartographer/common/math.h"
#include "cartographer/mapping/internal/pose_graph/cost_helpers.h" #include "cartographer/mapping/internal/optimization/cost_functions/cost_helpers.h"
#include "cartographer/mapping/pose_graph.h" #include "cartographer/mapping/pose_graph.h"
#include "cartographer/transform/rigid_transform.h" #include "cartographer/transform/rigid_transform.h"
#include "cartographer/transform/transform.h" #include "cartographer/transform/transform.h"
@ -31,7 +31,7 @@
namespace cartographer { namespace cartographer {
namespace mapping { namespace mapping {
namespace pose_graph { namespace optimization {
class SpaCostFunction2D { class SpaCostFunction2D {
public: public:
@ -45,8 +45,8 @@ class SpaCostFunction2D {
template <typename T> template <typename T>
bool operator()(const T* const c_i, const T* const c_j, T* e) const { bool operator()(const T* const c_i, const T* const c_j, T* e) const {
using pose_graph::ComputeUnscaledError; using optimization::ComputeUnscaledError;
using pose_graph::ScaleError; using optimization::ScaleError;
const std::array<T, 3> error = ScaleError( const std::array<T, 3> error = ScaleError(
ComputeUnscaledError(transform::Project2D(pose_.zbar_ij), c_i, c_j), ComputeUnscaledError(transform::Project2D(pose_.zbar_ij), c_i, c_j),
@ -62,8 +62,8 @@ class SpaCostFunction2D {
const PoseGraph::Constraint::Pose pose_; const PoseGraph::Constraint::Pose pose_;
}; };
} // namespace pose_graph } // namespace optimization
} // namespace mapping } // namespace mapping
} // namespace cartographer } // namespace cartographer
#endif // CARTOGRAPHER_MAPPING_INTERNAL_2D_POSE_GRAPH_SPA_COST_FUNCTION_2D_H_ #endif // CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_COST_FUNCTIONS_SPA_COST_FUNCTION_2D_H_

12
cartographer/mapping/internal/3d/pose_graph/spa_cost_function_3d.h → cartographer/mapping/internal/optimization/cost_functions/spa_cost_function_3d.h
View File

@ -14,15 +14,15 @@
* limitations under the License. * limitations under the License.
*/ */
#ifndef CARTOGRAPHER_MAPPING_INTERNAL_3D_POSE_GRAPH_SPA_COST_FUNCTION_3D_H_ #ifndef CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_COST_FUNCTIONS_SPA_COST_FUNCTION_3D_H_
#define CARTOGRAPHER_MAPPING_INTERNAL_3D_POSE_GRAPH_SPA_COST_FUNCTION_3D_H_ #define CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_COST_FUNCTIONS_SPA_COST_FUNCTION_3D_H_
#include <array> #include <array>
#include "Eigen/Core" #include "Eigen/Core"
#include "Eigen/Geometry" #include "Eigen/Geometry"
#include "cartographer/common/math.h" #include "cartographer/common/math.h"
#include "cartographer/mapping/internal/pose_graph/cost_helpers.h" #include "cartographer/mapping/internal/optimization/cost_functions/cost_helpers.h"
#include "cartographer/mapping/pose_graph.h" #include "cartographer/mapping/pose_graph.h"
#include "cartographer/transform/rigid_transform.h" #include "cartographer/transform/rigid_transform.h"
#include "cartographer/transform/transform.h" #include "cartographer/transform/transform.h"
@ -31,7 +31,7 @@
namespace cartographer { namespace cartographer {
namespace mapping { namespace mapping {
namespace pose_graph { namespace optimization {
class SpaCostFunction3D { class SpaCostFunction3D {
public: public:
@ -62,8 +62,8 @@ class SpaCostFunction3D {
const PoseGraph::Constraint::Pose pose_; const PoseGraph::Constraint::Pose pose_;
}; };
} // namespace pose_graph } // namespace optimization
} // namespace mapping } // namespace mapping
} // namespace cartographer } // namespace cartographer
#endif // CARTOGRAPHER_MAPPING_INTERNAL_3D_POSE_GRAPH_SPA_COST_FUNCTION_3D_H_ #endif // CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_COST_FUNCTIONS_SPA_COST_FUNCTION_3D_H_

15
cartographer/mapping/internal/2d/pose_graph/optimization_problem_2d.cc → cartographer/mapping/internal/optimization/optimization_problem_2d.cc
View File

@ -14,7 +14,7 @@
* limitations under the License. * limitations under the License.
*/ */
#include "cartographer/mapping/internal/2d/pose_graph/optimization_problem_2d.h" #include "cartographer/mapping/internal/optimization/optimization_problem_2d.h"
#include <algorithm> #include <algorithm>
#include <array> #include <array>
@ -27,10 +27,9 @@
#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/internal/2d/pose_graph/landmark_cost_function_2d.h" #include "cartographer/mapping/internal/optimization/ceres_pose.h"
#include "cartographer/mapping/internal/2d/pose_graph/spa_cost_function_2d.h" #include "cartographer/mapping/internal/optimization/cost_functions/landmark_cost_function_2d.h"
#include "cartographer/mapping/internal/pose_graph/ceres_pose.h" #include "cartographer/mapping/internal/optimization/cost_functions/spa_cost_function_2d.h"
#include "cartographer/mapping/internal/pose_graph/cost_helpers.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"
@ -38,10 +37,10 @@
namespace cartographer { namespace cartographer {
namespace mapping { namespace mapping {
namespace pose_graph { namespace optimization {
namespace { namespace {
using ::cartographer::mapping::pose_graph::CeresPose; using ::cartographer::mapping::optimization::CeresPose;
using LandmarkNode = ::cartographer::mapping::PoseGraphInterface::LandmarkNode; using LandmarkNode = ::cartographer::mapping::PoseGraphInterface::LandmarkNode;
// Converts a pose into the 3 optimization variable format used for Ceres: // Converts a pose into the 3 optimization variable format used for Ceres:
@ -361,6 +360,6 @@ OptimizationProblem2D::CalculateOdometryBetweenNodes(
return nullptr; return nullptr;
} }
} // namespace pose_graph } // namespace optimization
} // namespace mapping } // namespace mapping
} // namespace cartographer } // namespace cartographer

16
cartographer/mapping/internal/2d/pose_graph/optimization_problem_2d.h → cartographer/mapping/internal/optimization/optimization_problem_2d.h
View File

@ -14,8 +14,8 @@
* limitations under the License. * limitations under the License.
*/ */
#ifndef CARTOGRAPHER_MAPPING_INTERNAL_2D_POSE_GRAPH_OPTIMIZATION_PROBLEM_2D_H_ #ifndef CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_OPTIMIZATION_PROBLEM_2D_H_
#define CARTOGRAPHER_MAPPING_INTERNAL_2D_POSE_GRAPH_OPTIMIZATION_PROBLEM_2D_H_ #define CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_OPTIMIZATION_PROBLEM_2D_H_
#include <array> #include <array>
#include <deque> #include <deque>
@ -28,7 +28,7 @@
#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/internal/pose_graph/optimization_problem_interface.h" #include "cartographer/mapping/internal/optimization/optimization_problem_interface.h"
#include "cartographer/mapping/pose_graph/proto/optimization_problem_options.pb.h" #include "cartographer/mapping/pose_graph/proto/optimization_problem_options.pb.h"
#include "cartographer/mapping/pose_graph_interface.h" #include "cartographer/mapping/pose_graph_interface.h"
#include "cartographer/sensor/imu_data.h" #include "cartographer/sensor/imu_data.h"
@ -38,7 +38,7 @@
namespace cartographer { namespace cartographer {
namespace mapping { namespace mapping {
namespace pose_graph { namespace optimization {
struct NodeSpec2D { struct NodeSpec2D {
common::Time time; common::Time time;
@ -56,7 +56,7 @@ class OptimizationProblem2D
transform::Rigid2d> { transform::Rigid2d> {
public: public:
explicit OptimizationProblem2D( explicit OptimizationProblem2D(
const pose_graph::proto::OptimizationProblemOptions& options); const optimization::proto::OptimizationProblemOptions& options);
~OptimizationProblem2D(); ~OptimizationProblem2D();
OptimizationProblem2D(const OptimizationProblem2D&) = delete; OptimizationProblem2D(const OptimizationProblem2D&) = delete;
@ -108,7 +108,7 @@ class OptimizationProblem2D
int trajectory_id, const NodeSpec2D& first_node_data, int trajectory_id, const NodeSpec2D& first_node_data,
const NodeSpec2D& second_node_data) const; const NodeSpec2D& second_node_data) const;
pose_graph::proto::OptimizationProblemOptions options_; optimization::proto::OptimizationProblemOptions options_;
MapById<NodeId, NodeSpec2D> node_data_; MapById<NodeId, NodeSpec2D> node_data_;
MapById<SubmapId, SubmapSpec2D> submap_data_; MapById<SubmapId, SubmapSpec2D> submap_data_;
std::map<std::string, transform::Rigid3d> landmark_data_; std::map<std::string, transform::Rigid3d> landmark_data_;
@ -116,8 +116,8 @@ class OptimizationProblem2D
sensor::MapByTime<sensor::OdometryData> odometry_data_; sensor::MapByTime<sensor::OdometryData> odometry_data_;
}; };
} // namespace pose_graph } // namespace optimization
} // namespace mapping } // namespace mapping
} // namespace cartographer } // namespace cartographer
#endif // CARTOGRAPHER_MAPPING_INTERNAL_2D_POSE_GRAPH_OPTIMIZATION_PROBLEM_2D_H_ #endif // CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_OPTIMIZATION_PROBLEM_2D_H_

20
cartographer/mapping/internal/3d/pose_graph/optimization_problem_3d.cc → cartographer/mapping/internal/optimization/optimization_problem_3d.cc
View File

@ -14,7 +14,7 @@
* limitations under the License. * limitations under the License.
*/ */
#include "cartographer/mapping/internal/3d/pose_graph/optimization_problem_3d.h" #include "cartographer/mapping/internal/optimization/optimization_problem_3d.h"
#include <algorithm> #include <algorithm>
#include <array> #include <array>
@ -30,13 +30,13 @@
#include "cartographer/common/make_unique.h" #include "cartographer/common/make_unique.h"
#include "cartographer/common/math.h" #include "cartographer/common/math.h"
#include "cartographer/common/time.h" #include "cartographer/common/time.h"
#include "cartographer/mapping/internal/3d/acceleration_cost_function_3d.h"
#include "cartographer/mapping/internal/3d/imu_integration.h" #include "cartographer/mapping/internal/3d/imu_integration.h"
#include "cartographer/mapping/internal/3d/pose_graph/landmark_cost_function_3d.h"
#include "cartographer/mapping/internal/3d/pose_graph/spa_cost_function_3d.h"
#include "cartographer/mapping/internal/3d/rotation_cost_function_3d.h"
#include "cartographer/mapping/internal/3d/rotation_parameterization.h" #include "cartographer/mapping/internal/3d/rotation_parameterization.h"
#include "cartographer/mapping/internal/pose_graph/ceres_pose.h" #include "cartographer/mapping/internal/optimization/ceres_pose.h"
#include "cartographer/mapping/internal/optimization/cost_functions/acceleration_cost_function_3d.h"
#include "cartographer/mapping/internal/optimization/cost_functions/landmark_cost_function_3d.h"
#include "cartographer/mapping/internal/optimization/cost_functions/rotation_cost_function_3d.h"
#include "cartographer/mapping/internal/optimization/cost_functions/spa_cost_function_3d.h"
#include "cartographer/transform/timestamped_transform.h" #include "cartographer/transform/timestamped_transform.h"
#include "cartographer/transform/transform.h" #include "cartographer/transform/transform.h"
#include "ceres/ceres.h" #include "ceres/ceres.h"
@ -46,7 +46,7 @@
namespace cartographer { namespace cartographer {
namespace mapping { namespace mapping {
namespace pose_graph { namespace optimization {
namespace { namespace {
using LandmarkNode = ::cartographer::mapping::PoseGraphInterface::LandmarkNode; using LandmarkNode = ::cartographer::mapping::PoseGraphInterface::LandmarkNode;
@ -188,7 +188,7 @@ void AddLandmarkCostFunctions(
} // namespace } // namespace
OptimizationProblem3D::OptimizationProblem3D( OptimizationProblem3D::OptimizationProblem3D(
const pose_graph::proto::OptimizationProblemOptions& options) const optimization::proto::OptimizationProblemOptions& options)
: options_(options) {} : options_(options) {}
OptimizationProblem3D::~OptimizationProblem3D() {} OptimizationProblem3D::~OptimizationProblem3D() {}
@ -425,7 +425,7 @@ void OptimizationProblem3D::Solve(
trajectory_data.imu_calibration.data()); trajectory_data.imu_calibration.data());
} }
problem.AddResidualBlock( problem.AddResidualBlock(
RotationCostFunction::CreateAutoDiffCostFunction( RotationCostFunction3D::CreateAutoDiffCostFunction(
options_.rotation_weight(), result.delta_rotation), options_.rotation_weight(), result.delta_rotation),
nullptr /* loss function */, C_nodes.at(first_node_id).rotation(), nullptr /* loss function */, C_nodes.at(first_node_id).rotation(),
C_nodes.at(second_node_id).rotation(), C_nodes.at(second_node_id).rotation(),
@ -605,6 +605,6 @@ OptimizationProblem3D::CalculateOdometryBetweenNodes(
return nullptr; return nullptr;
} }
} // namespace pose_graph } // namespace optimization
} // namespace mapping } // namespace mapping
} // namespace cartographer } // namespace cartographer

16
cartographer/mapping/internal/3d/pose_graph/optimization_problem_3d.h → cartographer/mapping/internal/optimization/optimization_problem_3d.h
View File

@ -14,8 +14,8 @@
* limitations under the License. * limitations under the License.
*/ */
#ifndef CARTOGRAPHER_MAPPING_INTERNAL_3D_POSE_GRAPH_OPTIMIZATION_PROBLEM_3D_H_ #ifndef CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_OPTIMIZATION_PROBLEM_3D_H_
#define CARTOGRAPHER_MAPPING_INTERNAL_3D_POSE_GRAPH_OPTIMIZATION_PROBLEM_3D_H_ #define CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_OPTIMIZATION_PROBLEM_3D_H_
#include <array> #include <array>
#include <map> #include <map>
@ -28,7 +28,7 @@
#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/internal/pose_graph/optimization_problem_interface.h" #include "cartographer/mapping/internal/optimization/optimization_problem_interface.h"
#include "cartographer/mapping/pose_graph/proto/optimization_problem_options.pb.h" #include "cartographer/mapping/pose_graph/proto/optimization_problem_options.pb.h"
#include "cartographer/mapping/pose_graph_interface.h" #include "cartographer/mapping/pose_graph_interface.h"
#include "cartographer/sensor/fixed_frame_pose_data.h" #include "cartographer/sensor/fixed_frame_pose_data.h"
@ -39,7 +39,7 @@
namespace cartographer { namespace cartographer {
namespace mapping { namespace mapping {
namespace pose_graph { namespace optimization {
struct NodeSpec3D { struct NodeSpec3D {
common::Time time; common::Time time;
@ -56,7 +56,7 @@ class OptimizationProblem3D
transform::Rigid3d> { transform::Rigid3d> {
public: public:
explicit OptimizationProblem3D( explicit OptimizationProblem3D(
const pose_graph::proto::OptimizationProblemOptions& options); const optimization::proto::OptimizationProblemOptions& options);
~OptimizationProblem3D(); ~OptimizationProblem3D();
OptimizationProblem3D(const OptimizationProblem3D&) = delete; OptimizationProblem3D(const OptimizationProblem3D&) = delete;
@ -121,7 +121,7 @@ class OptimizationProblem3D
int trajectory_id, const NodeSpec3D& first_node_data, int trajectory_id, const NodeSpec3D& first_node_data,
const NodeSpec3D& second_node_data) const; const NodeSpec3D& second_node_data) const;
pose_graph::proto::OptimizationProblemOptions options_; optimization::proto::OptimizationProblemOptions options_;
MapById<NodeId, NodeSpec3D> node_data_; MapById<NodeId, NodeSpec3D> node_data_;
MapById<SubmapId, SubmapSpec3D> submap_data_; MapById<SubmapId, SubmapSpec3D> submap_data_;
std::map<std::string, transform::Rigid3d> landmark_data_; std::map<std::string, transform::Rigid3d> landmark_data_;
@ -131,8 +131,8 @@ class OptimizationProblem3D
std::map<int, PoseGraphInterface::TrajectoryData> trajectory_data_; std::map<int, PoseGraphInterface::TrajectoryData> trajectory_data_;
}; };
} // namespace pose_graph } // namespace optimization
} // namespace mapping } // namespace mapping
} // namespace cartographer } // namespace cartographer
#endif // CARTOGRAPHER_MAPPING_INTERNAL_3D_POSE_GRAPH_OPTIMIZATION_PROBLEM_3D_H_ #endif // CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_OPTIMIZATION_PROBLEM_3D_H_

12
cartographer/mapping/internal/3d/pose_graph/optimization_problem_3d_test.cc → cartographer/mapping/internal/optimization/optimization_problem_3d_test.cc
View File

@ -14,21 +14,21 @@
* limitations under the License. * limitations under the License.
*/ */
#include "cartographer/mapping/internal/3d/pose_graph/optimization_problem_3d.h" #include "cartographer/mapping/internal/optimization/optimization_problem_3d.h"
#include <random> #include <random>
#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/internal/pose_graph/optimization_problem_options.h" #include "cartographer/mapping/internal/optimization/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"
namespace cartographer { namespace cartographer {
namespace mapping { namespace mapping {
namespace pose_graph { namespace optimization {
namespace { namespace {
class OptimizationProblem3DTest : public ::testing::Test { class OptimizationProblem3DTest : public ::testing::Test {
@ -36,7 +36,7 @@ class OptimizationProblem3DTest : public ::testing::Test {
OptimizationProblem3DTest() OptimizationProblem3DTest()
: optimization_problem_(CreateOptions()), rng_(45387) {} : optimization_problem_(CreateOptions()), rng_(45387) {}
pose_graph::proto::OptimizationProblemOptions CreateOptions() { optimization::proto::OptimizationProblemOptions 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 +55,7 @@ class OptimizationProblem3DTest : public ::testing::Test {
num_threads = 4, num_threads = 4,
}, },
})text"); })text");
return pose_graph::CreateOptimizationProblemOptions( return optimization::CreateOptimizationProblemOptions(
parameter_dictionary.get()); parameter_dictionary.get());
} }
@ -191,6 +191,6 @@ TEST_F(OptimizationProblem3DTest, ReducesNoise) {
} }
} // namespace } // namespace
} // namespace pose_graph } // namespace optimization
} // namespace mapping } // namespace mapping
} // namespace cartographer } // namespace cartographer

10
cartographer/mapping/internal/pose_graph/optimization_problem_interface.h → cartographer/mapping/internal/optimization/optimization_problem_interface.h
View File

@ -14,8 +14,8 @@
* limitations under the License. * limitations under the License.
*/ */
#ifndef CARTOGRAPHER_MAPPING_INTERNAL_POSE_GRAPH_OPTIMIZATION_PROBLEM_INTERFACE_H_ #ifndef CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_OPTIMIZATION_PROBLEM_INTERFACE_H_
#define CARTOGRAPHER_MAPPING_INTERNAL_POSE_GRAPH_OPTIMIZATION_PROBLEM_INTERFACE_H_ #define CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_OPTIMIZATION_PROBLEM_INTERFACE_H_
#include <map> #include <map>
#include <set> #include <set>
@ -33,7 +33,7 @@
namespace cartographer { namespace cartographer {
namespace mapping { namespace mapping {
namespace pose_graph { namespace optimization {
// Implements the SPA loop closure method. // Implements the SPA loop closure method.
template <typename NodeDataType, typename SubmapDataType, template <typename NodeDataType, typename SubmapDataType,
@ -81,8 +81,8 @@ class OptimizationProblemInterface {
const = 0; const = 0;
}; };
} // namespace pose_graph } // namespace optimization
} // namespace mapping } // namespace mapping
} // namespace cartographer } // namespace cartographer
#endif // CARTOGRAPHER_MAPPING_INTERNAL_POSE_GRAPH_OPTIMIZATION_PROBLEM_INTERFACE_H_ #endif // CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_OPTIMIZATION_PROBLEM_INTERFACE_H_

6
cartographer/mapping/internal/pose_graph/optimization_problem_options.cc → cartographer/mapping/internal/optimization/optimization_problem_options.cc
View File

@ -14,13 +14,13 @@
* limitations under the License. * limitations under the License.
*/ */
#include "cartographer/mapping/internal/pose_graph/optimization_problem_options.h" #include "cartographer/mapping/internal/optimization/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 pose_graph { namespace optimization {
proto::OptimizationProblemOptions CreateOptimizationProblemOptions( proto::OptimizationProblemOptions CreateOptimizationProblemOptions(
common::LuaParameterDictionary* const parameter_dictionary) { common::LuaParameterDictionary* const parameter_dictionary) {
@ -50,6 +50,6 @@ proto::OptimizationProblemOptions CreateOptimizationProblemOptions(
return options; return options;
} }
} // namespace pose_graph } // namespace optimization
} // namespace mapping } // namespace mapping
} // namespace cartographer } // namespace cartographer

10
cartographer/mapping/internal/pose_graph/optimization_problem_options.h → cartographer/mapping/internal/optimization/optimization_problem_options.h
View File

@ -14,21 +14,21 @@
* limitations under the License. * limitations under the License.
*/ */
#ifndef CARTOGRAPHER_MAPPING_INTERNAL_POSE_GRAPH_OPTIMIZATION_PROBLEM_OPTIONS_H_ #ifndef CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_OPTIMIZATION_PROBLEM_OPTIONS_H_
#define CARTOGRAPHER_MAPPING_INTERNAL_POSE_GRAPH_OPTIMIZATION_PROBLEM_OPTIONS_H_ #define CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_OPTIMIZATION_PROBLEM_OPTIONS_H_
#include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/common/lua_parameter_dictionary.h"
#include "cartographer/mapping/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 pose_graph { namespace optimization {
proto::OptimizationProblemOptions CreateOptimizationProblemOptions( proto::OptimizationProblemOptions CreateOptimizationProblemOptions(
common::LuaParameterDictionary* parameter_dictionary); common::LuaParameterDictionary* parameter_dictionary);
} // namespace pose_graph } // namespace optimization
} // namespace mapping } // namespace mapping
} // namespace cartographer } // namespace cartographer
#endif // CARTOGRAPHER_MAPPING_INTERNAL_POSE_GRAPH_OPTIMIZATION_PROBLEM_OPTIONS_H_ #endif // CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_OPTIMIZATION_PROBLEM_OPTIONS_H_

4
cartographer/mapping/map_builder.cc
View File

@ -68,14 +68,14 @@ MapBuilder::MapBuilder(const proto::MapBuilderOptions& options)
if (options.use_trajectory_builder_2d()) { if (options.use_trajectory_builder_2d()) {
pose_graph_ = common::make_unique<PoseGraph2D>( pose_graph_ = common::make_unique<PoseGraph2D>(
options_.pose_graph_options(), options_.pose_graph_options(),
common::make_unique<pose_graph::OptimizationProblem2D>( common::make_unique<optimization::OptimizationProblem2D>(
options_.pose_graph_options().optimization_problem_options()), options_.pose_graph_options().optimization_problem_options()),
&thread_pool_); &thread_pool_);
} }
if (options.use_trajectory_builder_3d()) { if (options.use_trajectory_builder_3d()) {
pose_graph_ = common::make_unique<PoseGraph3D>( pose_graph_ = common::make_unique<PoseGraph3D>(
options_.pose_graph_options(), options_.pose_graph_options(),
common::make_unique<pose_graph::OptimizationProblem3D>( common::make_unique<optimization::OptimizationProblem3D>(
options_.pose_graph_options().optimization_problem_options()), options_.pose_graph_options().optimization_problem_options()),
&thread_pool_); &thread_pool_);
} }

4
cartographer/mapping/pose_graph.cc
View File

@ -16,8 +16,8 @@
#include "cartographer/mapping/pose_graph.h" #include "cartographer/mapping/pose_graph.h"
#include "cartographer/mapping/internal/optimization/optimization_problem_options.h"
#include "cartographer/mapping/internal/pose_graph/constraint_builder.h" #include "cartographer/mapping/internal/pose_graph/constraint_builder.h"
#include "cartographer/mapping/internal/pose_graph/optimization_problem_options.h"
#include "cartographer/transform/transform.h" #include "cartographer/transform/transform.h"
#include "glog/logging.h" #include "glog/logging.h"
@ -82,7 +82,7 @@ proto::PoseGraphOptions CreatePoseGraphOptions(
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() =
pose_graph::CreateOptimizationProblemOptions( optimization::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"));

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

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

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

@ -37,7 +37,7 @@ message PoseGraphOptions {
double matcher_rotation_weight = 8; double matcher_rotation_weight = 8;
// Options for the optimization problem. // Options for the optimization problem.
mapping.pose_graph.proto.OptimizationProblemOptions mapping.optimization.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