Skip to content

Commit

Permalink
[GenericPoseGraph] Rename class Optimizer -> Solver. (#1435)
Browse files Browse the repository at this point in the history
  • Loading branch information
pifon2a authored and wally-the-cartographer committed Oct 1, 2018
1 parent ba859a6 commit 64062ee
Show file tree
Hide file tree
Showing 19 changed files with 50 additions and 50 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -34,8 +34,8 @@ AccelerationConstraint3D::AccelerationConstraint3D(
cost_(new AccelerationCost3D(proto.parameters())),
ceres_cost_(absl::make_unique<AutoDiffFunction>(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,
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
2 changes: 1 addition & 1 deletion cartographer/pose_graph/constraint/constraint.h
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -33,7 +33,7 @@ InterpolatedRelativePoseConstraint2D::InterpolatedRelativePoseConstraint2D(
cost_(new InterpolatedRelativePoseCost2D(proto.parameters())),
ceres_cost_(absl::make_unique<AutoDiffFunction>(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.");
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -33,7 +33,7 @@ InterpolatedRelativePoseConstraint3D::InterpolatedRelativePoseConstraint3D(
cost_(new InterpolatedRelativePoseCost3D(proto.parameters())),
ceres_cost_(absl::make_unique<AutoDiffFunction>(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.");
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -32,8 +32,8 @@ RelativePoseConstraint2D::RelativePoseConstraint2D(
ceres_cost_(absl::make_unique<RelativePoseCost2D>(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,
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -32,8 +32,8 @@ RelativePoseConstraint3D::RelativePoseConstraint3D(
cost_(new RelativePoseCost3D(proto.parameters())),
ceres_cost_(absl::make_unique<AutoDiffFunction>(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,
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
4 changes: 2 additions & 2 deletions cartographer/pose_graph/constraint/rotation_constraint_3d.cc
Original file line number Diff line number Diff line change
Expand Up @@ -53,8 +53,8 @@ RotationContraint3D::RotationContraint3D(
cost_(new RotationCost3D(proto.parameters())),
ceres_cost_(absl::make_unique<AutoDiffFunction>(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,
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
4 changes: 2 additions & 2 deletions cartographer/pose_graph/pose_graph_controller.cc
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
10 changes: 5 additions & 5 deletions cartographer/pose_graph/pose_graph_controller.h
Original file line number Diff line number Diff line change
Expand Up @@ -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)
: optimizer_(std::move(optimizer)) {}
PoseGraphController(std::unique_ptr<Solver> optimizer)
: solver_(std::move(optimizer)) {}

PoseGraphController(const PoseGraphController&) = delete;
PoseGraphController& operator=(const PoseGraphController&) = delete;
Expand All @@ -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> optimizer_;
std::unique_ptr<Solver> solver_;

mutable absl::Mutex mutex_;
PoseGraphData data_ GUARDED_BY(mutex_);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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 {
Expand All @@ -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;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand All @@ -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_
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand Down Expand Up @@ -67,14 +67,14 @@ constexpr char kRelativePose2D[] = R"PROTO(
}
)PROTO";

TEST(CeresOptimizerTest, SmokeTest) {
TEST(CeresSolverTest, SmokeTest) {
PoseGraphData data;
AddNodeToPoseGraphData(ParseProto<proto::Node>(kStartNode), &data);
AddNodeToPoseGraphData(ParseProto<proto::Node>(kEndNode), &data);
AddConstraintToPoseGraphData(ParseProto<proto::Constraint>(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
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -14,32 +14,32 @@
* 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,
NO_CONVERGENCE,
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;
};

} // namespace pose_graph
} // namespace cartographer

#endif // CARTOGRAPHER_POSE_GRAPH_OPTIMIZER_OPTIMIZER_H_
#endif // CARTOGRAPHER_POSE_GRAPH_SOLVER_SOLVER_H_

0 comments on commit 64062ee

Please sign in to comment.