diff --git a/cartographer/pose_graph/constraint/acceleration_constraint_3d.cc b/cartographer/pose_graph/constraint/acceleration_constraint_3d.cc index b0b36ab..87fbf6a 100644 --- a/cartographer/pose_graph/constraint/acceleration_constraint_3d.cc +++ b/cartographer/pose_graph/constraint/acceleration_constraint_3d.cc @@ -34,8 +34,8 @@ AccelerationConstraint3D::AccelerationConstraint3D( cost_(new AccelerationCost3D(proto.parameters())), ceres_cost_(absl::make_unique(cost_)) {} -void AccelerationConstraint3D::AddToOptimizer(Nodes* nodes, - ceres::Problem* problem) const { +void AccelerationConstraint3D::AddToSolver(Nodes* nodes, + ceres::Problem* problem) const { FIND_NODE_OR_RETURN(first_node, first_, nodes->pose_3d_nodes, "First node was not found in pose_3d_nodes."); FIND_NODE_OR_RETURN(second_node, second_, nodes->pose_3d_nodes, diff --git a/cartographer/pose_graph/constraint/acceleration_constraint_3d.h b/cartographer/pose_graph/constraint/acceleration_constraint_3d.h index 97c5b82..0321a83 100644 --- a/cartographer/pose_graph/constraint/acceleration_constraint_3d.h +++ b/cartographer/pose_graph/constraint/acceleration_constraint_3d.h @@ -29,7 +29,7 @@ class AccelerationConstraint3D : public Constraint { const proto::LossFunction& loss_function_proto, const proto::Acceleration3D& proto); - void AddToOptimizer(Nodes* nodes, ceres::Problem* problem) const final; + void AddToSolver(Nodes* nodes, ceres::Problem* problem) const final; protected: proto::CostFunction ToCostFunctionProto() const final; diff --git a/cartographer/pose_graph/constraint/constraint.h b/cartographer/pose_graph/constraint/constraint.h index cbbebb4..0fcda85 100644 --- a/cartographer/pose_graph/constraint/constraint.h +++ b/cartographer/pose_graph/constraint/constraint.h @@ -43,7 +43,7 @@ class Constraint { const ConstraintId& constraint_id() const { return constraint_id_; } - virtual void AddToOptimizer(Nodes* nodes, ceres::Problem* problem) const = 0; + virtual void AddToSolver(Nodes* nodes, ceres::Problem* problem) const = 0; protected: virtual proto::CostFunction ToCostFunctionProto() const = 0; diff --git a/cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_2d.cc b/cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_2d.cc index 0f30b15..7e8154b 100644 --- a/cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_2d.cc +++ b/cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_2d.cc @@ -33,7 +33,7 @@ InterpolatedRelativePoseConstraint2D::InterpolatedRelativePoseConstraint2D( cost_(new InterpolatedRelativePoseCost2D(proto.parameters())), ceres_cost_(absl::make_unique(cost_)) {} -void InterpolatedRelativePoseConstraint2D::AddToOptimizer( +void InterpolatedRelativePoseConstraint2D::AddToSolver( Nodes* nodes, ceres::Problem* problem) const { FIND_NODE_OR_RETURN(first_node_start, first_start_, nodes->pose_2d_nodes, "First node (start) was not found in pose_2d_nodes."); diff --git a/cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_2d.h b/cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_2d.h index 1260733..5635334 100644 --- a/cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_2d.h +++ b/cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_2d.h @@ -29,7 +29,7 @@ class InterpolatedRelativePoseConstraint2D : public Constraint { const ConstraintId& id, const proto::LossFunction& loss_function_proto, const proto::InterpolatedRelativePose2D& proto); - void AddToOptimizer(Nodes* nodes, ceres::Problem* problem) const final; + void AddToSolver(Nodes* nodes, ceres::Problem* problem) const final; protected: proto::CostFunction ToCostFunctionProto() const final; diff --git a/cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_3d.cc b/cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_3d.cc index dc3f63e..73f31fa 100644 --- a/cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_3d.cc +++ b/cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_3d.cc @@ -33,7 +33,7 @@ InterpolatedRelativePoseConstraint3D::InterpolatedRelativePoseConstraint3D( cost_(new InterpolatedRelativePoseCost3D(proto.parameters())), ceres_cost_(absl::make_unique(cost_)) {} -void InterpolatedRelativePoseConstraint3D::AddToOptimizer( +void InterpolatedRelativePoseConstraint3D::AddToSolver( Nodes* nodes, ceres::Problem* problem) const { FIND_NODE_OR_RETURN(first_node_start, first_start_, nodes->pose_3d_nodes, "First node (start) was not found in pose_3d_nodes."); diff --git a/cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_3d.h b/cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_3d.h index 86c35b9..d836561 100644 --- a/cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_3d.h +++ b/cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_3d.h @@ -29,7 +29,7 @@ class InterpolatedRelativePoseConstraint3D : public Constraint { const ConstraintId& id, const proto::LossFunction& loss_function_proto, const proto::InterpolatedRelativePose3D& proto); - void AddToOptimizer(Nodes* nodes, ceres::Problem* problem) const final; + void AddToSolver(Nodes* nodes, ceres::Problem* problem) const final; protected: proto::CostFunction ToCostFunctionProto() const final; diff --git a/cartographer/pose_graph/constraint/relative_pose_constraint_2d.cc b/cartographer/pose_graph/constraint/relative_pose_constraint_2d.cc index 9b7faa6..46f8264 100644 --- a/cartographer/pose_graph/constraint/relative_pose_constraint_2d.cc +++ b/cartographer/pose_graph/constraint/relative_pose_constraint_2d.cc @@ -32,8 +32,8 @@ RelativePoseConstraint2D::RelativePoseConstraint2D( ceres_cost_(absl::make_unique(proto.parameters())) {} // TODO(pifon): Add a test. -void RelativePoseConstraint2D::AddToOptimizer(Nodes* nodes, - ceres::Problem* problem) const { +void RelativePoseConstraint2D::AddToSolver(Nodes* nodes, + ceres::Problem* problem) const { FIND_NODE_OR_RETURN(first_node, first_, nodes->pose_2d_nodes, "First node was not found in pose_2d_nodes."); FIND_NODE_OR_RETURN(second_node, second_, nodes->pose_2d_nodes, diff --git a/cartographer/pose_graph/constraint/relative_pose_constraint_2d.h b/cartographer/pose_graph/constraint/relative_pose_constraint_2d.h index dc76a6a..23c0bff 100644 --- a/cartographer/pose_graph/constraint/relative_pose_constraint_2d.h +++ b/cartographer/pose_graph/constraint/relative_pose_constraint_2d.h @@ -29,7 +29,7 @@ class RelativePoseConstraint2D : public Constraint { const proto::LossFunction& loss_function_proto, const proto::RelativePose2D& proto); - void AddToOptimizer(Nodes* nodes, ceres::Problem* problem) const final; + void AddToSolver(Nodes* nodes, ceres::Problem* problem) const final; protected: proto::CostFunction ToCostFunctionProto() const final; diff --git a/cartographer/pose_graph/constraint/relative_pose_constraint_3d.cc b/cartographer/pose_graph/constraint/relative_pose_constraint_3d.cc index dc19b8b..e688118 100644 --- a/cartographer/pose_graph/constraint/relative_pose_constraint_3d.cc +++ b/cartographer/pose_graph/constraint/relative_pose_constraint_3d.cc @@ -32,8 +32,8 @@ RelativePoseConstraint3D::RelativePoseConstraint3D( cost_(new RelativePoseCost3D(proto.parameters())), ceres_cost_(absl::make_unique(cost_)) {} -void RelativePoseConstraint3D::AddToOptimizer(Nodes* nodes, - ceres::Problem* problem) const { +void RelativePoseConstraint3D::AddToSolver(Nodes* nodes, + ceres::Problem* problem) const { FIND_NODE_OR_RETURN(first_node, first_, nodes->pose_3d_nodes, "First node was not found in pose_3d_nodes."); FIND_NODE_OR_RETURN(second_node, second_, nodes->pose_3d_nodes, diff --git a/cartographer/pose_graph/constraint/relative_pose_constraint_3d.h b/cartographer/pose_graph/constraint/relative_pose_constraint_3d.h index f6950cd..aadc066 100644 --- a/cartographer/pose_graph/constraint/relative_pose_constraint_3d.h +++ b/cartographer/pose_graph/constraint/relative_pose_constraint_3d.h @@ -29,7 +29,7 @@ class RelativePoseConstraint3D : public Constraint { const proto::LossFunction& loss_function_proto, const proto::RelativePose3D& proto); - void AddToOptimizer(Nodes* nodes, ceres::Problem* problem) const final; + void AddToSolver(Nodes* nodes, ceres::Problem* problem) const final; protected: proto::CostFunction ToCostFunctionProto() const final; diff --git a/cartographer/pose_graph/constraint/rotation_constraint_3d.cc b/cartographer/pose_graph/constraint/rotation_constraint_3d.cc index f1ed3db..b9bf6ee 100644 --- a/cartographer/pose_graph/constraint/rotation_constraint_3d.cc +++ b/cartographer/pose_graph/constraint/rotation_constraint_3d.cc @@ -53,8 +53,8 @@ RotationContraint3D::RotationContraint3D( cost_(new RotationCost3D(proto.parameters())), ceres_cost_(absl::make_unique(cost_)) {} -void RotationContraint3D::AddToOptimizer(Nodes* nodes, - ceres::Problem* problem) const { +void RotationContraint3D::AddToSolver(Nodes* nodes, + ceres::Problem* problem) const { FIND_NODE_OR_RETURN(first_node, first_, nodes->pose_3d_nodes, "First node was not found in pose_3d_nodes."); FIND_NODE_OR_RETURN(second_node, second_, nodes->pose_3d_nodes, diff --git a/cartographer/pose_graph/constraint/rotation_constraint_3d.h b/cartographer/pose_graph/constraint/rotation_constraint_3d.h index f681496..b0dbe24 100644 --- a/cartographer/pose_graph/constraint/rotation_constraint_3d.h +++ b/cartographer/pose_graph/constraint/rotation_constraint_3d.h @@ -29,7 +29,7 @@ class RotationContraint3D : public Constraint { const proto::LossFunction& loss_function_proto, const proto::Rotation3D& proto); - void AddToOptimizer(Nodes* nodes, ceres::Problem* problem) const final; + void AddToSolver(Nodes* nodes, ceres::Problem* problem) const final; protected: proto::CostFunction ToCostFunctionProto() const final; diff --git a/cartographer/pose_graph/pose_graph_controller.cc b/cartographer/pose_graph/pose_graph_controller.cc index b79b48c..bdd56d9 100644 --- a/cartographer/pose_graph/pose_graph_controller.cc +++ b/cartographer/pose_graph/pose_graph_controller.cc @@ -29,9 +29,9 @@ void PoseGraphController::AddConstraint(const proto::Constraint& constraint) { AddConstraintToPoseGraphData(constraint, &data_); } -Optimizer::SolverStatus PoseGraphController::Optimize() { +Solver::SolverStatus PoseGraphController::Optimize() { absl::MutexLock locker(&mutex_); - return optimizer_->Solve(&data_); + return solver_->Solve(&data_); } } // namespace pose_graph diff --git a/cartographer/pose_graph/pose_graph_controller.h b/cartographer/pose_graph/pose_graph_controller.h index fd18896..33359da 100644 --- a/cartographer/pose_graph/pose_graph_controller.h +++ b/cartographer/pose_graph/pose_graph_controller.h @@ -18,15 +18,15 @@ #define CARTOGRAPHER_POSE_GRAPH_POSE_GRAPH_CONTROLLER_H_ #include "absl/synchronization/mutex.h" -#include "cartographer/pose_graph/optimizer/optimizer.h" #include "cartographer/pose_graph/pose_graph_data.h" +#include "cartographer/pose_graph/solver/solver.h" namespace cartographer { namespace pose_graph { class PoseGraphController { - PoseGraphController(std::unique_ptr optimizer) - : optimizer_(std::move(optimizer)) {} + PoseGraphController(std::unique_ptr optimizer) + : solver_(std::move(optimizer)) {} PoseGraphController(const PoseGraphController&) = delete; PoseGraphController& operator=(const PoseGraphController&) = delete; @@ -35,10 +35,10 @@ class PoseGraphController { void AddConstraint(const proto::Constraint& constraint) LOCKS_EXCLUDED(mutex_); - Optimizer::SolverStatus Optimize() LOCKS_EXCLUDED(mutex_); + Solver::SolverStatus Optimize() LOCKS_EXCLUDED(mutex_); private: - std::unique_ptr optimizer_; + std::unique_ptr solver_; mutable absl::Mutex mutex_; PoseGraphData data_ GUARDED_BY(mutex_); diff --git a/cartographer/pose_graph/optimizer/ceres_optimizer.cc b/cartographer/pose_graph/solver/ceres_solver.cc similarity index 78% rename from cartographer/pose_graph/optimizer/ceres_optimizer.cc rename to cartographer/pose_graph/solver/ceres_solver.cc index d08347d..3a7421b 100644 --- a/cartographer/pose_graph/optimizer/ceres_optimizer.cc +++ b/cartographer/pose_graph/solver/ceres_solver.cc @@ -14,7 +14,7 @@ * limitations under the License. */ -#include "cartographer/pose_graph/optimizer/ceres_optimizer.h" +#include "cartographer/pose_graph/solver/ceres_solver.h" namespace cartographer { namespace pose_graph { @@ -31,28 +31,28 @@ ceres::Problem::Options CreateCeresProblemOptions() { return problem_options; } -Optimizer::SolverStatus ToSolverStatus( +Solver::SolverStatus ToSolverStatus( const ceres::TerminationType& termination_type) { switch (termination_type) { case (ceres::TerminationType::CONVERGENCE): - return Optimizer::SolverStatus::CONVERGENCE; + return Solver::SolverStatus::CONVERGENCE; case (ceres::TerminationType::NO_CONVERGENCE): - return Optimizer::SolverStatus::NO_CONVERGENCE; + return Solver::SolverStatus::NO_CONVERGENCE; default: - return Optimizer::SolverStatus::FAILURE; + return Solver::SolverStatus::FAILURE; } } } // namespace -CeresOptimizer::CeresOptimizer(const ceres::Solver::Options& options) +CeresSolver::CeresSolver(const ceres::Solver::Options& options) : problem_options_(CreateCeresProblemOptions()), solver_options_(options) {} -Optimizer::SolverStatus CeresOptimizer::Solve(PoseGraphData* data) const { +Solver::SolverStatus CeresSolver::Solve(PoseGraphData* data) const { ceres::Problem problem(problem_options_); for (const auto& constraint : data->constraints) { - constraint->AddToOptimizer(&data->nodes, &problem); + constraint->AddToSolver(&data->nodes, &problem); } ceres::Solver::Summary summary; diff --git a/cartographer/pose_graph/optimizer/ceres_optimizer.h b/cartographer/pose_graph/solver/ceres_solver.h similarity index 73% rename from cartographer/pose_graph/optimizer/ceres_optimizer.h rename to cartographer/pose_graph/solver/ceres_solver.h index 3689ff3..fcaa365 100644 --- a/cartographer/pose_graph/optimizer/ceres_optimizer.h +++ b/cartographer/pose_graph/solver/ceres_solver.h @@ -14,19 +14,19 @@ * limitations under the License. */ -#ifndef CARTOGRAPHER_POSE_GRAPH_OPTIMIZER_CERES_OPTIMIZER_H_ -#define CARTOGRAPHER_POSE_GRAPH_OPTIMIZER_CERES_OPTIMIZER_H_ +#ifndef CARTOGRAPHER_POSE_GRAPH_OPTIMIZER_CERES_SOLVER_H_ +#define CARTOGRAPHER_POSE_GRAPH_OPTIMIZER_CERES_SOLVER_H_ -#include "cartographer/pose_graph/optimizer/optimizer.h" +#include "cartographer/pose_graph/solver/solver.h" #include "ceres/problem.h" #include "ceres/solver.h" namespace cartographer { namespace pose_graph { -class CeresOptimizer : public Optimizer { +class CeresSolver : public Solver { public: - explicit CeresOptimizer(const ceres::Solver::Options& options); + explicit CeresSolver(const ceres::Solver::Options& options); SolverStatus Solve(PoseGraphData* data) const final; @@ -38,4 +38,4 @@ class CeresOptimizer : public Optimizer { } // namespace pose_graph } // namespace cartographer -#endif // CARTOGRAPHER_POSE_GRAPH_OPTIMIZER_CERES_OPTIMIZER_H_ +#endif // CARTOGRAPHER_POSE_GRAPH_OPTIMIZER_CERES_SOLVER_H_ diff --git a/cartographer/pose_graph/optimizer/ceres_optimizer_test.cc b/cartographer/pose_graph/solver/ceres_solver_test.cc similarity index 89% rename from cartographer/pose_graph/optimizer/ceres_optimizer_test.cc rename to cartographer/pose_graph/solver/ceres_solver_test.cc index d4ab8e4..f86b9ae 100644 --- a/cartographer/pose_graph/optimizer/ceres_optimizer_test.cc +++ b/cartographer/pose_graph/solver/ceres_solver_test.cc @@ -14,7 +14,7 @@ * limitations under the License. */ -#include "cartographer/pose_graph/optimizer/ceres_optimizer.h" +#include "cartographer/pose_graph/solver/ceres_solver.h" #include "absl/memory/memory.h" #include "cartographer/pose_graph/constraint/relative_pose_constraint_2d.h" @@ -67,14 +67,14 @@ constexpr char kRelativePose2D[] = R"PROTO( } )PROTO"; -TEST(CeresOptimizerTest, SmokeTest) { +TEST(CeresSolverTest, SmokeTest) { PoseGraphData data; AddNodeToPoseGraphData(ParseProto(kStartNode), &data); AddNodeToPoseGraphData(ParseProto(kEndNode), &data); AddConstraintToPoseGraphData(ParseProto(kRelativePose2D), &data); - CeresOptimizer optimizer(ceres::Solver::Options{}); - EXPECT_EQ(optimizer.Solve(&data), Optimizer::SolverStatus::CONVERGENCE); + CeresSolver optimizer(ceres::Solver::Options{}); + EXPECT_EQ(optimizer.Solve(&data), Solver::SolverStatus::CONVERGENCE); } } // namespace diff --git a/cartographer/pose_graph/optimizer/optimizer.h b/cartographer/pose_graph/solver/solver.h similarity index 73% rename from cartographer/pose_graph/optimizer/optimizer.h rename to cartographer/pose_graph/solver/solver.h index c6aa062..11bb2a1 100644 --- a/cartographer/pose_graph/optimizer/optimizer.h +++ b/cartographer/pose_graph/solver/solver.h @@ -14,15 +14,15 @@ * limitations under the License. */ -#ifndef CARTOGRAPHER_POSE_GRAPH_OPTIMIZER_OPTIMIZER_H_ -#define CARTOGRAPHER_POSE_GRAPH_OPTIMIZER_OPTIMIZER_H_ +#ifndef CARTOGRAPHER_POSE_GRAPH_SOLVER_SOLVER_H_ +#define CARTOGRAPHER_POSE_GRAPH_SOLVER_SOLVER_H_ #include "cartographer/pose_graph/pose_graph_data.h" namespace cartographer { namespace pose_graph { -class Optimizer { +class Solver { public: enum class SolverStatus { CONVERGENCE, @@ -30,11 +30,11 @@ class Optimizer { FAILURE, }; - Optimizer() = default; - virtual ~Optimizer() = default; + Solver() = default; + virtual ~Solver() = default; - Optimizer(const Optimizer&) = delete; - Optimizer& operator=(const Optimizer&) = delete; + Solver(const Solver&) = delete; + Solver& operator=(const Solver&) = delete; virtual SolverStatus Solve(PoseGraphData* data) const = 0; }; @@ -42,4 +42,4 @@ class Optimizer { } // namespace pose_graph } // namespace cartographer -#endif // CARTOGRAPHER_POSE_GRAPH_OPTIMIZER_OPTIMIZER_H_ +#endif // CARTOGRAPHER_POSE_GRAPH_SOLVER_SOLVER_H_