insertObservation method in COccupancyGridMap2D is producing warped results - mobile-robot-toolkit

Question

The function insertObservation in COccupancyGridMap2D takes in two parameters which are the CPose3D and CObservation2DRangeScan values, even though both of these values are accurate with no noise, the grid is producing warped boundaries. The only thing I can think of is the scan.aperture settings might be producing this effect but these are correct with a range of 2*PI and other visual aides for point clouds show no warpage at all. Below is an illustration of this.
On the right the occupancy grid is warped compared to the ground truth square boundary. The left points look fine and are using the same aperture and load FromVectors settings.
Here is example code to try to verify the warp effect your self.
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
The angles being casted from the occupany grid look correct, with a consistent interval, but the angle is still off from simplepoints map, length looks ok and it seems each ray could be rotated to match with one of the red points. Possibly what could be happening is a mapping issue, and since we try to make the angles into discrete horizontal and vertical steps this causes the misalignment. I've tried increasing the resolution but this does not help, I guess that makes sense since scaling a horizontal/vertical ratio would still result in the same ratio and mismatch. I might be missing something though, what else could be causing this distortion, is this expected and the best we can do? Thank you for any help.

Answer 1

It seems to me that the problem is in the assumption of which are the angles of each scan "ray".
Take a look at the class mrpt::obs::CSinCosLookUpTableFor2DScans, generate one such sin/cos LUT for your specific scan object, and double check if the sin/cos values coincide with yours, as used to generate the scan.
By the way, COccupancyGridMap2D has one method to simulate a 2D scan from a gridmap image, give it a try, and if that one generates warped results, please fill up a bug report (!) ;-)
Cheers.

Answer 2

I just realized what was going on, CSimplePointsMap and COccupancyGridMap2D use two slightly different references for point angles. CSimplePointsMap is expecting an overlap between the first and last point while COccupancyGridMap2D is not. The simple fix to all of this then is to read in one less scan for the COccupancyGridMap2D and then everything lines up. This is if your angles are being defined as so, which is fine for CSimplePointsMap.
for (int i = 0; i < Raysize; i++)
{
float angle = -angle_range / 2 + i * (angle_range) / (Raysize-1);
Here is the fix for OccupancyGridMap2D insertObservation using SCANS_SIZE-1 instead and CSimplePointsMap can still use SCANS_SIZE.
read_scan.loadFromVectors(SCANS_SIZE-1, SCAN_RANGES,SCAN_VALID);
gridmap.insertObservation(&read_scan,&transform);