-
Notifications
You must be signed in to change notification settings - Fork 1
/
Copy pathMap.cpp
94 lines (80 loc) · 3.11 KB
/
Map.cpp
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
#include "Map.h"
Map::Map(OccupancyGrid &ogrid) : ogrid(ogrid), matrix(ogrid.getWidth(), ogrid.getHeight(), CV_8UC3, cv::Scalar(255, 255, 255)) {
cv::namedWindow("Map");
}
void Map::initMap() {
unsigned char pixel;
for (int i = 250; i < ogrid.getWidth()-350; i++) {
for (int j = 250; j < ogrid.getHeight()-350; j++) {
if (ogrid.getCell(i, j) == CELL_FREE)
{
pixel = 255;//white
}
else if (ogrid.getCell(i, j) == CELL_OCCUPIED)
{
pixel = 0;//black
}
else if (ogrid.getCell(i, j) == CELL_UNKNOWN)
pixel = 128;//grey
matrix.at<cv::Vec3b>(2*(i-250),2*(j-250))[0] = pixel;
matrix.at<cv::Vec3b>(2*(i-250),2*(j-250))[1] = pixel;
matrix.at<cv::Vec3b>(2*(i-250),2*(j-250))[2] = pixel;
matrix.at<cv::Vec3b>(2*(i-250)+1,2*(j-250))[0] = pixel;
matrix.at<cv::Vec3b>(2*(i-250)+1,2*(j-250))[1] = pixel;
matrix.at<cv::Vec3b>(2*(i-250)+1,2*(j-250))[2] = pixel;
matrix.at<cv::Vec3b>(2*(i-250),2*(j-250)+1)[0] = pixel;
matrix.at<cv::Vec3b>(2*(i-250),2*(j-250)+1)[1] = pixel;
matrix.at<cv::Vec3b>(2*(i-250),2*(j-250)+1)[2] = pixel;
matrix.at<cv::Vec3b>(2*(i-250)+1,2*(j-250)+1)[0] = pixel;
matrix.at<cv::Vec3b>(2*(i-250)+1,2*(j-250)+1)[1] = pixel;
matrix.at<cv::Vec3b>(2*(i-250)+1,2*(j-250)+1)[2] = pixel;
}
}
}
void Map::drawParticles(vector<Particle *> particles) {
initMap();
int size = particles.size();
int best = size * 0.5;
int i = 0;
for (; i < size - best; i++) {//draw the low-belief half in red color
int x = particles[i]->i;
int y = particles[i]->j;
matrix.at<cv::Vec3b>(2*(x-250),2*(y-250))[0] = 0;
matrix.at<cv::Vec3b>(2*(x-250),2*(y-250))[1] = 0;
matrix.at<cv::Vec3b>(2*(x-250),2*(y-250))[2] = 255;
matrix.at<cv::Vec3b>(2*(x-250)+1,2*(y-250))[0] = 0;
matrix.at<cv::Vec3b>(2*(x-250)+1,2*(y-250))[1] = 0;
matrix.at<cv::Vec3b>(2*(x-250)+1,2*(y-250))[2] = 255;
matrix.at<cv::Vec3b>(2*(x-250),2*(y-250)+1)[0] = 0;
matrix.at<cv::Vec3b>(2*(x-250),2*(y-250)+1)[1] = 0;
matrix.at<cv::Vec3b>(2*(x-250),2*(y-250)+1)[2] = 255;
matrix.at<cv::Vec3b>(2*(x-250)+1,2*(y-250)+1)[0] = 0;
matrix.at<cv::Vec3b>(2*(x-250)+1,2*(y-250)+1)[1] = 0;
matrix.at<cv::Vec3b>(2*(x-250)+1,2*(y-250)+1)[2] = 255;
}
for (; i < size; i++) {//draw the high-belief half in blue color
int x = particles[i]->i;
int y = particles[i]->j;
matrix.at<cv::Vec3b>(2*(x-250),2*(y-250))[0] = 255;
matrix.at<cv::Vec3b>(2*(x-250),2*(y-250))[1] = 0;
matrix.at<cv::Vec3b>(2*(x-250),2*(y-250))[2] = 0;
matrix.at<cv::Vec3b>(2*(x-250)+1,2*(y-250))[0] = 255;
matrix.at<cv::Vec3b>(2*(x-250)+1,2*(y-250))[1] = 0;
matrix.at<cv::Vec3b>(2*(x-250)+1,2*(y-250))[2] = 0;
matrix.at<cv::Vec3b>(2*(x-250),2*(y-250)+1)[0] = 255;
matrix.at<cv::Vec3b>(2*(x-250),2*(y-250)+1)[1] = 0;
matrix.at<cv::Vec3b>(2*(x-250),2*(y-250)+1)[2] = 0;
matrix.at<cv::Vec3b>(2*(x-250)+1,2*(y-250)+1)[0] = 255;
matrix.at<cv::Vec3b>(2*(x-250)+1,2*(y-250)+1)[1] = 0;
matrix.at<cv::Vec3b>(2*(x-250)+1,2*(y-250)+1)[2] = 0;
}
}
void Map::showMap() {
// Show the matrix on the window
cv::imshow("Map", matrix);
// Delay for 1 millisecond to allow the window to process
// incoming events
cv::waitKey(1);
}
Map::~Map() {
}