Get grid cell from world coordinates

[SKroell]

Hi

I'm currently using RTABMAP in a SLAM project. It's written fully in C++, so no ROS.
I've started to look into implementing autonomous exploration, in this case frontier exploration. Currently I'm stuck at figuring out how I'm going to convert between world coordinates and cells in the 2D Occupancy Grid.

In short I'm looking for a way to implement a method that given a position, will return the cell information in the occupancy grid in that position.

I hope that makes sense, else let me know and I'll try to rephrase it.

- Stefan

[SKroell]

This is what I got so far, but it's seems that I've flipped it somehow. (see attached screenshot)

bool Navigation::worldToMap(double wx, double wy, unsigned int& mx, unsigned int& my) {
    float minX, minY;
    cv::Mat map = grid_.getMap(minX, minY);

    if (wx < minX || wy < minY)
      return false;

    mx = (int)((wx - minX) / grid_.getCellSize());
    my = (int)((wy - minY) / grid_.getCellSize());

    if (mx < map.cols && my < map.rows)
      return true;

    return false;
}

void Navigation::mapToWorld(int mx, int my, double& wx, double& wy) {
    // returns the center point of the cell
    float minX, minY;
    cv::Mat map = grid_.getMap(minX, minY);
    wx = minX + (mx + 0.5) * grid_.getCellSize();
    wy = minY + (my + 0.5) * grid_.getCellSize();
}

[SKroell]

Okay so I figured it out myself. Issues was not with any of the code, but how I calculate logodds for cells. I've simply forgot that cv::Mat are indexed row,collumn (y,x) and not (x,y)