在网格上对齐点云

Question

我必须测量两朵云对应点的 Z 距离。 我打算遍历一个云并使用另一云的相同 X 和 Y 计算 Z 坐标之间的距离。 不幸的是，它不起作用，因为在第二个云中的这些 X-Y 坐标上从来没有一个点。我目前的解决方法是在第二个云中搜索第一个云的 X-Y 的最近点。它可以工作，但速度很慢。

有没有办法使用 PCL 在定义的网格上对齐 X 和 Y 坐标点？这样我希望 X-Y 坐标匹配得更好。

编辑 好的，这里有一些图片和更多解释。

顶视图

侧视图

有一个马鞍和马背的扫描图。两者都是独立制作的，但在 Z 轴上对齐 - 两者的 Z 轴是平行的。 我想创建一个图层模型，它正好适合马鞍（不仅仅是一个矩形垫）。

所以给定层的厚度，我想遍历鞍点并找到马背上对应点的 Z 距离。由于 Y 坐标是浮动的，因此马身上几乎没有一个点与马鞍上的 XY 相同。

我想。如果我可以将所有点与给定密度的网格对齐，那么马上面的每个 XY 鞍点都会有一个对应的 XY 点。

Answer 1

我不确定这是否是您的意思，但也许您所说的“网格”可能就是image plane？因此，您可以使用深度图/深度图像，而不是使用 3D 点云，只需比较相同图像坐标处的两个深度图的值。这将假定记录已经对齐。

如果您只有点云数据，则必须在平面上执行投影（为此，您必须了解相机的内在函数）。

另一个选项可能是使用注册方法（例如ICP）对齐云。然后你也可以get the (sum of) distance(s)获取云的对应点。

Answer 2

我已经实现了概念验证并想分享它。不过，我希望有一个“正确”的解决方案——可能是一个 PCL API 函数。

bool alignToGrid( pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud, QMap<QString, float > & grid, int density )
{
    pcl::PointXYZRGBNormal p1;
    p1.r=0;
    p1.g=0;
    p1.b=255;
    QMap<QString, QList<float> > tmpGridMap;
    for( std::vector<pcl::PointXYZRGBNormal, Eigen::aligned_allocator<pcl::PointXYZRGBNormal> >::iterator it1 = cloud->points.begin();
         it1 != cloud->points.end(); it1++ )
    {
        p1.x = it1->x;
        p1.y = it1->y;
        p1.z = it1->z;
        int gridx = p1.x*density;
        int gridy = p1.y*density;
        QString pos = QString("%1x%2").arg(gridx).arg(gridy);
        tmpGridMap[pos].append(p1.z);
    }

    for (QMap<QString, QList<float> >::iterator it =  tmpGridMap.begin(); it!=tmpGridMap.end(); ++it)
    {
        float meanZ=0;

        foreach( float f, it.value() )
        {
            meanZ+=f;
        }
        meanZ /= it.value().size();


        grid[it.key()] = meanZ;
    }
    return true;
}

想法是遍历云并仅留下/创建点，XY 坐标位于定义的网格上。 Kinect 云的 Density 1000 结果大约为 1000。 1 毫米网格。 网格点周围的所有点都用于构建 Z 平均值。 云保持不变。输出是 xy 位置到 Z 的映射。XY 位置存储在字符串中（我知道很奇怪）作为 x。使用这张地图很容易在其他网格对齐的云中找到对应的 XY 点。

现在我可以使用任何密度绘制云图。在图像中，例如1 毫米和 1 厘米。