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