點雲法向量估計方法

        已經大半年沒更新博客了，記得去年開始寫與點雲相關的第一篇博客時，就信誓旦旦要堅守下去。這一篇技術博客姍姍來遲了些，正如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平面上的幾個點求解的。