CODEWITHSUNDEEP
c++data-structuresrosrobotics
VansFannel
VansFannel

Reputation: 45921

Data structure to store a grid (that will have negative indices)

I'm studying robotics at the university and I have to implement on my own SLAM algorithm. To do it I will use ROS, Gazebo and C++.

I have a doubt about what data structure I have to use to store the map (and what I'm going to store it, but this is another story).

I have thought to represent the map as a 2D grid and robot's start location is (0,0). But I don't know where exactly is the robot on the world that I have to map. It could be at the top left corner, at the middle of the world, or in any other unknonw location inside the world.

Each cell of the grid will be 1x1 meters. I will use a laser to know where are the obstacles. Using current robot's location, I will set to 1 on all the cells that represent an obstacle. For example, it laser detects an obstacle at 2 meters in front of the robot, I will set to 1 the cell at (0,2).

Using a vector, or a 2D matrix, here is a problem, because, vector and matrices indices start at 0, and there could be more room behind the robot to map. And that room will have an obstacle at (-1,-3).

On this data structure, I will need to store the cells that have an obstacle and the cells that I know they are free.

Which kind of data structure will I have to use?

UPDATE:

The process to store the map will be the following:

  1. Robot starts at (0,0) cell. It will detect the obstacles and store them in the map.
  2. Robot moves to (1,0) cell. And again, detect and store the obstacles in the map.
  3. Continue moving to free cells and storing the obstacles it founds.

The robot will detect the obstacles that are in front of it and to the sides, but never behind it.

My problem comes when the robot detects an obstacle on a negative cell (like (0,-1). I don't know how to store that obstacle if I have previously stored only the obstacle on "positive" cells. So, maybe the "offset", it is not a solution here (or maybe I'm wrong).

Upvotes: 2

Views: 1755

Answers (6)

Humphrey
Humphrey

Reputation: 9

I think I see what you are after here: you don't know how big the space is, or even what the coordinates may be.

This is very general, but I would create a class that holds all of the data using vectors (another option -- vector of pairs, or vector of Eigen (the library) vectors). As you discover new regions, you'll add the coordinates and occupancy information to the Map (via AddObservation(), or something similar).

Later, you can determine the minimum and maximum x and y coordinates, and create the appropriate grid, if you like. 

class RoboMap{
public:
    vector<int> map_x_coord;
    vector<int> map_y_coord;
    vector<bool> occupancy;

    RoboMap();


    void AddObservation(int x, int y, bool in_out){
        map_x_coord.push_back(x);
        map_y_coord.push_back(y);

        occupancy.push_back(in_out);
    }

};

Upvotes: 0

Flaxcode
Flaxcode

Reputation: 1

Class robotArray

{

Int* left,right;

}

RobotArray::RobotArray ()

{

Int* a=new int [50][50];

Int* b=new int[50][50];

//left for the -ve space and right for the positive space with

0,0 of the two arrays removed

Left=a+1;

Right=b+1;

}

Upvotes: 0

Tarick Welling
Tarick Welling

Reputation: 3255

You can use a std::set to represent a grid layout by using a position class you create. It contains a x and y variable and can therefore be used to intuitively be used to find points inside the grid. You can also use a std::map if you want to store information about a certain location inside the grid.

Please don't forget to fulfill the C++ named requirements for set/map such as Compare if you don't want to provide a comparison operator externally.

example: position.h

/* this class is used to store the position of things
 * it is made up by a horizontal and a vertical position.
 */
class position{
private:
    int32_t horizontalPosition;
    int32_t verticalPosition;
public:
    position::position(const int hPos = 0,const int vPos = 0) : horizontalPosition{hPos}, verticalPosition{vPos}{}
    position::position(position& inputPos) : position(inputPos.getHorPos(),inputPos.getVerPos()){}
    position::position(const position& inputPos) : position((inputPos).getHorPos(),(inputPos).getVerPos()){}

    //insertion operator, it enables the use of cout on this object: cout << position(0,0) << endl;
    friend std::ostream& operator<<(std::ostream& os, const position& dt){
        os << dt.getHorPos() << "," << dt.getVerPos();
        return os;
    }

    //greater than operator
    bool operator>(const position& rh) const noexcept{
        uint64_t ans1 = static_cast<uint64_t>(getVerPos()) | static_cast<uint64_t>(getHorPos())<<32;
        uint64_t ans2 = static_cast<uint64_t>(rh.getVerPos()) | static_cast<uint64_t>(rh.getHorPos())<<32;

        return(ans1 < ans2);
    }

    //lesser than operator
    bool operator<(const position& rh) const noexcept{
        uint64_t ans1 = static_cast<uint64_t>(getVerPos()) | static_cast<uint64_t>(getHorPos())<<32;
        uint64_t ans2 = static_cast<uint64_t>(rh.getVerPos()) | static_cast<uint64_t>(rh.getHorPos())<<32;

        return(ans1 > ans2);
    }

    //equal comparison operator
    bool operator==(const position& inputPos)const noexcept {
        return((getHorPos() == inputPos.getHorPos()) && (getVerPos() == inputPos.getVerPos()));
    }

    //not equal comparison operator
    bool operator!=(const position& inputPos)const noexcept {
        return((getHorPos() != inputPos.getHorPos()) || (getVerPos() != inputPos.getVerPos()));
    }

    void movNorth(void) noexcept{
        ++verticalPosition;
    }
    void movEast(void) noexcept{
        ++horizontalPosition;
    }
    void movSouth(void) noexcept{
        --verticalPosition;
    }
    void movWest(void) noexcept{
        --horizontalPosition;
    }

    position getNorthPosition(void)const noexcept{
        position aPosition(*this);
        aPosition.movNorth();
        return(aPosition);
    }
    position getEastPosition(void)const noexcept{
        position aPosition(*this);
        aPosition.movEast();
        return(aPosition);
    }
    position getSouthPosition(void)const noexcept{
        position aPosition(*this);
        aPosition.movSouth();
        return(aPosition);
    }
    position getWestPosition(void)const noexcept{
        position aPosition(*this);
        aPosition.movWest();
        return(aPosition);
    }

    int32_t getVerPos(void) const noexcept {
        return(verticalPosition);
    }
    int32_t getHorPos(void) const noexcept {
        return(horizontalPosition);
    }
};

std::set<position> gridNoData;
std::map<position, bool> gridWithData;

gridNoData.insert(point(1,1));
gridWithData.insert(point(1,1),true);

gridNoData.insert(point(0,0));
gridWithData.insert(point(0,0),true);

auto search = gridNoData.find(point(0,0));
if (search != gridNoData.end()) {
    std::cout << "0,0 exists" << '\n';
} else {
    std::cout << "0,0 doesn't exist\n";
}

auto search = gridWithData.find(point(0,0));
if (search != gridWithData.end()) {
    std::cout << "0,0 exists with value" << search->second  << '\n';
} else {
    std::cout << "0,0 doesn't exist\n";
}


The above class was used by me in a similar setting and we used a std::map defined as: 

std::map<position,directionalState> exploredMap;

To store if we had found any walls at a certain position.
By using this std::map based method you avoid having to do math to know what offset you have to have inside an 2D array (or some structure like that). It also allows you to move freely as there is no chance that you'll travel outside of the predefined bounds you set at construction. This structure is also more space efficient against a 2D array as this structure only saves the areas where the robot has been. This is also a C++ way of doing things: relying on the STL instead of creating your own 2D map using C constructs.

Upvotes: 1

Ped7g
Ped7g

Reputation: 16596

With offset solution (translation of values by fixed formula (we called it "mapping function" in math class), like doing "+50" to all coordinates, i.e. [-30,-29] will become [+20,+21] and [0,0] will become [+50,+50] ) you still need to have idea what is your maximum size.

In case you want to be dynamic like std::vector<> going from 0 to some N (as much as free memory allows), you can create more complex mapping function, for example map(x) = x*2 when (0 <= x) and x*(-2)-1 when (x < 0) ... this way you can use standard std::vector and let it grow as needed by reaching new maximum coordinates.

With 2D grid vs std::vector this is a bit more complicated as vector of vectors is sometimes not the best idea from performance point of view, but as long as your code can prefer shortness and simplicity over performance, maybe you can use the same mapping for both coordinates and use vector of vectors (using reserve(..) on all of them with some reasonable default to avoid resizing of vectors in common use cases, like if you know the 100m x 100m area will be usual maximum, you can reserve everything to capacity 201 initially to avoid vector resizing for common situations, but it can still grow infinitely (until heap memory is exhausted) in less common situations.

You can also add another mapping function converting 2D coordinates to 1D and use single vector only, and if you want really complicate things, you can for example map those 2D into 0,1,2,... sequence growing from area around center outward to save memory usage for small areas... you will probably easily spend 2-4 weeks on debugging it, if you are kinda fresh to C++ development, and you don't use unit testing and TDD approach (I.e. just go by simple vector of vectors for a start, this paragraph is JFYI, how things can get complicated if you are trying to be too smart :) ).

Upvotes: 0

Jeffrey
Jeffrey

Reputation: 11410

The options you have:

  • Have an offset. Simple and dirty. Your grid is 100x100 but stores -50,-50 to 50x50.
  • Have multiple offset'ed grids. When you go out of the grid allocate a new one beside it, with a different offset. A list or map of grids.
  • Have sparse structure. A set or map of coordinates.
  • Have an hierarchical structure. Your whole, say 50x50, grid is one cell in a grid at a higher level. Implement it with a linked list or something so when you move you build a tree of nest grids. Very efficient for memory and compute time, but much more complex to implement.

Upvotes: 1

Paul Evans
Paul Evans

Reputation: 27577

This is where you can write a class to help you:

class RoboArray 
{
     constexpr int width_ = ...
     constexpr int height_ = ...
     Cell grid_[width_ * 2][height_ * 2];
     ...
 public:
     ...
     Cell get(int x, int y) // can make this use [x][y] notation with a helper class
     {
           return grid_[x + width_][y + height];
     }
     ...
}

Upvotes: 1

Related Questions