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