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