点云法向量估计方法

        已经大半年没更新博客了，记得去年开始写与点云相关的第一篇博客时，就信誓旦旦要坚守下去。这一篇技术博客姗姗来迟了些，正如2002年的第一场雪一样，不过9102年武汉的第一场雪已经来过，近半年状态的持续低迷，以及琐事缠身根本无法潜心对一些技术进行专研，其实最初写博客的初衷,其一，更多的是表明一种活到老学到老的人生态度，无论何种状态下都要保持对知识技能的高度认可；其二，也是把自己在校期间的一些学习资源甚至是学习方法与网友们共享，如果以生产实际这个标准来衡量我的博客的话真心没有多大的价值可言，更多的还是为初学者提供入门的基础知识以及给予他们在科研、算法这条被玄幻笼罩的路上有坚持走下去的决心与力量。

      废话到此为止，这也表明今天这篇博客属于“失踪人口回归”系列，法线在点云里作为表述点云特征的一种重要信息，pcl提供了成熟的接口。当然自己去实现，算法难度也不大，为了熟悉pcl源码，博主之前对pcl源码求取法线的代码进行了剥离，同时对算法的实现流程做了一个简答的表述，让法线从此不再神秘。

      点云法向量的估计流程

                                                                      

         1.对于第一步就比较常规了，建议大家直接使用pcl的kdtree或者octree进行最近邻搜索

               

　　

         2.求协方差矩阵，这里贴一段代码（当然也可以自己实现，难度不大）

          

//协方差矩阵求解方法
unsigned int
computeMeanAndCovarianceMatrix(const pcl::PointCloud<PointXYZ> &cloud,
Eigen::Matrix<double, 3, 3> &covariance_matrix,
Eigen::Matrix<double, 4, 1> &centroid)
{
    // create the buffer on the stack which is much faster than using cloud[indices[i]] and centroid as a buffer
    Eigen::Matrix<double, 1, 9, Eigen::RowMajor> accu = Eigen::Matrix<double, 1, 9, Eigen::RowMajor>::Zero();
    size_t point_count;
    if (cloud.is_dense)
    {
        point_count = cloud.size();
        // For each point in the cloud
        for (size_t i = 0; i < point_count; ++i)
        {
            accu[0] += cloud[i].x * cloud[i].x;
            accu[1] += cloud[i].x * cloud[i].y;
            accu[2] += cloud[i].x * cloud[i].z;
            accu[3] += cloud[i].y * cloud[i].y; // 4
            accu[4] += cloud[i].y * cloud[i].z; // 5
            accu[5] += cloud[i].z * cloud[i].z; // 8
            accu[6] += cloud[i].x;
            accu[7] += cloud[i].y;
            accu[8] += cloud[i].z;
        }
    }
    else
    {
        point_count = 0;
        for (size_t i = 0; i < cloud.size(); ++i)
        {
            if (!isFinite(cloud[i]))
                continue;

            accu[0] += cloud[i].x * cloud[i].x;
            accu[1] += cloud[i].x * cloud[i].y;
            accu[2] += cloud[i].x * cloud[i].z;
            accu[3] += cloud[i].y * cloud[i].y;
            accu[4] += cloud[i].y * cloud[i].z;
            accu[5] += cloud[i].z * cloud[i].z;
            accu[6] += cloud[i].x;
            accu[7] += cloud[i].y;
            accu[8] += cloud[i].z;
            ++point_count;
        }
    }
    accu /= static_cast<double> (point_count);
    if (point_count != 0)
    {
        //centroid.head<3> () = accu.tail<3> ();    -- does not compile with Clang 3.0
        centroid[0] = accu[6]; centroid[1] = accu[7]; centroid[2] = accu[8];
        centroid[3] = 1;
        covariance_matrix.coeffRef(0) = accu[0] - accu[6] * accu[6];
        covariance_matrix.coeffRef(1) = accu[1] - accu[6] * accu[7];
        covariance_matrix.coeffRef(2) = accu[2] - accu[6] * accu[8];
        covariance_matrix.coeffRef(4) = accu[3] - accu[7] * accu[7];
        covariance_matrix.coeffRef(5) = accu[4] - accu[7] * accu[8];
        covariance_matrix.coeffRef(8) = accu[5] - accu[8] * accu[8];
        covariance_matrix.coeffRef(3) = covariance_matrix.coeff(1);
        covariance_matrix.coeffRef(6) = covariance_matrix.coeff(2);
        covariance_matrix.coeffRef(7) = covariance_matrix.coeff(5);
    }
    return (static_cast<unsigned int> (point_count));
}

         3.特征值以及对应的特征向量的求解（贴一段代码）

            说明一下eigen也提供了相关的方法，博主这里对pcl的源码进行了整理，二者所求结果不尽一致，孰优孰劣大家可以在日常使用中去进行检验。

  

#include <iostream>
#include <Eigen/Dense>
#include <Eigen/Eigenvalues>
//#include <pcl/common/centroid.h>
//#include <pcl/common/eigen.h>
using namespace Eigen;
using namespace std;
//using namespace pcl;


 void computeRoots2(const Matrix3d::Scalar& b, const Matrix3d::Scalar& c, Eigen::Vector3d& roots)
{
     roots(0) = Matrix3d::Scalar(0);
     Matrix3d::Scalar d = Matrix3d::Scalar(b * b - 4.0 * c);
    if (d < 0.0)  // no real roots ! THIS SHOULD NOT HAPPEN!
        d = 0.0;

    Matrix3d::Scalar sd = ::std::sqrt(d);

    roots(2) = 0.5f * (b + sd);
    roots(1) = 0.5f * (b - sd);
}

void computeRoots(const Matrix3d& m, Eigen::Vector3d& roots)
{
    typedef  Matrix3d::Scalar Scalar;

    // The characteristic equation is x^3 - c2*x^2 + c1*x - c0 = 0.  The
    // eigenvalues are the roots to this equation, all guaranteed to be
    // real-valued, because the matrix is symmetric.
    Scalar c0 = m(0, 0) * m(1, 1) * m(2, 2)
        + Scalar(2) * m(0, 1) * m(0, 2) * m(1, 2)
        - m(0, 0) * m(1, 2) * m(1, 2)
        - m(1, 1) * m(0, 2) * m(0, 2)
        - m(2, 2) * m(0, 1) * m(0, 1);
    Scalar c1 = m(0, 0) * m(1, 1) -
        m(0, 1) * m(0, 1) +
        m(0, 0) * m(2, 2) -
        m(0, 2) * m(0, 2) +
        m(1, 1) * m(2, 2) -
        m(1, 2) * m(1, 2);
    Scalar c2 = m(0, 0) + m(1, 1) + m(2, 2);

    if (fabs(c0) < Eigen::NumTraits < Scalar > ::epsilon())  // one root is 0 -> quadratic equation
        computeRoots2(c2, c1, roots);
    else
    {
        const Scalar s_inv3 = Scalar(1.0 / 3.0);
        const Scalar s_sqrt3 = std::sqrt(Scalar(3.0));
        // Construct the parameters used in classifying the roots of the equation
        // and in solving the equation for the roots in closed form.
        Scalar c2_over_3 = c2 * s_inv3;
        Scalar a_over_3 = (c1 - c2 * c2_over_3) * s_inv3;
        if (a_over_3 > Scalar(0))
            a_over_3 = Scalar(0);

        Scalar half_b = Scalar(0.5) * (c0 + c2_over_3 * (Scalar(2) * c2_over_3 * c2_over_3 - c1));

        Scalar q = half_b * half_b + a_over_3 * a_over_3 * a_over_3;
        if (q > Scalar(0))
            q = Scalar(0);

        // Compute the eigenvalues by solving for the roots of the polynomial.
        Scalar rho = std::sqrt(-a_over_3);
        Scalar theta = std::atan2(std::sqrt(-q), half_b) * s_inv3;
        Scalar cos_theta = std::cos(theta);
        Scalar sin_theta = std::sin(theta);
        roots(0) = c2_over_3 + Scalar(2) * rho * cos_theta;
        roots(1) = c2_over_3 - rho * (cos_theta + s_sqrt3 * sin_theta);
        roots(2) = c2_over_3 - rho * (cos_theta - s_sqrt3 * sin_theta);

        // Sort in increasing order.
        if (roots(0) >= roots(1))
            std::swap(roots(0), roots(1));
        if (roots(1) >= roots(2))
        {
            std::swap(roots(1), roots(2));
            if (roots(0) >= roots(1))
                std::swap(roots(0), roots(1));
        }

        if (roots(0) <= 0)  // eigenval for symetric positive semi-definite matrix can not be negative! Set it to 0
            computeRoots2(c2, c1, roots);
    }
}

void eigen33zx(const Matrix3d& mat, Matrix3d& evecs, Eigen::Vector3d& evals)
{
    typedef  Matrix3d::Scalar Scalar;
    // typedef  Matrix3d::Scalar Scalar;
    // Scale the matrix so its entries are in [-1,1].  The scaling is applied
    // only when at least one matrix entry has magnitude larger than 1.

    Scalar scale = mat.cwiseAbs().maxCoeff();
    if (scale <= std::numeric_limits < Scalar > ::min())
        scale = Scalar(1.0);

    Matrix3d scaledMat = mat / scale;

    // Compute the eigenvalues
    computeRoots(scaledMat, evals);

    if ((evals(2) - evals(0)) <= Eigen::NumTraits < Scalar > ::epsilon())
    {
        // all three equal
        evecs.setIdentity();
    }
    else if ((evals(1) - evals(0)) <= Eigen::NumTraits < Scalar > ::epsilon())
    {
        // first and second equal
        Matrix3d tmp;
        tmp = scaledMat;
        tmp.diagonal().array() -= evals(2);

        Eigen::Vector3d vec1 = tmp.row(0).cross(tmp.row(1));
        Eigen::Vector3d vec2 = tmp.row(0).cross(tmp.row(2));
        Eigen::Vector3d vec3 = tmp.row(1).cross(tmp.row(2));

        Scalar len1 = vec1.squaredNorm();
        Scalar len2 = vec2.squaredNorm();
        Scalar len3 = vec3.squaredNorm();

        if (len1 >= len2 && len1 >= len3)
            evecs.col(2) = vec1 / std::sqrt(len1);
        else if (len2 >= len1 && len2 >= len3)
            evecs.col(2) = vec2 / std::sqrt(len2);
        else
            evecs.col(2) = vec3 / std::sqrt(len3);

        evecs.col(1) = evecs.col(2).unitOrthogonal();
        evecs.col(0) = evecs.col(1).cross(evecs.col(2));
    }
    else if ((evals(2) - evals(1)) <= Eigen::NumTraits < Scalar > ::epsilon())
    {
        // second and third equal
        Matrix3d tmp;
        tmp = scaledMat;
        tmp.diagonal().array() -= evals(0);

        Eigen::Vector3d vec1 = tmp.row(0).cross(tmp.row(1));
        Eigen::Vector3d vec2 = tmp.row(0).cross(tmp.row(2));
        Eigen::Vector3d vec3 = tmp.row(1).cross(tmp.row(2));

        Scalar len1 = vec1.squaredNorm();
        Scalar len2 = vec2.squaredNorm();
        Scalar len3 = vec3.squaredNorm();

        if (len1 >= len2 && len1 >= len3)
            evecs.col(0) = vec1 / std::sqrt(len1);
        else if (len2 >= len1 && len2 >= len3)
            evecs.col(0) = vec2 / std::sqrt(len2);
        else
            evecs.col(0) = vec3 / std::sqrt(len3);

        evecs.col(1) = evecs.col(0).unitOrthogonal();
        evecs.col(2) = evecs.col(0).cross(evecs.col(1));
    }
    else
    {
        Matrix3d tmp;
        tmp = scaledMat;
        tmp.diagonal().array() -= evals(2);

        Eigen::Vector3d vec1 = tmp.row(0).cross(tmp.row(1));
        Eigen::Vector3d vec2 = tmp.row(0).cross(tmp.row(2));
        Eigen::Vector3d vec3 = tmp.row(1).cross(tmp.row(2));

        Scalar len1 = vec1.squaredNorm();
        Scalar len2 = vec2.squaredNorm();
        Scalar len3 = vec3.squaredNorm();
#ifdef _WIN32
        Scalar *mmax = new Scalar[3];
#else
        Scalar mmax[3];
#endif
        unsigned int min_el = 2;
        unsigned int max_el = 2;
        if (len1 >= len2 && len1 >= len3)
        {
            mmax[2] = len1;
            evecs.col(2) = vec1 / std::sqrt(len1);
        }
        else if (len2 >= len1 && len2 >= len3)
        {
            mmax[2] = len2;
            evecs.col(2) = vec2 / std::sqrt(len2);
        }
        else
        {
            mmax[2] = len3;
            evecs.col(2) = vec3 / std::sqrt(len3);
        }

        tmp = scaledMat;
        tmp.diagonal().array() -= evals(1);

        vec1 = tmp.row(0).cross(tmp.row(1));
        vec2 = tmp.row(0).cross(tmp.row(2));
        vec3 = tmp.row(1).cross(tmp.row(2));

        len1 = vec1.squaredNorm();
        len2 = vec2.squaredNorm();
        len3 = vec3.squaredNorm();
        if (len1 >= len2 && len1 >= len3)
        {
            mmax[1] = len1;
            evecs.col(1) = vec1 / std::sqrt(len1);
            min_el = len1 <= mmax[min_el] ? 1 : min_el;
            max_el = len1 > mmax[max_el] ? 1 : max_el;
        }
        else if (len2 >= len1 && len2 >= len3)
        {
            mmax[1] = len2;
            evecs.col(1) = vec2 / std::sqrt(len2);
            min_el = len2 <= mmax[min_el] ? 1 : min_el;
            max_el = len2 > mmax[max_el] ? 1 : max_el;
        }
        else
        {
            mmax[1] = len3;
            evecs.col(1) = vec3 / std::sqrt(len3);
            min_el = len3 <= mmax[min_el] ? 1 : min_el;
            max_el = len3 > mmax[max_el] ? 1 : max_el;
        }

        tmp = scaledMat;
        tmp.diagonal().array() -= evals(0);

        vec1 = tmp.row(0).cross(tmp.row(1));
        vec2 = tmp.row(0).cross(tmp.row(2));
        vec3 = tmp.row(1).cross(tmp.row(2));

        len1 = vec1.squaredNorm();
        len2 = vec2.squaredNorm();
        len3 = vec3.squaredNorm();
        if (len1 >= len2 && len1 >= len3)
        {
            mmax[0] = len1;
            evecs.col(0) = vec1 / std::sqrt(len1);
            min_el = len3 <= mmax[min_el] ? 0 : min_el;
            max_el = len3 > mmax[max_el] ? 0 : max_el;
        }
        else if (len2 >= len1 && len2 >= len3)
        {
            mmax[0] = len2;
            evecs.col(0) = vec2 / std::sqrt(len2);
            min_el = len3 <= mmax[min_el] ? 0 : min_el;
            max_el = len3 > mmax[max_el] ? 0 : max_el;
        }
        else
        {
            mmax[0] = len3;
            evecs.col(0) = vec3 / std::sqrt(len3);
            min_el = len3 <= mmax[min_el] ? 0 : min_el;
            max_el = len3 > mmax[max_el] ? 0 : max_el;
        }

        unsigned mid_el = 3 - min_el - max_el;
        evecs.col(min_el) = evecs.col((min_el + 1) % 3).cross(evecs.col((min_el + 2) % 3)).normalized();
        evecs.col(mid_el) = evecs.col((mid_el + 1) % 3).cross(evecs.col((mid_el + 2) % 3)).normalized();
#ifdef _WIN32
        delete[] mmax;
#endif
    }
    // Rescale back to the original size.
    evals *= scale;
}

　　

         4.根据3中的结果即可实现法向量的估计

         上面的代码抽取的pcl的源码，接口较多，不过也间接说明了底层算法人员的不易，其实每一个成熟稳定的接口都来自于底层算法人员长期努力的结果，这里向那些开源的算法库致敬。

         随便写的有点混乱，我还是来一个例子，对于近邻搜索这一块，网上例子铺天盖地，先略过。

         

 Matrix3d A;
    A << 6, -3,0, -3,6, 0, 0, 0, 0;
    cout << "Here is a 3x3 matrix, A:" << endl << A << endl << endl;
    Eigen:: Vector3d eigen_values;
    // pcl::eigen33(A, eigen_values);//pcl的接口
    Matrix3d eigenVectors1;
    eigen33zx(A, eigenVectors1, eigen_values);
    cout << eigenVectors1 << endl;
    cout << eigen_values << endl;

　　上面的矩阵A是一个协方差矩阵，是博主为了检验该方法是否准确，自己随便用XoY平面上的几个点求解的。