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

Используйте 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),'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