From df337194e21f98f8c7b0b88dab33f878066d4b56 Mon Sep 17 00:00:00 2001 From: Alexander Belyaev <32522095+pifon2a@users.noreply.github.com> Date: Thu, 19 Jul 2018 09:17:22 +0200 Subject: [PATCH] [GenericPoseGraph] Add RelativePoseConstraint2D. (#1297) --- .../pose_graph/constraint/constraint.cc | 30 +++++++ .../pose_graph/constraint/constraint.h | 55 ++++++++++++ .../constraint/relative_pose_constraint_2d.cc | 88 +++++++++++++++++++ .../constraint/relative_pose_constraint_2d.h | 45 ++++++++++ cartographer/pose_graph/pose_graph_data.h | 34 +++++++ 5 files changed, 252 insertions(+) create mode 100644 cartographer/pose_graph/constraint/constraint.cc create mode 100644 cartographer/pose_graph/constraint/constraint.h create mode 100644 cartographer/pose_graph/constraint/relative_pose_constraint_2d.cc create mode 100644 cartographer/pose_graph/constraint/relative_pose_constraint_2d.h create mode 100644 cartographer/pose_graph/pose_graph_data.h diff --git a/cartographer/pose_graph/constraint/constraint.cc b/cartographer/pose_graph/constraint/constraint.cc new file mode 100644 index 0000000..b40dc0f --- /dev/null +++ b/cartographer/pose_graph/constraint/constraint.cc @@ -0,0 +1,30 @@ +/* + * 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/constraint.h" + +namespace cartographer { +namespace pose_graph { + +proto::Constraint Constraint::ToProto() const { + proto::Constraint constraint; + constraint.set_id(constraint_id_); + *constraint.mutable_cost_function() = ToCostFunctionProto(); + return constraint; +} + +} // namespace pose_graph +} // namespace cartographer diff --git a/cartographer/pose_graph/constraint/constraint.h b/cartographer/pose_graph/constraint/constraint.h new file mode 100644 index 0000000..cce2c61 --- /dev/null +++ b/cartographer/pose_graph/constraint/constraint.h @@ -0,0 +1,55 @@ +/* + * 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_CONSTRAINT_H_ +#define CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_CONSTRAINT_H_ + +#include "cartographer/pose_graph/node/nodes.h" +#include "cartographer/pose_graph/proto/constraint.pb.h" +#include "ceres/problem.h" + +#include + +namespace cartographer { +namespace pose_graph { + +using ConstraintId = std::string; + +class Constraint { + public: + explicit Constraint(const ConstraintId& id) : constraint_id_(id) {} + virtual ~Constraint() = default; + + Constraint(const Constraint&) = delete; + Constraint& operator=(const Constraint&) = delete; + + proto::Constraint ToProto() const; + + const ConstraintId& constraint_id() const { return constraint_id_; } + + virtual void AddToOptimizer(Nodes* nodes, ceres::Problem* problem) const = 0; + + protected: + virtual proto::CostFunction ToCostFunctionProto() const = 0; + + private: + ConstraintId constraint_id_; +}; + +} // namespace pose_graph +} // namespace cartographer + +#endif // CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_CONSTRAINT_H_ diff --git a/cartographer/pose_graph/constraint/relative_pose_constraint_2d.cc b/cartographer/pose_graph/constraint/relative_pose_constraint_2d.cc new file mode 100644 index 0000000..2634e77 --- /dev/null +++ b/cartographer/pose_graph/constraint/relative_pose_constraint_2d.cc @@ -0,0 +1,88 @@ +/* + * 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/relative_pose_constraint_2d.h" + +#include "cartographer/common/make_unique.h" + +namespace cartographer { +namespace pose_graph { +namespace { + +// TODO(pifon): Move to common/utils.h. +template +ValueType* FindOrNull(MapType& map, const KeyType& key) { + auto it = map.find(key); + if (it == map.end()) return nullptr; + return &(it->second); +} + +} // namespace + +RelativePoseConstraint2D::RelativePoseConstraint2D( + const ConstraintId& id, const proto::RelativePose2D& proto) + : Constraint(id), + first_(proto.first()), + second_(proto.second()), + ceres_cost_(common::make_unique(proto.parameters())) { +} + +void RelativePoseConstraint2D::AddToOptimizer(Nodes* nodes, + ceres::Problem* problem) const { + auto first_node = FindOrNull(nodes->pose_2d_nodes, first_); + if (first_node == nullptr) { + LOG(INFO) << "First node was not found in pose_2d_nodes."; + return; + } + + auto second_node = FindOrNull(nodes->pose_2d_nodes, second_); + if (second_node == nullptr) { + LOG(INFO) << "Second node was not found in pose_2d_nodes."; + return; + } + + if (first_node->constant() && second_node->constant()) { + LOG(INFO) << "Both nodes are constant, skipping the constraint."; + return; + } + + auto first_pose = first_node->mutable_pose_2d(); + problem->AddParameterBlock(first_pose->data(), first_pose->size()); + if (first_node->constant()) { + problem->SetParameterBlockConstant(first_pose->data()); + } + + auto second_pose = second_node->mutable_pose_2d(); + problem->AddParameterBlock(second_pose->data(), second_pose->size()); + if (second_node->constant()) { + problem->SetParameterBlockConstant(second_pose->data()); + } + problem->AddResidualBlock(ceres_cost_.get(), nullptr /* loss function */, + first_pose->data(), second_pose->data()); +} + +proto::CostFunction RelativePoseConstraint2D::ToCostFunctionProto() const { + proto::CostFunction cost_function; + auto* relative_pose_2d = cost_function.mutable_relative_pose_2d(); + *relative_pose_2d->mutable_first() = first_.ToProto(); + *relative_pose_2d->mutable_second() = second_.ToProto(); + *relative_pose_2d->mutable_parameters() = ceres_cost_->ToProto(); + return cost_function; +} + +} // namespace pose_graph +} // namespace cartographer diff --git a/cartographer/pose_graph/constraint/relative_pose_constraint_2d.h b/cartographer/pose_graph/constraint/relative_pose_constraint_2d.h new file mode 100644 index 0000000..d304e0e --- /dev/null +++ b/cartographer/pose_graph/constraint/relative_pose_constraint_2d.h @@ -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. + */ + +#ifndef CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_RELATIVE_POSE_CONSTRAINT_2D_H_ +#define CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_RELATIVE_POSE_CONSTRAINT_2D_H_ + +#include "cartographer/pose_graph/constraint/constraint.h" +#include "cartographer/pose_graph/constraint/cost_function/relative_pose_cost_2d.h" + +namespace cartographer { +namespace pose_graph { + +class RelativePoseConstraint2D : public Constraint { + public: + RelativePoseConstraint2D(const ConstraintId& id, + const proto::RelativePose2D& proto); + + void AddToOptimizer(Nodes* nodes, ceres::Problem* problem) const final; + + protected: + proto::CostFunction ToCostFunctionProto() const final; + + private: + NodeId first_; + NodeId second_; + std::unique_ptr ceres_cost_; +}; + +} // namespace pose_graph +} // namespace cartographer + +#endif // CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_RELATIVE_POSE_CONSTRAINT_2D_H_ diff --git a/cartographer/pose_graph/pose_graph_data.h b/cartographer/pose_graph/pose_graph_data.h new file mode 100644 index 0000000..750c2d3 --- /dev/null +++ b/cartographer/pose_graph/pose_graph_data.h @@ -0,0 +1,34 @@ +/* + * 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_POSE_GRAPH_DATA_H_ +#define CARTOGRAPHER_POSE_GRAPH_POSE_GRAPH_DATA_H_ + +#include "cartographer/pose_graph/constraint/constraint.h" +#include "cartographer/pose_graph/nodes/nodes.h" + +namespace cartographer { +namespace pose_graph { + +struct PoseGraphData { + Nodes nodes; + std::vector> constraints; +}; + +} // namespace pose_graph +} // namespace cartographer + +#endif // CARTOGRAPHER_POSE_GRAPH_POSE_GRAPH_DATA_H_