点云的基本几何计算

1.计算法向量

原文件

function normal = estimateNormal(data, tree, query, radius, min_neighbors)
% ESTIMATENORMAL Given a point cloud and query point, estimate the surface 
% normal by performing an eigendecomposition of the covariance matrix created 
% from the nearest neighbors of the query point for a fixed radius.
%
%  Example: 
%       data = randn(256,3);
%       tree = KDTreeSearcher(data);
%       query = [data(1,1) data(1,2) data(1,3)];
%       radius = 1.0;
%       min_neighbors = 8;
%       normal = estimateNormal(data, tree, query, radius, min_neighbors)
%
%  Copyright (c) 2014, William J. Beksi <beksi@cs.umn.edu>
% 

% Find neighbors within the fixed radius 
idx = rangesearch(tree, query, radius);                                                   
idxs = idx{1};
neighbors = [data(idxs(:),1) data(idxs(:),2) data(idxs(:),3)];

if size(neighbors) < min_neighbors
    normal = {1};
    return;
end

% Compute the covariance matrix C
C = cov(neighbors);

% Compute the eigenvector of C
[v, lambda] = eig(C);

% Find the eigenvector corresponding to the minimum eigenvalue in C
[~, i] = min(diag(lambda));

% Normalize
normal = v(:,i) ./ norm(v(:,i));

end 

　　

2.计算曲率

　　曲线的曲率（curvature）就是针对曲线上某个点的切线方向角对弧长的转动率，通过微分来定义，表明曲线偏离直线的程度。数学上表明曲线在某一点的弯曲程度的数值。曲率越大，表示曲线的弯曲程度越大。曲率的倒数就是曲率半径。

　　平均曲率、高斯曲率、主曲率计算

　　贺美芳, et al. (2005.8). "散乱点云数据的曲率估算及应用."

ScalarType Neighbourhood::computeCurvature(unsigned neighbourIndex, CC_CURVATURE_TYPE cType)
{
    switch (cType)
    {
    case GAUSSIAN_CURV:
    case MEAN_CURV:
        {
            //we get 2D1/2 quadric parameters
            const PointCoordinateType* H = getQuadric();
            if (!H)
                return NAN_VALUE;

            //compute centroid
            const CCVector3* G = getGravityCenter();

            //we compute curvature at the input neighbour position + we recenter it by the way
            CCVector3 Q = *m_associatedCloud->getPoint(neighbourIndex) - *G;

            const unsigned char X = m_quadricEquationDirections.x;
            const unsigned char Y = m_quadricEquationDirections.y;

            //z = a+b.x+c.y+d.x^2+e.x.y+f.y^2
            //const PointCoordinateType& a = H[0];
            const PointCoordinateType& b = H[1];
            const PointCoordinateType& c = H[2];
            const PointCoordinateType& d = H[3];
            const PointCoordinateType& e = H[4];
            const PointCoordinateType& f = H[5];

            //See "CURVATURE OF CURVES AND SURFACES – A PARABOLIC APPROACH" by ZVI HAR’EL
            const PointCoordinateType  fx    = b + (d*2) * Q.u[X] + (e  ) * Q.u[Y];    // b+2d*X+eY
            const PointCoordinateType  fy    = c + (e  ) * Q.u[X] + (f*2) * Q.u[Y];    // c+2f*Y+eX
            const PointCoordinateType  fxx    = d*2;                                    // 2d
            const PointCoordinateType  fyy    = f*2;                                    // 2f
            const PointCoordinateType& fxy    = e;                                    // e

            const PointCoordinateType fx2 = fx*fx;
            const PointCoordinateType fy2 = fy*fy;
            const PointCoordinateType q = (1 + fx2 + fy2);

            switch (cType)
            {
            case GAUSSIAN_CURV:
                {
                    //to sign the curvature, we need a normal!
                    PointCoordinateType K = fabs( fxx*fyy - fxy*fxy ) / (q*q);
                    return static_cast<ScalarType>(K);
                }

            case MEAN_CURV:
                {
                    //to sign the curvature, we need a normal!
                    PointCoordinateType H = fabs( ((1+fx2)*fyy - 2*fx*fy*fxy + (1+fy2)*fxx) ) / (2*sqrt(q)*q);
                    return static_cast<ScalarType>(H);
                }

            default:
                assert(false);
            }
        }
        break;

    case NORMAL_CHANGE_RATE:
        {
            assert(m_associatedCloud);
            unsigned pointCount = (m_associatedCloud ? m_associatedCloud->size() : 0);

            //we need at least 4 points
            if (pointCount < 4)
            {
                //not enough points!
                return pointCount == 3 ? 0 : NAN_VALUE;
            }

            //we determine plane normal by computing the smallest eigen value of M = 1/n * S[(p-µ)*(p-µ)']
            CCLib::SquareMatrixd eigVectors;
            std::vector<double> eigValues;
            if (!Jacobi<double>::ComputeEigenValuesAndVectors(computeCovarianceMatrix(), eigVectors, eigValues))
            {
                //failure
                return NAN_VALUE;
            }

            //compute curvature as the rate of change of the surface
            double e0 = eigValues[0];
            double e1 = eigValues[1];
            double e2 = eigValues[2];
            double sum = fabs(e0+e1+e2);
            if (sum < ZERO_TOLERANCE)
            {
                return NAN_VALUE;
            }

            double eMin = std::min(std::min(e0,e1),e2);
            return static_cast<ScalarType>(fabs(eMin) / sum);
        }
        break;

    default:
        assert(false);
    }

    return NAN_VALUE;
}

 

3.计算点云密度

 　  CloudCompare中的实现方法　http://www.cnblogs.com/yhlx125/p/5874936.html

4.计算点云表面粗糙度

 　　 表面粗糙度

//"PER-CELL" METHOD: ROUGHNESS ESTIMATION (LEAST SQUARES PLANE FIT)
//ADDITIONNAL PARAMETERS (1):
// [0] -> (PointCoordinateType*) kernelRadius : neighbourhood radius
bool GeometricalAnalysisTools::computePointsRoughnessInACellAtLevel(const DgmOctree::octreeCell& cell, 
                                                                    void** additionalParameters,
                                                                    NormalizedProgress* nProgress/*=0*/)
{
    //parameter(s)
    PointCoordinateType radius = *static_cast<PointCoordinateType*>(additionalParameters[0]);

    //structure for nearest neighbors search
    DgmOctree::NearestNeighboursSphericalSearchStruct nNSS;
    nNSS.level = cell.level;
    nNSS.prepare(radius,cell.parentOctree->getCellSize(nNSS.level));
    cell.parentOctree->getCellPos(cell.truncatedCode,cell.level,nNSS.cellPos,true);
    cell.parentOctree->computeCellCenter(nNSS.cellPos,cell.level,nNSS.cellCenter);

    unsigned n = cell.points->size(); //number of points in the current cell
    
    //for each point in the cell
    for (unsigned i=0; i<n; ++i)
    {
        ScalarType d = NAN_VALUE;
        cell.points->getPoint(i,nNSS.queryPoint);

        //look for neighbors inside a sphere
        //warning: there may be more points at the end of nNSS.pointsInNeighbourhood than the actual nearest neighbors (= neighborCount)!
        unsigned neighborCount = cell.parentOctree->findNeighborsInASphereStartingFromCell(nNSS,radius,false);
        if (neighborCount > 3)
        {
            //find the query point in the nearest neighbors set and place it at the end
            const unsigned globalIndex = cell.points->getPointGlobalIndex(i);
            unsigned localIndex = 0;
            while (localIndex < neighborCount && nNSS.pointsInNeighbourhood[localIndex].pointIndex != globalIndex)
                ++localIndex;
            //the query point should be in the nearest neighbors set!
            assert(localIndex < neighborCount);
            if (localIndex+1 < neighborCount) //no need to swap with another point if it's already at the end!
            {
                std::swap(nNSS.pointsInNeighbourhood[localIndex],nNSS.pointsInNeighbourhood[neighborCount-1]);
            }

            DgmOctreeReferenceCloud neighboursCloud(&nNSS.pointsInNeighbourhood,neighborCount-1); //we don't take the query point into account!
            Neighbourhood Z(&neighboursCloud);

            const PointCoordinateType* lsPlane = Z.getLSPlane();
            if (lsPlane)
                d = fabs(DistanceComputationTools::computePoint2PlaneDistance(&nNSS.queryPoint,lsPlane));

            //swap the points back to their original position (DGM: not necessary)
            //if (localIndex+1 < neighborCount)
            //{
            //    std::swap(nNSS.pointsInNeighbourhood[localIndex],nNSS.pointsInNeighbourhood[neighborCount-1]);
            //}
        }

        cell.points->setPointScalarValue(i,d);

        if (nProgress && !nProgress->oneStep())
        {
            return false;
        }
    }

    return true;
}

　　地面粗糙度是指在一个特定的区域内，地球表面积与其投影面积之比。它也是反映地表形态的一个宏观指标。

5.计算点云重心

 　　

//计算重心
CCVector3 GeometricalAnalysisTools::computeGravityCenter(GenericCloud* theCloud)
{
    assert(theCloud);
    
    unsigned count = theCloud->size();
    if (count == 0)
        return CCVector3();

    CCVector3d sum(0,0,0);

    theCloud->placeIteratorAtBegining();
    const CCVector3 *P = 0;
    while ((P = theCloud->getNextPoint()))
    {
        sum += CCVector3d::fromArray(P->u);
    }

    sum /= static_cast<double>(count);
    return CCVector3::fromArray(sum.u);
}

 

6.计算点云权重重心

 　　

//计算权重中心
CCVector3 GeometricalAnalysisTools::computeWeightedGravityCenter(GenericCloud* theCloud, ScalarField* weights)
{
    assert(theCloud && weights);

    unsigned count = theCloud->size();
    if (count == 0 || !weights || weights->currentSize() < count)
        return CCVector3();

    CCVector3d sum(0, 0, 0);

    theCloud->placeIteratorAtBegining();
    double wSum = 0;
    for (unsigned i = 0; i < count; ++i)
    {
        const CCVector3* P = theCloud->getNextPoint();
        ScalarType w = weights->getValue(i);
        if (!ScalarField::ValidValue(w))
            continue;
        sum += CCVector3d::fromArray(P->u) * fabs(w);
        wSum += w;
    }

    if (wSum != 0)
        sum /= wSum;

    return CCVector3::fromArray(sum.u);
}

 

7.计算点云协方差

 　　

//计算协方差矩阵
CCLib::SquareMatrixd GeometricalAnalysisTools::computeCovarianceMatrix(GenericCloud* theCloud, const PointCoordinateType* _gravityCenter)
{
    assert(theCloud);
    unsigned n = (theCloud ? theCloud->size() : 0);
    if (n==0)
        return CCLib::SquareMatrixd();

    CCLib::SquareMatrixd covMat(3);
    covMat.clear();

    //gravity center
    CCVector3 G = (_gravityCenter ?  CCVector3(_gravityCenter) : computeGravityCenter(theCloud));

    //cross sums (we use doubles to avoid overflow)
    double mXX = 0;
    double mYY = 0;
    double mZZ = 0;
    double mXY = 0;
    double mXZ = 0;
    double mYZ = 0;

    theCloud->placeIteratorAtBegining();
    for (unsigned i=0;i<n;++i)
    {
        const CCVector3* Q = theCloud->getNextPoint();

        CCVector3 P = *Q-G;
        mXX += static_cast<double>(P.x*P.x);
        mYY += static_cast<double>(P.y*P.y);
        mZZ += static_cast<double>(P.z*P.z);
        mXY += static_cast<double>(P.x*P.y);
        mXZ += static_cast<double>(P.x*P.z);
        mYZ += static_cast<double>(P.y*P.z);
    }

    covMat.m_values[0][0] = mXX/static_cast<double>(n);
    covMat.m_values[0][0] = mYY/static_cast<double>(n);
    covMat.m_values[0][0] = mZZ/static_cast<double>(n);
    covMat.m_values[1][0] = covMat.m_values[0][1] = mXY/static_cast<double>(n);
    covMat.m_values[2][0] = covMat.m_values[0][2] = mXZ/static_cast<double>(n);
    covMat.m_values[2][1] = covMat.m_values[1][2] = mYZ/static_cast<double>(n);

    return covMat;
}

 

8.计算点云互协方差

　　

//计算2个点云的互协方差
CCLib::SquareMatrixd GeometricalAnalysisTools::computeCrossCovarianceMatrix(GenericCloud* P,
                                                                            GenericCloud* Q,
                                                                            const CCVector3& Gp,
                                                                            const CCVector3& Gq)
{
    assert(P && Q);
    assert(Q->size() == P->size());

    //shortcuts to output matrix lines
    CCLib::SquareMatrixd covMat(3);
    double* l1 = covMat.row(0);
    double* l2 = covMat.row(1);
    double* l3 = covMat.row(2);

    P->placeIteratorAtBegining();
    Q->placeIteratorAtBegining();

    //sums
    unsigned count = P->size();
    for (unsigned i=0; i<count; i++)
    {
        CCVector3 Pt = *P->getNextPoint() - Gp;
        CCVector3 Qt = *Q->getNextPoint() - Gq;

        l1[0] += Pt.x*Qt.x;
        l1[1] += Pt.x*Qt.y;
        l1[2] += Pt.x*Qt.z;
        l2[0] += Pt.y*Qt.x;
        l2[1] += Pt.y*Qt.y;
        l2[2] += Pt.y*Qt.z;
        l3[0] += Pt.z*Qt.x;
        l3[1] += Pt.z*Qt.y;
        l3[2] += Pt.z*Qt.z;
    }

    covMat.scale(1.0/static_cast<double>(count));

    return covMat;
}

 

//计算权重互协方差
CCLib::SquareMatrixd GeometricalAnalysisTools::computeWeightedCrossCovarianceMatrix(GenericCloud* P, //data
                                                                                    GenericCloud* Q, //model
                                                                                    const CCVector3& Gp,
                                                                                    const CCVector3& Gq,
                                                                                    ScalarField* coupleWeights/*=0*/)
{
    assert(P && Q);
    assert(Q->size() == P->size());
    assert(coupleWeights);
    assert(coupleWeights->currentSize() == P->size());

    //shortcuts to output matrix lines
    CCLib::SquareMatrixd covMat(3);
    double* r1 = covMat.row(0);
    double* r2 = covMat.row(1);
    double* r3 = covMat.row(2);

    P->placeIteratorAtBegining();
    Q->placeIteratorAtBegining();

    //sums
    unsigned count = P->size();
    double wSum = 0.0; //we will normalize by the sum
    for (unsigned i = 0; i<count; i++)
    {
        CCVector3d Pt = CCVector3d::fromArray((*P->getNextPoint() - Gp).u);
        CCVector3 Qt = *Q->getNextPoint() - Gq;

        //Weighting scheme for cross-covariance is inspired from
        //https://en.wikipedia.org/wiki/Weighted_arithmetic_mean#Weighted_sample_covariance
        double wi = 1.0;
        if (coupleWeights)
        {
            ScalarType w = coupleWeights->getValue(i);
            if (!ScalarField::ValidValue(w))
                continue;
            wi = fabs(w);
        }

        //DGM: we virtually make the P (data) point nearer if it has a lower weight
        Pt *= wi;
        wSum += wi;

        //1st row
        r1[0] += Pt.x * Qt.x;
        r1[1] += Pt.x * Qt.y;
        r1[2] += Pt.x * Qt.z;
        //2nd row
        r2[0] += Pt.y * Qt.x;
        r2[1] += Pt.y * Qt.y;
        r2[2] += Pt.y * Qt.z;
        //3rd row
        r3[0] += Pt.z * Qt.x;
        r3[1] += Pt.z * Qt.y;
        r3[2] += Pt.z * Qt.z;
    }

    if (wSum != 0.0)
        covMat.scale(1.0/wSum);

    return covMat;
}