Occupancy grid algorithm implementation in Cpp

mapping_slam/occupancy_grid_mapping/OccupancyGridMappingAlgorithm/LICENSE

@@ -0,0 +1,21 @@
+MIT License
+
+Copyright (c) 2018 Udacity
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.

mapping_slam/occupancy_grid_mapping/OccupancyGridMappingAlgorithm/README.md

@@ -0,0 +1,47 @@
+[![Udacity - Robotics NanoDegree Program](https://s3-us-west-1.amazonaws.com/udacity-robotics/Extra+Images/RoboND_flag.png)](https://www.udacity.com/robotics)
+
+# RoboND-OccupancyGridMappingAlgorithm
+You will visualize the mapped environment through the generated image
+
+### Instruction
+Code the visualization function which will plot the state of each grid cell using the matplotlib python library
+``` C++
+void visualization()
+{
+    //TODO: Initialize a plot named Map of size 300x150
+
+    //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 
+
+    //TODO: Save the image and close the plot 
+}
+```
+Here are some helpful commands you can use to generate plots with the `matplotlib` library:
+* *Set Title*: `plt::title("Your Title");`
+* *Set Limits*: `plt::xlim(x-axis lower limit, x-axis upper limit );`
+* *Plot Data*:`plt::plot({ x-value }, { y-value }, "Color and Shape");`
+* *Save Plot*: `plt::save("File name and directory")`;
+* *Close Plot*:   `plt::clf()`;
+
+Check out this [link](https://github.com/lava/matplotlib-cpp) for more information on the `matplotlib` C++ library. For information regarding the plot color and shape refer to the LineSpec and LineColor section of the [MATLAB](https://www.mathworks.com/help/matlab/ref/plot.html?requestedDomain=true) documentation. 
+
+### Compiling
+```sh
+$ cd /home/workspace/
+$ git clone https://github.com/udacity/RoboND-OccupancyGridMappingAlgorithm
+$ cd RoboND-OccupancyGridMappingAlgorithm/
+$ rm -rf Images/* #Delete the folder content and not the folder itself!
+$ g++ main.cpp -o app -std=c++11 -I/usr/include/python2.7 -lpython2.7
+```
+
+### Running
+```sh
+$ ./app
+```
+
+Now, wait for the program to generate the map!
+
+### Generated Map
+
+![alt text](Images/Map.png)
+

mapping_slam/occupancy_grid_mapping/OccupancyGridMappingAlgorithm/main.cpp

@@ -0,0 +1,137 @@
+#include <iostream>
+#include <math.h>
+#include <vector>
+#include "src/matplotlibcpp.h" //Graph Library
+
+using namespace std;
+namespace plt = matplotlibcpp;
+
+// 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 
+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()
+{
+    //Graph Format
+    plt::title("Map");
+    plt::xlim(0, (int)(mapWidth / gridWidth));
+    plt::ylim(0, (int)(mapHeight / gridHeight));
+
+    // Draw every grid of the map:
+    for (double x = 0; x < mapWidth / gridWidth; x++) {
+        cout << "Remaining Rows= " << mapWidth / gridWidth - x << endl;
+        for (double y = 0; y < mapHeight / gridHeight; y++) {
+            if (l[x][y] == 0) { //Green unkown state
+                plt::plot({ x }, { y }, "g.");
+            }
+            else if (l[x][y] > 0) { //Black occupied state
+                plt::plot({ x }, { y }, "k.");
+            }
+            else { //Red free state
+                plt::plot({ x }, { y }, "r.");
+            }
+        }
+    }
+
+    //Save the image and close the plot
+    plt::save("./Images/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;
+}
+