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

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

@ -41,7 +41,7 @@ namespace mapping {
PoseGraph2D::PoseGraph2D(
const proto::PoseGraphOptions& options,
std::unique_ptr<pose_graph::OptimizationProblem2D> optimization_problem,
std::unique_ptr<optimization::OptimizationProblem2D> optimization_problem,
common::ThreadPool* thread_pool)
: options_(options),
optimization_problem_(std::move(optimization_problem)),
@ -250,8 +250,9 @@ void PoseGraph2D::ComputeConstraintsForNode(
local_pose_2d;
optimization_problem_->AddTrajectoryNode(
matching_id.trajectory_id,
pose_graph::NodeSpec2D{constant_data->time, local_pose_2d, global_pose_2d,
constant_data->gravity_alignment});
optimization::NodeSpec2D{constant_data->time, local_pose_2d,
global_pose_2d,
constant_data->gravity_alignment});
for (size_t i = 0; i < insertion_submaps.size(); ++i) {
const SubmapId submap_id = submap_ids[i];
// Even if this was the last node added to 'submap_id', the submap will
@ -446,8 +447,8 @@ void PoseGraph2D::AddSubmapFromProto(
submap_data_.Insert(submap_id, InternalSubmapData());
submap_data_.at(submap_id).submap = submap_ptr;
// Immediately show the submap at the 'global_submap_pose'.
global_submap_poses_.Insert(submap_id,
pose_graph::SubmapSpec2D{global_submap_pose_2d});
global_submap_poses_.Insert(
submap_id, optimization::SubmapSpec2D{global_submap_pose_2d});
AddWorkItem([this, submap_id, global_submap_pose_2d]() REQUIRES(mutex_) {
submap_data_.at(submap_id).state = SubmapState::kFinished;
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());
optimization_problem_->InsertTrajectoryNode(
node_id,
pose_graph::NodeSpec2D{
optimization::NodeSpec2D{
constant_data->time,
transform::Project2D(constant_data->local_pose *
gravity_alignment_inverse),
@ -746,7 +747,7 @@ PoseGraph2D::GetAllSubmapPoses() {
}
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 {
auto begin_it = global_submap_poses.BeginOfTrajectory(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/mapping/2d/submap_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/pose_graph.h"
#include "cartographer/mapping/pose_graph_trimmer.h"
@ -61,7 +61,7 @@ class PoseGraph2D : public PoseGraph {
public:
PoseGraph2D(
const proto::PoseGraphOptions& options,
std::unique_ptr<pose_graph::OptimizationProblem2D> optimization_problem,
std::unique_ptr<optimization::OptimizationProblem2D> optimization_problem,
common::ThreadPool* thread_pool);
~PoseGraph2D() override;
@ -201,7 +201,7 @@ class PoseGraph2D : public PoseGraph {
// Computes the local to global map frame transform based on the given
// 'global_submap_poses'.
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_);
SubmapData GetSubmapDataUnderLock(const SubmapId& submap_id) REQUIRES(mutex_);
@ -239,7 +239,7 @@ class PoseGraph2D : public PoseGraph {
void DispatchOptimization() REQUIRES(mutex_);
// 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_);
std::vector<Constraint> constraints_ GUARDED_BY(mutex_);
@ -252,7 +252,7 @@ class PoseGraph2D : public PoseGraph {
int num_trajectory_nodes_ GUARDED_BY(mutex_) = 0;
// 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_);
// 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());
pose_graph_ = common::make_unique<PoseGraph2D>(
options,
common::make_unique<pose_graph::OptimizationProblem2D>(
common::make_unique<optimization::OptimizationProblem2D>(
options.optimization_problem_options()),
&thread_pool_);
}

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

@ -41,7 +41,7 @@ namespace mapping {
PoseGraph3D::PoseGraph3D(
const proto::PoseGraphOptions& options,
std::unique_ptr<pose_graph::OptimizationProblem3D> optimization_problem,
std::unique_ptr<optimization::OptimizationProblem3D> optimization_problem,
common::ThreadPool* thread_pool)
: options_(options),
optimization_problem_(std::move(optimization_problem)),
@ -267,7 +267,7 @@ void PoseGraph3D::ComputeConstraintsForNode(
insertion_submaps.front()->local_pose().inverse() * local_pose;
optimization_problem_->AddTrajectoryNode(
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) {
const SubmapId submap_id = submap_ids[i];
// 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;
// Immediately show the submap at the 'global_submap_pose'.
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_) {
submap_data_.at(submap_id).state = SubmapState::kFinished;
optimization_problem_->InsertSubmap(submap_id, global_submap_pose);
@ -483,8 +483,8 @@ void PoseGraph3D::AddNodeFromProto(const transform::Rigid3d& global_pose,
const auto& constant_data = trajectory_nodes_.at(node_id).constant_data;
optimization_problem_->InsertTrajectoryNode(
node_id,
pose_graph::NodeSpec3D{constant_data->time, constant_data->local_pose,
global_pose});
optimization::NodeSpec3D{constant_data->time, constant_data->local_pose,
global_pose});
});
}
@ -778,7 +778,7 @@ PoseGraph3D::GetAllSubmapPoses() {
}
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 {
auto begin_it = global_submap_poses.BeginOfTrajectory(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/mapping/3d/submap_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/pose_graph.h"
#include "cartographer/mapping/pose_graph_trimmer.h"
@ -60,7 +60,7 @@ class PoseGraph3D : public PoseGraph {
public:
PoseGraph3D(
const proto::PoseGraphOptions& options,
std::unique_ptr<pose_graph::OptimizationProblem3D> optimization_problem,
std::unique_ptr<optimization::OptimizationProblem3D> optimization_problem,
common::ThreadPool* thread_pool);
~PoseGraph3D() override;
@ -200,7 +200,7 @@ class PoseGraph3D : public PoseGraph {
// Computes the local to global map frame transform based on the given
// 'global_submap_poses'.
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_);
PoseGraph::SubmapData GetSubmapDataUnderLock(const SubmapId& submap_id)
@ -243,7 +243,7 @@ class PoseGraph3D : public PoseGraph {
void DispatchOptimization() REQUIRES(mutex_);
// 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_);
std::vector<Constraint> constraints_ GUARDED_BY(mutex_);
@ -256,7 +256,7 @@ class PoseGraph3D : public PoseGraph {
int num_trajectory_nodes_ GUARDED_BY(mutex_) = 0;
// 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_);
// 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 {
using ::cartographer::mapping::pose_graph::OptimizationProblem3D;
using ::cartographer::mapping::pose_graph::proto::OptimizationProblemOptions;
using ::cartographer::mapping::optimization::OptimizationProblem3D;
using ::cartographer::mapping::optimization::proto::OptimizationProblemOptions;
using ::cartographer::transform::Rigid3d;
class MockOptimizationProblem3D : public OptimizationProblem3D {
@ -46,7 +46,7 @@ class PoseGraph3DForTesting : public PoseGraph3D {
public:
PoseGraph3DForTesting(
const proto::PoseGraphOptions& options,
std::unique_ptr<pose_graph::OptimizationProblem3D> optimization_problem,
std::unique_ptr<optimization::OptimizationProblem3D> optimization_problem,
common::ThreadPool* thread_pool)
: PoseGraph3D(options, std::move(optimization_problem), thread_pool) {}
@ -68,7 +68,7 @@ class PoseGraph3DTest : public ::testing::Test {
void BuildPoseGraph() {
auto optimization_problem =
common::make_unique<pose_graph::OptimizationProblem3D>(
common::make_unique<optimization::OptimizationProblem3D>(
pose_graph_options_.optimization_problem_options());
pose_graph_ = common::make_unique<PoseGraph3DForTesting>(
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/rotation_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/transform.h"
#include "ceres/ceres.h"
@ -76,7 +76,7 @@ void CeresScanMatcher3D::Match(
transform::Rigid3d* const pose_estimate,
ceres::Solver::Summary* const summary) {
ceres::Problem problem;
pose_graph::CeresPose ceres_pose(
optimization::CeresPose ceres_pose(
initial_pose_estimate, nullptr /* translation_parameterization */,
options_.only_optimize_yaw()
? 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.
*/
#include "cartographer/mapping/internal/pose_graph/ceres_pose.h"
#include "cartographer/mapping/internal/optimization/ceres_pose.h"
namespace cartographer {
namespace mapping {
namespace pose_graph {
namespace optimization {
CeresPose::CeresPose(
const transform::Rigid3d& rigid,
@ -40,6 +40,6 @@ const transform::Rigid3d CeresPose::ToRigid() const {
return transform::Rigid3d::FromArrays(data_->rotation, data_->translation);
}
} // namespace pose_graph
} // namespace optimization
} // namespace mapping
} // 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.
*/
#ifndef CARTOGRAPHER_MAPPING_INTERNAL_POSE_GRAPH_CERES_POSE_H_
#define CARTOGRAPHER_MAPPING_INTERNAL_POSE_GRAPH_CERES_POSE_H_
#ifndef CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_CERES_POSE_H_
#define CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_CERES_POSE_H_
#include <array>
#include <memory>
@ -26,7 +26,7 @@
namespace cartographer {
namespace mapping {
namespace pose_graph {
namespace optimization {
class CeresPose {
public:
@ -53,8 +53,8 @@ class CeresPose {
std::shared_ptr<Data> data_;
};
} // namespace pose_graph
} // namespace optimization
} // namespace mapping
} // 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.
*/
#ifndef CARTOGRAPHER_MAPPING_INTERNAL_3D_ACCELERATION_COST_FUNCTION_3D_H_
#define 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_OPTIMIZATION_COST_FUNCTIONS_ACCELERATION_COST_FUNCTION_3D_H_
#include "Eigen/Core"
#include "Eigen/Geometry"
@ -101,4 +101,4 @@ class AccelerationCostFunction3D {
} // namespace mapping
} // 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.
*/
#ifndef CARTOGRAPHER_MAPPING_INTERNAL_POSE_GRAPH_COST_HELPERS_H_
#define CARTOGRAPHER_MAPPING_INTERNAL_POSE_GRAPH_COST_HELPERS_H_
#ifndef CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_COST_FUNCTIONS_COST_HELPERS_H_
#define CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_COST_FUNCTIONS_COST_HELPERS_H_
#include "Eigen/Core"
#include "Eigen/Geometry"
@ -24,7 +24,7 @@
namespace cartographer {
namespace mapping {
namespace pose_graph {
namespace optimization {
// Computes the error between the given relative pose and the difference of
// 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 double interpolation_parameter);
} // namespace pose_graph
} // namespace optimization
} // namespace mapping
} // 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.
*/
#ifndef CARTOGRAPHER_MAPPING_INTERNAL_POSE_GRAPH_COST_HELPERS_IMPL_H_
#define CARTOGRAPHER_MAPPING_INTERNAL_POSE_GRAPH_COST_HELPERS_IMPL_H_
#ifndef CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_COST_FUNCTIONS_COST_HELPERS_IMPL_H_
#define CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_COST_FUNCTIONS_COST_HELPERS_IMPL_H_
#include "Eigen/Core"
#include "cartographer/transform/rigid_transform.h"
@ -23,7 +23,7 @@
namespace cartographer {
namespace mapping {
namespace pose_graph {
namespace optimization {
template <typename T>
static std::array<T, 3> ComputeUnscaledError(
@ -190,8 +190,8 @@ InterpolateNodes2D(const T* const prev_node_pose,
T(0)}});
}
} // namespace pose_graph
} // namespace optimization
} // namespace mapping
} // 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.
*/
#ifndef CARTOGRAPHER_MAPPING_INTERNAL_2D_POSE_GRAPH_LANDMARK_COST_FUNCTION_2D_H_
#define 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_OPTIMIZATION_COST_FUNCTIONS_LANDMARK_COST_FUNCTION_2D_H_
#include "Eigen/Core"
#include "Eigen/Geometry"
#include "cartographer/mapping/internal/2d/pose_graph/optimization_problem_2d.h"
#include "cartographer/mapping/internal/pose_graph/cost_helpers.h"
#include "cartographer/mapping/internal/optimization/cost_functions/cost_helpers.h"
#include "cartographer/mapping/internal/optimization/optimization_problem_2d.h"
#include "cartographer/mapping/pose_graph_interface.h"
#include "cartographer/transform/rigid_transform.h"
#include "cartographer/transform/transform.h"
@ -29,7 +29,7 @@
namespace cartographer {
namespace mapping {
namespace pose_graph {
namespace optimization {
// Cost function measuring the weighted error between the observed pose given by
// the landmark measurement and the linearly interpolated pose of embedded in 3D
@ -91,8 +91,8 @@ class LandmarkCostFunction2D {
const double interpolation_parameter_;
};
} // namespace pose_graph
} // namespace optimization
} // namespace mapping
} // 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.
*/
#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>
@ -24,7 +24,7 @@
namespace cartographer {
namespace mapping {
namespace pose_graph {
namespace optimization {
namespace {
using ::testing::DoubleEq;
@ -72,6 +72,6 @@ TEST(LandmarkCostFunctionTest, SmokeTest) {
}
} // namespace
} // namespace pose_graph
} // namespace optimization
} // namespace mapping
} // 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.
*/
#ifndef CARTOGRAPHER_MAPPING_INTERNAL_3D_POSE_GRAPH_LANDMARK_COST_FUNCTION_3D_H_
#define 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_OPTIMIZATION_COST_FUNCTIONS_LANDMARK_COST_FUNCTION_3D_H_
#include "Eigen/Core"
#include "Eigen/Geometry"
#include "cartographer/mapping/internal/3d/pose_graph/optimization_problem_3d.h"
#include "cartographer/mapping/internal/pose_graph/cost_helpers.h"
#include "cartographer/mapping/internal/optimization/cost_functions/cost_helpers.h"
#include "cartographer/mapping/internal/optimization/optimization_problem_3d.h"
#include "cartographer/mapping/pose_graph_interface.h"
#include "cartographer/transform/rigid_transform.h"
#include "cartographer/transform/transform.h"
@ -29,7 +29,7 @@
namespace cartographer {
namespace mapping {
namespace pose_graph {
namespace optimization {
// Cost function measuring the weighted error between the observed pose given by
// the landmark measurement and the linearly interpolated pose.
@ -92,8 +92,8 @@ class LandmarkCostFunction3D {
const double interpolation_parameter_;
};
} // namespace pose_graph
} // namespace optimization
} // namespace mapping
} // 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.
*/
#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>
@ -24,7 +24,7 @@
namespace cartographer {
namespace mapping {
namespace pose_graph {
namespace optimization {
namespace {
using ::testing::DoubleEq;
@ -72,6 +72,6 @@ TEST(LandmarkCostFunction3DTest, SmokeTest) {
}
} // namespace
} // namespace pose_graph
} // namespace optimization
} // namespace mapping
} // namespace cartographer

20
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.
*/
#ifndef CARTOGRAPHER_MAPPING_INTERNAL_3D_ROTATION_COST_FUNCTION_3D_H_
#define 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_OPTIMIZATION_COST_FUNCTIONS_ROTATION_COST_FUNCTION_3D_H_
#include "Eigen/Core"
#include "Eigen/Geometry"
@ -25,15 +25,15 @@ namespace cartographer {
namespace mapping {
// Penalizes differences between IMU data and optimized orientations.
class RotationCostFunction {
class RotationCostFunction3D {
public:
static ceres::CostFunction* CreateAutoDiffCostFunction(
const double scaling_factor,
const Eigen::Quaterniond& delta_rotation_imu_frame) {
return new ceres::AutoDiffCostFunction<
RotationCostFunction, 3 /* residuals */, 4 /* rotation variables */,
RotationCostFunction3D, 3 /* residuals */, 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>
@ -56,13 +56,13 @@ class RotationCostFunction {
}
private:
RotationCostFunction(const double scaling_factor,
const Eigen::Quaterniond& delta_rotation_imu_frame)
RotationCostFunction3D(const double scaling_factor,
const Eigen::Quaterniond& delta_rotation_imu_frame)
: scaling_factor_(scaling_factor),
delta_rotation_imu_frame_(delta_rotation_imu_frame) {}
RotationCostFunction(const RotationCostFunction&) = delete;
RotationCostFunction& operator=(const RotationCostFunction&) = delete;
RotationCostFunction3D(const RotationCostFunction3D&) = delete;
RotationCostFunction3D& operator=(const RotationCostFunction3D&) = delete;
const double scaling_factor_;
const Eigen::Quaterniond delta_rotation_imu_frame_;
@ -71,4 +71,4 @@ class RotationCostFunction {
} // namespace mapping
} // 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.
*/
#ifndef CARTOGRAPHER_MAPPING_INTERNAL_2D_POSE_GRAPH_SPA_COST_FUNCTION_2D_H_
#define 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_OPTIMIZATION_COST_FUNCTIONS_SPA_COST_FUNCTION_2D_H_
#include <array>
#include "Eigen/Core"
#include "Eigen/Geometry"
#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/transform/rigid_transform.h"
#include "cartographer/transform/transform.h"
@ -31,7 +31,7 @@
namespace cartographer {
namespace mapping {
namespace pose_graph {
namespace optimization {
class SpaCostFunction2D {
public:
@ -45,8 +45,8 @@ class SpaCostFunction2D {
template <typename T>
bool operator()(const T* const c_i, const T* const c_j, T* e) const {
using pose_graph::ComputeUnscaledError;
using pose_graph::ScaleError;
using optimization::ComputeUnscaledError;
using optimization::ScaleError;
const std::array<T, 3> error = ScaleError(
ComputeUnscaledError(transform::Project2D(pose_.zbar_ij), c_i, c_j),
@ -62,8 +62,8 @@ class SpaCostFunction2D {
const PoseGraph::Constraint::Pose pose_;
};
} // namespace pose_graph
} // namespace optimization
} // namespace mapping
} // 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.
*/
#ifndef CARTOGRAPHER_MAPPING_INTERNAL_3D_POSE_GRAPH_SPA_COST_FUNCTION_3D_H_
#define 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_OPTIMIZATION_COST_FUNCTIONS_SPA_COST_FUNCTION_3D_H_
#include <array>
#include "Eigen/Core"
#include "Eigen/Geometry"
#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/transform/rigid_transform.h"
#include "cartographer/transform/transform.h"
@ -31,7 +31,7 @@
namespace cartographer {
namespace mapping {
namespace pose_graph {
namespace optimization {
class SpaCostFunction3D {
public:
@ -62,8 +62,8 @@ class SpaCostFunction3D {
const PoseGraph::Constraint::Pose pose_;
};
} // namespace pose_graph
} // namespace optimization
} // namespace mapping
} // 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.
*/
#include "cartographer/mapping/internal/2d/pose_graph/optimization_problem_2d.h"
#include "cartographer/mapping/internal/optimization/optimization_problem_2d.h"
#include <algorithm>
#include <array>
@ -27,10 +27,9 @@
#include "cartographer/common/ceres_solver_options.h"
#include "cartographer/common/histogram.h"
#include "cartographer/common/math.h"
#include "cartographer/mapping/internal/2d/pose_graph/landmark_cost_function_2d.h"
#include "cartographer/mapping/internal/2d/pose_graph/spa_cost_function_2d.h"
#include "cartographer/mapping/internal/pose_graph/ceres_pose.h"
#include "cartographer/mapping/internal/pose_graph/cost_helpers.h"
#include "cartographer/mapping/internal/optimization/ceres_pose.h"
#include "cartographer/mapping/internal/optimization/cost_functions/landmark_cost_function_2d.h"
#include "cartographer/mapping/internal/optimization/cost_functions/spa_cost_function_2d.h"
#include "cartographer/sensor/odometry_data.h"
#include "cartographer/transform/transform.h"
#include "ceres/ceres.h"
@ -38,10 +37,10 @@
namespace cartographer {
namespace mapping {
namespace pose_graph {
namespace optimization {
namespace {
using ::cartographer::mapping::pose_graph::CeresPose;
using ::cartographer::mapping::optimization::CeresPose;
using LandmarkNode = ::cartographer::mapping::PoseGraphInterface::LandmarkNode;
// Converts a pose into the 3 optimization variable format used for Ceres:
@ -361,6 +360,6 @@ OptimizationProblem2D::CalculateOdometryBetweenNodes(
return nullptr;
}
} // namespace pose_graph
} // namespace optimization
} // namespace mapping
} // 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.
*/
#ifndef CARTOGRAPHER_MAPPING_INTERNAL_2D_POSE_GRAPH_OPTIMIZATION_PROBLEM_2D_H_
#define CARTOGRAPHER_MAPPING_INTERNAL_2D_POSE_GRAPH_OPTIMIZATION_PROBLEM_2D_H_
#ifndef CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_OPTIMIZATION_PROBLEM_2D_H_
#define CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_OPTIMIZATION_PROBLEM_2D_H_
#include <array>
#include <deque>
@ -28,7 +28,7 @@
#include "cartographer/common/port.h"
#include "cartographer/common/time.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_interface.h"
#include "cartographer/sensor/imu_data.h"
@ -38,7 +38,7 @@
namespace cartographer {
namespace mapping {
namespace pose_graph {
namespace optimization {
struct NodeSpec2D {
common::Time time;
@ -56,7 +56,7 @@ class OptimizationProblem2D
transform::Rigid2d> {
public:
explicit OptimizationProblem2D(
const pose_graph::proto::OptimizationProblemOptions& options);
const optimization::proto::OptimizationProblemOptions& options);
~OptimizationProblem2D();
OptimizationProblem2D(const OptimizationProblem2D&) = delete;
@ -108,7 +108,7 @@ class OptimizationProblem2D
int trajectory_id, const NodeSpec2D& first_node_data,
const NodeSpec2D& second_node_data) const;
pose_graph::proto::OptimizationProblemOptions options_;
optimization::proto::OptimizationProblemOptions options_;
MapById<NodeId, NodeSpec2D> node_data_;
MapById<SubmapId, SubmapSpec2D> submap_data_;
std::map<std::string, transform::Rigid3d> landmark_data_;
@ -116,8 +116,8 @@ class OptimizationProblem2D
sensor::MapByTime<sensor::OdometryData> odometry_data_;
};
} // namespace pose_graph
} // namespace optimization
} // namespace mapping
} // 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.
*/
#include "cartographer/mapping/internal/3d/pose_graph/optimization_problem_3d.h"
#include "cartographer/mapping/internal/optimization/optimization_problem_3d.h"
#include <algorithm>
#include <array>
@ -30,13 +30,13 @@
#include "cartographer/common/make_unique.h"
#include "cartographer/common/math.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/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/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/transform.h"
#include "ceres/ceres.h"
@ -46,7 +46,7 @@
namespace cartographer {
namespace mapping {
namespace pose_graph {
namespace optimization {
namespace {
using LandmarkNode = ::cartographer::mapping::PoseGraphInterface::LandmarkNode;
@ -188,7 +188,7 @@ void AddLandmarkCostFunctions(
} // namespace
OptimizationProblem3D::OptimizationProblem3D(
const pose_graph::proto::OptimizationProblemOptions& options)
const optimization::proto::OptimizationProblemOptions& options)
: options_(options) {}
OptimizationProblem3D::~OptimizationProblem3D() {}
@ -425,7 +425,7 @@ void OptimizationProblem3D::Solve(
trajectory_data.imu_calibration.data());
}
problem.AddResidualBlock(
RotationCostFunction::CreateAutoDiffCostFunction(
RotationCostFunction3D::CreateAutoDiffCostFunction(
options_.rotation_weight(), result.delta_rotation),
nullptr /* loss function */, C_nodes.at(first_node_id).rotation(),
C_nodes.at(second_node_id).rotation(),
@ -605,6 +605,6 @@ OptimizationProblem3D::CalculateOdometryBetweenNodes(
return nullptr;
}
} // namespace pose_graph
} // namespace optimization
} // namespace mapping
} // 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.
*/
#ifndef CARTOGRAPHER_MAPPING_INTERNAL_3D_POSE_GRAPH_OPTIMIZATION_PROBLEM_3D_H_
#define CARTOGRAPHER_MAPPING_INTERNAL_3D_POSE_GRAPH_OPTIMIZATION_PROBLEM_3D_H_
#ifndef CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_OPTIMIZATION_PROBLEM_3D_H_
#define CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_OPTIMIZATION_PROBLEM_3D_H_
#include <array>
#include <map>
@ -28,7 +28,7 @@
#include "cartographer/common/port.h"
#include "cartographer/common/time.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_interface.h"
#include "cartographer/sensor/fixed_frame_pose_data.h"
@ -39,7 +39,7 @@
namespace cartographer {
namespace mapping {
namespace pose_graph {
namespace optimization {
struct NodeSpec3D {
common::Time time;
@ -56,7 +56,7 @@ class OptimizationProblem3D
transform::Rigid3d> {
public:
explicit OptimizationProblem3D(
const pose_graph::proto::OptimizationProblemOptions& options);
const optimization::proto::OptimizationProblemOptions& options);
~OptimizationProblem3D();
OptimizationProblem3D(const OptimizationProblem3D&) = delete;
@ -121,7 +121,7 @@ class OptimizationProblem3D
int trajectory_id, const NodeSpec3D& first_node_data,
const NodeSpec3D& second_node_data) const;
pose_graph::proto::OptimizationProblemOptions options_;
optimization::proto::OptimizationProblemOptions options_;
MapById<NodeId, NodeSpec3D> node_data_;
MapById<SubmapId, SubmapSpec3D> submap_data_;
std::map<std::string, transform::Rigid3d> landmark_data_;
@ -131,8 +131,8 @@ class OptimizationProblem3D
std::map<int, PoseGraphInterface::TrajectoryData> trajectory_data_;
};
} // namespace pose_graph
} // namespace optimization
} // namespace mapping
} // 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.
*/
#include "cartographer/mapping/internal/3d/pose_graph/optimization_problem_3d.h"
#include "cartographer/mapping/internal/optimization/optimization_problem_3d.h"
#include <random>
#include "Eigen/Core"
#include "cartographer/common/lua_parameter_dictionary_test_helpers.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 "glog/logging.h"
#include "gmock/gmock.h"
namespace cartographer {
namespace mapping {
namespace pose_graph {
namespace optimization {
namespace {
class OptimizationProblem3DTest : public ::testing::Test {
@ -36,7 +36,7 @@ class OptimizationProblem3DTest : public ::testing::Test {
OptimizationProblem3DTest()
: optimization_problem_(CreateOptions()), rng_(45387) {}
pose_graph::proto::OptimizationProblemOptions CreateOptions() {
optimization::proto::OptimizationProblemOptions CreateOptions() {
auto parameter_dictionary = common::MakeDictionary(R"text(
return {
acceleration_weight = 1e-4,
@ -55,7 +55,7 @@ class OptimizationProblem3DTest : public ::testing::Test {
num_threads = 4,
},
})text");
return pose_graph::CreateOptimizationProblemOptions(
return optimization::CreateOptimizationProblemOptions(
parameter_dictionary.get());
}
@ -191,6 +191,6 @@ TEST_F(OptimizationProblem3DTest, ReducesNoise) {
}
} // namespace
} // namespace pose_graph
} // namespace optimization
} // namespace mapping
} // 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.
*/
#ifndef CARTOGRAPHER_MAPPING_INTERNAL_POSE_GRAPH_OPTIMIZATION_PROBLEM_INTERFACE_H_
#define CARTOGRAPHER_MAPPING_INTERNAL_POSE_GRAPH_OPTIMIZATION_PROBLEM_INTERFACE_H_
#ifndef CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_OPTIMIZATION_PROBLEM_INTERFACE_H_
#define CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_OPTIMIZATION_PROBLEM_INTERFACE_H_
#include <map>
#include <set>
@ -33,7 +33,7 @@
namespace cartographer {
namespace mapping {
namespace pose_graph {
namespace optimization {
// Implements the SPA loop closure method.
template <typename NodeDataType, typename SubmapDataType,
@ -81,8 +81,8 @@ class OptimizationProblemInterface {
const = 0;
};
} // namespace pose_graph
} // namespace optimization
} // namespace mapping
} // 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.
*/
#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"
namespace cartographer {
namespace mapping {
namespace pose_graph {
namespace optimization {
proto::OptimizationProblemOptions CreateOptimizationProblemOptions(
common::LuaParameterDictionary* const parameter_dictionary) {
@ -50,6 +50,6 @@ proto::OptimizationProblemOptions CreateOptimizationProblemOptions(
return options;
}
} // namespace pose_graph
} // namespace optimization
} // namespace mapping
} // 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.
*/
#ifndef CARTOGRAPHER_MAPPING_INTERNAL_POSE_GRAPH_OPTIMIZATION_PROBLEM_OPTIONS_H_
#define CARTOGRAPHER_MAPPING_INTERNAL_POSE_GRAPH_OPTIMIZATION_PROBLEM_OPTIONS_H_
#ifndef CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_OPTIMIZATION_PROBLEM_OPTIONS_H_
#define CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_OPTIMIZATION_PROBLEM_OPTIONS_H_
#include "cartographer/common/lua_parameter_dictionary.h"
#include "cartographer/mapping/pose_graph/proto/optimization_problem_options.pb.h"
namespace cartographer {
namespace mapping {
namespace pose_graph {
namespace optimization {
proto::OptimizationProblemOptions CreateOptimizationProblemOptions(
common::LuaParameterDictionary* parameter_dictionary);
} // namespace pose_graph
} // namespace optimization
} // namespace mapping
} // 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()) {
pose_graph_ = common::make_unique<PoseGraph2D>(
options_.pose_graph_options(),
common::make_unique<pose_graph::OptimizationProblem2D>(
common::make_unique<optimization::OptimizationProblem2D>(
options_.pose_graph_options().optimization_problem_options()),
&thread_pool_);
}
if (options.use_trajectory_builder_3d()) {
pose_graph_ = common::make_unique<PoseGraph3D>(
options_.pose_graph_options(),
common::make_unique<pose_graph::OptimizationProblem3D>(
common::make_unique<optimization::OptimizationProblem3D>(
options_.pose_graph_options().optimization_problem_options()),
&thread_pool_);
}

4
cartographer/mapping/pose_graph.cc
View File

@ -16,8 +16,8 @@
#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/optimization_problem_options.h"
#include "cartographer/transform/transform.h"
#include "glog/logging.h"
@ -82,7 +82,7 @@ proto::PoseGraphOptions CreatePoseGraphOptions(
options.set_matcher_rotation_weight(
parameter_dictionary->GetDouble("matcher_rotation_weight"));
*options.mutable_optimization_problem_options() =
pose_graph::CreateOptimizationProblemOptions(
optimization::CreateOptimizationProblemOptions(
parameter_dictionary->GetDictionary("optimization_problem").get());
options.set_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";
package cartographer.mapping.pose_graph.proto;
package cartographer.mapping.optimization.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;
// Options for the optimization problem.
mapping.pose_graph.proto.OptimizationProblemOptions
mapping.optimization.proto.OptimizationProblemOptions
optimization_problem_options = 4;
// Number of iterations to use in 'optimization_problem_options' for the final