[GenericPoseGraph] Add IMU Calibration and Pose3D node types. (#1278)

cartographer/pose_graph/node/imu_calibration.cc

@@ -0,0 +1,48 @@
+/*
+ * 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/node/imu_calibration.h"
+
+namespace cartographer {
+namespace pose_graph {
+
+ImuCalibration::ImuCalibration(const NodeId& node_id, bool constant,
+                               double gravity_constant,
+                               const Eigen::Quaterniond& orientation)
+    : Node(node_id, constant),
+      gravity_constant_(gravity_constant),
+      orientation_{{orientation.x(), orientation.y(), orientation.z(),
+                    orientation.w()}} {}
+
+proto::Parameters ImuCalibration::ToParametersProto() const {
+  proto::Parameters parameters;
+  auto* imu_calibration = parameters.mutable_imu_calibration();
+
+  imu_calibration->set_gravity_constant(gravity_constant_);
+
+  // TODO(pifon): Use a common method to convert from Eigen::Quaterniond to
+  // proto. Probably, the one defined in transform.h.
+  auto* orientation = imu_calibration->mutable_orientation();
+  orientation->set_x(orientation_[0]);
+  orientation->set_y(orientation_[1]);
+  orientation->set_z(orientation_[2]);
+  orientation->set_w(orientation_[3]);
+
+  return parameters;
+}
+
+}  // namespace pose_graph
+}  // namespace cartographer

cartographer/pose_graph/node/imu_calibration.h

@@ -0,0 +1,51 @@
+/*
+ * 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_NODE_IMU_CALIBRATION_H_
+#define CARTOGRAPHER_POSE_GRAPH_NODE_IMU_CALIBRATION_H_
+
+#include "cartographer/pose_graph/node/node.h"
+
+#include <array>
+
+#include "cartographer/transform/transform.h"
+
+namespace cartographer {
+namespace pose_graph {
+
+class ImuCalibration : public Node {
+ public:
+  ImuCalibration(const NodeId& node_id, bool constant, double gravity_constant,
+                 const Eigen::Quaterniond& orientation);
+
+  double* mutable_gravity_constant() { return &gravity_constant_; }
+  double gravity_constant() const { return gravity_constant_; }
+
+  std::array<double, 4>* mutable_orientation() { return &orientation_; }
+  const std::array<double, 4>& orientation() const { return orientation_; }
+
+ protected:
+  proto::Parameters ToParametersProto() const final;
+
+ private:
+  double gravity_constant_;
+  std::array<double, 4> orientation_;
+};
+
+}  // namespace pose_graph
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_POSE_GRAPH_NODE_IMU_CALIBRATION_H_

cartographer/pose_graph/node/imu_calibration_test.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/node/imu_calibration.h"
+
+#include "cartographer/pose_graph/internal/testing/test_helpers.h"
+
+namespace cartographer {
+namespace pose_graph {
+namespace {
+
+constexpr char kExpectedNode[] = R"PROTO(
+  id { object_id: "accelerometer" }
+  constant: true
+  parameters {
+    imu_calibration {
+      gravity_constant: 10
+      orientation: { w: 0 x: 1 y: 2 z: 3 }
+    }
+  }
+)PROTO";
+
+TEST(Pose3DTest, ToProto) {
+  ImuCalibration imu_calibration("accelerometer", true, 10,
+                                 Eigen::Quaterniond(0., 1., 2., 3.));
+  EXPECT_THAT(imu_calibration.ToProto(), testing::EqualsProto(kExpectedNode));
+}
+
+}  // namespace
+}  // namespace pose_graph
+}  // namespace cartographer

cartographer/pose_graph/node/node.cc

@@ -14,7 +14,7 @@
  * limitations under the License.
  */
 
-#include "cartographer/pose_graph/node.h"
+#include "cartographer/pose_graph/node/node.h"
 
 namespace cartographer {
 namespace pose_graph {

cartographer/pose_graph/node/node.h

@@ -14,8 +14,8 @@
  * limitations under the License.
  */
 
-#ifndef CARTOGRAPHER_POSE_GRAPH_NODE_H_
+#ifndef CARTOGRAPHER_POSE_GRAPH_NODE_NODE_H_
-#define CARTOGRAPHER_POSE_GRAPH_NODE_H_
+#define CARTOGRAPHER_POSE_GRAPH_NODE_NODE_H_
 
 #include "cartographer/pose_graph/proto/node.pb.h"
 
@@ -32,20 +32,6 @@ using NodeId = std::string;
 
 class Node {
  public:
-  enum class Parameterization {
-    NONE,
-    YAW_ONLY,
-    CONSTANT_YAW,
-  };
-
-  struct ParameterBlock {
-    // Non-owning pointer to values corresponding to a single parameter block.
-    double* const values;
-    // Size of the parameter block.
-    const size_t size;
-    const Parameterization parameterization;
-  };
-
   explicit Node(NodeId id, bool constant) : node_id_(id), constant_(constant) {}
   ~Node() = default;
 
@@ -60,25 +46,15 @@ class Node {
   bool constant() const { return constant_; }
   void set_constant(bool constant) { constant_ = constant; }
 
-  std::vector<ParameterBlock>& parameter_blocks() { return parameter_blocks_; }
-
  protected:
   virtual proto::Parameters ToParametersProto() const = 0;
 
-  template <std::size_t ArraySize>
-  void AddParameterBlock(Parameterization parameterization,
-                         std::array<double, ArraySize>* values) {
-    parameter_blocks_.emplace_back(
-        ParameterBlock{values->data(), values->size(), parameterization});
-  }
-
  private:
   NodeId node_id_;
   bool constant_;
-  std::vector<ParameterBlock> parameter_blocks_;
 };
 
 }  // namespace pose_graph
 }  // namespace cartographer
 
-#endif  // CARTOGRAPHER_POSE_GRAPH_NODE_H_
+#endif  // CARTOGRAPHER_POSE_GRAPH_NODE_NODE_H_

cartographer/pose_graph/node/nodes.h

@@ -0,0 +1,39 @@
+/*
+ * 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_NODE_NODES_H_
+#define CARTOGRAPHER_POSE_GRAPH_NODE_NODES_H_
+
+#include "cartographer/pose_graph/node/imu_calibration.h"
+#include "cartographer/pose_graph/node/pose_2d.h"
+#include "cartographer/pose_graph/node/pose_3d.h"
+
+#include <map>
+
+namespace cartographer {
+namespace pose_graph {
+
+struct Nodes {
+  // TODO(pifon): Should it really be an std::map or smth else?
+  std::map<NodeId, Pose2D> pose_2d_nodes;
+  std::map<NodeId, Pose3D> pose_3d_nodes;
+  std::map<NodeId, Pose3D> imu_calibration_nodes;
+};
+
+}  // namespace pose_graph
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_POSE_GRAPH_NODE_NODES_H_

cartographer/pose_graph/node/pose_2d.cc

@@ -14,7 +14,7 @@
  * limitations under the License.
  */
 
-#include "cartographer/pose_graph/pose_2d.h"
+#include "cartographer/pose_graph/node/pose_2d.h"
 
 namespace cartographer {
 namespace pose_graph {
@@ -29,9 +29,7 @@ constexpr size_t kRotationIndex = 2;
 Pose2D::Pose2D(const NodeId& node_id, bool constant,
                const Eigen::Vector2d& translation, double rotation)
     : Node(node_id, constant),
-      pose_2d_{{translation.x(), translation.y(), rotation}} {
+      pose_2d_{{translation.x(), translation.y(), rotation}} {}
-  AddParameterBlock(Parameterization::NONE, &pose_2d_);
-}
 
 proto::Parameters Pose2D::ToParametersProto() const {
   proto::Parameters parameters;

cartographer/pose_graph/node/pose_2d.h

@@ -14,12 +14,13 @@
  * limitations under the License.
  */
 
-#ifndef CARTOGRAPHER_POSE_GRAPH_POSE_2D_H_
+#ifndef CARTOGRAPHER_POSE_GRAPH_NODE_POSE_2D_H_
-#define CARTOGRAPHER_POSE_GRAPH_POSE_2D_H_
+#define CARTOGRAPHER_POSE_GRAPH_NODE_POSE_2D_H_
 
-#include "cartographer/pose_graph/node.h"
+#include "cartographer/pose_graph/node/node.h"
 
 #include <array>
+
 #include "Eigen/Core"
 
 namespace cartographer {
@@ -30,6 +31,9 @@ class Pose2D : public Node {
   Pose2D(const NodeId& node_id, bool constant,
          const Eigen::Vector2d& translation, double rotation);
 
+  std::array<double, 3>* mutable_pose_2d() { return &pose_2d_; }
+  const std::array<double, 3>& pose_2d() const { return pose_2d_; }
+
  protected:
   proto::Parameters ToParametersProto() const final;
 
@@ -40,4 +44,4 @@ class Pose2D : public Node {
 }  // namespace pose_graph
 }  // namespace cartographer
 
-#endif  // CARTOGRAPHER_POSE_GRAPH_POSE_2D_H_
+#endif  // CARTOGRAPHER_POSE_GRAPH_NODE_POSE_2D_H_

cartographer/pose_graph/node/pose_2d_test.cc

@@ -14,7 +14,7 @@
  * limitations under the License.
  */
 
-#include "cartographer/pose_graph/pose_2d.h"
+#include "cartographer/pose_graph/node/pose_2d.h"
 
 #include "cartographer/pose_graph/internal/testing/test_helpers.h"
 

cartographer/pose_graph/node/pose_3d.cc

@@ -0,0 +1,48 @@
+/*
+ * 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/node/pose_3d.h"
+
+namespace cartographer {
+namespace pose_graph {
+
+Pose3D::Pose3D(const NodeId& node_id, bool constant,
+               const Eigen::Vector3d& translation,
+               const Eigen::Quaterniond& rotation)
+    : Node(node_id, constant),
+      translation_{{translation.x(), translation.y(), translation.z()}},
+      rotation_{{rotation.x(), rotation.y(), rotation.z(), rotation.w()}} {}
+
+proto::Parameters Pose3D::ToParametersProto() const {
+  proto::Parameters parameters;
+  auto* pose_3d = parameters.mutable_pose_3d();
+
+  auto* translation = pose_3d->mutable_translation();
+  translation->set_x(translation_[0]);
+  translation->set_y(translation_[1]);
+  translation->set_z(translation_[2]);
+
+  auto* rotation = pose_3d->mutable_rotation();
+  rotation->set_x(rotation_[0]);
+  rotation->set_y(rotation_[1]);
+  rotation->set_z(rotation_[2]);
+  rotation->set_w(rotation_[3]);
+
+  return parameters;
+}
+
+}  // namespace pose_graph
+}  // namespace cartographer

cartographer/pose_graph/node/pose_3d.h

@@ -0,0 +1,52 @@
+/*
+ * 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_NODE_POSE_3D_H_
+#define CARTOGRAPHER_POSE_GRAPH_NODE_POSE_3D_H_
+
+#include "cartographer/pose_graph/node/node.h"
+
+#include <array>
+
+#include "cartographer/transform/transform.h"
+
+namespace cartographer {
+namespace pose_graph {
+
+class Pose3D : public Node {
+ public:
+  Pose3D(const NodeId& node_id, bool constant,
+         const Eigen::Vector3d& translation,
+         const Eigen::Quaterniond& rotation);
+
+  std::array<double, 3>* mutable_translation() { return &translation_; }
+  const std::array<double, 3>& translation() const { return translation_; }
+
+  std::array<double, 4>* mutable_rotation() { return &rotation_; }
+  const std::array<double, 4>& rotation() const { return rotation_; }
+
+ protected:
+  proto::Parameters ToParametersProto() const final;
+
+ private:
+  std::array<double, 3> translation_;
+  std::array<double, 4> rotation_;
+};
+
+}  // namespace pose_graph
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_POSE_GRAPH_NODE_POSE_3D_H_

cartographer/pose_graph/node/pose_3d_test.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/node/pose_3d.h"
+
+#include "cartographer/pose_graph/internal/testing/test_helpers.h"
+
+namespace cartographer {
+namespace pose_graph {
+namespace {
+
+constexpr char kExpectedNode[] = R"PROTO(
+  id { object_id: "bumpy_world" }
+  constant: true
+  parameters {
+    pose_3d {
+      translation { x: 1 y: 2 z: 3 }
+      rotation: { w: 0 x: 1 y: 2 z: 3 }
+    }
+  }
+)PROTO";
+
+TEST(Pose3DTest, ToProto) {
+  Pose3D pose_3d("bumpy_world", true, Eigen::Vector3d(1., 2., 3.),
+                 Eigen::Quaterniond(0., 1., 2., 3.));
+  EXPECT_THAT(pose_3d.ToProto(), testing::EqualsProto(kExpectedNode));
+}
+
+}  // namespace
+}  // namespace pose_graph
+}  // namespace cartographer

cartographer/pose_graph/proto/node.proto

@@ -38,7 +38,7 @@ message Pose3D {
 
 message ImuCalibration {
   double gravity_constant = 1;
-  transform.proto.Quaterniond imu_calibration = 2;
+  transform.proto.Quaterniond orientation = 2;
   Parameterization rotation_parameterization = 3;
 }