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/transform/timestamped_transform.h" 00018 00019 #include "cartographer/transform/transform.h" 00020 00021 namespace cartographer { 00022 namespace transform { 00023 00024 TimestampedTransform Interpolate(const TimestampedTransform& start, 00025 const TimestampedTransform& end, 00026 const common::Time time) { 00027 CHECK_LE(start.time, time); 00028 CHECK_GE(end.time, time); 00029 00030 const double duration = common::ToSeconds(end.time - start.time); 00031 const double factor = common::ToSeconds(time - start.time) / duration; 00032 const Eigen::Vector3d origin = 00033 start.transform.translation() + 00034 (end.transform.translation() - start.transform.translation()) * factor; 00035 const Eigen::Quaterniond rotation = 00036 Eigen::Quaterniond(start.transform.rotation()) 00037 .slerp(factor, Eigen::Quaterniond(end.transform.rotation())); 00038 return TimestampedTransform{time, transform::Rigid3d(origin, rotation)}; 00039 } 00040 00041 TimestampedTransform FromProto(const proto::TimestampedTransform& proto) { 00042 return TimestampedTransform{common::FromUniversal(proto.time()), 00043 ToRigid3(proto.transform())}; 00044 } 00045 00046 proto::TimestampedTransform ToProto(const TimestampedTransform& transform) { 00047 proto::TimestampedTransform proto; 00048 proto.set_time(common::ToUniversal(transform.time)); 00049 *proto.mutable_transform() = ToProto(transform.transform); 00050 return proto; 00051 } 00052 00053 } // namespace transform 00054 } // namespace cartographer