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

[GenericPoseGraph] Rename class Optimizer -> Solver. (#1435)

master
Alexander Belyaev 2018-10-01 13:48:52 +02:00 committed by Wally B. Feed
parent ba859a6ed5
commit 64062ee720
19 changed files with 50 additions and 50 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
cartographer/pose_graph/constraint/acceleration_constraint_3d.cc
View File

@ -34,8 +34,8 @@ AccelerationConstraint3D::AccelerationConstraint3D(
cost_(new AccelerationCost3D(proto.parameters())), cost_(new AccelerationCost3D(proto.parameters())),
ceres_cost_(absl::make_unique<AutoDiffFunction>(cost_)) {} ceres_cost_(absl::make_unique<AutoDiffFunction>(cost_)) {}
void AccelerationConstraint3D::AddToOptimizer(Nodes* nodes, void AccelerationConstraint3D::AddToSolver(Nodes* nodes,
ceres::Problem* problem) const { ceres::Problem* problem) const {
FIND_NODE_OR_RETURN(first_node, first_, nodes->pose_3d_nodes, FIND_NODE_OR_RETURN(first_node, first_, nodes->pose_3d_nodes,
"First node was not found in pose_3d_nodes."); "First node was not found in pose_3d_nodes.");
FIND_NODE_OR_RETURN(second_node, second_, nodes->pose_3d_nodes, FIND_NODE_OR_RETURN(second_node, second_, nodes->pose_3d_nodes,

2
cartographer/pose_graph/constraint/acceleration_constraint_3d.h
View File

@ -29,7 +29,7 @@ class AccelerationConstraint3D : public Constraint {
const proto::LossFunction& loss_function_proto, const proto::LossFunction& loss_function_proto,
const proto::Acceleration3D& proto); const proto::Acceleration3D& proto);
void AddToOptimizer(Nodes* nodes, ceres::Problem* problem) const final; void AddToSolver(Nodes* nodes, ceres::Problem* problem) const final;
protected: protected:
proto::CostFunction ToCostFunctionProto() const final; proto::CostFunction ToCostFunctionProto() const final;

2
cartographer/pose_graph/constraint/constraint.h
View File

@ -43,7 +43,7 @@ class Constraint {
const ConstraintId& constraint_id() const { return constraint_id_; } 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: protected:
virtual proto::CostFunction ToCostFunctionProto() const = 0; virtual proto::CostFunction ToCostFunctionProto() const = 0;

2
cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_2d.cc
View File

@ -33,7 +33,7 @@ InterpolatedRelativePoseConstraint2D::InterpolatedRelativePoseConstraint2D(
cost_(new InterpolatedRelativePoseCost2D(proto.parameters())), cost_(new InterpolatedRelativePoseCost2D(proto.parameters())),
ceres_cost_(absl::make_unique<AutoDiffFunction>(cost_)) {} ceres_cost_(absl::make_unique<AutoDiffFunction>(cost_)) {}
void InterpolatedRelativePoseConstraint2D::AddToOptimizer( void InterpolatedRelativePoseConstraint2D::AddToSolver(
Nodes* nodes, ceres::Problem* problem) const { Nodes* nodes, ceres::Problem* problem) const {
FIND_NODE_OR_RETURN(first_node_start, first_start_, nodes->pose_2d_nodes, FIND_NODE_OR_RETURN(first_node_start, first_start_, nodes->pose_2d_nodes,
"First node (start) was not found in pose_2d_nodes."); "First node (start) was not found in pose_2d_nodes.");

2
cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_2d.h
View File

@ -29,7 +29,7 @@ class InterpolatedRelativePoseConstraint2D : public Constraint {
const ConstraintId& id, const proto::LossFunction& loss_function_proto, const ConstraintId& id, const proto::LossFunction& loss_function_proto,
const proto::InterpolatedRelativePose2D& proto); const proto::InterpolatedRelativePose2D& proto);
void AddToOptimizer(Nodes* nodes, ceres::Problem* problem) const final; void AddToSolver(Nodes* nodes, ceres::Problem* problem) const final;
protected: protected:
proto::CostFunction ToCostFunctionProto() const final; proto::CostFunction ToCostFunctionProto() const final;

2
cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_3d.cc
View File

@ -33,7 +33,7 @@ InterpolatedRelativePoseConstraint3D::InterpolatedRelativePoseConstraint3D(
cost_(new InterpolatedRelativePoseCost3D(proto.parameters())), cost_(new InterpolatedRelativePoseCost3D(proto.parameters())),
ceres_cost_(absl::make_unique<AutoDiffFunction>(cost_)) {} ceres_cost_(absl::make_unique<AutoDiffFunction>(cost_)) {}
void InterpolatedRelativePoseConstraint3D::AddToOptimizer( void InterpolatedRelativePoseConstraint3D::AddToSolver(
Nodes* nodes, ceres::Problem* problem) const { Nodes* nodes, ceres::Problem* problem) const {
FIND_NODE_OR_RETURN(first_node_start, first_start_, nodes->pose_3d_nodes, FIND_NODE_OR_RETURN(first_node_start, first_start_, nodes->pose_3d_nodes,
"First node (start) was not found in pose_3d_nodes."); "First node (start) was not found in pose_3d_nodes.");

2
cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_3d.h
View File

@ -29,7 +29,7 @@ class InterpolatedRelativePoseConstraint3D : public Constraint {
const ConstraintId& id, const proto::LossFunction& loss_function_proto, const ConstraintId& id, const proto::LossFunction& loss_function_proto,
const proto::InterpolatedRelativePose3D& proto); const proto::InterpolatedRelativePose3D& proto);
void AddToOptimizer(Nodes* nodes, ceres::Problem* problem) const final; void AddToSolver(Nodes* nodes, ceres::Problem* problem) const final;
protected: protected:
proto::CostFunction ToCostFunctionProto() const final; proto::CostFunction ToCostFunctionProto() const final;

4
cartographer/pose_graph/constraint/relative_pose_constraint_2d.cc
View File

@ -32,8 +32,8 @@ RelativePoseConstraint2D::RelativePoseConstraint2D(
ceres_cost_(absl::make_unique<RelativePoseCost2D>(proto.parameters())) {} ceres_cost_(absl::make_unique<RelativePoseCost2D>(proto.parameters())) {}
// TODO(pifon): Add a test. // TODO(pifon): Add a test.
void RelativePoseConstraint2D::AddToOptimizer(Nodes* nodes, void RelativePoseConstraint2D::AddToSolver(Nodes* nodes,
ceres::Problem* problem) const { ceres::Problem* problem) const {
FIND_NODE_OR_RETURN(first_node, first_, nodes->pose_2d_nodes, FIND_NODE_OR_RETURN(first_node, first_, nodes->pose_2d_nodes,
"First node was not found in pose_2d_nodes."); "First node was not found in pose_2d_nodes.");
FIND_NODE_OR_RETURN(second_node, second_, nodes->pose_2d_nodes, FIND_NODE_OR_RETURN(second_node, second_, nodes->pose_2d_nodes,

2
cartographer/pose_graph/constraint/relative_pose_constraint_2d.h
View File

@ -29,7 +29,7 @@ class RelativePoseConstraint2D : public Constraint {
const proto::LossFunction& loss_function_proto, const proto::LossFunction& loss_function_proto,
const proto::RelativePose2D& proto); const proto::RelativePose2D& proto);
void AddToOptimizer(Nodes* nodes, ceres::Problem* problem) const final; void AddToSolver(Nodes* nodes, ceres::Problem* problem) const final;
protected: protected:
proto::CostFunction ToCostFunctionProto() const final; proto::CostFunction ToCostFunctionProto() const final;

4
cartographer/pose_graph/constraint/relative_pose_constraint_3d.cc
View File

@ -32,8 +32,8 @@ RelativePoseConstraint3D::RelativePoseConstraint3D(
cost_(new RelativePoseCost3D(proto.parameters())), cost_(new RelativePoseCost3D(proto.parameters())),
ceres_cost_(absl::make_unique<AutoDiffFunction>(cost_)) {} ceres_cost_(absl::make_unique<AutoDiffFunction>(cost_)) {}
void RelativePoseConstraint3D::AddToOptimizer(Nodes* nodes, void RelativePoseConstraint3D::AddToSolver(Nodes* nodes,
ceres::Problem* problem) const { ceres::Problem* problem) const {
FIND_NODE_OR_RETURN(first_node, first_, nodes->pose_3d_nodes, FIND_NODE_OR_RETURN(first_node, first_, nodes->pose_3d_nodes,
"First node was not found in pose_3d_nodes."); "First node was not found in pose_3d_nodes.");
FIND_NODE_OR_RETURN(second_node, second_, nodes->pose_3d_nodes, FIND_NODE_OR_RETURN(second_node, second_, nodes->pose_3d_nodes,

2
cartographer/pose_graph/constraint/relative_pose_constraint_3d.h
View File

@ -29,7 +29,7 @@ class RelativePoseConstraint3D : public Constraint {
const proto::LossFunction& loss_function_proto, const proto::LossFunction& loss_function_proto,
const proto::RelativePose3D& proto); const proto::RelativePose3D& proto);
void AddToOptimizer(Nodes* nodes, ceres::Problem* problem) const final; void AddToSolver(Nodes* nodes, ceres::Problem* problem) const final;
protected: protected:
proto::CostFunction ToCostFunctionProto() const final; proto::CostFunction ToCostFunctionProto() const final;

4
cartographer/pose_graph/constraint/rotation_constraint_3d.cc
View File

@ -53,8 +53,8 @@ RotationContraint3D::RotationContraint3D(
cost_(new RotationCost3D(proto.parameters())), cost_(new RotationCost3D(proto.parameters())),
ceres_cost_(absl::make_unique<AutoDiffFunction>(cost_)) {} ceres_cost_(absl::make_unique<AutoDiffFunction>(cost_)) {}
void RotationContraint3D::AddToOptimizer(Nodes* nodes, void RotationContraint3D::AddToSolver(Nodes* nodes,
ceres::Problem* problem) const { ceres::Problem* problem) const {
FIND_NODE_OR_RETURN(first_node, first_, nodes->pose_3d_nodes, FIND_NODE_OR_RETURN(first_node, first_, nodes->pose_3d_nodes,
"First node was not found in pose_3d_nodes."); "First node was not found in pose_3d_nodes.");
FIND_NODE_OR_RETURN(second_node, second_, nodes->pose_3d_nodes, FIND_NODE_OR_RETURN(second_node, second_, nodes->pose_3d_nodes,

2
cartographer/pose_graph/constraint/rotation_constraint_3d.h
View File

@ -29,7 +29,7 @@ class RotationContraint3D : public Constraint {
const proto::LossFunction& loss_function_proto, const proto::LossFunction& loss_function_proto,
const proto::Rotation3D& proto); const proto::Rotation3D& proto);
void AddToOptimizer(Nodes* nodes, ceres::Problem* problem) const final; void AddToSolver(Nodes* nodes, ceres::Problem* problem) const final;
protected: protected:
proto::CostFunction ToCostFunctionProto() const final; proto::CostFunction ToCostFunctionProto() const final;

4
cartographer/pose_graph/pose_graph_controller.cc
View File

@ -29,9 +29,9 @@ void PoseGraphController::AddConstraint(const proto::Constraint& constraint) {
AddConstraintToPoseGraphData(constraint, &data_); AddConstraintToPoseGraphData(constraint, &data_);
} }
Optimizer::SolverStatus PoseGraphController::Optimize() { Solver::SolverStatus PoseGraphController::Optimize() {
absl::MutexLock locker(&mutex_); absl::MutexLock locker(&mutex_);
return optimizer_->Solve(&data_); return solver_->Solve(&data_);
} }
} // namespace pose_graph } // namespace pose_graph

10
cartographer/pose_graph/pose_graph_controller.h
View File

@ -18,15 +18,15 @@
#define CARTOGRAPHER_POSE_GRAPH_POSE_GRAPH_CONTROLLER_H_ #define CARTOGRAPHER_POSE_GRAPH_POSE_GRAPH_CONTROLLER_H_
#include "absl/synchronization/mutex.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/pose_graph_data.h"
#include "cartographer/pose_graph/solver/solver.h"
namespace cartographer { namespace cartographer {
namespace pose_graph { namespace pose_graph {
class PoseGraphController { class PoseGraphController {
PoseGraphController(std::unique_ptr<Optimizer> optimizer) PoseGraphController(std::unique_ptr<Solver> optimizer)
: optimizer_(std::move(optimizer)) {} : solver_(std::move(optimizer)) {}
PoseGraphController(const PoseGraphController&) = delete; PoseGraphController(const PoseGraphController&) = delete;
PoseGraphController& operator=(const PoseGraphController&) = delete; PoseGraphController& operator=(const PoseGraphController&) = delete;
@ -35,10 +35,10 @@ class PoseGraphController {
void AddConstraint(const proto::Constraint& constraint) void AddConstraint(const proto::Constraint& constraint)
LOCKS_EXCLUDED(mutex_); LOCKS_EXCLUDED(mutex_);
Optimizer::SolverStatus Optimize() LOCKS_EXCLUDED(mutex_); Solver::SolverStatus Optimize() LOCKS_EXCLUDED(mutex_);
private: private:
std::unique_ptr<Optimizer> optimizer_; std::unique_ptr<Solver> solver_;
mutable absl::Mutex mutex_; mutable absl::Mutex mutex_;
PoseGraphData data_ GUARDED_BY(mutex_); PoseGraphData data_ GUARDED_BY(mutex_);

16
cartographer/pose_graph/optimizer/ceres_optimizer.cc → cartographer/pose_graph/solver/ceres_solver.cc
View File

@ -14,7 +14,7 @@
* limitations under the License. * limitations under the License.
*/ */
#include "cartographer/pose_graph/optimizer/ceres_optimizer.h" #include "cartographer/pose_graph/solver/ceres_solver.h"
namespace cartographer { namespace cartographer {
namespace pose_graph { namespace pose_graph {
@ -31,28 +31,28 @@ ceres::Problem::Options CreateCeresProblemOptions() {
return problem_options; return problem_options;
} }
Optimizer::SolverStatus ToSolverStatus( Solver::SolverStatus ToSolverStatus(
const ceres::TerminationType& termination_type) { const ceres::TerminationType& termination_type) {
switch (termination_type) { switch (termination_type) {
case (ceres::TerminationType::CONVERGENCE): case (ceres::TerminationType::CONVERGENCE):
return Optimizer::SolverStatus::CONVERGENCE; return Solver::SolverStatus::CONVERGENCE;
case (ceres::TerminationType::NO_CONVERGENCE): case (ceres::TerminationType::NO_CONVERGENCE):
return Optimizer::SolverStatus::NO_CONVERGENCE; return Solver::SolverStatus::NO_CONVERGENCE;
default: default:
return Optimizer::SolverStatus::FAILURE; return Solver::SolverStatus::FAILURE;
} }
} }
} // namespace } // namespace
CeresOptimizer::CeresOptimizer(const ceres::Solver::Options& options) CeresSolver::CeresSolver(const ceres::Solver::Options& options)
: problem_options_(CreateCeresProblemOptions()), 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_); ceres::Problem problem(problem_options_);
for (const auto& constraint : data->constraints) { for (const auto& constraint : data->constraints) {
constraint->AddToOptimizer(&data->nodes, &problem); constraint->AddToSolver(&data->nodes, &problem);
} }
ceres::Solver::Summary summary; ceres::Solver::Summary summary;

12
cartographer/pose_graph/optimizer/ceres_optimizer.h → cartographer/pose_graph/solver/ceres_solver.h
View File

@ -14,19 +14,19 @@
* limitations under the License. * limitations under the License.
*/ */
#ifndef CARTOGRAPHER_POSE_GRAPH_OPTIMIZER_CERES_OPTIMIZER_H_ #ifndef CARTOGRAPHER_POSE_GRAPH_OPTIMIZER_CERES_SOLVER_H_
#define CARTOGRAPHER_POSE_GRAPH_OPTIMIZER_CERES_OPTIMIZER_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/problem.h"
#include "ceres/solver.h" #include "ceres/solver.h"
namespace cartographer { namespace cartographer {
namespace pose_graph { namespace pose_graph {
class CeresOptimizer : public Optimizer { class CeresSolver : public Solver {
public: public:
explicit CeresOptimizer(const ceres::Solver::Options& options); explicit CeresSolver(const ceres::Solver::Options& options);
SolverStatus Solve(PoseGraphData* data) const final; SolverStatus Solve(PoseGraphData* data) const final;
@ -38,4 +38,4 @@ class CeresOptimizer : public Optimizer {
} // namespace pose_graph } // namespace pose_graph
} // namespace cartographer } // namespace cartographer
#endif // CARTOGRAPHER_POSE_GRAPH_OPTIMIZER_CERES_OPTIMIZER_H_ #endif // CARTOGRAPHER_POSE_GRAPH_OPTIMIZER_CERES_SOLVER_H_

8
cartographer/pose_graph/optimizer/ceres_optimizer_test.cc → cartographer/pose_graph/solver/ceres_solver_test.cc
View File

@ -14,7 +14,7 @@
* limitations under the License. * 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 "absl/memory/memory.h"
#include "cartographer/pose_graph/constraint/relative_pose_constraint_2d.h" #include "cartographer/pose_graph/constraint/relative_pose_constraint_2d.h"
@ -67,14 +67,14 @@ constexpr char kRelativePose2D[] = R"PROTO(
} }
)PROTO"; )PROTO";
TEST(CeresOptimizerTest, SmokeTest) { TEST(CeresSolverTest, SmokeTest) {
PoseGraphData data; PoseGraphData data;
AddNodeToPoseGraphData(ParseProto<proto::Node>(kStartNode), &data); AddNodeToPoseGraphData(ParseProto<proto::Node>(kStartNode), &data);
AddNodeToPoseGraphData(ParseProto<proto::Node>(kEndNode), &data); AddNodeToPoseGraphData(ParseProto<proto::Node>(kEndNode), &data);
AddConstraintToPoseGraphData(ParseProto<proto::Constraint>(kRelativePose2D), AddConstraintToPoseGraphData(ParseProto<proto::Constraint>(kRelativePose2D),
&data); &data);
CeresOptimizer optimizer(ceres::Solver::Options{}); CeresSolver optimizer(ceres::Solver::Options{});
EXPECT_EQ(optimizer.Solve(&data), Optimizer::SolverStatus::CONVERGENCE); EXPECT_EQ(optimizer.Solve(&data), Solver::SolverStatus::CONVERGENCE);
} }
} // namespace } // namespace

16
cartographer/pose_graph/optimizer/optimizer.h → cartographer/pose_graph/solver/solver.h
View File

@ -14,15 +14,15 @@
* limitations under the License. * limitations under the License.
*/ */
#ifndef CARTOGRAPHER_POSE_GRAPH_OPTIMIZER_OPTIMIZER_H_ #ifndef CARTOGRAPHER_POSE_GRAPH_SOLVER_SOLVER_H_
#define CARTOGRAPHER_POSE_GRAPH_OPTIMIZER_OPTIMIZER_H_ #define CARTOGRAPHER_POSE_GRAPH_SOLVER_SOLVER_H_
#include "cartographer/pose_graph/pose_graph_data.h" #include "cartographer/pose_graph/pose_graph_data.h"
namespace cartographer { namespace cartographer {
namespace pose_graph { namespace pose_graph {
class Optimizer { class Solver {
public: public:
enum class SolverStatus { enum class SolverStatus {
CONVERGENCE, CONVERGENCE,
@ -30,11 +30,11 @@ class Optimizer {
FAILURE, FAILURE,
}; };
Optimizer() = default; Solver() = default;
virtual ~Optimizer() = default; virtual ~Solver() = default;
Optimizer(const Optimizer&) = delete; Solver(const Solver&) = delete;
Optimizer& operator=(const Optimizer&) = delete; Solver& operator=(const Solver&) = delete;
virtual SolverStatus Solve(PoseGraphData* data) const = 0; virtual SolverStatus Solve(PoseGraphData* data) const = 0;
}; };
@ -42,4 +42,4 @@ class Optimizer {
} // namespace pose_graph } // namespace pose_graph
} // namespace cartographer } // namespace cartographer
#endif // CARTOGRAPHER_POSE_GRAPH_OPTIMIZER_OPTIMIZER_H_ #endif // CARTOGRAPHER_POSE_GRAPH_SOLVER_SOLVER_H_