cartographer: timed_point_cloud

Go to the documentation of this file.
00001 /*
00002  * Copyright 2017 The Cartographer Authors
00003  *
00004  * Licensed under the Apache License, Version 2.0 (the "License");
00005  * you may not use this file except in compliance with the License.
00006  * You may obtain a copy of the License at
00007  *
00008  *      http://www.apache.org/licenses/LICENSE-2.0
00009  *
00010  * Unless required by applicable law or agreed to in writing, software
00011  * distributed under the License is distributed on an "AS IS" BASIS,
00012  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00013  * See the License for the specific language governing permissions and
00014  * limitations under the License.
00015  */
00016 
00017 #include "cartographer/sensor/timed_point_cloud_data.h"
00018 
00019 #include "cartographer/transform/proto/transform.pb.h"
00020 #include "cartographer/transform/transform.h"
00021 
00022 namespace cartographer {
00023 namespace sensor {
00024 
00025 proto::TimedPointCloudData ToProto(
00026     const TimedPointCloudData& timed_point_cloud_data) {
00027   proto::TimedPointCloudData proto;
00028   proto.set_timestamp(common::ToUniversal(timed_point_cloud_data.time));
00029   *proto.mutable_origin() = transform::ToProto(timed_point_cloud_data.origin);
00030   proto.mutable_point_data()->Reserve(timed_point_cloud_data.ranges.size());
00031   for (const TimedRangefinderPoint& range : timed_point_cloud_data.ranges) {
00032     *proto.add_point_data() = ToProto(range);
00033   }
00034   return proto;
00035 }
00036 
00037 TimedPointCloudData FromProto(const proto::TimedPointCloudData& proto) {
00038   TimedPointCloud timed_point_cloud;
00039   if (proto.point_data().size() > 0) {
00040     timed_point_cloud.reserve(proto.point_data().size());
00041     for (const auto& timed_point_proto : proto.point_data()) {
00042       timed_point_cloud.push_back(FromProto(timed_point_proto));
00043     }
00044   } else {
00045     timed_point_cloud.reserve(proto.point_data_legacy().size());
00046     for (const auto& timed_point_proto : proto.point_data_legacy()) {
00047       const Eigen::Vector4f timed_point = transform::ToEigen(timed_point_proto);
00048       timed_point_cloud.push_back({timed_point.head<3>(), timed_point[3]});
00049     }
00050   }
00051   return TimedPointCloudData{common::FromUniversal(proto.timestamp()),
00052                              transform::ToEigen(proto.origin()),
00053                              timed_point_cloud};
00054 }
00055 
00056 }  // namespace sensor
00057 }  // namespace cartographer