diff --git a/cartographer/sensor/proto/sensor.proto b/cartographer/sensor/proto/sensor.proto index fac666a..7dcb5c3 100644 --- a/cartographer/sensor/proto/sensor.proto +++ b/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; diff --git a/cartographer/sensor/timed_point_cloud_data.cc b/cartographer/sensor/timed_point_cloud_data.cc new file mode 100644 index 0000000..afa9e7e --- /dev/null +++ b/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( + transform::ToEigen)); + return TimedPointCloudData{common::FromUniversal(proto.timestamp()), + transform::ToEigen(proto.origin()), + timed_point_cloud}; +} + +} // namespace sensor +} // namespace cartographer diff --git a/cartographer/sensor/timed_point_cloud_data.h b/cartographer/sensor/timed_point_cloud_data.h index dc34fda..6ad5836 100644 --- a/cartographer/sensor/timed_point_cloud_data.h +++ b/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 diff --git a/cartographer/transform/proto/transform.proto b/cartographer/transform/proto/transform.proto index ba245ca..82ed9a6 100644 --- a/cartographer/transform/proto/transform.proto +++ b/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; diff --git a/cartographer/transform/transform.cc b/cartographer/transform/transform.cc index 722f460..851e646 100644 --- a/cartographer/transform/transform.cc +++ b/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()); diff --git a/cartographer/transform/transform.h b/cartographer/transform/transform.h index 5fd3024..2fd9b05 100644 --- a/cartographer/transform/transform.h +++ b/cartographer/transform/transform.h @@ -118,6 +118,7 @@ Rigid3 Embed3D(const Rigid2& 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); diff --git a/cartographer_grpc/proto/map_builder_service.proto b/cartographer_grpc/proto/map_builder_service.proto index 162dce5..b78ddd6 100644 --- a/cartographer_grpc/proto/map_builder_service.proto +++ b/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 {