这个例子展示了如何估计两个点云之间的刚性转换。在本例中,您使用特征提取和匹配来显著减少估计所需的点的数量。在你使用extractFPFHFeatures
函数从点云中提取快速点特征直方图(FPFH)特征,使用pcmatchfeatures
函数在提取的特征中搜索匹配。最后,你使用estimateGeometricTransform3D
函数和匹配特征来估计刚性变换。
通过对输入点云应用刚性转换来创建两个点云。
将点云数据读入工作区。
rng (“默认”) ptCld = pcread(“highwayScene.pcd”);ptCld。数
ans = 65536
向下采样点云以提高计算速度,因为它包含大约65,000个点。
ptCloud = pcdownsample (ptCld,“gridAverage”, 0.2);ptCloud。数
ans = 24596
创建一个具有30度旋转和5个单位平移的刚性变换矩阵x- - -y -轴。
rotAngle = 30;rot = [cosd(rotAngle) sind(rotAngle) 0;...信德(rotAngle) cosd (rotAngle) 0;...0 0 1);Trans = [5 5 0];tform = rigid3d(腐烂,反式);
转换输入点云。
ptCloudTformed = pctransform (ptCloud tform);
想象这两个点云。
pcshowpair(ptCloud,ptCloudTformed) xlim([-50 75]) ylim([-40 80]) legend(“原始”,“转换”,“输入TextColor”(1 1 0))
从两个点云提取特征使用extractFPFHFeatures
函数。
fixedFeature = extractFPFHFeatures (ptCloud);movingFeature = extractFPFHFeatures (ptCloudTformed);
查找匹配特征并显示匹配对的数量。
[matchingPairs,分数]= pcmatchfeatures (fixedFeature movingFeature,...ptCloud ptCloudTformed,“方法”,“详尽”);长度(matchingPairs)
ans = 1814
从点云中选择匹配点。
fixedPts =选择(ptCloud matchingPairs (: 1));matchingPts =选择(ptCloudTformed matchingPairs (:, 2));
利用匹配点估计变换矩阵。
estimatedTform = estimateGeometricTransform3D (fixedPts。的位置,...matchingPts。的位置,“刚性”);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 00 -0.5000 0.8660 0000 1.0000 0 5.0000 5.0000 0 1.0000
使用估计的转换来重新转换ptCloudTformed
回到最初的点云。
ptCloudTformed = pctransform (ptCloudTformed,反(estimatedTform));
想象这两个点云。
pcshowpair(ptCloud,ptCloudTformed) xlim([-50 50]) ylim([-40 60]) title(“对齐点云”)