gusucode.com > 基于三维曲面的顶点搜索算法源码程序 > 基于三维曲面的顶点搜索算法源码程序/findPointNormals/findPointNormals.m

    function [ normals, curvature ] = findPointNormals(points, numNeighbours, viewPoint, dirLargest)
%FINDPOINTNORMALS Estimates the normals of a sparse set of n 3d points by
% using a set of the closest neighbours to approximate a plane.
%
%   Required Inputs:
%   points- nx3 set of 3d points (x,y,z)
%
%   Optional Inputs: (will give default values on empty array [])
%   numNeighbours- number of neighbouring points to use in plane fitting
%       (default 9)
%   viewPoint- location all normals will point towards (default [0,0,0])
%   dirLargest- use only the largest component of the normal in determining
%       its direction wrt the viewPoint (generally provides a more stable
%       estimation of planes near the viewPoint, default false)
%
%   Outputs:
%   normals- nx3 set of normals (nx,ny,nz)
%   curvature- nx1 set giving the curvature
%
%   References-
%   The implementation closely follows the method given at
%   http://pointclouds.org/documentation/tutorials/normal_estimation.php
%   This code was used in generating the results for the journal paper
%   Multi-modal sensor calibration using a gradient orientation measure 
%   http://www.zjtaylor.com/welcome/download_pdf?pdf=JFR2013.pdf
%
%   This code was written by Zachary Taylor
%   zacharyjeremytaylor@gmail.com
%   http://www.zjtaylor.com

%% check inputs
validateattributes(points, {'numeric'},{'ncols',3});

if(nargin < 2)
    numNeighbours = [];
end
if(isempty(numNeighbours))
    numNeighbours = 9;
else
    validateattributes(numNeighbours, {'numeric'},{'scalar','positive'});
    if(numNeighbours > 100)
        warning(['%i neighbouring points will be used in plane'...
            ' estimation, expect long run times, large ram usage and'...
            ' poor results near edges'],numNeighbours);
    end
end

if(nargin < 3)
    viewPoint = [];
end
if(isempty(viewPoint))
    viewPoint = [0,0,0];
else
    validateattributes(viewPoint, {'numeric'},{'size',[1,3]});
end

if(nargin < 4)
    dirLargest = [];
end
if(isempty(dirLargest))
    dirLargest = false;
else
    validateattributes(dirLargest, {'logical'},{'scalar'});
end

%% setup

%ensure inputs of correct type
points = double(points);
viewPoint = double(viewPoint);

%get nearest neighbours
n = knnsearch(points,points,'k',(numNeighbours+1));

%remove self
n = n(:,2:end);

%find difference in position from neighbouring points
p = repmat(points(:,1:3),numNeighbours,1) - points(n(:),1:3);
p = reshape(p, size(points,1),numNeighbours,3);

%calculate values for covariance matrix
C = zeros(size(points,1),6);
C(:,1) = sum(p(:,:,1).*p(:,:,1),2);
C(:,2) = sum(p(:,:,1).*p(:,:,2),2);
C(:,3) = sum(p(:,:,1).*p(:,:,3),2);
C(:,4) = sum(p(:,:,2).*p(:,:,2),2);
C(:,5) = sum(p(:,:,2).*p(:,:,3),2);
C(:,6) = sum(p(:,:,3).*p(:,:,3),2);
C = C ./ numNeighbours;

%% find eigenvalues and vectors of covariance matrix

%form equation lambda^3 + a*lambda^2 + b*lambda + c = 0
a = -C(:,1) - C(:,4) - C(:,6);
b = C(:,1).*C(:,4) - C(:,2).^2 + C(:,1).*C(:,6) - C(:,3).^2 + C(:,4).*C(:,6) - C(:,5).^2;
c = -C(:,1).*C(:,4).*C(:,6) + C(:,1).*C(:,5).*C(:,5) + C(:,2).*C(:,2).*C(:,6) - 2*C(:,2).*C(:,5).*C(:,3) + C(:,3).*C(:,4).*C(:,3);

%solve for lambda
Q=sqrt(a.^2-3*b)/3;
R=(2*a.^3-9*a.*b+27*c)/54;
thita=acos(R./(Q.^3));
lambda = [-2*Q.*cos(thita/3)-a/3,-2*Q.*cos((thita+2*pi)/3)-a/3,-2*Q.*cos((thita-2*pi)/3)-a/3];

%find smallest eigenvalue
minLam = min(lambda,[],2);

%find assosiated eigenvector (eigenvector == normal direction)
normals = ones(size(minLam,1),3);
normals(:,2) = (-C(:,5).*C(:,1) + C(:,3).*C(:,2)) ./ (C(:,4).*C(:,1) - C(:,2).*C(:,2));
normals(:,1) = -(C(:,2).*normals(:,2) + C(:,3))./C(:,1);
normals = normals./repmat(sqrt(sum(normals.^2,2)),1,3);

%find curvature
curvature = minLam ./ sum(lambda,2);

%% flipping normals

%ensure normals point towards viewPoint
points = points - repmat(viewPoint,size(points,1),1);
if(dirLargest)
    [~,idxLam] = max(abs(normals),[],2);
    idxLam = (1:size(normals,1))' + (idxLam-1)*size(normals,1);
    dir = normals(idxLam).*points(idxLam) > 0;
else
    dir = sum(normals.*points,2) > 0;
end

normals(dir,:) = -normals(dir,:);

end