Оцените преобразование между двумя облаками точек с помощью функций
Этот пример показывает, как оценить твердое преобразование между двумя облаками точек. В примере вы используете редукцию данных и соответствие, чтобы значительно уменьшить число точек, необходимых для оценки. После того, как вы используете 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")