├── .gitignore ├── LICENSE ├── README.md ├── kitti360scripts ├── __init__.py ├── devkits │ ├── README.md │ ├── __init__.py │ ├── accumuLaser │ │ ├── CMakeLists.txt │ │ ├── README.md │ │ ├── cmake │ │ │ ├── FindOpenCV.cmake │ │ │ └── HandleLibraryTypes.cmake │ │ └── src │ │ │ ├── accumulation.cpp │ │ │ ├── accumulation.h │ │ │ ├── commons.cpp │ │ │ ├── commons.h │ │ │ ├── kdtree_nano.h │ │ │ ├── main.cpp │ │ │ ├── matrix.cpp │ │ │ ├── matrix.h │ │ │ ├── nanoflann.hpp │ │ │ ├── utils.cpp │ │ │ └── utils.h │ ├── commons │ │ ├── __init__.py │ │ └── loadCalibration.py │ ├── convertOxtsPose │ │ ├── .gitignore │ │ ├── matlab │ │ │ ├── convertOxtsToPose.m │ │ │ ├── convertPoseToOxts.m │ │ │ ├── latToScale.m │ │ │ ├── latlonToMercator.m │ │ │ ├── loadOxtsData.m │ │ │ ├── loadPoses.m │ │ │ ├── loadTimestamps.m │ │ │ ├── mercatorToLatlon.m │ │ │ ├── postprocessPoses.m │ │ │ ├── testOxtsToPose.m │ │ │ └── testPoseToOxts.m │ │ └── python │ │ │ ├── convertOxtsToPose.py │ │ │ ├── convertPoseToOxts.py │ │ │ ├── data.py │ │ │ ├── testOxtsToPose.py │ │ │ ├── testPoseToOxts.py │ │ │ └── utils.py │ ├── scripts │ │ └── run_accumulation.sh │ └── singularity │ │ ├── accumuLaser.recipe │ │ └── install_opencv.sh ├── evaluation │ ├── addToConfusionMatrix.pyx │ ├── addToConfusionMatrix_impl.c │ ├── novel_view_synthesis │ │ ├── README.md │ │ ├── data │ │ │ └── 2013_05_28_drive_train_drop50_frames.txt │ │ ├── evalNovelViewSynthesis.py │ │ └── metric.py │ ├── semantic_2d │ │ ├── README.md │ │ ├── __init__.py │ │ ├── evalInstanceLevelSemanticLabeling.py │ │ ├── evalPixelLevelSemanticLabeling.py │ │ ├── instance.py │ │ └── instances2dict.py │ ├── semantic_3d │ │ ├── README.md │ │ ├── evalDetection.py │ │ ├── evalInstanceLevelSemanticLabeling.py │ │ ├── evalPointLevelSemanticLabeling.py │ │ ├── evalSceneCompletion.py │ │ └── prepare_train_val_windows.py │ └── semantic_slam │ │ ├── CMakeLists.txt │ │ ├── README.md │ │ ├── evalSemanticSLAM.sh │ │ ├── evalTrajectory.sh │ │ ├── sparsify │ │ ├── CMakeLists.txt │ │ ├── README.md │ │ ├── kdtree.cpp │ │ ├── kdtree.h │ │ ├── main.cpp │ │ ├── run.sh │ │ ├── utils.cpp │ │ └── utils.h │ │ ├── src │ │ ├── accuracy.cc │ │ ├── accuracy.h │ │ ├── commons.cpp │ │ ├── commons.h │ │ ├── completeness.cc │ │ ├── completeness.h │ │ ├── main.cc │ │ ├── pose.cc │ │ ├── pose.h │ │ ├── semantic.cc │ │ ├── semantic.h │ │ └── util.h │ │ └── test_data │ │ ├── orbslam │ │ ├── test_poses_0.txt │ │ ├── test_poses_1.txt │ │ ├── test_poses_2.txt │ │ └── test_poses_3.txt │ │ └── semantic_suma │ │ ├── test_poses_0.txt │ │ ├── test_poses_1.txt │ │ ├── test_poses_2.txt │ │ └── test_poses_3.txt ├── helpers │ ├── __init__.py │ ├── annotation.py │ ├── csHelpers.py │ ├── curlVelodyneData.pyx │ ├── curlVelodyneData_impl.c │ ├── data.py │ ├── labels.py │ ├── ply.py │ ├── pointcloud.py │ └── project.py └── viewer │ ├── __init__.py │ ├── icons │ ├── back.png │ ├── disp.png │ ├── exit.png │ ├── filepath.png │ ├── help19.png │ ├── minus.png │ ├── next.png │ ├── open.png │ ├── play.png │ ├── plus.png │ ├── shuffle.png │ ├── switch.png │ ├── switch.svg │ └── zoom.png │ ├── kitti360Viewer.py │ ├── kitti360Viewer3D.py │ └── kitti360Viewer3DRaw.py ├── setup.cfg └── setup.py /.gitignore: -------------------------------------------------------------------------------- 1 | __pycache__/ 2 | *.py[cod] 3 | README.html 4 | *build/ 5 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2021 Autonomous Vision Group 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # The KITTI-360 Dataset 2 | 3 | This repository contains scripts for inspection of the KITTI-360 dataset. This large-scale dataset contains 320k images and 100k laser scans in a driving distance of 73.7km. We annotate both static and dynamic 3D scene elements with rough bounding primitives and transfer this information into the image domain, resulting in dense semantic & instance annotations on both 3D point clouds and 2D images. 4 | 5 | Details and download are available at: www.cvlibs.net/datasets/kitti-360 6 | 7 | ## Dataset Structure 8 | 9 | Dataset structure and data formats are available at: www.cvlibs.net/datasets/kitti-360/documentation.php 10 | 11 | ## Scripts 12 | 13 | ### Installation 14 | 15 | Install `kitti360Scripts` with `pip` 16 | ``` 17 | pip install git+https://github.com/autonomousvision/kitti360Scripts.git 18 | ``` 19 | 20 | For the 2D graphical tools you additionally need to install 21 | ``` 22 | sudo apt install python-tk python-qt5 23 | ``` 24 | 25 | We use open3D to visualize 3D point clouds and 3D bounding boxes: 26 | ``` 27 | pip install open3d 28 | ``` 29 | 30 | ### Usage 31 | 32 | This scripts contains helpers for loading and visualizing our dataset. For inspection, please download the dataset and add the root directory to your system path at first: 33 | 34 | ``` 35 | export KITTI360_DATASET=/PATH/TO/THE/DATASET 36 | ``` 37 | 38 | You can inspect the 2D images and labels using the following tool: 39 | ``` 40 | cd kitti360scripts/viewer 41 | python kitti360Viewer.py 42 | ``` 43 | 44 | You can visualize the 3D fused point clouds and labels using the following tool: 45 | ``` 46 | cd kitti360scripts/viewer 47 | python kitti360Viewer3D.py -sequence ${sequence} 48 | ``` 49 | 50 | ### Package Content 51 | 52 | The package is structured as follows 53 | - `helpers`: helper files that are included by other scripts 54 | - `viewer`: view the 2D image & labels and the 3D fused point clouds & labels 55 | 56 | Note that all files have a small documentation at the top. Most important files 57 | - `helpers/labels.py`: central file defining the IDs of all semantic classes and providing mapping between various class properties. 58 | - `helpers/annotations.py`: central file containing the loaders for all labels including 3D bounding boxes and fused point clouds 59 | 60 | ## Acknowledgment 61 | 62 | The 2D graphical tool is adapted from Cityscapes. 63 | 64 | ## Reference 65 | 66 | If you find this code or our dataset helpful in your research, please use the following BibTeX entry. 67 | 68 | ``` 69 | @article{Liao2021ARXIV, 70 | title = {{KITTI}-360: A Novel Dataset and Benchmarks for Urban Scene Understanding in 2D and 3D}, 71 | author = {Yiyi Liao and Jun Xie and Andreas Geiger}, 72 | journal = {arXiv preprint arXiv:2109.13410}, 73 | year = {2021}, 74 | } 75 | ``` 76 | 77 | ## Contact 78 | 79 | Please feel free to contact us with any questions, suggestions or comments: 80 | 81 | * Yiyi Liao, Andreas Geiger 82 | * yiyi.liao@tue.mpg.de, a.geiger@uni-tuebingen.de 83 | * www.cvlibs.net/datasets/kitti-360 84 | 85 | ## License 86 | 87 | Our __utility scripts__ in this repository are released under the following MIT license. 88 | 89 | --- 90 | 91 | MIT License 92 | 93 | Copyright (c) 2021 Autonomous Vision Group 94 | 95 | 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: 96 | 97 | The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 98 | 99 | 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. 100 | 101 | --- 102 | 103 | Our __datasets__ and __benchmarks__ are copyright by us and published under the [Creative Commons Attribution-NonCommercial-ShareAlike 3.0 License](https://creativecommons.org/licenses/by-nc-sa/3.0/). This means that you must attribute the work in the manner specified by the authors, you may not use this work for commercial purposes and if you alter, transform, or build upon this work, you may distribute the resulting work only under the same license. 104 | 105 | -------------------------------------------------------------------------------- /kitti360scripts/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/autonomousvision/kitti360Scripts/40ee015de8277b2d907d32a30cdbd97072c7d7c3/kitti360scripts/__init__.py -------------------------------------------------------------------------------- /kitti360scripts/devkits/README.md: -------------------------------------------------------------------------------- 1 | ## Development toolkits 2 | 3 | This folder contains scripts and code for developments on the KITTI-360 dataset. Please check out the READMEs in each folder for more details. 4 | 5 | 6 | ### Folder structures 7 | 8 | 9 | ##### accumuLaser/ 10 | 11 | Accumulate the raw Velodyne and Sick 3D scans 12 | 13 | ##### scripts/ 14 | 15 | Scripts to run the demos 16 | 17 | ##### singularity/ 18 | 19 | Singularity recipes to build containers 20 | 21 | ### License 22 | --- 23 | 24 | 25 | Copyright 2020 Autonomous Vision Group 26 | 27 | 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: 28 | 29 | The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 30 | 31 | 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. 32 | 33 | --- 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /kitti360scripts/devkits/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/autonomousvision/kitti360Scripts/40ee015de8277b2d907d32a30cdbd97072c7d7c3/kitti360scripts/devkits/__init__.py -------------------------------------------------------------------------------- /kitti360scripts/devkits/accumuLaser/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0) 2 | 3 | project(point_accumulator) 4 | 5 | SET(CMAKE_BUILD_TYPE "RELEASE") 6 | 7 | set(CMAKE_MODULE_PATH ${CMAKE_SOURCE_DIR}/cmake) 8 | 9 | find_package(OpenCV REQUIRED) 10 | find_package(Eigen3 3.3 REQUIRED NO_MODULE) 11 | 12 | # OPENCV 13 | set(OPENCV_LIBS ${OPENCV_CORE_LIBRARY} ${OPENCV_CALIB3D_LIBRARY}) 14 | 15 | # include 16 | include_directories(${OPENCV_INCLUDE_DIRS}) 17 | 18 | add_executable(point_accumulator src/main.cpp src/commons.cpp src/accumulation.cpp src/utils.cpp) 19 | target_link_libraries(point_accumulator ${OPENCV_LIBS} Eigen3::Eigen) 20 | -------------------------------------------------------------------------------- /kitti360scripts/devkits/accumuLaser/README.md: -------------------------------------------------------------------------------- 1 | ## Accumulation of raw 3D scans 2 | 3 | This folder contains code to accumulate raw 3D velodyne/sick scans. 4 | 5 | ### Dependencies 6 | 7 | * __OpenCV__ 2.4: 8 | Install according to the following doc: 9 | http://docs.opencv.org/2.4/doc/tutorials/introduction/linux_install/linux_install.html 10 | 11 | * __Eigen__ 3.3.1: 12 | 13 | ``` 14 | sudo apt install libeigen3-dev 15 | ``` 16 | 17 | It is recommended to use a singularity container. The recipe can be found in the ```../singularity/``` folder. You can build a container by 18 | 19 | ``` 20 | sudo singularity build accumuLaser.simg accumuLaser.recipe 21 | ``` 22 | 23 | 24 | ### Installation 25 | 26 | ``` 27 | mkdir build && cd build 28 | cmake .. 29 | make -j4 30 | ``` 31 | 32 | 33 | ### Accumulation 34 | 35 | #### Input preparation 36 | 37 | Please make sure that you have downloaded the following folders from our website and they are located in ```KITTI360_DATASET```. 38 | 39 | * calibrations 40 | * data_3d_raw 41 | * data_poses 42 | 43 | Please check if you have downloaded the __data_3d_timestamps.zip__ from the updated download script. It contains timestamps of our raw 3D scans which are required for accumulation. 44 | 45 | #### Run 46 | 47 | You can find scripts to run the accumulation under the ```../scripts``` folder. 48 | 49 | ``` 50 | $cd ../scripts/ 51 | $./run_accumulation.sh $KITTI360_DATASET $OUTPUT_DIR $SEQUENCE $FIRST_FRAME $LAST_FRAME $DATA_SOURCE 52 | ``` 53 | 54 | Here ```DATA_SOURCE``` specifies the type of laser scans to accumulate: 55 | 56 | * 0: accumulate sick scans only; 57 | * 1: accumulate velodyne scans only; 58 | * 2: accumulate both sick and velodyne scans. 59 | 60 | Here is an example: 61 | ``` 62 | ./run_accumulation.sh ./KITTI-360 output 2013_05_28_drive_0003_sync 2 282 2 63 | ``` 64 | 65 | Here is the example if you use the singularity container: 66 | ``` 67 | singularity exec ../singularity/accumuLaser.simg ./run_accumulation.sh ./KITTI-360 output 2013_05_28_drive_0003_sync 2 282 2 68 | ``` 69 | 70 | 71 | #### Output 72 | 73 | The output will be saved in ```$OUTPUT_DIR/$SEQUENCE_${FIRST_FRAME}_${LAST_FRAME}```, including the following files: 74 | 75 | * lidar_points_{type}.dat: Each row contains a 3D point in the format of ```x y z r g b```, where the color is generated according to the height, i.e., the ```z``` axis. 76 | * lidar_timestamp_{type}.dat: Each row contains the timestamp of the 3D point specified in the same row of lidar_points_{type}.dat. 77 | * lidar_loc.dat: Each row contains the system pose when the 3D point specified in the same row of lidar_points_{type}.dat is captured. 78 | * lidar_pose.dat: This contains the system poses from ```FIRST_FRAME``` to ```LAST_FRAME```. 79 | 80 | Here ```type``` can be _all_, _velodyne_ or _sick_ depending on the ```DATA_SOURCE``` argument. 81 | -------------------------------------------------------------------------------- /kitti360scripts/devkits/accumuLaser/cmake/FindOpenCV.cmake: -------------------------------------------------------------------------------- 1 | include(FindPackageHandleStandardArgs) 2 | include(HandleLibraryTypes) 3 | 4 | set(OpenCV_IncludeSearchPaths 5 | /usr/local/include 6 | ) 7 | 8 | set(OpenCV_LibrarySearchPaths 9 | /usr/local/lib 10 | ) 11 | 12 | find_path(OPENCV_INCLUDE_DIRS opencv2/opencv.hpp 13 | PATHS ${OpenCV_IncludeSearchPaths} NO_DEFAULT_PATH 14 | ) 15 | 16 | find_library(OPENCV_CALIB3D_LIBRARY 17 | NAMES opencv_calib3d 18 | PATHS ${OpenCV_LibrarySearchPaths} NO_DEFAULT_PATH 19 | ) 20 | 21 | find_library(OPENCV_CONTRIB_LIBRARY 22 | NAMES opencv_contrib 23 | PATHS ${OpenCV_LibrarySearchPaths} NO_DEFAULT_PATH 24 | ) 25 | 26 | find_library(OPENCV_CORE_LIBRARY 27 | NAMES opencv_core 28 | PATHS ${OpenCV_LibrarySearchPaths} NO_DEFAULT_PATH 29 | ) 30 | 31 | find_library(OPENCV_FEATURES2D_LIBRARY 32 | NAMES opencv_features2d 33 | PATHS ${OpenCV_LibrarySearchPaths} NO_DEFAULT_PATH 34 | ) 35 | 36 | find_library(OPENCV_FLANN_LIBRARY 37 | NAMES opencv_flann 38 | PATHS ${OpenCV_LibrarySearchPaths} NO_DEFAULT_PATH 39 | ) 40 | 41 | find_library(OPENCV_GPU_LIBRARY 42 | NAMES opencv_gpu 43 | PATHS ${OpenCV_LibrarySearchPaths} NO_DEFAULT_PATH 44 | ) 45 | 46 | find_library(OPENCV_HIGHGUI_LIBRARY 47 | NAMES opencv_highgui 48 | PATHS ${OpenCV_LibrarySearchPaths} NO_DEFAULT_PATH 49 | ) 50 | 51 | find_library(OPENCV_IMGPROC_LIBRARY 52 | NAMES opencv_imgproc 53 | PATHS ${OpenCV_LibrarySearchPaths} NO_DEFAULT_PATH 54 | ) 55 | 56 | find_library(OPENCV_LEGACY_LIBRARY 57 | NAMES opencv_legacy 58 | PATHS ${OpenCV_LibrarySearchPaths} NO_DEFAULT_PATH 59 | ) 60 | 61 | find_library(OPENCV_ML_LIBRARY 62 | NAMES opencv_ml 63 | PATHS ${OpenCV_LibrarySearchPaths} NO_DEFAULT_PATH 64 | ) 65 | 66 | find_library(OPENCV_OBJDETECT_LIBRARY 67 | NAMES opencv_objdetect 68 | PATHS ${OpenCV_LibrarySearchPaths} NO_DEFAULT_PATH 69 | ) 70 | 71 | find_library(OPENCV_VIDEO_LIBRARY 72 | NAMES opencv_video 73 | PATHS ${OpenCV_LibrarySearchPaths} NO_DEFAULT_PATH 74 | ) 75 | 76 | find_library(OPENCV_NONFREE_LIBRARY 77 | NAMES opencv_nonfree 78 | PATHS ${OpenCV_LibrarySearchPaths} NO_DEFAULT_PATH 79 | ) 80 | 81 | # Handle the REQUIRED argument and set the _FOUND variable 82 | find_package_handle_standard_args(OpenCV "Could NOT find opencv_calib3d (OpenCV)" 83 | OPENCV_CALIB3D_LIBRARY 84 | ) 85 | find_package_handle_standard_args(OpenCV "Could NOT find opencv_contrib (OpenCV)" 86 | OPENCV_CONTRIB_LIBRARY 87 | ) 88 | find_package_handle_standard_args(OpenCV "Could NOT find opencv_core (OpenCV)" 89 | OPENCV_INCLUDE_DIRS 90 | OPENCV_CORE_LIBRARY 91 | ) 92 | find_package_handle_standard_args(OpenCV "Could NOT find opencv_gpu (OpenCV)" 93 | OPENCV_GPU_LIBRARY 94 | ) 95 | find_package_handle_standard_args(OpenCV "Could NOT find opencv_highgui (OpenCV)" 96 | OPENCV_HIGHGUI_LIBRARY 97 | ) 98 | find_package_handle_standard_args(OpenCV "Could NOT find opencv_imgproc (OpenCV)" 99 | OPENCV_IMGPROC_LIBRARY 100 | ) 101 | find_package_handle_standard_args(OpenCV "Could NOT find opencv_legacy (OpenCV)" 102 | OPENCV_LEGACY_LIBRARY 103 | ) 104 | find_package_handle_standard_args(OpenCV "Could NOT find opencv_ml (OpenCV)" 105 | OPENCV_ML_LIBRARY 106 | ) 107 | 108 | 109 | # Collect optimized and debug libraries 110 | handle_library_types(OPENCV_CONTRIB_LIBRARY) 111 | handle_library_types(OPENCV_CORE_LIBRARY) 112 | handle_library_types(OPENCV_FEATURES2D_LIBRARY) 113 | handle_library_types(OPENCV_FLANN_LIBRARY) 114 | handle_library_types(OPENCV_GPU_LIBRARY) 115 | handle_library_types(OPENCV_HIGHGUI_LIBRARY) 116 | handle_library_types(OPENCV_IMGPROC_LIBRARY) 117 | handle_library_types(OPENCV_LEGACY_LIBRARY) 118 | handle_library_types(OPENCV_ML_LIBRARY) 119 | handle_library_types(OPENCV_OBJDETECT_LIBRARY) 120 | handle_library_types(OPENCV_VIDEO_LIBRARY) 121 | 122 | mark_as_advanced( 123 | OPENCV_INCLUDE_DIRS 124 | 125 | OPENCV_CONTRIB_LIBRARY 126 | OPENCV_CORE_LIBRARY 127 | OPENCV_FEATURES2D_LIBRARY 128 | OPENCV_FLANN_LIBRARY 129 | OPENCV_GPU_LIBRARY 130 | OPENCV_HIGHGUI_LIBRARY 131 | OPENCV_IMGPROC_LIBRARY 132 | OPENCV_LEGACY_LIBRARY 133 | OPENCV_ML_LIBRARY 134 | OPENCV_OBJDETECT_LIBRARY 135 | OPENCV_VIDEO_LIBRARY 136 | ) 137 | 138 | -------------------------------------------------------------------------------- /kitti360scripts/devkits/accumuLaser/cmake/HandleLibraryTypes.cmake: -------------------------------------------------------------------------------- 1 | function(handle_library_types _name) 2 | # Additional libraries can be added as additional arguments 3 | if(${_name}_LIBRARY_DEBUG AND ${_name}_LIBRARY_OPTIMIZED) 4 | set(${_name}_LIBRARY 5 | optimized ${${_name}_LIBRARY_OPTIMIZED} ${ARGN} 6 | debug ${${_name}_LIBRARY_DEBUG} ${ARGN} 7 | PARENT_SCOPE 8 | ) 9 | else() 10 | set(${_name}_LIBRARY 11 | ${${_name}_LIBRARY_OPTIMIZED} ${ARGN} 12 | PARENT_SCOPE 13 | ) 14 | endif() 15 | endfunction(handle_library_types) 16 | -------------------------------------------------------------------------------- /kitti360scripts/devkits/accumuLaser/src/accumulation.h: -------------------------------------------------------------------------------- 1 | #ifndef ACCUMULATION_H_ 2 | #define ACCUMULATION_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | 11 | using namespace std; 12 | 13 | class PointAccumulation { 14 | public: 15 | 16 | string rootDir; // root directory of the laser data 17 | string outputDir; 18 | string sequenceName; // sequence name (the laser data will be saved in root_dir/sequence) 19 | int firstFrame; // starting frame number 20 | int lastFrame; // ending frame number 21 | float travelPadding; // padding distance (in meters) 22 | int sourceType; // source of data (0: sick only, 1: velodyne only, 2: both) 23 | float minDistDense; // point cloud resolution in meter, optional default = 0.02 24 | bool verbose; // boolean number to indicate if display the msg, optional default = 1 25 | 26 | float outDistDense; // max distance for point to any neighbor to be inlier 27 | float maxPointDist; // max distance for 3d points to any of the poses 28 | 29 | int firstFrameWindow; 30 | int lastFrameWindow; 31 | 32 | string baseDir, calibDir, poseDir; 33 | 34 | // transformation matrices 35 | vector Tr_cam_pose; // cam0x -> pose 36 | Eigen::Matrix4d Tr_cam_velo; // cam00 -> velo 37 | Eigen::Matrix4d Tr_sick_velo; // sick -> velo 38 | vector Tr_pose_world; // global poses of the vehicle 39 | vector Tr_pose_index; // index of the global poses 40 | Eigen::Matrix4d Tr_velo_pose; // velo -> pose 41 | 42 | // timestamps 43 | vector sickTimestamps; 44 | vector veloTimestamps; 45 | 46 | Eigen::MatrixXd Md; // Accumulated point cloud 47 | Eigen::MatrixXd Ll; // Accumulated sensor locations 48 | Eigen::MatrixXd Md_prev; 49 | Eigen::MatrixXd Ll_prev; 50 | vector Tr_velo_window; // global poses of the velodyne within [firstFrame, lastFrame] 51 | vector Vp; // vehicle translations 52 | vector Ts; // timestamp of accumulated point cloud 53 | vector Fidx; // vector of valid frames 54 | vector Ap; // vector of dense point cloud at valid frames 55 | 56 | 57 | // output file names 58 | char outputPath[256]; 59 | char output_file[256]; 60 | char output_file_timestamp[256]; 61 | char output_file_loc[256]; 62 | char output_file_pose[256]; 63 | 64 | PointAccumulation(string root_dir, string output_dir, string sequence_, int first_frame, int last_frame, 65 | float travel_padding, int source_, float min_dist_dense=0.02, bool verbose_=1) 66 | { 67 | // initialization 68 | rootDir = root_dir; 69 | outputDir = output_dir; 70 | sequenceName = sequence_; 71 | firstFrame = first_frame; 72 | lastFrame = last_frame; 73 | travelPadding = travel_padding; 74 | sourceType = source_; 75 | minDistDense = min_dist_dense; 76 | verbose = verbose_; 77 | 78 | outDistDense = 0.2; 79 | maxPointDist = 30; 80 | 81 | // intialize output 82 | Md.resize(3, 0); 83 | Ll.resize(3, 0); 84 | Md_prev.resize(3, 0); 85 | Ll_prev.resize(3, 0); 86 | Ts.clear(); 87 | Ap.clear(); 88 | 89 | // directories 90 | baseDir = rootDir + "/data_3d_raw/" + sequenceName; 91 | poseDir = rootDir + "/data_poses/" + sequenceName; 92 | calibDir = rootDir + "/calibration"; 93 | 94 | // output directory 95 | sprintf(outputPath, "%s/%s_%06d_%06d", outputDir.c_str(), sequenceName.c_str(), firstFrame, lastFrame); 96 | 97 | // output filenames 98 | if (sourceType == 2) { 99 | sprintf(output_file, "%s/lidar_points_all.dat", outputPath); 100 | sprintf(output_file_timestamp, "%s/lidar_timestamp_all.dat", outputPath); 101 | } 102 | else if (sourceType == 1){ 103 | sprintf(output_file, "%s/lidar_points_velodyne.dat", outputPath); 104 | sprintf(output_file_timestamp, "%s/lidar_timestamp_velodyne.dat", outputPath); 105 | } 106 | else{ 107 | sprintf(output_file, "%s/lidar_points_sick.dat", outputPath); 108 | sprintf(output_file_timestamp, "%s/lidar_timestamp_sick.dat", outputPath); 109 | } 110 | sprintf(output_file_loc, "%s/lidar_loc.dat", outputPath); 111 | sprintf(output_file_pose, "%s/lidar_pose.dat", outputPath); 112 | } 113 | 114 | bool CreateOutputDir(void); 115 | 116 | bool LoadTransformations(void); 117 | bool LoadTimestamps(void); 118 | bool GetInterestedWindow(void); 119 | 120 | bool LoadSickData(int frame, Eigen::MatrixXd &data); 121 | bool LoadVelodyneData(int frame, float blind_splot_angle, Eigen::MatrixXd &data); 122 | void AddSickPoints(void); 123 | void AddVelodynePoints(void); 124 | 125 | void CurlParametersFromPoses(int frame, Eigen::Matrix4d Tr_pose_curr, Eigen::Vector3d &r, Eigen::Vector3d &t); 126 | int GetFrameIndex(int frame); 127 | void AddQdToMd(Eigen::MatrixXd &Qd, Eigen::MatrixXd &loc, int frame); 128 | void GetPointsInRange(void); 129 | 130 | void AddColorToPoints(void); 131 | 132 | void WriteToFiles(); 133 | 134 | 135 | 136 | }; 137 | 138 | #endif // ACCUMULATION_H_ 139 | -------------------------------------------------------------------------------- /kitti360scripts/devkits/accumuLaser/src/commons.cpp: -------------------------------------------------------------------------------- 1 | #include "commons.h" 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | #define KOGMO_TIMESTAMP_TICKSPERSECOND 1000000000.0 8 | 9 | 10 | /** 11 | * @brief LoadTransform Load a single transform from the file 12 | * @param filename 13 | * @param transform 4*4 matrix from file 14 | * @return True if loading is successful. 15 | */ 16 | bool LoadTransform(const char* filename, Eigen::Matrix4d &transform) { 17 | ifstream infile; 18 | infile.open(filename); 19 | 20 | if (!infile) { 21 | cout << "Failed to open transforms" << filename << endl; 22 | return false; 23 | } 24 | 25 | transform = Eigen::Matrix4d::Identity(); 26 | 27 | for (int i = 0; i < 12; i ++) { 28 | int xi = i/4; 29 | int yi = i%4; 30 | infile >> transform(xi, yi); 31 | } 32 | 33 | infile.close(); 34 | return true; 35 | } 36 | 37 | 38 | /** 39 | * @brief LoadCamPose Load poses from file. 40 | * @param filename 41 | * @param poses A vector of 4x4 matrix as the poses. 42 | * @return True if loading is successful. 43 | */ 44 | bool LoadCamPose(const char* filename, vector &poses) { 45 | ifstream infile; 46 | infile.open(filename); 47 | if (!infile) { 48 | cout << "Failed to open poses" << filename << endl; 49 | return false; 50 | } 51 | 52 | string trash; 53 | poses.resize(4); 54 | 55 | while (!infile.eof()) { 56 | infile >> trash; 57 | if (infile.eof()) break; 58 | if (trash.find("image_0") != string::npos) { 59 | Eigen::Matrix4d transform = Eigen::Matrix4d::Identity(); 60 | int index = atoi(trash.substr(7, 1).c_str()); 61 | for (int i = 0; i < 12; i ++) { 62 | int yi = i/4; 63 | int xi = i%4; 64 | if (!infile.eof()) { 65 | infile >> transform(yi, xi); 66 | } 67 | } 68 | poses[index] = transform; 69 | } 70 | } 71 | infile.close(); 72 | return true; 73 | } 74 | 75 | 76 | /** 77 | * @brief LoadCamPose Load poses from file. 78 | * @param filename 79 | * @param poses A vector of 4x4 matrix as the poses. 80 | * @param index Valid indices of the poses. 81 | * @return True if loading is successful. 82 | */ 83 | bool LoadPoses(const char* filename, vector &poses, 84 | vector &index) { 85 | ifstream infile; 86 | infile.open(filename); 87 | 88 | if (!infile) { 89 | cout << "Failed to open poses" << filename << endl; 90 | return false; 91 | } 92 | 93 | poses.clear(); 94 | index.clear(); 95 | 96 | Eigen::Matrix4d transform = Eigen::Matrix4d::Identity(); 97 | while (!infile.eof()) { 98 | int indx; 99 | infile >> indx; 100 | for (int i = 0; i < 12; i ++) { 101 | int yi = i/4; 102 | int xi = i%4; 103 | infile >> transform(yi, xi); 104 | } 105 | 106 | if (infile.eof()) break; 107 | poses.push_back(transform); 108 | index.push_back(indx); 109 | } 110 | 111 | infile.close(); 112 | assert(poses.size() == index.size()); 113 | return true; 114 | } 115 | 116 | /** 117 | * @brief LoadTimestamp Load timestamp from file. 118 | * @param filename 119 | * @param timestamps A vector of double values as the timestamps. 120 | * @return True if loading is successful. 121 | */ 122 | bool LoadTimestamp(const char* filename, vector ×tamps){ 123 | ifstream infile(filename); 124 | 125 | if (!infile) { 126 | cout << "Failed to open timestamps" << filename << endl; 127 | return false; 128 | } 129 | 130 | timestamps.clear(); 131 | string line; 132 | 133 | while (getline(infile, line)){ 134 | double ts = String2Timestamp(line.c_str()); 135 | if (ts==0){ 136 | cout << "Invalid timestamp at line " << line << endl; 137 | return false; 138 | } 139 | timestamps.push_back(ts); 140 | } 141 | 142 | return true; 143 | 144 | } 145 | 146 | /** 147 | * @brief String2Timestamp Convert timestamp in string to a double value. 148 | * @param time_str Timestamp in string format. 149 | * @return Timestamp in double value. 150 | */ 151 | double String2Timestamp (const char *time_str) { 152 | 153 | struct tm tmx; 154 | char ns_string[10] = ""; 155 | memset (&tmx,0,sizeof(struct tm)); 156 | 157 | // scan string 158 | int32_t err = sscanf (time_str, "%d-%d-%d%*[ _tT]%d:%d:%d.%9s", 159 | &tmx.tm_year, &tmx.tm_mon, &tmx.tm_mday, 160 | &tmx.tm_hour, &tmx.tm_min, &tmx.tm_sec, 161 | ns_string); 162 | 163 | // we need at least a date (time will be 00:00:00.0 then) 164 | if (err<3) { 165 | return 0; 166 | } 167 | 168 | // the ranges of those value are a horrible confusion, see mktime(3) 169 | tmx.tm_year -= +1900; 170 | tmx.tm_mon -= +1; 171 | tmx.tm_isdst = -1; 172 | time_t secs = mktime(&tmx); 173 | if (secs<0) { 174 | return 0; 175 | } 176 | char *ns_string_end; 177 | uint64_t ns = strtoul (ns_string,&ns_string_end,10); 178 | 179 | // calculate the correct decimal fraction (9 digits) 180 | // this is: ns *= pow ( 10, 9 - strlen (ns_string) ); 181 | // but prevent dependency from math-library 182 | for (int32_t i=ns_string_end-ns_string; i<9; i++) 183 | ns *= 10; 184 | 185 | double time_num = (double)ns + (double)secs * KOGMO_TIMESTAMP_TICKSPERSECOND; 186 | return time_num; 187 | 188 | } 189 | 190 | 191 | /** 192 | * @brief ReadMatrixCol Read a matrix from file with specified number of columns. 193 | * @param filename Filename of the matrix 194 | * @param matrix Eigen matrix with loaded value 195 | * @return True if loading is successful. 196 | */ 197 | bool ReadMatrixCol(const char *filename, const int cols, Eigen::MatrixXd &matrix) 198 | { 199 | 200 | // Read numbers from file into buffer. 201 | ifstream infile; 202 | infile.open(filename, ios::binary); 203 | 204 | if (!infile) { 205 | cout << "Failed to open file" << filename << endl; 206 | return false; 207 | } 208 | 209 | infile.seekg(0, infile.end); 210 | size_t length = infile.tellg()/sizeof(float); 211 | infile.seekg(0, infile.beg); 212 | 213 | float* buffer = new float[length]; 214 | infile.read( (char*)buffer, length*sizeof(float) ); 215 | infile.close(); 216 | 217 | if ( length % cols != 0 ){ 218 | cout << "Unmatched number of columns in file " << filename << endl; 219 | return false; 220 | } 221 | int rows = length / cols; 222 | 223 | infile.close(); 224 | 225 | // Populate matrix with numbers. 226 | matrix.resize(rows,cols); 227 | for (int i = 0; i < rows; i++) 228 | for (int j = 0; j < cols; j++) 229 | matrix(i,j) = buffer[ cols*i+j ]; 230 | 231 | return true; 232 | }; 233 | 234 | /** 235 | * @brief WriteMatrixToFile Write Eigen matrix to file 236 | * @param name Filename 237 | * @param mat Eigen matrix 238 | */ 239 | void WriteMatrixToFile(const char *name, Eigen::MatrixXd &mat) { 240 | ofstream outfile; 241 | outfile.open(name); 242 | outfile.precision(4); 243 | 244 | int rows = mat.rows(); 245 | int cols = mat.cols(); 246 | for (int i = 0; i < rows; i ++) { 247 | for (int j = 0; j < cols; j++) { 248 | outfile << std::fixed << mat(i,j) << " "; 249 | } 250 | outfile << "\n"; 251 | } 252 | outfile.close(); 253 | } 254 | 255 | /** 256 | * @brief WritePoseToFile Write pose to file 257 | * @param name Filename 258 | * @param idx Vector of frame numbers for all poses 259 | * @param poses Vector of poses 260 | */ 261 | void WritePoseToFile(const char *name, vector idx, vector &poses) { 262 | ofstream outfile; 263 | outfile.open(name); 264 | outfile.precision(4); 265 | 266 | int num = idx.size(); 267 | 268 | for (int i = 0; i < num; i ++) { 269 | outfile << idx[i] << " "; 270 | for (int l = 0; l < 16; l++) { 271 | int c = l/4; 272 | int r = l%4; 273 | outfile << std::fixed << poses[i](c,r) << " "; 274 | } 275 | outfile << "\n"; 276 | } 277 | outfile.close(); 278 | } 279 | 280 | /** 281 | * @brief WriteTimestampToFile Write timestamp to file 282 | * @param name Filename 283 | * @param timestamp Vector of timestamps 284 | */ 285 | void WriteTimestampToFile(const char *name, vector timestamp){ 286 | ofstream outfile; 287 | outfile.open(name); 288 | 289 | int num = timestamp.size(); 290 | for (int i=0; i 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | 12 | using namespace std; 13 | 14 | bool LoadTransform(const char* filename, Eigen::Matrix4d &transform); 15 | bool LoadCamPose(const char* filename, vector &poses); 16 | bool LoadPoses(const char* filename, vector &poses, 17 | vector &index); 18 | bool LoadTimestamp(const char* filename, vector ×tamps); 19 | double String2Timestamp(const char *time_str); 20 | bool ReadMatrixCol(const char *filename, const int cols, Eigen::MatrixXd &matrix); 21 | void WriteMatrixToFile(const char *name, Eigen::MatrixXd &mat); 22 | void WritePoseToFile(const char *name, vector idx, vector &poses); 23 | void WriteTimestampToFile(const char *name, vector timestamp); 24 | bool _mkdir(const char *dir); 25 | 26 | #endif // COMMONS_H_ -------------------------------------------------------------------------------- /kitti360scripts/devkits/accumuLaser/src/kdtree_nano.h: -------------------------------------------------------------------------------- 1 | #include "nanoflann.hpp" 2 | #include 3 | #include 4 | 5 | using namespace std; 6 | using namespace nanoflann; 7 | 8 | template 9 | struct Point 10 | { 11 | T x,y,z; 12 | Point(){} 13 | Point(T mx, T my, T mz) { 14 | x = mx; y = my; z = mz; 15 | } 16 | }; 17 | 18 | template 19 | struct PointCloud 20 | { 21 | std::vector > pts; 22 | 23 | // Must return the number of data points 24 | inline size_t kdtree_get_point_count() const { return pts.size(); } 25 | 26 | // Returns the distance between the vector "p1[0:size-1]" and the data point with index "idx_p2" stored in the class: 27 | inline T kdtree_distance(const T *p1, const size_t idx_p2,size_t /*size*/) const 28 | { 29 | const T d0=p1[0]-pts[idx_p2].x; 30 | const T d1=p1[1]-pts[idx_p2].y; 31 | const T d2=p1[2]-pts[idx_p2].z; 32 | return d0*d0+d1*d1+d2*d2; 33 | } 34 | 35 | // TODO 36 | 37 | // Returns the dim'th component of the idx'th point in the class: 38 | // Since this is inlined and the "dim" argument is typically an immediate value, the 39 | // "if/else's" are actually solved at compile time. 40 | inline T kdtree_get_pt(const size_t idx, int dim) const 41 | { 42 | if (dim==0) return pts[idx].x; 43 | else if (dim==1) return pts[idx].y; 44 | else return pts[idx].z; 45 | } 46 | 47 | // Optional bounding-box computation: return false to default to a standard bbox computation loop. 48 | // Return true if the BBOX was already computed by the class and returned in "bb" so it can be avoided to redo it again. 49 | // Look at bb.size() to find out the expected dimensionality (e.g. 2 or 3 for point clouds) 50 | template 51 | bool kdtree_get_bbox(BBOX& /*bb*/) const { return false; } 52 | 53 | /*PointCloud& operator=(const PointCloud &pcd) 54 | { 55 | for (int i = 0; i < pcd.pts.size(); i++) { 56 | this->pts.push_back(pcd.pts[i]); 57 | } 58 | return *this; 59 | }*/ 60 | 61 | }; 62 | 63 | template 64 | class KDtree 65 | { 66 | 67 | PointCloud cloud; 68 | 69 | public: 70 | typedef KDTreeSingleIndexAdaptor< 71 | L2_Simple_Adaptor > , 72 | PointCloud, 73 | 3 /* dim */ 74 | > 75 | kd_tree; 76 | 77 | 78 | kd_tree *tree; 79 | 80 | KDtree() { 81 | tree = new kd_tree(3 /*dim*/, cloud, KDTreeSingleIndexAdaptorParams(1000 /* max leaf */) ); 82 | tree->buildIndex(); 83 | } 84 | 85 | KDtree(PointCloud &pcd) { 86 | this->cloud = pcd; 87 | tree = new kd_tree(3 /*dim*/, cloud, KDTreeSingleIndexAdaptorParams(1000 /* max leaf */) ); 88 | tree->buildIndex(); 89 | } 90 | 91 | ~KDtree() { 92 | delete tree; 93 | } 94 | 95 | void knnSearch(const Point query, int k, std::vector &indices, std::vector &dist) 96 | { 97 | T q[3] = {query.x, query.y, query.z}; 98 | indices.resize(k); 99 | dist.resize(k); 100 | tree->knnSearch(&q[0], k, &indices[0], &dist[0]); 101 | } 102 | 103 | size_t radiusSearch(const Point query, T r, std::vector &indices, std::vector &dist) 104 | { 105 | std::vector > ret_matches; 106 | nanoflann::SearchParams params; 107 | //params.sorted = false; 108 | 109 | T q[3] = {query.x, query.y, query.z}; 110 | size_t nMatches = tree->radiusSearch(&q[0], r, ret_matches, params); 111 | 112 | for (size_t i = 0; i < ret_matches.size(); i++){ 113 | indices.push_back(ret_matches[i].first); 114 | dist.push_back(ret_matches[i].second); 115 | } 116 | return nMatches; 117 | } 118 | }; 119 | -------------------------------------------------------------------------------- /kitti360scripts/devkits/accumuLaser/src/main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include "accumulation.h" 4 | 5 | int main(int argc, char* argv[]){ 6 | 7 | if (argc < 8){ 8 | std::cerr << "Usage: " << argv[0] << " root_dir sequence output_dir first_frame last_frame \ 9 | travel_padding source [min_dist_dense] [verbose]" << std::endl; 10 | return 1; 11 | } 12 | 13 | std::string root_dir = argv[1]; 14 | std::string sequence = argv[2]; 15 | std::string output_dir = argv[3]; 16 | int first_frame = atoi(argv[4]); 17 | int last_frame = atoi(argv[5]); 18 | float travel_padding = atof(argv[6]); 19 | int source = atoi(argv[7]); 20 | float min_dist_dense = 0.02; 21 | bool verbose = true; 22 | 23 | if (argc > 8){ 24 | min_dist_dense = atof(argv[8]); 25 | } 26 | if (argc > 9 && atoi(argv[9])<1){ 27 | verbose = false; 28 | } 29 | 30 | 31 | PointAccumulation pointAcc(root_dir, output_dir, sequence, first_frame, last_frame, 32 | travel_padding, source, min_dist_dense, verbose); 33 | 34 | if (verbose) std::cout << "Initialization Done!" << std::endl; 35 | 36 | if (!pointAcc.CreateOutputDir()){ 37 | std::cerr << "Error: Unable to create the output directory!" << std::endl; 38 | return 1; 39 | } 40 | 41 | if (!pointAcc.LoadTransformations()){ 42 | std::cerr << "Error: Unable to load the calibrations!" << std::endl; 43 | return 1; 44 | } 45 | 46 | if (!pointAcc.GetInterestedWindow()){ 47 | std::cerr << "Error: Invalid window of interested!" << std::endl; 48 | return 1; 49 | } 50 | 51 | if (verbose) { 52 | std::cout << "Loaded " << pointAcc.Tr_pose_world.size() << " poses" << std::endl; 53 | } 54 | 55 | pointAcc.LoadTimestamps(); 56 | 57 | if (verbose) { 58 | std::cout << "Loaded " << pointAcc.sickTimestamps.size() << " sick timestamps" << std::endl; 59 | std::cout << "Loaded " << pointAcc.veloTimestamps.size() << " velo timestamps" << std::endl; 60 | } 61 | 62 | if (source==0 || source==2){ 63 | pointAcc.AddSickPoints(); 64 | } 65 | 66 | if (source==1 || source==2){ 67 | pointAcc.AddVelodynePoints(); 68 | } 69 | 70 | pointAcc.GetPointsInRange(); 71 | 72 | pointAcc.AddColorToPoints(); 73 | 74 | pointAcc.WriteToFiles(); 75 | 76 | 77 | return 0; 78 | } -------------------------------------------------------------------------------- /kitti360scripts/devkits/accumuLaser/src/matrix.h: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright 2011. All rights reserved. 3 | Institute of Measurement and Control Systems 4 | Karlsruhe Institute of Technology, Germany 5 | 6 | Authors: Andreas Geiger 7 | 8 | matrix is free software; you can redistribute it and/or modify it under the 9 | terms of the GNU General Public License as published by the Free Software 10 | Foundation; either version 2 of the License, or any later version. 11 | 12 | matrix is distributed in the hope that it will be useful, but WITHOUT ANY 13 | WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A 14 | PARTICULAR PURPOSE. See the GNU General Public License for more details. 15 | 16 | You should have received a copy of the GNU General Public License along with 17 | matrix; if not, write to the Free Software Foundation, Inc., 51 Franklin 18 | Street, Fifth Floor, Boston, MA 02110-1301, USA 19 | */ 20 | 21 | #ifndef MATRIX_H 22 | #define MATRIX_H 23 | 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | 30 | #ifndef _MSC_VER 31 | #include 32 | #else 33 | typedef __int8 int8_t; 34 | typedef __int16 int16_t; 35 | typedef __int32 int32_t; 36 | typedef __int64 int64_t; 37 | typedef unsigned __int8 uint8_t; 38 | typedef unsigned __int16 uint16_t; 39 | typedef unsigned __int32 uint32_t; 40 | typedef unsigned __int64 uint64_t; 41 | #endif 42 | 43 | #define endll endl << endl // double end line definition 44 | 45 | typedef double FLOAT; // double precision 46 | //typedef float FLOAT; // single precision 47 | 48 | class Matrix { 49 | 50 | public: 51 | 52 | // constructor / deconstructor 53 | Matrix (); // init empty 0x0 matrix 54 | Matrix (const int32_t m,const int32_t n); // init empty mxn matrix 55 | Matrix (const int32_t m,const int32_t n,const FLOAT* val_); // init mxn matrix with values from array 'val' 56 | Matrix (const Matrix &M); // creates deepcopy of M 57 | ~Matrix (); 58 | 59 | // assignment operator, copies contents of M 60 | Matrix& operator= (const Matrix &M); 61 | 62 | // copies submatrix of M into array 'val', default values copy whole row/column/matrix 63 | void getData(FLOAT* val_,int32_t i1=0,int32_t j1=0,int32_t i2=-1,int32_t j2=-1); 64 | 65 | // set or get submatrices of current matrix 66 | Matrix getMat(int32_t i1,int32_t j1,int32_t i2=-1,int32_t j2=-1); 67 | void setMat(const Matrix &M,const int32_t i,const int32_t j); 68 | 69 | // set sub-matrix to scalar (default 0), -1 as end replaces whole row/column/matrix 70 | void setVal(FLOAT s,int32_t i1=0,int32_t j1=0,int32_t i2=-1,int32_t j2=-1); 71 | 72 | // set (part of) diagonal to scalar, -1 as end replaces whole diagonal 73 | void setDiag(FLOAT s,int32_t i1=0,int32_t i2=-1); 74 | 75 | // clear matrix 76 | void zero(); 77 | 78 | // extract columns with given index 79 | Matrix extractCols (std::vector idx); 80 | 81 | // create identity matrix 82 | static Matrix eye (const int32_t m); 83 | void eye (); 84 | 85 | // create matrix with ones 86 | static Matrix ones(const int32_t m,const int32_t n); 87 | 88 | // create diagonal matrix with nx1 or 1xn matrix M as elements 89 | static Matrix diag(const Matrix &M); 90 | 91 | // returns the m-by-n matrix whose elements are taken column-wise from M 92 | static Matrix reshape(const Matrix &M,int32_t m,int32_t n); 93 | 94 | // create 3x3 rotation matrices (convention: http://en.wikipedia.org/wiki/Rotation_matrix) 95 | static Matrix rotMatX(const FLOAT &angle); 96 | static Matrix rotMatY(const FLOAT &angle); 97 | static Matrix rotMatZ(const FLOAT &angle); 98 | 99 | // simple arithmetic operations 100 | Matrix operator+ (const Matrix &M); // add matrix 101 | Matrix operator- (const Matrix &M); // subtract matrix 102 | Matrix operator* (const Matrix &M); // multiply with matrix 103 | Matrix operator* (const FLOAT &s); // multiply with scalar 104 | Matrix operator/ (const Matrix &M); // divide elementwise by matrix (or vector) 105 | Matrix operator/ (const FLOAT &s); // divide by scalar 106 | Matrix operator- (); // negative matrix 107 | Matrix operator~ (); // transpose 108 | FLOAT l2norm (); // euclidean norm (vectors) / frobenius norm (matrices) 109 | FLOAT mean (); // mean of all elements in matrix 110 | 111 | // complex arithmetic operations 112 | static Matrix cross (const Matrix &a, const Matrix &b); // cross product of two vectors 113 | static Matrix inv (const Matrix &M); // invert matrix M 114 | bool inv (); // invert this matrix 115 | FLOAT det (); // returns determinant of matrix 116 | bool solve (const Matrix &M,FLOAT eps=1e-20); // solve linear system M*x=B, replaces *this and M 117 | bool lu(int32_t *idx, FLOAT &d, FLOAT eps=1e-20); // replace *this by lower upper decomposition 118 | void svd(Matrix &U,Matrix &W,Matrix &V); // singular value decomposition *this = U*diag(W)*V^T 119 | 120 | // print matrix to stream 121 | friend std::ostream& operator<< (std::ostream& out,const Matrix& M); 122 | 123 | // direct data access 124 | FLOAT **val; 125 | int32_t m,n; 126 | 127 | private: 128 | 129 | void allocateMemory (const int32_t m_,const int32_t n_); 130 | void releaseMemory (); 131 | inline FLOAT pythag(FLOAT a,FLOAT b); 132 | 133 | }; 134 | 135 | #endif // MATRIX_H 136 | -------------------------------------------------------------------------------- /kitti360scripts/devkits/accumuLaser/src/utils.h: -------------------------------------------------------------------------------- 1 | #ifndef UTILS_H_ 2 | #define UTILS_H_ 3 | 4 | void SparsifyData (double *M,int32_t M_num,int32_t dim,int32_t* idx_sparse,int32_t &idx_size,double min_dist,double out_dist,int32_t idx_start); 5 | void ExtractCols(Eigen::MatrixXd &input, Eigen::MatrixXd &output, int32_t *idx_sparse, int32_t idx_size); 6 | void ExtractCols(Eigen::MatrixXd &input, Eigen::MatrixXd &output, vector idx); 7 | void ExtractRows(Eigen::MatrixXd &input, Eigen::MatrixXd &output, vector idx); 8 | 9 | void RemoveBlindSpot(Eigen::MatrixXd &matIn, Eigen::MatrixXd &matOut, float blind_splot_angle); 10 | void CropVelodyneData (Eigen::MatrixXd &matIn, Eigen::MatrixXd &matOut, float minDist, float maxDist); 11 | void CropVelodyneData (Eigen::MatrixXd &matIn, vector &idxOut, float minDist, float maxDist); 12 | void CurlVelodyneData (Eigen::MatrixXd &velo_in, Eigen::MatrixXd &velo_out, Eigen::Vector3d r, Eigen::Vector3d t); 13 | 14 | void GetHalfPoints(Eigen::MatrixXd &matIn, Eigen::MatrixXd &matOut, bool direction); 15 | void GetColorToHeight(Eigen::MatrixXd &pose, Eigen::MatrixXd &color); 16 | 17 | 18 | #endif //UTILS_H_ -------------------------------------------------------------------------------- /kitti360scripts/devkits/commons/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/autonomousvision/kitti360Scripts/40ee015de8277b2d907d32a30cdbd97072c7d7c3/kitti360scripts/devkits/commons/__init__.py -------------------------------------------------------------------------------- /kitti360scripts/devkits/commons/loadCalibration.py: -------------------------------------------------------------------------------- 1 | # Utils to load transformation to camera pose to system pose 2 | import os 3 | import numpy as np 4 | 5 | def checkfile(filename): 6 | if not os.path.isfile(filename): 7 | raise RuntimeError('%s does not exist!' % filename) 8 | 9 | def readVariable(fid,name,M,N): 10 | # rewind 11 | fid.seek(0,0) 12 | 13 | # search for variable identifier 14 | line = 1 15 | success = 0 16 | while line: 17 | line = fid.readline() 18 | if line.startswith(name): 19 | success = 1 20 | break 21 | 22 | # return if variable identifier not found 23 | if success==0: 24 | return None 25 | 26 | # fill matrix 27 | line = line.replace('%s:' % name, '') 28 | line = line.split() 29 | assert(len(line) == M*N) 30 | line = [float(x) for x in line] 31 | mat = np.array(line).reshape(M, N) 32 | 33 | return mat 34 | 35 | def loadCalibrationCameraToPose(filename): 36 | # check file 37 | checkfile(filename) 38 | 39 | # open file 40 | fid = open(filename,'r'); 41 | 42 | # read variables 43 | Tr = {} 44 | cameras = ['image_00', 'image_01', 'image_02', 'image_03'] 45 | lastrow = np.array([0,0,0,1]).reshape(1,4) 46 | for camera in cameras: 47 | Tr[camera] = np.concatenate((readVariable(fid, camera, 3, 4), lastrow)) 48 | 49 | # close file 50 | fid.close() 51 | return Tr 52 | 53 | 54 | def loadCalibrationRigid(filename): 55 | # check file 56 | checkfile(filename) 57 | 58 | lastrow = np.array([0,0,0,1]).reshape(1,4) 59 | return np.concatenate((np.loadtxt(filename).reshape(3,4), lastrow)) 60 | 61 | 62 | def loadPerspectiveIntrinsic(filename): 63 | # check file 64 | checkfile(filename) 65 | 66 | # open file 67 | fid = open(filename,'r'); 68 | 69 | # read variables 70 | Tr = {} 71 | intrinsics = ['P_rect_00', 'R_rect_00', 'P_rect_01', 'R_rect_01'] 72 | lastrow = np.array([0,0,0,1]).reshape(1,4) 73 | for intrinsic in intrinsics: 74 | if intrinsic.startswith('P_rect'): 75 | Tr[intrinsic] = np.concatenate((readVariable(fid, intrinsic, 3, 4), lastrow)) 76 | else: 77 | Tr[intrinsic] = readVariable(fid, intrinsic, 3, 3) 78 | 79 | # close file 80 | fid.close() 81 | 82 | return Tr 83 | 84 | if __name__=='__main__': 85 | 86 | if 'KITTI360_DATASET' in os.environ: 87 | kitti360Path = os.environ['KITTI360_DATASET'] 88 | else: 89 | kitti360Path = os.path.join(os.path.dirname( 90 | os.path.realpath(__file__)), '..', '..') 91 | 92 | fileCameraToPose = os.path.join(kitti360Path, 'calibration', 'calib_cam_to_pose.txt') 93 | Tr = loadCalibrationCameraToPose(fileCameraToPose) 94 | print('Loaded %s' % fileCameraToPose) 95 | print(Tr) 96 | 97 | fileCameraToVelo = os.path.join(kitti360Path, 'calibration', 'calib_cam_to_velo.txt') 98 | Tr = loadCalibrationRigid(fileCameraToVelo) 99 | print('Loaded %s' % fileCameraToVelo) 100 | print(Tr) 101 | 102 | fileSickToVelo = os.path.join(kitti360Path, 'calibration', 'calib_sick_to_velo.txt') 103 | Tr = loadCalibrationRigid(fileSickToVelo) 104 | print('Loaded %s' % fileSickToVelo) 105 | print(Tr) 106 | 107 | filePersIntrinsic = os.path.join(kitti360Path, 'calibration', 'perspective.txt') 108 | Tr = loadPerspectiveIntrinsic(filePersIntrinsic) 109 | print('Loaded %s' % filePersIntrinsic) 110 | print(Tr) 111 | -------------------------------------------------------------------------------- /kitti360scripts/devkits/convertOxtsPose/.gitignore: -------------------------------------------------------------------------------- 1 | matlab/output/ 2 | python/output/ 3 | -------------------------------------------------------------------------------- /kitti360scripts/devkits/convertOxtsPose/matlab/convertOxtsToPose.m: -------------------------------------------------------------------------------- 1 | function pose = convertOxtsToPose(oxts) 2 | % converts a list of oxts measurements into metric poses, 3 | % starting at (0,0,0) meters, OXTS coordinates are defined as 4 | % x = forward, y = right, z = down (see OXTS RT3000 user manual) 5 | % afterwards, pose{i} contains the transformation which takes a 6 | % 3D point in the i'th frame and projects it into the oxts 7 | % coordinates with the origin at a lake in Karlsruhe. 8 | 9 | single_value = ~iscell(oxts); 10 | if single_value 11 | oxts_ = oxts; 12 | clear oxts; 13 | oxts{1} = oxts_; 14 | end 15 | 16 | % origin in OXTS coordinate 17 | origin_oxts = [48.9843445, 8.4295857]; % lake in Karlsruhe 18 | 19 | % compute scale from lat value of the origin 20 | scale = latToScale(origin_oxts(1)); 21 | 22 | % origin in Mercator coordinate 23 | [origin(1,1) origin(2,1)] = latlonToMercator(origin_oxts(1),origin_oxts(2),scale); 24 | origin(3,1) = 0; 25 | 26 | pose = []; 27 | 28 | % for all oxts packets do 29 | for i=1:length(oxts) 30 | 31 | % if there is no data => no pose 32 | if isempty(oxts{i}) 33 | pose{i} = []; 34 | continue; 35 | end 36 | 37 | % translation vector 38 | [t(1,1) t(2,1)] = latlonToMercator(oxts{i}(1),oxts{i}(2),scale); 39 | t(3,1) = oxts{i}(3); 40 | 41 | % rotation matrix (OXTS RT3000 user manual, page 71/92) 42 | rx = oxts{i}(4); % roll 43 | ry = oxts{i}(5); % pitch 44 | rz = oxts{i}(6); % heading 45 | Rx = [1 0 0; 0 cos(rx) -sin(rx); 0 sin(rx) cos(rx)]; % base => nav (level oxts => rotated oxts) 46 | Ry = [cos(ry) 0 sin(ry); 0 1 0; -sin(ry) 0 cos(ry)]; % base => nav (level oxts => rotated oxts) 47 | Rz = [cos(rz) -sin(rz) 0; sin(rz) cos(rz) 0; 0 0 1]; % base => nav (level oxts => rotated oxts) 48 | R = Rz*Ry*Rx; 49 | 50 | % normalize translation 51 | t = t-origin; 52 | 53 | % add pose 54 | pose{i} = [R t;0 0 0 1]; 55 | 56 | end 57 | 58 | if single_value 59 | pose = pose{1}; 60 | end 61 | -------------------------------------------------------------------------------- /kitti360scripts/devkits/convertOxtsPose/matlab/convertPoseToOxts.m: -------------------------------------------------------------------------------- 1 | function oxts = convertPoseToOxts(pose) 2 | % converts a list of metric poses into oxts measurements, 3 | % starting at (0,0,0) meters, OXTS coordinates are defined as 4 | % x = forward, y = right, z = down (see OXTS RT3000 user manual) 5 | % afterwards, pose{i} contains the transformation which takes a 6 | % 3D point in the i'th frame and projects it into the oxts 7 | % coordinates with the origin at a lake in Karlsruhe. 8 | 9 | single_value = ~iscell(pose); 10 | if single_value 11 | pose_ = pose; 12 | clear pose; 13 | pose{1} = pose_; 14 | end 15 | 16 | % origin in OXTS coordinate 17 | origin_oxts = [48.9843445, 8.4295857]; % lake in Karlsruhe 18 | 19 | % compute scale from lat value of the origin 20 | scale = latToScale(origin_oxts(1)); 21 | 22 | % origin in Mercator coordinate 23 | [origin(1,1) origin(2,1)] = latlonToMercator(origin_oxts(1),origin_oxts(2),scale); 24 | origin(3,1) = 0; 25 | 26 | oxts = []; 27 | 28 | % for all oxts packets do 29 | for i=1:length(pose) 30 | 31 | % if there is no data => no pose 32 | if isempty(pose{i}) 33 | oxts{i} = []; 34 | continue; 35 | end 36 | 37 | % rotation and translation 38 | R = pose{i}(1:3, 1:3); 39 | t = pose{i}(1:3, 4); 40 | 41 | % unnormalize translation 42 | t = t+origin; 43 | 44 | % translation vector 45 | [lat, lon] = mercatorToLatlon(t(1), t(2), scale); 46 | alt = t(3); 47 | 48 | % rotation matrix (OXTS RT3000 user manual, page 71/92) 49 | yaw = atan2(R(2,1) , R(1,1)); 50 | pitch = atan2( - R(3,1) , sqrt(R(3,2)^2 + R(3,3)^2)); 51 | roll = atan2(R(3,2) , R(3,3)); 52 | 53 | % rx = oxts{i}(4); % roll 54 | % ry = oxts{i}(5); % pitch 55 | % rz = oxts{i}(6); % heading 56 | % Rx = [1 0 0; 0 cos(rx) -sin(rx); 0 sin(rx) cos(rx)]; % base => nav (level oxts => rotated oxts) 57 | % Ry = [cos(ry) 0 sin(ry); 0 1 0; -sin(ry) 0 cos(ry)]; % base => nav (level oxts => rotated oxts) 58 | % Rz = [cos(rz) -sin(rz) 0; sin(rz) cos(rz) 0; 0 0 1]; % base => nav (level oxts => rotated oxts) 59 | % R = Rz*Ry*Rx; 60 | 61 | % add oxts 62 | oxts{i} = [lat, lon, alt, roll, pitch, yaw]; 63 | 64 | end 65 | 66 | if single_value 67 | oxts = oxts{1}; 68 | end 69 | -------------------------------------------------------------------------------- /kitti360scripts/devkits/convertOxtsPose/matlab/latToScale.m: -------------------------------------------------------------------------------- 1 | function scale = latToScale(lat) 2 | % compute mercator scale from latitude 3 | 4 | scale = cos(lat * pi / 180.0); 5 | -------------------------------------------------------------------------------- /kitti360scripts/devkits/convertOxtsPose/matlab/latlonToMercator.m: -------------------------------------------------------------------------------- 1 | function [mx,my] = latlonToMercator(lat,lon,scale) 2 | % converts lat/lon coordinates to mercator coordinates using mercator scale 3 | 4 | er = 6378137; % average earth radius at the equator 5 | mx = scale * lon * pi * er / 180; 6 | my = scale * er * log( tan((90+lat) * pi / 360) ); 7 | -------------------------------------------------------------------------------- /kitti360scripts/devkits/convertOxtsPose/matlab/loadOxtsData.m: -------------------------------------------------------------------------------- 1 | function [oxts,ts] = loadOxtsData(oxts_dir,frames) 2 | % reads GPS/IMU data from files to memory. requires base directory 3 | % (=sequence directory as parameter). if frames is not specified, loads all frames. 4 | 5 | ts = []; 6 | 7 | if nargin==1 8 | 9 | ts = loadTimestamps(oxts_dir); 10 | oxts = []; 11 | for i=1:length(ts) 12 | oxts{i} = []; 13 | if ~isempty(ts{i}) 14 | try 15 | oxts{i} = dlmread([oxts_dir '/data/' num2str(i-1,'%010d') '.txt']); 16 | catch e 17 | oxts{i} = []; 18 | end 19 | end 20 | end 21 | 22 | else 23 | 24 | if length(frames)>1 25 | k = 1; 26 | oxts = []; 27 | for i=1:length(frames) 28 | try 29 | file_name = [oxts_dir '/data/' num2str(frames(i)-1,'%010d') '.txt']; 30 | oxts{k} = dlmread(file_name); 31 | catch e 32 | oxts{k} = []; 33 | end 34 | k=k+1; 35 | end 36 | 37 | % no cellarray for single value 38 | else 39 | file_name = [oxts_dir '/data/' num2str(frames(1)-1,'%010d') '.txt']; 40 | try 41 | oxts = dlmread(file_name); 42 | catch e 43 | oxts = []; 44 | end 45 | end 46 | 47 | end 48 | -------------------------------------------------------------------------------- /kitti360scripts/devkits/convertOxtsPose/matlab/loadPoses.m: -------------------------------------------------------------------------------- 1 | function [ts, poses] = loadPoses (pos_file) 2 | 3 | data = dlmread(pos_file); 4 | ts = data(:,1); 5 | pose = data(:,2:end); 6 | 7 | poses = []; 8 | for i = 1:size(pose, 1) 9 | poses{i} = [reshape(pose(i,:), [4, 3])'; 0 0 0 1]; 10 | end 11 | -------------------------------------------------------------------------------- /kitti360scripts/devkits/convertOxtsPose/matlab/loadTimestamps.m: -------------------------------------------------------------------------------- 1 | function ts = loadTimestamps(ts_dir) 2 | 3 | fid = fopen([ts_dir '/timestamps.txt']); 4 | col = textscan(fid,'%s\n',-1,'delimiter',','); 5 | ts = col{1}; 6 | fclose(fid); 7 | -------------------------------------------------------------------------------- /kitti360scripts/devkits/convertOxtsPose/matlab/mercatorToLatlon.m: -------------------------------------------------------------------------------- 1 | function [lat,lon] = mercatorToLatlon(mx,my,scale) 2 | % converts mercator coordinates using mercator scale to lat/lon coordinates 3 | 4 | er = 6378137; % average earth radius at the equator 5 | lon = mx * 180 / (scale * pi * er); 6 | lat = 360 / pi * atan(exp(my / (scale * er))) - 90; 7 | -------------------------------------------------------------------------------- /kitti360scripts/devkits/convertOxtsPose/matlab/postprocessPoses.m: -------------------------------------------------------------------------------- 1 | function poses = postprocessPoses (poses_in) 2 | 3 | R = [1 0 0 0; 0 -1 0 0; 0 0 -1 0; 0 0 0 1]; 4 | 5 | poses = []; 6 | 7 | for i = 1:length(poses_in) 8 | % if there is no data => no pose 9 | if isempty(poses_in{i}) 10 | pose{i} = []; 11 | continue; 12 | end 13 | 14 | P = poses_in{i}; 15 | poses{i} = (R * P')'; 16 | %H = R * [H_; 0 0 0 1]; 17 | %H = H(1:3,:)'; 18 | %poses(p,:) = H(:); 19 | end 20 | -------------------------------------------------------------------------------- /kitti360scripts/devkits/convertOxtsPose/matlab/testOxtsToPose.m: -------------------------------------------------------------------------------- 1 | % Test script for loading Oxts data and convert to Mercator coordinate 2 | 3 | % load Oxts data 4 | seq_id = 0; 5 | kitti360_path = getenv('KITTI360_DATASET'); 6 | oxts_dir = sprintf('%s/data_poses/2013_05_28_drive_%04d_sync/oxts', kitti360_path, seq_id); 7 | if ~exist(oxts_dir, 'dir') 8 | error(fprintf('%s does not exist! \nPlease specify KITTI360_DATASET in your system path.\nPlease check if you have downloaded system poses (data_poses.zip) and unzipped them under KITTI360_DATASET' , oxts_dir)); 9 | end 10 | [oxts,ts] = loadOxtsData(oxts_dir); 11 | fprintf('Loaded oxts data from %s\n', oxts_dir); 12 | 13 | % convert to Mercator coordinate 14 | poses = convertOxtsToPose(oxts); 15 | 16 | % convert coordinate system from 17 | % x=forward, y=right, z=down 18 | % to 19 | % x=forward, y=left, z=up 20 | poses = postprocessPoses(poses); 21 | 22 | % write to file 23 | output_dir = 'output'; 24 | if ~exist(output_dir, 'dir') 25 | mkdir(output_dir); 26 | end 27 | poses = cell2mat(poses); 28 | poses = permute(reshape(poses,4,4,[]), [2,1,3]); 29 | poses = reshape(poses,16,[])'; 30 | output_file = sprintf('%s/2013_05_28_drive_%04d_sync_oxts2pose.txt', output_dir, seq_id); 31 | dlmwrite(output_file, poses, 'delimiter',' ','precision',6); 32 | fprintf('Output written to %s\n', output_file); 33 | -------------------------------------------------------------------------------- /kitti360scripts/devkits/convertOxtsPose/matlab/testPoseToOxts.m: -------------------------------------------------------------------------------- 1 | % Test script for loading Oxts data and convert to Mercator coordinate 2 | 3 | % load poses 4 | seq_id = 0; 5 | kitti360_path = getenv('KITTI360_DATASET'); 6 | pose_file = sprintf('%s/data_poses/2013_05_28_drive_%04d_sync/poses.txt', kitti360_path, seq_id); 7 | if ~exist(pose_file, 'file') 8 | error(fprintf('%s does not exist! \nPlease specify KITTI360_DATASET in your system path.\nPlease check if you have downloaded system poses (data_poses.zip) and unzipped them under KITTI360_DATASET' , pose_file)); 9 | end 10 | [ts, poses] = loadPoses(pose_file); 11 | fprintf('Loaded pose file %s\n', pose_file); 12 | 13 | % convert coordinate system from 14 | % x=forward, y=left, z=up 15 | % to 16 | % x=forward, y=right, z=down 17 | poses = postprocessPoses(poses); 18 | 19 | % convert to lat/lon coordinate 20 | oxts = convertPoseToOxts(poses); 21 | 22 | % write to file 23 | output_dir = 'output'; 24 | if ~exist(output_dir, 'dir') 25 | mkdir(output_dir); 26 | end 27 | oxts = cell2mat(oxts); 28 | oxts = reshape(oxts,6,[])'; 29 | output_file = sprintf('%s/2013_05_28_drive_%04d_sync_pose2oxts.txt', output_dir, seq_id); 30 | dlmwrite(output_file, oxts, 'delimiter',' ','precision',6); 31 | fprintf('Output written to %s\n', output_file); 32 | -------------------------------------------------------------------------------- /kitti360scripts/devkits/convertOxtsPose/python/convertOxtsToPose.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from utils import latToScale, latlonToMercator 3 | 4 | def convertOxtsToPose(oxts): 5 | ''' converts a list of oxts measurements into metric poses, 6 | starting at (0,0,0) meters, OXTS coordinates are defined as 7 | x = forward, y = right, z = down (see OXTS RT3000 user manual) 8 | afterwards, pose{i} contains the transformation which takes a 9 | 3D point in the i'th frame and projects it into the oxts 10 | coordinates with the origin at a lake in Karlsruhe. ''' 11 | 12 | single_value = not isinstance(oxts, list) 13 | if single_value: 14 | oxts = [oxts] 15 | 16 | # origin in OXTS coordinate 17 | origin_oxts = [48.9843445, 8.4295857] # lake in Karlsruhe 18 | 19 | # compute scale from lat value of the origin 20 | scale = latToScale(origin_oxts[0]) 21 | 22 | # origin in Mercator coordinate 23 | ox,oy = latlonToMercator(origin_oxts[0],origin_oxts[1],scale) 24 | origin = np.array([ox, oy, 0]) 25 | 26 | pose = [] 27 | 28 | # for all oxts packets do 29 | for i in range(len(oxts)): 30 | 31 | # if there is no data => no pose 32 | if not len(oxts[i]): 33 | pose.append([]) 34 | continue 35 | 36 | # translation vector 37 | tx, ty = latlonToMercator(oxts[i][0],oxts[i][1],scale) 38 | t = np.array([tx, ty, oxts[i][2]]) 39 | 40 | # rotation matrix (OXTS RT3000 user manual, page 71/92) 41 | rx = oxts[i][3] # roll 42 | ry = oxts[i][4] # pitch 43 | rz = oxts[i][5] # heading 44 | Rx = np.array([[1,0,0],[0,np.cos(rx),-np.sin(rx)],[0,np.sin(rx),np.cos(rx)]]) # base => nav (level oxts => rotated oxts) 45 | Ry = np.array([[np.cos(ry),0,np.sin(ry)],[0,1,0],[-np.sin(ry),0,np.cos(ry)]]) # base => nav (level oxts => rotated oxts) 46 | Rz = np.array([[np.cos(rz),-np.sin(rz),0],[np.sin(rz),np.cos(rz),0],[0,0,1]]) # base => nav (level oxts => rotated oxts) 47 | R = np.matmul(np.matmul(Rz, Ry), Rx) 48 | 49 | # normalize translation 50 | t = t-origin 51 | 52 | # add pose 53 | pose.append(np.vstack((np.hstack((R,t.reshape(3,1))),np.array([0,0,0,1])))) 54 | 55 | if single_value: 56 | pose = pose[0] 57 | 58 | return pose 59 | -------------------------------------------------------------------------------- /kitti360scripts/devkits/convertOxtsPose/python/convertPoseToOxts.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from utils import latToScale, latlonToMercator, mercatorToLatlon 3 | 4 | def convertPoseToOxts(pose): 5 | '''converts a list of metric poses into oxts measurements, 6 | starting at (0,0,0) meters, OXTS coordinates are defined as 7 | x = forward, y = right, z = down (see OXTS RT3000 user manual) 8 | afterwards, pose{i} contains the transformation which takes a 9 | 3D point in the i'th frame and projects it into the oxts 10 | coordinates with the origin at a lake in Karlsruhe. ''' 11 | 12 | single_value = not isinstance(pose, list) 13 | if single_value: 14 | pose = [pose] 15 | 16 | # origin in OXTS coordinate 17 | origin_oxts = [48.9843445, 8.4295857] # lake in Karlsruhe 18 | 19 | # compute scale from lat value of the origin 20 | scale = latToScale(origin_oxts[0]) 21 | 22 | # origin in Mercator coordinate 23 | ox, oy = latlonToMercator(origin_oxts[0],origin_oxts[1],scale) 24 | origin = np.array([ox,oy,0]) 25 | 26 | oxts = [] 27 | 28 | # for all oxts packets do 29 | for i in range(len(pose)): 30 | 31 | # if there is no data => no pose 32 | if not len(pose[i]): 33 | oxts.append([]) 34 | continue 35 | 36 | # rotation and translation 37 | R = pose[i][0:3, 0:3] 38 | t = pose[i][0:3, 3] 39 | 40 | # unnormalize translation 41 | t = t+origin 42 | 43 | # translation vector 44 | lat, lon = mercatorToLatlon(t[0], t[1], scale) 45 | alt = t[2] 46 | 47 | # rotation matrix (OXTS RT3000 user manual, page 71/92) 48 | yaw = np.arctan2(R[1,0] , R[0,0]) 49 | pitch = np.arctan2( - R[2,0] , np.sqrt(R[2,1]**2 + R[2,2]**2)) 50 | roll = np.arctan2(R[2,1] , R[2,2]) 51 | 52 | # rx = oxts{i}(4) # roll 53 | # ry = oxts{i}(5) # pitch 54 | # rz = oxts{i}(6) # heading 55 | # Rx = [1 0 0 0 cos(rx) -sin(rx); 0 sin(rx) cos(rx)]; # base => nav (level oxts => rotated oxts) 56 | # Ry = [cos(ry) 0 sin(ry) 0 1 0; -sin(ry) 0 cos(ry)]; # base => nav (level oxts => rotated oxts) 57 | # Rz = [cos(rz) -sin(rz) 0 sin(rz) cos(rz) 0; 0 0 1]; # base => nav (level oxts => rotated oxts) 58 | # R = Rz*Ry*Rx 59 | 60 | # add oxts 61 | oxts.append([lat, lon, alt, roll, pitch, yaw]) 62 | 63 | if single_value: 64 | oxts = oxts[0] 65 | 66 | return oxts 67 | -------------------------------------------------------------------------------- /kitti360scripts/devkits/convertOxtsPose/python/data.py: -------------------------------------------------------------------------------- 1 | import os 2 | import numpy as np 3 | 4 | def loadOxtsData(oxts_dir, frames=None): 5 | ''' reads GPS/IMU data from files to memory. requires base directory 6 | (=sequence directory as parameter). if frames is not specified, loads all frames. ''' 7 | 8 | ts = [] 9 | 10 | if frames==None: 11 | 12 | ts = loadTimestamps(oxts_dir) 13 | oxts = [] 14 | for i in range(len(ts)): 15 | if len(ts[i]): 16 | try: 17 | oxts.append(np.loadtxt(os.path.join(oxts_dir, 'data', '%010d.txt'%i))) 18 | except: 19 | oxts.append([]) 20 | else: 21 | oxts.append([]) 22 | 23 | else: 24 | 25 | if len(frames)>1: 26 | k = 1 27 | oxts = [] 28 | for i in range(len(frames)): 29 | try: 30 | oxts.append(np.loadtxt(os.path.join(oxts_dir, 'data', '%010d.txt'%k))) 31 | except: 32 | oxts.append([]) 33 | k=k+1 34 | 35 | # no list for single value 36 | else: 37 | file_name = os.path.join(oxts_dir, 'data', '%010d.txt'%k) 38 | try: 39 | oxts = np.loadtxt(file_name) 40 | except: 41 | oxts = [] 42 | 43 | return oxts,ts 44 | 45 | def loadTimestamps(ts_dir): 46 | ''' load timestamps ''' 47 | 48 | with open(os.path.join(ts_dir, 'timestamps.txt')) as f: 49 | data=f.read().splitlines() 50 | ts = [l.split(' ')[0] for l in data] 51 | 52 | return ts 53 | 54 | def loadPoses (pos_file): 55 | ''' load system poses ''' 56 | 57 | data = np.loadtxt(pos_file) 58 | ts = data[:, 0].astype(np.int) 59 | poses = np.reshape(data[:, 1:], (-1, 3, 4)) 60 | poses = np.concatenate((poses, np.tile(np.array([0, 0, 0, 1]).reshape(1,1,4),(poses.shape[0],1,1))), 1) 61 | return ts, poses 62 | 63 | -------------------------------------------------------------------------------- /kitti360scripts/devkits/convertOxtsPose/python/testOxtsToPose.py: -------------------------------------------------------------------------------- 1 | # Test script for loading Oxts data and convert to Mercator coordinate 2 | import os 3 | from data import loadOxtsData 4 | from utils import postprocessPoses 5 | from convertOxtsToPose import convertOxtsToPose 6 | 7 | if __name__=="__main__": 8 | 9 | # root dir of KITTI-360 10 | if 'KITTI360_DATASET' in os.environ: 11 | kitti360_dir = os.environ['KITTI360_DATASET'] 12 | else: 13 | kitti360_dir = os.path.join(os.path.dirname( 14 | os.path.realpath(__file__)), '..', '..') 15 | 16 | # load Oxts data 17 | seq_id = 0 18 | oxts_dir = os.path.join(kitti360_dir, 'data_poses', '2013_05_28_drive_%04d_sync'%seq_id, 'oxts') 19 | if not os.path.isdir(oxts_dir): 20 | raise ValueError('%s does not exist! \nPlease specify KITTI360_DATASET in your system path.\nPlease check if you have downloaded OXTS poses (data_poses_oxts.zip) and unzipped them under KITTI360_DATASET' % oxts_dir) 21 | oxts,ts = loadOxtsData(oxts_dir) 22 | print('Loaded oxts data from %s' % oxts_dir) 23 | 24 | # convert to Mercator coordinate 25 | poses = convertOxtsToPose(oxts) 26 | 27 | # convert coordinate system from 28 | # x=forward, y=right, z=down 29 | # to 30 | # x=forward, y=left, z=up 31 | poses = postprocessPoses(poses) 32 | 33 | # write to file 34 | output_dir = 'output' 35 | if not os.path.isdir(output_dir): 36 | os.makedirs(output_dir) 37 | output_file = '%s/2013_05_28_drive_%04d_sync_oxts2pose.txt'% (output_dir, seq_id) 38 | 39 | with open(output_file, 'w') as f: 40 | for pose_ in poses: 41 | pose_ = ' '.join(['%.6f'%x for x in pose_.flatten()]) 42 | f.write('%s\n'%pose_) 43 | print('Output written to %s' % output_file) 44 | -------------------------------------------------------------------------------- /kitti360scripts/devkits/convertOxtsPose/python/testPoseToOxts.py: -------------------------------------------------------------------------------- 1 | # Test script for loading Oxts data and convert to Mercator coordinate 2 | import os 3 | from data import loadPoses 4 | from utils import postprocessPoses 5 | from convertPoseToOxts import convertPoseToOxts 6 | 7 | if __name__=="__main__": 8 | 9 | # root dir of KITTI-360 10 | if 'KITTI360_DATASET' in os.environ: 11 | kitti360_dir = os.environ['KITTI360_DATASET'] 12 | else: 13 | kitti360_dir = os.path.join(os.path.dirname( 14 | os.path.realpath(__file__)), '..', '..') 15 | 16 | # load poses 17 | seq_id = 0 18 | 19 | pose_file = os.path.join(kitti360_dir, 'data_poses', '2013_05_28_drive_%04d_sync'%seq_id, 'poses.txt') 20 | if not os.path.isfile(pose_file): 21 | raise ValueError('%s does not exist! \nPlease specify KITTI360_DATASET in your system path.\nPlease check if you have downloaded system poses (data_poses.zip) and unzipped them under KITTI360_DATASET' % pose_file) 22 | [ts, poses] = loadPoses(pose_file) 23 | print('Loaded pose file %s' % pose_file) 24 | 25 | # convert coordinate system from 26 | # x=forward, y=left, z=up 27 | # to 28 | # x=forward, y=right, z=down 29 | poses = postprocessPoses(poses) 30 | 31 | # convert to lat/lon coordinate 32 | oxts = convertPoseToOxts(poses) 33 | 34 | # write to file 35 | output_dir = 'output' 36 | if not os.path.isdir(output_dir): 37 | os.makedirs(output_dir) 38 | output_file = '%s/2013_05_28_drive_%04d_sync_pose2oxts.txt'% (output_dir, seq_id) 39 | with open(output_file, 'w') as f: 40 | for oxts_ in oxts: 41 | oxts_ = ' '.join(['%.6f'%x for x in oxts_]) 42 | f.write('%s\n'%oxts_) 43 | print('Output written to %s' % output_file) 44 | -------------------------------------------------------------------------------- /kitti360scripts/devkits/convertOxtsPose/python/utils.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | er = 6378137. # average earth radius at the equator 4 | 5 | def latlonToMercator(lat,lon,scale): 6 | ''' converts lat/lon coordinates to mercator coordinates using mercator scale ''' 7 | 8 | mx = scale * lon * np.pi * er / 180; 9 | my = scale * er * np.log( np.tan((90+lat) * np.pi / 360) ); 10 | return mx,my 11 | 12 | def mercatorToLatlon(mx,my,scale): 13 | ''' converts mercator coordinates using mercator scale to lat/lon coordinates ''' 14 | 15 | lon = mx * 180. / (scale * np.pi * er) 16 | lat = 360. / np.pi * np.arctan(np.exp(my / (scale * er))) - 90. 17 | return lat, lon 18 | 19 | def latToScale(lat): 20 | ''' compute mercator scale from latitude ''' 21 | scale = np.cos(lat * np.pi / 180.0); 22 | return scale 23 | 24 | def postprocessPoses (poses_in): 25 | 26 | R = np.array([[1,0,0,0], [0,-1,0,0], [0,0,-1,0], [0,0,0,1]]) 27 | 28 | poses = [] 29 | 30 | for i in range(len(poses_in)): 31 | # if there is no data => no pose 32 | if not len(poses_in[i]): 33 | pose.append([]) 34 | continue 35 | 36 | P = poses_in[i] 37 | poses.append( np.matmul(R, P.T).T ) 38 | 39 | return poses 40 | -------------------------------------------------------------------------------- /kitti360scripts/devkits/scripts/run_accumulation.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # This script accumulates velodyne/sick point clouds 4 | # Example usage: 5 | # ./run_accumulation.sh $KITTI360_DATASET $OUTPUT_DIR 2013_05_28_drive_0003_sync 2 282 6 | 7 | 8 | set -e 9 | 10 | # arguments 11 | root_dir=$1 12 | output_dir=$2 13 | sequence=$3 14 | first_frame=$4 15 | last_frame=$5 16 | data_source=$6 # 0: sick only, 1: velodyne only, 2: velodyne + sick 17 | 18 | #parameters 19 | #----------------------# 20 | travel_padding=20 21 | verbose=1 22 | space=0.005 23 | #----------------------# 24 | 25 | echo =================================================== 26 | echo "Processing $sequence: [$first_frame,$last_frame]" 27 | echo =================================================== 28 | 29 | # point cloud accumulation (generate a folder called data_raw) 30 | echo ./../accumuLaser/build/point_accumulator ${root_dir} ${sequence} ${output_dir} ${first_frame} ${last_frame} ${travel_padding} ${data_source} ${space} ${verbose} 31 | ./../accumuLaser/build/point_accumulator ${root_dir} ${sequence} ${output_dir} ${first_frame} ${last_frame} ${travel_padding} ${data_source} ${space} ${verbose} 32 | 33 | # done 34 | 35 | -------------------------------------------------------------------------------- /kitti360scripts/devkits/singularity/accumuLaser.recipe: -------------------------------------------------------------------------------- 1 | #header 2 | Bootstrap: docker 3 | From: ubuntu:16.04 4 | 5 | #Sections 6 | 7 | %help 8 | Singularity image of the ubuntu16.04-build docker container. 9 | OS: Ubuntu 16.04 10 | OpenCV 2.4 11 | Eigen 3.3.1 12 | 13 | 14 | %setup 15 | # commands executed on the host system after os has been installed 16 | 17 | 18 | 19 | %files 20 | # install scripts for opencv and pcl 21 | install_opencv.sh 22 | 23 | 24 | %labels 25 | # information labels about the container saved in /.singularity.d/labels.json 26 | maintainer Maximus Mutschler 27 | Version v0.1 28 | # overwrite True= to be able to build it multiple times 29 | overwrite False 30 | opencv.version 2.4 31 | 32 | 33 | %environment 34 | # set environment variables 35 | 36 | 37 | 38 | %post 39 | # commands executed inside the container after os has been installed. Used for setup of the container 40 | apt-get update 41 | # gcc 6.0 42 | apt-get install -y software-properties-common 43 | add-apt-repository ppa:ubuntu-toolchain-r/test 44 | apt-get update 45 | apt-get -y install gcc g++ gcc-6 g++-6 46 | update-alternatives --install /usr/bin/gcc gcc /usr/bin/gcc-6 80 --slave /usr/bin/g++ g++ /usr/bin/g++-6 47 | update-alternatives --set gcc /usr/bin/gcc-6 48 | ### dependencies 49 | # opencv 50 | ./install_opencv.sh 51 | # eigen 52 | apt-get -y install wget cmake 53 | wget https://gitlab.com/libeigen/eigen/-/archive/3.3.1/eigen-3.3.1.tar.gz 54 | tar -xzvf eigen-3.3.1.tar.gz 55 | cd eigen-3.3.1/ 56 | mkdir build && cd build 57 | cmake .. 58 | make 59 | make install 60 | 61 | %runscript 62 | # commands that are run when container's run command is used 63 | 64 | 65 | %test 66 | # validate setup at the end of the build process 67 | 68 | echo '------------------------------------------------------------------' 69 | echo 'Versions of installed tools:' 70 | echo 'opencv:' 71 | python -c 'import cv2 72 | print(cv2.__version__)' 73 | 74 | echo '------------------------------------------------------------------' 75 | 76 | -------------------------------------------------------------------------------- /kitti360scripts/devkits/singularity/install_opencv.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | set -e 3 | 4 | opencv="opencv" 5 | 6 | apt-get -y install build-essential 7 | apt-get -y install cmake git libgtk2.0-dev pkg-config libavcodec-dev libavformat-dev libswscale-dev 8 | apt-get -y install python-dev python-numpy libtbb2 libtbb-dev libjpeg-dev libpng-dev libtiff-dev libjasper-dev libdc1394-22-dev 9 | 10 | 11 | if [ ! -d $opencv ] 12 | then 13 | # For compatibility purpose, use version 2.4 instead. 14 | # However, for version 3.0, it should also be ok. 15 | git clone -b 2.4 https://github.com/opencv/opencv.git 16 | fi 17 | 18 | cd $opencv 19 | mkdir -p release 20 | cd release 21 | cmake -D CMAKE_BUILD_TYPE=RELEASE -D CMAKE_INSTALL_PREFIX=/usr/local .. 22 | 23 | make -j2 24 | make install 25 | -------------------------------------------------------------------------------- /kitti360scripts/evaluation/addToConfusionMatrix.pyx: -------------------------------------------------------------------------------- 1 | # cython methods to speed-up evaluation 2 | 3 | import numpy as np 4 | cimport cython 5 | cimport numpy as np 6 | import ctypes 7 | 8 | np.import_array() 9 | 10 | cdef extern from "addToConfusionMatrix_impl.c": 11 | void addToConfusionMatrix( const unsigned char* f_prediction_p , 12 | const unsigned char* f_groundTruth_p , 13 | const double* f_groundTruth_conf , 14 | const unsigned int f_width_i , 15 | const unsigned int f_height_i , 16 | double* f_confMatrix_p , 17 | const unsigned int f_confMatDim_i ) 18 | 19 | 20 | cdef tonumpyarray(double* data, unsigned long long size): 21 | if not (data and size >= 0): raise ValueError 22 | return np.PyArray_SimpleNewFromData(2, [size, size], np.NPY_FLOAT64, data) 23 | 24 | @cython.boundscheck(False) 25 | def cEvaluatePair( np.ndarray[np.uint8_t , ndim=2] predictionArr , 26 | np.ndarray[np.uint8_t , ndim=2] groundTruthArr , 27 | np.ndarray[np.float64_t , ndim=2] groundTruthConf , 28 | np.ndarray[np.float64_t , ndim=2] confMatrix , 29 | evalLabels ): 30 | cdef np.ndarray[np.uint8_t , ndim=2, mode="c"] predictionArr_c 31 | cdef np.ndarray[np.uint8_t , ndim=2, mode="c"] groundTruthArr_c 32 | cdef np.ndarray[np.float64_t , ndim=2, mode="c"] groundTruthConf_c 33 | cdef np.ndarray[np.float64_t , ndim=2, mode="c"] confMatrix_c 34 | 35 | predictionArr_c = np.ascontiguousarray(predictionArr , dtype=np.uint8 ) 36 | groundTruthArr_c = np.ascontiguousarray(groundTruthArr , dtype=np.uint8 ) 37 | groundTruthConf_c = np.ascontiguousarray(groundTruthConf, dtype=np.float64 ) 38 | confMatrix_c = np.ascontiguousarray(confMatrix , dtype=np.float64 ) 39 | 40 | cdef np.uint32_t height_ui = predictionArr.shape[1] 41 | cdef np.uint32_t width_ui = predictionArr.shape[0] 42 | cdef np.uint32_t confMatDim_ui = confMatrix.shape[0] 43 | 44 | addToConfusionMatrix(&predictionArr_c[0,0], &groundTruthArr_c[0,0], &groundTruthConf_c[0,0], height_ui, width_ui, &confMatrix_c[0,0], confMatDim_ui) 45 | 46 | confMatrix = np.ascontiguousarray(tonumpyarray(&confMatrix_c[0,0], confMatDim_ui)) 47 | 48 | return np.copy(confMatrix) 49 | -------------------------------------------------------------------------------- /kitti360scripts/evaluation/addToConfusionMatrix_impl.c: -------------------------------------------------------------------------------- 1 | // cython methods to speed-up evaluation 2 | 3 | void addToConfusionMatrix( const unsigned char* f_prediction_p , 4 | const unsigned char* f_groundTruth_p , 5 | const double* f_groundTruth_conf , 6 | const unsigned int f_width_i , 7 | const unsigned int f_height_i , 8 | double* f_confMatrix_p , 9 | const unsigned int f_confMatDim_i ) 10 | { 11 | const unsigned int size_ui = f_height_i * f_width_i; 12 | for (unsigned int i = 0; i < size_ui; ++i) 13 | { 14 | const unsigned char predPx = f_prediction_p [i]; 15 | const unsigned char gtPx = f_groundTruth_p[i]; 16 | f_confMatrix_p[f_confMatDim_i*gtPx + predPx] += f_groundTruth_conf[i]; 17 | } 18 | } 19 | -------------------------------------------------------------------------------- /kitti360scripts/evaluation/novel_view_synthesis/README.md: -------------------------------------------------------------------------------- 1 | ########################################################################### 2 | 3 | KITTI-360: THE NOVEL VIEW SYNTHESIS BENCHMARK 4 | Yiyi Liao Jun Xie Andreas Geiger 5 | University of Tübingen 6 | Max Planck Institute for Intelligent Systems, Tübingen 7 | www.cvlibs.net/datasets/kitti-360 8 | 9 | ########################################################################### 10 | 11 | 12 | 13 | This file describes the KITTI-360 2D semantic segmentation benchmark that consists of 910 test images. 14 | 15 | 16 | ## Training Views ## 17 | 18 | The RGB images for training and the train/test split can be found in: 19 | ``` 20 | Download -> 2D data & labels -> Test NVS (0.3G) 21 | ``` 22 | The camera poses can be found in: 23 | ``` 24 | Download -> Vechicle Poses (8.9M) 25 | ``` 26 | For camera poses, the first number at each row corresponds to the frame index. 27 | Please refer to the "2D Data Format" of our online [documentation](http://www.cvlibs.net/datasets/kitti-360/documentation.php) for more details of the data format. 28 | 29 | 30 | ## Test Data ## 31 | 32 | We evaluate on 100 images of the left perspective camera sampled from two test 33 | sequences, 0008 and 0018. Camera poses of the test views can be found in: 34 | ``` 35 | Download -> Vechicle Poses (8.9M) 36 | ``` 37 | 38 | ## Output for Novel View Appearance Synthesis ## 39 | 40 | The output structure should be analogous to the input. 41 | All results must be provided in the root directory of a zip file using the format of 8 bit png. The file names should follow `{seq:0>4}_{frame:0>10}.png`. Here is how the semantic predictions should look like in root directory of your zip file. 42 | ``` 43 | 0008_0000000828.png 44 | 0008_0000000830.png 45 | ... 46 | 0018_0000002391.png 47 | 0018_0000002393.png 48 | ... 49 | ``` 50 | -------------------------------------------------------------------------------- /kitti360scripts/evaluation/novel_view_synthesis/data/2013_05_28_drive_train_drop50_frames.txt: -------------------------------------------------------------------------------- 1 | data_2d_nvs_drop50/train_00/2013_05_28_drive_0008_sync/image_00/data_rect/0000000805.png data_2d_semantics/test1/2013_05_28_drive_0008_sync/semantic/0000000805.png 2 | data_2d_nvs_drop50/train_00/2013_05_28_drive_0008_sync/image_00/data_rect/0000000807.png data_2d_semantics/test1/2013_05_28_drive_0008_sync/semantic/0000000807.png 3 | -------------------------------------------------------------------------------- /kitti360scripts/evaluation/novel_view_synthesis/metric.py: -------------------------------------------------------------------------------- 1 | from __future__ import print_function, absolute_import, division 2 | import numpy as np 3 | import skimage.measure 4 | import torch 5 | import time 6 | 7 | try: 8 | import lpips 9 | lpips_fn = lpips.LPIPS(net='alex') 10 | except: 11 | lpips_fn = None 12 | print('LPIPS will not be evaluated due to missing dependency!') 13 | 14 | class Metric(object): 15 | def __init__(self): 16 | self.reset() 17 | 18 | def reset(self): 19 | pass 20 | 21 | def add(self, es, ta, ma=None): 22 | pass 23 | 24 | def get(self): 25 | return {} 26 | 27 | def items(self): 28 | return self.get().items() 29 | 30 | def __str__(self): 31 | return ", ".join( 32 | ["%s=%.5f" % (key, value) for key, value in self.get().items()] 33 | ) 34 | 35 | 36 | class MultipleMetric(Metric): 37 | def __init__(self, metrics, prefix="", **kwargs): 38 | self.metrics = metrics 39 | super(MultipleMetric, self).__init__(**kwargs) 40 | self.prefix = prefix 41 | 42 | def reset(self): 43 | for m in self.metrics: 44 | m.reset() 45 | 46 | def add(self, es, ta, ma=None): 47 | for m in self.metrics: 48 | m.add(es, ta, ma) 49 | 50 | def get(self): 51 | ret = {} 52 | for m in self.metrics: 53 | vals = m.get() 54 | for k in vals: 55 | ret["%s%s" % (self.prefix, k)] = vals[k] 56 | return ret 57 | 58 | def __str__(self): 59 | lines = [] 60 | for m in self.metrics: 61 | line = ", ".join( 62 | [ 63 | "%s%s=%.5f" % (self.prefix, key, value) 64 | for key, value in m.get().items() 65 | ] 66 | ) 67 | lines.append(line) 68 | return "\n".join(lines) 69 | 70 | 71 | class BaseDistanceMetric(Metric): 72 | def __init__(self, name="", stats=None, **kwargs): 73 | super(BaseDistanceMetric, self).__init__(**kwargs) 74 | self.name = name 75 | if stats is None: 76 | self.stats = {"mean": np.mean} 77 | else: 78 | self.stats = stats 79 | 80 | def reset(self): 81 | self.dists = [] 82 | 83 | def add(self, es, ta, ma=None): 84 | pass 85 | 86 | def get(self): 87 | dists = np.hstack(self.dists) 88 | return {"dist%s_%s" % (self.name, k): f(dists) for k, f in self.stats.items()} 89 | 90 | 91 | class DistanceMetric(BaseDistanceMetric): 92 | def __init__(self, vec_length=1, p=2, **kwargs): 93 | super(DistanceMetric, self).__init__(name=str(p), **kwargs) 94 | self.vec_length = vec_length 95 | self.p = p 96 | 97 | def add(self, es, ta, ma=None): 98 | if es.shape != ta.shape or es.shape[-1] != self.vec_length: 99 | print(es.shape, ta.shape) 100 | raise Exception( 101 | "es and ta have to be of shape N x vec_length(={self.vec_length})" 102 | ) 103 | es = es.reshape(-1, self.vec_length) 104 | ta = ta.reshape(-1, self.vec_length) 105 | if ma is not None: 106 | ma = ma.ravel() 107 | es = es[ma != 0] 108 | ta = ta[ma != 0] 109 | dist = np.linalg.norm(es - ta, ord=self.p, axis=1) 110 | self.dists.append(dist) 111 | 112 | class LPIPSMetric(BaseDistanceMetric): 113 | def __init__(self, **kwargs): 114 | super(LPIPSMetric, self).__init__(name='lpips', **kwargs) 115 | self.loss_fn = lpips_fn 116 | 117 | 118 | def add(self, es, ta, ma=None): 119 | if self.loss_fn is not None: 120 | if es.shape != ta.shape: 121 | raise Exception("es and ta have to be of shape Nxdim") 122 | if es.ndim == 3: 123 | es = es[..., None] 124 | ta = ta[..., None] 125 | if es.ndim != 4 or es.shape[3] not in [1, 3]: 126 | raise Exception( 127 | "es and ta have to be of shape bs x height x width x 0, 1, or 3" 128 | ) 129 | if ma is not None: 130 | es = ma * es 131 | ta = ma * ta 132 | es = torch.tensor(es).permute(0,3,1,2).to(torch.float32) 133 | ta = torch.tensor(ta).permute(0,3,1,2).to(torch.float32) 134 | # normalize to [-1,1] 135 | es = es * 2.0 - 1.0 136 | ta = ta * 2.0 - 1.0 137 | time_start = time.time() 138 | lpips = self.loss_fn(es, ta).item() 139 | print(time.time()-time_start) 140 | self.dists.append(lpips) 141 | else: 142 | self.dists.append(np.nan) 143 | 144 | class PSNRMetric(BaseDistanceMetric): 145 | def __init__(self, max=1, **kwargs): 146 | super(PSNRMetric, self).__init__(name="psnr", **kwargs) 147 | # distance between minimum and maximum possible value 148 | self.max = max 149 | 150 | def add(self, es, ta, ma=None): 151 | if es.shape != ta.shape: 152 | raise Exception("es and ta have to be of shape Nxdim") 153 | if es.ndim == 3: 154 | es = es[..., None] 155 | ta = ta[..., None] 156 | if es.ndim != 4 or es.shape[3] not in [1, 3]: 157 | raise Exception( 158 | "es and ta have to be of shape bs x height x width x 0, 1, or 3" 159 | ) 160 | if ma is not None: 161 | es = ma * es 162 | ta = ma * ta 163 | mse = np.mean((es - ta) ** 2, axis=(1, 2, 3)) 164 | psnr = 20 * np.log10(self.max) - 10 * np.log10(mse) 165 | self.dists.append(psnr) 166 | 167 | 168 | class SSIMMetric(BaseDistanceMetric): 169 | def __init__(self, data_range=None, mode="default", **kwargs): 170 | super(SSIMMetric, self).__init__(name="ssim", **kwargs) 171 | # distance between minimum and maximum possible value 172 | self.data_range = data_range 173 | self.mode = mode 174 | 175 | def add(self, es, ta, ma=None): 176 | if es.shape != ta.shape: 177 | raise Exception("es and ta have to be of shape Nxdim") 178 | if es.ndim == 3: 179 | es = es[..., None] 180 | ta = ta[..., None] 181 | if es.ndim != 4 or es.shape[3] not in [1, 3]: 182 | raise Exception( 183 | "es and ta have to be of shape bs x height x width x 0, 1, or 3" 184 | ) 185 | if ma is not None: 186 | es = ma * es 187 | ta = ma * ta 188 | 189 | for bidx in range(es.shape[0]): 190 | if self.mode == "default": 191 | ssim = skimage.measure.compare_ssim( 192 | es[bidx], ta[bidx], multichannel=True, data_range=self.data_range, 193 | gaussian_weights=True, sigma=1.5, k1=0.01, k2=0.03, 194 | use_sample_covariance=False, 195 | ) 196 | elif self.mode == "dv": 197 | ssim = 0 198 | for c in range(3): 199 | ssim += skimage.measure.compare_ssim( 200 | es[bidx, ..., c], 201 | ta[bidx, ..., c], 202 | gaussian_weights=True, 203 | sigma=1.5, 204 | use_sample_covariance=False, 205 | data_range=1.0, 206 | ) 207 | ssim /= 3 208 | else: 209 | raise Exception("invalid mode") 210 | self.dists.append(ssim) 211 | -------------------------------------------------------------------------------- /kitti360scripts/evaluation/semantic_2d/README.md: -------------------------------------------------------------------------------- 1 | ########################################################################### 2 | 3 | KITTI-360: THE 2D SEMANTIC INSTANCE SEGMENATION BENCHMARK 4 | Yiyi Liao Jun Xie Andreas Geiger 5 | University of Tübingen 6 | Max Planck Institute for Intelligent Systems, Tübingen 7 | www.cvlibs.net/datasets/kitti-360 8 | 9 | ########################################################################### 10 | 11 | 12 | 13 | This file describes the KITTI-360 2D semantic/instance segmentation benchmark that consists of 910 test images. 14 | 15 | 16 | ## Train/Val Data ## 17 | 18 | The RGB images for training and validation can be found in 19 | ``` 20 | Download -> 2D data & labels -> Perspective Images for Train & Val (128G) 21 | ``` 22 | The corresponding labels for training and validation as well as the training/validation split can be found in 23 | ``` 24 | Download -> 2D data & labels -> Semantics (1.8G) 25 | ``` 26 | Please refer to the "2D Data Format" of our online [documentation](http://www.cvlibs.net/datasets/kitti-360/documentation.php) for more details of the data format. 27 | 28 | 29 | ## Test Data ## 30 | 31 | We evaluate on 910 images of the left perspective camera sampled from two test 32 | sequences, 0008 and 0018. The RGB images for testing can be found in: ``` 33 | Download -> 2D data & labels -> Test Semantic (1.5G) ``` 34 | 35 | ## Output for 2D Semantic Segmentation ## 36 | 37 | The output structure should be analogous to the input. 38 | All results must be provided in the root directory of a zip file using the format of 8 bit grayscale png. The file names should follow `{seq:0>4}_{frame:0>10}.png`. Here is how the semantic predictions should look like in root directory of your zip file. 39 | ``` 40 | 0008_0000000002.png 41 | 0008_0000000012.png 42 | ... 43 | 0018_0000000079.png 44 | 0018_0000000094.png 45 | ... 46 | ``` 47 | The semantic labels should follow the definition of [labels.py](https://github.com/autonomousvision/kitti360Scripts/blob/master/kitti360scripts/helpers/labels.py). Note that `id` should be used instead of `kittiId` or `trainId`. 48 | 49 | ## Output for 2D Instance Segmentation ## 50 | 51 | For 2D instance segmentation, all results must be provided in the root directory of a zip file in the format of txt or png files. Here is how the instance predictions may look like in root directory of your zip file. 52 | ``` 53 | 0008_0000000002.txt 54 | 0008_0000000002_0000.png 55 | 0008_0000000002_0001.png 56 | ... 57 | 0018_0000000079.txt 58 | 0018_0000000079_0000.png 59 | 0018_0000000079_0001.png 60 | ``` 61 | The txt files of the instance segmentation should look as follows: 62 | ``` 63 | relPathPrediction1 labelIDPrediction1 confidencePrediction1 64 | relPathPrediction2 labelIDPrediction2 confidencePrediction2 65 | relPathPrediction3 labelIDPrediction3 confidencePrediction3 66 | ... 67 | ``` 68 | 69 | For example, the 0008_0000000002.txt may contain: 70 | ``` 71 | 0008_0000000002_0000.png 026 0.976347 72 | 0008_0000000002_0001.png 026 0.973782 73 | ... 74 | ``` 75 | -------------------------------------------------------------------------------- /kitti360scripts/evaluation/semantic_2d/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/autonomousvision/kitti360Scripts/40ee015de8277b2d907d32a30cdbd97072c7d7c3/kitti360scripts/evaluation/semantic_2d/__init__.py -------------------------------------------------------------------------------- /kitti360scripts/evaluation/semantic_2d/instance.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | # 3 | # Instance class 4 | # 5 | 6 | class Instance(object): 7 | instID = 0 8 | labelID = 0 9 | pixelCount = 0 10 | medDist = -1 11 | distConf = 0.0 12 | 13 | def __init__(self, imgNp, imgConf, instID): 14 | if (instID == -1): 15 | return 16 | self.instID = int(instID) 17 | self.labelID = int(self.getLabelID(instID)) 18 | self.pixelCount = self.getWeightedInstancePixels(imgNp, imgConf, instID) 19 | 20 | def getLabelID(self, instID): 21 | if (instID < 1000): 22 | return instID 23 | else: 24 | return int(instID / 1000) 25 | 26 | def getInstancePixels(self, imgNp, instLabel): 27 | return (imgNp == instLabel).sum() 28 | 29 | def getWeightedInstancePixels(self, imgNp, imgConf, instLabel): 30 | return imgConf[imgNp == instLabel].sum() 31 | 32 | def toJSON(self): 33 | return json.dumps(self, default=lambda o: o.__dict__, sort_keys=True, indent=4) 34 | 35 | def toDict(self): 36 | buildDict = {} 37 | buildDict["instID"] = self.instID 38 | buildDict["labelID"] = self.labelID 39 | buildDict["pixelCount"] = self.pixelCount 40 | buildDict["medDist"] = self.medDist 41 | buildDict["distConf"] = self.distConf 42 | return buildDict 43 | 44 | def fromJSON(self, data): 45 | self.instID = int(data["instID"]) 46 | self.labelID = int(data["labelID"]) 47 | self.pixelCount = int(data["pixelCount"]) 48 | if ("medDist" in data): 49 | self.medDist = float(data["medDist"]) 50 | self.distConf = float(data["distConf"]) 51 | 52 | def __str__(self): 53 | return "("+str(self.instID)+")" 54 | -------------------------------------------------------------------------------- /kitti360scripts/evaluation/semantic_2d/instances2dict.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | # 3 | # Convert instances from png files to a dictionary 4 | # 5 | 6 | from __future__ import print_function, absolute_import, division 7 | import os, sys 8 | 9 | # Cityscapes imports 10 | from kitti360scripts.evaluation.semantic_2d.instance import * 11 | from kitti360scripts.helpers.csHelpers import * 12 | 13 | # Confidence is saved in the format of uint16 14 | # Normalize confidence to [0,1] by dividing MAX_CONFIDENCE 15 | MAX_CONFIDENCE=65535.0 16 | 17 | def instances2dict(imageFileList, verbose=False): 18 | imgCount = 0 19 | instanceDict = {} 20 | 21 | if not isinstance(imageFileList, list): 22 | imageFileList = [imageFileList] 23 | 24 | if verbose: 25 | print("Processing {} images...".format(len(imageFileList))) 26 | 27 | for imageFileName,imageConfFileName in imageFileList: 28 | # Load image 29 | img = Image.open(imageFileName) 30 | 31 | # Image as numpy array 32 | imgNp = np.array(img) 33 | 34 | # Load confidence 35 | imgConf = Image.open(imageConfFileName) 36 | 37 | # Confidence as nnumpy array and normalize to [0,1] 38 | imgConf = np.array(imgConf) / MAX_CONFIDENCE 39 | 40 | # Initialize label categories 41 | instances = {} 42 | for label in labels: 43 | instances[label.name] = [] 44 | 45 | # Loop through all instance ids in instance image 46 | for instanceId in np.unique(imgNp): 47 | instanceObj = Instance(imgNp, imgConf, instanceId) 48 | 49 | instances[id2label[instanceObj.labelID].name].append(instanceObj.toDict()) 50 | 51 | # Merge garage instances to building instances 52 | # TODO: better solution? 53 | for instanceObj in instances['garage']: 54 | instanceObj['labelID'] = name2label['building'].id 55 | instances['building'].append(instanceObj) 56 | 57 | imgKey = os.path.abspath(imageFileName) 58 | instanceDict[imgKey] = instances 59 | imgCount += 1 60 | 61 | if verbose: 62 | print("\rImages Processed: {}".format(imgCount), end=' ') 63 | sys.stdout.flush() 64 | 65 | if verbose: 66 | print("") 67 | 68 | return instanceDict 69 | 70 | def main(argv): 71 | fileList = [] 72 | if (len(argv) > 2): 73 | for arg in argv: 74 | if ("png" in arg): 75 | fileList.append(arg) 76 | instances2dict(fileList, True) 77 | 78 | if __name__ == "__main__": 79 | main(sys.argv[1:]) 80 | -------------------------------------------------------------------------------- /kitti360scripts/evaluation/semantic_3d/README.md: -------------------------------------------------------------------------------- 1 | ########################################################################### 2 | 3 | KITTI-360: THE 3D SCENE UNDERSTANDING BENCHMARK 4 | Yiyi Liao Jun Xie Andreas Geiger 5 | University of Tübingen 6 | Max Planck Institute for Intelligent Systems, Tübingen 7 | www.cvlibs.net/datasets/kitti-360 8 | 9 | ########################################################################### 10 | 11 | 12 | # Segmentation and Bounding Box Detection 13 | 14 | This section describes the KITTI-360 3D scene understanding benchmark that consists of 42 test windows for evaluating 3D semantic segmentation, 3D instance segmentation as well as 3D bounding box detection. These tasks are evaluated on the `test1` split of KITTI-360. 15 | 16 | ## Train/Val Data ## 17 | 18 | The accumulated point clouds for training and validation as well as the training/validation split can be found in 19 | ``` 20 | Download -> 3D data & labels -> Accumulated Point Cloud for Train & Val (128G) 21 | ``` 22 | Note that the accumulated point clouds are released as a set of batches which we refer to as `windows`. A single window contains observations within a driving distance of about 200 meters (240 frames on average) and there is an overlap of 10 meters between two consecutive windows. We provide the following information for each 3D point of the accumulated point cloud: 23 | ``` 24 | - float x 25 | - float y 26 | - float z 27 | - uchar red 28 | - uchar green 29 | - uchar blue 30 | - int semantic 31 | - int instance 32 | - uchar visible 33 | - float confidence 34 | ``` 35 | where `x y z` is the location of a 3D point in the world coordinate, `red green blue` is the color of a 3D point obtained by projecting it to adjacent 2D images, `semantic instance` describes the label of a 3D point. `visible` is 0 if not visible in any of the 2D frames and 1 otherwise. `confidence` is the confidence of the 3D label. Please refer to the "3D Data Format" of our online [documentation](http://www.cvlibs.net/datasets/kitti-360/documentation.php) for more details of the data format. 36 | 37 | The training and validation split can be found in TODO 38 | 39 | 40 | ## Test Data ## 41 | 42 | We evaluate on 42 windows from two test sequences, 0008 and 0018. The accumulated point cloud of the test windows can be found in: 43 | ``` 44 | Download -> 3D data & labels -> Test Semantic (1.2G) 45 | ``` 46 | We provide the following information for the test windows: 47 | ``` 48 | - float x 49 | - float y 50 | - float z 51 | - uchar red 52 | - uchar green 53 | - uchar blue 54 | - uchar visible 55 | ``` 56 | Compared to the training & validation windows, `semantic`, `instance` and `confidence` are held out on the evaluation server. 57 | 58 | 59 | ## Output for 3D Semantic Segmentation ## 60 | 61 | The output structure should be analogous to the input. 62 | All results must be provided in the root directory of a zip file using the format of npy. The file names should follow `{seq:0>4}_{start_frame:0>10}_{end_frame:0>10}.npy`. Here is how the semantic predictions should look like in root directory of your zip file. 63 | ``` 64 | 0008_0000000002_0000000245.npy 65 | 0008_0000000235_0000000608.npy 66 | ... 67 | 0018_0000000002_0000000341.npy 68 | 0018_0000000330_0000000543.npy 69 | ... 70 | ``` 71 | Each numpy file should contain a vector of the semantic labels corresponding to the accumulated point cloud. Given `N` points in the accumulated point cloud, the submitted .npy file should contain __only__ a vector of length `N`, where the `i`th scalar indicates the semantic label of the `i`th point in the accumulated point cloud. Please save your predictions in the format of __uint8__, otherwise the zip file will be too large to be accepted by the evaluation server. 72 | 73 | The semantic labels should follow the definition of [labels.py](https://github.com/autonomousvision/kitti360Scripts/blob/master/kitti360scripts/helpers/labels.py). Note that `id` should be used instead of `kittiId` or `trainId`. 74 | 75 | ## Output for 3D Instance Segmentation ## 76 | 77 | The output structure should be analogous to the input. 78 | All results must be provided in the root directory of a zip file using the format of npy. The file names should follow `{seq:0>4}_{start_frame:0>10}_{end_frame:0>10}.txt` or `{seq:0>4}_{start_frame:0>10}_{end_frame:0>10}.npy`. Here is how the instance predictions should look like in root directory of your zip file. 79 | ``` 80 | 0008_0000000002_0000000245.txt 81 | 0008_0000000002_0000000245.npy 82 | 0008_0000000235_0000000608.txt 83 | 0008_0000000235_0000000608.npy 84 | ... 85 | 0018_0000000002_0000000341.txt 86 | 0018_0000000002_0000000341.npy 87 | 0018_0000000330_0000000543.txt 88 | 0018_0000000330_0000000543.npy 89 | ... 90 | ``` 91 | Each txt file should specify the specify the class of one instance and its corresponding confidence in each line as follows: 92 | ``` 93 | labelIDPrediction1 confidencePrediction1 94 | labelIDPrediction2 confidencePrediction2 95 | labelIDPrediction3 confidencePrediction3 96 | ... 97 | ``` 98 | For example, the 0008_0000000002_0000000245.txt may contain: 99 | ``` 100 | 26 0.976347 101 | 26 0.973782 102 | ... 103 | ``` 104 | To avoid submitting a very large file to the evaluation server, we only accept predictions that assign a single instance label to each 3D point. This means the instance masks should be saved in a single vector of length `N`, where `N` is the total amount of 3D points. Each element in this vector denotes an instance ID. Let `M` denote the number of lines in the txt file, then a valid instance ID should be in the range of `1` to `M`. An instance ID of `0` means that the 3D point does not belong to any instance listed in the txt file. 105 | 106 | The semantic labels should follow the definition of [labels.py](https://github.com/autonomousvision/kitti360Scripts/blob/master/kitti360scripts/helpers/labels.py). Note that `id` should be used instead of `kittiId` or `trainId`. Further note that we only evaluate two classes, __building__ and __car__ for 3D instance segmentation. 107 | 108 | 109 | ## Output for 3D Bounding Box Detection ## 110 | 111 | The output structure should be analogous to the input. 112 | All results must be provided in the root directory of a zip file using the format of npy. The file names should follow `{seq:0>4}_{start_frame:0>10}_{end_frame:0>10}.npy`. Here is how the 3D bounding box predictions should look like in root directory of your zip file. 113 | ``` 114 | 0008_0000000002_0000000245.npy 115 | 0008_0000000235_0000000608.npy 116 | ... 117 | 0018_0000000002_0000000341.npy 118 | 0018_0000000330_0000000543.npy 119 | ... 120 | ``` 121 | Each npy file should contain a set of bounding boxes with each line denoting a bounding box parameterized as follows: 122 | ``` 123 | center_x, center_y, center_z, size_x, size_y, size_z, heading_angle, id, confidence 124 | ``` 125 | - The `center_x, center_y, center_z` are in the __global / world / map coordinates__. 126 | - The `size_x, size_y, size_z` are in __object coordinates__ and refer to length, width and height of the object respectively. (The object coordinate is X right, Z up and Y inside). 127 | - The `heading_angle` (yaw) is positive ANTI-clockwise from positive X-axis about the Z-axis in __object coordinates__. 128 | - The `id` is the semantic label and should follow the definition of [labels.py](https://github.com/autonomousvision/kitti360Scripts/blob/master/kitti360scripts/helpers/labels.py). Note that `id` should be used instead of `kittiId` or `trainId`. 129 | 130 | Please check the `param2Bbox` function for more details. 131 | 132 | To run evaluation, first prepare the window data in `npy` (numpy) format. Type the following to get the windows: 133 | ```bash 134 | cd kitti360Scripts/kitti360scripts/evaluation/semantic_3d 135 | export KITTI360_DATASET=my_dataset_path 136 | python prepare_train_val_windows.py 137 | ``` 138 | This creates the `300` train and val windows inside the `kitti360Scripts/kitti360scripts/evaluation/semantic_3d` folder. 139 | 140 | We only evaluate two classes, __building__ and __car__ for 3D bounding box detection. Type the following to run the evaluation: 141 | ```bash 142 | cd kitti360Scripts/kitti360scripts/evaluation/semantic_3d 143 | export KITTI360_DATASET=my_dataset_path 144 | python evalDetection.py my_prediction_folder 145 | ``` 146 | 147 | # 3D Semantic Scene Completion 148 | 149 | We evaluate 3D semantic scene completion on the `test2` split of KITTI-360 contatining 38 test windows. To avoid submitting a large file to the evaluation server, we evaluate on the middle frame of each test window. 150 | 151 | ## Train/Val Data ## 152 | 153 | In contrast to the scene perception tasks considered above, the semantic scene completion task additionally requires predicting geometry and semantics in unobserved regions. This task takes as a single LiDAR scan as input and requires semantic scene completion within a corridor of 30m around the vehicle poses of a 100m trajectory. You are free to use all data of the KITTI-360 training split for training. Please refer to the `generateCroppedGroundTruth` function for preparing the training GT given an input LiDAR scan. 154 | 155 | ## Test Data ## 156 | 157 | The 38 frames we use for evaluation can be found at: 158 | ``` 159 | Download -> 3D data & labels -> Test Completion (35M) 160 | ``` 161 | Each file contains a raw input point cloud defined in the world coordinate where we accumulate the point clouds. 162 | 163 | 164 | ## Output for 3D Scene Completion ## 165 | 166 | All results must be provided in the root directory of a zip file using the format of npy. The file names should follow `{seq:0>4}_{frame:0>10}.npy`. Here is how the 3D scene completion results should look like in root directory of your zip file. 167 | ``` 168 | 0002_0000016048.npy 169 | ... 170 | 0004_0000000144.npy 171 | ... 172 | 0008_0000001602.npy 173 | ... 174 | ``` 175 | Each npy file should contain a point cloud defined in the world coordinate where the accumulated point cloud is defined. Each line of the npy file corresponds to a point location `x y z` in the world coordinate and its corresponding semantic label. 176 | The semantic labels should follow the definition of [labels.py](https://github.com/autonomousvision/kitti360Scripts/blob/master/kitti360scripts/helpers/labels.py). Note that `id` should be used instead of `kittiId` or `trainId`. 177 | -------------------------------------------------------------------------------- /kitti360scripts/evaluation/semantic_3d/prepare_train_val_windows.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Sample Run: 3 | python prepare_train_val_windows.py 4 | 5 | Converts KITTI-360 labels to windows in numpy format. 6 | Modified from SparseConvNet data preparation: https://github.com/facebookresearch/SparseConvNet/blob/master/examples/ScanNet/prepare_data.py 7 | ''' 8 | 9 | import glob, numpy as np, multiprocessing as mp, torch, json, argparse 10 | from kitti360scripts.helpers.labels import id2label 11 | from kitti360scripts.helpers.ply import read_ply 12 | from kitti360scripts.helpers.annotation import Annotation3D 13 | import sys 14 | import os 15 | BASE_DIR = os.path.dirname(os.path.abspath(__file__)) 16 | sys.path.append(BASE_DIR) 17 | sys.path.append(os.path.join(BASE_DIR, '../utils/')) 18 | 19 | if 'KITTI360_DATASET' in os.environ: 20 | kitti360Path = os.environ['KITTI360_DATASET'] 21 | else: 22 | kitti360Path = os.path.join(os.path.dirname( 23 | os.path.realpath(__file__)), '..', '..') 24 | 25 | parser = argparse.ArgumentParser() 26 | parser.add_argument('--data_split', help='data split (train / val / test)', default='train') 27 | opt = parser.parse_args() 28 | 29 | split = opt.data_split 30 | outputPath = os.path.join('.', split) 31 | os.makedirs(outputPath, exist_ok=True) 32 | split_path = 'test1' if split=='test' else split 33 | files = sorted(glob.glob(os.path.join(kitti360Path, 'data_3d_semantics', split_path, '*', 'static', '*.ply'))) 34 | 35 | print('================================================================================') 36 | print('data split: {}'.format(split)) 37 | print('output_folder: {}'.format(outputPath)) 38 | print('input_folder : {}'.format(os.path.join(kitti360Path, 'data_3d_semantics', split_path))) 39 | print('Found %d ply files' % len(files)) 40 | print('================================================================================') 41 | 42 | annotation3D_all = {} 43 | 44 | def f_test(fn): 45 | print(fn) 46 | 47 | f = plyfile.PlyData().read(fn) 48 | points = np.array([list(x) for x in f.elements[0]]) 49 | coords = np.ascontiguousarray(points[:, :3] - points[:, :3].mean(0)) 50 | colors = np.ascontiguousarray(points[:, 3:6]) / 127.5 - 1 51 | 52 | torch.save((coords, colors), fn[:-15] + '_inst_nostuff.pth') 53 | print('Saving to ' + fn[:-15] + '_inst_nostuff.pth') 54 | 55 | 56 | def f(fn): 57 | #print(fn) 58 | 59 | data = read_ply(fn) 60 | points = np.vstack((data['x'], data['y'], data['z'])).T 61 | points_center = points[:, :3].mean(0) 62 | coords = np.ascontiguousarray(points[:, :3] - points_center) 63 | colors = np.vstack((data['red'], data['green'], data['blue'])).T 64 | colors = np.ascontiguousarray(colors) / 127.5 - 1 65 | colors = colors.astype(np.float32) 66 | 67 | '''semantic''' 68 | ignore_label = -100 69 | sem_labels_raw = data['semantic'] 70 | sem_labels = np.ones_like(sem_labels_raw) * ignore_label 71 | for i in np.unique(sem_labels_raw): 72 | sem_labels[sem_labels_raw==i] = id2label[i].trainId 73 | sem_labels[sem_labels==255] = ignore_label 74 | 75 | '''instance''' 76 | instance_labels_raw = data['instance'] 77 | instance_labels = np.ones_like(instance_labels_raw) * ignore_label 78 | # unique instance id (regardless of semantic label) 79 | ins_cnt = 0 80 | ins_map = {} 81 | for i, ins_id in enumerate(np.unique(instance_labels_raw)): 82 | if ins_id%1000==0: 83 | instance_labels[instance_labels_raw==ins_id] = ignore_label 84 | else: 85 | instance_labels[instance_labels_raw==ins_id] = ins_cnt 86 | ins_map[ins_id] = ins_cnt 87 | ins_cnt+=1 88 | instance_labels[sem_labels==ignore_label] = ignore_label 89 | 90 | '''bounding box''' 91 | seq = [s for s in fn.split('/') if s.startswith('2013_05_28_drive_')] 92 | assert(len(seq)==1) 93 | sequence = seq[0] 94 | if not sequence in annotation3D_all.keys(): 95 | label3DBboxPath = os.path.join(kitti360Path, 'data_3d_bboxes') 96 | annotation3D = Annotation3D(label3DBboxPath, sequence) 97 | annotation3D_all[sequence] = annotation3D 98 | else: 99 | annotation3D = annotation3D_all[sequence] 100 | bboxes = [] 101 | for i, ins_id in enumerate(np.unique(instance_labels_raw)): 102 | if ins_id%1000==0: 103 | continue 104 | if id2label[ins_id//1000].trainId==255: # ignored class 105 | continue 106 | try: 107 | obj3D = annotation3D(ins_id//1000, ins_id%1000) 108 | scale = np.linalg.norm(obj3D.R, axis=0) 109 | center = obj3D.T# - points_center 110 | 111 | # The object frame of reference is X right, Z up and Y inside 112 | # The heading angle (\theta) is positive ANTI-clockwise from positive X-axis about the Z-axis. 113 | # The rotation matrix rot_z is from 114 | # https://github.com/autonomousvision/kitti360Scripts/blob/e0e3442991d3cf4c69debb84b48fcab3aabf2516/kitti360scripts/evaluation/semantic_3d/evalDetection.py#L186-L192 115 | # If \theta = 90, rotation matrix rot_z transforms the point (l/2,0) to (0,l/2). 116 | # +Y 117 | # | 118 | # | 119 | # | 120 | # | 121 | # | (l/2, 0) 122 | # |**********--------------+ X 123 | # 124 | # + Y 125 | # | 126 | # * (0,l/2) 127 | # * 128 | # * 129 | # * 130 | # *------------------------+ X 131 | # This example confirms that heading angle is positive anti-clockwise from positive X-axis. 132 | 133 | # Vertices are in the KITTI-360 annotation format: 134 | # x_corners = [l/2, l/2, l/2, l/2,-l/2,-l/2,-l/2,-l/2] 135 | # y_corners = [w/2, w/2,-w/2,-w/2, w/2, w/2,-w/2,-w/2] 136 | # z_corners = [h/2,-h/2, h/2,-h/2,-h/2, h/2,-h/2, h/2] 137 | # 138 | # +Y 139 | # | 140 | # | 141 | # 5 (-l/2, w/2) | 0 (l/2, w/2) 142 | # ==========|=========== 143 | # | | | 144 | # ---------|----------|-----------|-----------+ X 145 | # | | | 146 | # ==========|=========== 147 | # 7 (-l/2, -w/2) | 2 (l/2, -w/2) 148 | # | 149 | # | 150 | # 151 | # NOTE: The KITTI-360 annotation format is different from the 152 | # the KITTI-360 evaluation format listed here 153 | # https://github.com/autonomousvision/kitti360Scripts/blob/e0e3442991d3cf4c69debb84b48fcab3aabf2516/kitti360scripts/evaluation/semantic_3d/evalDetection.py#L549-L551 154 | # 155 | # To get the heading angle, we first take the midpoint of vertices 0 and 2 with box 156 | # centered at origin and take the arctan2 of the y-coordinate and x-coordinate 157 | midpoint = np.array([ 0.5*(obj3D.vertices[2,0] + obj3D.vertices[0,0]) - center[0], 0.5*(obj3D.vertices[2,1] + obj3D.vertices[0,1]) - center[1] ]) 158 | heading_angle = np.arctan2(midpoint[1], midpoint[0]) 159 | bboxes.append(([*center, *scale, heading_angle, id2label[ins_id//1000].id, ins_map[ins_id]])) 160 | except: 161 | print('Warning: error loading %d!' % ins_id) 162 | continue 163 | 164 | bboxes = np.array(bboxes) 165 | 166 | # save point center to recover the global coordinate 167 | seq_id = seq[0].split('_')[-2] 168 | output_fn = os.path.join(outputPath, '%s_'%seq_id + os.path.basename(fn).replace('.ply', '')+'_center.pth') 169 | # print('Saving to ' + output_fn) 170 | # torch.save(points_center, output_fn) 171 | 172 | # save bbox 173 | output_fn = os.path.join(outputPath, '%s_'%seq_id + os.path.basename(fn).replace('.ply', '')+'.pth') 174 | #print('Saving to ' + output_fn) 175 | #torch.save(bboxes, output_fn) 176 | output_fn = output_fn.replace(".pth", ".npy") 177 | print('Saving to ' + output_fn) 178 | np.save(output_fn, bboxes) 179 | 180 | # save coords and labels 181 | output_fn = os.path.join(outputPath, '%s_'%seq_id + os.path.basename(fn).replace('.ply', '')+'_inst_nostuff.pth') 182 | # print('Saving to ' + output_fn) 183 | # torch.save((coords, colors, sem_labels, instance_labels), output_fn) 184 | 185 | 186 | if __name__=="__main__": 187 | if 'KITTI360_DATASET' in os.environ: 188 | kitti360Path = os.environ['KITTI360_DATASET'] 189 | 190 | for fn in files: 191 | f(fn) 192 | -------------------------------------------------------------------------------- /kitti360scripts/evaluation/semantic_slam/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | project(KITTI360SemanticSlamEvaluation) 2 | cmake_minimum_required(VERSION 2.8) 3 | 4 | set(CMAKE_MODULE_PATH ${CMAKE_SOURCE_DIR}/cmake) 5 | 6 | # External dependency: boost filesystem 7 | find_package(Boost COMPONENTS filesystem system REQUIRED) 8 | include_directories(${Boost_INCLUDE_DIRS}) 9 | link_directories(${Boost_LIBRARY_DIRS}) 10 | 11 | # External dependency: Eigen3 12 | find_package(Eigen3 REQUIRED) 13 | include_directories(${EIGEN3_INCLUDE_DIRS}) 14 | 15 | # External dependency: PCL 16 | find_package(PCL 1.7 REQUIRED) 17 | include_directories(${PCL_INCLUDE_DIRS}) 18 | link_directories(${PCL_LIBRARY_DIRS}) 19 | add_definitions(${PCL_DEFINITIONS}) 20 | 21 | # Settings. 22 | if(NOT CMAKE_BUILD_TYPE) 23 | message(STATUS "Build type not specified, using RelWithDebInfo") 24 | set(CMAKE_BUILD_TYPE RelWithDebInfo) 25 | endif() 26 | 27 | if(NOT "${CMAKE_CXX_COMPILER_ID}" STREQUAL "MSVC") 28 | add_definitions("-O2 -msse2 -msse3 -std=c++11") 29 | endif() 30 | 31 | include_directories( 32 | ./src 33 | ) 34 | 35 | # Evaluation executable. 36 | add_executable(KITTI360SemanticSlamEvaluation 37 | src/accuracy.cc 38 | src/accuracy.h 39 | src/completeness.cc 40 | src/completeness.h 41 | src/main.cc 42 | src/pose.cc 43 | src/pose.h 44 | src/semantic.cc 45 | src/semantic.h 46 | src/commons.cpp 47 | src/commons.h 48 | src/util.h 49 | ) 50 | target_link_libraries(KITTI360SemanticSlamEvaluation 51 | ${PCL_LIBRARIES} 52 | ${Boost_LIBRARIES} 53 | ) 54 | -------------------------------------------------------------------------------- /kitti360scripts/evaluation/semantic_slam/README.md: -------------------------------------------------------------------------------- 1 | ########################################################################### 2 | 3 | KITTI-360: THE SEMANTIC SLAM BENCHMARK 4 | Yiyi Liao Jun Xie Andreas Geiger 5 | University of Tübingen 6 | Max Planck Institute for Intelligent Systems, Tübingen 7 | www.cvlibs.net/datasets/kitti-360 8 | 9 | ########################################################################### 10 | 11 | 12 | 13 | This file describes the KITTI-360 semantic localization and mapping benchmark that consists of 4 short test sequences. 14 | 15 | 16 | ## Test Data ## 17 | 18 | We evaluate all methods on 4 short sequences. For all sequences, we release both stereo images and Velodyne scans. 19 | You are free to choose the input modality. Please indicate the input modality you used when submitting to our benchmark. 20 | The stereo images of our test sequences are available in: 21 | ``` 22 | Download -> 2D data & labels -> Test SLAM (14G) 23 | ``` 24 | The Velodyne scans of our test sequences can be downlaoded in: 25 | ``` 26 | Download -> 3D data & labels -> Test SLAM (12G) 27 | ``` 28 | 29 | As we filter out very slow-moving frames, the frame ID of the test images may not be continuous. 30 | After filtering, there are 933, 1547, 2848, 3012 frames in our test sequences, respectively. To evaluate your method on our server, we expect exactly the same amount of frames in your submission. 31 | 32 | # Localization 33 | 34 | ## Output for Localication ## 35 | 36 | All results must be provided in the root directory of a zip file in the format of txt: 37 | ``` 38 | test_poses_0.txt 39 | test_poses_1.txt 40 | test_poses_2.txt 41 | test_poses_3.txt 42 | ``` 43 | For each txt file, one line indicates a flattened 3x4 matrix. Note that our ground truth is the rigid body transform from GPU/IMU coordinates to a world coordinate system (`poses.txt`). Please check our `test_data` folder for examples. 44 | 45 | 46 | ## Evaluation of Localization ## 47 | 48 | We adopt the great package [evo](https://github.com/MichaelGrupp/evo) to evaluate localization errors. 49 | To test your performance locally, you can install evo via: 50 | ``` 51 | pip install evo --upgrade --no-binary evo 52 | ``` 53 | Next, you can test our evaluation script via: 54 | ``` 55 | ./evalTrajectory.sh 56 | ``` 57 | Note that this test script compares the outputs of two methods, i.e., no ground truth is used here. 58 | 59 | # Semantic Mapping 60 | 61 | ## Output for Semantic Mapping ## 62 | 63 | All results must be provided in the root directory of a zip file. The file should contains the estimated poses, the reconstruction as well as the semantic labels: 64 | ``` 65 | test_poses_0.txt 66 | test_poses_1.txt 67 | test_poses_2.txt 68 | test_poses_3.txt 69 | test_0.ply 70 | test_1.ply 71 | test_2.ply 72 | test_3.ply 73 | test_semantic_0.txt 74 | test_semantic_1.txt 75 | test_semantic_2.txt 76 | test_semantic_3.txt 77 | ``` 78 | For each pose txt file, one line indicates a flattened 3x4 matrix. Note that our ground truth is the rigid body transform from GPU/IMU coordinates to a world coordinate system (`poses.txt`). Please check our `test_data` folder for examples. The ply files are reconstructed point clouds containing only `x y z` locations. The semantic label of each point should be provided in a seperate txt file containing a vector of length `M` equal to the number of points in the ply file. 79 | The semantic labels should follow the definition of [labels.py](https://github.com/autonomousvision/kitti360Scripts/blob/master/kitti360scripts/helpers/labels.py). Note that `id` should be used instead of `kittiId` or `trainId`. 80 | 81 | *IMPORTANT* Note that our evaluation server does not accept submission file larger than 200MB. It is required to apply our downsampling script to your reconstructed point cloud before submission. Please check the [sparify](https://github.com/autonomousvision/kitti360Scripts/tree/master/kitti360scripts/evaluation/semantic_slam/sparsify) code for more details. 82 | 83 | ## Evaluation of Semantic Mapping ## 84 | 85 | The evaluation script is adapted from the [ETH3D](https://github.com/ETH3D/multi-view-evaluation) benchmark. Here are the required dependencies: 86 | ``` 87 | Boost 88 | Eigen3 89 | PCL1.7 90 | ``` 91 | With the required dependencies installed, you can build the evaluation script via: 92 | ``` 93 | mkdir build && cd build 94 | cmake .. 95 | make 96 | ``` 97 | 98 | -------------------------------------------------------------------------------- /kitti360scripts/evaluation/semantic_slam/evalSemanticSLAM.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | reconstruction_dir=RECONSTRUCTION_DIR 4 | gt_dir=GT_DIR 5 | build_dir=./build 6 | 7 | test_ids=("0" "1" "2" "3") 8 | 9 | for test_id in "${test_ids[@]}" 10 | do 11 | echo "========== evaluating test sequence $test_id =========" 12 | output_dir=./${reconstruction_dir}/test_${test_id} 13 | mkdir -p $output_dir 14 | 15 | # evaluate completeness, accuracy and semantic at the threshold of 0.1m 16 | $build_dir/KITTI360SemanticSlamEvaluation --tolerances 0.1 \ 17 | --reconstruction_ply_path ${reconstruction_dir}/test_${test_id}.ply \ 18 | --reconstruction_pose_path ${reconstruction_dir}/test_poses_${test_id}.txt \ 19 | --reconstruction_semantic_path ${reconstruction_dir}/test_semantic_${test_id}.txt \ 20 | --ground_truth_data_path ${gt_dir}/gt_${test_id}.ply \ 21 | --ground_truth_pose_path ${gt_dir}/gt_poses_${test_id}.txt \ 22 | --ground_truth_observed_region_path ${gt_dir}/gt_observed_voxels_${test_id}.ply \ 23 | --ground_truth_semantic_path ${gt_dir}/gt_semantic_${test_id}.txt \ 24 | --ground_truth_conf_path ${gt_dir}/gt_confidence_binary_${test_id}.bin \ 25 | --completeness_cloud_output_path ${output_dir}/output_completeness \ 26 | --accuracy_cloud_output_path ${output_dir}/output_accuracy \ 27 | --result_output_path ${output_dir}/ 28 | #--voxel_size 0.075 29 | 30 | test_id=$(( test_id + 1 )) 31 | done 32 | 33 | exit 34 | -------------------------------------------------------------------------------- /kitti360scripts/evaluation/semantic_slam/evalTrajectory.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # This is NOT the real GT! In this demo, we compare the difference of Semantic SUMA and ORB-SLAM 4 | # to illustrate how the APE and RPE is evaluated on the KITTI-360 localication benchmark 5 | gt_dir=./test_data/semantic_suma 6 | reconstruction_dir=./test_data/orbslam 7 | 8 | out_dir=./results/$1/output 9 | mkdir -p $out_dir 10 | 11 | 12 | rm $out_dir/* 13 | 14 | test_ids=("0" "1" "2" "3") 15 | 16 | for test_id in "${test_ids[@]}" 17 | do 18 | 19 | if [ -f ${out_dir}/${test_id}_rpe.zip ]; then 20 | rm ${out_dir}/${test_id}_rpe.zip 21 | fi 22 | if [ -f ${out_dir}/${test_id}_ape.zip ]; then 23 | rm ${out_dir}/${test_id}_ape.zip 24 | fi 25 | 26 | evo_rpe kitti ${gt_dir}/test_poses_${test_id}.txt ${reconstruction_dir}/test_poses_${test_id}.txt \ 27 | -va \ 28 | --delta 1.0 \ 29 | --delta_unit m \ 30 | --plot_mode xy \ 31 | --save_plot ${out_dir}/${test_id}_rpe.pdf \ 32 | --save_results ${out_dir}/${test_id}_rpe.zip 33 | 34 | evo_ape kitti ${gt_dir}/test_poses_${test_id}.txt ${reconstruction_dir}/test_poses_${test_id}.txt \ 35 | -r trans_part \ 36 | -va \ 37 | --plot_mode xy \ 38 | --save_plot ${out_dir}/${test_id}_ape.pdf \ 39 | --save_results ${out_dir}/${test_id}_ape.zip 40 | 41 | done 42 | -------------------------------------------------------------------------------- /kitti360scripts/evaluation/semantic_slam/sparsify/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | project(sparsify) 2 | cmake_minimum_required(VERSION 2.8) 3 | 4 | set(CMAKE_MODULE_PATH ${CMAKE_SOURCE_DIR}/cmake) 5 | 6 | 7 | # External dependency: PCL 8 | find_package(PCL 1.7 REQUIRED) 9 | include_directories(${PCL_INCLUDE_DIRS}) 10 | link_directories(${PCL_LIBRARY_DIRS}) 11 | add_definitions(${PCL_DEFINITIONS}) 12 | 13 | # Settings. 14 | if(NOT CMAKE_BUILD_TYPE) 15 | message(STATUS "Build type not specified, using RelWithDebInfo") 16 | set(CMAKE_BUILD_TYPE RelWithDebInfo) 17 | endif() 18 | 19 | if(NOT "${CMAKE_CXX_COMPILER_ID}" STREQUAL "MSVC") 20 | add_definitions("-O2 -msse2 -msse3 -std=c++11") 21 | endif() 22 | 23 | # Evaluation executable. 24 | add_executable(sparsify 25 | main.cpp 26 | utils.cpp 27 | utils.h 28 | ) 29 | target_link_libraries(sparsify 30 | ${PCL_LIBRARIES} 31 | ) 32 | -------------------------------------------------------------------------------- /kitti360scripts/evaluation/semantic_slam/sparsify/README.md: -------------------------------------------------------------------------------- 1 | This script downsamples a given point cloud. 2 | It loops over all points and checks whether to keep or discard a point one by one. The output point cloud is first initialized to empty. If a point's distance to its nearest neighbor in the output point cloud is smaller than a given threshold, this point will be discarded. This downsampling ensures uniform downsampling without shifting the locations of the original points. 3 | 4 | ## Build 5 | 6 | ``` 7 | mkdir build && cd build 8 | cmake .. 9 | make 10 | ``` 11 | 12 | ## Run 13 | 14 | We provide an example script 15 | ``` 16 | ./run.sh 17 | ``` 18 | to demonstrate the usage of our downsampling script. 19 | 20 | 21 | -------------------------------------------------------------------------------- /kitti360scripts/evaluation/semantic_slam/sparsify/kdtree.h: -------------------------------------------------------------------------------- 1 | #ifndef __KDTREE_HPP 2 | #define __KDTREE_HPP 3 | 4 | // (c) Matthew B. Kennel, Institute for Nonlinear Science, UCSD (2004) 5 | // 6 | // Licensed under the Academic Free License version 1.1 found in file LICENSE 7 | // with additional provisions in that same file. 8 | // 9 | // Implement a kd tree for fast searching of points in a fixed data base 10 | // in k-dimensional Euclidean space. 11 | 12 | #include 13 | #include 14 | 15 | #include 16 | #include 17 | 18 | namespace kdtree { 19 | 20 | typedef boost::multi_array KDTreeArray; 21 | typedef boost::const_multi_array_ref KDTreeROArray; 22 | 23 | typedef struct { 24 | float lower, upper; 25 | } interval; 26 | 27 | // let the compiler know that this is a names of classes. 28 | class KDTreeNode; 29 | class SearchRecord; 30 | 31 | struct KDTreeResult { 32 | public: 33 | float dis; // square distance 34 | int idx; // neighbor index 35 | }; 36 | 37 | class KDTreeResultVector : public std::vector { 38 | // inherit a std::vector 39 | // but, optionally maintain it in heap form as a priority 40 | // queue. 41 | public: 42 | 43 | // 44 | // add one new element to the list of results, and 45 | // keep it in heap order. To keep it in ordinary, as inserted, 46 | // order, then simply use push_back() as inherited 47 | // via std::vector<> 48 | 49 | void push_element_and_heapify(KDTreeResult&); 50 | float replace_maxpri_elt_return_new_maxpri(KDTreeResult&); 51 | 52 | float max_value(); 53 | // return the distance which has the maximum value of all on list, 54 | // assuming that ALL insertions were made by 55 | // push_element_and_heapify() 56 | }; 57 | 58 | class KDTree { 59 | public: 60 | const KDTreeArray& the_data; 61 | // "the_data" is a reference to the underlying multi_array of the 62 | // data to be included in the tree. 63 | // 64 | // NOTE: this structure does *NOT* own the storage underlying this. 65 | // Hence, it would be a very bad idea to change the underlying data 66 | // during use of the search facilities of this tree. 67 | // Also, the user must deallocate the memory underlying it. 68 | 69 | 70 | const int N; // number of data points 71 | int dim; 72 | bool sort_results; // sorting result? 73 | const bool rearrange; // are we rearranging? 74 | 75 | public: 76 | 77 | // constructor, has optional 'dim_in' feature, to use only 78 | // first 'dim_in' components for definition of nearest neighbors. 79 | KDTree(KDTreeArray& data_in, bool rearrange_in = true, int dim_in=-1); 80 | 81 | // destructor 82 | ~KDTree(); 83 | 84 | 85 | public: 86 | 87 | void n_nearest_brute_force(std::vector& qv, int nn, KDTreeResultVector& result); 88 | // search for n nearest to a given query vector 'qv' usin 89 | // exhaustive slow search. For debugging, usually. 90 | 91 | void n_nearest(std::vector& qv, int nn, KDTreeResultVector& result); 92 | // search for n nearest to a given query vector 'qv'. 93 | 94 | void n_nearest_around_point(int idxin, int correltime, int nn, 95 | KDTreeResultVector& result); 96 | // search for 'nn' nearest to point [idxin] of the input data, excluding 97 | // neighbors within correltime 98 | 99 | void r_nearest(std::vector& qv, float r2, KDTreeResultVector& result); 100 | // search for all neighbors in ball of size (square Euclidean distance) 101 | // r2. Return number of neighbors in 'result.size()', 102 | 103 | void r_nearest_around_point(int idxin, int correltime, float r2, 104 | KDTreeResultVector& result); 105 | // like 'r_nearest', but around existing point, with decorrelation 106 | // interval. 107 | 108 | int r_count(std::vector& qv, float r2); 109 | // count number of neighbors within square distance r2. 110 | 111 | int r_count_around_point(int idxin, int correltime, float r2); 112 | // like r_count, c 113 | 114 | friend class KDTreeNode; 115 | friend class SearchRecord; 116 | 117 | private: 118 | 119 | KDTreeNode* root; // the root pointer 120 | 121 | const KDTreeArray* data; 122 | // pointing either to the_data or an internal 123 | // rearranged data as necessary 124 | 125 | std::vector ind; 126 | // the index for the tree leaves. Data in a leaf with bounds [l,u] are 127 | // in 'the_data[ind[l],*] to the_data[ind[u],*] 128 | 129 | KDTreeArray rearranged_data; 130 | // if rearrange is true then this is the rearranged data storage. 131 | 132 | static const int bucketsize = 12; // global constant. 133 | 134 | private: 135 | void set_data(KDTreeArray& din); 136 | void build_tree(); // builds the tree. Used upon construction. 137 | KDTreeNode* build_tree_for_range(int l, int u, KDTreeNode* parent); 138 | void select_on_coordinate(int c, int k, int l, int u); 139 | int select_on_coordinate_value(int c, float alpha, int l, int u); 140 | void spread_in_coordinate(int c, int l, int u, interval& interv); 141 | }; 142 | 143 | class KDTreeNode { 144 | public: 145 | KDTreeNode(int dim); 146 | ~KDTreeNode(); 147 | 148 | private: 149 | // visible to self and KDTree. 150 | friend class KDTree; // allow kdtree to access private data 151 | 152 | int cut_dim; // dimension to cut; 153 | float cut_val, cut_val_left, cut_val_right; //cut value 154 | int l, u; // extents in index array for searching 155 | 156 | std::vector box; // [min,max] of the box enclosing all points 157 | 158 | KDTreeNode *left, *right; // pointers to left and right nodes. 159 | 160 | void search(SearchRecord& sr); 161 | // recursive innermost core routine for searching.. 162 | 163 | bool box_in_search_range(SearchRecord& sr); 164 | // return true if the bounding box for this node is within the 165 | // search range given by the searchvector and maximum ballsize in 'sr'. 166 | 167 | void check_query_in_bound(SearchRecord& sr); // debugging only 168 | 169 | // for processing final buckets. 170 | void process_terminal_node(SearchRecord& sr); 171 | void process_terminal_node_fixedball(SearchRecord& sr); 172 | }; 173 | 174 | } // namespace kdtree 175 | 176 | #endif 177 | -------------------------------------------------------------------------------- /kitti360scripts/evaluation/semantic_slam/sparsify/main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "utils.h" 4 | 5 | using namespace std; 6 | 7 | void sparsify (PointCloudPtr &cloud, int32_t* idx_sparse, int32_t idx_size, double min_dist) { 8 | 9 | // create kdtree 10 | pcl::KdTreeFLANN kdtree; 11 | kdtree.setInputCloud (cloud); 12 | 13 | // for all data points do 14 | for (int32_t i=0; ipoints.size(); i++) { 15 | 16 | // Neighbors within radius search 17 | vector result; 18 | vector dist; 19 | 20 | kdtree.radiusSearch(cloud->points[i], min_dist, result, dist); 21 | 22 | bool neighbor_exists = false; 23 | for (int32_t j=1; j(0); 65 | } 66 | int32_t M_num = cloud->points.size(); 67 | std::cout << "Loaded point cloud of " << M_num << " points" << endl; 68 | 69 | // sparsify 70 | PointCloudPtr cloud_out(new PointCloud()); 71 | int32_t *idx_sparse = (int32_t*)calloc(M_num,sizeof(int32_t)); 72 | int32_t idx_size = 0; 73 | 74 | sparsify(cloud,idx_sparse, idx_size, min_dist); 75 | 76 | vector idx_vector; 77 | idx_vector.clear(); 78 | for (int32_t i=0; i semantic_label, semantic_label_filtered; 87 | if (!input_semantic_path.empty()) { 88 | LoadSemanticLabel(input_semantic_path, semantic_label); 89 | std::cout << "Loaded semantic label of " << semantic_label.size() << " points" << endl; 90 | RunExtractVector(semantic_label, idx_vector, semantic_label_filtered); 91 | std::cout << "Keep " << semantic_label_filtered.size() << " semantic points after filtering" << endl; 92 | } 93 | 94 | // output 95 | std::ostringstream file_path; 96 | file_path << output_path; 97 | pcl::io::savePLYFileBinary(file_path.str(), *cloud_out); 98 | 99 | if (!output_semantic_path.empty() && !input_semantic_path.empty()) { 100 | WriteData(output_semantic_path, semantic_label_filtered); 101 | } 102 | 103 | if (!output_mask_path.empty()) { 104 | WriteData(output_mask_path, idx_vector); 105 | } 106 | 107 | free(idx_sparse); 108 | 109 | return static_cast(1); 110 | } 111 | -------------------------------------------------------------------------------- /kitti360scripts/evaluation/semantic_slam/sparsify/run.sh: -------------------------------------------------------------------------------- 1 | 2 | input_dir=INPUT_DIR 3 | output_dir=INPUT_DIR_DOWNSAMPLE 4 | mkdir -p $output_dir 5 | 6 | test_ids=("0" "1" "2" "3") 7 | 8 | for test_id in "${test_ids[@]}" 9 | do 10 | 11 | ## downsample pred 12 | ./build/sparsify --input_path ${input_dir}/test_${test_id}.ply \ 13 | --input_semantic_path ${input_dir}/test_semantic_${test_id}.txt \ 14 | --output_path ${output_dir}/test_${test_id}.ply \ 15 | --output_semantic_path ${output_dir}/test_semantic_${test_id}.txt \ 16 | --min_dist 0.075 \ 17 | --output_mask_path ${output_dir}/test_mask_${test_id}.txt 18 | 19 | done 20 | -------------------------------------------------------------------------------- /kitti360scripts/evaluation/semantic_slam/sparsify/utils.cpp: -------------------------------------------------------------------------------- 1 | #include "utils.h" 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | /** 11 | * @brief RunExtractCloud: Extract point cloud given indices 12 | * @param cloud: Input point cloud 13 | * @param indices: Point indices 14 | * @param cloud_out: Output point cloud 15 | * @param setnegative: if true, extract the point not in the indices 16 | */ 17 | void RunExtractCloud(const PointCloudPtr &cloud, vector indices, 18 | PointCloudPtr &cloud_out, bool setnegative){ 19 | pcl::PointIndices::Ptr inliers (new pcl::PointIndices ()); 20 | inliers->indices = indices; 21 | 22 | pcl::ExtractIndices extract; 23 | extract.setInputCloud (cloud); 24 | extract.setIndices (inliers); 25 | extract.setNegative (setnegative); 26 | extract.filter (*cloud_out); 27 | } 28 | 29 | /** 30 | * @brief LoadConfidenceBinary Load semantic label from binary file. 31 | * @param filename 32 | * @param confidence Output confidence 33 | * @return length of loaded timestamp. 34 | */ 35 | int LoadConfidenceBinary(const std::string& filename, vector &confidence) { 36 | confidence.clear(); 37 | 38 | ifstream infile; 39 | infile.open(filename.c_str(), ios::in | ios::binary); 40 | 41 | if (!infile) { 42 | std::cout << "Failed to open timestamp data " << filename << std::endl; 43 | return -1; 44 | } 45 | 46 | float t; 47 | 48 | while (!infile.eof()) { 49 | infile.read((char*) &t, sizeof(t)); 50 | if (infile.eof()) break; 51 | 52 | confidence.push_back(t); 53 | } 54 | 55 | infile.close(); 56 | return confidence.size(); 57 | } 58 | 59 | /** 60 | * @brief LoadSemanticLabel Load semantic label from file. 61 | * @param filename 62 | * @param label Output semantic label 63 | * @return length of loaded timestamp. 64 | */ 65 | int LoadSemanticLabel(const std::string& filename, vector &label) { 66 | label.clear(); 67 | 68 | ifstream infile; 69 | infile.open(filename.c_str()); 70 | 71 | if (!infile) { 72 | std::cout << "Failed to open timestamp data " << filename << std::endl; 73 | return -1; 74 | } 75 | 76 | int t; 77 | 78 | while (!infile.eof()) { 79 | infile >> t; 80 | if (infile.eof()) break; 81 | 82 | label.push_back(t); 83 | } 84 | 85 | infile.close(); 86 | return label.size(); 87 | } 88 | -------------------------------------------------------------------------------- /kitti360scripts/evaluation/semantic_slam/sparsify/utils.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | using namespace std; 16 | 17 | typedef pcl::PointCloud PointCloud; 18 | typedef pcl::PointCloud::Ptr PointCloudPtr; 19 | 20 | void RunExtractCloud(const PointCloudPtr &cloud, vector indices, 21 | PointCloudPtr &cloud_out, bool setnegative); 22 | int LoadConfidenceBinary(const std::string& filename, vector &confidence); 23 | int LoadSemanticLabel(const std::string& filename, vector &label); 24 | 25 | template 26 | void RunExtractVector(const vector &input, vector indices, vector &output, bool inverse=false){ 27 | output.clear(); 28 | if (inverse){ 29 | if (indices.size()==0){ 30 | for (int i=0; i 53 | void WriteData(const std::string& filename, vector &vector){ 54 | ofstream outfile; 55 | outfile.open(filename.c_str()); 56 | 57 | for (int i = 0; i < vector.size(); i++) { 58 | outfile << vector[i] << "\n"; 59 | } 60 | outfile.close(); 61 | } 62 | -------------------------------------------------------------------------------- /kitti360scripts/evaluation/semantic_slam/src/accuracy.h: -------------------------------------------------------------------------------- 1 | // The evaluation code is built and modified based on the following repo: 2 | // https://github.com/ETH3D/multi-view-evaluation 3 | // following copyright and permission notice: 4 | // 5 | // Copyright 2017 Thomas Schöps 6 | // 7 | // Redistribution and use in source and binary forms, with or without 8 | // modification, are permitted provided that the following conditions are met: 9 | // 10 | // 1. Redistributions of source code must retain the above copyright notice, 11 | // this list of conditions and the following disclaimer. 12 | // 13 | // 2. Redistributions in binary form must reproduce the above copyright notice, 14 | // this list of conditions and the following disclaimer in the documentation 15 | // and/or other materials provided with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its contributors 18 | // may be used to endorse or promote products derived from this software 19 | // without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 23 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 24 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 25 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 26 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 27 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 28 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 29 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 30 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | // POSSIBILITY OF SUCH DAMAGE. 32 | 33 | #pragma once 34 | 35 | #include 36 | #include 37 | #include 38 | 39 | #include "util.h" 40 | 41 | enum class AccuracyResult : uint8_t { 42 | kUnobserved = 0, 43 | kAccurate = 1, 44 | kInaccurate = 2 45 | }; 46 | 47 | // Computes the completeness of a partial point cloud, save results to a 48 | // global output 49 | void ComputePointAccuracy(const PointCloudPtr& reconstruction, 50 | // global indices of each scan point 51 | const std::vector reconstruction_point_global_indices, 52 | const PointCloudPtr& scan, 53 | // free or occupied region obtained from offline Octomap 54 | const PointCloudPtr& occupied_or_free_voxel_centers, 55 | const float occupied_or_free_voxel_size, 56 | // Sorted by increasing tolerance. 57 | const std::vector& sorted_tolerances, 58 | // Indexed by: [scan_point_global_index]. 59 | std::vector* first_accuracy_tolerance_index, 60 | std::vector* inaccurate_classifications_exist, 61 | // Indexed by: [tolerance_index][scan_point_index]. 62 | std::vector>* point_is_accurate); 63 | 64 | void ComputeVoxelAccuracy( 65 | const PointCloud& reconstruction, 66 | const std::vector& first_accurate_tolerance_indices, 67 | const std::vector& inaccurate_classifications_exist, 68 | float voxel_size_inv, 69 | // Sorted by increasing tolerance. 70 | const std::vector& sorted_tolerances, 71 | // Indexed by: [tolerance_index]. Range: [0, 1]. 72 | std::vector* results); 73 | 74 | void ComputeAverageAccuracy( 75 | const PointCloud& reconstruction, 76 | const std::vector& first_accurate_tolerance_indices, 77 | const std::vector& inaccurate_classifications_exist, 78 | // Sorted by increasing tolerance. 79 | const std::vector& sorted_tolerances, 80 | // Indexed by: [tolerance_index]. Range: [0, 1]. 81 | std::vector* results); 82 | 83 | void WriteAccuracyVisualization( 84 | const std::string& base_path, 85 | const PointCloud& reconstruction, 86 | // Sorted by increasing tolerance. 87 | const std::vector& sorted_tolerances, 88 | // Indexed by: [tolerance_index][point_index]. 89 | const std::vector>& point_is_accurate, 90 | const int window_index=-1); 91 | -------------------------------------------------------------------------------- /kitti360scripts/evaluation/semantic_slam/src/commons.h: -------------------------------------------------------------------------------- 1 | 2 | #ifndef CODE_DATACONVERTER_SRC_COMMONS_H_ 3 | #define CODE_DATACONVERTER_SRC_COMMONS_H_ 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | #include "util.h" 10 | 11 | using namespace std; 12 | 13 | int LoadDenseData(const std::string& filename, PointCloud::Ptr &outputCloud, 14 | PointXYZ ¢er); 15 | int LoadDenseDataXYZ(const std::string& filename, PointCloud::Ptr &outputCloud); 16 | int LoadDenseTimestamp(const std::string& filename, vector ×tamp); 17 | int LoadDenseTimestampAscii(const std::string& filename, vector> ×tamp); 18 | int LoadDenseTimestampBinary(const std::string& filename, vector> ×tamp); 19 | bool LoadPoses(const std::string& filename, map &scan_infos); 20 | int LoadVoxelCenters(const std::string& filename, PointCloud::Ptr &outputCloud); 21 | int LoadSemanticLabel(const std::string& filename, vector &label); 22 | int LoadConfidence(const std::string& filename, vector &confidence); 23 | int LoadConfidenceBinary(const std::string& filename, vector &confidence); 24 | void WriteResult(const std::string& filename, std::vector tolerances, std::vector result); 25 | void WriteBinaryFile(const std::string& filename, pcl::PointCloud &cloud, int save_every=100 ); 26 | 27 | void GetPointsInRange(const PointCloudPtr cloud, 28 | std::vector poses, 29 | PointCloudPtr &output_cloud, 30 | std::vector &inlier_indices, 31 | double maxPointDist=80.); 32 | void RunExtractCloud(const PointCloudPtr cloud, pcl::PointIndices::Ptr inliers, 33 | PointCloudPtr &cloud_out, bool setnegative=false); 34 | void TransformCloud(const PointCloudPtr src_cloud, const Eigen::Matrix4f transform, 35 | PointCloudPtr &tgt_cloud); 36 | void DownsampleCloud(const PointCloudPtr src_cloud, PointCloudPtr &tgt_cloud, 37 | float leaf_size=0.1); 38 | #endif // CODE_DATACONVERTER_SRC_COMMONS_H_ 39 | -------------------------------------------------------------------------------- /kitti360scripts/evaluation/semantic_slam/src/completeness.h: -------------------------------------------------------------------------------- 1 | // The evaluation code is built and modified based on the following repo: 2 | // https://github.com/ETH3D/multi-view-evaluation 3 | // following copyright and permission notice: 4 | // 5 | // Copyright 2017 Thomas Schöps 6 | // 7 | // Redistribution and use in source and binary forms, with or without 8 | // modification, are permitted provided that the following conditions are met: 9 | // 10 | // 1. Redistributions of source code must retain the above copyright notice, 11 | // this list of conditions and the following disclaimer. 12 | // 13 | // 2. Redistributions in binary form must reproduce the above copyright notice, 14 | // this list of conditions and the following disclaimer in the documentation 15 | // and/or other materials provided with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its contributors 18 | // may be used to endorse or promote products derived from this software 19 | // without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 23 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 24 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 25 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 26 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 27 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 28 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 29 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 30 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | // POSSIBILITY OF SUCH DAMAGE. 32 | 33 | #pragma once 34 | 35 | #include 36 | #include 37 | #include 38 | 39 | //#include "meshlab_project.h" 40 | #include "util.h" 41 | 42 | // Computes the completeness of a partial point cloud, save results to a 43 | // global output 44 | void ComputePointCompleteness(const PointCloudPtr& scan, 45 | // global indices of each scan point 46 | const std::vector scan_point_global_indices, 47 | const PointCloudPtr& reconstruction, 48 | // global indices of each reconstruction point 49 | const std::vector reconstruction_point_global_indices, 50 | // Sorted by increasing tolerance. 51 | const std::vector& sorted_tolerances, 52 | // Indexed by: [scan_point_global_index]. 53 | std::vector* first_completeness_tolerance_index, 54 | // Indexed by: [tolerance_index][scan_point_index]. 55 | std::vector>* scan_nn_point_distances, 56 | // Indexed by: [tolerance_index][scan_point_index]. 57 | std::vector>* scan_nn_point_indices, 58 | // Indexed by: [tolerance_index][scan_point_index]. 59 | std::vector>* point_is_complete); 60 | 61 | // Computes the completeness of the reconstruction with respect to the given 62 | // scans. 63 | void ComputeVoxelCompleteness( 64 | const PointCloudPtr& scan, 65 | float voxel_size_inv, 66 | // Sorted by increasing tolerance. 67 | const std::vector& sorted_tolerances, 68 | // Indexed by: [scan_point_global_index]. 69 | const std::vector& first_completeness_tolerance_index, 70 | // Indexed by: [tolerance_index]. Range: [0, 1]. 71 | std::vector* results); 72 | 73 | void ComputeAverageCompleteness(const PointCloudPtr& scan, 74 | // Sorted by increasing tolerance. 75 | const std::vector& sorted_tolerances, 76 | // Indexed by: [scan_point_global_index]. 77 | const std::vector& first_completeness_tolerance_index, 78 | // Indexed by: [tolerance_index]. Range: [0, 1]. 79 | std::vector* results); 80 | 81 | void WriteCompletenessVisualization( 82 | const std::string& base_path, 83 | // const MeshLabMeshInfoVector& scan_infos, 84 | const std::map &scan_infos, 85 | const PointCloudPtr& scan, 86 | // Sorted by increasing tolerance. 87 | const std::vector& sorted_tolerances, 88 | // Indexed by: [tolerance_index][scan_point_index]. 89 | const std::vector>& point_is_complete, 90 | const int window_index=-1); 91 | -------------------------------------------------------------------------------- /kitti360scripts/evaluation/semantic_slam/src/pose.cc: -------------------------------------------------------------------------------- 1 | 2 | #include "pose.h" 3 | #include 4 | 5 | using namespace Eigen; 6 | using namespace std; 7 | 8 | void AlignTrajectory(std::vector ground_truth_poses, 9 | std::vector estimated_poses, 10 | std::vector &aligned_estimated_poses, 11 | Matrix4f &transform, 12 | Mode mode){ 13 | // Align the trajectory to the ground truth in the given mode 14 | // Add virtual points to align rotation 15 | Matrix estimated_points; 16 | estimated_points.resize(NoChange, estimated_poses.size()*4); 17 | for (std::size_t i = 0; i < estimated_poses.size(); ++ i) { 18 | estimated_points.col(i*4+0) = estimated_poses[i].topRightCorner<3,1>(); 19 | estimated_points.col(i*4+1) = estimated_poses[i].block<3,1>(0,0) + estimated_poses[i].topRightCorner<3,1>(); 20 | estimated_points.col(i*4+2) = estimated_poses[i].block<3,1>(0,1) + estimated_poses[i].topRightCorner<3,1>(); 21 | estimated_points.col(i*4+3) = estimated_poses[i].block<3,1>(0,2) + estimated_poses[i].topRightCorner<3,1>(); 22 | } 23 | 24 | Matrix ground_truth_points; 25 | ground_truth_points.resize(NoChange, ground_truth_poses.size()*4); 26 | for (std::size_t i = 0; i < ground_truth_poses.size(); ++ i) { 27 | ground_truth_points.col(i*4+0) = ground_truth_poses[i].topRightCorner<3,1>(); 28 | ground_truth_points.col(i*4+1) = ground_truth_poses[i].block<3,1>(0,0) + ground_truth_poses[i].topRightCorner<3,1>(); 29 | ground_truth_points.col(i*4+2) = ground_truth_poses[i].block<3,1>(0,1) + ground_truth_poses[i].topRightCorner<3,1>(); 30 | ground_truth_points.col(i*4+3) = ground_truth_poses[i].block<3,1>(0,2) + ground_truth_poses[i].topRightCorner<3,1>(); 31 | } 32 | 33 | // Estimate transform such that: ground_truth_point =approx= transform * estimated_point 34 | //Matrix transform = Eigen::umeyama(estimated_points, ground_truth_points, mode == Mode::Sim3); 35 | transform = Eigen::umeyama(estimated_points, ground_truth_points, mode == Mode::Sim3); 36 | 37 | aligned_estimated_poses.resize(estimated_points.size()); 38 | for (std::size_t i = 0; i < estimated_poses.size(); ++ i) { 39 | aligned_estimated_poses[i] = transform * estimated_poses[i]; 40 | } 41 | } 42 | 43 | -------------------------------------------------------------------------------- /kitti360scripts/evaluation/semantic_slam/src/pose.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #include 6 | #include "util.h" 7 | #include "commons.h" 8 | 9 | #pragma once 10 | 11 | using namespace Eigen; 12 | 13 | enum class Mode { 14 | SE3 = 0, 15 | Sim3 = 1 16 | }; 17 | 18 | void AlignTrajectory(std::vector ground_truth_poses, 19 | std::vector estimated_poses, 20 | std::vector &aligned_estimated_poses, 21 | Matrix4f &transform, 22 | Mode mode=Mode::SE3); 23 | -------------------------------------------------------------------------------- /kitti360scripts/evaluation/semantic_slam/src/semantic.cc: -------------------------------------------------------------------------------- 1 | #include "semantic.h" 2 | 3 | std::map classmap={ 4 | {0, 255}, // unlabeled 5 | {1, 255}, // ego vehicle 6 | {2, 255}, // rectification border 7 | {3, 255}, // out of roi 8 | {4, 255}, // static 9 | {5, 255}, // dynamic 10 | {6, 255}, // ground 11 | {7, 0}, // road 12 | {8, 1}, // sidewalk 13 | {9, 255}, // parking 14 | {10, 255}, // rail track 15 | {11, 2}, // building 16 | {12, 3}, // wall 17 | {13, 4}, // fence 18 | {14, 255}, // guard rail 19 | {15, 255}, // bridge 20 | {16, 255}, // tunnel 21 | {17, 5}, // pole 22 | {18, 255}, // polegroup 23 | {19, 6}, // traffic light 24 | {20, 7}, // traffic sign 25 | {21, 8}, // vegetation 26 | {22, 9}, // terrain 27 | {23, 10}, // sky 28 | {24, 11}, // person 29 | {25, 12}, // rider 30 | {26, 13}, // car 31 | {27, 14}, // truck 32 | {28, 15}, // bus 33 | {29, 255}, // caravan 34 | {30, 255}, // trailer 35 | {31, 16}, // train 36 | {32, 17}, // motorcycle 37 | {33, 18}, // bicycle 38 | {34, 2}, // garage 39 | {35, 4}, // gate 40 | {36, 255}, // stop 41 | {37, 5}, // smallpole 42 | {38, 255}, // lamp 43 | {39, 255}, // trash bin 44 | {40, 255}, // vending machine 45 | {41, 255}, // box 46 | {42, 255}, // unknown construction 47 | {43, 255}, // unknown vehicle 48 | {44, 255}, // unknown object 49 | }; 50 | 51 | void ComputeSemanticMeanIoU(const std::vector scan_semantic, 52 | const std::vector scan_confidence, 53 | const std::vector reconstruction_semantic, 54 | const std::vector> &scan_nn_point_indices, 55 | int semantic_class_count, 56 | std::vector* results){ 57 | 58 | int tolerances_count = scan_nn_point_indices.size(); 59 | 60 | // initialize confusion matrix 61 | std::vector>> confusion_matrices(tolerances_count); 62 | for (int k=0; k confusion_vector(semantic_class_count+1, 0.0); 66 | confusion_matrices[k].push_back(confusion_vector); 67 | } 68 | } 69 | 70 | // loop over all points in the ground truth cloud to obtain the confusion matrix 71 | for (int i=0; i-1){ 81 | int pred_class_raw = reconstruction_semantic[scan_nn_point_indices[tolerance_index][i]]; 82 | // map id to trainId 83 | int pred_class = classmap[pred_class_raw]; 84 | if (pred_class==255) pred_class=semantic_class_count; 85 | // confidence-weighted evaluation 86 | confusion_matrices[tolerance_index][gt_class][pred_class] += scan_confidence[i]; 87 | } 88 | // no reconstruction point completes the ground truth point, will be considered as false negative 89 | else{ 90 | // confidence-weighted evaluation 91 | confusion_matrices[tolerance_index][gt_class][semantic_class_count] += scan_confidence[i]; 92 | } 93 | } 94 | } 95 | 96 | // calculate average score 97 | for (int k=0; k true_positive_count(semantic_class_count, 0); 99 | std::vector false_negative_count(semantic_class_count, 0); 100 | std::vector false_positive_count(semantic_class_count+1, 0); 101 | // ignore last row, i.e., ground truth point is labeled as unknown/ignored classes 102 | for (int i=0; i score(semantic_class_count, 0); 118 | float mIoU = 0.0; 119 | float valid_count = 0.0; 120 | for (int i=0; i0){ 123 | score[i] = true_positive_count[i] / denom; 124 | valid_count += 1; 125 | mIoU += score[i]; 126 | } else{ 127 | score[i] = -1; 128 | } 129 | std::cout << "class " << i << " score " << score[i] << std::endl; 130 | } 131 | mIoU /= valid_count; 132 | results->at(k) = mIoU; 133 | } 134 | } 135 | -------------------------------------------------------------------------------- /kitti360scripts/evaluation/semantic_slam/src/semantic.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | void ComputeSemanticMeanIoU(const std::vector scan_semantic, 8 | const std::vector scan_confidence, 9 | const std::vector reconstruction_semantic, 10 | const std::vector> &scan_nn_point_indices, 11 | int semantic_class_count, 12 | // Indexed by: [tolerance_index]. Range: [0, 1]. 13 | std::vector* results); 14 | 15 | -------------------------------------------------------------------------------- /kitti360scripts/evaluation/semantic_slam/src/util.h: -------------------------------------------------------------------------------- 1 | // Copyright 2017 Thomas Schöps 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // 1. Redistributions of source code must retain the above copyright notice, 7 | // this list of conditions and the following disclaimer. 8 | // 9 | // 2. Redistributions in binary form must reproduce the above copyright notice, 10 | // this list of conditions and the following disclaimer in the documentation 11 | // and/or other materials provided with the distribution. 12 | // 13 | // 3. Neither the name of the copyright holder nor the names of its contributors 14 | // may be used to endorse or promote products derived from this software 15 | // without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | 29 | #pragma once 30 | 31 | #include 32 | 33 | #include 34 | #include 35 | #include 36 | #include 37 | 38 | typedef pcl::PointXYZ PointXYZ; 39 | typedef pcl::PointCloud PointCloud; 40 | typedef pcl::PointCloud::Ptr PointCloudPtr; 41 | typedef pcl::PointCloud::Ptr PointCloudRGBPtr; 42 | 43 | // Make std::unordered_map work with generic tuples. 44 | // Source: http://stackoverflow.com/questions/7110301 45 | // Note that this is not standard-conformant. 46 | namespace std { 47 | namespace { 48 | // Code from boost 49 | // Reciprocal of the golden ratio helps spread entropy 50 | // and handles duplicates. 51 | // See Mike Seymour in magic-numbers-in-boosthash-combine: 52 | // http://stackoverflow.com/questions/4948780 53 | 54 | template 55 | inline void hash_combine(std::size_t& seed, T const& v) { 56 | seed ^= std::hash()(v) + 0x9e3779b9 + (seed<<6) + (seed>>2); 57 | } 58 | 59 | // Recursive template code derived from Matthieu M. 60 | template ::value - 1> 61 | struct HashValueImpl { 62 | static void apply(size_t& seed, Tuple const& tuple) { 63 | HashValueImpl::apply(seed, tuple); 64 | hash_combine(seed, std::get(tuple)); 65 | } 66 | }; 67 | 68 | template 69 | struct HashValueImpl { 70 | static void apply(size_t& seed, Tuple const& tuple) { 71 | hash_combine(seed, std::get<0>(tuple)); 72 | } 73 | }; 74 | } 75 | 76 | template 77 | struct hash> { 78 | size_t operator()(std::tuple const& tt) const { 79 | size_t seed = 0; 80 | HashValueImpl >::apply(seed, tt); 81 | return seed; 82 | } 83 | }; 84 | } 85 | 86 | inline std::tuple CalcCellCoordinates( 87 | const pcl::PointXYZ& point, float voxel_size_inv, 88 | float shift_x, float shift_y, float shift_z) { 89 | float voxel_x = point.x * voxel_size_inv + shift_x; 90 | int x = static_cast(voxel_x); 91 | if (voxel_x - x != 0 && voxel_x < 0) { 92 | -- x; 93 | } 94 | 95 | float voxel_y = point.y * voxel_size_inv + shift_y; 96 | int y = static_cast(voxel_y); 97 | if (voxel_y - y != 0 && voxel_y < 0) { 98 | -- y; 99 | } 100 | 101 | float voxel_z = point.z * voxel_size_inv + shift_z; 102 | int z = static_cast(voxel_z); 103 | if (voxel_z - z != 0 && voxel_z < 0) { 104 | -- z; 105 | } 106 | 107 | return std::tuple(x, y, z); 108 | } 109 | -------------------------------------------------------------------------------- /kitti360scripts/helpers/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/autonomousvision/kitti360Scripts/40ee015de8277b2d907d32a30cdbd97072c7d7c3/kitti360scripts/helpers/__init__.py -------------------------------------------------------------------------------- /kitti360scripts/helpers/csHelpers.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | # 3 | # Various helper methods and includes for Cityscapes 4 | # 5 | 6 | # Python imports 7 | from __future__ import print_function, absolute_import, division 8 | import os, sys, getopt 9 | import glob 10 | import math 11 | import json 12 | from collections import namedtuple 13 | import logging 14 | import traceback 15 | 16 | # Image processing 17 | # Check if PIL is actually Pillow as expected 18 | try: 19 | from PIL import PILLOW_VERSION 20 | except: 21 | print("Please install the module 'Pillow' for image processing, e.g.") 22 | print("pip install pillow") 23 | sys.exit(-1) 24 | 25 | try: 26 | import PIL.Image as Image 27 | import PIL.ImageDraw as ImageDraw 28 | except: 29 | print("Failed to import the image processing packages.") 30 | sys.exit(-1) 31 | 32 | # Numpy for datastructures 33 | try: 34 | import numpy as np 35 | except: 36 | print("Failed to import numpy package.") 37 | sys.exit(-1) 38 | 39 | # Cityscapes modules 40 | try: 41 | #from kitti360scripts.helpers.annotation import Annotation 42 | from kitti360scripts.helpers.labels import labels, name2label, id2label, trainId2label, category2labels 43 | except ImportError as err: 44 | print("Failed to import all Cityscapes modules: %s" % err) 45 | sys.exit(-1) 46 | except Exception as e: 47 | logging.error(traceback.format_exc()) 48 | sys.exit(-1) 49 | except: 50 | print("Unexpected error in loading Cityscapes modules") 51 | print(sys.exc_info()[0]) 52 | sys.exit(-1) 53 | 54 | # Print an error message and quit 55 | def printError(message): 56 | print('ERROR: ' + str(message)) 57 | sys.exit(-1) 58 | 59 | # Class for colors 60 | class colors: 61 | RED = '\033[31;1m' 62 | GREEN = '\033[32;1m' 63 | YELLOW = '\033[33;1m' 64 | BLUE = '\033[34;1m' 65 | MAGENTA = '\033[35;1m' 66 | CYAN = '\033[36;1m' 67 | BOLD = '\033[1m' 68 | UNDERLINE = '\033[4m' 69 | ENDC = '\033[0m' 70 | 71 | # Colored value output if colorized flag is activated. 72 | def getColorEntry(val, args): 73 | if not args.colorized: 74 | return "" 75 | if not isinstance(val, float) or math.isnan(val): 76 | return colors.ENDC 77 | if (val < .20): 78 | return colors.RED 79 | elif (val < .40): 80 | return colors.YELLOW 81 | elif (val < .60): 82 | return colors.BLUE 83 | elif (val < .80): 84 | return colors.CYAN 85 | else: 86 | return colors.GREEN 87 | 88 | # Cityscapes files have a typical filename structure 89 | # ___[_]. 90 | # This class contains the individual elements as members 91 | # For the sequence and frame number, the strings are returned, including leading zeros 92 | CsFile = namedtuple( 'csFile' , [ 'sequenceNb' , 'frameNb' , 'ext' ] ) 93 | CsWindow = namedtuple( 'csFile' , [ 'sequenceNb' , 'firstFrameNb' , 'lastFrameNb' ] ) 94 | 95 | # Returns a CsFile object filled from the info in the given filename 96 | def getFileInfo(fileName): 97 | baseName = os.path.basename(fileName) 98 | sequenceName = [x for x in fileName.split('/') if x.startswith('2013_05_28')] 99 | if not len(sequenceName)==1: 100 | printError( 'Cannot parse given filename ({}). Does not seem to be a valid KITTI-360 file.'.format(fileName) ) 101 | sequenceNb = int(sequenceName[0].split('_')[4]) 102 | frameNb = int(os.path.splitext(baseName)[0]) 103 | csFile = CsFile(sequenceNb, frameNb, os.path.splitext(baseName)[1]) 104 | return csFile 105 | 106 | # Returns a CsFile object filled from the info in the given filename 107 | def getWindowInfo(fileName): 108 | baseName = os.path.basename(fileName) 109 | sequenceName = [x for x in fileName.split('/') if x.startswith('2013_05_28')] 110 | if not len(sequenceName)==1: 111 | printError( 'Cannot parse given filename ({}). Does not seem to be a valid KITTI-360 file.'.format(fileName) ) 112 | sequenceNb = int(sequenceName[0].split('_')[4]) 113 | windowName = os.path.splitext(baseName)[0] 114 | firstFrameNb = int(windowName.split('_')[0]) 115 | lastFrameNb = int(windowName.split('_')[1]) 116 | csWindow = CsWindow(sequenceNb, firstFrameNb, lastFrameNb) 117 | return csWindow 118 | 119 | # Returns a CsFile object filled from the info in the given filename 120 | def getCsFileInfo(fileName): 121 | baseName = os.path.basename(fileName) 122 | parts = baseName.split('_') 123 | parts = parts[:-1] + parts[-1].split('.') 124 | if not parts: 125 | printError( 'Cannot parse given filename ({}). Does not seem to be a valid Cityscapes file.'.format(fileName) ) 126 | if len(parts) == 5: 127 | csFile = CsFile( *parts[:-1] , type2="" , ext=parts[-1] ) 128 | elif len(parts) == 6: 129 | csFile = CsFile( *parts ) 130 | else: 131 | printError( 'Found {} part(s) in given filename ({}). Expected 5 or 6.'.format(len(parts) , fileName) ) 132 | 133 | return csFile 134 | 135 | # Returns the part of Cityscapes filenames that is common to all data types 136 | # e.g. for city_123456_123456_gtFine_polygons.json returns city_123456_123456 137 | def getCoreImageFileName(filename): 138 | csFile = getCsFileInfo(filename) 139 | return "{}_{}_{}".format( csFile.city , csFile.sequenceNb , csFile.frameNb ) 140 | 141 | # Returns the directory name for the given filename, e.g. 142 | # fileName = "/foo/bar/foobar.txt" 143 | # return value is "bar" 144 | # Not much error checking though 145 | def getDirectory(fileName): 146 | dirName = os.path.dirname(fileName) 147 | return os.path.basename(dirName) 148 | 149 | # Make sure that the given path exists 150 | def ensurePath(path): 151 | if not path: 152 | return 153 | if not os.path.isdir(path): 154 | os.makedirs(path) 155 | 156 | # Write a dictionary as json file 157 | def writeDict2JSON(dictName, fileName): 158 | with open(fileName, 'w') as f: 159 | f.write(json.dumps(dictName, default=lambda o: o.__dict__, sort_keys=True, indent=4)) 160 | 161 | # Convert rotation matrix to axis angle 162 | def Rodrigues(matrix): 163 | """Convert the rotation matrix into the axis-angle notation. 164 | Conversion equations 165 | ==================== 166 | From Wikipedia (http://en.wikipedia.org/wiki/Rotation_matrix), the conversion is given by:: 167 | x = Qzy-Qyz 168 | y = Qxz-Qzx 169 | z = Qyx-Qxy 170 | r = hypot(x,hypot(y,z)) 171 | t = Qxx+Qyy+Qzz 172 | theta = atan2(r,t-1) 173 | @param matrix: The 3x3 rotation matrix to update. 174 | @type matrix: 3x3 numpy array 175 | @return: The 3D rotation axis and angle. 176 | @rtype: numpy 3D rank-1 array, float 177 | """ 178 | 179 | # Axes. 180 | axis = np.zeros(3, np.float64) 181 | axis[0] = matrix[2,1] - matrix[1,2] 182 | axis[1] = matrix[0,2] - matrix[2,0] 183 | axis[2] = matrix[1,0] - matrix[0,1] 184 | 185 | # Angle. 186 | r = np.hypot(axis[0], np.hypot(axis[1], axis[2])) 187 | t = matrix[0,0] + matrix[1,1] + matrix[2,2] 188 | theta = np.arctan2(r, t-1) 189 | 190 | # Normalise the axis. 191 | axis = axis / r 192 | 193 | # Return the data. 194 | return axis * theta 195 | 196 | # dummy main 197 | if __name__ == "__main__": 198 | printError("Only for include, not executable on its own.") 199 | -------------------------------------------------------------------------------- /kitti360scripts/helpers/curlVelodyneData.pyx: -------------------------------------------------------------------------------- 1 | # cython methods to speed-up curling 2 | 3 | import numpy as np 4 | cimport cython 5 | cimport numpy as np 6 | import ctypes 7 | 8 | np.import_array() 9 | 10 | cdef extern from "curlVelodyneData_impl.c": 11 | void curl_velodyne_data (const double* velo_in, 12 | double* velo_out, 13 | const unsigned int pt_num, 14 | const double* r, 15 | const double* t) 16 | 17 | 18 | cdef tonumpyarray(double* data, unsigned long long size, unsigned long long dim): 19 | if not (data and size >= 0 and dim >= 0): 20 | raise ValueError 21 | return np.PyArray_SimpleNewFromData(2, [size, dim], np.NPY_FLOAT64, data) 22 | 23 | @cython.boundscheck(False) 24 | def cCurlVelodyneData( np.ndarray[np.float64_t , ndim=2] veloIn , 25 | np.ndarray[np.float64_t , ndim=2] veloOut , 26 | np.ndarray[np.float64_t , ndim=1] r, 27 | np.ndarray[np.float64_t , ndim=1] t): 28 | cdef np.ndarray[np.float64_t , ndim=2, mode="c"] veloIn_c 29 | cdef np.ndarray[np.float64_t , ndim=2, mode="c"] veloOut_c 30 | cdef np.ndarray[np.float64_t , ndim=1, mode="c"] r_c 31 | cdef np.ndarray[np.float64_t , ndim=1, mode="c"] t_c 32 | 33 | veloIn_c = np.ascontiguousarray(veloIn , dtype=np.float64 ) 34 | veloOut_c = np.ascontiguousarray(veloOut, dtype=np.float64 ) 35 | r_c = np.ascontiguousarray(r , dtype=np.float64 ) 36 | t_c = np.ascontiguousarray(t , dtype=np.float64 ) 37 | 38 | cdef np.uint32_t pt_num = veloIn.shape[0] 39 | cdef np.uint32_t pt_dim = veloIn.shape[1] 40 | 41 | curl_velodyne_data(&veloIn_c[0,0], &veloOut_c[0,0], pt_num, &r_c[0], &t_c[0]) 42 | 43 | veloOut = np.ascontiguousarray(tonumpyarray(&veloOut_c[0,0], pt_num, pt_dim)) 44 | 45 | return np.copy(veloOut) 46 | -------------------------------------------------------------------------------- /kitti360scripts/helpers/curlVelodyneData_impl.c: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | using namespace std; 4 | 5 | void curl_velodyne_data (const double* velo_in, 6 | double* velo_out, 7 | const int32_t pt_num, 8 | const double* r, 9 | const double* t) { 10 | 11 | // for all points do 12 | for (int32_t i=0; i1e-10) { 31 | 32 | double kx = rx/theta; 33 | double ky = ry/theta; 34 | double kz = rz/theta; 35 | 36 | double ct = cos(theta); 37 | double st = sin(theta); 38 | 39 | double kv = kx*vx+ky*vy+kz*vz; 40 | 41 | velo_out[i*4+0] = vx*ct + (ky*vz-kz*vy)*st + kx*kv*(1-ct) + tx; 42 | velo_out[i*4+1] = vy*ct + (kz*vx-kx*vz)*st + ky*kv*(1-ct) + ty; 43 | velo_out[i*4+2] = vz*ct + (kx*vy-ky*vx)*st + kz*kv*(1-ct) + tz; 44 | 45 | 46 | } else { 47 | 48 | velo_out[i*4+0] = vx + tx; 49 | velo_out[i*4+1] = vy + ty; 50 | velo_out[i*4+2] = vz + tz; 51 | 52 | } 53 | 54 | // intensity 55 | velo_out[i*4+3] = velo_in[i*4+3]; 56 | } 57 | } 58 | -------------------------------------------------------------------------------- /kitti360scripts/helpers/data.py: -------------------------------------------------------------------------------- 1 | import os 2 | import numpy as np 3 | 4 | class KITTI360(object): 5 | def __init__(self, data_dir, seq=0, cam=0): 6 | 7 | if cam!=0: 8 | raise NotImplementedError('Please generate cam%d_to_world.txt at first!') 9 | 10 | # intrinsics 11 | calib_dir = '%s/calibration' % (data_dir) 12 | self.intrinsic_file = os.path.join(calib_dir, 'perspective.txt') 13 | 14 | # camera poses 15 | sequence_dir = '%s/2013_05_28_drive_%04d_sync/' % (data_dir, seq) 16 | self.pose_file = os.path.join(sequence_dir, 'cam%d_to_world.txt' % cam) 17 | self.image_dir = '%s/image_%02d/data_rect/' % (sequence_dir, cam) 18 | 19 | assert os.path.isfile(self.pose_file), '%s does not exist!' % self.pose_file 20 | assert os.path.isfile(self.intrinsic_file), '%s does not exist!' % self.intrinsic_file 21 | 22 | print('-----------------------------------------------') 23 | print('Loading KITTI-360, sequence %04d, camera %d' % (seq, cam)) 24 | print('-----------------------------------------------') 25 | self.load_intrinsics() 26 | print('-----------------------------------------------') 27 | self.load_poses() 28 | print('-----------------------------------------------') 29 | 30 | def load_intrinsics(self): 31 | # load intrinsics 32 | intrinsic_loaded = False 33 | width = -1 34 | height = -1 35 | with open(self.intrinsic_file) as f: 36 | intrinsics = f.read().splitlines() 37 | for line in intrinsics: 38 | line = line.split(' ') 39 | if line[0] == 'P_rect_00:': 40 | K = [float(x) for x in line[1:]] 41 | K = np.reshape(K, [3,4]) 42 | intrinsic_loaded = True 43 | if line[0] == "S_rect_00:": 44 | width = int(float(line[1])) 45 | height = int(float(line[2])) 46 | assert(intrinsic_loaded==True) 47 | assert(width>0 and height>0) 48 | 49 | self.K = K 50 | self.width = width 51 | self.height = height 52 | print ('Image size %dx%d ' % (self.width, self.height)) 53 | print ('Intrinsics \n', self.K) 54 | 55 | def load_poses(self): 56 | # load poses of the current camera 57 | poses = np.loadtxt(self.pose_file) 58 | self.frames = poses[:,0].astype(np.int) 59 | self.poses = np.reshape(poses[:,1:], (-1, 4, 4)) 60 | print('Number of posed frames %d' % len(self.frames)) 61 | 62 | def __len__(self): 63 | return len(self.frames) 64 | 65 | def __getitem__(self, idx): 66 | frame = self.frames[idx] 67 | pose = self.poses[idx] 68 | basename = '%010d.png' % frame 69 | image_file = os.path.join(self.image_dir, basename) 70 | assert os.path.isfile(image_file), '%s does not exist!' % image_file 71 | print(pose) 72 | print(image_file) 73 | return 74 | 75 | 76 | 77 | if __name__=='__main__': 78 | dset = KITTI360('/is/rg/avg/datasets/KITTI360/2013_05_28/') 79 | for i in range(10): 80 | dset[i] 81 | 82 | -------------------------------------------------------------------------------- /kitti360scripts/helpers/pointcloud.py: -------------------------------------------------------------------------------- 1 | class PointCloud: 2 | 3 | def __init__(self, ptsDir, camera=0): 4 | self.ptsDir = ptsDir 5 | self.camera = camera 6 | 7 | self.w = 1408 8 | self.h = 376 9 | 10 | def loadPointCloud(self, frame): 11 | 12 | labelDir = '%s/annotation_%010d_%d_m.dat' % (self.ptsDir, frame, self.camera) 13 | print 'Processing %010d' %(f) 14 | 15 | if not (os.path.isfile(labelDir)): 16 | print ' annotation file doesnt exist' 17 | continue 18 | img = Image.new('RGB', [w, h]) 19 | imgDL = Image.new('RGB', [w, h]) 20 | 21 | labels = tuple(open(labelDir, 'r')) 22 | unarySparse = [] 23 | sparseLabel2D = [] 24 | 25 | offset = 6 26 | tupleNum = 3 27 | for l in labels: 28 | s = ([float(n) for n in l.split()]) 29 | if (s[offset] != 0): 30 | yy = int(math.floor(s[0]/w)) 31 | xx = int(s[0] % w) 32 | numCandidate = (len(s)-offset)/tupleNum 33 | # only feed for training when there is only one point 34 | candidate = [] 35 | for i in range(numCandidate): 36 | # skip the undefined label 37 | if int(s[i*tupleNum+1+offset])-1 != priority.shape[0]: 38 | candidate.append(int(s[i*tupleNum+1+offset])-1) 39 | if not candidate: 40 | continue 41 | index = random.sample(candidate, 1) 42 | if numCandidate>1: 43 | subprior = priority[np.ix_(candidate, candidate)] 44 | if np.std(subprior.flatten()) != 0: 45 | midx = np.argmax(np.sum(subprior, axis=1) ) 46 | index[0] = candidate[midx] 47 | img.putpixel((xx, yy), (colorMap[index[0]+1][0], colorMap[index[0]+1][1], colorMap[index[0]+1][2])) 48 | # take it for training only when there is no ambuity 49 | if np.std(candidate)==0: 50 | imgDL.putpixel((xx, yy), (colorMap[candidate[0]+1][0], colorMap[candidate[0]+1][1], colorMap[candidate[0]+1][2])) 51 | 52 | 53 | if __name__=='__main__': 54 | pcd = PointCloud('') 55 | -------------------------------------------------------------------------------- /kitti360scripts/helpers/project.py: -------------------------------------------------------------------------------- 1 | import os 2 | import numpy as np 3 | import re 4 | import yaml 5 | import sys 6 | from kitti360scripts.devkits.commons.loadCalibration import loadCalibrationCameraToPose 7 | 8 | def readYAMLFile(fileName): 9 | '''make OpenCV YAML file compatible with python''' 10 | ret = {} 11 | skip_lines=1 # Skip the first line which says "%YAML:1.0". Or replace it with "%YAML 1.0" 12 | with open(fileName) as fin: 13 | for i in range(skip_lines): 14 | fin.readline() 15 | yamlFileOut = fin.read() 16 | myRe = re.compile(r":([^ ])") # Add space after ":", if it doesn't exist. Python yaml requirement 17 | yamlFileOut = myRe.sub(r': \1', yamlFileOut) 18 | ret = yaml.load(yamlFileOut) 19 | return ret 20 | 21 | class Camera: 22 | def __init__(self): 23 | 24 | # load intrinsics 25 | self.load_intrinsics(self.intrinsic_file) 26 | 27 | # load poses 28 | poses = np.loadtxt(self.pose_file) 29 | frames = poses[:,0] 30 | poses = np.reshape(poses[:,1:],[-1,3,4]) 31 | self.cam2world = {} 32 | self.frames = frames 33 | for frame, pose in zip(frames, poses): 34 | pose = np.concatenate((pose, np.array([0.,0.,0.,1.]).reshape(1,4))) 35 | # consider the rectification for perspective cameras 36 | if self.cam_id==0 or self.cam_id==1: 37 | self.cam2world[frame] = np.matmul(np.matmul(pose, self.camToPose), 38 | np.linalg.inv(self.R_rect)) 39 | # fisheye cameras 40 | elif self.cam_id==2 or self.cam_id==3: 41 | self.cam2world[frame] = np.matmul(pose, self.camToPose) 42 | else: 43 | raise RuntimeError('Unknown Camera ID!') 44 | 45 | 46 | def world2cam(self, points, R, T, inverse=False): 47 | assert (points.ndim==R.ndim) 48 | assert (T.ndim==R.ndim or T.ndim==(R.ndim-1)) 49 | ndim=R.ndim 50 | if ndim==2: 51 | R = np.expand_dims(R, 0) 52 | T = np.reshape(T, [1, -1, 3]) 53 | points = np.expand_dims(points, 0) 54 | if not inverse: 55 | points = np.matmul(R, points.transpose(0,2,1)).transpose(0,2,1) + T 56 | else: 57 | points = np.matmul(R.transpose(0,2,1), (points - T).transpose(0,2,1)) 58 | 59 | if ndim==2: 60 | points = points[0] 61 | 62 | return points 63 | 64 | def cam2image(self, points): 65 | raise NotImplementedError 66 | 67 | def load_intrinsics(self, intrinsic_file): 68 | raise NotImplementedError 69 | 70 | def project_vertices(self, vertices, frameId, inverse=True): 71 | 72 | # current camera pose 73 | curr_pose = self.cam2world[frameId] 74 | T = curr_pose[:3, 3] 75 | R = curr_pose[:3, :3] 76 | 77 | # convert points from world coordinate to local coordinate 78 | points_local = self.world2cam(vertices, R, T, inverse) 79 | 80 | # perspective projection 81 | u,v,depth = self.cam2image(points_local) 82 | 83 | return (u,v), depth 84 | 85 | def __call__(self, obj3d, frameId): 86 | 87 | vertices = obj3d.vertices 88 | heading = obj3d.heading 89 | 90 | uv, depth = self.project_vertices(vertices, frameId) 91 | 92 | obj3d.vertices_proj = uv 93 | obj3d.vertices_depth = depth 94 | obj3d.generateMeshes() 95 | 96 | uv_heading, depth_heading = self.project_vertices(heading, frameId) 97 | obj3d.heading_proj = uv_heading 98 | obj3d.heading_depth = depth_heading 99 | 100 | 101 | class CameraPerspective(Camera): 102 | 103 | def __init__(self, root_dir, seq='2013_05_28_drive_0009_sync', cam_id=0): 104 | # perspective camera ids: {0,1}, fisheye camera ids: {2,3} 105 | assert (cam_id==0 or cam_id==1) 106 | 107 | pose_dir = os.path.join(root_dir, 'data_poses', seq) 108 | calib_dir = os.path.join(root_dir, 'calibration') 109 | self.pose_file = os.path.join(pose_dir, "poses.txt") 110 | self.intrinsic_file = os.path.join(calib_dir, 'perspective.txt') 111 | fileCameraToPose = os.path.join(calib_dir, 'calib_cam_to_pose.txt') 112 | self.camToPose = loadCalibrationCameraToPose(fileCameraToPose)['image_%02d' % cam_id] 113 | self.cam_id = cam_id 114 | super(CameraPerspective, self).__init__() 115 | 116 | def load_intrinsics(self, intrinsic_file): 117 | ''' load perspective intrinsics ''' 118 | 119 | intrinsic_loaded = False 120 | width = -1 121 | height = -1 122 | with open(intrinsic_file) as f: 123 | intrinsics = f.read().splitlines() 124 | for line in intrinsics: 125 | line = line.split(' ') 126 | if line[0] == 'P_rect_%02d:' % self.cam_id: 127 | K = [float(x) for x in line[1:]] 128 | K = np.reshape(K, [3,4]) 129 | intrinsic_loaded = True 130 | elif line[0] == 'R_rect_%02d:' % self.cam_id: 131 | R_rect = np.eye(4) 132 | R_rect[:3,:3] = np.array([float(x) for x in line[1:]]).reshape(3,3) 133 | elif line[0] == "S_rect_%02d:" % self.cam_id: 134 | width = int(float(line[1])) 135 | height = int(float(line[2])) 136 | assert(intrinsic_loaded==True) 137 | assert(width>0 and height>0) 138 | 139 | self.K = K 140 | self.width, self.height = width, height 141 | self.R_rect = R_rect 142 | 143 | def cam2image(self, points): 144 | ndim = points.ndim 145 | if ndim == 2: 146 | points = np.expand_dims(points, 0) 147 | points_proj = np.matmul(self.K[:3,:3].reshape([1,3,3]), points) 148 | depth = points_proj[:,2,:] 149 | depth[depth==0] = -1e-6 150 | u = np.round(points_proj[:,0,:]/np.abs(depth)).astype(np.int) 151 | v = np.round(points_proj[:,1,:]/np.abs(depth)).astype(np.int) 152 | 153 | if ndim==2: 154 | u = u[0]; v=v[0]; depth=depth[0] 155 | return u, v, depth 156 | 157 | class CameraFisheye(Camera): 158 | def __init__(self, root_dir, seq='2013_05_28_drive_0009_sync', cam_id=2): 159 | # perspective camera ids: {0,1}, fisheye camera ids: {2,3} 160 | assert (cam_id==2 or cam_id==3) 161 | 162 | pose_dir = os.path.join(root_dir, 'data_poses', seq) 163 | calib_dir = os.path.join(root_dir, 'calibration') 164 | self.pose_file = os.path.join(pose_dir, "poses.txt") 165 | self.intrinsic_file = os.path.join(calib_dir, 'image_%02d.yaml' % cam_id) 166 | fileCameraToPose = os.path.join(calib_dir, 'calib_cam_to_pose.txt') 167 | self.camToPose = loadCalibrationCameraToPose(fileCameraToPose)['image_%02d' % cam_id] 168 | self.cam_id = cam_id 169 | super(CameraFisheye, self).__init__() 170 | 171 | def load_intrinsics(self, intrinsic_file): 172 | ''' load fisheye intrinsics ''' 173 | 174 | intrinsics = readYAMLFile(intrinsic_file) 175 | 176 | self.width, self.height = intrinsics['image_width'], intrinsics['image_height'] 177 | self.fi = intrinsics 178 | 179 | def cam2image(self, points): 180 | ''' camera coordinate to image plane ''' 181 | points = points.T 182 | norm = np.linalg.norm(points, axis=1) 183 | 184 | x = points[:,0] / norm 185 | y = points[:,1] / norm 186 | z = points[:,2] / norm 187 | 188 | x /= z+self.fi['mirror_parameters']['xi'] 189 | y /= z+self.fi['mirror_parameters']['xi'] 190 | 191 | k1 = self.fi['distortion_parameters']['k1'] 192 | k2 = self.fi['distortion_parameters']['k2'] 193 | gamma1 = self.fi['projection_parameters']['gamma1'] 194 | gamma2 = self.fi['projection_parameters']['gamma2'] 195 | u0 = self.fi['projection_parameters']['u0'] 196 | v0 = self.fi['projection_parameters']['v0'] 197 | 198 | ro2 = x*x + y*y 199 | x *= 1 + k1*ro2 + k2*ro2*ro2 200 | y *= 1 + k1*ro2 + k2*ro2*ro2 201 | 202 | x = gamma1*x + u0 203 | y = gamma2*y + v0 204 | 205 | return x, y, norm * points[:,2] / np.abs(points[:,2]) 206 | 207 | if __name__=="__main__": 208 | import cv2 209 | import matplotlib.pyplot as plt 210 | from labels import id2label 211 | 212 | if 'KITTI360_DATASET' in os.environ: 213 | kitti360Path = os.environ['KITTI360_DATASET'] 214 | else: 215 | kitti360Path = os.path.join(os.path.dirname( 216 | os.path.realpath(__file__)), '..', '..') 217 | 218 | seq = 3 219 | cam_id = 2 220 | sequence = '2013_05_28_drive_%04d_sync'%seq 221 | # perspective 222 | if cam_id == 0 or cam_id == 1: 223 | camera = CameraPerspective(kitti360Path, sequence, cam_id) 224 | # fisheye 225 | elif cam_id == 2 or cam_id == 3: 226 | camera = CameraFisheye(kitti360Path, sequence, cam_id) 227 | print(camera.fi) 228 | else: 229 | raise RuntimeError('Invalid Camera ID!') 230 | 231 | # loop over frames 232 | for frame in camera.frames: 233 | # perspective 234 | if cam_id == 0 or cam_id == 1: 235 | image_file = os.path.join(kitti360Path, 'data_2d_raw', sequence, 'image_%02d' % cam_id, 'data_rect', '%010d.png'%frame) 236 | # fisheye 237 | elif cam_id == 2 or cam_id == 3: 238 | image_file = os.path.join(kitti360Path, 'data_2d_raw', sequence, 'image_%02d' % cam_id, 'data_rgb', '%010d.png'%frame) 239 | else: 240 | raise RuntimeError('Invalid Camera ID!') 241 | if not os.path.isfile(image_file): 242 | print('Missing %s ...' % image_file) 243 | continue 244 | 245 | 246 | print(image_file) 247 | image = cv2.imread(image_file) 248 | plt.imshow(image[:,:,::-1]) 249 | 250 | # 3D bbox 251 | from annotation import Annotation3D 252 | label3DBboxPath = os.path.join(kitti360Path, 'data_3d_bboxes') 253 | annotation3D = Annotation3D(label3DBboxPath, sequence) 254 | 255 | points = [] 256 | depths = [] 257 | for k,v in annotation3D.objects.items(): 258 | if len(v.keys())==1 and (-1 in v.keys()): # show static only 259 | obj3d = v[-1] 260 | if not id2label[obj3d.semanticId].name=='building': # show buildings only 261 | continue 262 | camera(obj3d, frame) 263 | vertices = np.asarray(obj3d.vertices_proj).T 264 | points.append(np.asarray(obj3d.vertices_proj).T) 265 | depths.append(np.asarray(obj3d.vertices_depth)) 266 | for line in obj3d.lines: 267 | v = [obj3d.vertices[line[0]]*x + obj3d.vertices[line[1]]*(1-x) for x in np.arange(0,1,0.01)] 268 | uv, d = camera.project_vertices(np.asarray(v), frame) 269 | mask = np.logical_and(np.logical_and(d>0, uv[0]>0), uv[1]>0) 270 | mask = np.logical_and(np.logical_and(mask, uv[0] 2 | 3 | 4 | 22 | 24 | 32 | 38 | 39 | 40 | 62 | 67 | 68 | 70 | 71 | 73 | image/svg+xml 74 | 76 | 77 | 78 | 79 | 80 | 85 | 91 | 94 | 100 | 106 | 112 | 113 | 116 | 122 | 128 | 134 | 135 | 136 | 137 | -------------------------------------------------------------------------------- /kitti360scripts/viewer/icons/zoom.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/autonomousvision/kitti360Scripts/40ee015de8277b2d907d32a30cdbd97072c7d7c3/kitti360scripts/viewer/icons/zoom.png -------------------------------------------------------------------------------- /kitti360scripts/viewer/kitti360Viewer3DRaw.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | # -*- coding: utf-8 -*- 3 | 4 | 5 | ################# 6 | ## Import modules 7 | ################# 8 | import sys 9 | # walk directories 10 | import glob 11 | # access to OS functionality 12 | import os 13 | # copy things 14 | import copy 15 | # numpy 16 | import numpy as np 17 | # matplotlib for colormaps 18 | import matplotlib.cm 19 | import matplotlib.pyplot as plt 20 | from mpl_toolkits.mplot3d import Axes3D 21 | # struct for reading binary ply files 22 | import struct 23 | from kitti360scripts.helpers.csHelpers import Rodrigues 24 | from kitti360scripts.devkits.commons.loadCalibration import loadCalibrationCameraToPose, loadCalibrationRigid 25 | 26 | CSUPPORT = True 27 | # Check if C-Support is available for better performance 28 | if CSUPPORT: 29 | try: 30 | from kitti360scripts.helpers import curlVelodyneData 31 | except: 32 | CSUPPORT = False 33 | print('CSUPPORT is required for unwrapping the velodyne data!') 34 | print('Run ``CYTHONIZE_EVAL= python setup.py build_ext --inplace`` to build with cython') 35 | sys.exit(-1) 36 | 37 | 38 | # the main class that loads raw 3D scans 39 | class Kitti360Viewer3DRaw(object): 40 | 41 | # Constructor 42 | def __init__(self, seq=0, mode='velodyne'): 43 | 44 | if 'KITTI360_DATASET' in os.environ: 45 | kitti360Path = os.environ['KITTI360_DATASET'] 46 | else: 47 | kitti360Path = os.path.join(os.path.dirname( 48 | os.path.realpath(__file__)), '..', '..') 49 | 50 | if mode=='velodyne': 51 | self.sensor_dir='velodyne_points' 52 | elif mode=='sick': 53 | self.sensor_dir='sick_points' 54 | else: 55 | raise RuntimeError('Unknown sensor type!') 56 | 57 | sequence = '2013_05_28_drive_%04d_sync' % seq 58 | self.raw3DPcdPath = os.path.join(kitti360Path, 'data_3d_raw', sequence, self.sensor_dir, 'data') 59 | 60 | self.kitti360Path = kitti360Path 61 | self.sequence = sequence 62 | self.loadPoses() 63 | self.loadExtrinsics() 64 | 65 | # poses are required to unwrap velodyne points to compensate for ego-motion 66 | def loadPoses(self): 67 | # load poses 68 | filePoses = os.path.join(self.kitti360Path, 'data_poses', self.sequence, 'poses.txt') 69 | poses = np.loadtxt(filePoses) 70 | frames = poses[:,0] 71 | poses = np.reshape(poses[:,1:],[-1,3,4]) 72 | self.Tr_pose_world = {} 73 | self.frames = frames 74 | for frame, pose in zip(frames, poses): 75 | pose = np.concatenate((pose, np.array([0.,0.,0.,1.]).reshape(1,4))) 76 | self.Tr_pose_world[frame] = pose 77 | 78 | def loadExtrinsics(self): 79 | # cam_0 to velo 80 | fileCameraToVelo = os.path.join(self.kitti360Path, 'calibration', 'calib_cam_to_velo.txt') 81 | TrCam0ToVelo = loadCalibrationRigid(fileCameraToVelo) 82 | 83 | # all cameras to system center 84 | fileCameraToPose = os.path.join(self.kitti360Path, 'calibration', 'calib_cam_to_pose.txt') 85 | TrCamToPose = loadCalibrationCameraToPose(fileCameraToPose) 86 | 87 | self.TrVeloToPose = TrCamToPose['image_00'] @ np.linalg.inv(TrCam0ToVelo) 88 | 89 | # velodyne to all cameras 90 | self.TrVeloToCam = {} 91 | for k, v in TrCamToPose.items(): 92 | # Tr(cam_k -> velo) = Tr(cam_k -> cam_0) @ Tr(cam_0 -> velo) 93 | TrCamkToCam0 = np.linalg.inv(TrCamToPose['image_00']) @ TrCamToPose[k] 94 | TrCamToVelo = TrCam0ToVelo @ TrCamkToCam0 95 | # Tr(velo -> cam_k) 96 | self.TrVeloToCam[k] = np.linalg.inv(TrCamToVelo) 97 | 98 | def loadVelodyneData(self, frame=0): 99 | pcdFile = os.path.join(self.raw3DPcdPath, '%010d.bin' % frame) 100 | if not os.path.isfile(pcdFile): 101 | raise RuntimeError('%s does not exist!' % pcdFile) 102 | pcd = np.fromfile(pcdFile, dtype=np.float32) 103 | pcd = np.reshape(pcd,[-1,4]) 104 | return pcd 105 | 106 | def loadSickData(self, frame=0): 107 | pcdFile = os.path.join(self.raw3DPcdPath, '%010d.bin' % frame) 108 | if not os.path.isfile(pcdFile): 109 | raise RuntimeError('%s does not exist!' % pcdFile) 110 | pcd = np.fromfile(pcdFile, dtype=np.float32) 111 | pcd = np.reshape(pcd,[-1,2]) 112 | pcd = np.concatenate([np.zeros_like(pcd[:,0:1]), -pcd[:,0:1], pcd[:,1:2]], axis=1) 113 | return pcd 114 | 115 | def curlParameterFromPoses(self, frame): 116 | Tr_pose_pose = np.eye(4) 117 | 118 | if frame in self.Tr_pose_world.keys(): 119 | if frame==1: 120 | if frame+1 in self.Tr_pose_world.keys(): 121 | Tr_pose_pose = np.linalg.inv(self.Tr_pose_world[frame+1]) @ self.Tr_pose_world[frame] 122 | else: 123 | if frame-1 in self.Tr_pose_world.keys(): 124 | Tr_pose_pose = np.linalg.inv(self.Tr_pose_world[frame]) @ self.Tr_pose_world[frame-1] 125 | Tr_delta = np.linalg.inv(self.TrVeloToPose) @ Tr_pose_pose @ self.TrVeloToPose 126 | 127 | r = Rodrigues(Tr_delta[0:3,0:3]) 128 | t = Tr_delta[0:3,3] 129 | return r.flatten(),t 130 | 131 | 132 | def curlVelodyneData(self, frame, pcd): 133 | pcd=pcd.astype(np.float64) 134 | pcd_curled = np.copy(pcd) 135 | # get curl parameters 136 | r,t = self.curlParameterFromPoses(frame) 137 | # unwrap points to compensate for ego motion 138 | pcd_curled = curlVelodyneData.cCurlVelodyneData(pcd, pcd_curled, r, t) 139 | return pcd_curled.astype(np.float32) 140 | 141 | 142 | def projectVeloToImage(cam_id=0, seq=0): 143 | from kitti360scripts.helpers.project import CameraPerspective, CameraFisheye 144 | from PIL import Image 145 | import matplotlib.pyplot as plt 146 | 147 | if 'KITTI360_DATASET' in os.environ: 148 | kitti360Path = os.environ['KITTI360_DATASET'] 149 | else: 150 | kitti360Path = os.path.join(os.path.dirname( 151 | os.path.realpath(__file__)), '..', '..') 152 | 153 | sequence = '2013_05_28_drive_%04d_sync'%seq 154 | 155 | # perspective camera 156 | if cam_id in [0,1]: 157 | camera = CameraPerspective(kitti360Path, sequence, cam_id) 158 | # fisheye camera 159 | elif cam_id in [2,3]: 160 | camera = CameraFisheye(kitti360Path, sequence, cam_id) 161 | else: 162 | raise RuntimeError('Unknown camera ID!') 163 | 164 | # object for parsing 3d raw data 165 | velo = Kitti360Viewer3DRaw(mode='velodyne', seq=seq) 166 | 167 | # take the rectification into account for perspective cameras 168 | if cam_id==0 or cam_id == 1: 169 | TrVeloToRect = np.matmul(camera.R_rect, velo.TrVeloToCam['image_%02d' % cam_id]) 170 | else: 171 | TrVeloToRect = velo.TrVeloToCam['image_%02d' % cam_id] 172 | 173 | # color map for visualizing depth map 174 | cm = plt.get_cmap('jet') 175 | 176 | # visualize a set of frame 177 | # for each frame, load the raw 3D scan and project to image plane 178 | for frame in range(0, 1000, 50): 179 | points = velo.loadVelodyneData(frame) 180 | # curl velodyne 181 | points = velo.curlVelodyneData(frame, points) 182 | 183 | points[:,3] = 1 184 | 185 | pointsCam = np.matmul(TrVeloToRect, points.T).T 186 | pointsCam = pointsCam[:,:3] 187 | # project to image space 188 | u,v, depth= camera.cam2image(pointsCam.T) 189 | u = u.astype(np.int) 190 | v = v.astype(np.int) 191 | 192 | # prepare depth map for visualization 193 | depthMap = np.zeros((camera.height, camera.width)) 194 | depthImage = np.zeros((camera.height, camera.width, 3)) 195 | mask = np.logical_and(np.logical_and(np.logical_and(u>=0, u=0), v0), depth<30) 198 | depthMap[v[mask],u[mask]] = depth[mask] 199 | layout = (2,1) if cam_id in [0,1] else (1,2) 200 | sub_dir = 'data_rect' if cam_id in [0,1] else 'data_rgb' 201 | fig, axs = plt.subplots(*layout, figsize=(18,12)) 202 | 203 | # load RGB image for visualization 204 | imagePath = os.path.join(kitti360Path, 'data_2d_raw', sequence, 'image_%02d' % cam_id, sub_dir, '%010d.png' % frame) 205 | if not os.path.isfile(imagePath): 206 | raise RuntimeError('Image file %s does not exist!' % imagePath) 207 | 208 | colorImage = np.array(Image.open(imagePath)) / 255. 209 | depthImage = cm(depthMap/depthMap.max())[...,:3] 210 | colorImage[depthMap>0] = depthImage[depthMap>0] 211 | 212 | axs[0].imshow(depthMap, cmap='jet', interpolation='none') 213 | axs[0].title.set_text('Projected Depth') 214 | axs[0].axis('off') 215 | axs[1].imshow(colorImage) 216 | axs[1].title.set_text('Projected Depth Overlaid on Image') 217 | axs[1].axis('off') 218 | plt.suptitle('Sequence %04d, Camera %02d, Frame %010d' % (seq, cam_id, frame)) 219 | plt.show() 220 | 221 | if __name__=='__main__': 222 | 223 | visualizeIn2D = True 224 | # sequence index 225 | seq = 2 226 | # set it to 0 or 1 for projection to perspective images 227 | # 2 or 3 for projecting to fisheye images 228 | cam_id = 3 229 | 230 | # visualize raw 3D velodyne scans in 2D 231 | if visualizeIn2D: 232 | projectVeloToImage(seq=seq, cam_id=cam_id) 233 | 234 | # visualize raw 3D scans in 3D 235 | else: 236 | # open3d 237 | try: 238 | import open3d 239 | except: 240 | raise ValueError("Open3d required to visualize raw 3D scans in 3D!") 241 | mode = 'sick' 242 | frame = 1000 243 | 244 | v = Kitti360Viewer3DRaw(mode=mode) 245 | if mode=='velodyne': 246 | points = v.loadVelodyneData(frame) 247 | elif mode=='sick': 248 | points = v.loadSickData(frame) 249 | pcd = open3d.geometry.PointCloud() 250 | pcd.points = open3d.utility.Vector3dVector(points[:,:3]) 251 | 252 | open3d.visualization.draw_geometries([pcd]) 253 | 254 | -------------------------------------------------------------------------------- /setup.cfg: -------------------------------------------------------------------------------- 1 | [metadata] 2 | description-file = README.md 3 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # 3 | # Enable cython support for slightly faster eval scripts: 4 | # python -m pip install cython numpy 5 | # CYTHONIZE_EVAL= python setup.py build_ext --inplace 6 | # 7 | # For MacOS X you may have to export the numpy headers in CFLAGS 8 | # export CFLAGS="-I /usr/local/lib/python3.6/site-packages/numpy/core/include $CFLAGS" 9 | 10 | import os 11 | from setuptools import setup, find_packages 12 | 13 | include_dirs = [] 14 | ext_modules = [] 15 | if 'CYTHONIZE_EVAL' in os.environ: 16 | from Cython.Build import cythonize 17 | import numpy as np 18 | include_dirs = [np.get_include()] 19 | 20 | os.environ["CC"] = "g++" 21 | os.environ["CXX"] = "g++" 22 | 23 | pyxFiles = [os.path.join("kitti360scripts", "evaluation", "addToConfusionMatrix.pyx"), 24 | os.path.join("kitti360scripts", "helpers", "curlVelodyneData.pyx")] 25 | ext_modules = cythonize(pyxFiles) 26 | 27 | with open("README.md") as f: 28 | readme = f.read() 29 | 30 | config = { 31 | 'name': 'kitti360Scripts', 32 | 'description': 'Scripts for the KITTI-360 Dataset', 33 | 'long_description': readme, 34 | 'long_description_content_type': "text/markdown", 35 | 'author': 'Yiyi Liao', 36 | 'url': 'https://github.com/autonomousvision/kitti360Scripts', 37 | 'license': 'https://github.com/autonomousvision/kitti360Scripts/blob/master/license.txt', 38 | 'version': '1.0.0', 39 | 'install_requires': ['numpy', 'matplotlib', 'pillow', 'pyyaml', 'scikit-image'], 40 | 'setup_requires': ['setuptools>=18.0'], 41 | 'packages': find_packages(), 42 | 'scripts': [], 43 | 'entry_points': {'gui_scripts': ['kitti360Viewer = kitti360scripts.viewer.kitti360Viewer:main']}, 44 | 'package_data': {'': ['icons/*.png']}, 45 | 'ext_modules': ext_modules, 46 | 'include_dirs': include_dirs 47 | } 48 | 49 | setup(**config) 50 | --------------------------------------------------------------------------------