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

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,7 +250,8 @@ 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,
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];
@ -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);

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.

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_);
}

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,7 +483,7 @@ 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,
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);

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.

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),

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>(

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

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_

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_

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_

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_

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_

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

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_

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

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,
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_

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_

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_

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

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_

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

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_

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

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_

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

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_

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_);
}

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"));

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";

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