El registro de puntos dispersos (SPR) es un algoritmo de registro de nubes de puntos en 2017. El objetivo principal de este algoritmo es registrar nubes de puntos dispersos y ha logrado buenos resultados y avances. Por un lado, este artículo es un análisis de principio simple y una implementación de código adicional del modelo de algoritmo de registro SPR. Por otro lado, es un resumen del trabajo anterior, que también es una publicación de blog. El siguiente trabajo trata principalmente sobre segmentación y predicción de flujo óptico.

1. Descripción general del modelo de algoritmo

1. Antecedentes del algoritmo

La llamada nube de puntos dispersa es un modelo de nube de puntos con pocos puntos. A veces necesitamos usar puntos clave en algunos objetos para registrarnos con el modelo objetivo y calcular algunos indicadores clave. El algoritmo tradicional de registro de nubes de puntos requiere que las dos nubes de puntos que se registrarán sean del mismo orden de magnitud, y también incluye dos etapas de registro grueso y registro fino. Los experimentos muestran que los algoritmos tradicionales de registro de nubes de puntos funcionan mal en el registro de nubes de puntos dispersos, por lo que el registro de nubes de puntos dispersos es muy crítico.

2. Modelo de algoritmo

El algoritmo SPR puede lograr un mejor efecto de registro de nube de puntos dispersos sin registro grueso.La idea central del algoritmo incluye principalmente tres partes : perturbación, iteración y refinamiento . El modelo de algoritmo SPR incluye los siguientes pasos:

Inicialice los datos de la nube de puntos del modelo de destino A, los datos de la nube de puntos dispersos B, el número máximo de iteraciones MaxIterations, el umbral de error de registro Umbral y el número de perturbaciones P

Calcule la cantidad de perturbación de la iteración actual de acuerdo con el tamaño de la nube de puntos y la distribución gaussiana (la cantidad de perturbación disminuye con el número de iteraciones y finalmente disminuye a cero) para obtener matrices de transformación de perturbaciones de perturbaciones P y luego realizar la transformación de perturbaciones en la corriente Nube de puntos B

Calcule el costo de la nube de puntos dispersos B y la nube de puntos A del modelo de destino después de P perturbaciones y transformaciones (aquí puede usar el Árbol KD para calcular la desviación de punto más cercana y tomarla como el Costo), y elija la que tenga la menor El costo como la perturbación óptima local

Con base en la nube de puntos dispersos B después de la transformación de perturbación óptima actual, realice un refinamiento de registro fino del algoritmo ICP o DQF (ICP se selecciona en este documento), obtenga la matriz de registro fino Tk y calcule el error en este momento

Si el error calculado es menor que el error mínimo e, actualice la matriz de transformación de borde óptima global T y el error mínimo e como resultado de la iteración actual.

Repita el proceso anterior hasta que se alcance el número máximo de iteraciones MaxIterations o el error de cálculo e<umbral de error de registro Threshold

3. Diagrama de flujo del algoritmo

Además, el documento también explica algunas precauciones más detalladas y la configuración de parámetros, como el número máximo de iteraciones MaxIterations, el umbral de error de registro Threshold, cuánto se establece el número de perturbaciones P, cómo realizar cálculos de perturbaciones y cómo elegir en el experimento Puntos, etc. Por supuesto, cuantos más puntos clave característicos en la nube de puntos dispersa, mejor será el efecto de registro.

2. Implementación del algoritmo (versión ICP)

El algoritmo SPR de este documento se implementa mediante programación Matlab y se utiliza la versión de registro fino de ICP. El algoritmo original se mejora durante el proceso de implementación: en el proceso iterativo, el cálculo de registro fino de ICP se realiza para cada transformación de perturbación que aparece, y luego comparar en función de este error, en lugar de calcular el ICP después de seleccionar la perturbación óptima como decía el algoritmo original, puede mejorar la precisión del algoritmo en la mayor medida, pero sacrifica la complejidad del tiempo. Parte del código se da a continuación.

(1)SPR.m

% SPR Main logic function:transform A sparse point cloud into B model point cloud coordinate system for registration
% - Params：
%   - A:sparse point cloud [n,3]
%   - B:model point cloud [m,3]
% - Return：
%   - R:optimal rotation matrix [3,3]
%   - T:optimal translation matrix [1,3]
%   - trans_res:transform point cloud [n,3]
function [ R,T,trans_res ] = SPR(A,B)

%1.kdtree model for B(euclidean distance) and size of the model
Mdl = KDTreeSearcher(B);
Size = max(range(B));
%2.Initialize R0,T0,e0
R = eye(3);
%Align the centroids of two point clouds A->B
cen_A = sum(A)/length(A);
cen_B = sum(B)/length(B);
T = cen_B - cen_A;
trans_res = transform(A,R,T);
e = calculateCost(trans_res,B,Mdl);
%3.Initialize the number of disturbed pose P, the maximum number of iterations I, and the minimum error threshold RMS
P = 10;
I = 30;
RMS = 1e-6;
%4.Initialize loop variable
k = 1;
%record data
x_loop = 1:I;
y_error = zeros(1,I);

%5.SPR Loop
while e > RMS && k <= I
    cost_best = inf;
    R_k = R;
    T_k = T;
    %(1). Perturbations for Sparse point cloud,and then the perturbation transformation matrix is obtained
    % - R_p: [3,3,P+1]
    % - T_p: [p+1,3]
    [R_p, T_p] = perturb(Size,P,k,I);
    %(2). Calculate the optimal pose
    for j = 1:P+1
        %Obtain the transformation matrix parameters rp_j(3,3), tp_j(1,3) of the current p-th disturbance pose
        rp_j = R_p(:,:,j)*R;
        tp_j = (R_p(:,:,j)*T')'+T_p(j,:);
        %Calculate current optimal cost(from ICP)
        [R_j,T_j,cost_j] = solveicp(A,B,Mdl,rp_j,tp_j);
        %judge optimal cost
        if cost_j < cost_best
            cost_best = cost_j;
            R_k = R_j;
            T_k = T_j;
        end
    end
    %(3). Error calculation and judge for update
    trans_k = transform(A,R_k,T_k);
    e_k = calculateCost(trans_k,B,Mdl);
    if e_k < e
        e = e_k;
        R = R_k;
        T = T_k;
    end
    y_error(k) = e;
    k = k+1;
end
trans_res = transform(A,R,T);

figure
plot(x_loop,y_error,marker='o', MarkerFaceColor='r');
xlabel('loop');
ylabel('ems error');

end

(2) Perturbar.m

%Disturbance function: Generates random perturbations based on iteration within loop and specified
% - Params：
%   - size:Size of the model
%   - P：Number of disturbances
%   - k：Number of iterations
%   - I：Iteration upper bound
% - Return：
%   - R_p:Perturbation rotation matrix [3,3,P+1]
%   - T_p:Perturbation translation matrix [P+1,3]
function [ R_p, T_p ] = perturb( size,P,k,I )

%1.Initial translation disturbance:10% of the model size
T_p = normrnd(0,(0.1*size*((I+1-k)/I)),[P,3]); %[P,3]

%2.Initial perturbation number of rotation:10 ° normal distribution(Right hand coordinate system)
%Around x
a = normrnd(0,(10*((I+1-k)/I)),[1,P]); %[1,P]
%Around y
b = normrnd(0,(10*((I+1-k)/I)),[1,P]);
%Around z
c = normrnd(0,(10*((I+1-k)/I)),[1,P]);

%3.convert to radians
a = a.*(pi()/180);
b = b.*(pi()/180);
c = c.*(pi()/180);

%4.initialize R_p
R_p = zeros(3,3,P);

%5.Generate P disturbances respectively (matlab subscript starts from 1)
for j = 1:P
    %rotation matrix about x
    r1 = [1,0,0; 0, cos(a(j)), -sin(a(j)); 0, sin(a(j)), cos(a(j))]; 
    %rotation matrix about y
    r2 = [cos(b(j)), 0, sin(b(j)); 0,1,0; -sin(b(j)), 0, cos(b(j))];
    %rotation matrix about z
    r3 = [cos(c(j)), -sin(c(j)), 0; sin(c(j)), cos(c(j)), 0; 0,0,1];
    %Perturbation Rotation
    R_p(:,:,j) = r1*r2*r3;
end
%P+1:Transformation without disturbance
R_p(:,:,P+1) = eye(3);
T_p(P+1,:) = [0,0,0];
end

(3) solveicp.m

%Function takes in A and B clusters, registers A to B using ICP, returns resultant transformation matrix and error calculation
% - Params：
%   - A:initial point cloud [n,3]
%   - B:target point cloud [m,3]
% - Return：
%   - TR:solve rotation matrix [1,3]
%   - RO:solve translation matrix [3,3]
%   - e:registers error
function [ R,T,e ] = solveicp( A,B,Mdl,R_0,T_0 )
 
%1.initialize variable parameters
k = 1;
error = 0.0001;
Iterator = 50;
R = R_0;
T = T_0;
%2.initial:kdtree algorithm to find the nearest point pair of two point clouds
trans = transform(A,R,T);
e = calculateCost(trans,B,Mdl);
%3.ICP loop
while e > error && k <= Iterator
    %find the nearest point
    idx = knnsearch(Mdl,trans);
    NN = B(idx,:);
    %svd solve: optimal matching matrix of two point clouds in current state
    [T_k, R_k] = solvesvd(trans,NN);
    %Cumulative transformation T=[1,3] R=[3,3]
    T = (R_k*T')'+T_k;
    R = R_k*R;
    trans = transform(A,R,T);
    %Update error E
    e = sum(sqrt(sum((NN-trans).^2,2)))/length(trans);
    k = k+1;
    
end

end

(4) resuelvevd.m

%function: svd solve yo find T and R between point set B and it nearest neighbour set
% - Params：
%   - B:initial point clouds [n,3]
%   - NN: target nearest neighbour point clouds [n,3]
% - Return：
%   - T:solve optimal translation matrix [1,3]
%   - R:solve optimal rotation matrix [3,3]
function [ T, R ] = solvesvd( B, NN )

num = length(B);
%1.Seeking centroid
cen_B = sum(B)/num;
cen_NN = sum(NN)/num;
%2.Decentralization:Set centroids as reference origin for both clusters
B2 = B-repmat(cen_B,num,1);
NN2 = NN-repmat(cen_NN,num,1);
%3.svd decompose:[U,S,V] = svd(A) ==> A = U*S*V'
H = B2'*NN2;
[U,~,V] = svd(H);
R = V*diag([1 1 sign(det(V*U'))])*U';
T = cen_NN' - R*cen_B';
T = T';

end

3. Darse cuenta del efecto

Nota: El azul en la imagen de arriba es la nube de puntos dispersos inicial, y el negro es el resultado de la nube de puntos dispersos después del registro

Registro de nube de puntos (4) Análisis del algoritmo de registro de puntos dispersos