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/mapping/trajectory_node.h" 00018 00019 #include "Eigen/Core" 00020 #include "cartographer/common/time.h" 00021 #include "cartographer/sensor/compressed_point_cloud.h" 00022 #include "cartographer/transform/transform.h" 00023 00024 namespace cartographer { 00025 namespace mapping { 00026 00027 proto::TrajectoryNodeData ToProto(const TrajectoryNode::Data& constant_data) { 00028 proto::TrajectoryNodeData proto; 00029 proto.set_timestamp(common::ToUniversal(constant_data.time)); 00030 *proto.mutable_gravity_alignment() = 00031 transform::ToProto(constant_data.gravity_alignment); 00032 *proto.mutable_filtered_gravity_aligned_point_cloud() = 00033 sensor::CompressedPointCloud( 00034 constant_data.filtered_gravity_aligned_point_cloud) 00035 .ToProto(); 00036 *proto.mutable_high_resolution_point_cloud() = 00037 sensor::CompressedPointCloud(constant_data.high_resolution_point_cloud) 00038 .ToProto(); 00039 *proto.mutable_low_resolution_point_cloud() = 00040 sensor::CompressedPointCloud(constant_data.low_resolution_point_cloud) 00041 .ToProto(); 00042 for (Eigen::VectorXf::Index i = 0; 00043 i != constant_data.rotational_scan_matcher_histogram.size(); ++i) { 00044 proto.add_rotational_scan_matcher_histogram( 00045 constant_data.rotational_scan_matcher_histogram(i)); 00046 } 00047 *proto.mutable_local_pose() = transform::ToProto(constant_data.local_pose); 00048 return proto; 00049 } 00050 00051 TrajectoryNode::Data FromProto(const proto::TrajectoryNodeData& proto) { 00052 Eigen::VectorXf rotational_scan_matcher_histogram( 00053 proto.rotational_scan_matcher_histogram_size()); 00054 for (int i = 0; i != proto.rotational_scan_matcher_histogram_size(); ++i) { 00055 rotational_scan_matcher_histogram(i) = 00056 proto.rotational_scan_matcher_histogram(i); 00057 } 00058 return TrajectoryNode::Data{ 00059 common::FromUniversal(proto.timestamp()), 00060 transform::ToEigen(proto.gravity_alignment()), 00061 sensor::CompressedPointCloud(proto.filtered_gravity_aligned_point_cloud()) 00062 .Decompress(), 00063 sensor::CompressedPointCloud(proto.high_resolution_point_cloud()) 00064 .Decompress(), 00065 sensor::CompressedPointCloud(proto.low_resolution_point_cloud()) 00066 .Decompress(), 00067 rotational_scan_matcher_histogram, 00068 transform::ToRigid3(proto.local_pose())}; 00069 } 00070 00071 } // namespace mapping 00072 } // namespace cartographer