first commit

CMakeLists.txt

@@ -0,0 +1,21 @@
+cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
+
+add_definitions(-std=c++11)
+
+set(CXX_FLAGS "-Wall" "-pedantic")
+set(CMAKE_CXX_FLAGS, "${CXX_FLAGS}")
+
+project(camera_fusion)
+
+find_package(OpenCV 4.1 REQUIRED)
+
+include_directories(${OpenCV_INCLUDE_DIRS})
+link_directories(${OpenCV_LIBRARY_DIRS})
+add_definitions(${OpenCV_DEFINITIONS})
+
+# Executables for exercises
+add_executable (show_lidar_top_view src/show_lidar_top_view.cpp src/structIO.cpp)
+target_link_libraries (show_lidar_top_view ${OpenCV_LIBRARIES})
+
+add_executable (project_lidar_to_camera src/project_lidar_to_camera.cpp src/structIO.cpp)
+target_link_libraries (project_lidar_to_camera ${OpenCV_LIBRARIES})

README.md

@@ -0,0 +1,91 @@
+
+
+# Lidar 3D  Cloud Projection
+
+This Project present how to associate regions in a camera image with Lidar points in 3D space. 
+
+![](https://williamhyin-1301408646.cos.ap-shanghai.myqcloud.com/img/20200425193128.png)
+
+![](https://williamhyin-1301408646.cos.ap-shanghai.myqcloud.com/img/20200425152627.png)
+
+## Dependencies for Running Locally
+* cmake >= 2.8
+  * All OSes: [click here for installation instructions](https://cmake.org/install/)
+* make >= 4.1 (Linux, Mac), 3.81 (Windows)
+  * Linux: make is installed by default on most Linux distros
+  * Mac: [install Xcode command line tools to get make](https://developer.apple.com/xcode/features/)
+  * Windows: [Click here for installation instructions](http://gnuwin32.sourceforge.net/packages/make.htm)
+* Git LFS
+  * Weight files are handled using [LFS](https://git-lfs.github.com/)
+* OpenCV >= 4.1
+  * This must be compiled from source using the `-D OPENCV_ENABLE_NONFREE=ON` cmake flag for testing the SIFT and SURF detectors.
+  * The OpenCV 4.1.0 source code can be found [here](https://github.com/opencv/opencv/tree/4.1.0)
+* gcc/g++ >= 5.4
+  * Linux: gcc / g++ is installed by default on most Linux distros
+  * Mac: same deal as make - [install Xcode command line tools](https://developer.apple.com/xcode/features/)
+  * Windows: recommend using [MinGW](http://www.mingw.org/)
+
+## Basic Build Instructions
+
+1. Clone this repo.
+
+3. Make a build directory in the top level project directory: `mkdir build && cd build`
+
+4. Compile: `cmake .. && make`
+
+5. Run it: `./project_lidar_to_camera`.
+
+
+
+## Steps
+
+1. Filtering Lidar Points
+
+   The code below shows how a filter can be applied to remove Lidar points that do not satisfy a set of constraints, i.e. they are …
+
+   1. … positioned behind the Lidar sensor and thus have a negative x coordinate.
+   2. … too far away in x-direction and thus exceeding an upper distance limit.
+   3. … too far off to the sides in y-direction and thus not relevant for collision detection
+   4. … too close to the road surface in negative z-direction.
+   5. … showing a reflectivity close to zero, which might indicate low reliability.
+
+   ```c++
+   for(auto it=lidarPoints.begin(); it!=lidarPoints.end(); ++it) {
+       float maxX = 25.0, maxY = 6.0, minZ = -1.4; 
+       if(it->x > maxX || it->x < 0.0 || abs(it->y) > maxY || it->z < minZ || it->r<0.01 )
+       {
+       	continue; // skip to next point
+   	}
+   }
+   ```
+
+   
+
+2. Convert current Lidar point into homogeneous coordinates and store it in the 4D variable X.
+
+   ```
+    X.at<double>(0, 0) = it->x;
+    X.at<double>(1, 0) = it->y;
+    X.at<double>(2, 0) = it->z;
+    X.at<double>(3, 0) = 1;
+   ```
+
+   
+
+2. Then, apply the projection equation to map X onto the image plane of the camera and Store the result in Y.
+
+   ```
+   Y = P_rect_00 * R_rect_00 * RT * X;
+   ```
+
+   
+
+3. Once this is done, transform Y back into Euclidean coordinates and store the result in the variable pt.
+
+   ```
+   cv::Point pt;
+   pt.x = Y.at<double>(0, 0) / Y.at<double>(0, 2);
+   pt.y = Y.at<double>(1, 0) / Y.at<double>(0, 2);
+   ```
+
+   

dat/calib_cam_to_cam.txt

@@ -0,0 +1,34 @@
+calib_time: 09-Jan-2012 13:57:47
+corner_dist: 9.950000e-02
+S_00: 1.392000e+03 5.120000e+02
+K_00: 9.842439e+02 0.000000e+00 6.900000e+02 0.000000e+00 9.808141e+02 2.331966e+02 0.000000e+00 0.000000e+00 1.000000e+00
+D_00: -3.728755e-01 2.037299e-01 2.219027e-03 1.383707e-03 -7.233722e-02
+R_00: 1.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 1.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 1.000000e+00
+T_00: 2.573699e-16 -1.059758e-16 1.614870e-16
+S_rect_00: 1.242000e+03 3.750000e+02
+R_rect_00: 9.999239e-01 9.837760e-03 -7.445048e-03 -9.869795e-03 9.999421e-01 -4.278459e-03 7.402527e-03 4.351614e-03 9.999631e-01
+P_rect_00: 7.215377e+02 0.000000e+00 6.095593e+02 0.000000e+00 0.000000e+00 7.215377e+02 1.728540e+02 0.000000e+00 0.000000e+00 0.000000e+00 1.000000e+00 0.000000e+00
+S_01: 1.392000e+03 5.120000e+02
+K_01: 9.895267e+02 0.000000e+00 7.020000e+02 0.000000e+00 9.878386e+02 2.455590e+02 0.000000e+00 0.000000e+00 1.000000e+00
+D_01: -3.644661e-01 1.790019e-01 1.148107e-03 -6.298563e-04 -5.314062e-02
+R_01: 9.993513e-01 1.860866e-02 -3.083487e-02 -1.887662e-02 9.997863e-01 -8.421873e-03 3.067156e-02 8.998467e-03 9.994890e-01
+T_01: -5.370000e-01 4.822061e-03 -1.252488e-02
+S_rect_01: 1.242000e+03 3.750000e+02
+R_rect_01: 9.996878e-01 -8.976826e-03 2.331651e-02 8.876121e-03 9.999508e-01 4.418952e-03 -2.335503e-02 -4.210612e-03 9.997184e-01
+P_rect_01: 7.215377e+02 0.000000e+00 6.095593e+02 -3.875744e+02 0.000000e+00 7.215377e+02 1.728540e+02 0.000000e+00 0.000000e+00 0.000000e+00 1.000000e+00 0.000000e+00
+S_02: 1.392000e+03 5.120000e+02
+K_02: 9.597910e+02 0.000000e+00 6.960217e+02 0.000000e+00 9.569251e+02 2.241806e+02 0.000000e+00 0.000000e+00 1.000000e+00
+D_02: -3.691481e-01 1.968681e-01 1.353473e-03 5.677587e-04 -6.770705e-02
+R_02: 9.999758e-01 -5.267463e-03 -4.552439e-03 5.251945e-03 9.999804e-01 -3.413835e-03 4.570332e-03 3.389843e-03 9.999838e-01
+T_02: 5.956621e-02 2.900141e-04 2.577209e-03
+S_rect_02: 1.242000e+03 3.750000e+02
+R_rect_02: 9.998817e-01 1.511453e-02 -2.841595e-03 -1.511724e-02 9.998853e-01 -9.338510e-04 2.827154e-03 9.766976e-04 9.999955e-01
+P_rect_02: 7.215377e+02 0.000000e+00 6.095593e+02 4.485728e+01 0.000000e+00 7.215377e+02 1.728540e+02 2.163791e-01 0.000000e+00 0.000000e+00 1.000000e+00 2.745884e-03
+S_03: 1.392000e+03 5.120000e+02
+K_03: 9.037596e+02 0.000000e+00 6.957519e+02 0.000000e+00 9.019653e+02 2.242509e+02 0.000000e+00 0.000000e+00 1.000000e+00
+D_03: -3.639558e-01 1.788651e-01 6.029694e-04 -3.922424e-04 -5.382460e-02
+R_03: 9.995599e-01 1.699522e-02 -2.431313e-02 -1.704422e-02 9.998531e-01 -1.809756e-03 2.427880e-02 2.223358e-03 9.997028e-01
+T_03: -4.731050e-01 5.551470e-03 -5.250882e-03
+S_rect_03: 1.242000e+03 3.750000e+02
+R_rect_03: 9.998321e-01 -7.193136e-03 1.685599e-02 7.232804e-03 9.999712e-01 -2.293585e-03 -1.683901e-02 2.415116e-03 9.998553e-01
+P_rect_03: 7.215377e+02 0.000000e+00 6.095593e+02 -3.395242e+02 0.000000e+00 7.215377e+02 1.728540e+02 2.199936e+00 0.000000e+00 0.000000e+00 1.000000e+00 2.729905e-03

dat/calib_velo_to_cam.txt

@@ -0,0 +1,5 @@
+calib_time: 15-Mar-2012 11:37:16
+R: 7.533745e-03 -9.999714e-01 -6.166020e-04 1.480249e-02 7.280733e-04 -9.998902e-01 9.998621e-01 7.523790e-03 1.480755e-02
+T: -4.069766e-03 -7.631618e-02 -2.717806e-01
+delta_f: 0.000000e+00 0.000000e+00
+delta_c: 0.000000e+00 0.000000e+00

src/dataStructures.h

@@ -0,0 +1,12 @@
+#ifndef dataStructures_h
+#define dataStructures_h
+
+#include <vector>
+#include <opencv2/core.hpp>
+
+struct LidarPoint { // single lidar point in space
+    double x,y,z,r; // x,y,z in [m], r is point reflectivity
+};
+
+
+#endif /* dataStructures_h */

src/project_lidar_to_camera.cpp

@@ -0,0 +1,120 @@
+#include <iostream>
+#include <numeric>
+#include <opencv2/core.hpp>
+#include <opencv2/highgui.hpp>
+#include <opencv2/imgproc.hpp>
+
+#include "structIO.hpp"
+
+using namespace std;
+
+void loadCalibrationData(cv::Mat &P_rect_00, cv::Mat &R_rect_00, cv::Mat &RT) {
+    RT.at<double>(0, 0) = 7.533745e-03;
+    RT.at<double>(0, 1) = -9.999714e-01;
+    RT.at<double>(0, 2) = -6.166020e-04;
+    RT.at<double>(0, 3) = -4.069766e-03;
+    RT.at<double>(1, 0) = 1.480249e-02;
+    RT.at<double>(1, 1) = 7.280733e-04;
+    RT.at<double>(1, 2) = -9.998902e-01;
+    RT.at<double>(1, 3) = -7.631618e-02;
+    RT.at<double>(2, 0) = 9.998621e-01;
+    RT.at<double>(2, 1) = 7.523790e-03;
+    RT.at<double>(2, 2) = 1.480755e-02;
+    RT.at<double>(2, 3) = -2.717806e-01;
+    RT.at<double>(3, 0) = 0.0;
+    RT.at<double>(3, 1) = 0.0;
+    RT.at<double>(3, 2) = 0.0;
+    RT.at<double>(3, 3) = 1.0;
+
+    R_rect_00.at<double>(0, 0) = 9.999239e-01;
+    R_rect_00.at<double>(0, 1) = 9.837760e-03;
+    R_rect_00.at<double>(0, 2) = -7.445048e-03;
+    R_rect_00.at<double>(0, 3) = 0.0;
+    R_rect_00.at<double>(1, 0) = -9.869795e-03;
+    R_rect_00.at<double>(1, 1) = 9.999421e-01;
+    R_rect_00.at<double>(1, 2) = -4.278459e-03;
+    R_rect_00.at<double>(1, 3) = 0.0;
+    R_rect_00.at<double>(2, 0) = 7.402527e-03;
+    R_rect_00.at<double>(2, 1) = 4.351614e-03;
+    R_rect_00.at<double>(2, 2) = 9.999631e-01;
+    R_rect_00.at<double>(2, 3) = 0.0;
+    R_rect_00.at<double>(3, 0) = 0;
+    R_rect_00.at<double>(3, 1) = 0;
+    R_rect_00.at<double>(3, 2) = 0;
+    R_rect_00.at<double>(3, 3) = 1;
+
+    P_rect_00.at<double>(0, 0) = 7.215377e+02;
+    P_rect_00.at<double>(0, 1) = 0.000000e+00;
+    P_rect_00.at<double>(0, 2) = 6.095593e+02;
+    P_rect_00.at<double>(0, 3) = 0.000000e+00;
+    P_rect_00.at<double>(1, 0) = 0.000000e+00;
+    P_rect_00.at<double>(1, 1) = 7.215377e+02;
+    P_rect_00.at<double>(1, 2) = 1.728540e+02;
+    P_rect_00.at<double>(1, 3) = 0.000000e+00;
+    P_rect_00.at<double>(2, 0) = 0.000000e+00;
+    P_rect_00.at<double>(2, 1) = 0.000000e+00;
+    P_rect_00.at<double>(2, 2) = 1.000000e+00;
+    P_rect_00.at<double>(2, 3) = 0.000000e+00;
+
+}
+
+void projectLidarToCamera2() {
+    // load image from file
+    cv::Mat img = cv::imread("../images/0000000000.png");
+
+    // load Lidar points from file
+    std::vector<LidarPoint> lidarPoints;
+    readLidarPts("../dat/C51_LidarPts_0000.dat", lidarPoints);
+
+    // store calibration data in OpenCV matrices
+    cv::Mat P_rect_00(3, 4, cv::DataType<double>::type); // 3x4 projection matrix after rectification
+    cv::Mat R_rect_00(4, 4, cv::DataType<double>::type); // 3x3 rectifying rotation to make image planes co-planar
+    cv::Mat RT(4, 4, cv::DataType<double>::type); // rotation matrix and translation vector
+    loadCalibrationData(P_rect_00, R_rect_00, RT);
+
+    // TODO: project lidar points
+    cv::Mat visImg = img.clone();
+    cv::Mat overlay = visImg.clone();
+
+    cv::Mat X(4, 1, cv::DataType<double>::type);
+    cv::Mat Y(3, 1, cv::DataType<double>::type);
+    for (auto it = lidarPoints.begin(); it != lidarPoints.end(); ++it) {
+        // filter the not needed points
+        float MaxX = 25.0, maxY = 6.0, minZ = -1.40;
+        if (it->x > MaxX ||it->x < 0.0 || abs(it->y) > maxY || it->z < minZ || it->r < 0.01) {
+            continue;
+        }
+
+        // 1. Convert current Lidar point into homogeneous coordinates and store it in the 4D variable X.
+        X.at<double>(0, 0) = it->x;
+        X.at<double>(1, 0) = it->y;
+        X.at<double>(2, 0) = it->z;
+        X.at<double>(3, 0) = 1;
+
+        // 2. Then, apply the projection equation as detailed in lesson 5.1 to map X onto the image plane of the camera. 
+        // Store the result in Y.
+        Y = P_rect_00 * R_rect_00 * RT * X;
+        // 3. Once this is done, transform Y back into Euclidean coordinates and store the result in the variable pt.
+        cv::Point pt;
+        pt.x = Y.at<double>(0, 0) / Y.at<double>(2, 0);
+        pt.y = Y.at<double>(1, 0) / Y.at<double>(2, 0);
+
+        float val = it->x;
+        float maxVal = 20.0;
+        int red = min(255, (int) (255 * abs((val - maxVal) / maxVal)));
+        int green = min(255, (int) (255 * (1 - abs((val - maxVal) / maxVal))));
+        cv::circle(overlay, pt, 5, cv::Scalar(0, green, red), -1);
+    }
+
+    float opacity = 0.6;
+    cv::addWeighted(overlay, opacity, visImg, 1 - opacity, 0, visImg);
+
+    string windowName = "LiDAR data on image overlay";
+    cv::namedWindow(windowName, 3);
+    cv::imshow(windowName, visImg);
+    cv::waitKey(0); // wait for key to be pressed
+}
+
+int main() {
+    projectLidarToCamera2();
+}

src/show_lidar_top_view.cpp

@@ -0,0 +1,70 @@
+#include <iostream>
+#include <numeric>
+#include <opencv2/core.hpp>
+#include <opencv2/highgui.hpp>
+#include <opencv2/imgproc.hpp>
+
+#include "structIO.hpp"
+
+using namespace std;
+
+void showLidarTopview()
+{
+    std::vector<LidarPoint> lidarPoints;
+    readLidarPts("../dat/C51_LidarPts_0000.dat", lidarPoints);
+
+    cv::Size worldSize(10.0, 20.0); // width and height of sensor field in m
+    cv::Size imageSize(1000, 2000); // corresponding top view image in pixel
+
+    // create topview image
+    cv::Mat topviewImg(imageSize, CV_8UC3, cv::Scalar(0, 0, 0));
+    float max_ref=0;
+    // plot Lidar points into image
+    for (auto it = lidarPoints.begin(); it != lidarPoints.end(); ++it)
+    {
+        float xw = (*it).x; // world position in m with x facing forward from sensor
+        float yw = (*it).y; // world position in m with y facing left from sensor
+
+        int y = (-xw * imageSize.height / worldSize.height) + imageSize.height;
+        int x = (-yw * imageSize.height / worldSize.height) + imageSize.width / 2;
+
+//        cv::circle(topviewImg, cv::Point(x, y), 5, cv::Scalar(0, 0, 255), -1);
+
+        // TODO: 
+        // 1. Change the color of the Lidar points such that 
+        // X=0.0m corresponds to red while X=20.0m is shown as green.
+        // 2. Remove all Lidar points on the road surface while preserving 
+        // measurements on the obstacles in the scene.
+        float zw=(*it).z;
+        if(zw>-1.40){
+            float val=it->x;
+            float reflectivity=it->r; // reflectivity of lidar point
+            float maxval =worldSize.height;
+            int red = min(255,(int)(255*abs((val-maxval)/maxval)));
+            int green = min(255,(int)(255*(1-abs((val-maxval)/maxval))));
+            int thickness=reflectivity>0.5?-1:1; // -1:ellipse arc outline 1:filled ellipse sector
+            cv::circle(topviewImg,cv::Point(x,y),5,cv::Scalar(0,green,red),thickness);
+
+        }
+    }
+    cout<<"max reflectivity: "<<max_ref<<endl;
+    // plot distance markers
+    float lineSpacing = 2.0; // gap between distance markers
+    int nMarkers = floor(worldSize.height / lineSpacing);
+    for (size_t i = 0; i < nMarkers; ++i)
+    {
+        int y = (-(i * lineSpacing) * imageSize.height / worldSize.height) + imageSize.height;
+        cv::line(topviewImg, cv::Point(0, y), cv::Point(imageSize.width, y), cv::Scalar(255, 0, 0));
+    }
+
+    // display image
+    string windowName = "Top-View Perspective of LiDAR data";
+    cv::namedWindow(windowName, 2);
+    cv::imshow(windowName, topviewImg);
+    cv::waitKey(0); // wait for key to be pressed
+}
+
+int main()
+{
+    showLidarTopview();
+}