[GenericPoseGraph] Add RelativePoseConstraint2D. (#1297)

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

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 <string>
+
+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_

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 <typename MapType, typename KeyType = typename MapType::key_type,
+          typename ValueType = typename MapType::mapped_type>
+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<RelativePoseCost2D>(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

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<RelativePoseCost2D> ceres_cost_;
+};
+
+}  // namespace pose_graph
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_RELATIVE_POSE_CONSTRAINT_2D_H_

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<std::unique_ptr<Constraint>> constraints;
+};
+
+}  // namespace pose_graph
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_POSE_GRAPH_POSE_GRAPH_DATA_H_