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

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