[Monte Carlo Laser Loc] bugs in poseToMap

[franmore-urjc]

The method poseToMap from the Monte Carlo Laser Localization exercise returns floating numbers, which are not valid map coordinates. One would expect to receive the indices of the map cell corresponding to the given pose.

Moreover, the methods poseToMap and mapToPose are not symmetrical: If one creates a cell in map coordinates, converts it to a pose, and then converts it back to map coordinates, the values are different. You can see an example in this image:

From the code related to this functions, I see that the offset applied when doing the transformation is not the same in both directions:

    def poseToMap(self, x_prime, y_prime, yaw_prime):
        x = 101.1 * ( 4.2 + y_prime)
        y = 101.1  * ( 5.7 - x_prime)
        yaw = yaw_prime + math.pi/2
        return [x, y, yaw]

    def mapToPose(self, map_x, map_y, map_yaw):
        x = (map_y - 576.27) / -101.1
        y = (map_x - 425.25) /  101.1
        yaw = map_yaw - math.pi/2
        return [x, y, yaw]

I don't know where do these numbers come from, so I don't know which one is the correct offset. Could you please fix this?

[javizqh]

Thanks for noticing, the error was that 425.25 was the for the old map. Now it is 424.62 (4.2 * 101.1).