使用CGAL自带的最小二乘法拟合直线的方法

现在网上能找到的CGAL教程很少了，很多教程由于时间久远，已经被搜索引擎沉底，不再优先展示了。本文的方法还是作者到处寻找，最后参考官方的例子做出来的，实在来之不易。

bool fittingLine(const pcl::PointCloud<pcl::PointXYZ>::Ptr& pointCloud,
    const pcl::IndicesPtr& indices,
    pcl::PointXYZ& outPointOnLine,
    pcl::PointXYZ& outLineDirection)
{
    // 定义CGAL的Kernal
    typedef CGAL::Simple_cartesian<float>  K;
    typedef K::Line_3                   Line;
    typedef K::Plane_3                  Plane;
    typedef K::Point_3                  Point;
    typedef K::Triangle_3               Triangle;
    typedef K::Direction_3              Direction;

    // 参数检查
    if (pointCloud == nullptr || pointCloud->empty())
    {
        return false;
    }
    if (indices != nullptr && indices->empty())
    {
        return false;
    }

    // 把PCL的点格式转换成CGAL的点格式，兼容不提供索引的情况
    std::vector<Point> cgalPoints;
    if (indices != nullptr)
    {
        cgalPoints.reserve(indices->size());
        for (int pointIndex : *indices)
        {
            const pcl::PointXYZ& pclPoint = pointCloud->at(pointIndex);
            cgalPoints.push_back(Point(pclPoint.x, pclPoint.y, pclPoint.z));
        }
    }
    else
    {
        cgalPoints.reserve(pointCloud->size());
        for (const pcl::PointXYZ& pclPoint : *pointCloud)
        {
            cgalPoints.push_back(Point(pclPoint.x, pclPoint.y, pclPoint.z));
        }
    }

    // 利用CGAL拟合三维直线，官方文档说使用的是最小二乘法
    Line line;
    linear_least_squares_fitting_3(cgalPoints.begin(), cgalPoints.end(), line, CGAL::Dimension_tag<0>());

    // 返回拟合结果
    const Point& linePoint = line.point();
    const Direction& lineDirection = line.direction();
    outPointOnLine.x = linePoint.x();
    outPointOnLine.y = linePoint.y();
    outPointOnLine.z = linePoint.z();
    outLineDirection.x = lineDirection.dx();
    outLineDirection.y = lineDirection.dy();
    outLineDirection.z = lineDirection.dz();
    return true;
}

这个函数接受的参数都是PCL的类型，这样可以和PCL相互配合，处理点云。

芸芸小站首发，阅读原文：http://xiaoyunyun.net/index.php/archives/216.html