В этом примере показано, как оценить жесткое преобразование между двумя облаками точек. В этом примере извлечение и сопоставление элементов используется для значительного уменьшения количества точек, необходимых для оценки. После использования extractFPFHFeatures для извлечения элементов быстрой точечной гистограммы (FPFH) из облаков точек используется pcmatchfeatures для поиска совпадений в извлеченных элементах. Наконец, вы используете tge estimateGeometricTransform3D функция и согласующие элементы для оценки жесткого преобразования.
Создайте два облака точек, применив жесткое преобразование к входному облаку точек.
Считывание данных облака точек в рабочее пространство.
rng("default") ptCld = pcread("highwayScene.pcd"); ptCld.Count
ans = 65536
Чтобы повысить скорость вычислений, необходимо выполнить анализ облака точек, поскольку оно содержит около 65 000 точек.
ptCloud = pcdownsample(ptCld,"gridAverage",0.2);
ptCloud.Countans = 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")