Move ImuData to sensor, add ImuData proto. (#381)

cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc

@@ -68,7 +68,7 @@ void OptimizationProblem::AddImuData(const int trajectory_id,
   imu_data_.resize(
       std::max(imu_data_.size(), static_cast<size_t>(trajectory_id) + 1));
   imu_data_[trajectory_id].push_back(
-      mapping_3d::ImuData{time, linear_acceleration, angular_velocity});
+      sensor::ImuData{time, linear_acceleration, angular_velocity});
 }
 
 void OptimizationProblem::AddTrajectoryNode(

cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h

@@ -28,7 +28,7 @@
 #include "cartographer/common/time.h"
 #include "cartographer/mapping/sparse_pose_graph.h"
 #include "cartographer/mapping/sparse_pose_graph/proto/optimization_problem_options.pb.h"
-#include "cartographer/mapping_3d/imu_integration.h"
+#include "cartographer/sensor/imu_data.h"
 
 namespace cartographer {
 namespace mapping_2d {
@@ -86,7 +86,7 @@ class OptimizationProblem {
     int num_trimmed_submaps = 0;
   };
   mapping::sparse_pose_graph::proto::OptimizationProblemOptions options_;
-  std::vector<std::deque<mapping_3d::ImuData>> imu_data_;
+  std::vector<std::deque<sensor::ImuData>> imu_data_;
   std::vector<std::deque<NodeData>> node_data_;
   std::vector<std::deque<SubmapData>> submap_data_;
   std::vector<TrajectoryData> trajectory_data_;

cartographer/mapping_3d/imu_integration.cc

@@ -20,8 +20,9 @@ namespace cartographer {
 namespace mapping_3d {
 
 IntegrateImuResult<double> IntegrateImu(
-    const std::deque<ImuData>& imu_data, const common::Time start_time,
+    const std::deque<sensor::ImuData>& imu_data, const common::Time start_time,
-    const common::Time end_time, std::deque<ImuData>::const_iterator* it) {
+    const common::Time end_time,
+    std::deque<sensor::ImuData>::const_iterator* it) {
   return IntegrateImu<double>(imu_data, Eigen::Affine3d::Identity(),
                               Eigen::Affine3d::Identity(), start_time, end_time,
                               it);

cartographer/mapping_3d/imu_integration.h

@@ -23,18 +23,13 @@
 #include "Eigen/Core"
 #include "Eigen/Geometry"
 #include "cartographer/common/time.h"
+#include "cartographer/sensor/imu_data.h"
 #include "cartographer/transform/transform.h"
 #include "glog/logging.h"
 
 namespace cartographer {
 namespace mapping_3d {
 
-struct ImuData {
-  common::Time time;
-  Eigen::Vector3d linear_acceleration;
-  Eigen::Vector3d angular_velocity;
-};
-
 template <typename T>
 struct IntegrateImuResult {
   Eigen::Matrix<T, 3, 1> delta_velocity;
@@ -43,17 +38,18 @@ struct IntegrateImuResult {
 
 // Returns velocity delta in map frame.
 IntegrateImuResult<double> IntegrateImu(
-    const std::deque<ImuData>& imu_data, const common::Time start_time,
+    const std::deque<sensor::ImuData>& imu_data, const common::Time start_time,
-    const common::Time end_time, std::deque<ImuData>::const_iterator* it);
+    const common::Time end_time,
+    std::deque<sensor::ImuData>::const_iterator* it);
 
 template <typename T>
 IntegrateImuResult<T> IntegrateImu(
-    const std::deque<ImuData>& imu_data,
+    const std::deque<sensor::ImuData>& imu_data,
     const Eigen::Transform<T, 3, Eigen::Affine>&
         linear_acceleration_calibration,
     const Eigen::Transform<T, 3, Eigen::Affine>& angular_velocity_calibration,
     const common::Time start_time, const common::Time end_time,
-    std::deque<ImuData>::const_iterator* it) {
+    std::deque<sensor::ImuData>::const_iterator* it) {
   CHECK_LE(start_time, end_time);
   CHECK(*it != imu_data.cend());
   CHECK_LE((*it)->time, start_time);

cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc

@@ -31,6 +31,7 @@
 #include "cartographer/common/time.h"
 #include "cartographer/mapping_3d/acceleration_cost_function.h"
 #include "cartographer/mapping_3d/ceres_pose.h"
+#include "cartographer/mapping_3d/imu_integration.h"
 #include "cartographer/mapping_3d/rotation_cost_function.h"
 #include "cartographer/mapping_3d/sparse_pose_graph/spa_cost_function.h"
 #include "cartographer/transform/transform.h"
@@ -86,7 +87,7 @@ void OptimizationProblem::AddImuData(const int trajectory_id,
   imu_data_.resize(
       std::max(imu_data_.size(), static_cast<size_t>(trajectory_id) + 1));
   imu_data_[trajectory_id].push_back(
-      ImuData{time, linear_acceleration, angular_velocity});
+      sensor::ImuData{time, linear_acceleration, angular_velocity});
 }
 
 void OptimizationProblem::AddTrajectoryNode(
@@ -197,7 +198,7 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints) {
     TrajectoryData& trajectory_data = trajectory_data_.at(trajectory_id);
     problem.AddParameterBlock(trajectory_data.imu_calibration.data(), 4,
                               new ceres::QuaternionParameterization());
-    const std::deque<ImuData>& imu_data = imu_data_.at(trajectory_id);
+    const std::deque<sensor::ImuData>& imu_data = imu_data_.at(trajectory_id);
     CHECK(!imu_data.empty());
     // TODO(whess): Add support for empty trajectories.
     const auto& node_data = node_data_[trajectory_id];

cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h

@@ -28,7 +28,7 @@
 #include "cartographer/common/time.h"
 #include "cartographer/mapping/sparse_pose_graph.h"
 #include "cartographer/mapping/sparse_pose_graph/proto/optimization_problem_options.pb.h"
-#include "cartographer/mapping_3d/imu_integration.h"
+#include "cartographer/sensor/imu_data.h"
 
 namespace cartographer {
 namespace mapping_3d {
@@ -82,7 +82,7 @@ class OptimizationProblem {
 
   mapping::sparse_pose_graph::proto::OptimizationProblemOptions options_;
   FixZ fix_z_;
-  std::vector<std::deque<ImuData>> imu_data_;
+  std::vector<std::deque<sensor::ImuData>> imu_data_;
   std::vector<std::vector<NodeData>> node_data_;
   std::vector<std::vector<SubmapData>> submap_data_;
   std::vector<TrajectoryData> trajectory_data_;

cartographer/sensor/imu_data.cc

@@ -0,0 +1,41 @@
+/*
+ * Copyright 2017 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/sensor/imu_data.h"
+
+#include "cartographer/transform/transform.h"
+
+namespace cartographer {
+namespace sensor {
+
+proto::ImuData ToProto(const ImuData& imu_data) {
+  proto::ImuData proto;
+  proto.set_timestamp(common::ToUniversal(imu_data.time));
+  *proto.mutable_linear_acceleration() =
+      transform::ToProto(imu_data.linear_acceleration);
+  *proto.mutable_angular_velocity() =
+      transform::ToProto(imu_data.angular_velocity);
+  return proto;
+}
+
+ImuData FromProto(const proto::ImuData& proto) {
+  return ImuData{common::FromUniversal(proto.timestamp()),
+                 transform::ToEigen(proto.linear_acceleration()),
+                 transform::ToEigen(proto.angular_velocity())};
+}
+
+}  // sensor
+}  // cartographer

cartographer/sensor/imu_data.h

@@ -0,0 +1,42 @@
+/*
+ * Copyright 2017 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_SENSOR_IMU_DATA_H_
+#define CARTOGRAPHER_SENSOR_IMU_DATA_H_
+
+#include "Eigen/Core"
+#include "cartographer/common/time.h"
+#include "cartographer/sensor/proto/sensor.pb.h"
+
+namespace cartographer {
+namespace sensor {
+
+struct ImuData {
+  common::Time time;
+  Eigen::Vector3d linear_acceleration;
+  Eigen::Vector3d angular_velocity;
+};
+
+// Converts 'imu_data' to a proto::ImuData.
+proto::ImuData ToProto(const ImuData& imu_data);
+
+// Converts 'proto' to an ImuData.
+ImuData FromProto(const proto::ImuData& proto);
+
+}  // namespace sensor
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_SENSOR_IMU_DATA_H_

cartographer/sensor/proto/sensor.proto

@@ -40,3 +40,10 @@ message RangeData {
   optional PointCloud point_cloud = 2;
   optional PointCloud missing_echo_point_cloud = 3;
 }
+
+// Proto representation of ::cartographer::sensor::ImuData
+message ImuData {
+  optional int64 timestamp = 1;
+  optional transform.proto.Vector3d linear_acceleration = 2;
+  optional transform.proto.Vector3d angular_velocity = 3;
+}