通过Eigen计算直线在不同坐标系统的转换

最新推荐文章于 2024-08-10 13:06:40 发布

小弧光

最新推荐文章于 2024-08-10 13:06:40 发布

阅读量1k

点赞数

分类专栏：数学算法文章标签： Eigen 坐标系

本文链接：https://blog.csdn.net/itfanr/article/details/83056288

版权

//
template <typename Scalar> bool
pcl::transformBetween2CoordinateSystems (const Eigen::Matrix<Scalar, Eigen::Dynamic, 1> from_line_x,
                                         const Eigen::Matrix<Scalar, Eigen::Dynamic, 1> from_line_y,
                                         const Eigen::Matrix<Scalar, Eigen::Dynamic, 1> to_line_x,
                                         const Eigen::Matrix<Scalar, Eigen::Dynamic, 1> to_line_y,
                                         Eigen::Transform<Scalar, 3, Eigen::Affine> &transformation)
{
  if (from_line_x.innerSize () != 6 || from_line_y.innerSize () != 6 || to_line_x.innerSize () != 6 || to_line_y.innerSize () != 6)
  {
    PCL_DEBUG ("transformBetween2CoordinateSystems: lines size != 6\n");
    return (false);
  }

  // Check if coordinate systems are valid
  if (!pcl::checkCoordinateSystem (from_line_x, from_line_y) || !pcl::checkCoordinateSystem (to_line_x, to_line_y))
  {
    PCL_DEBUG ("transformBetween2CoordinateSystems: coordinate systems invalid !\n");
    return (false);
  }

  // Convert lines into Vector3 :
  Eigen::Matrix<Scalar, 3, 1> fr0 (from_line_x.template head<3>()),
                              fr1 (from_line_x.template head<3>() + from_line_x.template tail<3>()),
                              fr2 (from_line_