Corner detection from object planes - Pseudo into MATLAB code

As part of a corner detection algorithm for point clouds, I started turning the following pseudocode into MATLAB code:

Input: Planes: pl = {pl_i}_i=1,2,··· ,n, theshold: dth.

Output: Corners: P : {P1, P2, · · · , Pm}.

P ← ∅;

for i=1 to size(pl)-2 do

for j=i+1 to size(pl)-1 do

for k=j+1 to size(pl) do

A ← (pli.n, plj.n, plk.n)T;

if det(A) == 0 then

break;

else

point ← A−1b;

l12.n ← pli.n × plj.n, l12.p ← point;

l13.n ← pli.n × plk.n, l13.p ← point;

l23.n ← plj.n × plk.n, l23.p ← point;

line ← (l12, l13, l23), plane ← (pli, plj, plk);

if isValid(line, plane, dth) then

P ← P ∪ point;

Function isValid(line, plane, dth);

l12 ← line(1), l13 ← line(2), l23 ← line(3);

pl1 ← plane(1), pl2 ← plane(2), pl3 ← plane(3);

if isInner(l12, l13, pl1, dth) then

if isInner(l12, l23, pl2, dth) then

if isInner(l13, l23, pl3, dth) then

return True;

Function isInner(l1, l2, pl, dth);

if intersect(l1, pl, dth) and intersect(l2, pl, dth) then

return True;

Function intersect(line, plane, dth);

P ← line.p, n ← line.n;

V ← plane.vertices, C ← plane.center;

VC ← V − C, PC ← P − C;

dvc ← n × VC, dpc ← n × PC;

if dvc < dpc then PV ← P − V, d ← min(n × PV);

if d ≤ dth then

return True;

else return True;

The code is supposed to validate corner points of an object that is defined by several planes. It takes every possible combination of 3 planes, calculates the intersection point, and checks whether the intersection point belongs to the object or not.

Specifically, I need help with the last part, the function intersect().

Input to this function is a line (intersection between two planes), the intersection point with the third plane, and one plane

Here is the MATLAB code I wrote. It gives me some results, however, many invalid points are still considered correct.

function boolean = intersect(line, plane, d_th)

P = line.P; % intersection point of 3 planes and potential object corner point

n = line.n; % intersection between two planes

C = plane.center;

V = plane.vertices; % 4 corner vertices of the plane

VC = V - C;

PC = P - C;

d_pc = cross(n, PC);

PV = P - V;

for i = 1:size(PV,2)

cross_products(i) = norm(cross(n, PV(:,i)));

end

[d, min_element] = min(cross_products);

d_vc = cross(n, VC(:, min_element);

boolean = false;

if (norm(d_vc) < norm(d_pc));

if abs(d) <= abs(d_th)

boolean = true;

end

else

boolean = true;

end

end

Thanks in advance for any kind of help!

Answer 1

[Maneet Kaur Bagga]

Hi,

As per my understanding, you are trying to implement a "corner detection algorithm" for point clouds in MATLAB, to identify the corners of an object represented by a collection of planar surfaces and the above algorithm involves iterating through all combinations of three planes from the input set to find potential corner points i.e the intersection of the planes.

Please refer to the following as a possible workaround:

• To make the code more efficient and concise subtract the centre "C" from all the vertices "C" at once.
• Initialize "cross_products" to store the norms of the cross products of "n" with each "PV".
• The condition "if norm(d_vc) < norm(d_pc)" is used to check if the point "P" is closer to the plane's centre "C" than the vertices "V". If "P" is closer, we then check if it is within the threshold distance "d_th". In the provided code above the comparison "if (norm(d_vc) < norm(d_pc));" might not correctly reflect the pseudocode's intent due to the misplaced semicolon.

Please refer to the code below for better understanding:

function boolean = intersect(line, plane, d_th)
    P = line.P; % intersection point of 3 planes and potential object corner point
    n = line.n; % normal vector of the intersection line between two planes
    C = plane.center; % center of the plane
    V = plane.vertices; % 4 corner vertices of the plane
    
    % Compute vectors from the plane center to each vertex
    VC = bsxfun(@minus, V, C);
    
    % Compute the vector from the plane center to the intersection point
    PC = P - C;
    
    % Compute the cross product of the normal with the PC vector
    d_pc = cross(n, PC);
    
    % Initialize a vector to store the cross product norms
    cross_products = zeros(1, size(V, 2));
    
    % Compute the cross product of the normal with each PV vector
    for i = 1:size(V, 2)
        PV = P - V(:, i);
        cross_products(i) = norm(cross(n, PV));
    end
    
    % Find the minimum norm of the cross products
    [d, min_element] = min(cross_products);
    
    % Compute the cross product of the normal with the corresponding VC vector
    d_vc = cross(n, VC(:, min_element));
    
    % Initialize boolean flag as false
    boolean = false;
    
    % Check if the intersection point is within the threshold distance
    if norm(d_vc) < norm(d_pc)
        if d <= d_th
            boolean = true;
        end
    else
        boolean = true;
    end
end

The code here assumes that "v" is a 3 by N matrix where each column represents the coordinates of a vertex, and "c" and "p" are column vectors representing the coordinates of centre and the intersection point.

Please refer to the following MATLAB Answer which gives a similar workaround on "Intersection between two planes in MATLAB" for further understanding:

https://www.mathworks.com/matlabcentral/answers/400504-find-the-intersection-point-between-2-plane-in-matlab?s_tid=answers_rc1-2_p2_MLT

Hope this helps!