Introduce TimedPointCloudData. (#748)

This data structure is needed to forward RangeFinderData over gRPC.

[RFC=0002](https://github.com/googlecartographer/rfcs/blob/master/text/0002-cloud-based-mapping-1.md)

cartographer/sensor/proto/sensor.proto

@@ -26,6 +26,13 @@ message CompressedPointCloud {
   repeated int32 point_data = 3;
 }
 
+// Proto representation of ::cartographer::sensor::TimedPointCloudData.
+message TimedPointCloudData {
+  int64 timestamp = 1;
+  transform.proto.Vector3f origin = 2;
+  repeated transform.proto.Vector4f point_data = 3;
+}
+
 // Proto representation of ::cartographer::sensor::ImuData.
 message ImuData {
   int64 timestamp = 1;

cartographer/sensor/timed_point_cloud_data.cc

@@ -0,0 +1,51 @@
+/*
+ * 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/timed_point_cloud_data.h"
+
+#include "cartographer/transform/proto/transform.pb.h"
+#include "cartographer/transform/transform.h"
+
+namespace cartographer {
+namespace sensor {
+
+proto::TimedPointCloudData ToProto(
+    const TimedPointCloudData& timed_point_cloud_data) {
+  proto::TimedPointCloudData proto;
+  proto.set_timestamp(common::ToUniversal(timed_point_cloud_data.time));
+  *proto.mutable_origin() = transform::ToProto(timed_point_cloud_data.origin);
+  proto.mutable_point_data()->Reserve(timed_point_cloud_data.ranges.size());
+  for (const auto& range : timed_point_cloud_data.ranges) {
+    *proto.add_point_data() = transform::ToProto(range);
+  }
+  return proto;
+}
+
+TimedPointCloudData FromProto(const proto::TimedPointCloudData& proto) {
+  TimedPointCloud timed_point_cloud;
+  timed_point_cloud.reserve(proto.point_data().size());
+  std::transform(
+      proto.point_data().begin(), proto.point_data().end(),
+      std::back_inserter(timed_point_cloud),
+      static_cast<Eigen::Vector4f (*)(const transform::proto::Vector4f&)>(
+          transform::ToEigen));
+  return TimedPointCloudData{common::FromUniversal(proto.timestamp()),
+                             transform::ToEigen(proto.origin()),
+                             timed_point_cloud};
+}
+
+}  // namespace sensor
+}  // namespace cartographer

cartographer/sensor/timed_point_cloud_data.h

@@ -30,6 +30,13 @@ struct TimedPointCloudData {
   sensor::TimedPointCloud ranges;
 };
 
+// Converts 'timed_point_cloud_data' to a proto::TimedPointCloudData.
+proto::TimedPointCloudData ToProto(
+    const TimedPointCloudData& timed_point_cloud_data);
+
+// Converts 'proto' to TimedPointCloudData.
+TimedPointCloudData FromProto(const proto::TimedPointCloudData& proto);
+
 }  // namespace sensor
 }  // namespace cartographer
 

cartographer/transform/proto/transform.proto

@@ -42,6 +42,13 @@ message Vector3f {
   float z = 3;
 }
 
+message Vector4f {
+  float x = 1;
+  float y = 2;
+  float z = 3;
+  float t = 4;
+}
+
 message Quaterniond {
   double x = 1;
   double y = 2;

cartographer/transform/transform.cc

@@ -32,6 +32,10 @@ Eigen::Vector3f ToEigen(const proto::Vector3f& vector) {
   return Eigen::Vector3f(vector.x(), vector.y(), vector.z());
 }
 
+Eigen::Vector4f ToEigen(const proto::Vector4f& vector) {
+  return Eigen::Vector4f(vector.x(), vector.y(), vector.z(), vector.t());
+}
+
 Eigen::Vector3d ToEigen(const proto::Vector3d& vector) {
   return Eigen::Vector3d(vector.x(), vector.y(), vector.z());
 }
@@ -91,6 +95,15 @@ proto::Vector3f ToProto(const Eigen::Vector3f& vector) {
   return proto;
 }
 
+proto::Vector4f ToProto(const Eigen::Vector4f& vector) {
+  proto::Vector4f proto;
+  proto.set_x(vector.x());
+  proto.set_y(vector.y());
+  proto.set_z(vector.z());
+  proto.set_t(vector.w());
+  return proto;
+}
+
 proto::Vector3d ToProto(const Eigen::Vector3d& vector) {
   proto::Vector3d proto;
   proto.set_x(vector.x());

cartographer/transform/transform.h

@@ -118,6 +118,7 @@ Rigid3<T> Embed3D(const Rigid2<T>& transform) {
 Rigid2d ToRigid2(const proto::Rigid2d& transform);
 Eigen::Vector2d ToEigen(const proto::Vector2d& vector);
 Eigen::Vector3f ToEigen(const proto::Vector3f& vector);
+Eigen::Vector4f ToEigen(const proto::Vector4f& vector);
 Eigen::Vector3d ToEigen(const proto::Vector3d& vector);
 Eigen::Quaterniond ToEigen(const proto::Quaterniond& quaternion);
 proto::Rigid2d ToProto(const Rigid2d& transform);
@@ -127,6 +128,7 @@ Rigid3d ToRigid3(const proto::Rigid3d& rigid);
 proto::Rigid3f ToProto(const Rigid3f& rigid);
 proto::Vector2d ToProto(const Eigen::Vector2d& vector);
 proto::Vector3f ToProto(const Eigen::Vector3f& vector);
+proto::Vector4f ToProto(const Eigen::Vector4f& vector);
 proto::Vector3d ToProto(const Eigen::Vector3d& vector);
 proto::Quaternionf ToProto(const Eigen::Quaternionf& quaternion);
 proto::Quaterniond ToProto(const Eigen::Quaterniond& quaternion);

cartographer_grpc/proto/map_builder_service.proto

@@ -47,7 +47,7 @@ message AddImuDataRequest {
 
 message AddRangefinderDataRequest {
   SensorMetadata sensor_metadata = 1;
-  cartographer.sensor.proto.CompressedPointCloud rangefinder_data = 2;
+  cartographer.sensor.proto.TimedPointCloudData timed_point_cloud_data = 2;
 }
 
 message FinishTrajectoryRequest {