-
Notifications
You must be signed in to change notification settings - Fork 0
/
main.cpp
143 lines (126 loc) · 4.62 KB
/
main.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
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
#include <iostream>
#include <math.h>
#include <vector>
#include "src/matplotlibcpp.h" //Graph Library
using namespace std;
namespace plt = matplotlibcpp;
/*
Defining Map Characteristics
*/
// Sensor characteristic: Min and Max ranges of the beams
double Zmax = 5000, Zmin = 170;
// Defining free cells(lfree), occupied cells(locc), unknown cells(l0) log odds values
double l0 = 0, locc = 0.4, lfree = -0.4;
// Grid dimensions
double gridWidth = 100, gridHeight = 100;
// Map dimensions
double mapWidth = 30000, mapHeight = 15000;
// Robot size with respect to the map // Map area in coordinates - values to be tranlated to this range
double robotXOffset = mapWidth / 5, robotYOffset = mapHeight / 3;
// Defining an l vector to store the log odds values of each cell
vector<vector<double> > l(mapWidth / gridWidth, vector<double>(mapHeight / gridHeight));
double inverseSensorModel(double x, double y, double theta, double xi, double yi, double sensorData[])
{
// Defining Sensor Characteristics
double Zk, thetaK, sensorTheta;
double minDelta = -1;
double alpha = 200, beta = 20;
//******************Compute r and phi**********************//
double r = sqrt(pow(xi - x, 2) + pow(yi - y, 2));
double phi = atan2(yi - y, xi - x) - theta;
//Scaling Measurement to [-90 -37.5 -22.5 -7.5 7.5 22.5 37.5 90]
for (int i = 0; i < 8; i++) {
if (i == 0) {
sensorTheta = -90 * (M_PI / 180);
}
else if (i == 1) {
sensorTheta = -37.5 * (M_PI / 180);
}
else if (i == 6) {
sensorTheta = 37.5 * (M_PI / 180);
}
else if (i == 7) {
sensorTheta = 90 * (M_PI / 180);
}
else {
sensorTheta = (-37.5 + (i - 1) * 15) * (M_PI / 180);
}
if (fabs(phi - sensorTheta) < minDelta || minDelta == -1) {
Zk = sensorData[i];
thetaK = sensorTheta;
minDelta = fabs(phi - sensorTheta);
}
}
//******************Evaluate the three cases**********************//
if (r > min((double)Zmax, Zk + alpha / 2) || fabs(phi - thetaK) > beta / 2 || Zk > Zmax || Zk < Zmin) {
return l0;
}
else if (Zk < Zmax && fabs(r - Zk) < alpha / 2) {
return locc;
}
else if (r <= Zk) {
return lfree;
}
}
void occupancyGridMapping(double Robotx, double Roboty, double Robottheta, double sensorData[])
{
//******************Code the Occupancy Grid Mapping Algorithm**********************//
for (int x = 0; x < mapWidth / gridWidth; x++) {
for (int y = 0; y < mapHeight / gridHeight; y++) {
double xi = x * gridWidth + gridWidth / 2 - robotXOffset;
double yi = -(y * gridHeight + gridHeight / 2) + robotYOffset;
if (sqrt(pow(xi - Robotx, 2) + pow(yi - Roboty, 2)) <= Zmax) {
l[x][y] = l[x][y] + inverseSensorModel(Robotx, Roboty, Robottheta, xi, yi, sensorData) - l0;
}
}
}
}
void visualization()
{
//TODO: Initialize a plot named Map of size 300x150
plt::title("Final map");
plt::xlim(0, 300 );
plt::ylim(0, 150 );
int i = 300*150;
//TODO: Loop over the log odds values of the cells and plot each cell state.
//Unkown state: green color, occupied state: black color, and free state: red color
for (int x=0; x< mapWidth/gridWidth; x++) {
for(int y=0; y<mapHeight/gridHeight; y++) {
cout<< "Remaining cells to process:"<<i<<endl;
i--;
if(l[x][y]==0){
plt::plot({x},{y},"g.");
}
else if(l[x][y]>0){
plt::plot({x},{y},"k.");
}
else {
plt::plot({x},{y},"r.");
}
}
}
//TODO: Save the image and close the plot
plt::save("./Images/final_map.png");
plt::clf();
}
int main()
{
double timeStamp;
double measurementData[8];
double robotX, robotY, robotTheta;
FILE* posesFile = fopen("Data/poses.txt", "r");
FILE* measurementFile = fopen("Data/measurement.txt", "r");
// Scanning the files and retrieving measurement and poses at each timestamp
while (fscanf(posesFile, "%lf %lf %lf %lf", &timeStamp, &robotX, &robotY, &robotTheta) != EOF) {
fscanf(measurementFile, "%lf", &timeStamp);
for (int i = 0; i < 8; i++) {
fscanf(measurementFile, "%lf", &measurementData[i]);
}
occupancyGridMapping(robotX, robotY, (robotTheta / 10) * (M_PI / 180), measurementData);
}
// Visualize the map at the final step
cout << "Wait for the image to generate" << endl;
visualization();
cout << "Done!" << endl;
return 0;
}