diff --git a/cartographer/pose_graph/constraint/constraint.cc b/cartographer/pose_graph/constraint/constraint.cc index b40dc0f..b6dce5a 100644 --- a/cartographer/pose_graph/constraint/constraint.cc +++ b/cartographer/pose_graph/constraint/constraint.cc @@ -19,10 +19,12 @@ namespace cartographer { namespace pose_graph { +// TODO(pifon): Add a test. proto::Constraint Constraint::ToProto() const { proto::Constraint constraint; constraint.set_id(constraint_id_); *constraint.mutable_cost_function() = ToCostFunctionProto(); + *constraint.mutable_loss_function() = loss_function_.ToProto(); return constraint; } diff --git a/cartographer/pose_graph/constraint/constraint.h b/cartographer/pose_graph/constraint/constraint.h index cce2c61..cbbebb4 100644 --- a/cartographer/pose_graph/constraint/constraint.h +++ b/cartographer/pose_graph/constraint/constraint.h @@ -17,6 +17,7 @@ #ifndef CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_CONSTRAINT_H_ #define CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_CONSTRAINT_H_ +#include "cartographer/pose_graph/constraint/loss_function/loss_function.h" #include "cartographer/pose_graph/node/nodes.h" #include "cartographer/pose_graph/proto/constraint.pb.h" #include "ceres/problem.h" @@ -30,7 +31,9 @@ using ConstraintId = std::string; class Constraint { public: - explicit Constraint(const ConstraintId& id) : constraint_id_(id) {} + Constraint(const ConstraintId& id, + const proto::LossFunction& loss_function_proto) + : constraint_id_(id), loss_function_(loss_function_proto) {} virtual ~Constraint() = default; Constraint(const Constraint&) = delete; @@ -45,8 +48,13 @@ class Constraint { protected: virtual proto::CostFunction ToCostFunctionProto() const = 0; + ceres::LossFunction* ceres_loss() const { + return loss_function_.ceres_loss(); + } + private: ConstraintId constraint_id_; + LossFunction loss_function_; }; } // namespace pose_graph diff --git a/cartographer/pose_graph/constraint/loss_function/loss_function.cc b/cartographer/pose_graph/constraint/loss_function/loss_function.cc new file mode 100644 index 0000000..c501516 --- /dev/null +++ b/cartographer/pose_graph/constraint/loss_function/loss_function.cc @@ -0,0 +1,44 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/pose_graph/constraint/loss_function/loss_function.h" + +#include "cartographer/common/make_unique.h" + +namespace cartographer { +namespace pose_graph { +namespace { + +std::unique_ptr CeresLossFromProto( + const proto::LossFunction& proto) { + switch (proto.Type_case()) { + case proto::LossFunction::kHuberLoss: + return common::make_unique(proto.huber_loss().scale()); + case proto::LossFunction::kQuadraticLoss: + return nullptr; + default: + LOG(FATAL) << "The loss function is not specified."; + return nullptr; + } +} + +} // namespace + +LossFunction::LossFunction(const proto::LossFunction& proto) + : proto_(proto), ceres_loss_(CeresLossFromProto(proto_)) {} + +} // namespace pose_graph +} // namespace cartographer diff --git a/cartographer/pose_graph/constraint/loss_function/loss_function.h b/cartographer/pose_graph/constraint/loss_function/loss_function.h new file mode 100644 index 0000000..5cbd533 --- /dev/null +++ b/cartographer/pose_graph/constraint/loss_function/loss_function.h @@ -0,0 +1,44 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_LOSS_FUNCTION_H_ +#define CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_LOSS_FUNCTION_H_ + +#include + +#include "cartographer/pose_graph/proto/loss_function.pb.h" +#include "ceres/loss_function.h" + +namespace cartographer { +namespace pose_graph { + +class LossFunction { + public: + explicit LossFunction(const proto::LossFunction& proto); + + const proto::LossFunction& ToProto() const { return proto_; } + + ceres::LossFunction* ceres_loss() const { return ceres_loss_.get(); } + + private: + const proto::LossFunction proto_; + const std::unique_ptr ceres_loss_; +}; + +} // namespace pose_graph +} // namespace cartographer + +#endif // CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_LOSS_FUNCTION_H_ diff --git a/cartographer/pose_graph/constraint/loss_function/loss_function_test.cc b/cartographer/pose_graph/constraint/loss_function/loss_function_test.cc new file mode 100644 index 0000000..6a751f3 --- /dev/null +++ b/cartographer/pose_graph/constraint/loss_function/loss_function_test.cc @@ -0,0 +1,45 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/pose_graph/constraint/loss_function/loss_function.h" + +#include "cartographer/pose_graph/internal/testing/test_helpers.h" + +namespace cartographer { +namespace pose_graph { +namespace { + +using testing::ParseProto; + +TEST(LossFunctionTest, ConstructQuadraticLoss) { + LossFunction quadratic_loss( + ParseProto(R"(quadratic_loss: {})")); + EXPECT_EQ(nullptr, quadratic_loss.ceres_loss()); +} + +TEST(LossFunctionTest, ConstructHuberLoss) { + LossFunction huber_loss( + ParseProto(R"(huber_loss: { scale: 0.5 })")); + EXPECT_NE(nullptr, dynamic_cast(huber_loss.ceres_loss())); +} + +TEST(LossFunctionDeathTest, FailToConstructUnspecifiedLoss) { + EXPECT_DEATH(LossFunction(proto::LossFunction{}), ""); +} + +} // namespace +} // namespace pose_graph +} // namespace cartographer diff --git a/cartographer/pose_graph/constraint/relative_pose_constraint_2d.cc b/cartographer/pose_graph/constraint/relative_pose_constraint_2d.cc index f4c6dd0..a1761ca 100644 --- a/cartographer/pose_graph/constraint/relative_pose_constraint_2d.cc +++ b/cartographer/pose_graph/constraint/relative_pose_constraint_2d.cc @@ -23,13 +23,15 @@ namespace cartographer { namespace pose_graph { RelativePoseConstraint2D::RelativePoseConstraint2D( - const ConstraintId& id, const proto::RelativePose2D& proto) - : Constraint(id), + const ConstraintId& id, const proto::LossFunction& loss_function_proto, + const proto::RelativePose2D& proto) + : Constraint(id, loss_function_proto), first_(proto.first()), second_(proto.second()), ceres_cost_(common::make_unique(proto.parameters())) { } +// TODO(pifon): Add a test. void RelativePoseConstraint2D::AddToOptimizer(Nodes* nodes, ceres::Problem* problem) const { auto first_node = common::FindOrNull(nodes->pose_2d_nodes, first_); @@ -60,8 +62,8 @@ void RelativePoseConstraint2D::AddToOptimizer(Nodes* nodes, if (second_node->constant()) { problem->SetParameterBlockConstant(second_pose->data()); } - problem->AddResidualBlock(ceres_cost_.get(), nullptr /* loss function */, - first_pose->data(), second_pose->data()); + problem->AddResidualBlock(ceres_cost_.get(), ceres_loss(), first_pose->data(), + second_pose->data()); } proto::CostFunction RelativePoseConstraint2D::ToCostFunctionProto() const { diff --git a/cartographer/pose_graph/constraint/relative_pose_constraint_2d.h b/cartographer/pose_graph/constraint/relative_pose_constraint_2d.h index d304e0e..dc76a6a 100644 --- a/cartographer/pose_graph/constraint/relative_pose_constraint_2d.h +++ b/cartographer/pose_graph/constraint/relative_pose_constraint_2d.h @@ -26,6 +26,7 @@ namespace pose_graph { class RelativePoseConstraint2D : public Constraint { public: RelativePoseConstraint2D(const ConstraintId& id, + const proto::LossFunction& loss_function_proto, const proto::RelativePose2D& proto); void AddToOptimizer(Nodes* nodes, ceres::Problem* problem) const final; diff --git a/cartographer/pose_graph/optimizer/ceres_optimizer_test.cc b/cartographer/pose_graph/optimizer/ceres_optimizer_test.cc index b49a391..3c090e1 100644 --- a/cartographer/pose_graph/optimizer/ceres_optimizer_test.cc +++ b/cartographer/pose_graph/optimizer/ceres_optimizer_test.cc @@ -77,7 +77,8 @@ TEST(CeresOptimizerTest, SmokeTest) { NodeId{"end_node", common::FromUniversal(1)}, GetPose2D(ParseProto(kEndNode))); data.constraints.emplace_back(common::make_unique( - "constraint_1", ParseProto(kRelativePose2D))); + "constraint_1", ParseProto(R"(quadratic_loss: {})"), + ParseProto(kRelativePose2D))); CeresOptimizer optimizer(ceres::Solver::Options{}); EXPECT_EQ(optimizer.Solve(&data), Optimizer::SolverStatus::CONVERGENCE);