From 7148fa06a812035e9a835021da139ea4d0ce7d8b Mon Sep 17 00:00:00 2001 From: Arnadi Murtiyoso Date: Wed, 15 Apr 2020 19:16:24 +0200 Subject: [PATCH] Update axedetect.m --- axedetect.m | 657 +++++++++++++++++++++++++++++----------------------- 1 file changed, 364 insertions(+), 293 deletions(-) diff --git a/axedetect.m b/axedetect.m index e1c4bf7..c98f4ec 100644 --- a/axedetect.m +++ b/axedetect.m @@ -1,342 +1,413 @@ -function [Beams] = beamSeg (ptCloud,normals2,h,beam_width) -% BEAMSEG +function [Clusters,Axes,remainPc] = axedetect(datapc,beamWidth) +% AXEDETECT % -% Function to perform greedy region growing on a point cloud, based on the -% normal and curvature values. See http://pointclouds.org/documentation/tutorials/region_growing_segmentation.php +% Function to detect the main axes in a planar point cloud and segment it +% automatically according to the detected axes. The algorithm employs +% Principal Componen Analysis (PCA) to transform the 3D point cloud into a +% 2D binary image, and then uses 2D Hough Transform to detect the edges and +% thereafter determines the axes. % % Inputs: -% - ptCloud: point cloud data -% - normals2: normals of each point. Compute with pcnormals -% - h: mean curvature of each point. Compute using Beksi (2014) -% - beam_width: width of the beam... this is the only manual information -% that needs to be provided by the user +% - datapc: input point cloud. The point cloud should be (more or less) +% planar +% - beamWidth: approximate width of the point cloud % % Outputs: -% - Beams: a struct containing the segmented regions for each beam and -% their cuboid RANSAC parameters +% - Clusters: a struct containing the segmented point clouds (segmented +% according the detected axes) +% - Axes: a struct containing the attributes of the axes (in 2D) +% - remainPc : the remaining unsegmented point cloud % % (c) Arnadi Murtiyoso (INSA Strasbourg - ICube-TRIO UMR 7357) -tic -format long g +% tic +%% step 1: transform the point cloud into a planar XY coordinate system +% compute the Principal Component Analysis +coeffs = pca(datapc.Location); -%% OCTREE-BASED REGION GROWING -[RegionsOct]=regiongrowingnormalsOct(ptCloud,normals2,h,20); +% transform the point cloud to PC-aligned system +TransformPCA = datapc.Location*coeffs(:,1:3); -%% FILTER REGIONS, DETECT IF THERE IS L OR Y-SHAPED FACES -threshold_pts = 1000; +% figure('name','Transformed Point Cloud') +% pcshow(pcTransformPCA) -nbRegions=numel(fieldnames(RegionsOct)); -j=1; +% project the point cloud to a plane (Z=0) +TransformPCA(:,3)=0; +pcTransformPCA = pointCloud(TransformPCA); -for i=1:nbRegions - %name of this region - thisRegionName=strcat('Region',int2str(i)); - - %point cloud of this region - thisRegion=RegionsOct.(thisRegionName); - - %if the number of points is less than the threshold, ignore - if thisRegion.CountminX & TransformPCA(:,1)< ... + minX+resolution_pix & TransformPCA(:,2)>minY & ... + TransformPCA(:,2)nbRegions - xy(nbRegions+1,:)=[]; -end -figure -gplot(adjM,xy,'.-') -for i=1:nbRegions - thisRegion=strcat('NewRegions',num2str(i)); - text(x1(i),y1(i),thisRegion,'HorizontalAlignment','center') -end -axis([-1 2 -1 2],'off') +% flip the resulting raster (needed because of the raster coord system) +raster=flip(raster); -%% ADD THE PARALLELENCY CONSTRAINT (CONSTRAINT 2: PARALLELISM) -%using the same principle as region growing to create the clusters +% close the waitbar +close(f) -%create a (dummy) list of available regions -listRegions=1:1:nbRegions; -i=1; +figure('name','Transformed Raster') +imshow(raster) +%% step 3: perform Hough Transform to detect lines +% detect edges in the raster +BW = edge(raster,'canny'); -%create a duplicate of the adjacency matrix -adjM_work=adjM; +% Compute the Hough transform of the binary image returned by edge +[H,theta,rho] = hough(BW); -%initialise the index of the beams -BeamIndex=zeros(nbRegions,2); -BeamIndex(:,1)=listRegions; -seedID=1; %FIRST seed ID +% % Optional: Display the transform, H, returned by the hough function. +% figure +% imshow(imadjust(rescale(H)),[],... +% 'XData',theta,... +% 'YData',rho,... +% 'InitialMagnification','fit'); +% xlabel('\theta (degrees)') +% ylabel('\rho') +% axis on +% axis normal +% hold on +% colormap(gca,hot) -while ~all(isnan(listRegions)) - %immediately, put the current seed out of the function - listRegions(seedID)=NaN; - - %...and put its index in the beam index matrix - BeamIndex(seedID,2)=i; - - %set the seed point cloud - seedPtCloud = NewRegions(seedID).ptCloud; - - %compute its PCA - seedPCA=pca(seedPtCloud.Location); +% Find the peaks in the Hough transform matrix, H, using the houghpeaks +% function. +%P = houghpeaks(H,10,'threshold',ceil(0.3*max(H(:)))); +P = houghpeaks(H,15,'threshold',ceil(0.3*max(H(:)))); + +% Find lines in the image using the houghlines function. +%lines = houghlines(BW,theta,rho,P,'FillGap',5,'MinLength',7); +lines = houghlines(BW,theta,rho,P); + +% % Optional: create a plot that displays the original image with the lines +% % superimposed on it. +% figure, imshow(raster), hold on +% for k = 1:length(lines) +% xy = [lines(k).point1; lines(k).point2]; +% plot(xy(:,1),xy(:,2),'LineWidth',2,'Color','green'); +% % waitforbuttonpress; +% +% % Plot beginnings and ends of lines +% plot(xy(1,1),xy(1,2),'x','LineWidth',2,'Color','yellow'); +% plot(xy(2,1),xy(2,2),'x','LineWidth',2,'Color','red'); +% +% end + +%% step 4: filter the lines generated by Hough Transform (remove colinear vectors) +% create a dummy duplicate of the list +if isempty(lines) + disp('Data quality insufficient to determine axes! Assuming 1 axis...'); + Clusters(1).ptCloud=datapc; + Axes=[]; + remainPc=[]; + return +end + +linesDummy=lines; +j=1; + +% while the duplicate still has elements... +while ~isempty(linesDummy) + [~,nbLines]=size(linesDummy); - %...and extract the 1st order - seedVec=[seedPCA(1,1);seedPCA(2,1);seedPCA(3,1)]; + % take the first row as reference + refTheta=linesDummy(1).theta; + refRho=linesDummy(1).rho; - %put our seed into the seed list - listSeed=find(adjM_work(seedID,:)==1); + % create Clusters to stock the colinear lines + ClusterName=strcat('Cluster',num2str(j)); - %put our seed as NaN in the adjacency matrix duplicate - adjM_work(:,seedID)=NaN; - adjM_work(seedID,:)=NaN; + % create a struct to stock the clusters + lines2.(ClusterName){1}=linesDummy(1); - %keep on looping until no seeds are left - while ~isempty(listSeed) - k=length(listSeed); - - %set a secondary dummy seed list - listSeed2=listSeed; + for i=2:nbLines + % use the subsequent rows as check + checkTheta=linesDummy(i).theta; + checkRho=linesDummy(i).rho; - for j=1:k - %set the new seed ID (actually the first seed's neighborand - %point cloud, and compute also its 1st order PCA parameters - newSeedID=listSeed(j); - newSeedPtCloud = NewRegions(newSeedID).ptCloud; - newSeedPCA = pca(newSeedPtCloud.Location); - newSeedVec = [newSeedPCA(1,1);newSeedPCA(2,1);newSeedPCA(3,1)]; - - %definition of parallelism: their axes' vector follows the - %following relation: v2 = k*v1 - %since the PCA vectors' magnitude is 1 (normalised vectors), - %the disparity between the vectors should be zero if they are - %parallel - disparity=abs(newSeedVec-seedVec); - magDisparity=sqrt(disparity(1)^2+disparity(2)^2+disparity(3)^2); - - %set a tolerance, if the disparity between vectors is less - %than... - if magDisparity < 0.1 - BeamIndex(newSeedID,2)=i; - - %...means than they are parallel. Now let's check if they - %are neighbors - neighbors=find(adjM_work(newSeedID,:)==1); - - %if they are neighbors, add the neighbors to the list of - %seed. Otherwise don't add them - if ~isempty(neighbors) - listSeed2=[listSeed,neighbors]; - end - - %'kill' this particular neighbor in the list of regions and - %the adjacency matrix - listRegions(newSeedID)=NaN; - - adjM_work(:,newSeedID)=NaN; - adjM_work(newSeedID,:)=NaN; - end - - %'kill' this particular neighbor in the dummy seed list - idx = listSeed2==newSeedID; - listSeed2(idx) = []; + % if the difference between reference and check is less than 10 + % degrees an distance less than 10 object units, add to cluster + if abs(refTheta-checkTheta)<10 && abs(refRho-checkRho)<10 + lines2.(ClusterName){i}=linesDummy(i); + %when a row has been added to the cluster, put its values as NaN + linesDummy(i).point1=NaN; + linesDummy(i).point2=NaN; + linesDummy(i).theta=NaN; + linesDummy(i).rho=NaN; end - %update the seed list - listSeed=listSeed2; - end - %find the index of a non-NaN (unkilled) element in the adjacency matrix - indexNonNan=find(~isnan(adjM_work)); - - %take the smallest value of indexing - indexNonNan=min(indexNonNan); - - %this can be improved. I used rem because find returns a matricial - %index instead of row/column index, while I only want the row... - %maybe use listRegion instead... for the moment too lazy to do it - indexNonNan=rem(indexNonNan,nbRegions); - if indexNonNan==0 - indexNonNan=nbRegions; + + % put the values of the reference (1st row) as NaN + linesDummy(1).point1=NaN; + linesDummy(1).point2=NaN; + linesDummy(1).theta=NaN; + linesDummy(1).rho=NaN; + + % delete the struct row with NaNs + F = @(s)any(structfun(@(a)isscalar(a)&&isnan(a),s)); % or ALL + X = arrayfun(F,linesDummy); + linesDummy(X) = []; + + % eliminate empty struct + lines2.(ClusterName)=lines2.(ClusterName)(~cellfun('isempty',... + lines2.(ClusterName))); + + % not important: transpose the result + lines2.(ClusterName)=transpose(lines2.(ClusterName)); + j=j+1; +end +%% step 5: simplify the colinear Hough lines (merge them) +nbColinears=numel(fieldnames(lines2)); +for i=1:nbColinears + ClusterNm=string(strcat('Cluster',{num2str(i)})); + [nbColinearEls,~]=size(lines2.(ClusterNm)); + + %look for extremes + listPotentialExtremes1 = zeros(nbColinearEls,2); + listPotentialExtremes2 = zeros(nbColinearEls,2); + for k=1:nbColinearEls + %get a list of point 1s + listPotentialExtremes1(k,1) = lines2.(ClusterNm){k}.point1(1); + listPotentialExtremes1(k,2) = lines2.(ClusterNm){k}.point1(2); + %get a list of point 2s + listPotentialExtremes2(k,1) = lines2.(ClusterNm){k}.point2(1); + listPotentialExtremes2(k,2) = lines2.(ClusterNm){k}.point2(2); end + %merge the list + listPotentialExtremes=[listPotentialExtremes1;listPotentialExtremes2]; + + %compute the centroid of these points + centroid=[mean(listPotentialExtremes(:,1)), ... + mean(listPotentialExtremes(:,2))]; - %renew the seed ID with the newly found index - seedID=indexNonNan; + %compute the length of each node to the centroid + for k=1:length(listPotentialExtremes) + listPotentialExtremes(k,3)=sqrt((centroid(1)- ... + listPotentialExtremes(k,1))^2+((centroid(2)- ... + listPotentialExtremes(k,2))^2)); + end - %carry on.... - i=i+1; + % take the two points located farthest from centroid as extremities + [~, ind1] = max(listPotentialExtremes(:,3)); + listPotentialExtremes(ind1,3) = -Inf; + [~, ind2] = max(listPotentialExtremes(:,3)); + listPotentialExtremes(ind2,3) = -Inf; + + % create a struct to store the simplified lines, taking the ind1 and + % ind2 points as the extremities + Vector(i).point1=listPotentialExtremes(ind1,1:2)*resolution_pix; + Vector(i).point1(1)=Vector(i).point1(1)+minX; + Vector(i).point1(2)=maxY-Vector(i).point1(2); + Vector(i).point2=listPotentialExtremes(ind2,1:2)*resolution_pix; + Vector(i).point2(1)=Vector(i).point2(1)+minX; + Vector(i).point2(2)=maxY-Vector(i).point2(2); + Vector(i).theta=lines2.(ClusterNm){1,1}.theta; + Vector(i).rho=lines2.(ClusterNm){1,1}.rho; + Vector(i).length=sqrt((Vector(i).point1(1)-Vector(i).point2(1))^2+... + (Vector(i).point1(2)-Vector(i).point2(2))^2); end -%so how many beams do we have? -nbBeams=max(BeamIndex(:,2)); +% % Optional: show the detected merged lines superposed on the point cloud +% figure +% pcshow(pcTransformPCA) +% hold on +% for k = 1:length(Vector) +% xy = [Vector(k).point1; Vector(k).point2]; +% plot3(xy(:,1),xy(:,2),[0;0],'LineWidth',2); +% end -%wait a minute! if only one face is present, it cannot constitute a beam -%(needs at least two beam faces) -for i=1:nbBeams - nbFacesofBeams = sum(BeamIndex(:,2) == i); - if nbFacesofBeams<2 - idx=BeamIndex(:,2)==i; - BeamIndex(idx,:)=[]; - end -end -%so here we have the final number of beams -nbBeams=max(BeamIndex(:,2)); -disp(strcat('Oh happy day! Found',32,num2str(nbBeams),32,'beam(s)!')); +%% step 6: determine the number of axes in the input data +i=1; -%% FINAL STEPS: MERGE THE BEAM FACES PT CLOUD TO CREATE THE BEAM PT CLOUD -for i=1:nbBeams - %find the index of the beam face - idx=BeamIndex(:,2)==i; - thisBeamIndex=BeamIndex(idx,:); - - %compute the number of faces for this particular beam - [nbFaces,~]=size(thisBeamIndex); - - %initialise the beam point cloud with the first face - mergedPtCloud=NewRegions(thisBeamIndex(1,1)).ptCloud; - - %merge the reste progressively to the first face point cloud - for j=2:nbFaces - nextFacePtCloud=NewRegions(thisBeamIndex(j,1)).ptCloud; - mergedPtCloud=pcmerge(mergedPtCloud,nextFacePtCloud,0.001); +% create a duplicate of the previous structure +VectorDummy=Vector; +if isempty(Vector) + disp('Data quality insufficient to determine axes! Assuming 1 axis...'); + Clusters(1).ptCloud=datapc; + Axes=[]; + remainPc=[]; + return +end +Axes=[]; +while ~isempty(VectorDummy) + %take the longest row as reference + a=[VectorDummy.length]; + [~,idx]=max(a); + refTheta=VectorDummy(idx).theta; + + % look for rows whose theta column is similar to the reference + fun = @(x) VectorDummy(x).theta > refTheta-5 && ... + VectorDummy(x).theta < refTheta+5; % useful for complicated fields + tf2 = arrayfun(fun, 1:numel(VectorDummy)); + index2 = find(tf2); + + % get the number of lines having the same theta direction + [~,nbLines] = size(index2); + + % if there is only 1 line or more than 2, impossible to determine the + % axe... + if nbLines<2 || nbLines>2 + VectorDummy(idx).theta=NaN; + + %delete the struct row with NaNs + F = @(s)any(structfun(@(a)isscalar(a)&&isnan(a),s)); % or ALL + X = arrayfun(F,VectorDummy); + VectorDummy(X) = []; + continue end - %store the result in a struct - Beams(i).ptCloud=mergedPtCloud; + % the lines represent the edges of the point cloud. We want to get the + % axe which is located at the center. Solution: averaging the two line + % equations + + % first, determine the line equation components (a and b in y=ax+b) + x1a=VectorDummy(index2(1)).point1(1); + y1a=VectorDummy(index2(1)).point1(2); + x2a=VectorDummy(index2(1)).point2(1); + y2a=VectorDummy(index2(1)).point2(2); + + % a and b of the first line + aa=(y1a-y2a)/(x1a-x2a); + ba=(y2a*x1a-y1a*x2a)/(x1a-x2a); + + x1b=VectorDummy(index2(2)).point1(1); + y1b=VectorDummy(index2(2)).point1(2); + x2b=VectorDummy(index2(2)).point2(1); + y2b=VectorDummy(index2(2)).point2(2); + + % a and b of the second line + ab=(y1b-y2b)/(x1b-x2b); + bb=(y2b*x1b-y1b*x2b)/(x1b-x2b); + + % put the information in a struct 'Axes' + Axes(i).a=(aa+ab)/2; + Axes(i).b=(ba+bb)/2; + Axes(i).theta=VectorDummy(index2(1)).theta; + + % set an arbitrary first and second point for the line representing the + % axe + Axes(i).x1 = minX; + Axes(i).y1 = (minX*Axes(i).a)+Axes(i).b; + Axes(i).z1 = 0; + Axes(i).x2 = maxX; + Axes(i).y2 = (maxX*Axes(i).a)+Axes(i).b; + Axes(i).z2 = 0; + + % when the duplicate row has been used, set one of its elements to NaN + VectorDummy(index2(1)).theta=NaN; + VectorDummy(index2(2)).theta=NaN; + + %delete the struct row with NaNs + F = @(s)any(structfun(@(a)isscalar(a)&&isnan(a),s)); % or ALL + X = arrayfun(F,VectorDummy); + VectorDummy(X) = []; + + i=i+1; end - -%visualise the result -figure -for i=1:numel(Beams) - thisPtCloud=Beams(i).ptCloud; - labels=zeros(thisPtCloud.Count,1); - labels(:,1)=1; - for l=1:thisPtCloud.Count - labels(l,1)=i+1; - end -pcshow(thisPtCloud.Location,labels) -colormap(hsv(i+1)) +if isempty(Axes) + disp('Data quality insufficient to determine axes! Assuming 1 axis...'); + Clusters(1).ptCloud=datapc; + remainPc=[]; + return +elseif length(Axes)==1 + disp('1 axis was found!'); + Clusters(1).ptCloud=datapc; + remainPc=[]; + Axes=[]; + return +end + +nbAxes = length(Axes); +disp(strcat(num2str(nbAxes),32,'axes were found!')); +% Plot the axes superposed on the (PCA-transformed) point cloud +figure +pcshow(pcTransformPCA) +title(strcat('Number of axes found:',num2str(nbAxes))) hold on +for k = 1:nbAxes + plot3([Axes(k).x1;Axes(k).x2],[Axes(k).y1;Axes(k).y2],[0;0],... + 'LineWidth',2); end -%% EXTRA STEP: CUBOID RANSAC -%use this part to generate geometric primitives (cuboids) from the -%now-segmented beams -figure -pcshow(ptCloud) -for i=1:length(Beams) - ptCloud=Beams(i).ptCloud; - coords=double(ptCloud.Location); - [model, CuboidParameters, inlierIndices, outlierIndices] = ... - CuboidRANSAC( coords ); - - %store the cuboid parameters in the struct - Beams(i).CuboidModel=model; - Beams(i).CuboidParameters=CuboidParameters; - Beams(i).CuboidInlierIdx=inlierIndices; - Beams(i).CuboidOutlierIdx=outlierIndices; - - %visualise the cuboids - DisplayModel(numel(inlierIndices), model); - hold on +%% step 7: segment the point cloud according to the axe +% create duplicates just in case +ptCloudinput=datapc; +ptCloudinput2=pcTransformPCA; + +% do a loop for each detected axe +for k=1:nbAxes + + % create a line with the two arbitrary points + P = [Axes(k).x1 Axes(k).y1;Axes(k).x2 Axes(k).y2]; + + % create a buffer zone around this line, using the width as input + polyout1 = polybuffer(P,'lines',(beamWidth+0.2*beamWidth)/2); + + % check if there are points inside the buffer zone + TFin = isinterior(polyout1,ptCloudinput2.Location(:,1), ... + ptCloudinput2.Location(:,2)); + + % retrieve the indices of points located inside the buffer zone + rows = find(TFin(:,1)==1); + + % segment the point cloud with the inlier indices directly from the + % original input point cloud + ptCloudAxe=select(ptCloudinput,rows); + + % retrive the remaining points + rows2 = find(TFin(:,1)==0); + remainPcPCA=select(ptCloudinput2,rows2); + ptCloudinput2=remainPcPCA; + remainPc=select(ptCloudinput,rows2); + ptCloudinput=remainPc; + + % create a struct containing the segmented point clouds + Clusters(k).ptCloud=ptCloudAxe; end -toc + +% toc