COccupancyGridMap2D中的insertObservation方法产生扭曲结果

Question

insertObservation中的COccupancyGridMap2D函数接受两个参数，即CPose3D和CObservation2DRangeScan值，即使这两个值都准确无噪音，网格也是如此正在产生翘曲的边界。我唯一能想到的是scan.aperture设置可能会产生这种效果，但这些设置是正确的，范围为2 * PI，点云的其他视觉助手根本没有显示翘曲。以下是对此的说明。

在右侧，占地网格与地面实况边界相比变形。左边的点看起来很好，并使用相同的光圈并加载FromVectors设置。

以下是尝试验证自身扭曲效果的示例代码。

COccupancyGridMap2D gridmap;
gridmap.setSize(-4.0,4.0,-4.0,4.0,0.025f);

#define SCANS_SIZE 100
char SCAN_VALID[] = {1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1};

CPose3D transform = CPose3D(0,0,0,0,0,0);

CObservation2DRangeScan read_scan;

read_scan.aperture = 2*M_PIf;
read_scan.rightToLeft = true;


vector<float> landmark = {2.9f,2.906f,2.924f,2.953f,2.996f,3.052f,3.124f,3.212f,3.319f,3.447f,3.601f,3.786f,4.007f,3.948f,3.736f,3.560f,3.413f,3.290f,3.188f,3.104f,3.037f,2.984f,2.945f,2.918f,2.903f,2.900f,2.909f,2.930f,2.963f,3.009f,3.069f,3.144f,3.237f,3.349f,3.483f,3.644f,3.837f,4.069f,3.891f,3.689f,3.521f,3.380f,3.263f,3.166f,3.086f,3.022f,2.973f,2.937f,2.913f,2.901f,2.901f,2.913f,2.937f,2.973f,3.022f,3.086f,3.166f,3.263f,3.380f,3.521f,3.689f,3.891f,4.069f,3.837f,3.644f,3.483f,3.349f,3.237f,3.144f,3.069f,3.009f,2.963f,2.930f,2.909f,2.900f,2.903f,2.918f,2.945f,2.984f,3.037f,3.104f,3.188f,3.290f,3.413f,3.560f,3.736f,3.948f,4.007f,3.786f,3.601f,3.447f,3.319f,3.212f,3.124f,3.052f,2.996f,2.953f,2.924f,2.906f,2.900f};
float *SCAN_RANGES = &landmark[0];

read_scan.loadFromVectors(SCANS_SIZE, SCAN_RANGES,SCAN_VALID);

gridmap.insertObservation(&read_scan,&transform);

CSimplePointsMap m3;
m3.insertObservation(&read_scan);

m3.getAllPoints(map_xs,map_ys,map_zs);

Here is a image of the simplePointsMap plot (red points) vs the OccupanyGrid

从占用网格投射的角度看起来是正确的，具有一致的间隔，但角度仍然偏离简单点地图，长度看起来不错，似乎每条光线都可以旋转以匹配其中一个红点。可能会发生的是一个映射问题，因为我们试图将角度分成水平和垂直的离散步骤，这会导致错位。我已经尝试过增加分辨率，但这没有用，我想这是有道理的，因为缩放水平/垂直比率仍然会导致相同的比率和不匹配。我可能会遗漏一些东西，还有什么可能导致这种失真，这是预期的，我们能做的最好吗？谢谢你的帮助。

Answer 1

似乎对我来说，问题在于假设每次扫描的角度＆＃34; ray＆＃34;。

查看类mrpt::obs::CSinCosLookUpTableFor2DScans，为您的特定扫描对象生成一个这样的sin / cos LUT，并仔细检查sin / cos值是否与您的一致，如用于生成扫描。

顺便说一下，COccupancyGridMap2D有一种方法可以从网格图图像模拟2D扫描，试一试，如果那个产生扭曲的结果，请填写错误报告（！）;-)

干杯。

Answer 2

我刚刚意识到发生了什么，CSimplePointsMap和COccupancyGridMap2D使用了两个稍微不同的点角度参考。 CSimplePointsMap期望在第一个和最后一个点之间重叠，而COccupancyGridMap2D则不是。所有这一切的简单修复就是在COccupancyGridMap2D中少读一次扫描，然后所有内容排成一行。这是因为你的角度被定义为如此，这对CSimplePointsMap来说很好。

for (int i = 0; i < Raysize; i++) 
{

    float angle = -angle_range / 2 + i * (angle_range) / (Raysize-1);

以下是使用SCANS_SIZE-1修复OccupancyGridMap2D insertObservation，而CSimplePointsMap仍然可以使用SCANS_SIZE。

read_scan.loadFromVectors(SCANS_SIZE-1, SCAN_RANGES,SCAN_VALID);
gridmap.insertObservation(&read_scan,&transform);