Exponenta Banner

Оцените преобразование между двумя облаками точек с помощью функций

Открыть Live Script

Этот пример показывает, как оценить твердое преобразование между двумя облаками точек. В примере вы используете редукцию данных и соответствие, чтобы значительно уменьшить число точек, необходимых для оценки. После того, как вы используете extractFPFHFeatures функция для извлечения гистограммы признаков быстрой точки (FPFH) функций из облаков точек, вы используете pcmatchfeatures функция для поиска совпадений в извлеченных функциях. Наконец, вы используете tge estimateGeometricTransform3D функция и совпадающие функции для оценки твердого преобразования.

Предварительная обработка

Создайте две облака точек путем применения твердого преобразования к входу облаку точек.

Считайте данные облака точек в рабочую область.

rng("default")
ptCld = pcread("highwayScene.pcd");
ptCld.Count
ans = 65536

Понижайте значение облака точек, чтобы улучшить скорость расчетов, так как он содержит около 65 000 точек.

ptCloud = pcdownsample(ptCld,"gridAverage",0.2);
ptCloud.Count
ans = 24596

Создайте твердую матрицу преобразования с поворотом на 30 градусов и перемещением на 5 модули в осях X и оси Y

rotAngle = 30;
rot = [cosd(rotAngle)  sind(rotAngle) 0; ...
      -sind(rotAngle)  cosd(rotAngle) 0; ...
            0           0       1];
trans = [5 5 0];
tform = rigid3d(rot,trans);

Преобразуйте входное облако точек.

ptCloudTformed = pctransform(ptCloud,tform);

Визуализация двух облаков точек.

pcshowpair(ptCloud,ptCloudTformed)
xlim([-50 75])
ylim([-40 80])
legend("Original", "Transformed","TextColor",[1 1 0])

Редукция данных

Извлечение функций из обоих облаков точек с помощью extractFPFHFeatures функция.

fixedFeature = extractFPFHFeatures(ptCloud);
movingFeature = extractFPFHFeatures(ptCloudTformed);

Найдите соответствующие функции и отобразите количество совпадающих пар. 

[matchingPairs,scores] = pcmatchfeatures(fixedFeature,movingFeature, ...
    ptCloud,ptCloudTformed,"Method","Exhaustive");
length(matchingPairs)
ans = 1814

Выберите подходящие точки из облаков точек.

fixedPts = select(ptCloud,matchingPairs(:,1));
matchingPts = select(ptCloudTformed,matchingPairs(:,2));

Оцените матрицу преобразования, используя совпадающие точки.

estimatedTform = estimateGeometricTransform3D(fixedPts.Location, ...
    matchingPts.Location,"rigid");
disp(estimatedTform.T)
    0.8660    0.5000    0.0002         0
   -0.5000    0.8660   -0.0002         0
   -0.0003    0.0000    1.0000         0
    4.9995    5.0022    0.0020    1.0000

Отобразите определенную матрицу преобразования.

disp(tform.T)
    0.8660    0.5000         0         0
   -0.5000    0.8660         0         0
         0         0    1.0000         0
    5.0000    5.0000         0    1.0000

Используйте предполагаемое преобразование для повторного преобразования ptCloudTformed назад к начальному облаку точек.

ptCloudTformed = pctransform(ptCloudTformed,invert(estimatedTform));

Визуализация двух облаков точек.

pcshowpair(ptCloud,ptCloudTformed)
xlim([-50 50])
ylim([-40 60])
title("Aligned Point Clouds")

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

Поддержка