Документация

Составьте серию лазерных сканирований с изменениями положения

Используйте функцию matchScans, чтобы вычислить различие в положении между серией лазерных сканирований. Составьте относительные положения при помощи заданной функции composePoses, чтобы получить преобразование к начальному кадру. Затем преобразуйте все лазерные сканирования в начальный кадр с помощью этих составленных положений.

Задайте исходное лазерное сканирование и смещения, чтобы сгенерировать серию переключенных лазерных сканирований. Выполните итерации через сканирования и преобразуйте исходное сканирование на основе каждого смещения. Постройте лазерные сканирования, чтобы видеть переключенные данные.

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

Выполните сканирование, соответствующее на каждом лазерном наборе сканирования, чтобы получить относительное положение между каждым сканированием. Выходные параметры от функции matchScans близко к заданным смещениям. Начальное сканирование находится в начальном кадре, таким образом, различием в положении является [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),...
        'SolverAlgorithm','fminunc','CellSize',1);
end

Используйте функцию composePoses в цикле, чтобы получить абсолютное преобразование для каждого лазерного сканирования. Эта функция задана в конце примера. Преобразуйте каждое сканирование, чтобы получить их всех в начальном кадре.

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

Постройте преобразованные области значений и углы. Они накладываются хорошо, на основе расчетных преобразований от matchScans.

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

Задайте функцию composePoses. Эта функция берет в преобразовании начального кадра к опорной раме и относительном преобразовании от начального кадра до второго кадра. Для серии лазерных сканирований вход relative является относительным положением между последними двумя кадрами, и вход base является составленным положением по всем предыдущим сканированиям.

Можно также задать эту функцию в отдельном скрипте и сохранить в текущую папку.

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

Смотрите также

matchScans | transformScan