euclidean distance for each point in a point cloud

1
votes

I want to compute the minimum eucidean distance between each point in one point cloud and all the other points in the second point cloud.
My point clouds are named pc1 and pc2. Np is a matrix of normal vectors for each point.

sofar I use the following code:

dist = zeros(size(pc2,1),1);
sign = zeros(size(pc2,1),1);

for i = 1:size(pc2,1)
     d = (pc1(:,1)-pc2(i,1)).^2 + (pc1(:,2)-pc2(i,2)).^2 + (pc1(:,3) - pc2(i,3)).^2;
    [d, ind] = min(d);
    dist(i,1) = sqrt(d);
    sign(i,1) = Np(ind,:)*(pc2(i,:)-pc1(ind,:))';
end

the last bit with "sign" is from this paper. I added it because I want to know if my point lies inside or outside the other point cloud. (I received the point clouds from STL files and they represent surfaces)

Since I am working with rather large point clouds around 200.000 to 3.000.000 points the computation takes a while and I was wondering if anyone could suggest optimizations for my code. Maybe it could be vectorized and I'm not seeing it.
All your suggestions are welcome. Thank you in advance for your time and help.

edit: just to clearify. Each row in my point cloud matrix is a point. the first column is the x-, the second the y-, the third the z-value.

2
matlabpoint-cloudseuclidean-distance

2 Answers

1
votes

You can certainly do this in vectorized form, use pdist2 and min as shown below. 

dmat = pdist2(pc1, pc2);
[dist, ind] = min(dmat);

I believe the following will work for sign, but you should verify the results. May have to tweak slightly based on the matrix shapes.

sign = sum(Np(ind,:).*(pc2-pc1(ind,:)), 2);
0
votes

Another alternative is to use KdTree's. This is also the approach of PCL.

With that approach, you basically create a KdTree from the reference point cloud, and then do a nearest neighbor search for each point from the query point cloud.

This approach will be magnitudes faster than the brute-force approach.