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

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