cartographer算法TimestampedTransform结构及Interpolate函数

TimestampedTransform结构指的是带有时间戳的变换矩阵，结构体如下所示：

struct TimestampedTransform {
  common::Time time;
  transform::Rigid3d transform;
};

Interpolate函数的作用是计算两个TimestampedTransform之间某个时刻的TimestampedTransform，其中start指的是起始时刻的TimestampedTransform，end指的是终止时刻的TimestampedTransform，time指的是需要插值计算TimestampedTransform的时刻，如下所示：

TimestampedTransform Interpolate(const TimestampedTransform& start,
                                 const TimestampedTransform& end,
                                 const common::Time time) {
  CHECK_LE(start.time, time);//检查time是否大于等于start的时间
  CHECK_GE(end.time, time);//检查time是否小于等于start的时间

  const double duration = common::ToSeconds(end.time - start.time);//起止时间差
  const double factor = common::ToSeconds(time - start.time) / duration;//时间因子，用于后续计算
  const Eigen::Vector3d origin =
      start.transform.translation() +
      (end.transform.translation() - start.transform.translation()) * factor;//位置计算
  const Eigen::Quaterniond rotation =
      Eigen::Quaterniond(start.transform.rotation())
          .slerp(factor, Eigen::Quaterniond(end.transform.rotation()));//姿态计算
  return TimestampedTransform{time, transform::Rigid3d(origin, rotation)};//返回结果
}