Combining Camera and Lidar exercises

Author: SudKul

Combining Camera and Lidar/Creating 3D-Objects/.student_bashrc

@@ -0,0 +1 @@
+cd /home/workspace/cluster_with_roi

Combining Camera and Lidar/Creating 3D-Objects/cluster_with_roi/CMakeLists.txt

@@ -0,0 +1,18 @@
+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 (cluster_with_roi src/cluster_with_roi.cpp src/structIO.cpp)
+target_link_libraries (cluster_with_roi ${OpenCV_LIBRARIES})

Combining Camera and Lidar/Creating 3D-Objects/cluster_with_roi/dat/C53A3_currBoundingBoxes.dat

@@ -0,0 +1,140 @@
+#include <iostream>
+#include <numeric>
+#include <opencv2/core.hpp>
+#include <opencv2/highgui.hpp>
+#include <opencv2/imgproc.hpp>
+
+#include "structIO.hpp"
+#include "dataStructures.h"
+
+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 showLidarTopview(std::vector<LidarPoint> &lidarPoints, cv::Size worldSize, cv::Size imageSize)
+{
+    // create topview image
+    cv::Mat topviewImg(imageSize, CV_8UC3, cv::Scalar(0, 0, 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;
+
+        float zw = (*it).z; // world position in m with y facing left from sensor
+        if(zw > -1.40){       
+
+            float val = it->x;
+            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))));
+            cv::circle(topviewImg, cv::Point(x, y), 5, cv::Scalar(0, green, red), -1);
+        }
+    }
+
+    // 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
+}
+
+
+void clusterLidarWithROI(std::vector<BoundingBox> &boundingBoxes, std::vector<LidarPoint> &lidarPoints)
+{
+    // store calibration data in OpenCV matrices
+    cv::Mat P_rect_xx(3,4,cv::DataType<double>::type); // 3x4 projection matrix after rectification
+    cv::Mat R_rect_xx(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_xx, R_rect_xx, RT);
+
+    // loop over all Lidar points and associate them to a 2D bounding box
+    cv::Mat X(4, 1, cv::DataType<double>::type);
+    cv::Mat Y(3, 1, cv::DataType<double>::type);
+
+    for (auto it1 = lidarPoints.begin(); it1 != lidarPoints.end(); ++it1)
+    {
+        // assemble vector for matrix-vector-multiplication
+        X.at<double>(0, 0) = it1->x;
+        X.at<double>(1, 0) = it1->y;
+        X.at<double>(2, 0) = it1->z;
+        X.at<double>(3, 0) = 1;
+
+        // project Lidar point into camera
+        Y = P_rect_xx * R_rect_xx * RT * X;
+        cv::Point pt;
+        pt.x = Y.at<double>(0, 0) / Y.at<double>(0, 2); // pixel coordinates
+        pt.y = Y.at<double>(1, 0) / Y.at<double>(0, 2);
+
+        double shrinkFactor = 0.10;
+        vector<vector<BoundingBox>::iterator> enclosingBoxes; // pointers to all bounding boxes which enclose the current Lidar point
+        for (vector<BoundingBox>::iterator it2 = boundingBoxes.begin(); it2 != boundingBoxes.end(); ++it2)
+        {
+            // shrink current bounding box slightly to avoid having too many outlier points around the edges
+            cv::Rect smallerBox;
+            smallerBox.x = (*it2).roi.x + shrinkFactor * (*it2).roi.width / 2.0;
+            smallerBox.y = (*it2).roi.y + shrinkFactor * (*it2).roi.height / 2.0;
+            smallerBox.width = (*it2).roi.width * (1 - shrinkFactor);
+            smallerBox.height = (*it2).roi.height * (1 - shrinkFactor);
+
+            // check wether point is within current bounding box
+            if (smallerBox.contains(pt))
+            {
+                it2->lidarPoints.push_back(*it1);
+                lidarPoints.erase(it1);
+                it1--;
+                break;
+            }
+        } // eof loop over all bounding boxes
+
+    } // eof loop over all Lidar points
+}
+
+int main()
+{
+    std::vector<LidarPoint> lidarPoints;
+    readLidarPts("../dat/C53A3_currLidarPts.dat", lidarPoints);
+
+    std::vector<BoundingBox> boundingBoxes;
+    readBoundingBoxes("../dat/C53A3_currBoundingBoxes.dat", boundingBoxes);
+
+    clusterLidarWithROI(boundingBoxes, lidarPoints);
+    for (auto it = boundingBoxes.begin(); it != boundingBoxes.end(); ++it)
+    {
+        if (it->lidarPoints.size() > 0)
+        {
+            showLidarTopview(it->lidarPoints, cv::Size(10.0, 25.0), cv::Size(1000, 2000));
+        }
+    }   
+
+    return 0;
+}

Combining Camera and Lidar/Creating 3D-Objects/cluster_with_roi/dat/C53A3_currLidarPts.dat

@@ -0,0 +1,25 @@
+#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
+};
+
+struct BoundingBox { // bounding box around a classified object (contains both 2D and 3D data)
+    
+    int boxID; // unique identifier for this bounding box
+    int trackID; // unique identifier for the track to which this bounding box belongs
+    
+    cv::Rect roi; // 2D region-of-interest in image coordinates
+    int classID; // ID based on class file provided to YOLO framework
+    double confidence; // classification trust
+
+    std::vector<LidarPoint> lidarPoints; // Lidar 3D points which project into 2D image roi
+    std::vector<cv::KeyPoint> keypoints; // keypoints enclosed by 2D roi
+    std::vector<cv::DMatch> kptMatches; // keypoint matches enclosed by 2D roi
+};
+
+#endif /* dataStructures_h */

Combining Camera and Lidar/Creating 3D-Objects/cluster_with_roi/src/cluster_with_roi.cpp

@@ -0,0 +1,124 @@
+#include <iostream>
+#include <fstream>
+#include <algorithm>
+#include <opencv2/highgui/highgui.hpp>
+#include "structIO.hpp"
+
+using namespace std;
+
+
+/* TEMPLATES */
+template<typename T> void write_pod(std::ofstream& out, T& t)
+{
+    out.write(reinterpret_cast<char*>(&t), sizeof(T));
+}
+
+
+template<typename T> void read_pod(std::ifstream& in, T& t)
+{
+    in.read(reinterpret_cast<char*>(&t), sizeof(T));
+}
+
+
+template<typename T> void read_pod_vector(std::ifstream& in, std::vector<T>& vect)
+{
+    long size;
+    
+    read_pod(in, size);
+    for(int i = 0;i < size;++i)
+    {
+        T t;
+        read_pod(in, t);
+        vect.push_back(t);
+    }
+}
+
+template<typename T> void write_pod_vector(std::ofstream& out, std::vector<T>& vect)
+{
+    long size = vect.size();
+    write_pod<long>(out, size);
+    for(auto it=vect.begin(); it!=vect.end(); ++it)
+    {
+        write_pod<T>(out,*it);
+    }
+}
+
+
+
+
+/* DATATYPE WRAPPERS */
+
+void writeBoundingBoxes(std::vector<BoundingBox> &input, const char* fileName)
+{
+    std::ofstream out(fileName);
+    write_pod_vector(out, input);
+    out.close();
+}
+
+void readBoundingBoxes(const char* fileName, std::vector<BoundingBox> &output)
+{
+    std::ifstream in(fileName);
+    read_pod_vector(in, output);
+}
+
+void writeLidarPts(std::vector<LidarPoint> &input, const char* fileName)
+{
+    std::ofstream out(fileName);
+    write_pod_vector(out, input);
+    out.close();
+}
+
+
+void readLidarPts(const char* fileName, std::vector<LidarPoint> &output)
+{
+    std::ifstream in(fileName);
+    read_pod_vector(in, output);
+}
+
+
+void writeKeypoints(std::vector<cv::KeyPoint> &input, const char* fileName)
+{
+    std::ofstream out(fileName);
+    write_pod_vector(out, input);
+    out.close();
+}
+
+
+void readKeypoints(const char* fileName, std::vector<cv::KeyPoint> &output)
+{
+    std::ifstream in(fileName);
+    read_pod_vector(in, output);
+}
+
+
+void writeKptMatches(std::vector<cv::DMatch> &input, const char* fileName)
+{
+    std::ofstream out(fileName);
+    write_pod_vector(out, input);
+    out.close();
+}
+
+
+void readKptMatches(const char* fileName, std::vector<cv::DMatch> &output)
+{
+    std::ifstream in(fileName);
+    read_pod_vector(in, output);
+}
+
+
+
+void writeDescriptors(cv::Mat &input, const char* fileName)
+{   
+    cv::FileStorage opencv_file(fileName, cv::FileStorage::WRITE);
+    opencv_file << "desc_matrix" << input;
+    opencv_file.release();
+}
+
+
+void readDescriptors(const char* fileName, cv::Mat &output)
+{
+    cv::FileStorage opencv_file(fileName, cv::FileStorage::READ);
+    opencv_file["desc_matrix"] >> output;
+    opencv_file.release();
+}
+

Combining Camera and Lidar/Creating 3D-Objects/cluster_with_roi/src/dataStructures.h

@@ -0,0 +1,28 @@
+#ifndef structIO_hpp
+#define structIO_hpp
+
+#include <stdio.h>
+#include "dataStructures.h"
+
+void writeLidarPts(std::vector<LidarPoint> &input, const char* fileName);
+void readLidarPts(const char* fileName, std::vector<LidarPoint> &output);
+
+void writeKeypoints(std::vector<cv::KeyPoint> &input, const char* fileName);
+void readKeypoints(const char* fileName, std::vector<cv::KeyPoint> &output);
+
+void writeKptMatches(std::vector<cv::DMatch> &input, const char* fileName);
+void readKptMatches(const char* fileName, std::vector<cv::DMatch> &output);
+
+void writeDescriptors(cv::Mat &input, const char* fileName);
+void readDescriptors(const char* fileName, cv::Mat &output);
+
+void writeBoundingBoxes(std::vector<BoundingBox> &input, const char* fileName);
+void readBoundingBoxes(const char* fileName, std::vector<BoundingBox> &output);
+
+
+template<typename T> void write_pod(std::ofstream& out, T& t);
+template<typename T> void read_pod(std::ifstream& in, T& t);
+template<typename T> void read_pod_vector(std::ifstream& in, std::vector<T>& vect);
+template<typename T> void write_pod_vector(std::ofstream& out, std::vector<T>& vect);
+
+#endif /* structIO_hpp */

Combining Camera and Lidar/Creating 3D-Objects/cluster_with_roi/src/structIO.cpp

@@ -0,0 +1 @@
+cd /home/workspace/lidar_to_camera

Combining Camera and Lidar/Creating 3D-Objects/cluster_with_roi/src/structIO.hpp

@@ -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})