├── .gitignore ├── CMakeLists.txt ├── HOWTO_RUN_CPP.md ├── HOWTO_RUN_PYTHON.md ├── LICENSE ├── README.md ├── data ├── KITTI07 │ ├── calib.txt │ ├── image_0 │ │ ├── 000000.png │ │ ├── 000001.png │ │ ├── 000002.png │ │ ├── 000003.png │ │ ├── 000004.png │ │ ├── 000005.png │ │ ├── 000006.png │ │ ├── 000007.png │ │ ├── 000008.png │ │ ├── 000009.png │ │ ├── 000010.png │ │ ├── 000011.png │ │ ├── 000012.png │ │ ├── 000013.png │ │ ├── 000014.png │ │ ├── 000015.png │ │ ├── 000016.png │ │ ├── 000017.png │ │ ├── 000018.png │ │ ├── 000019.png │ │ ├── 000020.png │ │ ├── 000021.png │ │ ├── 000022.png │ │ ├── 000023.png │ │ ├── 000024.png │ │ ├── 000025.png │ │ ├── 000026.png │ │ ├── 000027.png │ │ ├── 000028.png │ │ ├── 000029.png │ │ ├── 000030.png │ │ ├── 000031.png │ │ ├── 000032.png │ │ ├── 000033.png │ │ ├── 000034.png │ │ ├── 000035.png │ │ ├── 000036.png │ │ ├── 000037.png │ │ ├── 000038.png │ │ ├── 000039.png │ │ ├── 000040.png │ │ ├── 000041.png │ │ ├── 000042.png │ │ ├── 000043.png │ │ ├── 000044.png │ │ ├── 000045.png │ │ ├── 000046.png │ │ ├── 000047.png │ │ ├── 000048.png │ │ ├── 000049.png │ │ ├── 000050.png │ │ ├── 000051.png │ │ ├── 000052.png │ │ ├── 000053.png │ │ ├── 000054.png │ │ ├── 000055.png │ │ ├── 000056.png │ │ ├── 000057.png │ │ ├── 000058.png │ │ ├── 000059.png │ │ ├── 000060.png │ │ ├── 000061.png │ │ ├── 000062.png │ │ ├── 000063.png │ │ ├── 000064.png │ │ ├── 000065.png │ │ ├── 000066.png │ │ ├── 000067.png │ │ ├── 000068.png │ │ ├── 000069.png │ │ ├── 000070.png │ │ ├── 000071.png │ │ ├── 000072.png │ │ ├── 000073.png │ │ ├── 000074.png │ │ ├── 000075.png │ │ ├── 000076.png │ │ ├── 000077.png │ │ ├── 000078.png │ │ ├── 000079.png │ │ ├── 000080.png │ │ ├── 000081.png │ │ ├── 000082.png │ │ ├── 000083.png │ │ ├── 000084.png │ │ ├── 000085.png │ │ ├── 000086.png │ │ ├── 000087.png │ │ ├── 000088.png │ │ ├── 000089.png │ │ ├── 000090.png │ │ ├── 000091.png │ │ ├── 000092.png │ │ ├── 000093.png │ │ ├── 000094.png │ │ ├── 000095.png │ │ ├── 000096.png │ │ ├── 000097.png │ │ ├── 000098.png │ │ ├── 000099.png │ │ ├── 000100.png │ │ ├── 000101.png │ │ ├── 000102.png │ │ ├── 000103.png │ │ ├── 000104.png │ │ ├── 000105.png │ │ ├── 000106.png │ │ ├── 000107.png │ │ ├── 000108.png │ │ ├── 000109.png │ │ ├── 000110.png │ │ ├── 000111.png │ │ ├── 000112.png │ │ ├── 000113.png │ │ ├── 000114.png │ │ ├── 000115.png │ │ ├── 000116.png │ │ ├── 000117.png │ │ ├── 000118.png │ │ ├── 000119.png │ │ ├── 000120.png │ │ ├── 000121.png │ │ ├── 000122.png │ │ ├── 000123.png │ │ ├── 000124.png │ │ ├── 000125.png │ │ ├── 000126.png │ │ ├── 000127.png │ │ ├── 000128.png │ │ ├── 000129.png │ │ ├── 000130.png │ │ ├── 000131.png │ │ ├── 000132.png │ │ ├── 000133.png │ │ ├── 000134.png │ │ ├── 000135.png │ │ ├── 000136.png │ │ ├── 000137.png │ │ ├── 000138.png │ │ ├── 000139.png │ │ ├── 000140.png │ │ ├── 000141.png │ │ ├── 000142.png │ │ ├── 000143.png │ │ ├── 000144.png │ │ ├── 000145.png │ │ ├── 000146.png │ │ ├── 000147.png │ │ ├── 000148.png │ │ ├── 000149.png │ │ ├── 000150.png │ │ ├── 000151.png │ │ ├── 000152.png │ │ ├── 000153.png │ │ ├── 000154.png │ │ ├── 000155.png │ │ ├── 000156.png │ │ ├── 000157.png │ │ ├── 000158.png │ │ └── 000159.png │ └── times.txt ├── blais.jpg ├── blais.mp4 ├── box.xyz ├── chessboard.avi ├── daejeon_station.png ├── hill01.jpg ├── hill02.jpg ├── image_formation0.xyz ├── image_formation1.xyz ├── image_formation2.xyz ├── image_formation3.xyz ├── image_formation4.xyz ├── relief │ ├── 00.jpg │ ├── 01.jpg │ ├── 02.jpg │ ├── 03.jpg │ └── 04.jpg ├── sunglok_card.jpg └── traffic.avi ├── examples ├── 3d_rotation_conversion.py ├── affine_estimation_implement.py ├── bundle_adjustment.cpp ├── bundle_adjustment.hpp ├── bundle_adjustment.py ├── camera_calibration.cpp ├── camera_calibration.py ├── camera_calibration_implement.py ├── distortion_correction.cpp ├── distortion_correction.py ├── distortion_visualization.py ├── epipolar_line_visualization.py ├── feature_matching.py ├── feature_tracking_klt.py ├── fundamental_mat_estimation.py ├── fundamental_mat_estimation_implement.py ├── harris_corner_implement.py ├── homography_estimation_implement.py ├── image_formation.cpp ├── image_formation.py ├── image_stitching.cpp ├── image_stitching.py ├── image_stitching_implement.py ├── image_warping_implement.py ├── line_fitting_m_estimator.cpp ├── line_fitting_m_estimator.py ├── line_fitting_ransac.cpp ├── line_fitting_ransac.py ├── object_localization.cpp ├── object_localization.py ├── perspective_correction.cpp ├── perspective_correction.py ├── pose_estimation_book1.cpp ├── pose_estimation_book1.py ├── pose_estimation_book2.cpp ├── pose_estimation_book2.py ├── pose_estimation_book3.cpp ├── pose_estimation_book3.py ├── pose_estimation_chessboard.cpp ├── pose_estimation_chessboard.py ├── pose_estimation_implement.py ├── sfm.hpp ├── sfm_global.cpp ├── sfm_global.py ├── sfm_inc.cpp ├── sfm_inc.py ├── triangulation.cpp ├── triangulation.py ├── triangulation_implement.py ├── video_stabilization.cpp ├── video_stabilization.py ├── vo_epipolar.cpp └── vo_epipolar.py ├── requirements.txt └── slides ├── 01_introduction.pdf ├── 02_single-view_geometry.pdf ├── 03_two-view_geometry.pdf ├── 04_solving_problems.pdf ├── 05_correspondence.pdf ├── 06_multi-view_geometry.pdf └── 07_visual_slam.pdf /.gitignore: -------------------------------------------------------------------------------- 1 | data/KITTI07/image_0 2 | slides 3 | 4 | __pycache__ 5 | build 6 | x64 7 | Debug 8 | Release 9 | *.exe 10 | *.dll 11 | *.obj 12 | *.iobj 13 | *.pdb 14 | *.ipdb -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(3dv_tutorial) 3 | 4 | # Find external packages and specify their locations 5 | find_package(OpenCV REQUIRED) 6 | find_package(Ceres REQUIRED) 7 | 8 | # Specify project files and locations 9 | set(EXAMPLE_DIR "${CMAKE_SOURCE_DIR}/examples") 10 | file(GLOB EXAMPLE_FILES ${EXAMPLE_DIR}/*.cpp) 11 | 12 | # Set common configuration 13 | include_directories( 14 | ${OpenCV_INCLUDE_DIRS} 15 | ${Ceres_INCLUDE_DIRS} 16 | ) 17 | link_directories( 18 | ${OpenCV_LIB_DIRS} 19 | ${Ceres_LIB_DIRS} 20 | ) 21 | link_libraries( 22 | ${OpenCV_LIBS} 23 | ${CERES_LIBRARIES} 24 | ) 25 | add_compile_definitions(GLOG_NO_ABBREVIATED_SEVERITIES) 26 | 27 | # Add an executable 28 | foreach(example_file ${EXAMPLE_FILES}) 29 | string(REPLACE ".cpp" "" example_name ${example_file}) 30 | string(REPLACE "${EXAMPLE_DIR}/" "" example_name ${example_name}) 31 | add_executable(${example_name} ${example_file}) 32 | endforeach() -------------------------------------------------------------------------------- /HOWTO_RUN_CPP.md: -------------------------------------------------------------------------------- 1 | ## How to Run Example Codes with Microsoft Visual Studio 2 | ### Prerequisites 3 | * Install [Microsoft Visual Studio](https://www.visualstudio.com/) (shortly MSVS) 4 | * :memo: Note) Microsoft Visual Studio **Community** is _free_ for students, open-source contributors, and individual developers. 5 | * Install [Git](https://git-scm.com/) and [CMake](https://cmake.org/) 6 | * You need to [add their directory to PATH environment in Windows](https://stackoverflow.com/questions/44272416/how-to-add-a-folder-to-path-environment-variable-in-windows-10-with-screensho). 7 | * Install [vcpkg](https://vcpkg.io/) ([more details](https://vcpkg.io/en/getting-started)) 8 | * You need to move your working directory where you want to install _vcpkg_ (I assume your working directory as `C:/`). 9 | ```bash 10 | git clone https://github.com/Microsoft/vcpkg.git 11 | .\vcpkg\bootstrap-vcpkg.bat 12 | ``` 13 | * Install [OpenCV](https://opencv.org/) and [Ceres Solver](http://ceres-solver.org/) using _vcpkg_ 14 | ```bash 15 | cd vcpkg 16 | vcpkg install opencv[world,contrib]:x64-windows --recurse 17 | vcpkg install ceres[suitesparse,cxsparse,eigensparse,tools]:x64-windows 18 | vcpkg integrate install 19 | ``` 20 | 21 | ### Compiling and Running Examples 22 | 1. Clone the repository: `git clone https://github.com/mint-lab/3dv_tutorial.git` 23 | * You need to move your working directory where you want to copy the repository. 24 | * Or you can download [example codes and slides as a ZIP file](https://github.com/sunglok/3dv_tutorial/archive/master.zip) and unzip it where you want. 25 | 1. Generate MSVS solution and project files 26 | * You need to specify the _vcpkg_ directory where you installed it. (I assume you install it at `C:/`) 27 | ```bash 28 | cd 3dv_tutorial 29 | mkdir build 30 | cd build 31 | cmake .. "-DCMAKE_TOOLCHAIN_FILE=C:/vcpkg/scripts/buildsystems/vcpkg.cmake" 32 | ``` 33 | 1. Run your MSVS and open the generated solution file, `build/3dv_tutorial.sln`. 34 | 1. Build the example codes in the solution (Menu > `Build` > `Build Solution`) 35 | 1. Run the examples using `F5` after specify your target as the startup project (Project Context Menu > `Set as Startup Project`) 36 | 37 | 38 | 39 | ## How to Run Example Codes in Linux 40 | ### Prerequisites 41 | * Install [GCC](https://gcc.gnu.org/), [Git](https://git-scm.com/), [CMake](https://cmake.org/), [OpenCV](https://opencv.org/), and [Ceres Solver](http://ceres-solver.org/) 42 | 43 | ### Running Examples 44 | 1. Clone the repository: `git clone https://github.com/mint-lab/3dv_tutorial.git` 45 | * You need to move your working directory where you want to copy the repository. 46 | * Or you can download [example codes and slides as a ZIP file](https://github.com/sunglok/3dv_tutorial/archive/master.zip) and unzip it where you want. 47 | 1. Generate `Makefile` and project files 48 | ```bash 49 | cd 3dv_tutorial 50 | mkdir build 51 | cd build 52 | cmake .. 53 | make install 54 | ``` 55 | 1. Run the examples -------------------------------------------------------------------------------- /HOWTO_RUN_PYTHON.md: -------------------------------------------------------------------------------- 1 | ## How to Run Python Example Codes 2 | 3 | ### Prerequisites 4 | * Install [Python](https://www.python.org/) (or [Anaconda](https://www.anaconda.com/)) 5 | * Install the following required packages: `pip install -r requirements.txt` 6 | ``` 7 | numpy 8 | matplotlib 9 | scipy 10 | opencv-python 11 | opencv-contrib-python 12 | ``` 13 | * Clone the repository: `git clone https://github.com/mint-lab/3dv_tutorial.git` 14 | * You need to move your working directory where you want to copy the repository. 15 | * Or you can download [example codes and slides as a ZIP file](https://github.com/sunglok/3dv_tutorial/archive/master.zip) and unzip it where you want. 16 | 17 | 18 | ### Running Examples in Console 19 | 1. Move to the directory where example codes exist: `cd examples` 20 | 2. Run an example (e.g. `image_formation.py`): `python image_formation.py` 21 | * Or you can run an example on your favorite Python IDE or editor. -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | /* 2 | * ---------------------------------------------------------------------------- 3 | * "THE BEER-WARE LICENSE" (Revision 42): 4 | * wrote this file. As long as you retain this notice you 5 | * can do whatever you want with this stuff. If we meet some day, and you think 6 | * this stuff is worth it, you can buy me a beer in return. Sunglok Choi 7 | * ---------------------------------------------------------------------------- 8 | */ 9 | -------------------------------------------------------------------------------- /data/KITTI07/calib.txt: -------------------------------------------------------------------------------- 1 | P0: 7.070912000000e+02 0.000000000000e+00 6.018873000000e+02 0.000000000000e+00 0.000000000000e+00 7.070912000000e+02 1.831104000000e+02 0.000000000000e+00 0.000000000000e+00 0.000000000000e+00 1.000000000000e+00 0.000000000000e+00 2 | P1: 7.070912000000e+02 0.000000000000e+00 6.018873000000e+02 -3.798145000000e+02 0.000000000000e+00 7.070912000000e+02 1.831104000000e+02 0.000000000000e+00 0.000000000000e+00 0.000000000000e+00 1.000000000000e+00 0.000000000000e+00 3 | P2: 7.070912000000e+02 0.000000000000e+00 6.018873000000e+02 4.688783000000e+01 0.000000000000e+00 7.070912000000e+02 1.831104000000e+02 1.178601000000e-01 0.000000000000e+00 0.000000000000e+00 1.000000000000e+00 6.203223000000e-03 4 | P3: 7.070912000000e+02 0.000000000000e+00 6.018873000000e+02 -3.334597000000e+02 0.000000000000e+00 7.070912000000e+02 1.831104000000e+02 1.930130000000e+00 0.000000000000e+00 0.000000000000e+00 1.000000000000e+00 3.318498000000e-03 5 | -------------------------------------------------------------------------------- /data/KITTI07/image_0/000000.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000000.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000001.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000001.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000002.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000002.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000003.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000003.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000004.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000004.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000005.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000005.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000006.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000006.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000007.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000007.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000008.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000008.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000009.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000009.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000010.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000010.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000011.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000011.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000012.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000012.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000013.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000013.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000014.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000014.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000015.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000015.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000016.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000016.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000017.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000017.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000018.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000018.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000019.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000019.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000020.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000020.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000021.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000021.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000022.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000022.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000023.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000023.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000024.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000024.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000025.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000025.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000026.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000026.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000027.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000027.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000028.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000028.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000029.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000029.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000030.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000030.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000031.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000031.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000032.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000032.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000033.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000033.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000034.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000034.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000035.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000035.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000036.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000036.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000037.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000037.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000038.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000038.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000039.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000039.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000040.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000040.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000041.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000041.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000042.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000042.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000043.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000043.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000044.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000044.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000045.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000045.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000046.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000046.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000047.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000047.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000048.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000048.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000049.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000049.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000050.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000050.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000051.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000051.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000052.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000052.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000053.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000053.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000054.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000054.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000055.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000055.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000056.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000056.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000057.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000057.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000058.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000058.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000059.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000059.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000060.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000060.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000061.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000061.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000062.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000062.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000063.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000063.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000064.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000064.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000065.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000065.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000066.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000066.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000067.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000067.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000068.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000068.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000069.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000069.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000070.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000070.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000071.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000071.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000072.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000072.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000073.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000073.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000074.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000074.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000075.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000075.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000076.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000076.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000077.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000077.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000078.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000078.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000079.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000079.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000080.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000080.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000081.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000081.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000082.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000082.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000083.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000083.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000084.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000084.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000085.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000085.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000086.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000086.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000087.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000087.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000088.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000088.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000089.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000089.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000090.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000090.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000091.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000091.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000092.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000092.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000093.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000093.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000094.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000094.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000095.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000095.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000096.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000096.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000097.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000097.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000098.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000098.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000099.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000099.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000100.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000100.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000101.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000101.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000102.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000102.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000103.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000103.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000104.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000104.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000105.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000105.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000106.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000106.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000107.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000107.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000108.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000108.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000109.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000109.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000110.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000110.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000111.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000111.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000112.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000112.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000113.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000113.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000114.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000114.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000115.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000115.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000116.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000116.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000117.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000117.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000118.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000118.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000119.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000119.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000120.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000120.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000121.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000121.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000122.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000122.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000123.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000123.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000124.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000124.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000125.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000125.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000126.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000126.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000127.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000127.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000128.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000128.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000129.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000129.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000130.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000130.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000131.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000131.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000132.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000132.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000133.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000133.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000134.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000134.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000135.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000135.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000136.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000136.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000137.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000137.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000138.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000138.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000139.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000139.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000140.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000140.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000141.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000141.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000142.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000142.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000143.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000143.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000144.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000144.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000145.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000145.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000146.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000146.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000147.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000147.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000148.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000148.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000149.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000149.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000150.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000150.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000151.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000151.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000152.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000152.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000153.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000153.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000154.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000154.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000155.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000155.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000156.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000156.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000157.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000157.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000158.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000158.png -------------------------------------------------------------------------------- /data/KITTI07/image_0/000159.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/KITTI07/image_0/000159.png -------------------------------------------------------------------------------- /data/blais.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/blais.jpg -------------------------------------------------------------------------------- /data/blais.mp4: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/blais.mp4 -------------------------------------------------------------------------------- /data/box.xyz: -------------------------------------------------------------------------------- 1 | -1.0 0.5 5 2 | -0.9 0.5 5 3 | -0.8 0.5 5 4 | -0.7 0.5 5 5 | -0.6 0.5 5 6 | -0.5 0.5 5 7 | -0.4 0.5 5 8 | -0.3 0.5 5 9 | -0.2 0.5 5 10 | -0.1 0.5 5 11 | 0.0 0.5 5 12 | 0.1 0.5 5 13 | 0.2 0.5 5 14 | 0.3 0.5 5 15 | 0.4 0.5 5 16 | 0.5 0.5 5 17 | 0.6 0.5 5 18 | 0.7 0.5 5 19 | 0.8 0.5 5 20 | 0.9 0.5 5 21 | 1.0 0.5 5 22 | 23 | -1.0 -0.4 5 24 | -1.0 -0.3 5 25 | -1.0 -0.2 5 26 | -1.0 -0.1 5 27 | -1.0 0.0 5 28 | -1.0 0.1 5 29 | -1.0 0.2 5 30 | -1.0 0.3 5 31 | -1.0 0.4 5 32 | 33 | 1.0 -0.4 5 34 | 1.0 -0.3 5 35 | 1.0 -0.2 5 36 | 1.0 -0.1 5 37 | 1.0 0.0 5 38 | 1.0 0.1 5 39 | 1.0 0.2 5 40 | 1.0 0.3 5 41 | 1.0 0.4 5 42 | 43 | -1.0 -0.5 5 44 | -0.9 -0.5 5 45 | -0.8 -0.5 5 46 | -0.7 -0.5 5 47 | -0.6 -0.5 5 48 | -0.5 -0.5 5 49 | -0.4 -0.5 5 50 | -0.3 -0.5 5 51 | -0.2 -0.5 5 52 | -0.1 -0.5 5 53 | 0.0 -0.5 5 54 | 0.1 -0.5 5 55 | 0.2 -0.5 5 56 | 0.3 -0.5 5 57 | 0.4 -0.5 5 58 | 0.5 -0.5 5 59 | 0.6 -0.5 5 60 | 0.7 -0.5 5 61 | 0.8 -0.5 5 62 | 0.9 -0.5 5 63 | 1.0 -0.5 5 64 | 65 | 66 | 67 | -1.0 0.5 6 68 | -0.9 0.5 6 69 | -0.8 0.5 6 70 | -0.7 0.5 6 71 | -0.6 0.5 6 72 | -0.5 0.5 6 73 | -0.4 0.5 6 74 | -0.3 0.5 6 75 | -0.2 0.5 6 76 | -0.1 0.5 6 77 | 0.0 0.5 6 78 | 0.1 0.5 6 79 | 0.2 0.5 6 80 | 0.3 0.5 6 81 | 0.4 0.5 6 82 | 0.5 0.5 6 83 | 0.6 0.5 6 84 | 0.7 0.5 6 85 | 0.8 0.5 6 86 | 0.9 0.5 6 87 | 1.0 0.5 6 88 | 89 | -1.0 -0.4 6 90 | -1.0 -0.3 6 91 | -1.0 -0.2 6 92 | -1.0 -0.1 6 93 | -1.0 0.0 6 94 | -1.0 0.1 6 95 | -1.0 0.2 6 96 | -1.0 0.3 6 97 | -1.0 0.4 6 98 | 99 | 1.0 -0.4 6 100 | 1.0 -0.3 6 101 | 1.0 -0.2 6 102 | 1.0 -0.1 6 103 | 1.0 0.0 6 104 | 1.0 0.1 6 105 | 1.0 0.2 6 106 | 1.0 0.3 6 107 | 1.0 0.4 6 108 | 109 | -1.0 -0.5 6 110 | -0.9 -0.5 6 111 | -0.8 -0.5 6 112 | -0.7 -0.5 6 113 | -0.6 -0.5 6 114 | -0.5 -0.5 6 115 | -0.4 -0.5 6 116 | -0.3 -0.5 6 117 | -0.2 -0.5 6 118 | -0.1 -0.5 6 119 | 0.0 -0.5 6 120 | 0.1 -0.5 6 121 | 0.2 -0.5 6 122 | 0.3 -0.5 6 123 | 0.4 -0.5 6 124 | 0.5 -0.5 6 125 | 0.6 -0.5 6 126 | 0.7 -0.5 6 127 | 0.8 -0.5 6 128 | 0.9 -0.5 6 129 | 1.0 -0.5 6 130 | 131 | 132 | 133 | -1.0 0.5 5.1 134 | -1.0 0.5 5.1 135 | -1.0 0.5 5.2 136 | -1.0 0.5 5.3 137 | -1.0 0.5 5.4 138 | -1.0 0.5 5.5 139 | -1.0 0.5 5.6 140 | -1.0 0.5 5.7 141 | -1.0 0.5 5.8 142 | -1.0 0.5 5.9 143 | 144 | 1.0 0.5 5.1 145 | 1.0 0.5 5.1 146 | 1.0 0.5 5.2 147 | 1.0 0.5 5.3 148 | 1.0 0.5 5.4 149 | 1.0 0.5 5.5 150 | 1.0 0.5 5.6 151 | 1.0 0.5 5.7 152 | 1.0 0.5 5.8 153 | 1.0 0.5 5.9 154 | 155 | -1.0 -0.5 5.1 156 | -1.0 -0.5 5.1 157 | -1.0 -0.5 5.2 158 | -1.0 -0.5 5.3 159 | -1.0 -0.5 5.4 160 | -1.0 -0.5 5.5 161 | -1.0 -0.5 5.6 162 | -1.0 -0.5 5.7 163 | -1.0 -0.5 5.8 164 | -1.0 -0.5 5.9 165 | 166 | 1.0 -0.5 5.1 167 | 1.0 -0.5 5.1 168 | 1.0 -0.5 5.2 169 | 1.0 -0.5 5.3 170 | 1.0 -0.5 5.4 171 | 1.0 -0.5 5.5 172 | 1.0 -0.5 5.6 173 | 1.0 -0.5 5.7 174 | 1.0 -0.5 5.8 175 | 1.0 -0.5 5.9 176 | -------------------------------------------------------------------------------- /data/chessboard.avi: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/chessboard.avi -------------------------------------------------------------------------------- /data/daejeon_station.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/daejeon_station.png -------------------------------------------------------------------------------- /data/hill01.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/hill01.jpg -------------------------------------------------------------------------------- /data/hill02.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/hill02.jpg -------------------------------------------------------------------------------- /data/relief/00.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/relief/00.jpg -------------------------------------------------------------------------------- /data/relief/01.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/relief/01.jpg -------------------------------------------------------------------------------- /data/relief/02.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/relief/02.jpg -------------------------------------------------------------------------------- /data/relief/03.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/relief/03.jpg -------------------------------------------------------------------------------- /data/relief/04.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/relief/04.jpg -------------------------------------------------------------------------------- /data/sunglok_card.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/sunglok_card.jpg -------------------------------------------------------------------------------- /data/traffic.avi: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/data/traffic.avi -------------------------------------------------------------------------------- /examples/3d_rotation_conversion.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from scipy.spatial.transform import Rotation 3 | 4 | # The given 3D rotation 5 | euler = (45, 30, 60) # Unit: [deg] in the XYZ-order 6 | 7 | # Generate 3D rotation object 8 | robj = Rotation.from_euler('zyx', euler[::-1], degrees=True) 9 | 10 | # Print other representations 11 | print('\n## Euler Angle (ZYX)') 12 | print(np.rad2deg(robj.as_euler('zyx'))) # [60, 30, 45] [deg] in the ZYX-order 13 | print('\n## Rotation Matrix') 14 | print(robj.as_matrix()) 15 | print('\n## Rotation Vector') 16 | print(robj.as_rotvec()) # [0.97, 0.05, 1.17] 17 | print('\n## Quaternion (XYZW)') 18 | print(robj.as_quat()) # [0.44, 0.02, 0.53, 0.72] 19 | -------------------------------------------------------------------------------- /examples/affine_estimation_implement.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import cv2 as cv 3 | 4 | def getAffineTransform(src, dst): 5 | if len(src) == len(dst): 6 | # Solve 'Ax = b' 7 | A, b = [], [] 8 | for p, q in zip(src, dst): 9 | A.append([p[0], p[1], 0, 0, 1, 0]) 10 | A.append([0, 0, p[0], p[1], 0, 1]) 11 | b.append(q[0]) 12 | b.append(q[1]) 13 | x = np.linalg.pinv(A) @ b 14 | 15 | # Reorganize `x` as a matrix 16 | H = np.array([[x[0], x[1], x[4]], [x[2], x[3], x[5]]]) 17 | return H 18 | 19 | if __name__ == '__main__': 20 | src = np.array([[115, 401], [776, 180], [330, 793]], dtype=np.float32) 21 | dst = np.array([[0, 0], [900, 0], [0, 500]], dtype=np.float32) 22 | 23 | my_H = getAffineTransform(src, dst) 24 | cv_H = cv.getAffineTransform(src, dst) # Note) It accepts only 3 pairs of points. 25 | 26 | print('\n### My Affine Transformation') 27 | print(my_H) 28 | print('\n### OpenCV Affine Transformation') 29 | print(cv_H) -------------------------------------------------------------------------------- /examples/bundle_adjustment.cpp: -------------------------------------------------------------------------------- 1 | #include "bundle_adjustment.hpp" 2 | 3 | int main() 4 | { 5 | // c.f. The following dataset were generated by 'image_formation.cpp'. 6 | const char* input = "../data/image_formation%d.xyz"; 7 | int input_num = 5; 8 | double f = 1000, cx = 320, cy= 240; 9 | 10 | // Load 2D points observed from multiple views 11 | std::vector> xs; 12 | for (int i = 0; i < input_num; i++) 13 | { 14 | FILE* fin = fopen(cv::format(input, i).c_str(), "rt"); 15 | if (fin == NULL) return -1; 16 | std::vector pts; 17 | while (!feof(fin)) 18 | { 19 | double x, y, w; 20 | if (fscanf(fin, "%lf %lf %lf", &x, &y, &w) == 3) 21 | pts.push_back(cv::Point2d(x, y)); 22 | } 23 | fclose(fin); 24 | xs.push_back(pts); 25 | if (xs.front().size() != xs.back().size()) return -1; 26 | } 27 | 28 | // Assumption 29 | // - All cameras have the same and known camera matrix. 30 | // - All points are visible on all camera views. 31 | 32 | // Initialize cameras and 3D points 33 | std::vector cameras(xs.size()); 34 | std::vector Xs(xs.front().size(), cv::Point3d(0, 0, 5.5)); 35 | 36 | // Optimize camera pose and 3D points together (bundle adjustment) 37 | ceres::Problem ba; 38 | for (size_t j = 0; j < xs.size(); j++) 39 | { 40 | for (size_t i = 0; i < xs[j].size(); i++) 41 | { 42 | ceres::CostFunction* cost_func = ReprojectionError::create(xs[j][i], f, cv::Point2d(cx, cy)); 43 | double* camera = (double*)(&(cameras[j])); 44 | double* X = (double*)(&(Xs[i])); 45 | ba.AddResidualBlock(cost_func, NULL, camera, X); 46 | } 47 | } 48 | ceres::Solver::Options options; 49 | options.linear_solver_type = ceres::ITERATIVE_SCHUR; 50 | options.num_threads = 8; 51 | options.minimizer_progress_to_stdout = true; 52 | ceres::Solver::Summary summary; 53 | ceres::Solve(options, &ba, &summary); 54 | 55 | // Store the 3D points to an XYZ file 56 | FILE* fpts = fopen("bundle_adjustment_global(point).xyz", "wt"); 57 | if (fpts == NULL) return -1; 58 | for (size_t i = 0; i < Xs.size(); i++) 59 | fprintf(fpts, "%f %f %f\n", Xs[i].x, Xs[i].y, Xs[i].z); // Format: x, y, z 60 | fclose(fpts); 61 | 62 | // Store the camera poses to an XYZ file 63 | FILE* fcam = fopen("bundle_adjustment_global(camera).xyz", "wt"); 64 | if (fcam == NULL) return -1; 65 | for (size_t j = 0; j < cameras.size(); j++) 66 | { 67 | cv::Vec3d rvec(cameras[j][0], cameras[j][1], cameras[j][2]), t(cameras[j][3], cameras[j][4], cameras[j][5]); 68 | cv::Matx33d R; 69 | cv::Rodrigues(rvec, R); 70 | cv::Vec3d p = -R.t() * t; 71 | fprintf(fcam, "%f %f %f %f %f %f\n", p[0], p[1], p[2], R.t()(0, 2), R.t()(1, 2), R.t()(2, 2)); // Format: x, y, z, n_x, n_y, n_z 72 | } 73 | fclose(fcam); 74 | return 0; 75 | } 76 | -------------------------------------------------------------------------------- /examples/bundle_adjustment.hpp: -------------------------------------------------------------------------------- 1 | #ifndef __BUNDLE_ADJUSTMENT__ 2 | #define __BUNDLE_ADJUSTMENT__ 3 | 4 | #include "opencv2/opencv.hpp" 5 | #include "ceres/ceres.h" 6 | #include "ceres/rotation.h" 7 | 8 | // Reprojection error for bundle adjustment 9 | struct ReprojectionError 10 | { 11 | ReprojectionError(const cv::Point2d& _x, double _f, const cv::Point2d& _c) : x(_x), f(_f), c(_c) { } 12 | 13 | template 14 | bool operator()(const T* const camera, const T* const point, T* residuals) const 15 | { 16 | // X' = R*X + t 17 | T X[3]; 18 | ceres::AngleAxisRotatePoint(camera, point, X); 19 | X[0] += camera[3]; 20 | X[1] += camera[4]; 21 | X[2] += camera[5]; 22 | 23 | // x' = K*X' 24 | T x_p = f * X[0] / X[2] + c.x; 25 | T y_p = f * X[1] / X[2] + c.y; 26 | 27 | // residual = x - x' 28 | residuals[0] = T(x.x) - x_p; 29 | residuals[1] = T(x.y) - y_p; 30 | return true; 31 | } 32 | 33 | static ceres::CostFunction* create(const cv::Point2d& _x, double _f, const cv::Point2d& _c) 34 | { 35 | return (new ceres::AutoDiffCostFunction(new ReprojectionError(_x, _f, _c))); 36 | } 37 | 38 | private: 39 | const cv::Point2d x; 40 | const double f; 41 | const cv::Point2d c; 42 | }; 43 | 44 | #endif // End of '__BUNDLE_ADJUSTMENT__' 45 | -------------------------------------------------------------------------------- /examples/bundle_adjustment.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from scipy.sparse import lil_matrix 3 | from scipy.optimize import least_squares, minimize 4 | from scipy.spatial.transform import Rotation as R 5 | import time 6 | import matplotlib.pyplot as plt 7 | import argparse 8 | 9 | f, cx, cy = 1000, 320, 240 10 | 11 | msg = """This script is a Python file related to global bundle adjustment. 12 | Both the least_square and minimum versions of scipy can be used, and the 13 | least square can also be used with or without the Jacobian matrix. 14 | By the way, minimizing takes a long time, so please don't think it's impossible and wait. 15 | """ 16 | 17 | def bundle_adjustment_sparsity(n_cameras, n_points, camera_indices, point_indices): 18 | m = camera_indices.size * 2 19 | n = n_cameras * 6 + n_points * 3 20 | A = lil_matrix((m, n), dtype=int) 21 | 22 | i = np.arange(camera_indices.size) 23 | for s in range(6): 24 | A[2 * i, camera_indices * 6 + s] = 1 25 | A[2 * i + 1, camera_indices * 6 + s] = 1 26 | 27 | for s in range(3): 28 | A[2 * i, n_cameras * 6 + point_indices * 3 + s] = 1 29 | A[2 * i + 1, n_cameras * 6 + point_indices * 3 + s] = 1 30 | 31 | return A 32 | 33 | def project(points, cam_trans): 34 | """나중에 scipy의 rotation으로 바꾸기""" 35 | global f, cx, cy 36 | K = np.array([[f, 0, cx], [0, f, cy], [0, 0, 1]], dtype=np.float32) 37 | # print("f, cx, cy = ", f, cy, cx) 38 | """Get R and t then project points""" 39 | rot_vecs = cam_trans[:, :3] # 40 | t_vecs = cam_trans[:, 3:] 41 | theta = np.linalg.norm(rot_vecs, axis=1)[:, np.newaxis] 42 | with np.errstate(invalid='ignore'): 43 | v = rot_vecs / theta 44 | v = np.nan_to_num(v) 45 | dot = np.sum(points * v, axis=1)[:, np.newaxis] 46 | cos_theta = np.cos(theta) 47 | sin_theta = np.sin(theta) 48 | points_proj = cos_theta * points + sin_theta * np.cross(v, points) + dot * (1 - cos_theta) * v 49 | points_proj += t_vecs # points num 50 | points_proj = points_proj @ K.T 51 | points_proj /= points_proj[:, 2, np.newaxis] # 52 | 53 | return points_proj[:, :2].ravel() 54 | 55 | def func2(params, points_2d, n_cameras, n_points, camera_indices, point_indices): 56 | camera_params = params[:n_cameras * 6].reshape((n_cameras, 6)) 57 | points_3d = params[n_cameras * 6:].reshape((n_points, 3)) 58 | points_proj = project(points_3d[point_indices], camera_params[camera_indices]) # 둘의 개수를 맞춰줬다! 적절한 index로 2d points의 개수를 파악하자. 59 | result = (points_2d - points_proj).ravel() 60 | return result 61 | 62 | def func3(params, points_2d, n_cameras, n_points, camera_indices, point_indices): 63 | camera_params = params[:n_cameras * 6].reshape((n_cameras, 6)) 64 | points_3d = params[n_cameras * 6:].reshape((n_points, 3)) 65 | points_proj = project(points_3d[point_indices], camera_params[camera_indices]) # 둘의 개수를 맞춰줬다! 적절한 index로 2d points의 개수를 파악하자. 66 | result = np.sum(((points_2d - points_proj)**2).ravel()) 67 | return result 68 | 69 | def main(): 70 | global f, cx, cy 71 | parser = argparse.ArgumentParser(description=msg) 72 | parser.add_argument('--jac', required=False, default=True, help='Using Jacobian to boost up optimizer') 73 | parser.add_argument('--method', required=False, default='least_square' , help='Choose between least_square and minimize. Only two things are suppported') 74 | parser.add_argument('--show-time', required=False, default=False, help='Show how much time is spended for calculating') 75 | parser.add_argument('--show-test', required=False, default=False, help='Show test points for check real points') 76 | 77 | args = parser.parse_args() 78 | METHOD = args.method 79 | JAC = args.jac 80 | SHOW_TIME = args.show_time 81 | SHOW_TEST = args.show_test 82 | 83 | K = np.array([[f, 0, cx], [0, f, cy], [0, 0, 1]], dtype=np.float32) 84 | # Following datasets were generated by image_formation.py 85 | input_num = 5 86 | 87 | # Load 2D points observed from multiple views 88 | xs = [] 89 | for i in range(input_num): 90 | input = f"../bin/data/image_formation{i}.xyz" 91 | x = np.genfromtxt(input, delimiter=" ") 92 | x = x[:, :2] 93 | xs.append(x) 94 | 95 | xs = np.array(xs) 96 | 97 | n_cameras = xs.shape[0] 98 | n_points = xs.shape[1] 99 | 100 | # Assumption 101 | # - All cameras have the same and known camera matrix 102 | # - All points are visible on all camera views 103 | 104 | # Matching index for cam & 3d points 105 | cam_indices = np.array([]) 106 | length_c_ind = np.arange(n_points, dtype=int) 107 | for i in range(n_cameras): 108 | cam_indices = np.hstack((cam_indices, np.full_like(length_c_ind, i))) # 0x160, 1x160, ... 109 | 110 | point_indices = np.array([]) 111 | # length_p_ind = np.arange(n_cameras, dtype=int) 112 | for i in range(n_cameras): 113 | point_indices = np.hstack((point_indices, np.linspace(0, 159, 160, dtype=int))) # (0 ~ 159) * 5 114 | # point_indices = np.hstack((point_indices, np.full_like(length_p_ind, i))) # 0x5, 1x5, 2x5, ..., 159x5 115 | 116 | cam_indices = cam_indices.astype(int) 117 | point_indices = point_indices.astype(int) 118 | 119 | # Initialize cameras and 3D points 120 | cameras = np.zeros((xs.shape[0], 6)) # rotation and translation 121 | cameras[:,2] = 1 # watching forward 122 | Xs = np.full((xs.shape[1], xs.shape[2]+1), np.array([[0, 0, 5.5]])) # 3d points initial num & pose 123 | x0 = np.hstack((cameras.ravel(), Xs.ravel())) # camera pose and 3d points 124 | xs = xs.ravel() 125 | 126 | 127 | if METHOD == 'least_square': 128 | # Add Jacobian sparsity 129 | if JAC: 130 | J = bundle_adjustment_sparsity(n_cameras=n_cameras, n_points=n_points, camera_indices=cam_indices, point_indices=point_indices) 131 | else: 132 | J = None 133 | # Optimize camera pose and 3D points together (bundle adjustment) 134 | 135 | if SHOW_TIME: 136 | t = time.time() 137 | res = least_squares(func2, x0, verbose=2, ftol=1e-15, method='trf', jac_sparsity=J, args=(xs, n_cameras, n_points, cam_indices, point_indices)) 138 | print("total time:",time.time() - t) 139 | 140 | else: 141 | res = least_squares(func2, x0, verbose=2, ftol=1e-15, method='trf', jac_sparsity=J, args=(xs, n_cameras, n_points, cam_indices, point_indices)) 142 | 143 | elif METHOD == 'minimize': 144 | res = minimize(func3, x0, args=(xs, n_cameras, n_points, cam_indices, point_indices)) 145 | 146 | opt_cameras = res.x[:n_cameras * 6].reshape((n_cameras, 6)) # rotation and translation 147 | opt_points = res.x[n_cameras * 6: ].reshape((n_points, 3)) # 3d points 148 | 149 | if SHOW_TEST: 150 | # extrinsic 을 깜빡했다..... 일단 0번째는 됨. 151 | for i in range(len(opt_cameras)): 152 | test_rvec = R.from_rotvec(opt_cameras[i, :3]) # theta x, y ,z translation x, y, z 153 | test_tvec = opt_cameras[i, 3:] 154 | test_points = opt_points[0] # 첫번째 x, y, z로 테스트하기. 155 | a = test_rvec.apply(test_points) + test_tvec 156 | a = a @ K.T 157 | a /= a[2] 158 | print(a) 159 | 160 | f0 = func2(x0, xs, n_cameras, n_points, cam_indices, point_indices) 161 | plt.plot(f0, 'r', label='with_err') 162 | plt.plot(res.fun, 'b', label='no_err') 163 | plt.legend() 164 | plt.show() 165 | # Store the 3D points to an XYZ file 166 | point_file = "bundle_adjustment_global(point)_by_cjh.xyz" 167 | with open(point_file, 'wt') as f: 168 | for i in range(n_points): 169 | data = f"{opt_points[i, 0]} {opt_points[i, 1]} {opt_points[i, 2]}\n" 170 | f.write(data) 171 | 172 | camera_file = "bundle_adjustment_global(camera)_by_cjh.xyz" 173 | with open(camera_file, 'wt') as f: 174 | for i in range(n_cameras): 175 | data = f"{opt_cameras[i, 0]} {opt_cameras[i, 1]} {opt_cameras[i, 2]} {opt_cameras[i, 3]} {opt_cameras[i, 4]} {opt_cameras[i, 5]}\n" 176 | f.write(data) 177 | 178 | if __name__ == "__main__": 179 | main() -------------------------------------------------------------------------------- /examples/camera_calibration.cpp: -------------------------------------------------------------------------------- 1 | #include "opencv2/opencv.hpp" 2 | #include "iostream" 3 | 4 | int main() 5 | { 6 | const char* video_file = "../data/chessboard.avi"; 7 | cv::Size board_pattern(10, 7); 8 | float board_cellsize = 0.025f; 9 | bool select_images = true; 10 | 11 | // Open a video 12 | cv::VideoCapture video; 13 | if (!video.open(video_file)) return -1; 14 | 15 | // Select images 16 | std::vector images; 17 | while (true) 18 | { 19 | // Grab an image from the video 20 | cv::Mat image; 21 | video >> image; 22 | if (image.empty()) break; 23 | 24 | if (select_images) 25 | { 26 | // Show the image and keep it if selected 27 | cv::imshow("Camera Calibration", image); 28 | int key = cv::waitKey(1); 29 | if (key == 32) // Space: Pause and show corners 30 | { 31 | std::vector pts; 32 | bool complete = cv::findChessboardCorners(image, board_pattern, pts); 33 | cv::Mat display = image.clone(); 34 | cv::drawChessboardCorners(display, board_pattern, pts, complete); 35 | cv::imshow("Camera Calibration", display); 36 | key = cv::waitKey(); 37 | if (key == 13) images.push_back(image); // Enter: Select the image 38 | } 39 | if (key == 27) break; // ESC: Exit (Complete image selection) 40 | } 41 | else images.push_back(image); 42 | } 43 | video.release(); 44 | if (images.empty()) return -1; 45 | 46 | // Find 2D corner points from the given images 47 | std::vector> img_points; 48 | for (size_t i = 0; i < images.size(); i++) 49 | { 50 | std::vector pts; 51 | if (cv::findChessboardCorners(images[i], board_pattern, pts)) 52 | img_points.push_back(pts); 53 | } 54 | if (img_points.empty()) return -1; 55 | 56 | // Prepare 3D points of the chess board 57 | std::vector> obj_points(1); 58 | for (int r = 0; r < board_pattern.height; r++) 59 | for (int c = 0; c < board_pattern.width; c++) 60 | obj_points[0].push_back(cv::Point3f(board_cellsize * c, board_cellsize * r, 0)); 61 | obj_points.resize(img_points.size(), obj_points[0]); // Copy 62 | 63 | // Calibrate the camera 64 | cv::Mat K = cv::Mat::eye(3, 3, CV_64F); 65 | cv::Mat dist_coeff = cv::Mat::zeros(4, 1, CV_64F); 66 | std::vector rvecs, tvecs; 67 | double rms = cv::calibrateCamera(obj_points, img_points, images[0].size(), K, dist_coeff, rvecs, tvecs); 68 | 69 | // Print calibration results 70 | std::cout << "## Camera Calibration Results" << std::endl; 71 | std::cout << "* The number of applied images = " << img_points.size() << std::endl; 72 | std::cout << "* RMS error = " << rms << std::endl; 73 | std::cout << "* Camera matrix (K) = " << std::endl << " " << K.row(0) << K.row(1) << K.row(2) << std::endl; 74 | std::cout << "* Distortion coefficient (k1, k2, p1, p2, k3, ...) = " << std::endl << " " << dist_coeff.t() << std::endl; 75 | return 0; 76 | } 77 | -------------------------------------------------------------------------------- /examples/camera_calibration.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import cv2 as cv 3 | 4 | def select_img_from_video(video_file, board_pattern, select_all=False, wait_msec=10, wnd_name='Camera Calibration'): 5 | # Open a video 6 | video = cv.VideoCapture(video_file) 7 | assert video.isOpened() 8 | 9 | # Select images 10 | img_select = [] 11 | while True: 12 | # Grab an images from the video 13 | valid, img = video.read() 14 | if not valid: 15 | break 16 | 17 | if select_all: 18 | img_select.append(img) 19 | else: 20 | # Show the image 21 | display = img.copy() 22 | cv.putText(display, f'NSelect: {len(img_select)}', (10, 25), cv.FONT_HERSHEY_DUPLEX, 0.6, (0, 255, 0)) 23 | cv.imshow(wnd_name, display) 24 | 25 | # Process the key event 26 | key = cv.waitKey(wait_msec) 27 | if key == ord(' '): # Space: Pause and show corners 28 | complete, pts = cv.findChessboardCorners(img, board_pattern) 29 | cv.drawChessboardCorners(display, board_pattern, pts, complete) 30 | cv.imshow(wnd_name, display) 31 | key = cv.waitKey() 32 | if key == ord('\r'): 33 | img_select.append(img) # Enter: Select the image 34 | if key == 27: # ESC: Exit (Complete image selection) 35 | break 36 | 37 | cv.destroyAllWindows() 38 | return img_select 39 | 40 | def calib_camera_from_chessboard(images, board_pattern, board_cellsize, K=None, dist_coeff=None, calib_flags=None): 41 | # Find 2D corner points from given images 42 | img_points = [] 43 | for img in images: 44 | gray = cv.cvtColor(img, cv.COLOR_BGR2GRAY) 45 | complete, pts = cv.findChessboardCorners(gray, board_pattern) 46 | if complete: 47 | img_points.append(pts) 48 | assert len(img_points) > 0 49 | 50 | # Prepare 3D points of the chess board 51 | obj_pts = [[c, r, 0] for r in range(board_pattern[1]) for c in range(board_pattern[0])] 52 | obj_points = [np.array(obj_pts, dtype=np.float32) * board_cellsize] * len(img_points) # Must be `np.float32` 53 | 54 | # Calibrate the camera 55 | return cv.calibrateCamera(obj_points, img_points, gray.shape[::-1], K, dist_coeff, flags=calib_flags) 56 | 57 | if __name__ == '__main__': 58 | video_file = '../data/chessboard.avi' 59 | board_pattern = (10, 7) 60 | board_cellsize = 0.025 61 | 62 | img_select = select_img_from_video(video_file, board_pattern) 63 | assert len(img_select) > 0, 'There is no selected images!' 64 | rms, K, dist_coeff, rvecs, tvecs = calib_camera_from_chessboard(img_select, board_pattern, board_cellsize) 65 | 66 | # Print calibration results 67 | print('## Camera Calibration Results') 68 | print(f'* The number of selected images = {len(img_select)}') 69 | print(f'* RMS error = {rms}') 70 | print(f'* Camera matrix (K) = \n{K}') 71 | print(f'* Distortion coefficient (k1, k2, p1, p2, k3, ...) = {dist_coeff.flatten()}') 72 | -------------------------------------------------------------------------------- /examples/camera_calibration_implement.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from scipy.optimize import least_squares 3 | from pose_estimation_implement import project_no_distort 4 | 5 | def fcxcy_to_K(f, cx, cy): 6 | return np.array([[f, 0, cx], [0, f, cy], [0, 0, 1]]) 7 | 8 | def reproject_error_calib(unknown, Xs, xs): 9 | K = fcxcy_to_K(*unknown[0:3]) 10 | err = [] 11 | for j in range(len(xs)): 12 | offset = 3 + 6 * j 13 | rvec, tvec = unknown[offset:offset+3], unknown[offset+3:offset+6] 14 | xp = project_no_distort(Xs[j], rvec, tvec, K) 15 | err.append(xs[j] - xp) 16 | return np.vstack(err).ravel() 17 | 18 | def calibrateCamera(obj_pts, img_pts, img_size): 19 | img_n = len(img_pts) 20 | unknown_init = np.array([img_size[0], img_size[0]/2, img_size[1]/2] + img_n * [0, 0, 0, 0, 0, 1.]) # Sequence: f, cx, cy, img_n * (rvec, tvec) 21 | result = least_squares(reproject_error_calib, unknown_init, args=(obj_pts, img_pts)) 22 | K = fcxcy_to_K(*result['x'][0:3]) 23 | rvecs = [result['x'][(6*i+3):(6*i+6)] for i in range(img_n)] 24 | tvecs = [result['x'][(6*i+6):(6*i+9)] for i in range(img_n)] 25 | return result['cost'], K, np.zeros(5), rvecs, tvecs 26 | 27 | if __name__ == '__main__': 28 | img_size = (640, 480) 29 | img_files = ['../data/image_formation1.xyz', '../data/image_formation2.xyz'] 30 | img_pts = [] 31 | for file in img_files: 32 | pts = np.loadtxt(file, dtype=np.float32) 33 | img_pts.append(pts[:,:2]) 34 | 35 | pts = np.loadtxt('../data/box.xyz', dtype=np.float32) 36 | obj_pts = [pts] * len(img_pts) # Copy the object point as much as the number of image observation 37 | 38 | # Calibrate the camera 39 | _, K, *_ = calibrateCamera(obj_pts, img_pts, img_size) 40 | 41 | print('\n### Ground Truth') 42 | print('* f, cx, cy = 1000, 320, 240') 43 | print('\n### My Calibration') 44 | print(f'* f, cx, cy = {K[0,0]:.1f}, {K[0,2]:.1f}, {K[1,2]:.1f}') -------------------------------------------------------------------------------- /examples/distortion_correction.cpp: -------------------------------------------------------------------------------- 1 | #include "opencv2/opencv.hpp" 2 | 3 | int main() 4 | { 5 | // The given video and calibration data 6 | const char* video_file = "../data/chessboard.avi"; 7 | cv::Matx33d K(432.7390364738057, 0, 476.0614994349778, 8 | 0, 431.2395555913084, 288.7602152621297, 9 | 0, 0, 1); // Derived from `calibrate_camera.py` 10 | std::vector dist_coeff = { -0.2852754904152874, 0.1016466459919075, -0.0004420196146339175, 0.0001149909868437517, -0.01803978785585194 }; 11 | 12 | // Open a video 13 | cv::VideoCapture video; 14 | if (!video.open(video_file)) return -1; 15 | 16 | // Run distortion correction 17 | bool show_rectify = true; 18 | cv::Mat map1, map2; 19 | while (true) 20 | { 21 | // Read an image from the video 22 | cv::Mat image; 23 | video >> image; 24 | if (image.empty()) break; 25 | 26 | // Rectify geometric distortion (Alternative: `cv.undistort()`) 27 | cv::String info = "Original"; 28 | if (show_rectify) 29 | { 30 | if (map1.empty() || map2.empty()) 31 | cv::initUndistortRectifyMap(K, dist_coeff, cv::Mat(), cv::Mat(), image.size(), CV_32FC1, map1, map2); 32 | cv::remap(image, image, map1, map2, cv::InterpolationFlags::INTER_LINEAR); 33 | info = "Rectified"; 34 | } 35 | cv::putText(image, info, cv::Point(5, 15), cv::FONT_HERSHEY_PLAIN, 1, cv::Vec3b(0, 255, 0)); 36 | 37 | // Show the image 38 | cv::imshow("Geometric Distortion Correction", image); 39 | int key = cv::waitKey(10); 40 | if (key == 32) key = cv::waitKey(); // Space: Pause 41 | if (key == 27) break; // ESC: Exit 42 | else if (key == 9) show_rectify = !show_rectify; // Tab: Toggle the mode 43 | } 44 | 45 | video.release(); 46 | return 0; 47 | } 48 | -------------------------------------------------------------------------------- /examples/distortion_correction.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import cv2 as cv 3 | 4 | # The given video and calibration data 5 | video_file = '../data/chessboard.avi' 6 | K = np.array([[432.7390364738057, 0, 476.0614994349778], 7 | [0, 431.2395555913084, 288.7602152621297], 8 | [0, 0, 1]]) # Derived from `calibrate_camera.py` 9 | dist_coeff = np.array([-0.2852754904152874, 0.1016466459919075, -0.0004420196146339175, 0.0001149909868437517, -0.01803978785585194]) 10 | 11 | # Open a video 12 | video = cv.VideoCapture(video_file) 13 | assert video.isOpened(), 'Cannot read the given input, ' + video_file 14 | 15 | # Run distortion correction 16 | show_rectify = True 17 | map1, map2 = None, None 18 | while True: 19 | # Read an image from the video 20 | valid, img = video.read() 21 | if not valid: 22 | break 23 | 24 | # Rectify geometric distortion (Alternative: `cv.undistort()`) 25 | info = "Original" 26 | if show_rectify: 27 | if map1 is None or map2 is None: 28 | map1, map2 = cv.initUndistortRectifyMap(K, dist_coeff, None, None, (img.shape[1], img.shape[0]), cv.CV_32FC1) 29 | img = cv.remap(img, map1, map2, interpolation=cv.INTER_LINEAR) 30 | info = "Rectified" 31 | cv.putText(img, info, (10, 25), cv.FONT_HERSHEY_DUPLEX, 0.6, (0, 255, 0)) 32 | 33 | # Show the image and process the key event 34 | cv.imshow("Geometric Distortion Correction", img) 35 | key = cv.waitKey(10) 36 | if key == ord(' '): # Space: Pause 37 | key = cv.waitKey() 38 | if key == 27: # ESC: Exit 39 | break 40 | elif key == ord('\t'): # Tab: Toggle the mode 41 | show_rectify = not show_rectify 42 | 43 | video.release() 44 | cv.destroyAllWindows() 45 | -------------------------------------------------------------------------------- /examples/distortion_visualization.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import cv2 as cv 3 | 4 | # The initial camera configuration 5 | img_w, img_h = (640, 480) 6 | K = np.array([[800, 0, 320], 7 | [0, 800, 240], 8 | [0, 0, 1.]]) 9 | dist_coeff = np.array([-0.2, 0.1, 0, 0]) 10 | grid_x, grid_y, grid_z = (-18, 19), (-15, 16), 20 11 | 12 | obj_pts = np.array([[x, y, grid_z] for y in range(*grid_y) for x in range(*grid_x)], dtype=np.float32) 13 | while True: 14 | # Project 3D points with/without distortion 15 | dist_pts, _ = cv.projectPoints(obj_pts, np.zeros(3), np.zeros(3), K, dist_coeff) 16 | zero_pts, _ = cv.projectPoints(obj_pts, np.zeros(3), np.zeros(3), K, np.zeros(4)) 17 | 18 | # Draw vectors 19 | img_vector = np.full((img_h, img_w, 3), 255, dtype=np.uint8) 20 | for zero_pt, dist_pt in zip(zero_pts, dist_pts): 21 | cv.line(img_vector, np.int32(zero_pt.flatten()), np.int32(dist_pt.flatten()), (255, 0, 0)) 22 | for pt in dist_pts: 23 | cv.circle(img_vector, np.int32(pt.flatten()), 2, (0, 0, 255), -1) 24 | 25 | # Draw grids 26 | img_grid = np.full((img_h, img_w, 3), 255, dtype=np.uint8) 27 | dist_pts = dist_pts.reshape(len(range(*grid_y)), -1, 2) 28 | for pts in dist_pts: 29 | cv.polylines(img_grid, [np.int32(pts)], False, (0, 0, 255)) 30 | for pts in dist_pts.swapaxes(0, 1): 31 | cv.polylines(img_grid, [np.int32(pts)], False, (0, 0, 255)) 32 | 33 | # Show all images and process key event 34 | merge = np.hstack((img_vector, img_grid)) 35 | info = f'Focal: {K[0, 0]:.0f}, k1: {dist_coeff[0]:.2f}, k2: {dist_coeff[1]:.2f}, p1: {dist_coeff[2]:.2f}, p2: {dist_coeff[3]:.2f}' 36 | cv.putText(merge, info, (10, 25), cv.FONT_HERSHEY_DUPLEX, 0.6, (0, 0, 0)) 37 | cv.imshow('Geometric Distortion Visualization: Vectors | Grids', merge) 38 | key = cv.waitKey() 39 | if key == 27: # ESC 40 | break 41 | elif key == ord(')') or key == ord('0'): 42 | K[0,0] += 100 43 | K[1,1] += 100 44 | elif key == ord('(') or key == ord('9'): 45 | K[0,0] -= 100 46 | K[1,1] -= 100 47 | elif key == ord('+') or key == ord('='): 48 | dist_coeff[0] += 0.05 49 | elif key == ord('-') or key == ord('_'): 50 | dist_coeff[0] -= 0.05 51 | elif key == ord(']') or key == ord('}'): 52 | dist_coeff[1] += 0.05 53 | elif key == ord('[') or key == ord('{'): 54 | dist_coeff[1] -= 0.05 55 | elif key == ord('"') or key == ord("'"): 56 | dist_coeff[2] += 0.01 57 | elif key == ord(':') or key == ord(';'): 58 | dist_coeff[2] -= 0.01 59 | elif key == ord('>') or key == ord('.'): 60 | dist_coeff[3] += 0.01 61 | elif key == ord('<') or key == ord(','): 62 | dist_coeff[3] -= 0.01 63 | 64 | cv.destroyAllWindows() 65 | -------------------------------------------------------------------------------- /examples/epipolar_line_visualization.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import cv2 as cv 3 | import random 4 | 5 | def mouse_event_handler(event, x, y, flags, param): 6 | if event == cv.EVENT_LBUTTONDOWN: 7 | param.append((x, y)) 8 | 9 | def draw_straight_line(img, line, color, thickness=1): 10 | assert img.ndim >= 2 11 | h, w, *_ = img.shape 12 | a, b, c = line # Line: ax + by + c = 0 13 | if abs(a) > abs(b): 14 | pt1 = (int(c / -a), 0) 15 | pt2 = (int((b*h + c) / -a), h) 16 | else: 17 | pt1 = (0, int(c / -b)) 18 | pt2 = (w, int((a*w + c) / -b)) 19 | cv.line(img, pt1, pt2, color, thickness) 20 | 21 | if __name__ == '__main__': 22 | # Load two images 23 | img1 = cv.imread('../data/KITTI07/image_0/000000.png', cv.IMREAD_COLOR) 24 | img2 = cv.imread('../data/KITTI07/image_0/000023.png', cv.IMREAD_COLOR) 25 | assert (img1 is not None) and (img2 is not None), 'Cannot read the given images' 26 | # Note) `F` is derived from `fundamental_mat_estimation.py`. 27 | F = np.array([[ 3.34638533e-07, 7.58547151e-06, -2.04147752e-03], 28 | [-5.83765868e-06, 1.36498636e-06, 2.67566877e-04], 29 | [ 1.45892349e-03, -4.37648316e-03, 1.00000000e+00]]) 30 | 31 | # Register event handlers and show images 32 | wnd1_name, wnd2_name = 'Epipolar Line: Image #1', 'Epipolar Line: Image #2' 33 | img1_pts, img2_pts = [], [] 34 | cv.namedWindow(wnd1_name) 35 | cv.namedWindow(wnd2_name) 36 | cv.setMouseCallback(wnd1_name, mouse_event_handler, img1_pts) 37 | cv.setMouseCallback(wnd2_name, mouse_event_handler, img2_pts) 38 | cv.imshow(wnd1_name, img1) 39 | cv.imshow(wnd2_name, img2) 40 | 41 | # Get a point from a image and draw its correponding epipolar line on the other image 42 | while True: 43 | if len(img1_pts) > 0: 44 | for x, y in img1_pts: 45 | color = (random.randrange(256), random.randrange(256), random.randrange(256)) 46 | cv.circle(img1, (x, y), 4, color, -1) 47 | epipolar_line = F @ [[x], [y], [1]] 48 | draw_straight_line(img2, epipolar_line, color, 2) 49 | img1_pts.clear() 50 | if len(img2_pts) > 0: 51 | for x, y in img2_pts: 52 | color = (random.randrange(256), random.randrange(256), random.randrange(256)) 53 | cv.circle(img2, (x, y), 4, color, -1) 54 | epipolar_line = F.T @ [[x], [y], [1]] 55 | draw_straight_line(img1, epipolar_line, color, 2) 56 | img2_pts.clear() 57 | cv.imshow(wnd2_name, img2) 58 | cv.imshow(wnd1_name, img1) 59 | key = cv.waitKey(10) 60 | if key == 27: # ESC 61 | break 62 | 63 | cv.destroyAllWindows() -------------------------------------------------------------------------------- /examples/feature_matching.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import cv2 as cv 3 | import time 4 | 5 | # Load two images 6 | img1 = cv.imread('../data/hill01.jpg') 7 | img2 = cv.imread('../data/hill02.jpg') 8 | assert (img1 is not None) and (img2 is not None), 'Cannot read the given images' 9 | 10 | # Instantiate feature detectors and matchers 11 | # Note) You can specify options for each detector in its creation function. 12 | features = [ 13 | {'name': 'AKAZE', 'detector': cv.AKAZE_create(), 'matcher': cv.DescriptorMatcher_create('BruteForce-Hamming')}, 14 | {'name': 'BRISK', 'detector': cv.BRISK_create(), 'matcher': cv.DescriptorMatcher_create('BruteForce-Hamming')}, 15 | {'name': 'FAST', 'detector': cv.FastFeatureDetector_create(), 'matcher': None}, # No descriptor 16 | {'name': 'GFTT', 'detector': cv.GFTTDetector_create(), 'matcher': None}, # No descriptor 17 | {'name': 'KAZE', 'detector': cv.KAZE_create(), 'matcher': None}, # No descriptor 18 | {'name': 'MSER', 'detector': cv.MSER_create(), 'matcher': None}, # No descriptor 19 | {'name': 'ORB', 'detector': cv.ORB_create(), 'matcher': cv.DescriptorMatcher_create('BruteForce-Hamming')}, 20 | {'name': 'SIFT', 'detector': cv.SIFT_create(), 'matcher': cv.DescriptorMatcher_create('BruteForce')}, 21 | ] 22 | 23 | # Initialize control parameters 24 | f_select = 0 25 | 26 | while True: 27 | # Detect feature points 28 | time_start = time.time() 29 | keypoints1 = features[f_select]['detector'].detect(img1) 30 | keypoints2 = features[f_select]['detector'].detect(img2) 31 | time_detect = time.time() 32 | 33 | if features[f_select]['matcher'] is not None: 34 | # Extract feature descriptors 35 | keypoints1, descriptors1 = features[f_select]['detector'].compute(img1, keypoints1) 36 | keypoints2, descriptors2 = features[f_select]['detector'].compute(img2, keypoints2) 37 | time_compute = time.time() 38 | 39 | # Match the feature descriptors 40 | match = features[f_select]['matcher'].match(descriptors1, descriptors2) 41 | time_match = time.time() 42 | else: 43 | time_compute = time_detect 44 | time_match = time_compute 45 | 46 | # Show the matched image 47 | if features[f_select]['matcher'] is not None: 48 | img_merged = cv.drawMatches(img1, keypoints1, img2, keypoints2, match, None) 49 | else: 50 | img1_keypts = cv.drawKeypoints(img1, keypoints1, None) 51 | img2_keypts = cv.drawKeypoints(img2, keypoints2, None) 52 | img_merged = np.hstack((img1_keypts, img2_keypts)) 53 | info = features[f_select]['name'] + f': ({(time_detect-time_start)*1000:.0f} + {(time_compute-time_detect)*1000:.0f} + {(time_match-time_compute)*1000:.0f}) = {(time_match-time_start)*1000:.0f} [msec]' 54 | cv.putText(img_merged, info, (5, 15), cv.FONT_HERSHEY_PLAIN, 1, (0, 0, 255)) 55 | cv.imshow('Feature Matching', img_merged) 56 | 57 | # Process the key event 58 | key = cv.waitKey(0) 59 | if key == 27: # ESC 60 | break 61 | elif key == ord('-') or key == ord('_'): 62 | f_select = (f_select - 1) % len(features) 63 | else: 64 | f_select = (f_select + 1) % len(features) 65 | 66 | cv.destroyAllWindows() -------------------------------------------------------------------------------- /examples/feature_tracking_klt.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib.pyplot as plt 3 | import cv2 as cv 4 | 5 | video_file = '../data/KITTI07/image_0/%06d.png' 6 | min_track_error = 5 7 | 8 | # Open a video and get an initial image 9 | video = cv.VideoCapture(video_file) 10 | assert video.isOpened() 11 | 12 | _, gray_prev = video.read() 13 | assert gray_prev.size > 0 14 | if gray_prev.ndim >= 3 and gray_prev.shape[2] > 1: 15 | gray_prev = cv.cvtColor(gray_prev, cv.COLOR_BGR2GRAY) 16 | 17 | # Run the KLT feature tracker 18 | while True: 19 | # Grab an image from the video 20 | valid, img = video.read() 21 | if not valid: 22 | break 23 | if img.ndim >= 3 and img.shape[2] > 1: 24 | gray = cv.cvtColor(img, cv.COLOR_BGR2GRAY) 25 | else: 26 | gray = img.copy() 27 | 28 | # Extract optical flow 29 | pts_prev = cv.goodFeaturesToTrack(gray_prev, 2000, 0.01, 10) 30 | pts, status, error = cv.calcOpticalFlowPyrLK(gray_prev, gray, pts_prev, None) 31 | gray_prev = gray 32 | 33 | # Show the optical flow on the image 34 | if img.ndim < 3 or img.shape[2] < 3: 35 | img = cv.cvtColor(img, cv.COLOR_GRAY2BGR) 36 | for pt, pt_prev, tracked, err in zip(pts, pts_prev, status, error): 37 | if tracked and err < min_track_error: 38 | cv.line(img, pt_prev.flatten().astype(np.int32), pt.flatten().astype(np.int32), (0, 255, 0)) 39 | cv.imshow('KLT Feature Tracking', img) 40 | key = cv.waitKey(1) 41 | if key == ord(' '): 42 | key = cv.waitKey() 43 | if key == 27: # ESC 44 | break 45 | 46 | video.release() 47 | cv.destroyAllWindows() -------------------------------------------------------------------------------- /examples/fundamental_mat_estimation.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import cv2 as cv 3 | 4 | # Load two images 5 | img1 = cv.imread('../data/KITTI07/image_0/000000.png') 6 | img2 = cv.imread('../data/KITTI07/image_0/000023.png') 7 | assert (img1 is not None) and (img2 is not None), 'Cannot read the given images' 8 | f, cx, cy = 707.0912, 601.8873, 183.1104 # From the KITTI dataset 9 | K = np.array([[f, 0, cx], [0, f, cy], [0, 0, 1]]) 10 | 11 | # Retrieve matching points 12 | fdetector = cv.BRISK_create() 13 | keypoints1, descriptors1 = fdetector.detectAndCompute(img1, None) 14 | keypoints2, descriptors2 = fdetector.detectAndCompute(img2, None) 15 | 16 | fmatcher = cv.DescriptorMatcher_create('BruteForce-Hamming') 17 | match = fmatcher.match(descriptors1, descriptors2) 18 | 19 | # Calculate the fundamental matrix 20 | pts1, pts2 = [], [] 21 | for i in range(len(match)): 22 | pts1.append(keypoints1[match[i].queryIdx].pt) 23 | pts2.append(keypoints2[match[i].trainIdx].pt) 24 | pts1 = np.array(pts1, dtype=np.float32) 25 | pts2 = np.array(pts2, dtype=np.float32) 26 | F, inlier_mask = cv.findFundamentalMat(pts1, pts2, cv.FM_RANSAC, 0.5, 0.999) 27 | print(f'* F = {F}') 28 | print(f'* The number of inliers = {sum(inlier_mask.ravel())}') 29 | 30 | # Extract relative camera pose between two images 31 | E = K.T @ F @ K 32 | positive_num, R, t, positive_mask = cv.recoverPose(E, pts1, pts2, K, mask=inlier_mask) 33 | print(f'* R = {R}') 34 | print(f'* t = {t}') 35 | print(f'* The position of Image #2 = {-R.T @ t}') # [-0.57, 0.09, 0.82] 36 | print(f'* The number of positive-depth inliers = {sum(positive_mask.ravel())}') 37 | 38 | # Show the matched images 39 | img_matched = cv.drawMatches(img1, keypoints1, img2, keypoints2, match, None, None, None, 40 | matchesMask=inlier_mask.ravel().tolist()) # Remove `matchesMask` if you want to show all putative matches 41 | cv.namedWindow('Fundamental Matrix Estimation', cv.WINDOW_NORMAL) 42 | cv.imshow('Fundamental Matrix Estimation', img_matched) 43 | cv.waitKey(0) 44 | cv.destroyAllWindows() 45 | -------------------------------------------------------------------------------- /examples/fundamental_mat_estimation_implement.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import cv2 as cv 3 | 4 | def findFundamentalMat(pts1, pts2): 5 | if len(pts1) == len(pts2): 6 | # Make homogeneous coordiates if necessary 7 | if pts1.shape[1] == 2: 8 | pts1 = np.hstack((pts1, np.ones((len(pts1), 1), dtype=pts1.dtype))) 9 | if pts2.shape[1] == 2: 10 | pts2 = np.hstack((pts2, np.ones((len(pts2), 1), dtype=pts2.dtype))) 11 | 12 | # Solve 'Ax = 0' 13 | A = [] 14 | for p, q in zip(pts1, pts2): 15 | A.append([q[0]*p[0], q[0]*p[1], q[0]*p[2], q[1]*p[0], q[1]*p[1], q[1]*p[2], q[2]*p[0], q[2]*p[1], q[2]*p[2]]) 16 | _, _, Vt = np.linalg.svd(A, full_matrices=True) 17 | x = Vt[-1] 18 | 19 | # Reorganize `x` as `F` and enforce 'rank(F) = 2' 20 | F = x.reshape(3, -1) 21 | U, S, Vt = np.linalg.svd(F) 22 | S[-1] = 0 23 | F = U @ np.diag(S) @ Vt 24 | return F / F[-1,-1] # Normalize the last element as 1 25 | 26 | if __name__ == '__main__': 27 | pts0 = np.loadtxt('../data/image_formation0.xyz') 28 | pts1 = np.loadtxt('../data/image_formation1.xyz') 29 | 30 | my_F = findFundamentalMat(pts0, pts1) 31 | cv_F, _ = cv.findFundamentalMat(pts0, pts1, cv.FM_8POINT) 32 | 33 | print('\n### My Fundamental Matrix') 34 | print(my_F) 35 | print('\n### OpenCV Fundamental Matrix') 36 | print(cv_F) # Note) The result is slightly different because OpenCV considered normalization -------------------------------------------------------------------------------- /examples/harris_corner_implement.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib.pyplot as plt 3 | import cv2 as cv 4 | 5 | def cornerHarris(img, ksize=3, k=0.04): 6 | # Compute gradients and M matrix 7 | Ix = cv.Sobel(img, cv.CV_32F, 1, 0) 8 | Iy = cv.Sobel(img, cv.CV_32F, 0, 1) 9 | M11 = cv.GaussianBlur(Ix*Ix, (ksize, ksize), 0) 10 | M22 = cv.GaussianBlur(Iy*Iy, (ksize, ksize), 0) 11 | M12 = cv.GaussianBlur(Ix*Iy, (ksize, ksize), 0) 12 | 13 | # Compute Harris cornerness 14 | detM = M11 * M22 - M12 * M12 15 | traceM = M11 + M22 16 | cornerness = detM - k * traceM**2 17 | return cornerness 18 | 19 | if __name__ == '__main__': 20 | video = cv.VideoCapture('../data/chessboard.avi') 21 | assert video.isOpened() 22 | 23 | # Run and show video stabilization 24 | while True: 25 | # Read an image from `video` 26 | valid, img = video.read() 27 | if not valid: 28 | break 29 | if img.ndim >= 3: 30 | gray = cv.cvtColor(img, cv.COLOR_BGR2GRAY) 31 | else: 32 | gray = img.copy() 33 | 34 | # Extract Harris corners 35 | harris = cornerHarris(gray) 36 | corners = harris > 5e8 37 | 38 | # Show the corners on the image 39 | heatmap = np.dstack((np.zeros_like(corners), np.zeros_like(corners), corners*255)) 40 | heatmap = (0.3 * img + 0.7 * heatmap).astype(np.uint8) 41 | cv.imshow('Harris Cornerness', heatmap) 42 | key = cv.waitKey(1) 43 | if key == ord(' '): 44 | key = cv.waitKey() 45 | if key == 27: # ESC 46 | break -------------------------------------------------------------------------------- /examples/homography_estimation_implement.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import cv2 as cv 3 | 4 | def getPerspectiveTransform(src, dst): 5 | if len(src) == len(dst): 6 | # Make homogeneous coordiates if necessary 7 | if src.shape[1] == 2: 8 | src = np.hstack((src, np.ones((len(src), 1), dtype=src.dtype))) 9 | if dst.shape[1] == 2: 10 | dst = np.hstack((dst, np.ones((len(dst), 1), dtype=dst.dtype))) 11 | 12 | # Solve 'Ax = 0' 13 | A = [] 14 | for p, q in zip(src, dst): 15 | A.append([0, 0, 0, q[2]*p[0], q[2]*p[1], q[2]*p[2], -q[1]*p[0], -q[1]*p[1], -q[1]*p[2]]) 16 | A.append([q[2]*p[0], q[2]*p[1], q[2]*p[2], 0, 0, 0, -q[0]*p[0], -q[0]*p[1], -q[0]*p[2]]) 17 | _, _, Vt = np.linalg.svd(A, full_matrices=True) 18 | x = Vt[-1] 19 | 20 | # Reorganize `x` as a matrix 21 | H = x.reshape(3, -1) / x[-1] # Normalize the last element as 1 22 | return H 23 | 24 | if __name__ == '__main__': 25 | src = np.array([[115, 401], [776, 180], [330, 793], [1080, 383]], dtype=np.float32) 26 | dst = np.array([[0, 0], [900, 0], [0, 500], [900, 500]], dtype=np.float32) 27 | 28 | my_H = getPerspectiveTransform(src, dst) 29 | cv_H = cv.getPerspectiveTransform(src, dst) # Note) It accepts only 4 pairs of points. 30 | 31 | print('\n### My Planar Homography') 32 | print(my_H) 33 | print('\n### OpenCV Planar Homography') 34 | print(cv_H) -------------------------------------------------------------------------------- /examples/image_formation.cpp: -------------------------------------------------------------------------------- 1 | #include "opencv2/opencv.hpp" 2 | 3 | #define Rx(rx) (cv::Mat_(3, 3) << 1, 0, 0, 0, cos(rx), -sin(rx), 0, sin(rx), cos(rx)) 4 | #define Ry(ry) (cv::Mat_(3, 3) << cos(ry), 0, sin(ry), 0, 1, 0, -sin(ry), 0, cos(ry)) 5 | #define Rz(rz) (cv::Mat_(3, 3) << cos(rz), -sin(rz), 0, sin(rz), cos(rz), 0, 0, 0, 1) 6 | 7 | int main() 8 | { 9 | // The given camera configuration: Focal length, principal point, image resolution, position, and orientation 10 | double f = 1000, cx = 320, cy = 240, noise_std = 1; 11 | cv::Size img_res(640, 480); 12 | std::vector cam_pos = { cv::Point3d(0, 0, 0), cv::Point3d(-2, -2, 0), cv::Point3d(2, 2, 0), cv::Point3d(-2, 2, 0), cv::Point3d(2, -2, 0) }; 13 | std::vector cam_ori = { cv::Point3d(0, 0, 0), cv::Point3d(-CV_PI / 12, CV_PI / 12, 0), cv::Point3d(CV_PI / 12, -CV_PI / 12, 0), cv::Point3d(CV_PI / 12, CV_PI / 12, 0), cv::Point3d(-CV_PI / 12, -CV_PI / 12, 0) }; 14 | 15 | // Load a point cloud in the homogeneous coordinate 16 | FILE* fin = fopen("../data/box.xyz", "rt"); 17 | if (fin == NULL) return -1; 18 | cv::Mat X; 19 | while (!feof(fin)) 20 | { 21 | double x, y, z; 22 | if (fscanf(fin, "%lf %lf %lf", &x, &y, &z) == 3) X.push_back(cv::Vec4d(x, y, z, 1)); 23 | } 24 | fclose(fin); 25 | X = X.reshape(1).t(); // Convert to a 4 x N matrix 26 | 27 | // Generate images for each camera pose 28 | cv::Mat K = (cv::Mat_(3, 3) << f, 0, cx, 0, f, cy, 0, 0, 1); 29 | for (size_t i = 0; i < cam_pos.size(); i++) 30 | { 31 | // Derive a projection matrix 32 | cv::Mat Rc = Rz(cam_ori[i].z) * Ry(cam_ori[i].y) * Rx(cam_ori[i].x); 33 | cv::Mat pos(cam_pos[i]); 34 | cv::Mat Rt; 35 | cv::hconcat(Rc.t(), -Rc.t() * pos, Rt); 36 | cv::Mat P = K * Rt; 37 | 38 | // Project the points (c.f. OpenCV provides 'cv::projectPoints()' with consideration of distortion.) 39 | cv::Mat x = P * X; 40 | x.row(0) = x.row(0) / x.row(2); 41 | x.row(1) = x.row(1) / x.row(2); 42 | x.row(2) = 1; 43 | 44 | // Add Gaussian noise 45 | cv::Mat noise(2, x.cols, x.type()); 46 | cv::randn(noise, cv::Scalar(0), cv::Scalar(noise_std)); 47 | x.rowRange(0, 2) = x.rowRange(0, 2) + noise; 48 | 49 | // Show and save the points 50 | cv::Mat image = cv::Mat::zeros(img_res, CV_8UC1); 51 | for (int c = 0; c < x.cols; c++) 52 | { 53 | cv::Point p(x.col(c).rowRange(0, 2)); 54 | if (p.x >= 0 && p.x < img_res.width && p.y >= 0 && p.y < img_res.height) 55 | cv::circle(image, p, 2, 255, -1); 56 | } 57 | cv::imshow(cv::format("Image Formation %d", i), image); 58 | 59 | FILE* fout = fopen(cv::format("image_formation%d.xyz", i).c_str(), "wt"); 60 | if (fout == NULL) return -1; 61 | for (int c = 0; c < x.cols; c++) 62 | fprintf(fout, "%f %f 1\n", x.at(0, c), x.at(1, c)); 63 | fclose(fout); 64 | } 65 | 66 | cv::waitKey(0); 67 | return 0; 68 | } 69 | -------------------------------------------------------------------------------- /examples/image_formation.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import cv2 as cv 3 | from scipy.spatial.transform import Rotation 4 | 5 | # The given camera configuration: Focal length, principal point, image resolution, position, and orientation 6 | f, cx, cy, noise_std = 1000, 320, 240, 1 7 | img_res = (640, 480) 8 | cam_pos = [[0, 0, 0], [-2, -2, 0], [2, 2, 0], [-2, 2, 0], [2, -2, 0]] # Unit: [m] 9 | cam_ori = [[0, 0, 0], [-15 , 15, 0], [15, -15, 0], [15, 15, 0], [-15, -15, 0]] # Unit: [deg] 10 | 11 | # Load a point cloud in the homogeneous coordinate 12 | X = np.loadtxt('../data/box.xyz') # Size: N x 3 13 | 14 | # Generate images for each camera pose 15 | K = np.array([[f, 0, cx], [0, f, cy], [0, 0, 1]]) 16 | for i, (pos, ori) in enumerate(zip(cam_pos, cam_ori)): 17 | # Derive 'R' and 't' 18 | Rc = Rotation.from_euler('zyx', ori[::-1], degrees=True).as_matrix() 19 | R = Rc.T 20 | t = -Rc.T @ pos 21 | 22 | # Project the points (Alternative: `cv.projectPoints()`) 23 | x = K @ (R @ X.T + t.reshape(-1, 1)) # Size: 3 x N 24 | x /= x[-1] 25 | 26 | # Add Gaussian noise 27 | noise = np.random.normal(scale=noise_std, size=(2, len(X))) 28 | x[0:2,:] += noise 29 | 30 | # Show and save the points 31 | img = np.zeros(img_res[::-1], dtype=np.uint8) 32 | for c in range(x.shape[1]): 33 | cv.circle(img, x[0:2,c].astype(np.int32), 2, 255, -1) 34 | cv.imshow(f'Image Formation {i}', img) 35 | np.savetxt(f'image_formation{i}.xyz', x.T) # Size: N x 2 36 | 37 | cv.waitKey() 38 | cv.destroyAllWindows() 39 | -------------------------------------------------------------------------------- /examples/image_stitching.cpp: -------------------------------------------------------------------------------- 1 | #include "opencv2/opencv.hpp" 2 | 3 | int main() 4 | { 5 | // Load two images 6 | cv::Mat image1 = cv::imread("../data/hill01.jpg"); 7 | cv::Mat image2 = cv::imread("../data/hill02.jpg"); 8 | if (image1.empty() || image2.empty()) return -1; 9 | 10 | // Retrieve matching points 11 | cv::Ptr fdetector = cv::BRISK::create(); 12 | std::vector keypoint1, keypoint2; 13 | cv::Mat descriptor1, descriptor2; 14 | fdetector->detectAndCompute(image1, cv::Mat(), keypoint1, descriptor1); 15 | fdetector->detectAndCompute(image2, cv::Mat(), keypoint2, descriptor2); 16 | cv::Ptr fmatcher = cv::DescriptorMatcher::create("BruteForce-Hamming"); 17 | std::vector match; 18 | fmatcher->match(descriptor1, descriptor2, match); 19 | 20 | // Calculate planar homography and merge them 21 | std::vector points1, points2; 22 | for (size_t i = 0; i < match.size(); i++) 23 | { 24 | points1.push_back(keypoint1.at(match.at(i).queryIdx).pt); 25 | points2.push_back(keypoint2.at(match.at(i).trainIdx).pt); 26 | } 27 | cv::Mat inlier_mask; 28 | cv::Mat H = cv::findHomography(points2, points1, inlier_mask, cv::RANSAC); 29 | 30 | cv::Mat merged; 31 | cv::warpPerspective(image2, merged, H, cv::Size(image1.cols * 2, image1.rows)); 32 | merged.colRange(0, image1.cols) = image1 * 1; // Copy 33 | 34 | // Show the merged image 35 | cv::Mat original, matched; 36 | cv::drawMatches(image1, keypoint1, image2, keypoint2, match, matched, cv::Scalar::all(-1), cv::Scalar::all(-1), inlier_mask); // Remove 'inlier_mask' if you want to show all putative matches 37 | cv::hconcat(image1, image2, original); 38 | cv::vconcat(original, matched, matched); 39 | cv::vconcat(matched, merged, merged); 40 | cv::imshow("Planar Image Stitching", merged); 41 | cv::waitKey(0); 42 | return 0; 43 | } 44 | -------------------------------------------------------------------------------- /examples/image_stitching.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import cv2 as cv 3 | 4 | # Load two images 5 | img1 = cv.imread('../data/hill01.jpg') 6 | img2 = cv.imread('../data/hill02.jpg') 7 | assert (img1 is not None) and (img2 is not None), 'Cannot read the given images' 8 | 9 | # Retrieve matching points 10 | fdetector = cv.BRISK_create() 11 | keypoints1, descriptors1 = fdetector.detectAndCompute(img1, None) 12 | keypoints2, descriptors2 = fdetector.detectAndCompute(img2, None) 13 | 14 | fmatcher = cv.DescriptorMatcher_create('BruteForce-Hamming') 15 | match = fmatcher.match(descriptors1, descriptors2) 16 | 17 | # Calculate planar homography and merge them 18 | pts1, pts2 = [], [] 19 | for i in range(len(match)): 20 | pts1.append(keypoints1[match[i].queryIdx].pt) 21 | pts2.append(keypoints2[match[i].trainIdx].pt) 22 | pts1 = np.array(pts1, dtype=np.float32) 23 | pts2 = np.array(pts2, dtype=np.float32) 24 | 25 | H, inlier_mask = cv.findHomography(pts2, pts1, cv.RANSAC) 26 | img_merged = cv.warpPerspective(img2, H, (img1.shape[1]*2, img1.shape[0])) 27 | img_merged[:,:img1.shape[1]] = img1 # Copy 28 | 29 | # Show the merged image 30 | img_matched = cv.drawMatches(img1, keypoints1, img2, keypoints2, match, None, None, None, 31 | matchesMask=inlier_mask.ravel().tolist()) # Remove `matchesMask` if you want to show all putative matches 32 | merge = np.vstack((np.hstack((img1, img2)), img_matched, img_merged)) 33 | cv.imshow('Planar Image Stitching', merge) 34 | cv.waitKey(0) 35 | cv.destroyAllWindows() 36 | -------------------------------------------------------------------------------- /examples/image_stitching_implement.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import cv2 as cv 3 | import random 4 | from homography_estimation_implement import getPerspectiveTransform 5 | from image_warping_implement import warpPerspective2 6 | 7 | def evaluate_homography(H, p, q): 8 | p2q = H @ np.array([[p[0]], [p[1]], [1]]) 9 | p2q /= p2q[-1] 10 | return np.linalg.norm(p2q[:2].flatten() - q) 11 | 12 | def findHomography(src, dst, n_sample, ransac_trial, ransac_threshold): 13 | best_score = -1 14 | best_model = None 15 | for _ in range(ransac_trial): 16 | # Step 1: Hypothesis generation 17 | sample_idx = random.choices(range(len(src)), k=n_sample) 18 | model = getPerspectiveTransform(src[sample_idx], dst[sample_idx]) 19 | 20 | # Step 2: Hypothesis evaluation 21 | score = 0 22 | for (p, q) in zip(src, dst): 23 | error = evaluate_homography(model, p, q) 24 | if error < ransac_threshold: 25 | score += 1 26 | if score > best_score: 27 | best_score = score 28 | best_model = model 29 | 30 | # Generate the best inlier mask 31 | best_inlier_mask = np.zeros(len(src), dtype=np.uint8) 32 | for idx, (p, q) in enumerate(zip(src, dst)): 33 | error = evaluate_homography(best_model, p, q) 34 | if error < ransac_threshold: 35 | best_inlier_mask[idx] = 1 36 | 37 | return best_model, best_inlier_mask 38 | 39 | if __name__ == '__main__': 40 | # Load two images 41 | img1 = cv.imread('../data/hill01.jpg') 42 | img2 = cv.imread('../data/hill02.jpg') 43 | assert (img1 is not None) and (img2 is not None), 'Cannot read the given images' 44 | 45 | # Retrieve matching points 46 | fdetector = cv.BRISK_create() 47 | keypoints1, descriptors1 = fdetector.detectAndCompute(img1, None) 48 | keypoints2, descriptors2 = fdetector.detectAndCompute(img2, None) 49 | 50 | fmatcher = cv.DescriptorMatcher_create('BruteForce-Hamming') 51 | match = fmatcher.match(descriptors1, descriptors2) 52 | 53 | # Calculate planar homography and merge them 54 | pts1, pts2 = [], [] 55 | for i in range(len(match)): 56 | pts1.append(keypoints1[match[i].queryIdx].pt) 57 | pts2.append(keypoints2[match[i].trainIdx].pt) 58 | pts1 = np.array(pts1, dtype=np.float32) 59 | pts2 = np.array(pts2, dtype=np.float32) 60 | 61 | H, inlier_mask = findHomography(pts2, pts1, 4, 1000, 2) # log(1 - 0.999) / log(1 - 0.3^4) = 849 62 | img_merged = warpPerspective2(img2, H, (img1.shape[1]*2, img1.shape[0])) 63 | img_merged[:,:img1.shape[1]] = img1 # Copy 64 | 65 | # Show the merged image 66 | img_matched = cv.drawMatches(img1, keypoints1, img2, keypoints2, match, None, None, None, 67 | matchesMask=inlier_mask) # Remove `matchesMask` if you want to show all putative matches 68 | merge = np.vstack((np.hstack((img1, img2)), img_matched, img_merged)) 69 | cv.imshow(f'Planar Image Stitching with My RANSAC (score={sum(inlier_mask)})', merge) 70 | cv.waitKey(0) 71 | cv.destroyAllWindows() 72 | -------------------------------------------------------------------------------- /examples/image_warping_implement.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import cv2 as cv 3 | from homography_estimation_implement import getPerspectiveTransform 4 | 5 | def warpPerspective1(src, H, dst_size): 6 | # Generate an empty image 7 | width, height = dst_size 8 | channel = src.shape[2] if src.ndim > 2 else 1 9 | dst = np.zeros((height, width, channel), dtype=src.dtype) 10 | 11 | # Copy a pixel from `src` to `dst` (forword mapping) 12 | for py in range(img.shape[0]): 13 | for px in range(img.shape[1]): 14 | q = H @ [px, py, 1] 15 | qx, qy = int(q[0]/q[-1] + 0.5), int(q[1]/q[-1] + 0.5) 16 | if qx >= 0 and qy >= 0 and qx < width and qy < height: 17 | dst[qy, qx] = src[py, px] 18 | return dst 19 | 20 | def warpPerspective2(src, H, dst_size): 21 | # Generate an empty image 22 | width, height = dst_size 23 | channel = src.shape[2] if src.ndim > 2 else 1 24 | dst = np.zeros((height, width, channel), dtype=src.dtype) 25 | 26 | # Copy a pixel from `src` to `dst` (backward mapping) 27 | H_inv = np.linalg.inv(H) 28 | for qy in range(height): 29 | for qx in range(width): 30 | p = H_inv @ [qx, qy, 1] 31 | px, py = int(p[0]/p[-1] + 0.5), int(p[1]/p[-1] + 0.5) 32 | if px >= 0 and py >= 0 and px < src.shape[1] and py < src.shape[0]: 33 | dst[qy, qx] = src[py, px] 34 | return dst 35 | 36 | if __name__ == '__main__': 37 | img = cv.imread('../data/sunglok_card.jpg') 38 | wnd_name = 'Image Warping' 39 | card_size = (900, 480) 40 | pts_src = np.array([[95, 243], [743, 121], [157, 652], [969, 372]], dtype=np.float32) 41 | pts_dst = np.array([[0, 0], [card_size[0], 0], [0, card_size[1]], card_size], dtype=np.float32) 42 | 43 | # Find planar homography and transform the original image 44 | H = getPerspectiveTransform(pts_src, pts_dst) 45 | warp1 = warpPerspective1(img, H, card_size) 46 | warp2 = warpPerspective2(img, H, card_size) 47 | 48 | # Show images generated from two methods 49 | cv.imshow(wnd_name + ' (Method 1)', warp1) 50 | cv.imshow(wnd_name + ' (Method 2)', warp2) 51 | cv.waitKey(0) 52 | cv.destroyAllWindows() -------------------------------------------------------------------------------- /examples/line_fitting_m_estimator.cpp: -------------------------------------------------------------------------------- 1 | #include "opencv2/opencv.hpp" 2 | #include "ceres/ceres.h" 3 | 4 | // Convert a line format, [n_x, n_y, x_0, y_0] to [a, b, c] 5 | // c.f. A line model in OpenCV: n_x * (x - x_0) = n_y * (y - y_0) 6 | #define CONVERT_LINE(line) (cv::Vec3d(line[0], -line[1], -line[0] * line[2] + line[1] * line[3])) 7 | 8 | struct GeometricError 9 | { 10 | GeometricError(const cv::Point2d& pt) : datum(pt) { } 11 | 12 | template 13 | bool operator()(const T* const line, T* residual) const 14 | { 15 | residual[0] = (line[0] * T(datum.x) + line[1] * T(datum.y) + line[2]) / sqrt(line[0] * line[0] + line[1] * line[1]); 16 | return true; 17 | } 18 | 19 | private: 20 | const cv::Point2d datum; 21 | }; 22 | 23 | int main() 24 | { 25 | cv::Vec3d truth(1.0 / sqrt(2.0), 1.0 / sqrt(2.0), -240.0); // The line model: a*x + b*y + c = 0 (a^2 + b^2 = 1) 26 | double loss_width = 3.0; // 3 x 'data_inlier_noise'; if this value is less than equal to 0, M-estimator is disabled. 27 | int data_num = 1000; 28 | double data_inlier_ratio = 0.5, data_inlier_noise = 1.0; 29 | 30 | // Generate data 31 | std::vector data; 32 | cv::RNG rng; 33 | for (int i = 0; i < data_num; i++) 34 | { 35 | if (rng.uniform(0.0, 1.0) < data_inlier_ratio) 36 | { 37 | double x = rng.uniform(0.0, 480.0); 38 | double y = (truth[0] * x + truth[2]) / -truth[1]; 39 | x += rng.gaussian(data_inlier_noise); 40 | y += rng.gaussian(data_inlier_noise); 41 | data.push_back(cv::Point2d(x, y)); // Inlier 42 | } 43 | else data.push_back(cv::Point2d(rng.uniform(0.0, 640.0), rng.uniform(0.0, 480.0))); // Outlier 44 | } 45 | 46 | // Estimate a line using M-estimator 47 | cv::Vec3d opt_line(1, 0, 0); 48 | ceres::Problem problem; 49 | for (size_t i = 0; i < data.size(); i++) 50 | { 51 | ceres::CostFunction* cost_func = new ceres::AutoDiffCostFunction(new GeometricError(data[i])); 52 | ceres::LossFunction* loss_func = NULL; 53 | if (loss_width > 0) loss_func = new ceres::CauchyLoss(loss_width); 54 | problem.AddResidualBlock(cost_func, loss_func, opt_line.val); 55 | } 56 | ceres::Solver::Options options; 57 | options.linear_solver_type = ceres::ITERATIVE_SCHUR; 58 | options.num_threads = 8; 59 | options.minimizer_progress_to_stdout = true; 60 | ceres::Solver::Summary summary; 61 | ceres::Solve(options, &problem, &summary); 62 | std::cout << summary.FullReport() << std::endl; 63 | opt_line /= sqrt(opt_line[0] * opt_line[0] + opt_line[1] * opt_line[1]); // Normalize 64 | 65 | // Estimate a line using least squares method (for reference) 66 | cv::Vec4d nnxy; 67 | cv::fitLine(data, nnxy, cv::DIST_L2, 0, 0.01, 0.01); 68 | cv::Vec3d lsm_line = CONVERT_LINE(nnxy); 69 | 70 | // Display estimates 71 | printf("* The Truth: %.3f, %.3f, %.3f\n", truth[0], truth[1], truth[2]); 72 | printf("* Estimate (M-estimator): %.3f, %.3f, %.3f (Cost: %.3f)\n", opt_line[0], opt_line[1], opt_line[2], summary.final_cost / data.size()); 73 | printf("* Estimate (LSM): %.3f, %.3f, %.3f\n", lsm_line[0], lsm_line[1], lsm_line[2]); 74 | return 0; 75 | } 76 | -------------------------------------------------------------------------------- /examples/line_fitting_m_estimator.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import cv2 as cv 3 | from scipy.optimize import least_squares 4 | import matplotlib.pyplot as plt 5 | 6 | def geometric_error(line, pts): 7 | a, b, c = line 8 | err = [(a*x + b*y + c) / np.sqrt(a*a + b*b) for (x, y) in pts] 9 | return err 10 | 11 | if __name__ == '__main__': 12 | true_line = np.array([2, 3, -14]) / np.sqrt(2*2 + 3*3) # The line model: a*x + b*y + c = 0 (a^2 + b^2 = 1) 13 | data_range = np.array([-4, 12]) 14 | data_num = 100 15 | noise_std = 0.2 16 | outlier_ratio = 0.7 17 | 18 | # Generate noisy points with outliers 19 | line2y = lambda line, x: (line[0] * x + line[2]) / -line[1] # ax + by + c = 0 -> y = (ax + c) / -b 20 | y_range = sorted(line2y(true_line, data_range)) 21 | data = [] 22 | for _ in range(data_num): 23 | x = np.random.uniform(*data_range) 24 | if np.random.rand() < outlier_ratio: 25 | y = np.random.uniform(*y_range) 26 | else: 27 | y = line2y(true_line, x) 28 | x += np.random.normal(scale=noise_std) 29 | y += np.random.normal(scale=noise_std) 30 | data.append((x, y)) 31 | data = np.array(data) 32 | 33 | # Estimate a line using least squares with a robust kernel 34 | init_line = [1, 1, 0] 35 | result = least_squares(geometric_error, init_line, args=(data,), loss='huber', f_scale=0.3) 36 | mest_line = result['x'] / np.linalg.norm(result['x'][:2]) 37 | 38 | # Estimate a line using OpenCV (for reference) 39 | # Note) OpenCV line model: n_y * (x - x_0) = n_x * (y - y_0) 40 | nnxy = cv.fitLine(data, cv.DIST_L2, 0, 0.01, 0.01).flatten() 41 | lsqr_line = np.array([nnxy[1], -nnxy[0], -nnxy[1]*nnxy[2] + nnxy[0]*nnxy[3]]) 42 | nnxy = cv.fitLine(data, cv.DIST_HUBER, 0, 0.01, 0.01).flatten() 43 | huber_line = np.array([nnxy[1], -nnxy[0], -nnxy[1]*nnxy[2] + nnxy[0]*nnxy[3]]) 44 | 45 | # Plot the data and result 46 | plt.plot(data_range, line2y(true_line, data_range), 'r-', label='The true line') 47 | plt.plot(data[:,0], data[:,1], 'b.', label='Noisy data') 48 | plt.plot(data_range, line2y(mest_line, data_range), 'g-', label='M-estimator (Huber loss)') 49 | plt.plot(data_range, line2y(lsqr_line, data_range), 'm-', label='OpenCV (L2 loss)') 50 | plt.plot(data_range, line2y(huber_line, data_range), 'm:', label='OpenCV (Huber loss)') 51 | plt.legend() 52 | plt.xlim(data_range) 53 | plt.show() -------------------------------------------------------------------------------- /examples/line_fitting_ransac.cpp: -------------------------------------------------------------------------------- 1 | #include "opencv2/opencv.hpp" 2 | 3 | // Convert a line format, [n_x, n_y, x_0, y_0] to [a, b, c] 4 | // c.f. A line model in OpenCV: n_x * (x - x_0) = n_y * (y - y_0) 5 | #define CONVERT_LINE(line) (cv::Vec3d(line[0], -line[1], -line[0] * line[2] + line[1] * line[3])) 6 | 7 | int main() 8 | { 9 | cv::Vec3d truth(1.0 / sqrt(2.0), 1.0 / sqrt(2.0), -240.0); // The line model: a*x + b*y + c = 0 (a^2 + b^2 = 1) 10 | int ransac_trial = 50, ransac_n_sample = 2; 11 | double ransac_thresh = 3.0; // 3 x 'data_inlier_noise' 12 | int data_num = 1000; 13 | double data_inlier_ratio = 0.5, data_inlier_noise = 1.0; 14 | 15 | // Generate data 16 | std::vector data; 17 | cv::RNG rng; 18 | for (int i = 0; i < data_num; i++) 19 | { 20 | if (rng.uniform(0.0, 1.0) < data_inlier_ratio) 21 | { 22 | double x = rng.uniform(0.0, 480.0); 23 | double y = (truth[0] * x + truth[2]) / -truth[1]; 24 | x += rng.gaussian(data_inlier_noise); 25 | y += rng.gaussian(data_inlier_noise); 26 | data.push_back(cv::Point2d(x, y)); // Inlier 27 | } 28 | else data.push_back(cv::Point2d(rng.uniform(0.0, 640.0), rng.uniform(0.0, 480.0))); // Outlier 29 | } 30 | 31 | // Estimate a line using RANSAC 32 | int best_score = -1; 33 | cv::Vec3d best_line; 34 | for (int i = 0; i < ransac_trial; i++) 35 | { 36 | // Step 1: Hypothesis generation 37 | std::vector sample; 38 | for (int j = 1; j < ransac_n_sample; j++) 39 | { 40 | int index = rng.uniform(0, int(data.size())); 41 | sample.push_back(data[index]); 42 | } 43 | cv::Vec4d nnxy; 44 | cv::fitLine(sample, nnxy, cv::DIST_L2, 0, 0.01, 0.01); 45 | cv::Vec3d line = CONVERT_LINE(nnxy); 46 | 47 | // Step 2: Hypothesis evaluation 48 | int score = 0; 49 | for (size_t j = 0; j < data.size(); j++) 50 | { 51 | double error = fabs(line(0) * data[j].x + line(1) * data[j].y + line(2)); 52 | if (error < ransac_thresh) score++; 53 | } 54 | 55 | if (score > best_score) 56 | { 57 | best_score = score; 58 | best_line = line; 59 | } 60 | } 61 | 62 | // Estimate a line using least squares method (for reference) 63 | cv::Vec4d nnxy; 64 | cv::fitLine(data, nnxy, cv::DIST_L2, 0, 0.01, 0.01); 65 | cv::Vec3d lsm_line = CONVERT_LINE(nnxy); 66 | 67 | // Display estimates 68 | printf("* The Truth: %.3f, %.3f, %.3f\n", truth[0], truth[1], truth[2]); 69 | printf("* Estimate (RANSAC): %.3f, %.3f, %.3f (Score: %d)\n", best_line[0], best_line[1], best_line[2], best_score); 70 | printf("* Estimate (LSM): %.3f, %.3f, %.3f\n", lsm_line[0], lsm_line[1], lsm_line[2]); 71 | return 0; 72 | } 73 | -------------------------------------------------------------------------------- /examples/line_fitting_ransac.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import cv2 as cv 3 | import random 4 | import matplotlib.pyplot as plt 5 | 6 | def generate_line(pts): 7 | # Line model: y = ax + b 8 | a = (pts[1][1] - pts[0][1]) / (pts[1][0] - pts[0][0]) 9 | b = pts[0][1] - a * pts[0][0] 10 | 11 | # Line model: ax + by + c = 0 (a^2 + b^2 = 1) 12 | line = np.array([a, -1, b]) 13 | return line / np.linalg.norm(line[:2]) 14 | 15 | def evaluate_line(line, p): 16 | a, b, c = line 17 | x, y = p 18 | return np.fabs(a*x + b*y + c) 19 | 20 | def fit_line_ransac(data, n_sample, ransac_trial, ransac_threshold): 21 | best_score = -1 22 | best_model = None 23 | for _ in range(ransac_trial): 24 | # Step 1: Hypothesis generation 25 | sample = random.choices(data, k=n_sample) 26 | model = generate_line(sample) 27 | 28 | # Step 2: Hypothesis evaluation 29 | score = 0 30 | for p in data: 31 | error = evaluate_line(model, p) 32 | if error < ransac_threshold: 33 | score += 1 34 | if score > best_score: 35 | best_score = score 36 | best_model = model 37 | 38 | return best_model, best_score 39 | 40 | if __name__ == '__main__': 41 | true_line = np.array([2, 3, -14]) / np.sqrt(2*2 + 3*3) # The line model: a*x + b*y + c = 0 (a^2 + b^2 = 1) 42 | data_range = np.array([-4, 12]) 43 | data_num = 100 44 | noise_std = 0.2 45 | outlier_ratio = 0.7 46 | 47 | # Generate noisy points with outliers 48 | line2y = lambda line, x: (line[0] * x + line[2]) / -line[1] # ax + by + c = 0 -> y = (ax + c) / -b 49 | y_range = sorted(line2y(true_line, data_range)) 50 | data = [] 51 | for _ in range(data_num): 52 | x = np.random.uniform(*data_range) 53 | if np.random.rand() < outlier_ratio: 54 | y = np.random.uniform(*y_range) # Generate an outlier 55 | else: 56 | y = line2y(true_line, x) # Generate an inlier 57 | x += np.random.normal(scale=noise_std) # Add Gaussian noise 58 | y += np.random.normal(scale=noise_std) 59 | data.append((x, y)) 60 | data = np.array(data) 61 | 62 | # Estimate a line using RANSAC 63 | best_line, best_score = fit_line_ransac(data, 2, 100, 0.3) # log(1 - 0.999) / log(1 - 0.3^2) = 73 64 | 65 | # Estimate a line using OpenCV (for reference) 66 | # Note) OpenCV line model: n_y * (x - x_0) = n_x * (y - y_0) 67 | nnxy = cv.fitLine(data, cv.DIST_L2, 0, 0.01, 0.01).flatten() 68 | lsqr_line = np.array([nnxy[1], -nnxy[0], -nnxy[1]*nnxy[2] + nnxy[0]*nnxy[3]]) 69 | 70 | # Plot the data and result 71 | plt.plot(data_range, line2y(true_line, data_range), 'r-', label='The true line') 72 | plt.plot(data[:,0], data[:,1], 'b.', label='Noisy data') 73 | plt.plot(data_range, line2y(best_line, data_range), 'g-', label=f'RASAC (score={best_score})') 74 | plt.plot(data_range, line2y(lsqr_line, data_range), 'm-', label='Least square method') 75 | plt.legend() 76 | plt.xlim(data_range) 77 | plt.show() 78 | -------------------------------------------------------------------------------- /examples/object_localization.cpp: -------------------------------------------------------------------------------- 1 | #include "opencv2/opencv.hpp" 2 | 3 | #define DEG2RAD(v) (v * CV_PI / 180) 4 | #define Rx(rx) (cv::Matx33d(1, 0, 0, 0, cos(rx), -sin(rx), 0, sin(rx), cos(rx))) 5 | #define Ry(ry) (cv::Matx33d(cos(ry), 0, sin(ry), 0, 1, 0, -sin(ry), 0, cos(ry))) 6 | #define Rz(rz) (cv::Matx33d(cos(rz), -sin(rz), 0, sin(rz), cos(rz), 0, 0, 0, 1)) 7 | 8 | class MouseDrag 9 | { 10 | public: 11 | MouseDrag() : dragged(false) { } 12 | bool dragged; 13 | cv::Point xy_s, xy_e; 14 | }; 15 | 16 | void MouseEventHandler(int event, int x, int y, int flags, void* param) 17 | { 18 | // Change 'mouse_state' (given as 'param') according to the mouse 'event' 19 | if (param == NULL) return; 20 | MouseDrag* drag = (MouseDrag*)param; 21 | if (event == cv::EVENT_LBUTTONDOWN) 22 | { 23 | drag->dragged = true; 24 | drag->xy_s = cv::Point(x, y); 25 | drag->xy_e = cv::Point(0, 0); 26 | } 27 | else if (event == cv::EVENT_MOUSEMOVE) 28 | { 29 | if (drag->dragged) drag->xy_e = cv::Point(x, y); 30 | } 31 | else if (event == cv::EVENT_LBUTTONUP) 32 | { 33 | if (drag->dragged) 34 | { 35 | drag->dragged = false; 36 | drag->xy_e = cv::Point(x, y); 37 | } 38 | } 39 | } 40 | 41 | int main() 42 | { 43 | // The given image and its calibration data 44 | const char* img_file = "../data/daejeon_station.png"; 45 | double f = 810.5, cx = 480, cy = 270, L = 3.31; // Unit: [px], [px], [px], [m] 46 | cv::Point3d cam_ori(DEG2RAD(-18.7), DEG2RAD(-8.2), DEG2RAD(2.0)); // Unit: [deg] 47 | cv::Range grid_x(-2, 3), grid_z(5, 36); // Unit: [m] 48 | 49 | // Load an images 50 | cv::Mat img = cv::imread(img_file); 51 | if (img.empty()) return -1; 52 | 53 | // Register the mouse callback function 54 | MouseDrag drag; 55 | cv::namedWindow("Object Localization and Measurement"); 56 | cv::setMouseCallback("Object Localization and Measurement", MouseEventHandler, &drag); 57 | 58 | // Prepare the camera projection 59 | cv::Matx33d K(f, 0, cx, 0, f, cy, 0, 0, 1); 60 | cv::Matx33d Rc = Rz(cam_ori.z) * Ry(cam_ori.y) * Rx(cam_ori.x); 61 | cv::Point3d tc = cv::Point3d(0, -L, 0); 62 | cv::Matx33d R = Rc.t(); 63 | cv::Point3d t = -Rc.t() * tc; 64 | 65 | // Draw X- and Z-grids on the ground 66 | for (int z = grid_z.start; z <= grid_z.end; z++) 67 | { 68 | cv::Point3d p = K * (R * cv::Point3d(grid_x.start, 0, z) + t); 69 | cv::Point3d q = K * (R * cv::Point3d(grid_x.end, 0, z) + t); 70 | cv::line(img, cv::Point2d(p.x / p.z, p.y / p.z), cv::Point2d(q.x / q.z, q.y / q.z), cv::Vec3b(64, 128, 64), 1); 71 | } 72 | for (int x = grid_x.start; x <= grid_x.end; x++) 73 | { 74 | cv::Point3d p = K * (R * cv::Point3d(x, 0, grid_z.start) + t); 75 | cv::Point3d q = K * (R * cv::Point3d(x, 0, grid_z.end) + t); 76 | cv::line(img, cv::Point2d(p.x / p.z, p.y / p.z), cv::Point2d(q.x / q.z, q.y / q.z), cv::Vec3b(64, 128, 64), 1); 77 | } 78 | 79 | while (true) 80 | { 81 | cv::Mat img_copy = img.clone(); 82 | if (drag.xy_e.x > 0 && drag.xy_e.y > 0) 83 | { 84 | // Calculate object location and height 85 | cv::Point3d c = R.t() * cv::Point3d(drag.xy_s.x - cx, drag.xy_s.y - cy, f); 86 | if (c.y < DBL_EPSILON) continue; // Skip the degenerate case (beyond the horizon) 87 | cv::Point3d h = R.t() * cv::Point3d(drag.xy_e.x - cx, drag.xy_e.y - cy, f); 88 | double Z = c.z / c.y * L, X = c.x / c.y * L, H = (c.y / c.z - h.y / h.z) * Z; 89 | 90 | // Draw head/contact points and location/height 91 | cv::line(img_copy, drag.xy_s, drag.xy_e, cv::Vec3b(0, 0, 255), 2); 92 | cv::circle(img_copy, drag.xy_e, 4, cv::Vec3b(255, 0, 0), -1); 93 | cv::circle(img_copy, drag.xy_s, 4, cv::Vec3b(0, 255, 0), -1); 94 | cv::putText(img_copy, cv::format("X:%.2f, Z:%.2f, H:%.2f", X, Z, H), drag.xy_s + cv::Point(-20, 20), cv::FONT_HERSHEY_PLAIN, 1, cv::Vec3b(0, 255, 0)); 95 | } 96 | 97 | // Show the image 98 | cv::imshow("Object Localization and Measurement", img_copy); 99 | int key = cv::waitKey(1); 100 | if (key == 27) break; // ESC 101 | } 102 | return 0; 103 | } 104 | -------------------------------------------------------------------------------- /examples/object_localization.py: -------------------------------------------------------------------------------- 1 | import cv2 as cv 2 | import numpy as np 3 | from scipy.spatial.transform import Rotation 4 | 5 | def mouse_event_handler(event, x, y, flags, param): 6 | # Change 'mouse_state' (given as 'param') according to the mouse 'event' 7 | if event == cv.EVENT_LBUTTONDOWN: 8 | param['dragged'] = True 9 | param['xy_s'] = (x, y) 10 | param['xy_e'] = (0, 0) 11 | elif event == cv.EVENT_MOUSEMOVE: 12 | if param['dragged']: 13 | param['xy_e'] = (x, y) 14 | elif event == cv.EVENT_LBUTTONUP: 15 | if param['dragged']: 16 | param['dragged'] = False 17 | param['xy_e'] = (x, y) 18 | 19 | if __name__ == '__main__': 20 | # The given image and its calibration data 21 | img_file = '../data/daejeon_station.png' 22 | f, cx, cy, L = 810.5, 480, 270, 3.31 # Unit: [px], [px], [px], [m] 23 | cam_ori = [-18.7, -8.2, 2.0] # Unit: [deg] 24 | grid_x, grid_z = (-2, 3), (5, 36) # Unit: [m] 25 | 26 | # Load an image 27 | img = cv.imread(img_file) 28 | assert img is not None 29 | 30 | # Register the mouse callback function 31 | mouse_state = {'dragged': False, 'xy_s': (0, 0), 'xy_e': (0, 0)} 32 | cv.namedWindow('Object Localization and Measurement') 33 | cv.setMouseCallback('Object Localization and Measurement', mouse_event_handler, mouse_state) 34 | 35 | # Prepare the camera projection 36 | K = np.array([[f, 0, cx], [0, f, cy], [0, 0, 1]]) 37 | Rc = Rotation.from_euler('zyx', cam_ori[::-1], degrees=True).as_matrix() 38 | tc = np.array([0, -L, 0]) 39 | R = Rc.T 40 | t = -Rc.T @ tc.T 41 | 42 | # Draw X- and Z-grids on the ground 43 | for x in range(*grid_x): 44 | s, e = [x, 0, grid_z[0]], [x, 0, grid_z[1] - 1] 45 | p = K @ (R @ s + t) 46 | q = K @ (R @ e + t) 47 | cv.line(img, (int(p[0] / p[2]), int(p[1] / p[2])), (int(q[0] / q[2]), int(q[1] / q[2])), (64, 128, 64), 1) 48 | for z in range(*grid_z): 49 | s, e = [grid_x[0], 0, z], [grid_x[1] - 1, 0, z] 50 | p = K @ (R @ s + t) 51 | q = K @ (R @ e + t) 52 | cv.line(img, (int(p[0] / p[2]), int(p[1] / p[2])), (int(q[0] / q[2]), int(q[1] / q[2])), (64, 128, 64), 1) 53 | 54 | while True: 55 | img_copy = img.copy() 56 | if mouse_state['xy_e'][0] > 0 and mouse_state['xy_e'][1] > 0: 57 | # Calculate object location and height 58 | c = R.T @ [mouse_state['xy_s'][0] - cx, mouse_state['xy_s'][1] - cy, f] 59 | h = R.T @ [mouse_state['xy_e'][0] - cx, mouse_state['xy_e'][1] - cy, f] 60 | if c[1] < 1e-6: # Skip the degenerate case (beyond the horizon) 61 | continue 62 | X = c[0] / c[1] * L # Object location X [m] 63 | Z = c[2] / c[1] * L # Object location Y [m] 64 | H = (c[1] / c[2] - h[1] / h[2]) * Z # Object height [m] 65 | 66 | # Draw the head/contact points and location/height 67 | cv.line(img_copy, mouse_state['xy_s'], mouse_state['xy_e'], (0, 0, 255), 2) 68 | cv.circle(img_copy, mouse_state['xy_e'], 4, (255, 0, 0), -1) # Head point 69 | cv.circle(img_copy, mouse_state['xy_s'], 4, (0, 255, 0), -1) # Contact point 70 | info = f'X: {X:.3f}, Z: {Z:.3f}, H: {H:.3f}' 71 | cv.putText(img_copy, info, np.array(mouse_state['xy_s']) + (-20, 20), cv.FONT_HERSHEY_DUPLEX, 0.6, (0, 255, 0)) 72 | 73 | cv.imshow('Object Localization and Measurement', img_copy) 74 | key = cv.waitKey(10) 75 | if key == 27: # ESC 76 | break 77 | 78 | cv.destroyAllWindows() 79 | -------------------------------------------------------------------------------- /examples/perspective_correction.cpp: -------------------------------------------------------------------------------- 1 | #include "opencv2/opencv.hpp" 2 | 3 | void MouseEventHandler(int event, int x, int y, int flags, void* param) 4 | { 5 | if (event == cv::EVENT_LBUTTONDOWN) 6 | { 7 | // Add the point to the given vector 8 | std::vector *points_src = (std::vector *)param; 9 | points_src->push_back(cv::Point(x, y)); 10 | printf("A point (index: %zd) is selected at (%d, %d).\n", points_src->size() - 1, x, y); 11 | } 12 | } 13 | 14 | int main() 15 | { 16 | const char* input = "../data/sunglok_card.jpg"; 17 | cv::Size card_size(450, 250); 18 | 19 | // Prepare the rectified points 20 | std::vector points_dst; 21 | points_dst.push_back(cv::Point(0, 0)); 22 | points_dst.push_back(cv::Point(card_size.width, 0)); 23 | points_dst.push_back(cv::Point(0, card_size.height)); 24 | points_dst.push_back(cv::Point(card_size.width, card_size.height)); 25 | 26 | // Load an image 27 | cv::Mat original = cv::imread(input); 28 | if (original.empty()) return -1; 29 | 30 | // Get the matched points from a user's mouse 31 | std::vector points_src; 32 | cv::namedWindow("3DV Tutorial: Perspective Correction"); 33 | cv::setMouseCallback("3DV Tutorial: Perspective Correction", MouseEventHandler, &points_src); 34 | while (points_src.size() < 4) 35 | { 36 | cv::Mat display = original.clone(); 37 | cv::rectangle(display, cv::Rect(cv::Point(10, 10), card_size), cv::Vec3b(0, 0, 255), 2); 38 | size_t idx = cv::min(points_src.size(), points_dst.size() - 1); 39 | cv::circle(display, points_dst[idx] + cv::Point(10, 10), 5, cv::Vec3b(0, 255, 0), -1); 40 | cv::imshow("3DV Tutorial: Perspective Correction", display); 41 | if (cv::waitKey(1) == 27) break; // 'ESC' key: Exit 42 | } 43 | if (points_src.size() < 4) return -1; 44 | 45 | // Calculate planar homography and rectify perspective distortion 46 | cv::Mat H = cv::findHomography(points_src, points_dst); 47 | cv::Mat rectify; 48 | cv::warpPerspective(original, rectify, H, card_size); 49 | 50 | // Show the rectified image 51 | cv::imshow("3DV Tutorial: Perspective Correction", rectify); 52 | cv::waitKey(0); 53 | return 0; 54 | } 55 | -------------------------------------------------------------------------------- /examples/perspective_correction.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import cv2 as cv 3 | 4 | def mouse_event_handler(event, x, y, flags, param): 5 | if event == cv.EVENT_LBUTTONDOWN: 6 | param.append((x, y)) 7 | 8 | if __name__ == '__main__': 9 | img_file = '../data/sunglok_card.jpg' 10 | card_size = (450, 250) 11 | offset = 10 12 | 13 | # Prepare the rectified points 14 | pts_dst = np.array([[0, 0], [card_size[0], 0], [0, card_size[1]], [card_size[0], card_size[1]]]) 15 | 16 | # Load an image 17 | img = cv.imread(img_file) 18 | assert img is not None, 'Cannot read the given image, ' + img_file 19 | 20 | # Get the matched points from mouse clicks 21 | pts_src = [] 22 | wnd_name = 'Perspective Correction: Point Selection' 23 | cv.namedWindow(wnd_name) 24 | cv.setMouseCallback(wnd_name, mouse_event_handler, pts_src) 25 | while len(pts_src) < 4: 26 | img_display = img.copy() 27 | cv.rectangle(img_display, (offset, offset), (offset + card_size[0], offset + card_size[1]), (0, 0, 255), 2) 28 | idx = min(len(pts_src), len(pts_dst)) 29 | cv.circle(img_display, offset + pts_dst[idx], 5, (0, 255, 0), -1) 30 | cv.imshow(wnd_name, img_display) 31 | key = cv.waitKey(10) 32 | if key == 27: # ESC 33 | break 34 | 35 | if len(pts_src) == 4: 36 | # Calculate planar homography and rectify perspective distortion 37 | H, _ = cv.findHomography(np.array(pts_src), pts_dst) 38 | print(f'pts_src = {pts_src}') 39 | print(f'pts_dst = {pts_dst}') 40 | print(f'H = {H}') 41 | img_rectify = cv.warpPerspective(img, H, card_size) 42 | 43 | # Show the rectified image 44 | cv.imshow('Perspective Distortion Correction: Rectified Image', img_rectify) 45 | cv.waitKey(0) 46 | 47 | cv.destroyAllWindows() 48 | -------------------------------------------------------------------------------- /examples/pose_estimation_book1.cpp: -------------------------------------------------------------------------------- 1 | #include "opencv2/opencv.hpp" 2 | 3 | int main() 4 | { 5 | const char *video_file = "../data/blais.mp4", *cover_file = "../data/blais.jpg"; 6 | double f = 1000, cx = 320, cy = 240; 7 | size_t min_inlier_num = 100; 8 | 9 | cv::Ptr fdetector = cv::ORB::create(); 10 | cv::Ptr fmatcher = cv::DescriptorMatcher::create("BruteForce-Hamming"); 11 | 12 | // Load the object image and extract features 13 | cv::Mat obj_image = cv::imread(cover_file); 14 | if (obj_image.empty()) return -1; 15 | 16 | std::vector obj_keypoint; 17 | cv::Mat obj_descriptor; 18 | fdetector->detectAndCompute(obj_image, cv::Mat(), obj_keypoint, obj_descriptor); 19 | if (obj_keypoint.empty() || obj_descriptor.empty()) return -1; 20 | fmatcher->add(obj_descriptor); 21 | 22 | // Open a video 23 | cv::VideoCapture video; 24 | if (!video.open(video_file)) return -1; 25 | 26 | // Prepare a box for simple AR 27 | std::vector box_lower = { cv::Point3f(30, 145, 0), cv::Point3f(30, 200, 0), cv::Point3f(200, 200, 0), cv::Point3f(200, 145, 0) }; 28 | std::vector box_upper = { cv::Point3f(30, 145, -50), cv::Point3f(30, 200, -50), cv::Point3f(200, 200, -50), cv::Point3f(200, 145, -50) }; 29 | 30 | // Run pose estimation 31 | cv::Mat K = (cv::Mat_(3, 3) << f, 0, cx, 0, f, cy, 0, 0, 1); 32 | cv::Mat dist_coeff = cv::Mat::zeros(5, 1, CV_64F), rvec, tvec; 33 | while (true) 34 | { 35 | // Read an image from the video 36 | cv::Mat img; 37 | video >> img; 38 | if (img.empty()) break; 39 | 40 | // Extract features and match them to the object features 41 | std::vector img_keypoint; 42 | cv::Mat img_descriptor; 43 | fdetector->detectAndCompute(img, cv::Mat(), img_keypoint, img_descriptor); 44 | if (img_keypoint.empty() || img_descriptor.empty()) continue; 45 | std::vector match; 46 | fmatcher->match(img_descriptor, match); 47 | if (match.size() < min_inlier_num) continue; 48 | std::vector obj_points; 49 | std::vector obj_project, img_points; 50 | for (auto m = match.begin(); m < match.end(); m++) 51 | { 52 | obj_points.push_back(cv::Point3f(obj_keypoint[m->trainIdx].pt)); 53 | obj_project.push_back(obj_keypoint[m->trainIdx].pt); 54 | img_points.push_back(img_keypoint[m->queryIdx].pt); 55 | } 56 | 57 | // Determine whether each matched feature is an inlier or not 58 | std::vector inlier; 59 | cv::solvePnPRansac(obj_points, img_points, K, dist_coeff, rvec, tvec, false, 500, 2, 0.99, inlier); 60 | cv::Mat inlier_mask = cv::Mat::zeros(int(match.size()), 1, CV_8U); 61 | for (size_t i = 0; i < inlier.size(); i++) inlier_mask.at(inlier[i]) = 1; 62 | cv::Mat image_result; 63 | cv::drawMatches(img, img_keypoint, obj_image, obj_keypoint, match, image_result, cv::Vec3b(0, 0, 255), cv::Vec3b(0, 127, 0), inlier_mask); 64 | 65 | // Check whether inliers are enough or not 66 | size_t inlier_num = inlier.size(); 67 | if (inlier_num > min_inlier_num) 68 | { 69 | // Estimate camera pose with inliers 70 | std::vector obj_inlier; 71 | std::vector img_inlier; 72 | for (int idx = 0; idx < inlier_mask.rows; idx++) 73 | { 74 | if (inlier_mask.at(idx)) 75 | { 76 | obj_inlier.push_back(obj_points[idx]); 77 | img_inlier.push_back(img_points[idx]); 78 | } 79 | } 80 | cv::solvePnP(obj_inlier, img_inlier, K, dist_coeff, rvec, tvec); 81 | 82 | // Draw the box on the image 83 | cv::Mat line_lower, line_upper; 84 | cv::projectPoints(box_lower, rvec, tvec, K, dist_coeff, line_lower); 85 | cv::projectPoints(box_upper, rvec, tvec, K, dist_coeff, line_upper); 86 | line_lower.reshape(1).convertTo(line_lower, CV_32S); // Change 4 x 1 matrix (CV_64FC2) to 4 x 2 matrix (CV_32SC1) 87 | line_upper.reshape(1).convertTo(line_upper, CV_32S); // because 'cv::polylines()' only accepts 'CV_32S' depth. 88 | cv::polylines(image_result, line_lower, true, cv::Vec3b(255, 0, 0), 2); 89 | for (int i = 0; i < line_lower.rows; i++) 90 | cv::line(image_result, cv::Point(line_lower.row(i)), cv::Point(line_upper.row(i)), cv::Vec3b(0, 255, 0), 2); 91 | cv::polylines(image_result, line_upper, true, cv::Vec3b(0, 0, 255), 2); 92 | } 93 | 94 | // Show the image and process the key event 95 | cv::String info = cv::format("Inliers: %d (%d%%), Focal Length: %.0f", inlier_num, 100 * inlier_num / match.size(), K.at(0)); 96 | cv::putText(image_result, info, cv::Point(5, 15), cv::FONT_HERSHEY_PLAIN, 1, cv::Vec3b(0, 255, 0)); 97 | cv::imshow("Pose Estimation (Book)", image_result); 98 | int key = cv::waitKey(1); 99 | if (key == 32) key = cv::waitKey(); // Space 100 | if (key == 27) break; // ESC 101 | } 102 | 103 | video.release(); 104 | return 0; 105 | } 106 | -------------------------------------------------------------------------------- /examples/pose_estimation_book1.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import cv2 as cv 3 | 4 | video_file, cover_file = '../data/blais.mp4', '../data/blais.jpg' 5 | f, cx, cy = 1000, 320, 240 6 | min_inlier_num = 100 7 | 8 | fdetector = cv.ORB_create() 9 | fmatcher = cv.DescriptorMatcher_create('BruteForce-Hamming') 10 | 11 | # Load the object image and extract features 12 | obj_image = cv.imread(cover_file) 13 | assert obj_image is not None 14 | obj_keypoints, obj_descriptors = fdetector.detectAndCompute(obj_image, None) 15 | assert len(obj_keypoints) >= min_inlier_num 16 | fmatcher.add(obj_descriptors) 17 | 18 | # Open a video 19 | video = cv.VideoCapture(video_file) 20 | assert video.isOpened(), 'Cannot read the given video, ' + video_file 21 | 22 | # Prepare a box for simple AR 23 | box_lower = np.array([[30, 145, 0], [30, 200, 0], [200, 200, 0], [200, 145, 0]], dtype=np.float32) 24 | box_upper = np.array([[30, 145, -50], [30, 200, -50], [200, 200, -50], [200, 145, -50]], dtype=np.float32) 25 | 26 | # Run pose extimation 27 | K = np.array([[f, 0, cx], [0, f, cy], [0, 0, 1]], dtype=np.float32) 28 | dist_coeff = np.zeros(5) 29 | while True: 30 | # Read an image from the video 31 | valid, img = video.read() 32 | if not valid: 33 | break 34 | 35 | # Extract features and match them to the object features 36 | img_keypoints, img_descriptors = fdetector.detectAndCompute(img, None) 37 | match = fmatcher.match(img_descriptors, obj_descriptors) 38 | if len(match) < min_inlier_num: 39 | continue 40 | 41 | obj_pts, img_pts = [], [] 42 | for m in match: 43 | obj_pts.append(obj_keypoints[m.trainIdx].pt) 44 | img_pts.append(img_keypoints[m.queryIdx].pt) 45 | obj_pts = np.array(obj_pts, dtype=np.float32) 46 | obj_pts = np.hstack((obj_pts, np.zeros((len(obj_pts), 1), dtype=np.float32))) # Make 2D to 3D 47 | img_pts = np.array(img_pts, dtype=np.float32) 48 | 49 | # Deterimine whether each matched feature is an inlier or not 50 | ret, rvec, tvec, inliers = cv.solvePnPRansac(obj_pts, img_pts, K, dist_coeff, useExtrinsicGuess=False, 51 | iterationsCount=500, reprojectionError=2., confidence=0.99) 52 | inlier_mask = np.zeros(len(match), dtype=np.uint8) 53 | inlier_mask[inliers] = 1 54 | img_result = cv.drawMatches(img, img_keypoints, obj_image, obj_keypoints, match, None, (0, 0, 255), (0, 127, 0), inlier_mask) 55 | 56 | # Check whether inliers are enough or not 57 | inlier_num = sum(inlier_mask) 58 | if inlier_num > min_inlier_num: 59 | # Estimate camera pose with inliers 60 | ret, rvec, tvec = cv.solvePnP(obj_pts[inliers], img_pts[inliers], K, dist_coeff) 61 | 62 | # Draw the box on the image 63 | line_lower, _ = cv.projectPoints(box_lower, rvec, tvec, K, dist_coeff) 64 | line_upper, _ = cv.projectPoints(box_upper, rvec, tvec, K, dist_coeff) 65 | cv.polylines(img_result, [np.int32(line_lower)], True, (255, 0, 0), 2) 66 | cv.polylines(img_result, [np.int32(line_upper)], True, (0, 0, 255), 2) 67 | for b, t in zip(line_lower, line_upper): 68 | cv.line(img_result, np.int32(b.flatten()), np.int32(t.flatten()), (0, 255, 0), 2) 69 | 70 | # Show the image and process the key event 71 | info = f'Inliers: {inlier_num} ({inlier_num*100/len(match):.0f}), Focal length: {f}' 72 | cv.putText(img_result, info, (10, 25), cv.FONT_HERSHEY_DUPLEX, 0.6, (0, 255, 0)) 73 | cv.imshow('Pose Estimation (Book)', img_result) 74 | key = cv.waitKey(1) 75 | if key == ord(' '): 76 | key = cv.waitKey() 77 | if key == 27: # ESC 78 | break 79 | 80 | video.release() 81 | cv.destroyAllWindows() 82 | -------------------------------------------------------------------------------- /examples/pose_estimation_book2.cpp: -------------------------------------------------------------------------------- 1 | #include "opencv2/opencv.hpp" 2 | 3 | int main() 4 | { 5 | const char *video_file = "../data/blais.mp4", *cover_file = "../data/blais.jpg"; 6 | double f_init = 1000, cx_init = 320, cy_init = 240; 7 | size_t min_inlier_num = 100; 8 | 9 | // Load the object image and extract features 10 | cv::Mat obj_image = cv::imread(cover_file); 11 | if (obj_image.empty()) return -1; 12 | 13 | cv::Ptr fdetector = cv::ORB::create(); 14 | cv::Ptr fmatcher = cv::DescriptorMatcher::create("BruteForce-Hamming"); 15 | std::vector obj_keypoint; 16 | cv::Mat obj_descriptor; 17 | fdetector->detectAndCompute(obj_image, cv::Mat(), obj_keypoint, obj_descriptor); 18 | if (obj_keypoint.empty() || obj_descriptor.empty()) return -1; 19 | fmatcher->add(obj_descriptor); 20 | 21 | // Open a video 22 | cv::VideoCapture video; 23 | if (!video.open(video_file)) return -1; 24 | 25 | // Prepare a box for simple AR 26 | std::vector box_lower = { cv::Point3f(30, 145, 0), cv::Point3f(30, 200, 0), cv::Point3f(200, 200, 0), cv::Point3f(200, 145, 0) }; 27 | std::vector box_upper = { cv::Point3f(30, 145, -50), cv::Point3f(30, 200, -50), cv::Point3f(200, 200, -50), cv::Point3f(200, 145, -50) }; 28 | 29 | // Run pose estimation and camera calibration together 30 | cv::Mat K = (cv::Mat_(3, 3) << f_init, 0, cx_init, 0, f_init, cy_init, 0, 0, 1); 31 | cv::Mat dist_coeff = cv::Mat::zeros(5, 1, CV_64F), rvec, tvec; 32 | while (true) 33 | { 34 | // Read an image from the video 35 | cv::Mat img; 36 | video >> img; 37 | if (img.empty()) break; 38 | 39 | // Extract features and match them to the object features 40 | std::vector img_keypoint; 41 | cv::Mat img_descriptor; 42 | fdetector->detectAndCompute(img, cv::Mat(), img_keypoint, img_descriptor); 43 | if (img_keypoint.empty() || img_descriptor.empty()) continue; 44 | std::vector match; 45 | fmatcher->match(img_descriptor, match); 46 | if (match.size() < min_inlier_num) continue; 47 | std::vector obj_points; 48 | std::vector obj_project, img_points; 49 | for (auto m = match.begin(); m < match.end(); m++) 50 | { 51 | obj_points.push_back(cv::Point3f(obj_keypoint[m->trainIdx].pt)); 52 | obj_project.push_back(obj_keypoint[m->trainIdx].pt); 53 | img_points.push_back(img_keypoint[m->queryIdx].pt); 54 | } 55 | 56 | // Determine whether each matched feature is an inlier or not 57 | std::vector inlier; 58 | cv::solvePnPRansac(obj_points, img_points, K, dist_coeff, rvec, tvec, false, 500, 2, 0.99, inlier); 59 | cv::Mat inlier_mask = cv::Mat::zeros(int(match.size()), 1, CV_8U); 60 | for (size_t i = 0; i < inlier.size(); i++) inlier_mask.at(inlier[i]) = 1; 61 | cv::Mat image_result; 62 | cv::drawMatches(img, img_keypoint, obj_image, obj_keypoint, match, image_result, cv::Vec3b(0, 0, 255), cv::Vec3b(0, 127, 0), inlier_mask); 63 | 64 | // Check whether inliers are enough or not 65 | size_t inlier_num = inlier.size(); 66 | if (inlier_num > min_inlier_num) 67 | { 68 | // Calibrate the camera and estimate its pose with inliers 69 | std::vector obj_inlier; 70 | std::vector img_inlier; 71 | for (int idx = 0; idx < inlier_mask.rows; idx++) 72 | { 73 | if (inlier_mask.at(idx)) 74 | { 75 | obj_inlier.push_back(obj_points[idx]); 76 | img_inlier.push_back(img_points[idx]); 77 | } 78 | } 79 | std::vector rvecs, tvecs; 80 | cv::calibrateCamera(std::vector>(1, obj_inlier), std::vector>(1, img_inlier), img.size(), K, dist_coeff, rvecs, tvecs, 81 | cv::CALIB_FIX_ASPECT_RATIO | cv::CALIB_FIX_PRINCIPAL_POINT | cv::CALIB_ZERO_TANGENT_DIST | cv::CALIB_FIX_K1 | cv::CALIB_FIX_K2 | cv::CALIB_FIX_K3 | cv::CALIB_FIX_K4 | cv::CALIB_FIX_K5 | cv::CALIB_FIX_K6 | cv::CALIB_FIX_S1_S2_S3_S4 | cv::CALIB_FIX_TAUX_TAUY); 82 | rvec = rvecs[0].clone(); 83 | tvec = tvecs[0].clone(); 84 | 85 | // Draw the box on the image 86 | cv::Mat line_lower, line_upper; 87 | cv::projectPoints(box_lower, rvec, tvec, K, dist_coeff, line_lower); 88 | cv::projectPoints(box_upper, rvec, tvec, K, dist_coeff, line_upper); 89 | line_lower.reshape(1).convertTo(line_lower, CV_32S); // Change 4 x 1 matrix (CV_64FC2) to 4 x 2 matrix (CV_32SC1) 90 | line_upper.reshape(1).convertTo(line_upper, CV_32S); // because 'cv::polylines()' only accepts 'CV_32S' depth. 91 | cv::polylines(image_result, line_lower, true, cv::Vec3b(255, 0, 0), 2); 92 | for (int i = 0; i < line_lower.rows; i++) 93 | cv::line(image_result, cv::Point(line_lower.row(i)), cv::Point(line_upper.row(i)), cv::Vec3b(0, 255, 0), 2); 94 | cv::polylines(image_result, line_upper, true, cv::Vec3b(0, 0, 255), 2); 95 | } 96 | 97 | // Show the image and process the key event 98 | cv::String info = cv::format("Inliers: %d (%d%%), Focal Length: %.0f", inlier_num, 100 * inlier_num / match.size(), K.at(0)); 99 | cv::putText(image_result, info, cv::Point(5, 15), cv::FONT_HERSHEY_PLAIN, 1, cv::Vec3b(0, 255, 0)); 100 | cv::imshow("Pose Estimation (Book)", image_result); 101 | int key = cv::waitKey(1); 102 | if (key == 32) key = cv::waitKey(); // Space 103 | if (key == 27) break; // ESC 104 | } 105 | 106 | video.release(); 107 | return 0; 108 | } 109 | -------------------------------------------------------------------------------- /examples/pose_estimation_book2.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import cv2 as cv 3 | 4 | video_file, cover_file = '../data/blais.mp4', '../data/blais.jpg' 5 | f_init, cx_init, cy_init = 1000, 320, 240 6 | min_inlier_num = 100 7 | 8 | fdetector = cv.ORB_create() 9 | fmatcher = cv.DescriptorMatcher_create('BruteForce-Hamming') 10 | 11 | # Load the object image and extract features 12 | obj_image = cv.imread(cover_file) 13 | assert obj_image is not None 14 | obj_keypoints, obj_descriptors = fdetector.detectAndCompute(obj_image, None) 15 | assert len(obj_keypoints) >= min_inlier_num 16 | fmatcher.add(obj_descriptors) 17 | 18 | # Open a video 19 | video = cv.VideoCapture(video_file) 20 | assert video.isOpened(), 'Cannot read the given video, ' + video_file 21 | 22 | # Prepare a box for simple AR 23 | box_lower = np.array([[30, 145, 0], [30, 200, 0], [200, 200, 0], [200, 145, 0]], dtype=np.float32) 24 | box_upper = np.array([[30, 145, -50], [30, 200, -50], [200, 200, -50], [200, 145, -50]], dtype=np.float32) 25 | 26 | # Run pose extimation 27 | K = np.array([[f_init, 0, cx_init], [0, f_init, cy_init], [0, 0, 1]], dtype=np.float32) 28 | dist_coeff = np.zeros(5) 29 | calib_param = cv.CALIB_FIX_ASPECT_RATIO | cv.CALIB_FIX_PRINCIPAL_POINT | cv.CALIB_ZERO_TANGENT_DIST | cv.CALIB_FIX_K3 | cv.CALIB_FIX_K4 | cv.CALIB_FIX_K5 | cv.CALIB_FIX_S1_S2_S3_S4 | cv.CALIB_FIX_TAUX_TAUY 30 | while True: 31 | # Read an image from the video 32 | valid, img = video.read() 33 | if not valid: 34 | break 35 | 36 | # Extract features and match them to the object features 37 | img_keypoints, img_descriptors = fdetector.detectAndCompute(img, None) 38 | match = fmatcher.match(img_descriptors, obj_descriptors) 39 | if len(match) < min_inlier_num: 40 | continue 41 | 42 | obj_pts, img_pts = [], [] 43 | for m in match: 44 | obj_pts.append(obj_keypoints[m.trainIdx].pt) 45 | img_pts.append(img_keypoints[m.queryIdx].pt) 46 | obj_pts = np.array(obj_pts, dtype=np.float32) 47 | obj_pts = np.hstack((obj_pts, np.zeros((len(obj_pts), 1), dtype=np.float32))) # Make 2D to 3D 48 | img_pts = np.array(img_pts, dtype=np.float32) 49 | 50 | # Deterimine whether each matched feature is an inlier or not 51 | ret, rvec, tvec, inliers = cv.solvePnPRansac(obj_pts, img_pts, K, dist_coeff, useExtrinsicGuess=False, 52 | iterationsCount=500, reprojectionError=2., confidence=0.99) 53 | inlier_mask = np.zeros(len(match), dtype=np.uint8) 54 | inlier_mask[inliers] = 1 55 | img_result = cv.drawMatches(img, img_keypoints, obj_image, obj_keypoints, match, None, (0, 0, 255), (0, 127, 0), inlier_mask) 56 | 57 | # Check whether inliers are enough or not 58 | inlier_num = sum(inlier_mask) 59 | if inlier_num > min_inlier_num: 60 | # Calibrate the camera and estimate its pose with inliers 61 | ret, K, dist_coeff, rvecs, tvecs = cv.calibrateCamera([obj_pts[inliers]], [img_pts[inliers]], (img.shape[0], img.shape[1]), K, dist_coeff, None, None, calib_param) 62 | rvec, tvec = rvecs[0], tvecs[0] 63 | 64 | # Draw the box on the image 65 | line_lower, _ = cv.projectPoints(box_lower, rvec, tvec, K, dist_coeff) 66 | line_upper, _ = cv.projectPoints(box_upper, rvec, tvec, K, dist_coeff) 67 | cv.polylines(img_result, [np.int32(line_lower)], True, (255, 0, 0), 2) 68 | cv.polylines(img_result, [np.int32(line_upper)], True, (0, 0, 255), 2) 69 | for b, t in zip(line_lower, line_upper): 70 | cv.line(img_result, np.int32(b.flatten()), np.int32(t.flatten()), (0, 255, 0), 2) 71 | 72 | # Show the image and process the key event 73 | info = f'Inliers: {inlier_num} ({inlier_num*100/len(match):.0f}), Focal length: {K[0,0]:.0f}' 74 | cv.putText(img_result, info, (10, 25), cv.FONT_HERSHEY_DUPLEX, 0.6, (0, 255, 0)) 75 | cv.imshow('Pose Estimation (Book)', img_result) 76 | key = cv.waitKey(1) 77 | if key == ord(' '): 78 | key = cv.waitKey() 79 | if key == 27: # ESC 80 | break 81 | 82 | video.release() 83 | cv.destroyAllWindows() 84 | -------------------------------------------------------------------------------- /examples/pose_estimation_book3.cpp: -------------------------------------------------------------------------------- 1 | #include "opencv2/opencv.hpp" 2 | 3 | int main() 4 | { 5 | const char *video_file = "../data/blais.mp4", *cover_file = "../data/blais.jpg"; 6 | size_t min_inlier_num = 100; 7 | 8 | // Load the object image and extract features 9 | cv::Mat obj_image = cv::imread(cover_file); 10 | if (obj_image.empty()) return -1; 11 | 12 | cv::Ptr fdetector = cv::ORB::create(); 13 | cv::Ptr fmatcher = cv::DescriptorMatcher::create("BruteForce-Hamming"); 14 | std::vector obj_keypoint; 15 | cv::Mat obj_descriptor; 16 | fdetector->detectAndCompute(obj_image, cv::Mat(), obj_keypoint, obj_descriptor); 17 | if (obj_keypoint.empty() || obj_descriptor.empty()) return -1; 18 | fmatcher->add(obj_descriptor); 19 | 20 | // Open a video 21 | cv::VideoCapture video; 22 | if (!video.open(video_file)) return -1; 23 | 24 | // Prepare a box for simple AR 25 | std::vector box_lower = { cv::Point3f(30, 145, 0), cv::Point3f(30, 200, 0), cv::Point3f(200, 200, 0), cv::Point3f(200, 145, 0) }; 26 | std::vector box_upper = { cv::Point3f(30, 145, -50), cv::Point3f(30, 200, -50), cv::Point3f(200, 200, -50), cv::Point3f(200, 145, -50) }; 27 | 28 | // Run pose estimation and camera calibration together 29 | while (true) 30 | { 31 | // Read an image from the video 32 | cv::Mat img; 33 | video >> img; 34 | if (img.empty()) break; 35 | 36 | // Extract features and match them to the object features 37 | std::vector img_keypoint; 38 | cv::Mat img_descriptor; 39 | fdetector->detectAndCompute(img, cv::Mat(), img_keypoint, img_descriptor); 40 | if (img_keypoint.empty() || img_descriptor.empty()) continue; 41 | std::vector match; 42 | fmatcher->match(img_descriptor, match); 43 | if (match.size() < min_inlier_num) continue; 44 | std::vector obj_points; 45 | std::vector obj_project, img_points; 46 | for (auto m = match.begin(); m < match.end(); m++) 47 | { 48 | obj_points.push_back(cv::Point3f(obj_keypoint[m->trainIdx].pt)); 49 | obj_project.push_back(obj_keypoint[m->trainIdx].pt); 50 | img_points.push_back(img_keypoint[m->queryIdx].pt); 51 | } 52 | 53 | // Determine whether each matched feature is an inlier or not 54 | cv::Mat inlier_mask = cv::Mat::zeros(int(match.size()), 1, CV_8U); 55 | cv::Mat H = cv::findHomography(img_points, obj_project, inlier_mask, cv::RANSAC, 2); 56 | cv::Mat image_result; 57 | cv::drawMatches(img, img_keypoint, obj_image, obj_keypoint, match, image_result, cv::Vec3b(0, 0, 255), cv::Vec3b(0, 127, 0), inlier_mask); 58 | 59 | // Check whether inliers are enough or not 60 | double f = 0; 61 | size_t inlier_num = static_cast(cv::sum(inlier_mask)[0]); 62 | if (inlier_num > min_inlier_num) 63 | { 64 | // Calibrate the camera and estimate its pose with inliers 65 | std::vector obj_inlier; 66 | std::vector img_inlier; 67 | for (int idx = 0; idx < inlier_mask.rows; idx++) 68 | { 69 | if (inlier_mask.at(idx)) 70 | { 71 | obj_inlier.push_back(obj_points[idx]); 72 | img_inlier.push_back(img_points[idx]); 73 | } 74 | } 75 | cv::Mat K, dist_coeff; 76 | std::vector rvecs, tvecs; 77 | cv::calibrateCamera(std::vector>(1, obj_inlier), std::vector>(1, img_inlier), img.size(), K, dist_coeff, rvecs, tvecs, 78 | cv::CALIB_FIX_ASPECT_RATIO | cv::CALIB_FIX_PRINCIPAL_POINT | cv::CALIB_ZERO_TANGENT_DIST | cv::CALIB_FIX_K1 | cv::CALIB_FIX_K2 | cv::CALIB_FIX_K3 | cv::CALIB_FIX_K4 | cv::CALIB_FIX_K5 | cv::CALIB_FIX_K6 | cv::CALIB_FIX_S1_S2_S3_S4 | cv::CALIB_FIX_TAUX_TAUY); 79 | cv::Mat rvec = rvecs[0].clone(); 80 | cv::Mat tvec = tvecs[0].clone(); 81 | f = K.at(0); 82 | 83 | // Draw the box on the image 84 | cv::Mat line_lower, line_upper; 85 | cv::projectPoints(box_lower, rvec, tvec, K, dist_coeff, line_lower); 86 | cv::projectPoints(box_upper, rvec, tvec, K, dist_coeff, line_upper); 87 | line_lower.reshape(1).convertTo(line_lower, CV_32S); // Change 4 x 1 matrix (CV_64FC2) to 4 x 2 matrix (CV_32SC1) 88 | line_upper.reshape(1).convertTo(line_upper, CV_32S); // because 'cv::polylines()' only accepts 'CV_32S' depth. 89 | cv::polylines(image_result, line_lower, true, cv::Vec3b(255, 0, 0), 2); 90 | for (int i = 0; i < line_lower.rows; i++) 91 | cv::line(image_result, cv::Point(line_lower.row(i)), cv::Point(line_upper.row(i)), cv::Vec3b(0, 255, 0), 2); 92 | cv::polylines(image_result, line_upper, true, cv::Vec3b(0, 0, 255), 2); 93 | } 94 | 95 | // Show the image and process the key event 96 | cv::String info = cv::format("Inliers: %d (%d%%), Focal Length: %.0f", inlier_num, 100 * inlier_num / match.size(), f); 97 | cv::putText(image_result, info, cv::Point(5, 15), cv::FONT_HERSHEY_PLAIN, 1, cv::Vec3b(0, 255, 0)); 98 | cv::imshow("Pose Estimation (Book)", image_result); 99 | int key = cv::waitKey(1); 100 | if (key == 32) key = cv::waitKey(); // Space 101 | if (key == 27) break; // ESC 102 | } 103 | 104 | video.release(); 105 | return 0; 106 | } 107 | -------------------------------------------------------------------------------- /examples/pose_estimation_book3.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import cv2 as cv 3 | 4 | video_file, cover_file = '../data/blais.mp4', '../data/blais.jpg' 5 | min_inlier_num = 100 6 | 7 | fdetector = cv.ORB_create() 8 | fmatcher = cv.DescriptorMatcher_create('BruteForce-Hamming') 9 | 10 | # Load the object image and extract features 11 | obj_image = cv.imread(cover_file) 12 | assert obj_image is not None 13 | obj_keypoints, obj_descriptors = fdetector.detectAndCompute(obj_image, None) 14 | assert len(obj_keypoints) >= min_inlier_num 15 | fmatcher.add(obj_descriptors) 16 | 17 | # Open a video 18 | video = cv.VideoCapture(video_file) 19 | assert video.isOpened(), 'Cannot read the given video, ' + video_file 20 | 21 | # Prepare a box for simple AR 22 | box_lower = np.array([[30, 145, 0], [30, 200, 0], [200, 200, 0], [200, 145, 0]], dtype=np.float32) 23 | box_upper = np.array([[30, 145, -50], [30, 200, -50], [200, 200, -50], [200, 145, -50]], dtype=np.float32) 24 | 25 | # Run pose extimation 26 | calib_param = cv.CALIB_FIX_ASPECT_RATIO | cv.CALIB_FIX_PRINCIPAL_POINT | cv.CALIB_ZERO_TANGENT_DIST | cv.CALIB_FIX_K3 | cv.CALIB_FIX_K4 | cv.CALIB_FIX_K5 | cv.CALIB_FIX_S1_S2_S3_S4 | cv.CALIB_FIX_TAUX_TAUY 27 | while True: 28 | # Read an image from the video 29 | valid, img = video.read() 30 | if not valid: 31 | break 32 | 33 | # Extract features and match them to the object features 34 | img_keypoints, img_descriptors = fdetector.detectAndCompute(img, None) 35 | match = fmatcher.match(img_descriptors, obj_descriptors) 36 | if len(match) < min_inlier_num: 37 | continue 38 | 39 | obj_pts, img_pts = [], [] 40 | for m in match: 41 | obj_pts.append(obj_keypoints[m.trainIdx].pt) 42 | img_pts.append(img_keypoints[m.queryIdx].pt) 43 | obj_pts = np.array(obj_pts, dtype=np.float32) 44 | obj_pts = np.hstack((obj_pts, np.zeros((len(obj_pts), 1), dtype=np.float32))) # Make 2D to 3D 45 | img_pts = np.array(img_pts, dtype=np.float32) 46 | 47 | # Deterimine whether each matched feature is an inlier or not 48 | H, inlier_mask = cv.findHomography(obj_pts, img_pts, cv.RANSAC, 2) 49 | inlier_mask = inlier_mask.flatten() 50 | img_result = cv.drawMatches(img, img_keypoints, obj_image, obj_keypoints, match, None, (0, 0, 255), (0, 127, 0), inlier_mask) 51 | 52 | # Check whether inliers are enough or not 53 | inlier_num = sum(inlier_mask) 54 | if inlier_num > min_inlier_num: 55 | # Calibrate the camera and estimate its pose with inliers 56 | ret, K, dist_coeff, rvecs, tvecs = cv.calibrateCamera([obj_pts[inlier_mask.astype(bool)]], [img_pts[inlier_mask.astype(bool)]], (img.shape[0], img.shape[1]), None, None, None, None, calib_param) 57 | rvec, tvec = rvecs[0], tvecs[0] 58 | 59 | # Draw the box on the image 60 | line_lower, _ = cv.projectPoints(box_lower, rvec, tvec, K, dist_coeff) 61 | line_upper, _ = cv.projectPoints(box_upper, rvec, tvec, K, dist_coeff) 62 | cv.polylines(img_result, [np.int32(line_lower)], True, (255, 0, 0), 2) 63 | cv.polylines(img_result, [np.int32(line_upper)], True, (0, 0, 255), 2) 64 | for b, t in zip(line_lower, line_upper): 65 | cv.line(img_result, np.int32(b.flatten()), np.int32(t.flatten()), (0, 255, 0), 2) 66 | info = f'Inliers: {inlier_num} ({inlier_num*100/len(match):.0f}), Focal length: {K[0,0]:.0f}' 67 | cv.putText(img_result, info, (10, 25), cv.FONT_HERSHEY_DUPLEX, 0.6, (0, 255, 0)) 68 | 69 | # Show the image and process the key event 70 | cv.imshow('Pose Estimation (Book)', img_result) 71 | key = cv.waitKey(1) 72 | if key == ord(' '): 73 | key = cv.waitKey() 74 | if key == 27: # ESC 75 | break 76 | 77 | video.release() 78 | cv.destroyAllWindows() 79 | -------------------------------------------------------------------------------- /examples/pose_estimation_chessboard.cpp: -------------------------------------------------------------------------------- 1 | #include "opencv2/opencv.hpp" 2 | 3 | int main() 4 | { 5 | // The given video and calibration data 6 | const char* video_file = "../data/chessboard.avi"; 7 | cv::Matx33d K(432.7390364738057, 0, 476.0614994349778, 8 | 0, 431.2395555913084, 288.7602152621297, 9 | 0, 0, 1); 10 | std::vector dist_coeff = { -0.2852754904152874, 0.1016466459919075, -0.0004420196146339175, 0.0001149909868437517, -0.01803978785585194 }; 11 | cv::Size board_pattern(10, 7); 12 | double board_cellsize = 0.025; 13 | 14 | // Open a video 15 | cv::VideoCapture video; 16 | if (!video.open(video_file)) return -1; 17 | 18 | // Prepare a 3D box for simple AR 19 | std::vector box_lower = { cv::Point3d(4 * board_cellsize, 2 * board_cellsize, 0), cv::Point3d(5 * board_cellsize, 2 * board_cellsize, 0), cv::Point3d(5 * board_cellsize, 4 * board_cellsize, 0), cv::Point3d(4 * board_cellsize, 4 * board_cellsize, 0) }; 20 | std::vector box_upper = { cv::Point3d(4 * board_cellsize, 2 * board_cellsize, -board_cellsize), cv::Point3d(5 * board_cellsize, 2 * board_cellsize, -board_cellsize), cv::Point3d(5 * board_cellsize, 4 * board_cellsize, -board_cellsize), cv::Point3d(4 * board_cellsize, 4 * board_cellsize, -board_cellsize) }; 21 | 22 | // Prepare 3D points on a chessboard 23 | std::vector obj_points; 24 | for (int r = 0; r < board_pattern.height; r++) 25 | for (int c = 0; c < board_pattern.width; c++) 26 | obj_points.push_back(cv::Point3d(board_cellsize * c, board_cellsize * r, 0)); 27 | 28 | // Run pose estimation 29 | while (true) 30 | { 31 | // Grab an image from the video 32 | cv::Mat img; 33 | video >> img; 34 | if (img.empty()) break; 35 | 36 | // Estimate the camera pose 37 | std::vector img_points; 38 | bool success = cv::findChessboardCorners(img, board_pattern, img_points, cv::CALIB_CB_ADAPTIVE_THRESH + cv::CALIB_CB_NORMALIZE_IMAGE + cv::CALIB_CB_FAST_CHECK); 39 | if (success) 40 | { 41 | cv::Mat rvec, tvec; 42 | cv::solvePnP(obj_points, img_points, K, dist_coeff, rvec, tvec); 43 | 44 | // Draw the box on the image 45 | cv::Mat line_lower, line_upper; 46 | cv::projectPoints(box_lower, rvec, tvec, K, dist_coeff, line_lower); 47 | cv::projectPoints(box_upper, rvec, tvec, K, dist_coeff, line_upper); 48 | line_lower.reshape(1).convertTo(line_lower, CV_32S); // Change 4 x 1 matrix (CV_64FC2) to 4 x 2 matrix (CV_32SC1) 49 | line_upper.reshape(1).convertTo(line_upper, CV_32S); // Because 'cv::polylines()' only accepts 'CV_32S' depth. 50 | cv::polylines(img, line_lower, true, cv::Vec3b(255, 0, 0), 2); 51 | for (int i = 0; i < line_lower.rows; i++) 52 | cv::line(img, cv::Point(line_lower.row(i)), cv::Point(line_upper.row(i)), cv::Vec3b(0, 255, 0), 2); 53 | cv::polylines(img, line_upper, true, cv::Vec3b(0, 0, 255), 2); 54 | 55 | // Print the camera position 56 | cv::Mat R; 57 | cv::Rodrigues(rvec, R); 58 | cv::Mat p = -R.t() * tvec; 59 | cv::String info = cv::format("XYZ: [%.3f, %.3f, %.3f]", cv::Point3d(p)); 60 | cv::putText(img, info, cv::Point(5, 15), cv::FONT_HERSHEY_PLAIN, 1, cv::Vec3b(0, 255, 0)); 61 | } 62 | 63 | // Show the image and process the key event 64 | cv::imshow("Pose Estimation (Chessboard)", img); 65 | int key = cv::waitKey(1); 66 | if (key == 32) key = cv::waitKey(); // Space 67 | if (key == 27) break; // ESC 68 | } 69 | 70 | video.release(); 71 | return 0; 72 | } 73 | -------------------------------------------------------------------------------- /examples/pose_estimation_chessboard.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import cv2 as cv 3 | 4 | # The given video and calibration data 5 | video_file = '../data/chessboard.avi' 6 | K = np.array([[432.7390364738057, 0, 476.0614994349778], 7 | [0, 431.2395555913084, 288.7602152621297], 8 | [0, 0, 1]]) 9 | dist_coeff = np.array([-0.2852754904152874, 0.1016466459919075, -0.0004420196146339175, 0.0001149909868437517, -0.01803978785585194]) 10 | board_pattern = (10, 7) 11 | board_cellsize = 0.025 12 | board_criteria = cv.CALIB_CB_ADAPTIVE_THRESH + cv.CALIB_CB_NORMALIZE_IMAGE + cv.CALIB_CB_FAST_CHECK 13 | 14 | # Open a video 15 | video = cv.VideoCapture(video_file) 16 | assert video.isOpened(), 'Cannot read the given input, ' + video_file 17 | 18 | # Prepare a 3D box for simple AR 19 | box_lower = board_cellsize * np.array([[4, 2, 0], [5, 2, 0], [5, 4, 0], [4, 4, 0]]) 20 | box_upper = board_cellsize * np.array([[4, 2, -1], [5, 2, -1], [5, 4, -1], [4, 4, -1]]) 21 | 22 | # Prepare 3D points on a chessboard 23 | obj_points = board_cellsize * np.array([[c, r, 0] for r in range(board_pattern[1]) for c in range(board_pattern[0])]) 24 | 25 | # Run pose estimation 26 | while True: 27 | # Read an image from the video 28 | valid, img = video.read() 29 | if not valid: 30 | break 31 | 32 | # Estimate the camera pose 33 | success, img_points = cv.findChessboardCorners(img, board_pattern, board_criteria) 34 | if success: 35 | ret, rvec, tvec = cv.solvePnP(obj_points, img_points, K, dist_coeff) 36 | 37 | # Draw the box on the image 38 | line_lower, _ = cv.projectPoints(box_lower, rvec, tvec, K, dist_coeff) 39 | line_upper, _ = cv.projectPoints(box_upper, rvec, tvec, K, dist_coeff) 40 | cv.polylines(img, [np.int32(line_lower)], True, (255, 0, 0), 2) 41 | cv.polylines(img, [np.int32(line_upper)], True, (0, 0, 255), 2) 42 | for b, t in zip(line_lower, line_upper): 43 | cv.line(img, np.int32(b.flatten()), np.int32(t.flatten()), (0, 255, 0), 2) 44 | 45 | # Print the camera position 46 | R, _ = cv.Rodrigues(rvec) # Alternative) `scipy.spatial.transform.Rotation` 47 | p = (-R.T @ tvec).flatten() 48 | info = f'XYZ: [{p[0]:.3f} {p[1]:.3f} {p[2]:.3f}]' 49 | cv.putText(img, info, (10, 25), cv.FONT_HERSHEY_DUPLEX, 0.6, (0, 255, 0)) 50 | 51 | # Show the image and process the key event 52 | cv.imshow('Pose Estimation (Chessboard)', img) 53 | key = cv.waitKey(10) 54 | if key == ord(' '): 55 | key = cv.waitKey() 56 | if key == 27: # ESC 57 | break 58 | 59 | video.release() 60 | cv.destroyAllWindows() 61 | -------------------------------------------------------------------------------- /examples/pose_estimation_implement.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from scipy.optimize import least_squares 3 | from scipy.spatial.transform import Rotation 4 | import cv2 as cv 5 | 6 | def project_no_distort(X, rvec, t, K): 7 | R = Rotation.from_rotvec(rvec.flatten()).as_matrix() 8 | XT = X @ R.T + t # Transpose of 'X = R @ X + t' 9 | xT = XT @ K.T # Transpose of 'x = KX' 10 | xT = xT / xT[:,-1].reshape((-1, 1)) # Normalize 11 | return xT[:,0:2] 12 | 13 | def reproject_error_pnp(unknown, X, x, K): 14 | rvec, tvec = unknown[:3], unknown[3:] 15 | xp = project_no_distort(X, rvec, tvec, K) 16 | err = x - xp 17 | return err.ravel() 18 | 19 | def solvePnP(obj_pts, img_pts, K): 20 | unknown_init = np.array([0, 0, 0, 0, 0, 1.]) # Sequence: rvec(3), tvec(3) 21 | result = least_squares(reproject_error_pnp, unknown_init, args=(obj_pts, img_pts, K)) 22 | return result['success'], result['x'][:3], result['x'][3:] 23 | 24 | if __name__ == '__main__': 25 | f, cx, cy = 1000., 320., 240. 26 | obj_pts = np.loadtxt('../data/box.xyz') 27 | img_pts = np.loadtxt('../data/image_formation1.xyz')[:,:2].copy() 28 | K = np.array([[f, 0, cx], [0, f, cy], [0, 0, 1]]) 29 | dist_coeff = np.zeros(4) 30 | 31 | # Estimate camera pose 32 | _, rvec, tvec = solvePnP(obj_pts, img_pts, K) # Note) Ignore lens distortion 33 | R = Rotation.from_rotvec(rvec.flatten()).as_matrix() 34 | my_ori = Rotation.from_matrix(R.T).as_euler('xyz') 35 | my_pos = -R.T @ tvec 36 | 37 | # Estimate camera pose using OpenCV 38 | _, rvec, tvec = cv.solvePnP(obj_pts, img_pts, K, dist_coeff) 39 | R = Rotation.from_rotvec(rvec.flatten()).as_matrix() 40 | cv_ori = Rotation.from_matrix(R.T).as_euler('xyz') 41 | cv_pos = -R.T @ tvec.flatten() 42 | 43 | print('\n### Ground Truth') 44 | print('* Camera orientation: [-15, 15, 0] [deg]') 45 | print('* Camera position : [-2, -2, 0] [m]') 46 | print('\n### My Camera Pose') 47 | print(f'* Camera orientation: {np.rad2deg(my_ori)} [deg]') 48 | print(f'* Camera position : {my_pos} [m]') 49 | print('\n### OpenCV Camera Pose') 50 | print(f'* Camera orientation: {np.rad2deg(cv_ori)} [deg]') 51 | print(f'* Camera position : {cv_pos} [m]') -------------------------------------------------------------------------------- /examples/sfm.hpp: -------------------------------------------------------------------------------- 1 | #ifndef __SFM__ 2 | #define __SFM__ 3 | 4 | #include "opencv2/opencv.hpp" 5 | #include "ceres/ceres.h" 6 | #include "ceres/rotation.h" 7 | #include "bundle_adjustment.hpp" 8 | #include 9 | 10 | // Reprojection error for bundle adjustment with 7 DOF cameras 11 | // - 7 DOF = 3 DOF rotation + 3 DOF translation + 1 DOF focal length 12 | struct ReprojectionError7DOF 13 | { 14 | ReprojectionError7DOF(const cv::Point2d& _x, const cv::Point2d& _c) : x(_x), c(_c) { } 15 | 16 | template 17 | bool operator()(const T* const camera, const T* const point, T* residuals) const 18 | { 19 | // X' = R*X + t 20 | T X[3]; 21 | ceres::AngleAxisRotatePoint(camera, point, X); 22 | X[0] += camera[3]; 23 | X[1] += camera[4]; 24 | X[2] += camera[5]; 25 | 26 | // x' = K*X' 27 | const T& f = camera[6]; 28 | T x_p = f * X[0] / X[2] + c.x; 29 | T y_p = f * X[1] / X[2] + c.y; 30 | 31 | // residual = x - x' 32 | residuals[0] = T(x.x) - x_p; 33 | residuals[1] = T(x.y) - y_p; 34 | return true; 35 | } 36 | 37 | static ceres::CostFunction* create(const cv::Point2d& _x, const cv::Point2d& _c) 38 | { 39 | return (new ceres::AutoDiffCostFunction(new ReprojectionError7DOF(_x, _c))); 40 | } 41 | 42 | private: 43 | const cv::Point2d x; 44 | const cv::Point2d c; 45 | }; 46 | 47 | class SFM 48 | { 49 | public: 50 | 51 | typedef cv::Vec Vec9d; 52 | 53 | typedef std::unordered_map VisibilityGraph; 54 | 55 | static inline uint genKey(uint cam_idx, uint obs_idx) { return ((cam_idx << 16) + obs_idx); } 56 | 57 | static inline uint getCamIdx(uint key) { return ((key >> 16) & 0xFFFF); } 58 | 59 | static inline uint getObsIdx(uint key) { return (key & 0xFFFF); } 60 | 61 | static bool addCostFunc7DOF(ceres::Problem& problem, const cv::Point3d& X, const cv::Point2d& x, const Vec9d& camera, double loss_width = -1) 62 | { 63 | double* _X = (double*)(&(X.x)); 64 | double* _camera = (double*)(&(camera[0])); 65 | ceres::CostFunction* cost_func = ReprojectionError7DOF::create(x, cv::Point2d(camera[7], camera[8])); 66 | ceres::LossFunction* loss_func = NULL; 67 | if (loss_width > 0) loss_func = new ceres::CauchyLoss(loss_width); 68 | problem.AddResidualBlock(cost_func, loss_func, _camera, _X); 69 | return true; 70 | } 71 | 72 | static bool addCostFunc6DOF(ceres::Problem& problem, const cv::Point3d& X, const cv::Point2d& x, const Vec9d& camera, double loss_width = -1) 73 | { 74 | double* _X = (double*)(&(X.x)); 75 | double* _camera = (double*)(&(camera[0])); 76 | ceres::CostFunction* cost_func = ReprojectionError::create(x, camera[6], cv::Point2d(camera[7], camera[8])); 77 | ceres::LossFunction* loss_func = NULL; 78 | if (loss_width > 0) loss_func = new ceres::CauchyLoss(loss_width); 79 | problem.AddResidualBlock(cost_func, loss_func, _camera, _X); 80 | return true; 81 | } 82 | }; 83 | 84 | #endif // End of '__SFM__' 85 | -------------------------------------------------------------------------------- /examples/sfm_global.cpp: -------------------------------------------------------------------------------- 1 | #include "sfm.hpp" 2 | 3 | std::vector maskNoisyPoints(std::vector& Xs, const std::vector>& xs, const std::vector& views, const SFM::VisibilityGraph& visibility, double reproj_error2) 4 | { 5 | std::vector is_noisy(Xs.size(), false); 6 | if (reproj_error2 > 0) 7 | { 8 | for (auto visible = visibility.begin(); visible != visibility.end(); visible++) 9 | { 10 | cv::Point3d& X = Xs[visible->second]; 11 | if (X.z < 0) continue; 12 | int img_idx = SFM::getCamIdx(visible->first), pt_idx = SFM::getObsIdx(visible->first); 13 | const cv::Point2d& x = xs[img_idx][pt_idx].pt; 14 | const SFM::Vec9d& view = views[img_idx]; 15 | 16 | // Project the given 'X' 17 | cv::Vec3d rvec(view[0], view[1], view[2]); 18 | cv::Matx33d R; 19 | cv::Rodrigues(rvec, R); 20 | cv::Point3d X_p = R * X + cv::Point3d(view[3], view[4], view[5]); 21 | const double &f = view[6], &cx = view[7], &cy = view[8]; 22 | cv::Point2d x_p(f * X_p.x / X_p.z + cx, f * X_p.y / X_p.z + cy); 23 | 24 | // Calculate distance between 'x' and 'x_p' 25 | cv::Point2d d = x - x_p; 26 | if (d.x * d.x + d.y * d.y > reproj_error2) is_noisy[visible->second] = true; 27 | } 28 | } 29 | return is_noisy; 30 | } 31 | 32 | int main() 33 | { 34 | const char* input = "../data/relief/%02d.jpg"; 35 | double img_resize = 0.25, f_init = 500, cx_init = -1, cy_init = -1, Z_init = 2, Z_limit = 100, ba_loss_width = 9; // Negative 'loss_width' makes BA not to use a loss function. 36 | int min_inlier_num = 200, ba_num_iter = 200; // Negative 'ba_num_iter' uses the default value for BA minimization 37 | bool show_match = false; 38 | 39 | // Load images and extract features 40 | cv::VideoCapture video; 41 | if (!video.open(input)) return -1; 42 | cv::Ptr fdetector = cv::BRISK::create(); 43 | std::vector> img_keypoint; 44 | std::vector img_set, img_descriptor; 45 | while (true) 46 | { 47 | cv::Mat image; 48 | video >> image; 49 | if (image.empty()) break; 50 | if (img_resize != 1) cv::resize(image, image, cv::Size(), img_resize, img_resize); 51 | 52 | std::vector keypoint; 53 | cv::Mat descriptor; 54 | fdetector->detectAndCompute(image, cv::Mat(), keypoint, descriptor); 55 | img_set.push_back(image); 56 | img_keypoint.push_back(keypoint); 57 | img_descriptor.push_back(descriptor.clone()); 58 | } 59 | if (img_set.size() < 2) return -1; 60 | if (cx_init < 0) cx_init = img_set.front().cols / 2; 61 | if (cy_init < 0) cy_init = img_set.front().rows / 2; 62 | 63 | // Match features and find good matches 64 | cv::Ptr fmatcher = cv::DescriptorMatcher::create("BruteForce-Hamming"); 65 | std::vector> match_pair; // Good matches (image pairs) 66 | std::vector> match_inlier; // Good matches (inlier feature matches) 67 | for (size_t i = 0; i < img_set.size(); i++) 68 | { 69 | for (size_t j = i + 1; j < img_set.size(); j++) 70 | { 71 | // Match features of two image pair (i, j) and find their inliers 72 | std::vector match, inlier; 73 | fmatcher->match(img_descriptor[i], img_descriptor[j], match); 74 | std::vector src, dst; 75 | for (auto itr = match.begin(); itr != match.end(); itr++) 76 | { 77 | src.push_back(img_keypoint[i][itr->queryIdx].pt); 78 | dst.push_back(img_keypoint[j][itr->trainIdx].pt); 79 | } 80 | cv::Mat inlier_mask; 81 | cv::findFundamentalMat(src, dst, inlier_mask, cv::RANSAC); 82 | for (int k = 0; k < inlier_mask.rows; k++) 83 | if (inlier_mask.at(k)) inlier.push_back(match[k]); 84 | printf("3DV Tutorial: Image %zd - %zd are matched (%zd / %zd).\n", i, j, inlier.size(), match.size()); 85 | 86 | // Determine whether the image pair is good or not 87 | if (inlier.size() < min_inlier_num) continue; 88 | printf("3DV Tutorial: Image %zd - %zd are selected.\n", i, j); 89 | match_pair.push_back(std::make_pair(uint(i), uint(j))); 90 | match_inlier.push_back(inlier); 91 | if (show_match) 92 | { 93 | cv::Mat match_image; 94 | cv::drawMatches(img_set[i], img_keypoint[i], img_set[j], img_keypoint[j], match, match_image, cv::Scalar::all(-1), cv::Scalar::all(-1), inlier_mask); 95 | cv::imshow("3DV Tutorial: Structure-from-Motion", match_image); 96 | cv::waitKey(); 97 | } 98 | } 99 | } 100 | if (match_pair.size() < 1) return -1; 101 | 102 | // 1) Initialize cameras (rotation, translation, intrinsic parameters) 103 | std::vector cameras(img_set.size(), SFM::Vec9d(0, 0, 0, 0, 0, 0, f_init, cx_init, cy_init)); 104 | 105 | // 2) Initialize 3D points and build a visibility graph 106 | std::vector Xs; 107 | std::vector Xs_rgb; 108 | SFM::VisibilityGraph xs_visited; 109 | for (size_t m = 0; m < match_pair.size(); m++) 110 | { 111 | for (size_t in = 0; in < match_inlier[m].size(); in++) 112 | { 113 | const uint &cam1_idx = match_pair[m].first, &cam2_idx = match_pair[m].second; 114 | const uint &x1_idx = match_inlier[m][in].queryIdx, &x2_idx = match_inlier[m][in].trainIdx; 115 | const uint key1 = SFM::genKey(cam1_idx, x1_idx), key2 = SFM::genKey(cam2_idx, x2_idx); 116 | auto visit1 = xs_visited.find(key1), visit2 = xs_visited.find(key2); 117 | if (visit1 != xs_visited.end() && visit2 != xs_visited.end()) 118 | { 119 | // Remove previous observations if they are not consistent 120 | if (visit1->second != visit2->second) 121 | { 122 | xs_visited.erase(visit1); 123 | xs_visited.erase(visit2); 124 | } 125 | continue; // Skip if two observations are already visited 126 | } 127 | 128 | uint X_idx = 0; 129 | if (visit1 != xs_visited.end()) X_idx = visit1->second; 130 | else if (visit2 != xs_visited.end()) X_idx = visit2->second; 131 | else 132 | { 133 | // Add a new point if two observations are not visited 134 | X_idx = uint(Xs.size()); 135 | Xs.push_back(cv::Point3d(0, 0, Z_init)); 136 | Xs_rgb.push_back(img_set[cam1_idx].at(img_keypoint[cam1_idx][x1_idx].pt)); 137 | } 138 | if (visit1 == xs_visited.end()) xs_visited[key1] = X_idx; 139 | if (visit2 == xs_visited.end()) xs_visited[key2] = X_idx; 140 | } 141 | } 142 | printf("3DV Tutorial: # of 3D points: %zd\n", Xs.size()); 143 | 144 | // 3) Optimize camera pose and 3D points together (bundle adjustment) 145 | ceres::Problem ba; 146 | for (auto visit = xs_visited.begin(); visit != xs_visited.end(); visit++) 147 | { 148 | int cam_idx = SFM::getCamIdx(visit->first), x_idx = SFM::getObsIdx(visit->first); 149 | const cv::Point2d& x = img_keypoint[cam_idx][x_idx].pt; 150 | SFM::addCostFunc6DOF(ba, Xs[visit->second], x, cameras[cam_idx], ba_loss_width); 151 | } 152 | ceres::Solver::Options options; 153 | options.linear_solver_type = ceres::ITERATIVE_SCHUR; 154 | if (ba_num_iter > 0) options.max_num_iterations = ba_num_iter; 155 | options.num_threads = 8; 156 | options.minimizer_progress_to_stdout = true; 157 | ceres::Solver::Summary summary; 158 | ceres::Solve(options, &ba, &summary); 159 | std::cout << summary.FullReport() << std::endl; 160 | 161 | // Mark erroneous points to reject them 162 | std::vector is_noisy = maskNoisyPoints(Xs, img_keypoint, cameras, xs_visited, ba_loss_width); 163 | int num_noisy = std::accumulate(is_noisy.begin(), is_noisy.end(), 0); 164 | printf("3DV Tutorial: # of 3D points: %zd (Rejected: %d)\n", Xs.size(), num_noisy); 165 | for (size_t j = 0; j < cameras.size(); j++) 166 | printf("3DV Tutorial: Camera %zd's (f, cx, cy) = (%.3f, %.1f, %.1f)\n", j, cameras[j][6], cameras[j][7], cameras[j][8]); 167 | 168 | // Store the 3D points to an XYZ file 169 | FILE* fpts = fopen("sfm_global(point).xyz", "wt"); 170 | if (fpts == NULL) return -1; 171 | for (size_t i = 0; i < Xs.size(); i++) 172 | { 173 | if (Xs[i].z > -Z_limit && Xs[i].z < Z_limit && !is_noisy[i]) 174 | fprintf(fpts, "%f %f %f %d %d %d\n", Xs[i].x, Xs[i].y, Xs[i].z, Xs_rgb[i][2], Xs_rgb[i][1], Xs_rgb[i][0]); // Format: x, y, z, R, G, B 175 | } 176 | fclose(fpts); 177 | 178 | // Store the camera poses to an XYZ file 179 | FILE* fcam = fopen("sfm_global(camera).xyz", "wt"); 180 | if (fcam == NULL) return -1; 181 | for (size_t j = 0; j < cameras.size(); j++) 182 | { 183 | cv::Vec3d rvec(cameras[j][0], cameras[j][1], cameras[j][2]), t(cameras[j][3], cameras[j][4], cameras[j][5]); 184 | cv::Matx33d R; 185 | cv::Rodrigues(rvec, R); 186 | cv::Vec3d p = -R.t() * t; 187 | fprintf(fcam, "%f %f %f %f %f %f\n", p[0], p[1], p[2], R.t()(0, 2), R.t()(1, 2), R.t()(2, 2)); // Format: x, y, z, n_x, n_y, n_z 188 | } 189 | fclose(fcam); 190 | return 0; 191 | } 192 | -------------------------------------------------------------------------------- /examples/sfm_inc.py: -------------------------------------------------------------------------------- 1 | from pickletools import read_uint1 2 | from scipy.spatial.transform import Rotation 3 | from copy import deepcopy 4 | import cv2 5 | import numpy as np 6 | import time 7 | 8 | def get_camera_mat(cam_vec): 9 | f, cx, cy = cam_vec[0], cam_vec[1], cam_vec[2] 10 | return np.array([[f, 0, cx], [0, f, cy], [0, 0, 1]], dtype=np.float32) 11 | 12 | def update_camera_pose(cam_vec, R, t): 13 | # 14 | t = t.squeeze() 15 | rvec = Rotation.from_matrix(R).as_rotvec() 16 | result = np.array([cam_vec[0], cam_vec[1], cam_vec[2], rvec[0], rvec[1], rvec[2], t[0], t[1], t[2]], dtype=np.float32) 17 | return result 18 | 19 | def get_projection_mat(cam_vec): 20 | K = get_camera_mat(cam_vec=cam_vec) 21 | R = Rotation.from_rotvec(cam_vec[3:6]).as_matrix() 22 | t = cam_vec[6:9, np.newaxis] 23 | Rt = np.hstack((R, t)) 24 | result = K @ Rt 25 | return result 26 | 27 | def isBadPoint(point_3d, camera1, camera2, Z_limit, max_cos_parallax): 28 | # 무슨 내용인지 모름 29 | if point_3d[2] < -Z_limit or point_3d[2] > Z_limit: 30 | return True 31 | rvec1, rvec2 = np.array([camera1[3], camera1[4], camera1[5]], dtype=np.float32), np.array([camera2[3], camera2[4], camera2[5]], dtype=np.float32) 32 | R1, R2 = Rotation.from_rotvec(rvec1).as_matrix(), Rotation.from_rotvec(rvec2).as_matrix() 33 | # rot_vec to rot_mat 34 | t1, t2 = np.array([[camera1[6], camera1[7], camera1[8]]], dtype=np.float32), np.array([[camera2[6], camera2[7], camera2[8]]], dtype=np.float32) 35 | p1 = R1 @ point_3d[:, np.newaxis] + t1.T 36 | p2 = R2 @ point_3d[:, np.newaxis] + t2.T 37 | if p1[2,0] <= 0 or p2[2,0] <= 0 : return True 38 | v2 = R1 @ R2.T @ p2 39 | cos_parallax = p1.T @ v2 / (np.linalg.norm(p1) * np.linalg.norm(v2)) 40 | if cos_parallax > max_cos_parallax: return True 41 | return False 42 | 43 | 44 | def main(): 45 | img_path = "./bin/data/relief/%02d.jpg" 46 | img_resize = 0.25 47 | f_init, cx_init, cy_init, Z_init, Z_limit = 500, -1, -1, 2, 100 48 | ba_loss_width = 9 49 | max_cos_parallax = np.cos(10*np.pi / 180) 50 | min_inlier_num, ba_num_iter = 200, 200 51 | SHOW_MATCH = False 52 | 53 | # Load images and extract features 54 | img_keypoints = [] 55 | img_descriptors = [] 56 | img_set = [] 57 | detector = cv2.BRISK_create() 58 | cam = cv2.VideoCapture(img_path) 59 | 60 | while True: 61 | _, img = cam.read() 62 | if img is None: break 63 | img = cv2.resize(img, dsize=(0, 0), fx=img_resize, fy=img_resize) 64 | img_keypoint, img_descriptor = detector.detectAndCompute(img, None) 65 | img_keypoints.append(img_keypoint) 66 | img_descriptors.append(img_descriptor) 67 | img_set.append(img) 68 | cam.release() 69 | 70 | if cx_init < 0: cx_init = int(img_set[0].shape[1] / 2) 71 | if cy_init < 0: cy_init = int(img_set[0].shape[0] / 2) 72 | 73 | img_keypoints = np.array(img_keypoints, dtype=object) 74 | img_descriptors = np.array(img_descriptors, dtype=object) 75 | img_set = np.array(img_set) 76 | 77 | # Match features and find good matches 78 | fmatcher = cv2.DescriptorMatcher_create("BruteForce-Hamming") 79 | match_pair, match_inlier = [], [] 80 | for i in range(len(img_set)): 81 | for j in range(i+1, len(img_set)): 82 | src, dst, inlier = [], [], [] 83 | match = fmatcher.match(img_descriptors[i], img_descriptors[j]) 84 | match = np.array(match) 85 | for m in match: 86 | src.append(img_keypoints[i][m.queryIdx].pt) 87 | dst.append(img_keypoints[j][m.trainIdx].pt) 88 | 89 | src = np.array(src, dtype=np.float32) 90 | dst = np.array(dst, dtype=np.float32) 91 | 92 | F, inlier_mask = cv2.findFundamentalMat(src, dst, cv2.RANSAC) # inlier_mask = 3x3 93 | for k in range(len(inlier_mask)): 94 | if inlier_mask[k]: 95 | inlier.append(match[k]) # 96 | inlier = np.array(inlier) 97 | print(f"3DV Tutorial: Image {i} - {j} are matched ({inlier.size} / {match.size}).\n") 98 | 99 | # Determin whether the image pair is good or not 100 | if inlier.size < min_inlier_num: continue 101 | print(f"3DV Tutorial: Image {i} - {j} are selected.\n") 102 | match_pair.append((i, j)) 103 | match_inlier.append(inlier) 104 | 105 | if SHOW_MATCH: 106 | match_image = cv2.drawMatches(img_set[i], img_keypoints[i], img_set[j], img_keypoints[j], match, (0, 255, 0), (255, 0, 0), matchesMask=inlier_mask) 107 | cv2.imshow("3DV Tutorial: Structure-from-Motion", match_image) 108 | cv2.waitKey() 109 | if len(match_pair) < 1: return 110 | 111 | # Start Initialize cameras(cam_params, rotation, translation) 112 | cameras = np.full((img_set.shape[0], 9), np.array([f_init, cx_init, cy_init, 0, 0, 0, 0, 0, 0]), dtype=np.float32) 113 | best_pair = 0 114 | best_score = [i for i in range(len(match_inlier))] 115 | best_points_3d = None 116 | while True: 117 | # 1) Select the best pair 118 | for i in best_score: 119 | if i > best_score[best_pair]: 120 | best_pair = i 121 | if best_score[best_pair] == 0: 122 | print("3DV Tutorial: There is no good match. Try again after reducing 'max_cos_parallax\n") 123 | return -1 124 | best_cam0, best_cam1 = match_pair[best_pair][0], match_pair[best_pair][1] 125 | 126 | # 2) Estimate relative pose from the best two view (epipolar geometry) 127 | src, dst = [], [] 128 | for itr in match_inlier[best_pair]: 129 | src.append(img_keypoints[best_cam0][itr.queryIdx].pt) 130 | dst.append(img_keypoints[best_cam1][itr.trainIdx].pt) 131 | src = np.array(src, dtype=np.float32) 132 | dst = np.array(dst, dtype=np.float32) 133 | E, inlier_mask = cv2.findEssentialMat(src, dst, f_init, (cx_init, cy_init), cv2.RANSAC, 0.999, 1.0) 134 | inlier_num, R, t, mask = cv2.recoverPose(E, src, dst) 135 | 136 | for r in range(len(inlier_mask)-1, -1, -1): # 왜 뒤에서 부터 한 것일까... 137 | if not inlier_mask[r]: 138 | # Remove additionally detected ouliers 139 | src = np.delete(src, r, axis=0) 140 | dst = np.delete(dst, r, axis=0) 141 | match_inlier[best_pair] = np.delete(match_inlier[best_pair], r) 142 | cameras[best_cam0] = update_camera_pose(cameras[best_cam0], R, t) 143 | 144 | # 3) Reconstruct 3D points of the best two views (triangulation) 145 | P0, P1 = get_projection_mat(cameras[best_cam0]), get_projection_mat(cameras[best_cam1]) 146 | best_points_3d = cv2.triangulatePoints(P0, P1, src.T, dst.T) 147 | best_points_3d = best_points_3d.T 148 | best_points_3d[0] /= best_points_3d[3] 149 | best_points_3d[1] /= best_points_3d[3] 150 | best_points_3d[2] /= best_points_3d[3] 151 | 152 | best_score[best_pair] = 0 153 | for best_point_3d in best_points_3d: 154 | if isBadPoint(best_point_3d[:3], cameras[best_cam0], cameras[best_cam1], Z_limit, max_cos_parallax): continue 155 | best_score[best_pair] += 1 156 | print(f"3DV Tutorial: Image {best_cam0} - {best_cam1} were checked as the best match (# of inliers = {len(match_inlier[best_pair])}, # of good points = {best_score[best_pair]})") 157 | if best_score[best_pair] > 100: break 158 | 159 | # End Initialize cameras 160 | best_cam0 = match_pair[best_pair] 161 | 162 | # Prepare the initial 3D points 163 | 164 | # Start Prepare BA 165 | while True: 166 | # 4) Select the next image to add 167 | 168 | # 5) Estimate relative pose of the next view (PnP) 169 | 170 | # 6) Reconstruct newly observed 3D points (triangulation) 171 | 172 | # 7) Optimize camera pose and 3D points together (bundle adjustment) 173 | break 174 | # End Prepare BA 175 | 176 | 177 | # Store the 3D points to an XYZ file 178 | # points_3d_name = "sfm_global(point).xyz" 179 | # with open(points_3d_name, "wt") as f: 180 | # for i in range(n_point_3d): 181 | # data = f"{opt_points_3d[i,0]:.3f} {opt_points_3d[i,1]:.3f} {opt_points_3d[i,2]:.3f}\n" 182 | # f.write(data) 183 | 184 | # points_rgb_name = "sfm_global(rgb).xyz" 185 | # with open(points_rgb_name, "wt") as f: 186 | # for i in range(n_point_3d): 187 | # data = f"{points_rgb[i][0]:.3f} {points_rgb[i][1]:.3f} {points_rgb[i][2]:.3f}\n" 188 | # f.write(data) 189 | 190 | # camera_file = "sfm_global(camera).xyz" 191 | # with open(camera_file, 'wt') as f: 192 | # for i in range(n_cameras): 193 | # data = f"{opt_cam_params[i, 0]:.3f} {opt_cam_params[i, 1]:.3f} {opt_cam_params[i, 2]:.3f} {opt_cam_params[i, 3]:.3f} {opt_cam_params[i, 4]:.3f} {opt_cam_params[i, 5]:.3f}\n" 194 | # f.write(data) 195 | 196 | # print("END!!!") 197 | if __name__ == "__main__": 198 | main() -------------------------------------------------------------------------------- /examples/triangulation.cpp: -------------------------------------------------------------------------------- 1 | #include "opencv2/opencv.hpp" 2 | 3 | int main() 4 | { 5 | double f = 1000, cx = 320, cy = 240; 6 | const char *pts0_file = "../data/image_formation0.xyz", *pts1_file = "../data/image_formation1.xyz"; 7 | const char *output_file = "triangulation.xyz"; 8 | 9 | // Load 2D points observed from two views 10 | std::vector pts0, pts1; 11 | FILE* fin0 = fopen(pts0_file, "rt"); 12 | FILE* fin1 = fopen(pts1_file, "rt"); 13 | if (fin0 == NULL || fin1 == NULL) return -1; 14 | while (!feof(fin0) || !feof(fin1)) 15 | { 16 | double x, y, w; 17 | if (!feof(fin0) && fscanf(fin0, "%lf %lf %lf", &x, &y, &w) == 3) 18 | pts0.push_back(cv::Point2d(x, y)); 19 | if (!feof(fin1) && fscanf(fin1, "%lf %lf %lf", &x, &y, &w) == 3) 20 | pts1.push_back(cv::Point2d(x, y)); 21 | } 22 | fclose(fin0); 23 | fclose(fin1); 24 | if (pts0.size() != pts1.size()) return -1; 25 | 26 | // Estimate relative pose of two views 27 | cv::Mat F = cv::findFundamentalMat(pts0, pts1, cv::FM_8POINT); 28 | cv::Mat K = (cv::Mat_(3, 3) << f, 0, cx, 0, f, cy, 0, 0, 1); 29 | cv::Mat E = K.t() * F * K; 30 | cv::Mat R, t; 31 | cv::recoverPose(E, pts0, pts1, K, R, t); 32 | 33 | // Reconstruct 3D points (triangulation) 34 | cv::Mat P0 = K * cv::Mat::eye(3, 4, CV_64F); 35 | cv::Mat Rt, X; 36 | cv::hconcat(R, t, Rt); 37 | cv::Mat P1 = K * Rt; 38 | cv::triangulatePoints(P0, P1, pts0, pts1, X); 39 | X.row(0) = X.row(0) / X.row(3); 40 | X.row(1) = X.row(1) / X.row(3); 41 | X.row(2) = X.row(2) / X.row(3); 42 | X.row(3) = 1; 43 | 44 | // Store the 3D points 45 | FILE* fout = fopen(output_file, "wt"); 46 | if (fout == NULL) return -1; 47 | for (int c = 0; c < X.cols; c++) 48 | fprintf(fout, "%f %f %f\n", X.at(0, c), X.at(1, c), X.at(2, c)); 49 | fclose(fout); 50 | return 0; 51 | } 52 | -------------------------------------------------------------------------------- /examples/triangulation.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib.pyplot as plt 3 | import cv2 as cv 4 | 5 | f, cx, cy = 1000., 320., 240. 6 | pts0 = np.loadtxt('../data/image_formation0.xyz')[:,:2] 7 | pts1 = np.loadtxt('../data/image_formation1.xyz')[:,:2] 8 | output_file = 'triangulation.xyz' 9 | 10 | # Estimate relative pose of two view 11 | F, _ = cv.findFundamentalMat(pts0, pts1, cv.FM_8POINT) 12 | K = np.array([[f, 0, cx], [0, f, cy], [0, 0, 1]]) 13 | E = K.T @ F @ K 14 | _, R, t, _ = cv.recoverPose(E, pts0, pts1) 15 | 16 | # Reconstruct 3D points (triangulation) 17 | P0 = K @ np.eye(3, 4, dtype=np.float32) 18 | Rt = np.hstack((R, t)) 19 | P1 = K @ Rt 20 | X = cv.triangulatePoints(P0, P1, pts0.T, pts1.T) 21 | X /= X[3] 22 | X = X.T 23 | 24 | # Write the reconstructed 3D points 25 | np.savetxt(output_file, X) 26 | 27 | # Visualize the reconstructed 3D points 28 | ax = plt.figure(layout='tight').add_subplot(projection='3d') 29 | ax.plot(X[:,0], X[:,1], X[:,2], 'ro') 30 | ax.set_aspect('equal') 31 | ax.set_xlabel('X [m]') 32 | ax.set_ylabel('Y [m]') 33 | ax.set_zlabel('Z [m]') 34 | ax.grid(True) 35 | plt.show() -------------------------------------------------------------------------------- /examples/triangulation_implement.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import cv2 as cv 3 | 4 | def triangulatePoints(P0, P1, pts0, pts1): 5 | Xs = [] 6 | for (p, q) in zip(pts0.T, pts1.T): 7 | # Solve 'AX = 0' 8 | A = np.vstack((p[0] * P0[2] - P0[0], 9 | p[1] * P0[2] - P0[1], 10 | q[0] * P1[2] - P1[0], 11 | q[1] * P1[2] - P1[1])) 12 | _, _, Vt = np.linalg.svd(A, full_matrices=True) 13 | Xs.append(Vt[-1]) 14 | return np.vstack(Xs).T 15 | 16 | 17 | 18 | if __name__ == '__main__': 19 | f, cx, cy = 1000., 320., 240. 20 | pts0 = np.loadtxt('../data/image_formation0.xyz')[:,:2] 21 | pts1 = np.loadtxt('../data/image_formation1.xyz')[:,:2] 22 | output_file = 'triangulation_implement.xyz' 23 | 24 | # Estimate relative pose of two view 25 | F, _ = cv.findFundamentalMat(pts0, pts1, cv.FM_8POINT) 26 | K = np.array([[f, 0, cx], [0, f, cy], [0, 0, 1]]) 27 | E = K.T @ F @ K 28 | _, R, t, _ = cv.recoverPose(E, pts0, pts1) 29 | 30 | # Reconstruct 3D points (triangulation) 31 | P0 = K @ np.eye(3, 4, dtype=np.float32) 32 | Rt = np.hstack((R, t)) 33 | P1 = K @ Rt 34 | X = triangulatePoints(P0, P1, pts0.T, pts1.T) 35 | X /= X[3] 36 | X = X.T 37 | 38 | # Write the reconstructed 3D points 39 | np.savetxt(output_file, X) -------------------------------------------------------------------------------- /examples/video_stabilization.cpp: -------------------------------------------------------------------------------- 1 | #include "opencv2/opencv.hpp" 2 | 3 | int main() 4 | { 5 | // Open a video and get the reference image and feature points 6 | cv::VideoCapture video; 7 | if (!video.open("../data/traffic.avi")) return -1; 8 | 9 | cv::Mat gray_ref; 10 | video >> gray_ref; 11 | if (gray_ref.empty()) 12 | { 13 | video.release(); 14 | return -1; 15 | } 16 | if (gray_ref.channels() > 1) cv::cvtColor(gray_ref, gray_ref, cv::COLOR_RGB2GRAY); 17 | 18 | std::vector point_ref; 19 | cv::goodFeaturesToTrack(gray_ref, point_ref, 2000, 0.01, 10); 20 | if (point_ref.size() < 4) 21 | { 22 | video.release(); 23 | return -1; 24 | } 25 | 26 | // Run and show video stabilization 27 | while (true) 28 | { 29 | // Grab an image from `video` 30 | cv::Mat image, gray; 31 | video >> image; 32 | if (image.empty()) break; 33 | if (image.channels() > 1) cv::cvtColor(image, gray, cv::COLOR_RGB2GRAY); 34 | else gray = image.clone(); 35 | 36 | // Extract optical flow and calculate planar homography 37 | std::vector point; 38 | std::vector status; 39 | cv::Mat err, inlier_mask; 40 | cv::calcOpticalFlowPyrLK(gray_ref, gray, point_ref, point, status, err); 41 | cv::Mat H = cv::findHomography(point, point_ref, inlier_mask, cv::RANSAC); 42 | 43 | // Synthesize a stabilized image 44 | cv::Mat warp; 45 | cv::warpPerspective(image, warp, H, cv::Size(image.cols, image.rows)); 46 | 47 | // Show the original and rectified images together 48 | for (int i = 0; i < point_ref.size(); i++) 49 | { 50 | cv::Vec3b color = cv::Vec3b(0, 127, 0); 51 | if (inlier_mask.at(i) > 0) color = cv::Vec3b(0, 0, 255); 52 | cv::line(image, point_ref[i], point[i], color); 53 | } 54 | cv::hconcat(image, warp, image); 55 | cv::imshow("2D Video Stabilization", image); 56 | int key = cv::waitKey(1); 57 | if (key == 32) key = cv::waitKey(); // Space 58 | if (key == 27) break; // ESC 59 | } 60 | 61 | video.release(); 62 | return 0; 63 | } 64 | -------------------------------------------------------------------------------- /examples/video_stabilization.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import cv2 as cv 3 | 4 | # Open a video and get the reference image and feature points 5 | video = cv.VideoCapture('../data/traffic.avi') 6 | assert video.isOpened() 7 | 8 | _, gray_ref = video.read() 9 | assert gray_ref.size > 0 10 | if gray_ref.ndim >= 3: 11 | gray_ref = cv.cvtColor(gray_ref, cv.COLOR_BGR2GRAY) 12 | pts_ref = cv.goodFeaturesToTrack(gray_ref, 2000, 0.01, 10) 13 | 14 | # Run and show video stabilization 15 | while True: 16 | # Read an image from `video` 17 | valid, img = video.read() 18 | if not valid: 19 | break 20 | if img.ndim >= 3: 21 | gray = cv.cvtColor(img, cv.COLOR_BGR2GRAY) 22 | else: 23 | gray = img.copy() 24 | 25 | # Extract optical flow and calculate planar homography 26 | pts, status, error = cv.calcOpticalFlowPyrLK(gray_ref, gray, pts_ref, None) 27 | H, inlier_mask = cv.findHomography(pts, pts_ref, cv.RANSAC) 28 | 29 | # Synthesize a stabilized image 30 | warp = cv.warpPerspective(img, H, (img.shape[1], img.shape[0])) 31 | 32 | # Show the original and stabilized images together 33 | for pt, pt_ref, inlier in zip(pts, pts_ref, inlier_mask): 34 | color = (0, 0, 255) if inlier else (0, 127, 0) 35 | cv.line(img, pt.flatten().astype(np.int32), pt_ref.flatten().astype(np.int32), color) 36 | cv.imshow('2D Video Stabilization', np.hstack((img, warp))) 37 | key = cv.waitKey(1) 38 | if key == ord(' '): 39 | key = cv.waitKey() 40 | if key == 27: # ESC 41 | break 42 | 43 | video.release() 44 | cv.destroyAllWindows() 45 | -------------------------------------------------------------------------------- /examples/vo_epipolar.cpp: -------------------------------------------------------------------------------- 1 | #include "opencv2/opencv.hpp" 2 | 3 | int main() 4 | { 5 | const char* video_file = "../data/KITTI07/image_0/%06d.png"; 6 | double f = 707.0912; 7 | cv::Point2d c(601.8873, 183.1104); 8 | bool use_5pt = true; 9 | int min_inlier_num = 100; 10 | double min_inlier_ratio = 0.2; 11 | const char* traj_file = "vo_epipolar.xyz"; 12 | 13 | // Open a video and get an initial image 14 | cv::VideoCapture video; 15 | if (!video.open(video_file)) return -1; 16 | 17 | cv::Mat gray_prev; 18 | video >> gray_prev; 19 | if (gray_prev.empty()) 20 | { 21 | video.release(); 22 | return -1; 23 | } 24 | if (gray_prev.channels() > 1) cv::cvtColor(gray_prev, gray_prev, cv::COLOR_RGB2GRAY); 25 | 26 | // Run the monocular visual odometry 27 | cv::Mat K = (cv::Mat_(3, 3) << f, 0, c.x, 0, f, c.y, 0, 0, 1); 28 | cv::Mat camera_pose = cv::Mat::eye(4, 4, CV_64F); 29 | FILE* camera_traj = fopen(traj_file, "wt"); 30 | if (camera_traj == NULL) return -1; 31 | while (true) 32 | { 33 | // Grab an image from the video 34 | cv::Mat img, gray; 35 | video >> img; 36 | if (img.empty()) break; 37 | if (img.channels() > 1) cv::cvtColor(img, gray, cv::COLOR_RGB2GRAY); 38 | else gray = img.clone(); 39 | 40 | // Extract optical flow 41 | std::vector pts_prev, pts; 42 | cv::goodFeaturesToTrack(gray_prev, pts_prev, 2000, 0.01, 10); 43 | std::vector status; 44 | cv::Mat err; 45 | cv::calcOpticalFlowPyrLK(gray_prev, gray, pts_prev, pts, status, err); 46 | gray_prev = gray; 47 | 48 | // Calculate relative pose 49 | cv::Mat E, inlier_mask; 50 | if (use_5pt) 51 | { 52 | E = cv::findEssentialMat(pts_prev, pts, f, c, cv::RANSAC, 0.99, 1, inlier_mask); 53 | } 54 | else 55 | { 56 | cv::Mat F = cv::findFundamentalMat(pts_prev, pts, cv::FM_RANSAC, 1, 0.99, inlier_mask); 57 | E = K.t() * F * K; 58 | } 59 | cv::Mat R, t; 60 | int inlier_num = cv::recoverPose(E, pts_prev, pts, R, t, f, c, inlier_mask); 61 | double inlier_ratio = inlier_num / pts.size(); 62 | 63 | // Accumulate relative pose if result is reliable 64 | cv::Vec3b info_color(0, 255, 0); 65 | if ((inlier_num > min_inlier_num) && (inlier_ratio > min_inlier_ratio)) 66 | { 67 | cv::Mat T = cv::Mat::eye(4, 4, R.type()); 68 | T(cv::Rect(0, 0, 3, 3)) = R * 1.0; 69 | T.col(3).rowRange(0, 3) = t * 1.0; 70 | camera_pose = camera_pose * T.inv(); 71 | info_color = cv::Vec3b(0, 0, 255); 72 | } 73 | 74 | // Show the image and write camera pose 75 | if (img.channels() < 3) cv::cvtColor(img, img, cv::COLOR_GRAY2RGB); 76 | for (int i = 0; i < pts_prev.size(); i++) 77 | { 78 | if (inlier_mask.at(i) > 0) cv::line(img, pts_prev[i], pts[i], cv::Vec3b(0, 0, 255)); 79 | else cv::line(img, pts_prev[i], pts[i], cv::Vec3b(0, 127, 0)); 80 | } 81 | double x = camera_pose.at(0, 3), y = camera_pose.at(1, 3), z = camera_pose.at(2, 3); 82 | cv::String info = cv::format("Inliers: %d (%d%%), XYZ: [%.3f, %.3f, %.3f]", inlier_num, inlier_ratio*100, x, y, z); 83 | cv::putText(img, info, cv::Point(5, 15), cv::FONT_HERSHEY_PLAIN, 1, info_color); 84 | cv::imshow("Monocular Visual Odometry (Epipolar)", img); 85 | fprintf(camera_traj, "%.6f %.6f %.6f\n", x, y, z); 86 | int key = cv::waitKey(1); 87 | if (key == 32) key = cv::waitKey(); // Space 88 | if (key == 27) break; // ESC 89 | } 90 | 91 | fclose(camera_traj); 92 | video.release(); 93 | return 0; 94 | } 95 | -------------------------------------------------------------------------------- /examples/vo_epipolar.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib.pyplot as plt 3 | import cv2 as cv 4 | 5 | video_file = '../data/KITTI07/image_0/%06d.png' 6 | f, cx, cy = 707.0912, 601.8873, 183.1104 7 | use_5pt = True 8 | min_inlier_num = 100 9 | min_inlier_ratio = 0.2 10 | traj_file = 'vo_epipolar.xyz' 11 | 12 | # Open a video and get an initial image 13 | video = cv.VideoCapture(video_file) 14 | assert video.isOpened() 15 | 16 | _, gray_prev = video.read() 17 | assert gray_prev.size > 0 18 | if gray_prev.ndim >= 3 and gray_prev.shape[2] > 1: 19 | gray_prev = cv.cvtColor(gray_prev, cv.COLOR_BGR2GRAY) 20 | 21 | # Prepare a plot to visualize the camera trajectory 22 | plt.ion() 23 | traj_axes = plt.figure(layout='tight').add_subplot(projection='3d') 24 | traj_axes.set_xlabel('X [m]') 25 | traj_axes.set_ylabel('Y [m]') 26 | traj_axes.set_zlabel('Z [m]') 27 | traj_axes.grid(True) 28 | traj_axes.view_init(azim=-90) 29 | traj_line, = plt.plot([], [], [], 'b-') 30 | 31 | # Run the monocular visual odometry 32 | K = np.array([[f, 0, cx], [0, f, cy], [0, 0, 1]]) 33 | camera_pose = np.eye(4) 34 | camera_traj = np.zeros((1, 3)) 35 | while True: 36 | # Grab an image from the video 37 | valid, img = video.read() 38 | if not valid: 39 | break 40 | if img.ndim >= 3 and img.shape[2] > 1: 41 | gray = cv.cvtColor(img, cv.COLOR_BGR2GRAY) 42 | else: 43 | gray = img.copy() 44 | 45 | # Extract optical flow 46 | pts_prev = cv.goodFeaturesToTrack(gray_prev, 2000, 0.01, 10) 47 | pts, status, error = cv.calcOpticalFlowPyrLK(gray_prev, gray, pts_prev, None) 48 | gray_prev = gray 49 | 50 | # Calculate relative pose 51 | if use_5pt: 52 | E, inlier_mask = cv.findEssentialMat(pts_prev, pts, f, (cx, cy), cv.FM_RANSAC, 0.99, 1) 53 | else: 54 | F, inlier_mask = cv.findFundamentalMat(pts_prev, pts, cv.FM_RANSAC, 1, 0.99) 55 | E = K.T @ F @ K 56 | inlier_num, R, t, inlier_mask = cv.recoverPose(E, pts_prev, pts, focal=f, pp=(cx, cy), mask=inlier_mask) 57 | inlier_ratio = inlier_num / len(pts) 58 | 59 | # Accumulate relative pose if result is reliable 60 | info_color = (0, 255, 0) 61 | if inlier_num > min_inlier_num and inlier_ratio > min_inlier_ratio: 62 | T = np.eye(4) 63 | T[:3, :3] = R 64 | T[:3, 3] = t.flatten() 65 | camera_pose = camera_pose @ np.linalg.inv(T) 66 | info_color = (0, 0, 255) 67 | 68 | # Show the camera trajectory interactively 69 | x, y, z = camera_pose[:3, 3] 70 | camera_traj = np.vstack((camera_traj, [x, y, z])) 71 | traj_axes.set_xlim(min(camera_traj[:,0]), max(camera_traj[:,0])) 72 | traj_axes.set_ylim(min(camera_traj[:,1]), max(camera_traj[:,1])) 73 | traj_axes.set_zlim(min(camera_traj[:,2]), max(camera_traj[:,2])) 74 | traj_axes.set_aspect('equal') 75 | traj_line.set_data_3d(camera_traj[:,0], camera_traj[:,1], camera_traj[:,2]) 76 | plt.draw() 77 | 78 | # Show the image and write camera pose 79 | if img.ndim < 3 or img.shape[2] < 3: 80 | img = cv.cvtColor(img, cv.COLOR_GRAY2BGR) 81 | for pt, pt_prev, inlier in zip(pts, pts_prev, inlier_mask): 82 | color = (0, 0, 255) if inlier else (0, 127, 0) 83 | cv.line(img, pt_prev.flatten().astype(np.int32), pt.flatten().astype(np.int32), color) 84 | info = f'Inliers: {inlier_num} ({inlier_ratio*100:.0f}%), XYZ: [{x:.3f} {y:.3f} {z:.3f}]' 85 | cv.putText(img, info, (5, 15), cv.FONT_HERSHEY_PLAIN, 1, info_color) 86 | cv.imshow('Monocular Visual Odometry (Epipolar)', img) 87 | key = cv.waitKey(1) 88 | if key == ord(' '): 89 | key = cv.waitKey() 90 | if key == 27: # ESC 91 | break 92 | 93 | np.savetxt(traj_file, camera_traj) 94 | video.release() 95 | cv.destroyAllWindows() -------------------------------------------------------------------------------- /requirements.txt: -------------------------------------------------------------------------------- 1 | numpy 2 | matplotlib 3 | scipy 4 | opencv-python 5 | opencv-contrib-python -------------------------------------------------------------------------------- /slides/01_introduction.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/slides/01_introduction.pdf -------------------------------------------------------------------------------- /slides/02_single-view_geometry.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/slides/02_single-view_geometry.pdf -------------------------------------------------------------------------------- /slides/03_two-view_geometry.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/slides/03_two-view_geometry.pdf -------------------------------------------------------------------------------- /slides/04_solving_problems.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/slides/04_solving_problems.pdf -------------------------------------------------------------------------------- /slides/05_correspondence.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/slides/05_correspondence.pdf -------------------------------------------------------------------------------- /slides/06_multi-view_geometry.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/slides/06_multi-view_geometry.pdf -------------------------------------------------------------------------------- /slides/07_visual_slam.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mint-lab/3dv_tutorial/557dd2ed046376b8d7c2170f7952a70307a9529b/slides/07_visual_slam.pdf --------------------------------------------------------------------------------