Function PrintTransform

common/math.h:188–196

template <typename T>
void PrintTransform(const Eigen::Rigid3::Transform<T>& t) {
  Eigen::Vector6<T> vec_6d = TransformToVector6(t);
  const double factor = 180. / M_PI;
  std::cout << "  translation : (" << vec_6d[0] << ", " << vec_6d[1] << ", "
            << vec_6d[2] << ")\n"
            << "  rotation    : (" << vec_6d[3] * factor << ", "
            << vec_6d[4] * factor << ", " << vec_6d[5] * factor << ")"
            << std::endl;
}

template <typename T>
Eigen::Matrix<T, 4, 4> InterpolateTransform(const Eigen::Matrix<T, 4, 4>& t1,