Speed up calculation of normal vectors metric

Hey there!

Given a measured pointcloud (by using a 3D measuring device), I would like to calculate the p2 norm deviation of each normal vector from the mean normal vector in a given sphere to estimate the uniformity of the point cloud for each point. I think that my code is working but it's super slow (facing ~ 1 mio. points). I calculate all normals by using pcnormals, then I'm growing a kdtree and isolate the 10 nearest neighbors. Then I trim all neighbors beyond a radial threshold and calculte the mean normal vector. Right now I don't see any implementation without using a for-loop.

Note that I'm using the fast DNorm2 MEX implementation by Jan but you can also use vecnorm or pdist2. I attachted a small part of the pointcloud for test purposes.

Maybe one of the pros can help me out.

EDIT: It is not super important to use a spherical ROI. Some sort of voxel-filter would also be fine.

Lennart

K=10; %10-NN Search
dist_thres=0.015; %Maximum distance ROI => Sphere with radius dist_thres
normals = pcnormals(pointCloud(points),6);


for i=1:length(points)
    %disp(i);
    idx=knnsearch(points([1:i-1 i+1:end],:),points(i,:),'K',K); %Find K Nearest Neighbors by growing a kdtree
    distances=DNorm2(repmat(points(i,:),K,1)'-points(idx,:)'); %Euclidean Distance of all NNs
    idx(distances>dist_thres)=[]; %Remove neighbor outliers
    metric(i)=DNorm2(normals(i)-mean(normals(idx,:),1)); %calculate p2 norm of deviation of mean normal inside sphere and the normal of the corresponding point
end

Answer 1

[Guillaume]

Why do you remove the current point from the search set. Doing that means copying the whole point matrix (bar one point) for each step of the loop. A million copy operation of a million minus 1 points is always going to take a while. Can't you just search for the nearest K+1 point and remove the nearest (the one with distance 0) from the returned set?

Also, since you're calling knnsearch for each of your points, you're reconstruction the Kd-tree each time. Doing that a million times is probably not very efficient. I would think that building the tree just once with kdtreesearcher would be a lot more efficient.

So, I suspect this would be faster. I don't have the stats toolbox, so I'm just going from the doc:

%...
searcher = kdtreesearcher(points);
for ptidx = 1:size(points, 1)   %don't use length on a matrix!
    [idx, d] = knnsearch(searcher, points(ptidx, :), 'K', K+1);   %Note that it's the kdtreesearcher.knnsearch method being called here, not the generic knnsearch function
    idx(d == 0) = [];  %remove current point from those returned
    %... rest of the calculations
end

Also, from your description shouldn't you be using rangesearch instead of knnsearch?

edit: was missing the points input in knnsearch

[Lennart Hinz]

Hey!

Thank you very much for your suggestions. In the meantime, I was able to develop a very fast and loopless implementation of all calculations which leads to approx. 4-5 seconds of computation time (0.85 mio. points, K=20, thres_rad=0.05 mm). I would still be glad to recieve any additional feedback to speed up the calculations.

Unfortunately the usage of the rangesearch was way too slow (~ 25 seconds) and is therefore impracticable. I'm aware of limiting the kdtree to a certain number of NNs is leading to some sort of approximation of the desired metric. I did some calculations and testing and could identify the mean K for a given radial threshold. This lead to approx. 95% accuracy (dividing all identified NNs by the total amount of all NNs) which seems to be sufficient. By incrementing K a bit, you can easily get to 99% and still be much faster than the rangesearch method.

This is the code:

normals = pcnormals(pointCloud(points),6);
index=linspace(1,size(points,1),size(points,1));
[idx,mask] = findSphereNeighbors(points,index,thres_rad,K);
metric=DNorm2(normals'-squeeze(mean(reshape(normals(idx,:),[],K,3).*mask,2))')';

And here are some implementations of the findSphereNeighbors function. I tried to adress your feedback and calculated the median computation time of 10 loops (including the code above).

5.3251 seconds:

 mask=ones(size(points,1),K);
 idx(index,:)=knnsearch(points(:,:),points(index,:),'K',K+1);
 idx(:,1)=[];
 distances(index,:)=reshape(DNorm2(repmat(points(index,:),K,1)'-points(idx(index,:),:)'),size(points,1),K);
 mask(distances(index,:)>thres_rad)=0;
 idx=reshape(idx,length(points),[]);

5.1775 seconds:

mask=ones(size(points,1),K);
idx(index,:)=knnsearch(points([1:index-1 index+1:end],:),points(index,:),'K',K);
distances(index,:)=reshape(DNorm2(repmat(points(index,:),K,1)'-points(idx(index,:),:)'),size(points,1),K);
mask(distances(index,:)>thres_rad)=0;
idx=reshape(idx,size(points,1),[]);

4.2898 seconds:

mask=ones(size(points,1),K);
searcher = KDTreeSearcher(points);
[idx, distances] = knnsearch(searcher,points(index,:), 'K', K+1);
idx(:,1)=[];
distances(:,1)=[];  
mask(distances(index,:)>thres_rad)=0;
idx=reshape(idx,size(points,1),[]);

4.1868 seconds:

mask=ones(size(points,1),K);
searcher = KDTreeSearcher(points([1:index-1 index+1:end],:));
[idx, distances] = knnsearch(searcher,points(index,:), 'K', K);    
mask(distances(index,:)>thres_rad)=0;
idx=reshape(idx,size(points,1),[]);

Note that it seems that contrary to your theory it is much faster to remove a point from the current set than to enlarge the tree by one K :P

If an expert still has some ideas how to speed up the calculations I would be very happy about further feedback.

Lennart

[Guillaume]

Glad you've got something that works.

As I don't have the toolbox I can't do any testing so I'm not sure I can help much further. However:

index=linspace(1,size(points,1),size(points,1));

is the strangest way I've seen of simply doing:

index = 1:size(points, 1);

In some places you use length(points). I'd recommend you always use size(points, 1) as length would return the number of columns if for some reason you only had 1 or 2 points. I know it's probably unlikely but since you mean to query the number of rows, do ask for the number of rows.

This:

 mask=ones(size(points,1),K);
 %...
 mask(distances(index,:)>thres_rad)=0;

is also a strange way of doing:

 mask = distances < thres_rad;

It looks like all the findSphereNeighbors snippets you've posted are meant to be in a loop with the query point being just one point (so one row). As a result, idx and distances would be just one row and the idx(:, 1), and distances(:, 1), are just misleanding, this : expands to just 1. idx(1) and distances(1) would be clearer. On the other hand, you then have max(distances(index, :)) so I'm a bit confused.

Note that if you do have a 2D distance matrix, make sure it is preallocated before the loop if you're filling it one row at a time.

Note that my suggestion was to build the Kd-tree only once before the loop. All the methods you show appear to rebuild it each time inside loop. Then yes, searching for more point would be less efficient.

But actually, rereading the knnsearch doc, I see you can pass all query points at once so you don't even need a loop. This should be more efficient, at least for the seach. You might still need a loop for the distance filtering afterward:

indices = knnsearch(points, points, 'K', K+1);  %search the K+1 nearest neighbours to all the points at once
indices(:, 1) = [];  %from your code I assume that 1st index of each row is the nearest neighbour, i.e. the current point. Note that index is a matrix here

And actually, looking at the distance calculation, I don't think you need a loop for that either:

nearestpoints = permute(reshape(points(indices, :), size(points, 1), K, []), [1, 3 ,2]);  %3D matrix of nearest points (K points across 3rd dimension)
distance = squeeze(sqrt(sum((points - nearestpoints) .^ 2, 2)));
mask = distance <= thresh_rad
%...

The only time consuming part above is the permute. It's still very fast on a 1e6x10x3 matrix.