Esta página se ha traducido mediante traducción automática. Haga clic aquí para ver la última versión en inglés.

Redacte una serie de escaneos láser con cambios de pose

Utilice la función matchScans para calcular la diferencia de pose entre una serie de escaneos láser. Componga las poses relativas utilizando una función composePoses definida para obtener una transformación al cuadro inicial. Luego, transforme todos los escaneos láser en el cuadro inicial usando estas poses compuestas.

Especifique el escaneo láser original y las compensaciones para generar una serie de escaneos láser desplazados. Repita los escaneos y transforme el escaneo original en función de cada desplazamiento. Trace los escaneos láser para ver los datos desplazados.

ranges = zeros(300,4);
angles = zeros(300,4);
ranges(:,1) = 5*ones(300,1);
ranges(11:30,1) = 4*ones(1,20);
ranges(101:200,1) = 3*ones(1,100);
angles(:,1) = linspace(-pi/2,pi/2,300);
offset(1,:) = [0.1 0.1 0];
offset(2,:) = [0.4 0.1 0.1];
offset(3,:) = [-0.2 0 -0.1];

for i = 2:4
    [ranges(:,i),angles(:,i)] = transformScan(ranges(:,i-1),angles(:,i-1),offset(i-1,:));
end

[x,y] = pol2cart(angles,ranges);
plot(x,y)
axis equal

Realice una comparación de escaneos en cada conjunto de escaneo láser para obtener la pose relativa entre cada escaneo. Las salidas de la función matchScans están cerca de los desplazamientos especificados. El escaneo inicial se realiza en el cuadro inicial, por lo que la diferencia de pose es [0 0 0].

relPoses(1,:) = [0 0 0];

for i = 2:4
    relPoses(i,:) = matchScans(ranges(:,i),angles(:,i),...
                       ranges(:,i-1),angles(:,i-1),'CellSize',1);
end

Utilice la función composePoses en un bucle para obtener la transformación absoluta para cada escaneo láser. Esta función se define al final del ejemplo. Transforma cada escaneo para colocarlos todos en el cuadro inicial.

transRanges = zeros(300,4);
transAngles = zeros(300,4);
transRanges(:,1) = ranges(:,1);
transAngles(:,1) = angles(:,1);
composedPoses(1,:) = [0 0 0];
for i = 2:4
    composedPoses(i,:) = composePoses(relPoses(i,:),composedPoses(i-1,:));
    [transRanges(:,i),transAngles(:,i)] = transformScan(ranges(:,i),angles(:,i),composedPoses(i,:)); 
end

Traza los rangos y ángulos transformados. Se superponen bien, según las transformaciones calculadas a partir de matchScans.

[x,y] = pol2cart(transAngles,transRanges);
plot(x,y)
axis equal

Define la función composePoses. Esta función toma en cuenta la transformación del fotograma inicial al fotograma base y la transformación relativa del fotograma inicial a un segundo fotograma. Para una serie de escaneos láser, la entrada relative es la pose relativa entre los dos últimos fotogramas y la entrada base es la pose compuesta de todos los escaneos anteriores.

También puede definir esta función en un script separado y guardarla en la carpeta actual.

function composedPose = composePoses(relative,base) 
    %Convert both poses (3-by-1 vector) to transformations (4-by-4 matrix) and multiply
    %together using pose2tform function.
    tform = pose2tform(base)*pose2tform(relative);
    
    % Extract translational vector and Euler angles as ZYX.
    trvec = tform2trvec(tform); 
    eul = tform2eul(tform); 
    
    % Concatenate the elements of the transform as [x y theta].
    composedPose = [trvec(1:2) eul(1)]; 

    % Function to convert pose to transform.
    function tform = pose2tform(pose) 
        x = pose(1); 
        y = pose(2); 
        th = wrapTo2Pi(pose(3)); 
        tform = trvec2tform([x y 0])*eul2tform([th 0 0]); 
    end 
end

Consulte también

transformScan | matchScans