increaseGranularityOfMap/increaseMapGranularity.cpp at master · arangatang/increaseGranularityOfMap · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
#include <vector>
#include <iostream>
using namespace std;
int main(){
    float value;
    int height = 3;
    int width = 3;
    int index = 0;
    float map[] = { 0,  0,  0,
                    0  ,100,  0,
                    100,  0,  100};
    for(int i = 0; i<3; i++){
        for(int j=0; j<3; j++){
            cout << map[index++] << "\t";
        }
        cout << endl;
    }

    cout << endl;

    // increase the granularity of the map by 4
    index = 0;
    std::vector<std::vector<float> > occupancyGrid, tmp;
    for (int i = 0; i < height; i++) {
        std::vector<float> row1,row2,row3,row4;
        for (int j = 0; j < width; j++) {
            row1.push_back(map[index]);
            row1.push_back(map[index]);
            row1.push_back(map[index]);
            row1.push_back(map[index]);

            row2.push_back(map[index]);
            row2.push_back(map[index]);
            row2.push_back(map[index]);
            row2.push_back(map[index]);

            row3.push_back(map[index]);
            row3.push_back(map[index]);
            row3.push_back(map[index]);
            row3.push_back(map[index]);

            row4.push_back(map[index]);
            row4.push_back(map[index]);
            row4.push_back(map[index]);
            row4.push_back(map[index]);
            index++;
        }
        occupancyGrid.push_back(row1);
        occupancyGrid.push_back(row2);
        occupancyGrid.push_back(row3);
        occupancyGrid.push_back(row4);
    }
    tmp = occupancyGrid;

    cout << "rows: " << occupancyGrid.size() << " cols: " << occupancyGrid.at(0).size() << endl;
    for(int i = 0; i<occupancyGrid.size(); i++){
        for(int j=0; j<occupancyGrid.at(0).size(); j++){
            cout << occupancyGrid.at(i).at(j) << "\t";
        }
        cout << endl;
    }

    for(int i = 0; i<occupancyGrid.size(); i++){
        for(int j=0; j<occupancyGrid.at(0).size(); j++){
            double value = occupancyGrid.at(i).at(j);
            // the row above
            if(value > 0){
                try{
                    tmp.at(i-1).at(j-1) = value;
                } catch (...){};
                try{
                    tmp.at(i-1).at(j) = value;
                } catch(...){};
                try{
                    tmp.at(i-1).at(j+1) = value;
                } catch(...){};

                // this row
                try{
                    tmp.at(i).at(j-1) = value;
                } catch(...){};
                try{
                    tmp.at(i).at(j) = value;
                } catch(...){};
                try{
                    tmp.at(i).at(j+1) = value;
                } catch(...){};

                // row below
                try{
                    tmp.at(i+1).at(j-1) = value;
                } catch(...){};
                try{
                    tmp.at(i+1).at(j) = value;
                } catch(...){};
                try{
                    tmp.at(i+1).at(j+1) = value;
                } catch(...){};
            }
        }
    }

    occupancyGrid = tmp;

    cout << "rows: " << occupancyGrid.size() << " cols: " << occupancyGrid.at(0).size() << endl;
    for(int i = 0; i<occupancyGrid.size(); i++){
        for(int j=0; j<occupancyGrid.at(0).size(); j++){
            cout << occupancyGrid.at(i).at(j) << "\t";
        }
        cout << endl;
    }
    return 0;
}