First commit.

CMakeLists.txt

@@ -0,0 +1,23 @@
+cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
+
+add_definitions(-std=c++11)
+
+set(CXX_FLAGS "-Wall")
+set(CMAKE_CXX_FLAGS, "${CXX_FLAGS}")
+
+project(playback)
+
+find_package(PCL 1.2 REQUIRED)
+
+include_directories(${PCL_INCLUDE_DIRS})
+link_directories(${PCL_LIBRARY_DIRS})
+add_definitions(${PCL_DEFINITIONS})
+list(REMOVE_ITEM PCL_LIBRARIES "vtkproj4")
+
+
+add_executable (environment src/environment.cpp src/render/render.cpp src/processPointClouds.cpp)
+target_link_libraries (environment ${PCL_LIBRARIES})
+
+
+
+

README.md

@@ -0,0 +1,31 @@
+# Sensor Fusion Self-Driving Car Course
+
+<img src="https://github.com/awbrown90/SensorFusionHighway/blob/master/media/ObstacleDetectionFPS.gif" width="700" height="400" />
+
+### Welcome to the Sensor Fusion course for self-driving cars.
+
+In this course we will be talking about sensor fusion, whch is the process of taking data from multiple sensors and combining it to give us a better understanding of the world around us. we will mostly be focusing on two sensors, lidar, and radar. By the end we will be fusing the data from these two sensors to track multiple cars on the road, estimating their positions and speed.
+
+**Lidar** sensing gives us high resolution data by sending out thousands of laser signals. These lasers bounce off objects, returning to the sensor where we can then determine how far away objects are by timing how long it takes for the signal to return. Also we can tell a little bit about the object that was hit by measuring the intesity of the returned signal. Each laser ray is in the infrared spectrum, and is sent out at many different angles, usually in a 360 degree range. While lidar sensors gives us very high accurate models for the world around us in 3D, they are currently very expensive, upwards of $60,000 for a standard unit.
+
+**Radar** data is typically very sparse and in a limited range, however it can directly tell us how fast an object is moving in a certain direction. This ability makes radars a very pratical sensor for doing things like cruise control where its important to know how fast the car infront of you is traveling. Radar sensors are also very affordable and common now of days in newer cars.
+
+**Sensor Fusion** by combing lidar's high resoultion imaging with radar's ability to measure velocity of objects we can get a better understanding of the sorrounding environment than we could using one of the sensors alone.
+
+
+## Installation
+
+### Linux Ubuntu 16
+
+Install PCL, C++
+
+The link here is very helpful, 
+https://larrylisky.com/2014/03/03/installing-pcl-on-ubuntu/
+
+A few updates to the instructions above were needed.
+
+* libvtk needed to be updated to libvtk6-dev instead of (libvtk5-dev). The linker was having trouble locating libvtk5-dev while building, but this might not be a problem for everyone.
+
+* BUILD_visualization needed to be manually turned on, this link shows you how to do that,
+http://www.pointclouds.org/documentation/tutorials/building_pcl.php
+

src/environment.cpp

@@ -0,0 +1,92 @@
+/* \author Aaron Brown */
+// Create simple 3d highway enviroment using PCL
+// for exploring self-driving car sensors
+
+#include "sensors/lidar.h"
+#include "render/render.h"
+#include "processPointClouds.h"
+// using templates for processPointClouds so also include .cpp to help linker
+#include "processPointClouds.cpp"
+
+std::vector<Car> initHighway(bool renderScene, pcl::visualization::PCLVisualizer::Ptr& viewer)
+{
+
+    Car egoCar( Vect3(0,0,0), Vect3(4,2,2), Color(0,1,0), "egoCar");
+    Car car1( Vect3(15,0,0), Vect3(4,2,2), Color(0,0,1), "car1");
+    Car car2( Vect3(8,-4,0), Vect3(4,2,2), Color(0,0,1), "car2");	
+    Car car3( Vect3(-12,4,0), Vect3(4,2,2), Color(0,0,1), "car3");
+
+    std::vector<Car> cars;
+    cars.push_back(egoCar);
+    cars.push_back(car1);
+    cars.push_back(car2);
+    cars.push_back(car3);
+
+    if(renderScene)
+    {
+        renderHighway(viewer);
+        egoCar.render(viewer);
+        car1.render(viewer);
+        car2.render(viewer);
+        car3.render(viewer);
+    }
+
+    return cars;
+}
+
+
+void simpleHighway(pcl::visualization::PCLVisualizer::Ptr& viewer)
+{
+    // ----------------------------------------------------
+    // -----Open 3D viewer and display simple highway -----
+    // ----------------------------------------------------
+
+    // RENDER OPTIONS
+    bool renderScene = true;
+    std::vector<Car> cars = initHighway(renderScene, viewer);
+
+    // TODO:: Create lidar sensor 
+
+    // TODO:: Create point processor
+
+}
+
+
+//setAngle: SWITCH CAMERA ANGLE {XY, TopDown, Side, FPS}
+void initCamera(CameraAngle setAngle, pcl::visualization::PCLVisualizer::Ptr& viewer)
+{
+
+    viewer->setBackgroundColor (0, 0, 0);
+
+    // set camera position and angle
+    viewer->initCameraParameters();
+    // distance away in meters
+    int distance = 16;
+
+    switch(setAngle)
+    {
+        case XY : viewer->setCameraPosition(-distance, -distance, distance, 1, 1, 0); break;
+        case TopDown : viewer->setCameraPosition(0, 0, distance, 1, 0, 1); break;
+        case Side : viewer->setCameraPosition(0, -distance, 0, 0, 0, 1); break;
+        case FPS : viewer->setCameraPosition(-10, 0, 0, 0, 0, 1);
+    }
+
+    if(setAngle!=FPS)
+        viewer->addCoordinateSystem (1.0);
+}
+
+
+int main (int argc, char** argv)
+{
+    std::cout << "starting enviroment" << std::endl;
+
+    pcl::visualization::PCLVisualizer::Ptr viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
+    CameraAngle setAngle = XY;
+    initCamera(setAngle, viewer);
+    simpleHighway(viewer);
+
+    while (!viewer->wasStopped ())
+    {
+        viewer->spinOnce ();
+    } 
+}

src/processPointClouds.cpp

@@ -0,0 +1,142 @@
+// PCL lib Functions for processing point clouds 
+
+#include "processPointClouds.h"
+
+
+//constructor:
+template<typename PointT>
+ProcessPointClouds<PointT>::ProcessPointClouds() {}
+
+
+//de-constructor:
+template<typename PointT>
+ProcessPointClouds<PointT>::~ProcessPointClouds() {}
+
+
+template<typename PointT>
+void ProcessPointClouds<PointT>::numPoints(typename pcl::PointCloud<PointT>::Ptr cloud)
+{
+    std::cout << cloud->points.size() << std::endl;
+}
+
+
+template<typename PointT>
+typename pcl::PointCloud<PointT>::Ptr ProcessPointClouds<PointT>::FilterCloud(typename pcl::PointCloud<PointT>::Ptr cloud, float filterRes, Eigen::Vector4f minPoint, Eigen::Vector4f maxPoint)
+{
+
+    // Time segmentation process
+    auto startTime = std::chrono::steady_clock::now();
+
+    // TODO:: Fill in the function to do voxel grid point reduction and region based filtering
+
+    auto endTime = std::chrono::steady_clock::now();
+    auto elapsedTime = std::chrono::duration_cast<std::chrono::milliseconds>(endTime - startTime);
+    std::cout << "filtering took " << elapsedTime.count() << " milliseconds" << std::endl;
+
+    return cloud;
+
+}
+
+
+template<typename PointT>
+std::pair<typename pcl::PointCloud<PointT>::Ptr, typename pcl::PointCloud<PointT>::Ptr> ProcessPointClouds<PointT>::SeparateClouds(pcl::PointIndices::Ptr inliers, typename pcl::PointCloud<PointT>::Ptr cloud) 
+{
+  // TODO: Create two new point clouds, one cloud with obstacles and other with segmented plane
+
+    std::pair<typename pcl::PointCloud<PointT>::Ptr, typename pcl::PointCloud<PointT>::Ptr> segResult(cloud, cloud);
+    return segResult;
+}
+
+
+template<typename PointT>
+std::pair<typename pcl::PointCloud<PointT>::Ptr, typename pcl::PointCloud<PointT>::Ptr> ProcessPointClouds<PointT>::SegmentPlane(typename pcl::PointCloud<PointT>::Ptr cloud, int maxIterations, float distanceThreshold)
+{
+    // Time segmentation process
+    auto startTime = std::chrono::steady_clock::now();
+
+    // TODO:: Fill in the function to segment cloud into two parts, the driveable plane and obstacles
+
+    auto endTime = std::chrono::steady_clock::now();
+    auto elapsedTime = std::chrono::duration_cast<std::chrono::milliseconds>(endTime - startTime);
+    std::cout << "plane segmentation took " << elapsedTime.count() << " milliseconds" << std::endl;
+
+    std::pair<typename pcl::PointCloud<PointT>::Ptr, typename pcl::PointCloud<PointT>::Ptr> segResult(cloud, cloud);
+    return segResult;
+}
+
+
+template<typename PointT>
+std::vector<typename pcl::PointCloud<PointT>::Ptr> ProcessPointClouds<PointT>::Clustering(typename pcl::PointCloud<PointT>::Ptr cloud, float clusterTolerance, int minSize, int maxSize)
+{
+
+    // Time clustering process
+    auto startTime = std::chrono::steady_clock::now();
+
+    std::vector<typename pcl::PointCloud<PointT>::Ptr> clusters;
+
+    // TODO:: Fill in the function to perform euclidean clustering to group detected obstacles
+
+    auto endTime = std::chrono::steady_clock::now();
+    auto elapsedTime = std::chrono::duration_cast<std::chrono::milliseconds>(endTime - startTime);
+    std::cout << "clustering took " << elapsedTime.count() << " milliseconds and found " << clusters.size() << " clusters" << std::endl;
+
+    return clusters;
+}
+
+
+template<typename PointT>
+Box ProcessPointClouds<PointT>::BoundingBox(typename pcl::PointCloud<PointT>::Ptr cluster)
+{
+
+    // Find bounding box for one of the clusters
+    PointT minPoint, maxPoint;
+    pcl::getMinMax3D(*cluster, minPoint, maxPoint);
+
+    Box box;
+    box.x_min = minPoint.x;
+    box.y_min = minPoint.y;
+    box.z_min = minPoint.z;
+    box.x_max = maxPoint.x;
+    box.y_max = maxPoint.y;
+    box.z_max = maxPoint.z;
+
+    return box;
+}
+
+
+template<typename PointT>
+void ProcessPointClouds<PointT>::savePcd(typename pcl::PointCloud<PointT>::Ptr cloud, std::string file)
+{
+    pcl::io::savePCDFileASCII (file, *cloud);
+    std::cerr << "Saved " << cloud->points.size () << " data points to "+file << std::endl;
+}
+
+
+template<typename PointT>
+typename pcl::PointCloud<PointT>::Ptr ProcessPointClouds<PointT>::loadPcd(std::string file)
+{
+
+    typename pcl::PointCloud<PointT>::Ptr cloud (new pcl::PointCloud<PointT>);
+
+    if (pcl::io::loadPCDFile<PointT> (file, *cloud) == -1) //* load the file
+    {
+        PCL_ERROR ("Couldn't read file \n");
+    }
+    std::cerr << "Loaded " << cloud->points.size () << " data points from "+file << std::endl;
+
+    return cloud;
+}
+
+
+template<typename PointT>
+std::vector<boost::filesystem::path> ProcessPointClouds<PointT>::streamPcd(std::string dataPath)
+{
+
+    std::vector<boost::filesystem::path> paths(boost::filesystem::directory_iterator{dataPath}, boost::filesystem::directory_iterator{});
+
+    // sort files in accending order so playback is chronological
+    sort(paths.begin(), paths.end());
+
+    return paths;
+
+}

src/processPointClouds.h

@@ -0,0 +1,50 @@
+// PCL lib Functions for processing point clouds 
+
+#ifndef PROCESSPOINTCLOUDS_H_
+#define PROCESSPOINTCLOUDS_H_
+
+#include <pcl/io/pcd_io.h>
+#include <pcl/common/common.h>
+#include <pcl/filters/extract_indices.h>
+#include <pcl/filters/voxel_grid.h>
+#include <pcl/filters/crop_box.h>
+#include <pcl/kdtree/kdtree.h>
+#include <pcl/segmentation/sac_segmentation.h>
+#include <pcl/segmentation/extract_clusters.h>
+#include <pcl/common/transforms.h>
+#include <iostream> 
+#include <string>  
+#include <vector>
+#include <ctime>
+#include <chrono>
+#include "render/box.h"
+
+template<typename PointT>
+class ProcessPointClouds {
+public:
+
+    //constructor
+    ProcessPointClouds();
+    //deconstructor
+    ~ProcessPointClouds();
+
+    void numPoints(typename pcl::PointCloud<PointT>::Ptr cloud);
+
+    typename pcl::PointCloud<PointT>::Ptr FilterCloud(typename pcl::PointCloud<PointT>::Ptr cloud, float filterRes, Eigen::Vector4f minPoint, Eigen::Vector4f maxPoint);
+
+    std::pair<typename pcl::PointCloud<PointT>::Ptr, typename pcl::PointCloud<PointT>::Ptr> SeparateClouds(pcl::PointIndices::Ptr inliers, typename pcl::PointCloud<PointT>::Ptr cloud);
+
+    std::pair<typename pcl::PointCloud<PointT>::Ptr, typename pcl::PointCloud<PointT>::Ptr> SegmentPlane(typename pcl::PointCloud<PointT>::Ptr cloud, int maxIterations, float distanceThreshold);
+
+    std::vector<typename pcl::PointCloud<PointT>::Ptr> Clustering(typename pcl::PointCloud<PointT>::Ptr cloud, float clusterTolerance, int minSize, int maxSize);
+
+    Box BoundingBox(typename pcl::PointCloud<PointT>::Ptr cluster);
+
+    void savePcd(typename pcl::PointCloud<PointT>::Ptr cloud, std::string file);
+
+    typename pcl::PointCloud<PointT>::Ptr loadPcd(std::string file);
+
+    std::vector<boost::filesystem::path> streamPcd(std::string dataPath);
+
+};
+#endif /* PROCESSPOINTCLOUDS_H_ */

src/quiz/cluster/CMakeLists.txt

@@ -0,0 +1,23 @@
+cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
+
+add_definitions(-std=c++11)
+
+set(CXX_FLAGS "-Wall")
+set(CMAKE_CXX_FLAGS, "${CXX_FLAGS}")
+
+project(playback)
+
+find_package(PCL 1.2 REQUIRED)
+
+include_directories(${PCL_INCLUDE_DIRS})
+link_directories(${PCL_LIBRARY_DIRS})
+add_definitions(${PCL_DEFINITIONS})
+list(REMOVE_ITEM PCL_LIBRARIES "vtkproj4")
+
+
+add_executable (quizCluster cluster.cpp ../../render/render.cpp)
+target_link_libraries (quizCluster ${PCL_LIBRARIES})
+
+
+
+