diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..777954e --- /dev/null +++ b/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}) diff --git a/README.md b/README.md new file mode 100644 index 0000000..82cec76 --- /dev/null +++ b/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(0, 0) = it->x; + X.at(1, 0) = it->y; + X.at(2, 0) = it->z; + X.at(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(0, 0) / Y.at(0, 2); + pt.y = Y.at(1, 0) / Y.at(0, 2); + ``` + + \ No newline at end of file diff --git a/dat/C51_LidarPts_0000.dat b/dat/C51_LidarPts_0000.dat new file mode 100644 index 0000000..951474c Binary files /dev/null and b/dat/C51_LidarPts_0000.dat differ diff --git a/dat/C51_LidarPts_0001.dat b/dat/C51_LidarPts_0001.dat new file mode 100644 index 0000000..b2cc5b9 Binary files /dev/null and b/dat/C51_LidarPts_0001.dat differ diff --git a/dat/C51_LidarPts_0002.dat b/dat/C51_LidarPts_0002.dat new file mode 100644 index 0000000..f35c924 Binary files /dev/null and b/dat/C51_LidarPts_0002.dat differ diff --git a/dat/C51_LidarPts_0003.dat b/dat/C51_LidarPts_0003.dat new file mode 100644 index 0000000..47ac6a2 Binary files /dev/null and b/dat/C51_LidarPts_0003.dat differ diff --git a/dat/C51_LidarPts_0004.dat b/dat/C51_LidarPts_0004.dat new file mode 100644 index 0000000..c65568b Binary files /dev/null and b/dat/C51_LidarPts_0004.dat differ diff --git a/dat/C51_LidarPts_0005.dat b/dat/C51_LidarPts_0005.dat new file mode 100644 index 0000000..0ee17bf Binary files /dev/null and b/dat/C51_LidarPts_0005.dat differ diff --git a/dat/C51_LidarPts_0006.dat b/dat/C51_LidarPts_0006.dat new file mode 100644 index 0000000..7540a3a Binary files /dev/null and b/dat/C51_LidarPts_0006.dat differ diff --git a/dat/C51_LidarPts_0007.dat b/dat/C51_LidarPts_0007.dat new file mode 100644 index 0000000..03e0b36 Binary files /dev/null and b/dat/C51_LidarPts_0007.dat differ diff --git a/dat/C51_LidarPts_0008.dat b/dat/C51_LidarPts_0008.dat new file mode 100644 index 0000000..45d4c75 Binary files /dev/null and b/dat/C51_LidarPts_0008.dat differ diff --git a/dat/C51_LidarPts_0009.dat b/dat/C51_LidarPts_0009.dat new file mode 100644 index 0000000..5d14d24 Binary files /dev/null and b/dat/C51_LidarPts_0009.dat differ diff --git a/dat/calib_cam_to_cam.txt b/dat/calib_cam_to_cam.txt new file mode 100644 index 0000000..04a75a0 --- /dev/null +++ b/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 diff --git a/dat/calib_velo_to_cam.txt b/dat/calib_velo_to_cam.txt new file mode 100644 index 0000000..f80c46a --- /dev/null +++ b/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 diff --git a/images/0000000000.png b/images/0000000000.png new file mode 100644 index 0000000..8b5d6d2 Binary files /dev/null and b/images/0000000000.png differ diff --git a/images/s_thrun.jpg b/images/s_thrun.jpg new file mode 100644 index 0000000..7bc6b60 Binary files /dev/null and b/images/s_thrun.jpg differ diff --git a/src/dataStructures.h b/src/dataStructures.h new file mode 100644 index 0000000..94b9794 --- /dev/null +++ b/src/dataStructures.h @@ -0,0 +1,12 @@ +#ifndef dataStructures_h +#define dataStructures_h + +#include +#include + +struct LidarPoint { // single lidar point in space + double x,y,z,r; // x,y,z in [m], r is point reflectivity +}; + + +#endif /* dataStructures_h */ diff --git a/src/project_lidar_to_camera.cpp b/src/project_lidar_to_camera.cpp new file mode 100644 index 0000000..56eafc5 --- /dev/null +++ b/src/project_lidar_to_camera.cpp @@ -0,0 +1,120 @@ +#include +#include +#include +#include +#include + +#include "structIO.hpp" + +using namespace std; + +void loadCalibrationData(cv::Mat &P_rect_00, cv::Mat &R_rect_00, cv::Mat &RT) { + RT.at(0, 0) = 7.533745e-03; + RT.at(0, 1) = -9.999714e-01; + RT.at(0, 2) = -6.166020e-04; + RT.at(0, 3) = -4.069766e-03; + RT.at(1, 0) = 1.480249e-02; + RT.at(1, 1) = 7.280733e-04; + RT.at(1, 2) = -9.998902e-01; + RT.at(1, 3) = -7.631618e-02; + RT.at(2, 0) = 9.998621e-01; + RT.at(2, 1) = 7.523790e-03; + RT.at(2, 2) = 1.480755e-02; + RT.at(2, 3) = -2.717806e-01; + RT.at(3, 0) = 0.0; + RT.at(3, 1) = 0.0; + RT.at(3, 2) = 0.0; + RT.at(3, 3) = 1.0; + + R_rect_00.at(0, 0) = 9.999239e-01; + R_rect_00.at(0, 1) = 9.837760e-03; + R_rect_00.at(0, 2) = -7.445048e-03; + R_rect_00.at(0, 3) = 0.0; + R_rect_00.at(1, 0) = -9.869795e-03; + R_rect_00.at(1, 1) = 9.999421e-01; + R_rect_00.at(1, 2) = -4.278459e-03; + R_rect_00.at(1, 3) = 0.0; + R_rect_00.at(2, 0) = 7.402527e-03; + R_rect_00.at(2, 1) = 4.351614e-03; + R_rect_00.at(2, 2) = 9.999631e-01; + R_rect_00.at(2, 3) = 0.0; + R_rect_00.at(3, 0) = 0; + R_rect_00.at(3, 1) = 0; + R_rect_00.at(3, 2) = 0; + R_rect_00.at(3, 3) = 1; + + P_rect_00.at(0, 0) = 7.215377e+02; + P_rect_00.at(0, 1) = 0.000000e+00; + P_rect_00.at(0, 2) = 6.095593e+02; + P_rect_00.at(0, 3) = 0.000000e+00; + P_rect_00.at(1, 0) = 0.000000e+00; + P_rect_00.at(1, 1) = 7.215377e+02; + P_rect_00.at(1, 2) = 1.728540e+02; + P_rect_00.at(1, 3) = 0.000000e+00; + P_rect_00.at(2, 0) = 0.000000e+00; + P_rect_00.at(2, 1) = 0.000000e+00; + P_rect_00.at(2, 2) = 1.000000e+00; + P_rect_00.at(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 lidarPoints; + readLidarPts("../dat/C51_LidarPts_0000.dat", lidarPoints); + + // store calibration data in OpenCV matrices + cv::Mat P_rect_00(3, 4, cv::DataType::type); // 3x4 projection matrix after rectification + cv::Mat R_rect_00(4, 4, cv::DataType::type); // 3x3 rectifying rotation to make image planes co-planar + cv::Mat RT(4, 4, cv::DataType::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::type); + cv::Mat Y(3, 1, cv::DataType::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(0, 0) = it->x; + X.at(1, 0) = it->y; + X.at(2, 0) = it->z; + X.at(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(0, 0) / Y.at(2, 0); + pt.y = Y.at(1, 0) / Y.at(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(); +} \ No newline at end of file diff --git a/src/show_lidar_top_view.cpp b/src/show_lidar_top_view.cpp new file mode 100644 index 0000000..a95a8a7 --- /dev/null +++ b/src/show_lidar_top_view.cpp @@ -0,0 +1,70 @@ +#include +#include +#include +#include +#include + +#include "structIO.hpp" + +using namespace std; + +void showLidarTopview() +{ + std::vector 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: "< +#include +#include +#include +#include "structIO.hpp" + +using namespace std; + + +/* TEMPLATES */ +template void write_pod(std::ofstream& out, T& t) +{ + out.write(reinterpret_cast(&t), sizeof(T)); +} + + +template void read_pod(std::ifstream& in, T& t) +{ + in.read(reinterpret_cast(&t), sizeof(T)); +} + + +template void read_pod_vector(std::ifstream& in, std::vector& 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 void write_pod_vector(std::ofstream& out, std::vector& vect) +{ + long size = vect.size(); + write_pod(out, size); + for(auto it=vect.begin(); it!=vect.end(); ++it) + { + write_pod(out,*it); + } +} + + + + +/* DATATYPE WRAPPERS */ + +void writeLidarPts(std::vector &input, const char* fileName) +{ + std::ofstream out(fileName); + write_pod_vector(out, input); + out.close(); +} + + +void readLidarPts(const char* fileName, std::vector &output) +{ + std::ifstream in(fileName); + read_pod_vector(in, output); +} + + +void writeKeypoints(std::vector &input, const char* fileName) +{ + std::ofstream out(fileName); + write_pod_vector(out, input); + out.close(); +} + + +void readKeypoints(const char* fileName, std::vector &output) +{ + std::ifstream in(fileName); + read_pod_vector(in, output); +} + + +void writeKptMatches(std::vector &input, const char* fileName) +{ + std::ofstream out(fileName); + write_pod_vector(out, input); + out.close(); +} + + +void readKptMatches(const char* fileName, std::vector &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(); +} + diff --git a/src/structIO.hpp b/src/structIO.hpp new file mode 100644 index 0000000..febc176 --- /dev/null +++ b/src/structIO.hpp @@ -0,0 +1,25 @@ +#ifndef structIO_hpp +#define structIO_hpp + +#include +#include "dataStructures.h" + +void writeLidarPts(std::vector &input, const char* fileName); +void readLidarPts(const char* fileName, std::vector &output); + +void writeKeypoints(std::vector &input, const char* fileName); +void readKeypoints(const char* fileName, std::vector &output); + +void writeKptMatches(std::vector &input, const char* fileName); +void readKptMatches(const char* fileName, std::vector &output); + +void writeDescriptors(cv::Mat &input, const char* fileName); +void readDescriptors(const char* fileName, cv::Mat &output); + + +template void write_pod(std::ofstream& out, T& t); +template void read_pod(std::ifstream& in, T& t); +template void read_pod_vector(std::ifstream& in, std::vector& vect); +template void write_pod_vector(std::ofstream& out, std::vector& vect); + +#endif /* structIO_hpp */