├── .vscode └── c_cpp_properties.json ├── CMakeLists.txt ├── README.md ├── data ├── asymmetric_circles_grid │ ├── asymmetric_circles_grid01.jpg │ ├── asymmetric_circles_grid02.jpg │ ├── asymmetric_circles_grid03.jpg │ ├── asymmetric_circles_grid04.jpg │ ├── asymmetric_circles_grid05.jpg │ ├── asymmetric_circles_grid06.jpg │ ├── asymmetric_circles_grid07.jpg │ ├── asymmetric_circles_grid08.jpg │ ├── asymmetric_circles_grid09.jpg │ ├── asymmetric_circles_grid10.jpg │ ├── asymmetric_circles_grid11.jpg │ └── asymmetric_circles_grid12.jpg ├── charuco │ ├── charuco01.jpg │ ├── charuco02.jpg │ ├── charuco03.jpg │ ├── charuco04.jpg │ ├── charuco05.jpg │ ├── charuco06.jpg │ ├── charuco07.jpg │ ├── charuco08.jpg │ ├── charuco09.jpg │ ├── charuco10.jpg │ ├── charuco11.jpg │ ├── charuco12.jpg │ └── charuco13.jpg ├── circles_grid │ ├── circles_grid01.jpg │ ├── circles_grid02.jpg │ ├── circles_grid03.jpg │ ├── circles_grid04.jpg │ ├── circles_grid05.jpg │ ├── circles_grid06.jpg │ ├── circles_grid07.jpg │ ├── circles_grid08.jpg │ ├── circles_grid09.jpg │ ├── circles_grid10.jpg │ └── circles_grid11.jpg ├── fisheye │ ├── fisheye_kannala-brandt_camera_calib.yaml │ ├── fisheye_mei_camera_calib.yaml │ ├── fisheye_pinhole_camera_calib.yaml │ ├── left1.jpg │ ├── left10.jpg │ ├── left11.jpg │ ├── left12.jpg │ ├── left13.jpg │ ├── left14.jpg │ ├── left15.jpg │ ├── left16.jpg │ ├── left17.jpg │ ├── left18.jpg │ ├── left19.jpg │ ├── left2.jpg │ ├── left20.jpg │ ├── left21.jpg │ ├── left22.jpg │ ├── left23.jpg │ ├── left24.jpg │ ├── left25.jpg │ ├── left26.jpg │ ├── left27.jpg │ ├── left28.jpg │ ├── left29.jpg │ ├── left3.jpg │ ├── left4.jpg │ ├── left5.jpg │ ├── left6.jpg │ ├── left7.jpg │ ├── left8.jpg │ ├── left9.jpg │ ├── right1.jpg │ ├── right10.jpg │ ├── right11.jpg │ ├── right12.jpg │ ├── right13.jpg │ ├── right14.jpg │ ├── right15.jpg │ ├── right16.jpg │ ├── right17.jpg │ ├── right18.jpg │ ├── right19.jpg │ ├── right2.jpg │ ├── right20.jpg │ ├── right21.jpg │ ├── right22.jpg │ ├── right23.jpg │ ├── right24.jpg │ ├── right25.jpg │ ├── right26.jpg │ ├── right27.jpg │ ├── right28.jpg │ ├── right29.jpg │ ├── right3.jpg │ ├── right4.jpg │ ├── right5.jpg │ ├── right6.jpg │ ├── right7.jpg │ ├── right8.jpg │ └── right9.jpg ├── osmo │ ├── osmo_01.jpg │ ├── osmo_02.jpg │ ├── osmo_03.jpg │ ├── osmo_04.jpg │ ├── osmo_05.jpg │ ├── osmo_06.jpg │ ├── osmo_07.jpg │ ├── osmo_08.jpg │ ├── osmo_09.jpg │ ├── osmo_10.jpg │ ├── osmo_11.jpg │ ├── osmo_12.jpg │ ├── osmo_13.jpg │ ├── osmo_14.jpg │ ├── osmo_15.jpg │ ├── osmo_16.jpg │ ├── osmo_17.jpg │ ├── osmo_18.jpg │ ├── osmo_kannala-brandt_camera_calib.yaml │ └── osmo_pinhole_camera_calib.yaml ├── pattern │ ├── ChArUco-A4_5-7.launch.txt │ ├── ChArUco-A4_5-7.pdf │ ├── README.md │ ├── acircleboard.pdf │ ├── acircleboard.svg │ ├── charuco.yaml │ ├── chessboard.pdf │ ├── chessboard.svg │ ├── circleboard.pdf │ ├── circleboard.svg │ ├── detector_params.yml │ ├── gen_pattern.py │ └── svgfig.py └── stereo_images │ ├── left1.jpg │ ├── left10.jpg │ ├── left11.jpg │ ├── left12.jpg │ ├── left13.jpg │ ├── left14.jpg │ ├── left15.jpg │ ├── left16.jpg │ ├── left17.jpg │ ├── left18.jpg │ ├── left19.jpg │ ├── left2.jpg │ ├── left20.jpg │ ├── left21.jpg │ ├── left22.jpg │ ├── left23.jpg │ ├── left24.jpg │ ├── left25.jpg │ ├── left26.jpg │ ├── left27.jpg │ ├── left28.jpg │ ├── left29.jpg │ ├── left3.jpg │ ├── left4.jpg │ ├── left5.jpg │ ├── left6.jpg │ ├── left7.jpg │ ├── left8.jpg │ ├── left9.jpg │ ├── right1.jpg │ ├── right10.jpg │ ├── right11.jpg │ ├── right12.jpg │ ├── right13.jpg │ ├── right14.jpg │ ├── right15.jpg │ ├── right16.jpg │ ├── right17.jpg │ ├── right18.jpg │ ├── right19.jpg │ ├── right2.jpg │ ├── right20.jpg │ ├── right21.jpg │ ├── right22.jpg │ ├── right23.jpg │ ├── right24.jpg │ ├── right25.jpg │ ├── right26.jpg │ ├── right27.jpg │ ├── right28.jpg │ ├── right29.jpg │ ├── right3.jpg │ ├── right4.jpg │ ├── right5.jpg │ ├── right6.jpg │ ├── right7.jpg │ ├── right8.jpg │ └── right9.jpg ├── include └── camera_model │ ├── calib │ ├── CameraCalibration.h │ └── StereoCameraCalibration.h │ ├── camera_models │ ├── Camera.h │ ├── CameraFactory.h │ ├── CataCamera.h │ ├── CostFunctionFactory.h │ ├── EquidistantCamera.h │ ├── PinholeCamera.h │ └── ScaramuzzaCamera.h │ ├── chessboard │ ├── Chessboard.h │ ├── ChessboardCorner.h │ ├── ChessboardQuad.h │ └── Spline.h │ ├── gpl │ ├── EigenQuaternionParameterization.h │ ├── EigenUtils.h │ └── gpl.h │ └── sparse_graph │ └── Transform.h └── src ├── calib ├── CameraCalibration.cc └── StereoCameraCalibration.cc ├── camera_models ├── Camera.cc ├── CameraFactory.cc ├── CataCamera.cc ├── CostFunctionFactory.cc ├── EquidistantCamera.cc ├── PinholeCamera.cc └── ScaramuzzaCamera.cc ├── chessboard └── Chessboard.cc ├── gpl ├── EigenQuaternionParameterization.cc └── gpl.cc ├── intrinsic_calib.cc ├── sparse_graph └── Transform.cc └── stereo_calib.cc /.vscode/c_cpp_properties.json: -------------------------------------------------------------------------------- 1 | { 2 | "configurations": [ 3 | { 4 | "name": "Linux", 5 | "includePath": [ 6 | "${default}", 7 | "/usr/include/", 8 | "/usr/include/eigen3/", 9 | "/opt/ros/noetic/include/", 10 | "/opt/ros/noetic/", 11 | "/usr/include/pcl-1.10/", 12 | "src", 13 | "include", 14 | "/usr/include/ceres/", 15 | "/usr/include/vtk-7.1/" 16 | ], 17 | "defines": [], 18 | "compilerPath": "/usr/bin/clang", 19 | "cStandard": "c11", 20 | "cppStandard": "c++14", 21 | "intelliSenseMode": "clang-x64" 22 | } 23 | ], 24 | "version": 4 25 | } -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0) 2 | project(camera_model) 3 | 4 | set(CMAKE_BUILD_TYPE "Release") 5 | set(CMAKE_CXX_FLAGS "-std=c++11 -march=native") 6 | set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -fPIC") 7 | 8 | # find_package(catkin REQUIRED COMPONENTS 9 | # roscpp 10 | # std_msgs 11 | # ) 12 | 13 | find_package(Boost REQUIRED COMPONENTS filesystem program_options system) 14 | include_directories(${Boost_INCLUDE_DIRS}) 15 | 16 | find_package(OpenCV REQUIRED) 17 | find_package(Ceres REQUIRED) 18 | include_directories(${CERES_INCLUDE_DIRS}) 19 | 20 | include_directories("include") 21 | 22 | add_library(camera_model SHARED 23 | src/chessboard/Chessboard.cc 24 | src/calib/CameraCalibration.cc 25 | src/camera_models/Camera.cc 26 | src/camera_models/CameraFactory.cc 27 | src/camera_models/CostFunctionFactory.cc 28 | src/camera_models/PinholeCamera.cc 29 | src/camera_models/CataCamera.cc 30 | src/camera_models/EquidistantCamera.cc 31 | src/camera_models/ScaramuzzaCamera.cc 32 | src/sparse_graph/Transform.cc 33 | src/gpl/gpl.cc 34 | src/gpl/EigenQuaternionParameterization.cc) 35 | target_link_libraries(camera_model ${Boost_LIBRARIES} ${OpenCV_LIBS} ${CERES_LIBRARIES}) 36 | 37 | 38 | add_executable(intrinsic_calib src/intrinsic_calib.cc) 39 | target_link_libraries(intrinsic_calib camera_model ${Boost_LIBRARIES} ${OpenCV_LIBS} ${CERES_LIBRARIES}) 40 | 41 | add_executable(stereo_calib src/stereo_calib.cc src/calib/StereoCameraCalibration.cc) 42 | target_link_libraries(stereo_calib camera_model) 43 | 44 | file(GLOB CALIB_HEADER_FILES include/camera_model/calib/*.h) 45 | file(GLOB CAMERA_MODELS_HEADER_FILES include/camera_model/camera_models/*.h) 46 | file(GLOB CHESSBOARD_HEADER_FILES include/camera_model/chessboard/*.h) 47 | file(GLOB GPL_HEADER_FILES include/camera_model/gpl/*.h) 48 | file(GLOB SPARSE_GRAPH_HEADER_FILES include/camera_model/sparse_graph/*.h) 49 | 50 | install(TARGETS camera_model 51 | LIBRARY DESTINATION lib 52 | ARCHIVE DESTINATION lib) 53 | 54 | install(FILES ${CALIB_HEADER_FILES} DESTINATION include/camera_model/calib) 55 | install(FILES ${CAMERA_MODELS_HEADER_FILES} DESTINATION include/camera_model/camera_models) 56 | install(FILES ${CHESSBOARD_HEADER_FILES} DESTINATION include/camera_model/chessboard) 57 | install(FILES ${GPL_HEADER_FILES} DESTINATION include/camera_model/gpl) 58 | install(FILES ${SPARSE_GRAPH_HEADER_FILES} DESTINATION include/camera_model/sparse_graph) -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # camera_model 2 | 3 | 相机内参标定和双目标定 4 | 5 | 支持多种相机模型: 6 | * pinhole model 7 | * kannala-brandt model 8 | * mei model 9 | * scaramuzza model 10 | 11 | 支持多种标定板 12 | * chessboard 13 | * circles grid 14 | * asymmetric circles grid 15 | * aruco makers 16 | * charuco makrers 17 | 18 | ## 编译 & 安装 19 | 依赖项 20 | * OpenCV (with opencv_contrib) 21 | * Ceres Solver 22 | * boost 23 | 24 | ``` 25 | cd camera_model 26 | mkdir build 27 | cd build 28 | cmake .. 29 | make 30 | sudo make install 31 | ``` 32 | 33 | ## 调用 34 | 编译安装之后， camera_model会作为共享库被安装在系统目录, 使用时无需添加源码，只添加依赖关系即可 35 | ``` 36 | cmake_minimum_required(VERSION 3.0) 37 | project(camera_model_example) 38 | 39 | add_executable(example example.cc) 40 | target_link_libraries(example camera_model) 41 | ``` 42 | 43 | # 参考 & 感谢 44 | part of [camodocal](https://github.com/hengli/camodocal) 45 | 46 | 感谢原作者 [Lionel Heng](https://github.com/hengli) 47 | 48 | 感谢 [YZF](https://github.com/dvorak0) 整理camera_model 49 | 50 | 感谢 [Tianbo](https://github.com/groundmelon) 添加Scaramuzza模型实现 51 | 52 | # 用法 53 | ## 相机内参标定 intrinsic_calib 54 | 55 | Use [intrinsic_calib.cc](https://github.com/libing64/camera_model/blob/master/src/intrinsic_calib.cc) to calibrate your camera. 56 | 57 | 58 | ## 双目标定 stereo_calib 59 | 60 | Use [stereo_calib.cc](https://github.com/libing64/camera_model/blob/master/src/stereo_calib.cc) to calibrate your camera. 61 | 62 | ## Undistortion: 63 | 64 | See [Camera.h](https://github.com/libing64/camera_model/blob/master/include/camera_model/camera_models/Camera.h) for general interface: 65 | 66 | - liftProjective: Lift points from the image plane to the projective space. 67 | - spaceToPlane: Projects 3D points to the image plane (Pi function) 68 | 69 | # 案例 70 | # 相机内参标定 intrinsic calib 71 | 72 | ## mei model 73 | ``` 74 | ./intrinsic_calib -w 9 -h 6 -i ../data/fisheye -p left -e jpg --camera-model mei --camera-name fisheye_mei -v 75 | ``` 76 | 77 | ## pinhole model 78 | ``` 79 | ./intrinsic_calib -w 9 -h 6 -i ../data/fisheye -p left -e jpg --camera-model pinhole --camera-name fisheye_pinhole -v 80 | ``` 81 | 82 | ## kannala-brandt 83 | ``` 84 | ./intrinsic_calib -w 9 -h 6 -i ../data/fisheye -p left -e jpg --camera-model kannala-brandt --camera-name fisheye_kannala-brandt -v 85 | ``` 86 | ## scaramuzza 87 | 88 | ``` 89 | ./intrinsic_calib -w 9 -h 6 -i ../data/fisheye -p left -e jpg --camera-model scaramuzza --camera-name fisheye_scaramuzza -v --view-results 90 | ``` 91 | 92 | ## asymmetric cirlces grid 93 | ``` 94 | ./intrinsic_calib -w 4 -h 11 -i ../data/asymmetric_circles_grid -p asymmetric_circles_grid -e jpg --pattern asymmetric_circles_grid --camera-model pinhole --camera-name webcam -v --view-results 95 | ``` 96 | 97 | 注意，这里一定要4x11, 设置为11x4就不行 98 | 99 | ## circles grid 100 | ``` 101 | ./intrinsic_calib -w 9 -h 6 -i ../data/circles_grid -p circles_grid -e jpg --pattern circles_grid --camera-model pinhole --camera-name circles_grid_cam -v --view-results 102 | ``` 103 | 104 | ## charuco marker 105 | ``` 106 | ./intrinsic_calib -w 4 -h 6 -s 0.04 --marker-size 0.024 -i ../data/charuco -p charuco -e jpg --pattern charuco --dp ../data/pattern/detector_params.yml -d 10 --camera-model pinhole --camera-name charuco_cam -v --view-results 107 | ``` 108 | 109 | ``` 110 | ./intrinsic_calib -w 4 -h 6 -s 0.04 --marker-size 0.024 -i ../data/charuco -p charuco -e jpg --pattern charuco --dp ../data/pattern/detector_params.yml -d 10 --camera-model kannala-brandt --camera-name charuco_cam -v --view-results 111 | ``` 112 | 113 | 114 | ## stereo calibration 115 | ``` 116 | ./stereo_calib -i ../data/stereo_images/ -e jpg --prefix-l left --prefix-r right --camera-model mei -v --view-results 117 | ``` 118 | 119 | ## stereo calibration circles (virtual test with mono image) 120 | ``` 121 | ./stereo_calib -i ../data/circles_grid --pattern circles_grid -e jpg --prefix-l circles_grid --prefix-r circles_grid --camera-model mei -v --view-results 122 | ``` 123 | 124 | # TODO 125 | - [ ] add aruco marker 126 | -------------------------------------------------------------------------------- /data/asymmetric_circles_grid/asymmetric_circles_grid01.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/asymmetric_circles_grid/asymmetric_circles_grid01.jpg -------------------------------------------------------------------------------- /data/asymmetric_circles_grid/asymmetric_circles_grid02.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/asymmetric_circles_grid/asymmetric_circles_grid02.jpg -------------------------------------------------------------------------------- /data/asymmetric_circles_grid/asymmetric_circles_grid03.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/asymmetric_circles_grid/asymmetric_circles_grid03.jpg -------------------------------------------------------------------------------- /data/asymmetric_circles_grid/asymmetric_circles_grid04.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/asymmetric_circles_grid/asymmetric_circles_grid04.jpg -------------------------------------------------------------------------------- /data/asymmetric_circles_grid/asymmetric_circles_grid05.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/asymmetric_circles_grid/asymmetric_circles_grid05.jpg -------------------------------------------------------------------------------- /data/asymmetric_circles_grid/asymmetric_circles_grid06.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/asymmetric_circles_grid/asymmetric_circles_grid06.jpg -------------------------------------------------------------------------------- /data/asymmetric_circles_grid/asymmetric_circles_grid07.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/asymmetric_circles_grid/asymmetric_circles_grid07.jpg -------------------------------------------------------------------------------- /data/asymmetric_circles_grid/asymmetric_circles_grid08.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/asymmetric_circles_grid/asymmetric_circles_grid08.jpg -------------------------------------------------------------------------------- /data/asymmetric_circles_grid/asymmetric_circles_grid09.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/asymmetric_circles_grid/asymmetric_circles_grid09.jpg -------------------------------------------------------------------------------- /data/asymmetric_circles_grid/asymmetric_circles_grid10.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/asymmetric_circles_grid/asymmetric_circles_grid10.jpg -------------------------------------------------------------------------------- /data/asymmetric_circles_grid/asymmetric_circles_grid11.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/asymmetric_circles_grid/asymmetric_circles_grid11.jpg -------------------------------------------------------------------------------- /data/asymmetric_circles_grid/asymmetric_circles_grid12.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/asymmetric_circles_grid/asymmetric_circles_grid12.jpg -------------------------------------------------------------------------------- /data/charuco/charuco01.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/charuco/charuco01.jpg -------------------------------------------------------------------------------- /data/charuco/charuco02.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/charuco/charuco02.jpg -------------------------------------------------------------------------------- /data/charuco/charuco03.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/charuco/charuco03.jpg -------------------------------------------------------------------------------- /data/charuco/charuco04.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/charuco/charuco04.jpg -------------------------------------------------------------------------------- /data/charuco/charuco05.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/charuco/charuco05.jpg -------------------------------------------------------------------------------- /data/charuco/charuco06.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/charuco/charuco06.jpg -------------------------------------------------------------------------------- /data/charuco/charuco07.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/charuco/charuco07.jpg -------------------------------------------------------------------------------- /data/charuco/charuco08.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/charuco/charuco08.jpg -------------------------------------------------------------------------------- /data/charuco/charuco09.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/charuco/charuco09.jpg -------------------------------------------------------------------------------- /data/charuco/charuco10.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/charuco/charuco10.jpg -------------------------------------------------------------------------------- /data/charuco/charuco11.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/charuco/charuco11.jpg -------------------------------------------------------------------------------- /data/charuco/charuco12.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/charuco/charuco12.jpg -------------------------------------------------------------------------------- /data/charuco/charuco13.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/charuco/charuco13.jpg -------------------------------------------------------------------------------- /data/circles_grid/circles_grid01.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/circles_grid/circles_grid01.jpg -------------------------------------------------------------------------------- /data/circles_grid/circles_grid02.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/circles_grid/circles_grid02.jpg -------------------------------------------------------------------------------- /data/circles_grid/circles_grid03.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/circles_grid/circles_grid03.jpg -------------------------------------------------------------------------------- /data/circles_grid/circles_grid04.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/circles_grid/circles_grid04.jpg -------------------------------------------------------------------------------- /data/circles_grid/circles_grid05.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/circles_grid/circles_grid05.jpg -------------------------------------------------------------------------------- /data/circles_grid/circles_grid06.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/circles_grid/circles_grid06.jpg -------------------------------------------------------------------------------- /data/circles_grid/circles_grid07.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/circles_grid/circles_grid07.jpg -------------------------------------------------------------------------------- /data/circles_grid/circles_grid08.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/circles_grid/circles_grid08.jpg -------------------------------------------------------------------------------- /data/circles_grid/circles_grid09.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/circles_grid/circles_grid09.jpg -------------------------------------------------------------------------------- /data/circles_grid/circles_grid10.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/circles_grid/circles_grid10.jpg -------------------------------------------------------------------------------- /data/circles_grid/circles_grid11.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/circles_grid/circles_grid11.jpg -------------------------------------------------------------------------------- /data/fisheye/fisheye_kannala-brandt_camera_calib.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | --- 3 | model_type: KANNALA_BRANDT 4 | camera_name: fisheye_kannala-brandt 5 | image_width: 960 6 | image_height: 600 7 | projection_parameters: 8 | k2: 2.5773912070218150e-02 9 | k3: -2.7345542064950989e-02 10 | k4: 2.5950947011271125e-02 11 | k5: -9.7522429072694344e-03 12 | mu: 2.2735285216121716e+02 13 | mv: 2.2657943420362352e+02 14 | u0: 4.7166695251443213e+02 15 | v0: 3.0549483712507038e+02 16 | -------------------------------------------------------------------------------- /data/fisheye/fisheye_mei_camera_calib.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | --- 3 | model_type: MEI 4 | camera_name: fisheye_mei 5 | image_width: 960 6 | image_height: 600 7 | mirror_parameters: 8 | xi: 1.0799110400771077e+00 9 | distortion_parameters: 10 | k1: -2.4625306784743095e-01 11 | k2: 5.3962422839901353e-02 12 | p1: 2.6183788901745280e-03 13 | p2: -2.1197577430378737e-03 14 | projection_parameters: 15 | gamma1: 4.7724621607048920e+02 16 | gamma2: 4.7564921574092671e+02 17 | u0: 4.7288499237101860e+02 18 | v0: 3.0409070421914112e+02 19 | -------------------------------------------------------------------------------- /data/fisheye/fisheye_pinhole_camera_calib.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | --- 3 | model_type: PINHOLE 4 | camera_name: fisheye_pinhole 5 | image_width: 960 6 | image_height: 600 7 | distortion_parameters: 8 | k1: -2.5736129390828005e-01 9 | k2: 5.2765685001765629e-02 10 | p1: 4.1658997477126702e-03 11 | p2: -4.1063465533036922e-03 12 | projection_parameters: 13 | fx: 2.4604583393139453e+02 14 | fy: 2.4537035815615729e+02 15 | cx: 4.8818073024007793e+02 16 | cy: 2.9174349471122207e+02 17 | -------------------------------------------------------------------------------- /data/fisheye/left1.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/fisheye/left1.jpg -------------------------------------------------------------------------------- /data/fisheye/left10.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/fisheye/left10.jpg -------------------------------------------------------------------------------- /data/fisheye/left11.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/fisheye/left11.jpg -------------------------------------------------------------------------------- /data/fisheye/left12.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/fisheye/left12.jpg -------------------------------------------------------------------------------- /data/fisheye/left13.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/fisheye/left13.jpg -------------------------------------------------------------------------------- /data/fisheye/left14.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/fisheye/left14.jpg -------------------------------------------------------------------------------- /data/fisheye/left15.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/fisheye/left15.jpg -------------------------------------------------------------------------------- /data/fisheye/left16.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/fisheye/left16.jpg -------------------------------------------------------------------------------- /data/fisheye/left17.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/fisheye/left17.jpg -------------------------------------------------------------------------------- /data/fisheye/left18.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/fisheye/left18.jpg -------------------------------------------------------------------------------- /data/fisheye/left19.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/fisheye/left19.jpg -------------------------------------------------------------------------------- /data/fisheye/left2.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/fisheye/left2.jpg -------------------------------------------------------------------------------- /data/fisheye/left20.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/fisheye/left20.jpg -------------------------------------------------------------------------------- /data/fisheye/left21.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/fisheye/left21.jpg -------------------------------------------------------------------------------- /data/fisheye/left22.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/fisheye/left22.jpg -------------------------------------------------------------------------------- /data/fisheye/left23.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/fisheye/left23.jpg -------------------------------------------------------------------------------- /data/fisheye/left24.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/fisheye/left24.jpg -------------------------------------------------------------------------------- /data/fisheye/left25.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/fisheye/left25.jpg -------------------------------------------------------------------------------- /data/fisheye/left26.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/fisheye/left26.jpg -------------------------------------------------------------------------------- /data/fisheye/left27.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/fisheye/left27.jpg -------------------------------------------------------------------------------- /data/fisheye/left28.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/fisheye/left28.jpg -------------------------------------------------------------------------------- /data/fisheye/left29.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/fisheye/left29.jpg -------------------------------------------------------------------------------- /data/fisheye/left3.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/fisheye/left3.jpg -------------------------------------------------------------------------------- /data/fisheye/left4.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/fisheye/left4.jpg -------------------------------------------------------------------------------- /data/fisheye/left5.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/fisheye/left5.jpg -------------------------------------------------------------------------------- /data/fisheye/left6.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/fisheye/left6.jpg -------------------------------------------------------------------------------- /data/fisheye/left7.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/fisheye/left7.jpg -------------------------------------------------------------------------------- /data/fisheye/left8.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/fisheye/left8.jpg -------------------------------------------------------------------------------- /data/fisheye/left9.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/fisheye/left9.jpg -------------------------------------------------------------------------------- /data/fisheye/right1.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/fisheye/right1.jpg -------------------------------------------------------------------------------- /data/fisheye/right10.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/fisheye/right10.jpg -------------------------------------------------------------------------------- /data/fisheye/right11.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/fisheye/right11.jpg -------------------------------------------------------------------------------- /data/fisheye/right12.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/fisheye/right12.jpg -------------------------------------------------------------------------------- /data/fisheye/right13.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/fisheye/right13.jpg -------------------------------------------------------------------------------- /data/fisheye/right14.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/fisheye/right14.jpg -------------------------------------------------------------------------------- /data/fisheye/right15.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/fisheye/right15.jpg -------------------------------------------------------------------------------- /data/fisheye/right16.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/fisheye/right16.jpg -------------------------------------------------------------------------------- /data/fisheye/right17.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/fisheye/right17.jpg -------------------------------------------------------------------------------- /data/fisheye/right18.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/fisheye/right18.jpg -------------------------------------------------------------------------------- /data/fisheye/right19.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/fisheye/right19.jpg -------------------------------------------------------------------------------- /data/fisheye/right2.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/fisheye/right2.jpg -------------------------------------------------------------------------------- /data/fisheye/right20.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/fisheye/right20.jpg -------------------------------------------------------------------------------- /data/fisheye/right21.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/fisheye/right21.jpg -------------------------------------------------------------------------------- /data/fisheye/right22.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/fisheye/right22.jpg -------------------------------------------------------------------------------- /data/fisheye/right23.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/fisheye/right23.jpg -------------------------------------------------------------------------------- /data/fisheye/right24.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/fisheye/right24.jpg -------------------------------------------------------------------------------- /data/fisheye/right25.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/fisheye/right25.jpg -------------------------------------------------------------------------------- /data/fisheye/right26.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/fisheye/right26.jpg -------------------------------------------------------------------------------- /data/fisheye/right27.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/fisheye/right27.jpg -------------------------------------------------------------------------------- /data/fisheye/right28.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/fisheye/right28.jpg -------------------------------------------------------------------------------- /data/fisheye/right29.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/fisheye/right29.jpg -------------------------------------------------------------------------------- /data/fisheye/right3.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/fisheye/right3.jpg -------------------------------------------------------------------------------- /data/fisheye/right4.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/fisheye/right4.jpg -------------------------------------------------------------------------------- /data/fisheye/right5.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/fisheye/right5.jpg -------------------------------------------------------------------------------- /data/fisheye/right6.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/fisheye/right6.jpg -------------------------------------------------------------------------------- /data/fisheye/right7.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/fisheye/right7.jpg -------------------------------------------------------------------------------- /data/fisheye/right8.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/fisheye/right8.jpg -------------------------------------------------------------------------------- /data/fisheye/right9.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/fisheye/right9.jpg -------------------------------------------------------------------------------- /data/osmo/osmo_01.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/osmo/osmo_01.jpg -------------------------------------------------------------------------------- /data/osmo/osmo_02.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/osmo/osmo_02.jpg -------------------------------------------------------------------------------- /data/osmo/osmo_03.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/osmo/osmo_03.jpg -------------------------------------------------------------------------------- /data/osmo/osmo_04.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/osmo/osmo_04.jpg -------------------------------------------------------------------------------- /data/osmo/osmo_05.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/osmo/osmo_05.jpg -------------------------------------------------------------------------------- /data/osmo/osmo_06.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/osmo/osmo_06.jpg -------------------------------------------------------------------------------- /data/osmo/osmo_07.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/osmo/osmo_07.jpg -------------------------------------------------------------------------------- /data/osmo/osmo_08.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/osmo/osmo_08.jpg -------------------------------------------------------------------------------- /data/osmo/osmo_09.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/osmo/osmo_09.jpg -------------------------------------------------------------------------------- /data/osmo/osmo_10.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/osmo/osmo_10.jpg -------------------------------------------------------------------------------- /data/osmo/osmo_11.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/osmo/osmo_11.jpg -------------------------------------------------------------------------------- /data/osmo/osmo_12.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/osmo/osmo_12.jpg -------------------------------------------------------------------------------- /data/osmo/osmo_13.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/osmo/osmo_13.jpg -------------------------------------------------------------------------------- /data/osmo/osmo_14.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/osmo/osmo_14.jpg -------------------------------------------------------------------------------- /data/osmo/osmo_15.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/osmo/osmo_15.jpg -------------------------------------------------------------------------------- /data/osmo/osmo_16.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/osmo/osmo_16.jpg -------------------------------------------------------------------------------- /data/osmo/osmo_17.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/osmo/osmo_17.jpg -------------------------------------------------------------------------------- /data/osmo/osmo_18.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/osmo/osmo_18.jpg -------------------------------------------------------------------------------- /data/osmo/osmo_kannala-brandt_camera_calib.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | --- 3 | model_type: KANNALA_BRANDT 4 | camera_name: osmo_kannala-brandt 5 | image_width: 1920 6 | image_height: 1080 7 | projection_parameters: 8 | k2: 4.5248623378188629e-01 9 | k3: 8.6819612088591391e-01 10 | k4: -9.4386775354486829e+00 11 | k5: 2.0473684233998849e+01 12 | mu: 1.1641271411914624e+03 13 | mv: 1.1637931376564286e+03 14 | u0: 9.5420683314112648e+02 15 | v0: 5.4210747707944529e+02 16 | -------------------------------------------------------------------------------- /data/osmo/osmo_pinhole_camera_calib.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | --- 3 | model_type: PINHOLE 4 | camera_name: osmo_pinhole 5 | image_width: 1920 6 | image_height: 1080 7 | distortion_parameters: 8 | k1: 1.7838481547946314e-01 9 | k2: -6.0098681119904285e-01 10 | p1: -7.1415451256699324e-04 11 | p2: 2.7555942977269680e-04 12 | projection_parameters: 13 | fx: 1.1630858422284912e+03 14 | fy: 1.1627390910219065e+03 15 | cx: 9.5472500492563722e+02 16 | cy: 5.4036641550554498e+02 17 | -------------------------------------------------------------------------------- /data/pattern/ChArUco-A4_5-7.launch.txt: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /data/pattern/ChArUco-A4_5-7.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/pattern/ChArUco-A4_5-7.pdf -------------------------------------------------------------------------------- /data/pattern/README.md: -------------------------------------------------------------------------------- 1 | # Create your own pattern 2 | Now, if you want to create your own pattern, you will need python to use https://github.com/opencv/opencv/blob/master/doc/pattern_tools/gen_pattern.py 3 | 4 | Example 5 | 6 | create a checkerboard pattern in file chessboard.svg with 9 rows, 6 columns and a square size of 20mm: 7 | ``` 8 | python gen_pattern.py -o chessboard.svg --rows 10 --columns 7 --type checkerboard --square_size 25 9 | ``` 10 | 11 | 12 | create a circle board pattern in file circleboard.svg with 9 rows, 6 columns and a radius of 15mm: 13 | ``` 14 | python gen_pattern.py -o circleboard.svg --rows 9 --columns 6 --type circles --square_size 30 --radius_rate 3.5 15 | 16 | ``` 17 | 18 | create a circle board pattern in file acircleboard.svg with 7 rows, 5 columns and a square size of 10mm and less spacing between circle: 19 | 20 | ``` 21 | python gen_pattern.py -o acircleboard.svg --rows 11 --columns 4 --type acircles --square_size 25 --radius_rate 2 22 | ``` 23 | 24 | If you want to change unit use -u option (mm inches, px, m) 25 | 26 | If you want to change page size use -w and -h options 27 | 28 | If you want to create a ChArUco board read tutorial Detection of ChArUco Corners in opencv_contrib tutorial. -------------------------------------------------------------------------------- /data/pattern/acircleboard.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/pattern/acircleboard.pdf -------------------------------------------------------------------------------- /data/pattern/acircleboard.svg: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | -------------------------------------------------------------------------------- /data/pattern/charuco.yaml: -------------------------------------------------------------------------------- 1 | adaptiveThreshWinSizeMin: 3 2 | adaptiveThreshWinSizeMax: 23 3 | adaptiveThreshWinSizeStep: 10 4 | adaptiveThreshConstant: 7.0 5 | minMarkerPerimeterRate: 0.03 6 | maxMarkerPerimeterRate: 4.0 7 | polygonalApproxAccuracyRate: 0.03 8 | minCornerDistanceRate: 0.05 9 | minDistanceToBorder: 3 10 | minMarkerDistanceRate: 0.05 11 | cornerRefinementMethod: 0 12 | cornerRefinementWinSize: 5 13 | cornerRefinementMaxIterations: 30 14 | cornerRefinementMinAccuracy: 0.1 15 | markerBorderBits: 1 16 | perspectiveRemovePixelPerCell: 4 17 | perspectiveRemoveIgnoredMarginPerCell: 0.13 18 | maxErroneousBitsInBorderRate: 0.35 19 | minOtsuStdDev: 5.0 20 | errorCorrectionRate: 0.6 21 | -------------------------------------------------------------------------------- /data/pattern/chessboard.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/pattern/chessboard.pdf -------------------------------------------------------------------------------- /data/pattern/chessboard.svg: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | -------------------------------------------------------------------------------- /data/pattern/circleboard.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/pattern/circleboard.pdf -------------------------------------------------------------------------------- /data/pattern/circleboard.svg: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | -------------------------------------------------------------------------------- /data/pattern/detector_params.yml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | nmarkers: 1024 3 | adaptiveThreshWinSizeMin: 3 4 | adaptiveThreshWinSizeMax: 23 5 | adaptiveThreshWinSizeStep: 10 6 | adaptiveThreshWinSize: 21 7 | adaptiveThreshConstant: 7 8 | minMarkerPerimeterRate: 0.03 9 | maxMarkerPerimeterRate: 4.0 10 | polygonalApproxAccuracyRate: 0.05 11 | minCornerDistance: 10.0 12 | minDistanceToBorder: 3 13 | minMarkerDistance: 10.0 14 | minMarkerDistanceRate: 0.05 15 | cornerRefinementMethod: 0 16 | cornerRefinementWinSize: 5 17 | cornerRefinementMaxIterations: 30 18 | cornerRefinementMinAccuracy: 0.1 19 | markerBorderBits: 1 20 | perspectiveRemovePixelPerCell: 8 21 | perspectiveRemoveIgnoredMarginPerCell: 0.13 22 | maxErroneousBitsInBorderRate: 0.04 23 | minOtsuStdDev: 5.0 24 | errorCorrectionRate: 0.6 25 | -------------------------------------------------------------------------------- /data/pattern/gen_pattern.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | """gen_pattern.py 4 | Usage example: 5 | python gen_pattern.py -o out.svg -r 11 -c 8 -T circles -s 20.0 -R 5.0 -u mm -w 216 -h 279 6 | -o, --output - output file (default out.svg) 7 | -r, --rows - pattern rows (default 11) 8 | -c, --columns - pattern columns (default 8) 9 | -T, --type - type of pattern, circles, acircles, checkerboard (default circles) 10 | -s, --square_size - size of squares in pattern (default 20.0) 11 | -R, --radius_rate - circles_radius = square_size/radius_rate (default 5.0) 12 | -u, --units - mm, inches, px, m (default mm) 13 | -w, --page_width - page width in units (default 216) 14 | -h, --page_height - page height in units (default 279) 15 | -a, --page_size - page size (default A4), supersedes -h -w arguments 16 | -H, --help - show help 17 | """ 18 | 19 | import argparse 20 | 21 | from svgfig import * 22 | 23 | 24 | class PatternMaker: 25 | def __init__(self, cols, rows, output, units, square_size, radius_rate, page_width, page_height): 26 | self.cols = cols 27 | self.rows = rows 28 | self.output = output 29 | self.units = units 30 | self.square_size = square_size 31 | self.radius_rate = radius_rate 32 | self.width = page_width 33 | self.height = page_height 34 | self.g = SVG("g") # the svg group container 35 | 36 | def make_circles_pattern(self): 37 | spacing = self.square_size 38 | r = spacing / self.radius_rate 39 | pattern_width = ((self.cols - 1.0) * spacing) + (2.0 * r) 40 | pattern_height = ((self.rows - 1.0) * spacing) + (2.0 * r) 41 | x_spacing = (self.width - pattern_width) / 2.0 42 | y_spacing = (self.height - pattern_height) / 2.0 43 | for x in range(0, self.cols): 44 | for y in range(0, self.rows): 45 | dot = SVG("circle", cx=(x * spacing) + x_spacing + r, 46 | cy=(y * spacing) + y_spacing + r, r=r, fill="black", stroke="none") 47 | self.g.append(dot) 48 | 49 | def make_acircles_pattern(self): 50 | spacing = self.square_size 51 | r = spacing / self.radius_rate 52 | pattern_width = ((self.cols-1.0) * 2 * spacing) + spacing + (2.0 * r) 53 | pattern_height = ((self.rows-1.0) * spacing) + (2.0 * r) 54 | x_spacing = (self.width - pattern_width) / 2.0 55 | y_spacing = (self.height - pattern_height) / 2.0 56 | for x in range(0, self.cols): 57 | for y in range(0, self.rows): 58 | dot = SVG("circle", cx=(2 * x * spacing) + (y % 2)*spacing + x_spacing + r, 59 | cy=(y * spacing) + y_spacing + r, r=r, fill="black", stroke="none") 60 | self.g.append(dot) 61 | 62 | def make_checkerboard_pattern(self): 63 | spacing = self.square_size 64 | xspacing = (self.width - self.cols * self.square_size) / 2.0 65 | yspacing = (self.height - self.rows * self.square_size) / 2.0 66 | for x in range(0, self.cols): 67 | for y in range(0, self.rows): 68 | if x % 2 == y % 2: 69 | square = SVG("rect", x=x * spacing + xspacing, y=y * spacing + yspacing, width=spacing, 70 | height=spacing, fill="black", stroke="none") 71 | self.g.append(square) 72 | 73 | def save(self): 74 | c = canvas(self.g, width="%d%s" % (self.width, self.units), height="%d%s" % (self.height, self.units), 75 | viewBox="0 0 %d %d" % (self.width, self.height)) 76 | c.save(self.output) 77 | 78 | 79 | def main(): 80 | # parse command line options 81 | parser = argparse.ArgumentParser(description="generate camera-calibration pattern", add_help=False) 82 | parser.add_argument("-H", "--help", help="show help", action="store_true", dest="show_help") 83 | parser.add_argument("-o", "--output", help="output file", default="out.svg", action="store", dest="output") 84 | parser.add_argument("-c", "--columns", help="pattern columns", default="8", action="store", dest="columns", 85 | type=int) 86 | parser.add_argument("-r", "--rows", help="pattern rows", default="11", action="store", dest="rows", type=int) 87 | parser.add_argument("-T", "--type", help="type of pattern", default="circles", action="store", dest="p_type", 88 | choices=["circles", "acircles", "checkerboard"]) 89 | parser.add_argument("-u", "--units", help="length unit", default="mm", action="store", dest="units", 90 | choices=["mm", "inches", "px", "m"]) 91 | parser.add_argument("-s", "--square_size", help="size of squares in pattern", default="20.0", action="store", 92 | dest="square_size", type=float) 93 | parser.add_argument("-R", "--radius_rate", help="circles_radius = square_size/radius_rate", default="5.0", 94 | action="store", dest="radius_rate", type=float) 95 | parser.add_argument("-w", "--page_width", help="page width in units", default=argparse.SUPPRESS, action="store", 96 | dest="page_width", type=float) 97 | parser.add_argument("-h", "--page_height", help="page height in units", default=argparse.SUPPRESS, action="store", 98 | dest="page_height", type=float) 99 | parser.add_argument("-a", "--page_size", help="page size, superseded if -h and -w are set", default="A4", action="store", 100 | dest="page_size", choices=["A0", "A1", "A2", "A3", "A4", "A5"]) 101 | args = parser.parse_args() 102 | 103 | show_help = args.show_help 104 | if show_help: 105 | parser.print_help() 106 | return 107 | output = args.output 108 | columns = args.columns 109 | rows = args.rows 110 | p_type = args.p_type 111 | units = args.units 112 | square_size = args.square_size 113 | radius_rate = args.radius_rate 114 | if 'page_width' and 'page_height' in args: 115 | page_width = args.page_width 116 | page_height = args.page_height 117 | else: 118 | page_size = args.page_size 119 | # page size dict (ISO standard, mm) for easy lookup. format - size: [width, height] 120 | page_sizes = {"A0": [840, 1188], "A1": [594, 840], "A2": [420, 594], "A3": [297, 420], "A4": [210, 297], 121 | "A5": [148, 210]} 122 | page_width = page_sizes[page_size][0] 123 | page_height = page_sizes[page_size][1] 124 | pm = PatternMaker(columns, rows, output, units, square_size, radius_rate, page_width, page_height) 125 | # dict for easy lookup of pattern type 126 | mp = {"circles": pm.make_circles_pattern, "acircles": pm.make_acircles_pattern, 127 | "checkerboard": pm.make_checkerboard_pattern} 128 | mp[p_type]() 129 | # this should save pattern to output 130 | pm.save() 131 | 132 | 133 | if __name__ == "__main__": 134 | main() 135 | -------------------------------------------------------------------------------- /data/stereo_images/left1.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/stereo_images/left1.jpg -------------------------------------------------------------------------------- /data/stereo_images/left10.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/stereo_images/left10.jpg -------------------------------------------------------------------------------- /data/stereo_images/left11.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/stereo_images/left11.jpg -------------------------------------------------------------------------------- /data/stereo_images/left12.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/stereo_images/left12.jpg -------------------------------------------------------------------------------- /data/stereo_images/left13.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/stereo_images/left13.jpg -------------------------------------------------------------------------------- /data/stereo_images/left14.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/stereo_images/left14.jpg -------------------------------------------------------------------------------- /data/stereo_images/left15.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/stereo_images/left15.jpg -------------------------------------------------------------------------------- /data/stereo_images/left16.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/stereo_images/left16.jpg -------------------------------------------------------------------------------- /data/stereo_images/left17.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/stereo_images/left17.jpg -------------------------------------------------------------------------------- /data/stereo_images/left18.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/stereo_images/left18.jpg -------------------------------------------------------------------------------- /data/stereo_images/left19.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/stereo_images/left19.jpg -------------------------------------------------------------------------------- /data/stereo_images/left2.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/stereo_images/left2.jpg -------------------------------------------------------------------------------- /data/stereo_images/left20.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/stereo_images/left20.jpg -------------------------------------------------------------------------------- /data/stereo_images/left21.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/stereo_images/left21.jpg -------------------------------------------------------------------------------- /data/stereo_images/left22.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/stereo_images/left22.jpg -------------------------------------------------------------------------------- /data/stereo_images/left23.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/stereo_images/left23.jpg -------------------------------------------------------------------------------- /data/stereo_images/left24.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/stereo_images/left24.jpg -------------------------------------------------------------------------------- /data/stereo_images/left25.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/stereo_images/left25.jpg -------------------------------------------------------------------------------- /data/stereo_images/left26.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/stereo_images/left26.jpg -------------------------------------------------------------------------------- /data/stereo_images/left27.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/stereo_images/left27.jpg -------------------------------------------------------------------------------- /data/stereo_images/left28.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/stereo_images/left28.jpg -------------------------------------------------------------------------------- /data/stereo_images/left29.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/stereo_images/left29.jpg -------------------------------------------------------------------------------- /data/stereo_images/left3.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/stereo_images/left3.jpg -------------------------------------------------------------------------------- /data/stereo_images/left4.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/stereo_images/left4.jpg -------------------------------------------------------------------------------- /data/stereo_images/left5.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/stereo_images/left5.jpg -------------------------------------------------------------------------------- /data/stereo_images/left6.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/stereo_images/left6.jpg -------------------------------------------------------------------------------- /data/stereo_images/left7.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/stereo_images/left7.jpg -------------------------------------------------------------------------------- /data/stereo_images/left8.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/stereo_images/left8.jpg -------------------------------------------------------------------------------- /data/stereo_images/left9.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/stereo_images/left9.jpg -------------------------------------------------------------------------------- /data/stereo_images/right1.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/stereo_images/right1.jpg -------------------------------------------------------------------------------- /data/stereo_images/right10.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/stereo_images/right10.jpg -------------------------------------------------------------------------------- /data/stereo_images/right11.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/stereo_images/right11.jpg -------------------------------------------------------------------------------- /data/stereo_images/right12.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/stereo_images/right12.jpg -------------------------------------------------------------------------------- /data/stereo_images/right13.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/stereo_images/right13.jpg -------------------------------------------------------------------------------- /data/stereo_images/right14.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/stereo_images/right14.jpg -------------------------------------------------------------------------------- /data/stereo_images/right15.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/stereo_images/right15.jpg -------------------------------------------------------------------------------- /data/stereo_images/right16.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/stereo_images/right16.jpg -------------------------------------------------------------------------------- /data/stereo_images/right17.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/stereo_images/right17.jpg -------------------------------------------------------------------------------- /data/stereo_images/right18.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/stereo_images/right18.jpg -------------------------------------------------------------------------------- /data/stereo_images/right19.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/stereo_images/right19.jpg -------------------------------------------------------------------------------- /data/stereo_images/right2.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/stereo_images/right2.jpg -------------------------------------------------------------------------------- /data/stereo_images/right20.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/stereo_images/right20.jpg -------------------------------------------------------------------------------- /data/stereo_images/right21.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/stereo_images/right21.jpg -------------------------------------------------------------------------------- /data/stereo_images/right22.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/stereo_images/right22.jpg -------------------------------------------------------------------------------- /data/stereo_images/right23.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/stereo_images/right23.jpg -------------------------------------------------------------------------------- /data/stereo_images/right24.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/stereo_images/right24.jpg -------------------------------------------------------------------------------- /data/stereo_images/right25.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/stereo_images/right25.jpg -------------------------------------------------------------------------------- /data/stereo_images/right26.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/stereo_images/right26.jpg -------------------------------------------------------------------------------- /data/stereo_images/right27.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/stereo_images/right27.jpg -------------------------------------------------------------------------------- /data/stereo_images/right28.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/stereo_images/right28.jpg -------------------------------------------------------------------------------- /data/stereo_images/right29.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/stereo_images/right29.jpg -------------------------------------------------------------------------------- /data/stereo_images/right3.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/stereo_images/right3.jpg -------------------------------------------------------------------------------- /data/stereo_images/right4.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/stereo_images/right4.jpg -------------------------------------------------------------------------------- /data/stereo_images/right5.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/stereo_images/right5.jpg -------------------------------------------------------------------------------- /data/stereo_images/right6.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/stereo_images/right6.jpg -------------------------------------------------------------------------------- /data/stereo_images/right7.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/stereo_images/right7.jpg -------------------------------------------------------------------------------- /data/stereo_images/right8.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/stereo_images/right8.jpg -------------------------------------------------------------------------------- /data/stereo_images/right9.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/libing64/camera_model/1b8f622762056ffe799be63e27df9174135fe8e7/data/stereo_images/right9.jpg -------------------------------------------------------------------------------- /include/camera_model/calib/CameraCalibration.h: -------------------------------------------------------------------------------- 1 | #ifndef CAMERACALIBRATION_H 2 | #define CAMERACALIBRATION_H 3 | 4 | #include 5 | 6 | #include "camera_model/camera_models/Camera.h" 7 | 8 | namespace camera_model 9 | { 10 | 11 | class CameraCalibration 12 | { 13 | public: 14 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 15 | CameraCalibration(); 16 | 17 | CameraCalibration(Camera::ModelType modelType, 18 | const std::string& cameraName, 19 | const cv::Size& imageSize, 20 | const cv::Size& boardSize, 21 | float squareSize); 22 | 23 | void clear(void); 24 | 25 | void addChessboardData(const std::vector& corners); 26 | void addCornersData(const std::vector &corners, const std::vector& scenePoints); 27 | 28 | bool calibrate(void); 29 | 30 | int sampleCount(void) const; 31 | std::vector >& imagePoints(void); 32 | const std::vector >& imagePoints(void) const; 33 | std::vector >& scenePoints(void); 34 | const std::vector >& scenePoints(void) const; 35 | CameraPtr& camera(void); 36 | const CameraConstPtr camera(void) const; 37 | 38 | Eigen::Matrix2d& measurementCovariance(void); 39 | const Eigen::Matrix2d& measurementCovariance(void) const; 40 | 41 | cv::Mat& cameraPoses(void); 42 | const cv::Mat& cameraPoses(void) const; 43 | 44 | void drawResults(std::vector& images) const; 45 | 46 | void writeParams(const std::string& filename) const; 47 | 48 | bool writeChessboardData(const std::string& filename) const; 49 | bool readChessboardData(const std::string& filename); 50 | 51 | void setVerbose(bool verbose); 52 | 53 | private: 54 | bool calibrateHelper(CameraPtr& camera, 55 | std::vector& rvecs, std::vector& tvecs) const; 56 | 57 | void optimize(CameraPtr& camera, 58 | std::vector& rvecs, std::vector& tvecs) const; 59 | 60 | template 61 | void readData(std::ifstream& ifs, T& data) const; 62 | 63 | template 64 | void writeData(std::ofstream& ofs, T data) const; 65 | 66 | cv::Size m_boardSize; 67 | float m_squareSize; 68 | 69 | CameraPtr m_camera; 70 | cv::Mat m_cameraPoses; 71 | 72 | std::vector > m_imagePoints; 73 | std::vector > m_scenePoints; 74 | 75 | Eigen::Matrix2d m_measurementCovariance; 76 | 77 | bool m_verbose; 78 | }; 79 | 80 | } 81 | 82 | #endif 83 | -------------------------------------------------------------------------------- /include/camera_model/calib/StereoCameraCalibration.h: -------------------------------------------------------------------------------- 1 | #ifndef STEREOCAMERACALIBRATION_H 2 | #define STEREOCAMERACALIBRATION_H 3 | 4 | #include "camera_model/calib/CameraCalibration.h" 5 | 6 | namespace camera_model 7 | { 8 | 9 | class StereoCameraCalibration 10 | { 11 | public: 12 | StereoCameraCalibration(Camera::ModelType modelType, 13 | const std::string& cameraLeftName, 14 | const std::string& cameraRightName, 15 | const cv::Size& imageSize, 16 | const cv::Size& boardSize, 17 | float squareSize); 18 | 19 | void clear(void); 20 | 21 | void addChessboardData(const std::vector& cornersLeft, 22 | const std::vector& cornersRight); 23 | 24 | void addCornersData(const std::vector &cornersL, 25 | const std::vector &cornersR, 26 | const std::vector &scenePoints); 27 | bool calibrate(void); 28 | 29 | int sampleCount(void) const; 30 | const std::vector >& imagePointsLeft(void) const; 31 | const std::vector >& imagePointsRight(void) const; 32 | const std::vector >& scenePoints(void) const; 33 | 34 | CameraPtr& cameraLeft(void); 35 | const CameraConstPtr cameraLeft(void) const; 36 | 37 | CameraPtr& cameraRight(void); 38 | const CameraConstPtr cameraRight(void) const; 39 | 40 | void drawResults(std::vector& imagesLeft, 41 | std::vector& imagesRight) const; 42 | 43 | void writeParams(const std::string& directory) const; 44 | void setVerbose(bool verbose); 45 | 46 | private: 47 | CameraCalibration m_calibLeft; 48 | CameraCalibration m_calibRight; 49 | 50 | Eigen::Quaterniond m_q; 51 | Eigen::Vector3d m_t; 52 | 53 | bool m_verbose; 54 | }; 55 | 56 | } 57 | 58 | #endif 59 | -------------------------------------------------------------------------------- /include/camera_model/camera_models/Camera.h: -------------------------------------------------------------------------------- 1 | #ifndef CAMERA_H 2 | #define CAMERA_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | namespace camera_model 10 | { 11 | 12 | class Camera 13 | { 14 | public: 15 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 16 | enum ModelType 17 | { 18 | KANNALA_BRANDT, 19 | MEI, 20 | PINHOLE, 21 | SCARAMUZZA 22 | }; 23 | enum PatternType 24 | { 25 | CHESSBOARD, 26 | CIRCLES_GRID, 27 | ASYMMETRIC_CIRCLES_GRID, 28 | ARUCO, 29 | CHARUCO, 30 | }; 31 | class Parameters 32 | { 33 | public: 34 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 35 | Parameters(ModelType modelType); 36 | 37 | Parameters(ModelType modelType, const std::string& cameraName, 38 | int w, int h); 39 | 40 | ModelType& modelType(void); 41 | std::string& cameraName(void); 42 | int& imageWidth(void); 43 | int& imageHeight(void); 44 | 45 | ModelType modelType(void) const; 46 | const std::string& cameraName(void) const; 47 | int imageWidth(void) const; 48 | int imageHeight(void) const; 49 | 50 | int nIntrinsics(void) const; 51 | 52 | virtual bool readFromYamlFile(const std::string& filename) = 0; 53 | virtual void writeToYamlFile(const std::string& filename) const = 0; 54 | 55 | protected: 56 | ModelType m_modelType; 57 | int m_nIntrinsics; 58 | std::string m_cameraName; 59 | int m_imageWidth; 60 | int m_imageHeight; 61 | }; 62 | 63 | virtual ModelType modelType(void) const = 0; 64 | virtual const std::string& cameraName(void) const = 0; 65 | virtual int imageWidth(void) const = 0; 66 | virtual int imageHeight(void) const = 0; 67 | 68 | virtual cv::Mat& mask(void); 69 | virtual const cv::Mat& mask(void) const; 70 | 71 | virtual void estimateIntrinsics(const cv::Size& boardSize, 72 | const std::vector< std::vector >& objectPoints, 73 | const std::vector< std::vector >& imagePoints) = 0; 74 | virtual void estimateExtrinsics(const std::vector& objectPoints, 75 | const std::vector& imagePoints, 76 | cv::Mat& rvec, cv::Mat& tvec) const; 77 | 78 | // Lift points from the image plane to the sphere 79 | virtual void liftSphere(const Eigen::Vector2d& p, Eigen::Vector3d& P) const = 0; 80 | //%output P 81 | 82 | // Lift points from the image plane to the projective space 83 | virtual void liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const = 0; 84 | //%output P 85 | 86 | // Projects 3D points to the image plane (Pi function) 87 | virtual void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p) const = 0; 88 | //%output p 89 | 90 | // Projects 3D points to the image plane (Pi function) 91 | // and calculates jacobian 92 | //virtual void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p, 93 | // Eigen::Matrix& J) const = 0; 94 | //%output p 95 | //%output J 96 | 97 | virtual void undistToPlane(const Eigen::Vector2d& p_u, Eigen::Vector2d& p) const = 0; 98 | //%output p 99 | 100 | //virtual void initUndistortMap(cv::Mat& map1, cv::Mat& map2, double fScale = 1.0) const = 0; 101 | virtual cv::Mat initUndistortRectifyMap(cv::Mat& map1, cv::Mat& map2, 102 | float fx = -1.0f, float fy = -1.0f, 103 | cv::Size imageSize = cv::Size(0, 0), 104 | float cx = -1.0f, float cy = -1.0f, 105 | cv::Mat rmat = cv::Mat::eye(3, 3, CV_32F)) const = 0; 106 | 107 | virtual int parameterCount(void) const = 0; 108 | 109 | virtual void readParameters(const std::vector& parameters) = 0; 110 | virtual void writeParameters(std::vector& parameters) const = 0; 111 | 112 | virtual void writeParametersToYamlFile(const std::string& filename) const = 0; 113 | 114 | virtual std::string parametersToString(void) const = 0; 115 | 116 | /** 117 | * \brief Calculates the reprojection distance between points 118 | * 119 | * \param P1 first 3D point coordinates 120 | * \param P2 second 3D point coordinates 121 | * \return euclidean distance in the plane 122 | */ 123 | double reprojectionDist(const Eigen::Vector3d& P1, const Eigen::Vector3d& P2) const; 124 | 125 | double reprojectionError(const std::vector< std::vector >& objectPoints, 126 | const std::vector< std::vector >& imagePoints, 127 | const std::vector& rvecs, 128 | const std::vector& tvecs, 129 | cv::OutputArray perViewErrors = cv::noArray()) const; 130 | 131 | double reprojectionError(const Eigen::Vector3d& P, 132 | const Eigen::Quaterniond& camera_q, 133 | const Eigen::Vector3d& camera_t, 134 | const Eigen::Vector2d& observed_p) const; 135 | 136 | void projectPoints(const std::vector& objectPoints, 137 | const cv::Mat& rvec, 138 | const cv::Mat& tvec, 139 | std::vector& imagePoints) const; 140 | protected: 141 | cv::Mat m_mask; 142 | }; 143 | 144 | typedef boost::shared_ptr CameraPtr; 145 | typedef boost::shared_ptr CameraConstPtr; 146 | 147 | } 148 | 149 | #endif 150 | -------------------------------------------------------------------------------- /include/camera_model/camera_models/CameraFactory.h: -------------------------------------------------------------------------------- 1 | #ifndef CAMERAFACTORY_H 2 | #define CAMERAFACTORY_H 3 | 4 | #include 5 | #include 6 | 7 | #include "camera_model/camera_models/Camera.h" 8 | 9 | namespace camera_model 10 | { 11 | 12 | class CameraFactory 13 | { 14 | public: 15 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 16 | CameraFactory(); 17 | 18 | static boost::shared_ptr instance(void); 19 | 20 | CameraPtr generateCamera(Camera::ModelType modelType, 21 | const std::string& cameraName, 22 | cv::Size imageSize) const; 23 | 24 | CameraPtr generateCameraFromYamlFile(const std::string& filename); 25 | 26 | private: 27 | static boost::shared_ptr m_instance; 28 | }; 29 | 30 | } 31 | 32 | #endif 33 | -------------------------------------------------------------------------------- /include/camera_model/camera_models/CataCamera.h: -------------------------------------------------------------------------------- 1 | #ifndef CATACAMERA_H 2 | #define CATACAMERA_H 3 | 4 | #include 5 | #include 6 | 7 | #include "ceres/rotation.h" 8 | #include "Camera.h" 9 | 10 | namespace camera_model 11 | { 12 | 13 | /** 14 | * C. Mei, and P. Rives, Single View Point Omnidirectional Camera Calibration 15 | * from Planar Grids, ICRA 2007 16 | */ 17 | 18 | class CataCamera: public Camera 19 | { 20 | public: 21 | class Parameters: public Camera::Parameters 22 | { 23 | public: 24 | Parameters(); 25 | Parameters(const std::string& cameraName, 26 | int w, int h, 27 | double xi, 28 | double k1, double k2, double p1, double p2, 29 | double gamma1, double gamma2, double u0, double v0); 30 | 31 | double& xi(void); 32 | double& k1(void); 33 | double& k2(void); 34 | double& p1(void); 35 | double& p2(void); 36 | double& gamma1(void); 37 | double& gamma2(void); 38 | double& u0(void); 39 | double& v0(void); 40 | 41 | double xi(void) const; 42 | double k1(void) const; 43 | double k2(void) const; 44 | double p1(void) const; 45 | double p2(void) const; 46 | double gamma1(void) const; 47 | double gamma2(void) const; 48 | double u0(void) const; 49 | double v0(void) const; 50 | 51 | bool readFromYamlFile(const std::string& filename); 52 | void writeToYamlFile(const std::string& filename) const; 53 | 54 | Parameters& operator=(const Parameters& other); 55 | friend std::ostream& operator<< (std::ostream& out, const Parameters& params); 56 | 57 | private: 58 | double m_xi; 59 | double m_k1; 60 | double m_k2; 61 | double m_p1; 62 | double m_p2; 63 | double m_gamma1; 64 | double m_gamma2; 65 | double m_u0; 66 | double m_v0; 67 | }; 68 | 69 | CataCamera(); 70 | 71 | /** 72 | * \brief Constructor from the projection model parameters 73 | */ 74 | CataCamera(const std::string& cameraName, 75 | int imageWidth, int imageHeight, 76 | double xi, double k1, double k2, double p1, double p2, 77 | double gamma1, double gamma2, double u0, double v0); 78 | /** 79 | * \brief Constructor from the projection model parameters 80 | */ 81 | CataCamera(const Parameters& params); 82 | 83 | Camera::ModelType modelType(void) const; 84 | const std::string& cameraName(void) const; 85 | int imageWidth(void) const; 86 | int imageHeight(void) const; 87 | 88 | void estimateIntrinsics(const cv::Size& boardSize, 89 | const std::vector< std::vector >& objectPoints, 90 | const std::vector< std::vector >& imagePoints); 91 | 92 | // Lift points from the image plane to the sphere 93 | void liftSphere(const Eigen::Vector2d& p, Eigen::Vector3d& P) const; 94 | //%output P 95 | 96 | // Lift points from the image plane to the projective space 97 | void liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const; 98 | //%output P 99 | 100 | // Projects 3D points to the image plane (Pi function) 101 | void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p) const; 102 | //%output p 103 | 104 | // Projects 3D points to the image plane (Pi function) 105 | // and calculates jacobian 106 | void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p, 107 | Eigen::Matrix& J) const; 108 | //%output p 109 | //%output J 110 | 111 | void undistToPlane(const Eigen::Vector2d& p_u, Eigen::Vector2d& p) const; 112 | //%output p 113 | 114 | template 115 | static void spaceToPlane(const T* const params, 116 | const T* const q, const T* const t, 117 | const Eigen::Matrix& P, 118 | Eigen::Matrix& p); 119 | 120 | void distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u) const; 121 | void distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u, 122 | Eigen::Matrix2d& J) const; 123 | 124 | void initUndistortMap(cv::Mat& map1, cv::Mat& map2, double fScale = 1.0) const; 125 | cv::Mat initUndistortRectifyMap(cv::Mat& map1, cv::Mat& map2, 126 | float fx = -1.0f, float fy = -1.0f, 127 | cv::Size imageSize = cv::Size(0, 0), 128 | float cx = -1.0f, float cy = -1.0f, 129 | cv::Mat rmat = cv::Mat::eye(3, 3, CV_32F)) const; 130 | 131 | int parameterCount(void) const; 132 | 133 | const Parameters& getParameters(void) const; 134 | void setParameters(const Parameters& parameters); 135 | 136 | void readParameters(const std::vector& parameterVec); 137 | void writeParameters(std::vector& parameterVec) const; 138 | 139 | void writeParametersToYamlFile(const std::string& filename) const; 140 | 141 | std::string parametersToString(void) const; 142 | 143 | private: 144 | Parameters mParameters; 145 | 146 | double m_inv_K11, m_inv_K13, m_inv_K22, m_inv_K23; 147 | bool m_noDistortion; 148 | }; 149 | 150 | typedef boost::shared_ptr CataCameraPtr; 151 | typedef boost::shared_ptr CataCameraConstPtr; 152 | 153 | template 154 | void 155 | CataCamera::spaceToPlane(const T* const params, 156 | const T* const q, const T* const t, 157 | const Eigen::Matrix& P, 158 | Eigen::Matrix& p) 159 | { 160 | T P_w[3]; 161 | P_w[0] = T(P(0)); 162 | P_w[1] = T(P(1)); 163 | P_w[2] = T(P(2)); 164 | 165 | // Convert quaternion from Eigen convention (x, y, z, w) 166 | // to Ceres convention (w, x, y, z) 167 | T q_ceres[4] = {q[3], q[0], q[1], q[2]}; 168 | 169 | T P_c[3]; 170 | ceres::QuaternionRotatePoint(q_ceres, P_w, P_c); 171 | 172 | P_c[0] += t[0]; 173 | P_c[1] += t[1]; 174 | P_c[2] += t[2]; 175 | 176 | // project 3D object point to the image plane 177 | T xi = params[0]; 178 | T k1 = params[1]; 179 | T k2 = params[2]; 180 | T p1 = params[3]; 181 | T p2 = params[4]; 182 | T gamma1 = params[5]; 183 | T gamma2 = params[6]; 184 | T alpha = T(0); //cameraParams.alpha(); 185 | T u0 = params[7]; 186 | T v0 = params[8]; 187 | 188 | // Transform to model plane 189 | T len = sqrt(P_c[0] * P_c[0] + P_c[1] * P_c[1] + P_c[2] * P_c[2]); 190 | P_c[0] /= len; 191 | P_c[1] /= len; 192 | P_c[2] /= len; 193 | 194 | T u = P_c[0] / (P_c[2] + xi); 195 | T v = P_c[1] / (P_c[2] + xi); 196 | 197 | T rho_sqr = u * u + v * v; 198 | T L = T(1.0) + k1 * rho_sqr + k2 * rho_sqr * rho_sqr; 199 | T du = T(2.0) * p1 * u * v + p2 * (rho_sqr + T(2.0) * u * u); 200 | T dv = p1 * (rho_sqr + T(2.0) * v * v) + T(2.0) * p2 * u * v; 201 | 202 | u = L * u + du; 203 | v = L * v + dv; 204 | p(0) = gamma1 * (u + alpha * v) + u0; 205 | p(1) = gamma2 * v + v0; 206 | } 207 | 208 | } 209 | 210 | #endif 211 | -------------------------------------------------------------------------------- /include/camera_model/camera_models/CostFunctionFactory.h: -------------------------------------------------------------------------------- 1 | #ifndef COSTFUNCTIONFACTORY_H 2 | #define COSTFUNCTIONFACTORY_H 3 | 4 | #include 5 | #include 6 | 7 | #include "camera_model/camera_models/Camera.h" 8 | 9 | namespace ceres 10 | { 11 | class CostFunction; 12 | } 13 | 14 | namespace camera_model 15 | { 16 | 17 | enum 18 | { 19 | CAMERA_INTRINSICS = 1 << 0, 20 | CAMERA_POSE = 1 << 1, 21 | POINT_3D = 1 << 2, 22 | ODOMETRY_INTRINSICS = 1 << 3, 23 | ODOMETRY_3D_POSE = 1 << 4, 24 | ODOMETRY_6D_POSE = 1 << 5, 25 | CAMERA_ODOMETRY_TRANSFORM = 1 << 6 26 | }; 27 | 28 | class CostFunctionFactory 29 | { 30 | public: 31 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 32 | CostFunctionFactory(); 33 | 34 | static boost::shared_ptr instance(void); 35 | 36 | ceres::CostFunction* generateCostFunction(const CameraConstPtr& camera, 37 | const Eigen::Vector3d& observed_P, 38 | const Eigen::Vector2d& observed_p, 39 | int flags) const; 40 | 41 | ceres::CostFunction* generateCostFunction(const CameraConstPtr& camera, 42 | const Eigen::Vector3d& observed_P, 43 | const Eigen::Vector2d& observed_p, 44 | const Eigen::Matrix2d& sqrtPrecisionMat, 45 | int flags) const; 46 | 47 | ceres::CostFunction* generateCostFunction(const CameraConstPtr& camera, 48 | const Eigen::Vector2d& observed_p, 49 | int flags, bool optimize_cam_odo_z = true) const; 50 | 51 | ceres::CostFunction* generateCostFunction(const CameraConstPtr& camera, 52 | const Eigen::Vector2d& observed_p, 53 | const Eigen::Matrix2d& sqrtPrecisionMat, 54 | int flags, bool optimize_cam_odo_z = true) const; 55 | 56 | ceres::CostFunction* generateCostFunction(const CameraConstPtr& camera, 57 | const Eigen::Vector3d& odo_pos, 58 | const Eigen::Vector3d& odo_att, 59 | const Eigen::Vector2d& observed_p, 60 | int flags, bool optimize_cam_odo_z = true) const; 61 | 62 | ceres::CostFunction* generateCostFunction(const CameraConstPtr& camera, 63 | const Eigen::Quaterniond& cam_odo_q, 64 | const Eigen::Vector3d& cam_odo_t, 65 | const Eigen::Vector3d& odo_pos, 66 | const Eigen::Vector3d& odo_att, 67 | const Eigen::Vector2d& observed_p, 68 | int flags) const; 69 | 70 | ceres::CostFunction* generateCostFunction(const CameraConstPtr& cameraLeft, 71 | const CameraConstPtr& cameraRight, 72 | const Eigen::Vector3d& observed_P, 73 | const Eigen::Vector2d& observed_p_left, 74 | const Eigen::Vector2d& observed_p_right) const; 75 | 76 | private: 77 | static boost::shared_ptr m_instance; 78 | }; 79 | 80 | } 81 | 82 | #endif 83 | -------------------------------------------------------------------------------- /include/camera_model/camera_models/EquidistantCamera.h: -------------------------------------------------------------------------------- 1 | #ifndef EQUIDISTANTCAMERA_H 2 | #define EQUIDISTANTCAMERA_H 3 | 4 | #include 5 | #include 6 | 7 | #include "ceres/rotation.h" 8 | #include "Camera.h" 9 | 10 | namespace camera_model 11 | { 12 | 13 | /** 14 | * J. Kannala, and S. Brandt, A Generic Camera Model and Calibration Method 15 | * for Conventional, Wide-Angle, and Fish-Eye Lenses, PAMI 2006 16 | */ 17 | 18 | class EquidistantCamera: public Camera 19 | { 20 | public: 21 | class Parameters: public Camera::Parameters 22 | { 23 | public: 24 | Parameters(); 25 | Parameters(const std::string& cameraName, 26 | int w, int h, 27 | double k2, double k3, double k4, double k5, 28 | double mu, double mv, 29 | double u0, double v0); 30 | 31 | double& k2(void); 32 | double& k3(void); 33 | double& k4(void); 34 | double& k5(void); 35 | double& mu(void); 36 | double& mv(void); 37 | double& u0(void); 38 | double& v0(void); 39 | 40 | double k2(void) const; 41 | double k3(void) const; 42 | double k4(void) const; 43 | double k5(void) const; 44 | double mu(void) const; 45 | double mv(void) const; 46 | double u0(void) const; 47 | double v0(void) const; 48 | 49 | bool readFromYamlFile(const std::string& filename); 50 | void writeToYamlFile(const std::string& filename) const; 51 | 52 | Parameters& operator=(const Parameters& other); 53 | friend std::ostream& operator<< (std::ostream& out, const Parameters& params); 54 | 55 | private: 56 | // projection 57 | double m_k2; 58 | double m_k3; 59 | double m_k4; 60 | double m_k5; 61 | 62 | double m_mu; 63 | double m_mv; 64 | double m_u0; 65 | double m_v0; 66 | }; 67 | 68 | EquidistantCamera(); 69 | 70 | /** 71 | * \brief Constructor from the projection model parameters 72 | */ 73 | EquidistantCamera(const std::string& cameraName, 74 | int imageWidth, int imageHeight, 75 | double k2, double k3, double k4, double k5, 76 | double mu, double mv, 77 | double u0, double v0); 78 | /** 79 | * \brief Constructor from the projection model parameters 80 | */ 81 | EquidistantCamera(const Parameters& params); 82 | 83 | Camera::ModelType modelType(void) const; 84 | const std::string& cameraName(void) const; 85 | int imageWidth(void) const; 86 | int imageHeight(void) const; 87 | 88 | void estimateIntrinsics(const cv::Size& boardSize, 89 | const std::vector< std::vector >& objectPoints, 90 | const std::vector< std::vector >& imagePoints); 91 | 92 | // Lift points from the image plane to the sphere 93 | virtual void liftSphere(const Eigen::Vector2d& p, Eigen::Vector3d& P) const; 94 | //%output P 95 | 96 | // Lift points from the image plane to the projective space 97 | void liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const; 98 | //%output P 99 | 100 | // Projects 3D points to the image plane (Pi function) 101 | void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p) const; 102 | //%output p 103 | 104 | // Projects 3D points to the image plane (Pi function) 105 | // and calculates jacobian 106 | void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p, 107 | Eigen::Matrix& J) const; 108 | //%output p 109 | //%output J 110 | 111 | void undistToPlane(const Eigen::Vector2d& p_u, Eigen::Vector2d& p) const; 112 | //%output p 113 | 114 | template 115 | static void spaceToPlane(const T* const params, 116 | const T* const q, const T* const t, 117 | const Eigen::Matrix& P, 118 | Eigen::Matrix& p); 119 | 120 | void initUndistortMap(cv::Mat& map1, cv::Mat& map2, double fScale = 1.0) const; 121 | cv::Mat initUndistortRectifyMap(cv::Mat& map1, cv::Mat& map2, 122 | float fx = -1.0f, float fy = -1.0f, 123 | cv::Size imageSize = cv::Size(0, 0), 124 | float cx = -1.0f, float cy = -1.0f, 125 | cv::Mat rmat = cv::Mat::eye(3, 3, CV_32F)) const; 126 | 127 | int parameterCount(void) const; 128 | 129 | const Parameters& getParameters(void) const; 130 | void setParameters(const Parameters& parameters); 131 | 132 | void readParameters(const std::vector& parameterVec); 133 | void writeParameters(std::vector& parameterVec) const; 134 | 135 | void writeParametersToYamlFile(const std::string& filename) const; 136 | 137 | std::string parametersToString(void) const; 138 | 139 | private: 140 | template 141 | static T r(T k2, T k3, T k4, T k5, T theta); 142 | 143 | 144 | void fitOddPoly(const std::vector& x, const std::vector& y, 145 | int n, std::vector& coeffs) const; 146 | 147 | void backprojectSymmetric(const Eigen::Vector2d& p_u, 148 | double& theta, double& phi) const; 149 | 150 | Parameters mParameters; 151 | 152 | double m_inv_K11, m_inv_K13, m_inv_K22, m_inv_K23; 153 | }; 154 | 155 | typedef boost::shared_ptr EquidistantCameraPtr; 156 | typedef boost::shared_ptr EquidistantCameraConstPtr; 157 | 158 | template 159 | T 160 | EquidistantCamera::r(T k2, T k3, T k4, T k5, T theta) 161 | { 162 | // k1 = 1 163 | return theta + 164 | k2 * theta * theta * theta + 165 | k3 * theta * theta * theta * theta * theta + 166 | k4 * theta * theta * theta * theta * theta * theta * theta + 167 | k5 * theta * theta * theta * theta * theta * theta * theta * theta * theta; 168 | } 169 | 170 | template 171 | void 172 | EquidistantCamera::spaceToPlane(const T* const params, 173 | const T* const q, const T* const t, 174 | const Eigen::Matrix& P, 175 | Eigen::Matrix& p) 176 | { 177 | T P_w[3]; 178 | P_w[0] = T(P(0)); 179 | P_w[1] = T(P(1)); 180 | P_w[2] = T(P(2)); 181 | 182 | // Convert quaternion from Eigen convention (x, y, z, w) 183 | // to Ceres convention (w, x, y, z) 184 | T q_ceres[4] = {q[3], q[0], q[1], q[2]}; 185 | 186 | T P_c[3]; 187 | ceres::QuaternionRotatePoint(q_ceres, P_w, P_c); 188 | 189 | P_c[0] += t[0]; 190 | P_c[1] += t[1]; 191 | P_c[2] += t[2]; 192 | 193 | // project 3D object point to the image plane; 194 | T k2 = params[0]; 195 | T k3 = params[1]; 196 | T k4 = params[2]; 197 | T k5 = params[3]; 198 | T mu = params[4]; 199 | T mv = params[5]; 200 | T u0 = params[6]; 201 | T v0 = params[7]; 202 | 203 | T len = sqrt(P_c[0] * P_c[0] + P_c[1] * P_c[1] + P_c[2] * P_c[2]); 204 | T theta = acos(P_c[2] / len); 205 | T phi = atan2(P_c[1], P_c[0]); 206 | 207 | Eigen::Matrix p_u = r(k2, k3, k4, k5, theta) * Eigen::Matrix(cos(phi), sin(phi)); 208 | 209 | p(0) = mu * p_u(0) + u0; 210 | p(1) = mv * p_u(1) + v0; 211 | } 212 | 213 | } 214 | 215 | #endif 216 | -------------------------------------------------------------------------------- /include/camera_model/camera_models/PinholeCamera.h: -------------------------------------------------------------------------------- 1 | #ifndef PINHOLECAMERA_H 2 | #define PINHOLECAMERA_H 3 | 4 | #include 5 | #include 6 | 7 | #include "ceres/rotation.h" 8 | #include "Camera.h" 9 | 10 | namespace camera_model 11 | { 12 | 13 | class PinholeCamera: public Camera 14 | { 15 | public: 16 | class Parameters: public Camera::Parameters 17 | { 18 | public: 19 | Parameters(); 20 | Parameters(const std::string& cameraName, 21 | int w, int h, 22 | double k1, double k2, double p1, double p2, 23 | double fx, double fy, double cx, double cy); 24 | 25 | double& k1(void); 26 | double& k2(void); 27 | double& p1(void); 28 | double& p2(void); 29 | double& fx(void); 30 | double& fy(void); 31 | double& cx(void); 32 | double& cy(void); 33 | 34 | double xi(void) const; 35 | double k1(void) const; 36 | double k2(void) const; 37 | double p1(void) const; 38 | double p2(void) const; 39 | double fx(void) const; 40 | double fy(void) const; 41 | double cx(void) const; 42 | double cy(void) const; 43 | 44 | bool readFromYamlFile(const std::string& filename); 45 | void writeToYamlFile(const std::string& filename) const; 46 | 47 | Parameters& operator=(const Parameters& other); 48 | friend std::ostream& operator<< (std::ostream& out, const Parameters& params); 49 | 50 | private: 51 | double m_k1; 52 | double m_k2; 53 | double m_p1; 54 | double m_p2; 55 | double m_fx; 56 | double m_fy; 57 | double m_cx; 58 | double m_cy; 59 | }; 60 | 61 | PinholeCamera(); 62 | 63 | /** 64 | * \brief Constructor from the projection model parameters 65 | */ 66 | PinholeCamera(const std::string& cameraName, 67 | int imageWidth, int imageHeight, 68 | double k1, double k2, double p1, double p2, 69 | double fx, double fy, double cx, double cy); 70 | /** 71 | * \brief Constructor from the projection model parameters 72 | */ 73 | PinholeCamera(const Parameters& params); 74 | 75 | Camera::ModelType modelType(void) const; 76 | const std::string& cameraName(void) const; 77 | int imageWidth(void) const; 78 | int imageHeight(void) const; 79 | 80 | void estimateIntrinsics(const cv::Size& boardSize, 81 | const std::vector< std::vector >& objectPoints, 82 | const std::vector< std::vector >& imagePoints); 83 | 84 | // Lift points from the image plane to the sphere 85 | virtual void liftSphere(const Eigen::Vector2d& p, Eigen::Vector3d& P) const; 86 | //%output P 87 | 88 | // Lift points from the image plane to the projective space 89 | void liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const; 90 | //%output P 91 | 92 | // Projects 3D points to the image plane (Pi function) 93 | void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p) const; 94 | //%output p 95 | 96 | // Projects 3D points to the image plane (Pi function) 97 | // and calculates jacobian 98 | void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p, 99 | Eigen::Matrix& J) const; 100 | //%output p 101 | //%output J 102 | 103 | void undistToPlane(const Eigen::Vector2d& p_u, Eigen::Vector2d& p) const; 104 | //%output p 105 | 106 | template 107 | static void spaceToPlane(const T* const params, 108 | const T* const q, const T* const t, 109 | const Eigen::Matrix& P, 110 | Eigen::Matrix& p); 111 | 112 | void distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u) const; 113 | void distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u, 114 | Eigen::Matrix2d& J) const; 115 | 116 | void initUndistortMap(cv::Mat& map1, cv::Mat& map2, double fScale = 1.0) const; 117 | cv::Mat initUndistortRectifyMap(cv::Mat& map1, cv::Mat& map2, 118 | float fx = -1.0f, float fy = -1.0f, 119 | cv::Size imageSize = cv::Size(0, 0), 120 | float cx = -1.0f, float cy = -1.0f, 121 | cv::Mat rmat = cv::Mat::eye(3, 3, CV_32F)) const; 122 | 123 | int parameterCount(void) const; 124 | 125 | const Parameters& getParameters(void) const; 126 | void setParameters(const Parameters& parameters); 127 | 128 | void readParameters(const std::vector& parameterVec); 129 | void writeParameters(std::vector& parameterVec) const; 130 | 131 | void writeParametersToYamlFile(const std::string& filename) const; 132 | 133 | std::string parametersToString(void) const; 134 | 135 | private: 136 | Parameters mParameters; 137 | 138 | double m_inv_K11, m_inv_K13, m_inv_K22, m_inv_K23; 139 | bool m_noDistortion; 140 | }; 141 | 142 | typedef boost::shared_ptr PinholeCameraPtr; 143 | typedef boost::shared_ptr PinholeCameraConstPtr; 144 | 145 | template 146 | void 147 | PinholeCamera::spaceToPlane(const T* const params, 148 | const T* const q, const T* const t, 149 | const Eigen::Matrix& P, 150 | Eigen::Matrix& p) 151 | { 152 | T P_w[3]; 153 | P_w[0] = T(P(0)); 154 | P_w[1] = T(P(1)); 155 | P_w[2] = T(P(2)); 156 | 157 | // Convert quaternion from Eigen convention (x, y, z, w) 158 | // to Ceres convention (w, x, y, z) 159 | T q_ceres[4] = {q[3], q[0], q[1], q[2]}; 160 | 161 | T P_c[3]; 162 | ceres::QuaternionRotatePoint(q_ceres, P_w, P_c); 163 | 164 | P_c[0] += t[0]; 165 | P_c[1] += t[1]; 166 | P_c[2] += t[2]; 167 | 168 | // project 3D object point to the image plane 169 | T k1 = params[0]; 170 | T k2 = params[1]; 171 | T p1 = params[2]; 172 | T p2 = params[3]; 173 | T fx = params[4]; 174 | T fy = params[5]; 175 | T alpha = T(0); //cameraParams.alpha(); 176 | T cx = params[6]; 177 | T cy = params[7]; 178 | 179 | // Transform to model plane 180 | T u = P_c[0] / P_c[2]; 181 | T v = P_c[1] / P_c[2]; 182 | 183 | T rho_sqr = u * u + v * v; 184 | T L = T(1.0) + k1 * rho_sqr + k2 * rho_sqr * rho_sqr; 185 | T du = T(2.0) * p1 * u * v + p2 * (rho_sqr + T(2.0) * u * u); 186 | T dv = p1 * (rho_sqr + T(2.0) * v * v) + T(2.0) * p2 * u * v; 187 | 188 | u = L * u + du; 189 | v = L * v + dv; 190 | p(0) = fx * (u + alpha * v) + cx; 191 | p(1) = fy * v + cy; 192 | } 193 | 194 | } 195 | 196 | #endif 197 | -------------------------------------------------------------------------------- /include/camera_model/camera_models/ScaramuzzaCamera.h: -------------------------------------------------------------------------------- 1 | #ifndef SCARAMUZZACAMERA_H 2 | #define SCARAMUZZACAMERA_H 3 | 4 | #include 5 | #include 6 | 7 | #include "ceres/rotation.h" 8 | #include "Camera.h" 9 | 10 | namespace camera_model 11 | { 12 | 13 | #define SCARAMUZZA_POLY_SIZE 5 14 | #define SCARAMUZZA_INV_POLY_SIZE 20 15 | 16 | #define SCARAMUZZA_CAMERA_NUM_PARAMS (SCARAMUZZA_POLY_SIZE + SCARAMUZZA_INV_POLY_SIZE + 2 /*center*/ + 3 /*affine*/) 17 | 18 | /** 19 | * Scaramuzza Camera (Omnidirectional) 20 | * https://sites.google.com/site/scarabotix/ocamcalib-toolbox 21 | */ 22 | 23 | class OCAMCamera : public Camera 24 | { 25 | public: 26 | class Parameters : public Camera::Parameters 27 | { 28 | public: 29 | Parameters(); 30 | 31 | double &C(void) { return m_C; } 32 | double &D(void) { return m_D; } 33 | double &E(void) { return m_E; } 34 | 35 | double ¢er_x(void) { return m_center_x; } 36 | double ¢er_y(void) { return m_center_y; } 37 | 38 | double &poly(int idx) { return m_poly[idx]; } 39 | double &inv_poly(int idx) { return m_inv_poly[idx]; } 40 | 41 | double C(void) const { return m_C; } 42 | double D(void) const { return m_D; } 43 | double E(void) const { return m_E; } 44 | 45 | double center_x(void) const { return m_center_x; } 46 | double center_y(void) const { return m_center_y; } 47 | 48 | double poly(int idx) const { return m_poly[idx]; } 49 | double inv_poly(int idx) const { return m_inv_poly[idx]; } 50 | 51 | bool readFromYamlFile(const std::string &filename); 52 | void writeToYamlFile(const std::string &filename) const; 53 | 54 | Parameters &operator=(const Parameters &other); 55 | friend std::ostream &operator<<(std::ostream &out, const Parameters ¶ms); 56 | 57 | private: 58 | double m_poly[SCARAMUZZA_POLY_SIZE]; 59 | double m_inv_poly[SCARAMUZZA_INV_POLY_SIZE]; 60 | double m_C; 61 | double m_D; 62 | double m_E; 63 | double m_center_x; 64 | double m_center_y; 65 | }; 66 | 67 | OCAMCamera(); 68 | 69 | /** 70 | * \brief Constructor from the projection model parameters 71 | */ 72 | OCAMCamera(const Parameters ¶ms); 73 | 74 | Camera::ModelType modelType(void) const; 75 | const std::string &cameraName(void) const; 76 | int imageWidth(void) const; 77 | int imageHeight(void) const; 78 | 79 | void estimateIntrinsics(const cv::Size &boardSize, 80 | const std::vector> &objectPoints, 81 | const std::vector> &imagePoints); 82 | 83 | // Lift points from the image plane to the sphere 84 | void liftSphere(const Eigen::Vector2d &p, Eigen::Vector3d &P) const; 85 | //%output P 86 | 87 | // Lift points from the image plane to the projective space 88 | void liftProjective(const Eigen::Vector2d &p, Eigen::Vector3d &P) const; 89 | //%output P 90 | 91 | // Projects 3D points to the image plane (Pi function) 92 | void spaceToPlane(const Eigen::Vector3d &P, Eigen::Vector2d &p) const; 93 | //%output p 94 | 95 | // Projects 3D points to the image plane (Pi function) 96 | // and calculates jacobian 97 | //void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p, 98 | // Eigen::Matrix& J) const; 99 | //%output p 100 | //%output J 101 | 102 | void undistToPlane(const Eigen::Vector2d &p_u, Eigen::Vector2d &p) const; 103 | //%output p 104 | 105 | template 106 | static void spaceToPlane(const T *const params, 107 | const T *const q, const T *const t, 108 | const Eigen::Matrix &P, 109 | Eigen::Matrix &p); 110 | template 111 | static void spaceToSphere(const T *const params, 112 | const T *const q, const T *const t, 113 | const Eigen::Matrix &P, 114 | Eigen::Matrix &P_s); 115 | template 116 | static void LiftToSphere(const T *const params, 117 | const Eigen::Matrix &p, 118 | Eigen::Matrix &P); 119 | 120 | template 121 | static void SphereToPlane(const T *const params, const Eigen::Matrix &P, 122 | Eigen::Matrix &p); 123 | 124 | void initUndistortMap(cv::Mat &map1, cv::Mat &map2, double fScale = 1.0) const; 125 | cv::Mat initUndistortRectifyMap(cv::Mat &map1, cv::Mat &map2, 126 | float fx = -1.0f, float fy = -1.0f, 127 | cv::Size imageSize = cv::Size(0, 0), 128 | float cx = -1.0f, float cy = -1.0f, 129 | cv::Mat rmat = cv::Mat::eye(3, 3, CV_32F)) const; 130 | 131 | int parameterCount(void) const; 132 | 133 | const Parameters &getParameters(void) const; 134 | void setParameters(const Parameters ¶meters); 135 | 136 | void readParameters(const std::vector ¶meterVec); 137 | void writeParameters(std::vector ¶meterVec) const; 138 | 139 | void writeParametersToYamlFile(const std::string &filename) const; 140 | 141 | std::string parametersToString(void) const; 142 | 143 | private: 144 | Parameters mParameters; 145 | 146 | double m_inv_scale; 147 | }; 148 | 149 | typedef boost::shared_ptr OCAMCameraPtr; 150 | typedef boost::shared_ptr OCAMCameraConstPtr; 151 | 152 | template 153 | void 154 | OCAMCamera::spaceToPlane(const T *const params, 155 | const T *const q, const T *const t, 156 | const Eigen::Matrix &P, 157 | Eigen::Matrix &p) 158 | { 159 | T P_c[3]; 160 | { 161 | T P_w[3]; 162 | P_w[0] = T(P(0)); 163 | P_w[1] = T(P(1)); 164 | P_w[2] = T(P(2)); 165 | 166 | // Convert quaternion from Eigen convention (x, y, z, w) 167 | // to Ceres convention (w, x, y, z) 168 | T q_ceres[4] = {q[3], q[0], q[1], q[2]}; 169 | 170 | ceres::QuaternionRotatePoint(q_ceres, P_w, P_c); 171 | 172 | P_c[0] += t[0]; 173 | P_c[1] += t[1]; 174 | P_c[2] += t[2]; 175 | } 176 | 177 | T c = params[0]; 178 | T d = params[1]; 179 | T e = params[2]; 180 | T xc[2] = {params[3], params[4]}; 181 | 182 | //T poly[SCARAMUZZA_POLY_SIZE]; 183 | //for (int i=0; i < SCARAMUZZA_POLY_SIZE; i++) 184 | // poly[i] = params[5+i]; 185 | 186 | T inv_poly[SCARAMUZZA_INV_POLY_SIZE]; 187 | for (int i = 0; i < SCARAMUZZA_INV_POLY_SIZE; i++) 188 | inv_poly[i] = params[5 + SCARAMUZZA_POLY_SIZE + i]; 189 | 190 | T norm_sqr = P_c[0] * P_c[0] + P_c[1] * P_c[1]; 191 | T norm = T(0.0); 192 | if (norm_sqr > T(0.0)) 193 | norm = sqrt(norm_sqr); 194 | 195 | T theta = atan2(-P_c[2], norm); 196 | T rho = T(0.0); 197 | T theta_i = T(1.0); 198 | 199 | for (int i = 0; i < SCARAMUZZA_INV_POLY_SIZE; i++) 200 | { 201 | rho += theta_i * inv_poly[i]; 202 | theta_i *= theta; 203 | } 204 | 205 | T invNorm = T(1.0) / norm; 206 | T xn[2] = { 207 | P_c[0] * invNorm * rho, 208 | P_c[1] * invNorm * rho}; 209 | 210 | p(0) = xn[0] * c + xn[1] * d + xc[0]; 211 | p(1) = xn[0] * e + xn[1] + xc[1]; 212 | } 213 | 214 | template 215 | void 216 | OCAMCamera::spaceToSphere(const T *const params, 217 | const T *const q, const T *const t, 218 | const Eigen::Matrix &P, 219 | Eigen::Matrix &P_s) 220 | { 221 | T P_c[3]; 222 | { 223 | T P_w[3]; 224 | P_w[0] = T(P(0)); 225 | P_w[1] = T(P(1)); 226 | P_w[2] = T(P(2)); 227 | 228 | // Convert quaternion from Eigen convention (x, y, z, w) 229 | // to Ceres convention (w, x, y, z) 230 | T q_ceres[4] = {q[3], q[0], q[1], q[2]}; 231 | 232 | ceres::QuaternionRotatePoint(q_ceres, P_w, P_c); 233 | 234 | P_c[0] += t[0]; 235 | P_c[1] += t[1]; 236 | P_c[2] += t[2]; 237 | } 238 | 239 | //T poly[SCARAMUZZA_POLY_SIZE]; 240 | //for (int i=0; i < SCARAMUZZA_POLY_SIZE; i++) 241 | // poly[i] = params[5+i]; 242 | 243 | T norm_sqr = P_c[0] * P_c[0] + P_c[1] * P_c[1] + P_c[2] * P_c[2]; 244 | T norm = T(0.0); 245 | if (norm_sqr > T(0.0)) 246 | norm = sqrt(norm_sqr); 247 | 248 | P_s(0) = P_c[0] / norm; 249 | P_s(1) = P_c[1] / norm; 250 | P_s(2) = P_c[2] / norm; 251 | } 252 | 253 | template 254 | void 255 | OCAMCamera::LiftToSphere(const T *const params, 256 | const Eigen::Matrix &p, 257 | Eigen::Matrix &P) 258 | { 259 | T c = params[0]; 260 | T d = params[1]; 261 | T e = params[2]; 262 | T cc[2] = {params[3], params[4]}; 263 | T poly[SCARAMUZZA_POLY_SIZE]; 264 | for (int i = 0; i < SCARAMUZZA_POLY_SIZE; i++) 265 | poly[i] = params[5 + i]; 266 | 267 | // Relative to Center 268 | T p_2d[2]; 269 | p_2d[0] = T(p(0)); 270 | p_2d[1] = T(p(1)); 271 | 272 | T xc[2] = {p_2d[0] - cc[0], p_2d[1] - cc[1]}; 273 | 274 | T inv_scale = T(1.0) / (c - d * e); 275 | 276 | // Affine Transformation 277 | T xc_a[2]; 278 | 279 | xc_a[0] = inv_scale * (xc[0] - d * xc[1]); 280 | xc_a[1] = inv_scale * (-e * xc[0] + c * xc[1]); 281 | 282 | T norm_sqr = xc_a[0] * xc_a[0] + xc_a[1] * xc_a[1]; 283 | T phi = sqrt(norm_sqr); 284 | T phi_i = T(1.0); 285 | T z = T(0.0); 286 | 287 | for (int i = 0; i < SCARAMUZZA_POLY_SIZE; i++) 288 | { 289 | if (i != 1) 290 | { 291 | z += phi_i * poly[i]; 292 | } 293 | phi_i *= phi; 294 | } 295 | 296 | T p_3d[3]; 297 | p_3d[0] = xc[0]; 298 | p_3d[1] = xc[1]; 299 | p_3d[2] = -z; 300 | 301 | T p_3d_norm_sqr = p_3d[0] * p_3d[0] + p_3d[1] * p_3d[1] + p_3d[2] * p_3d[2]; 302 | T p_3d_norm = sqrt(p_3d_norm_sqr); 303 | 304 | P << p_3d[0] / p_3d_norm, p_3d[1] / p_3d_norm, p_3d[2] / p_3d_norm; 305 | } 306 | 307 | template 308 | void OCAMCamera::SphereToPlane(const T *const params, const Eigen::Matrix &P, 309 | Eigen::Matrix &p) 310 | { 311 | T P_c[3]; 312 | { 313 | P_c[0] = T(P(0)); 314 | P_c[1] = T(P(1)); 315 | P_c[2] = T(P(2)); 316 | } 317 | 318 | T c = params[0]; 319 | T d = params[1]; 320 | T e = params[2]; 321 | T xc[2] = {params[3], params[4]}; 322 | 323 | T inv_poly[SCARAMUZZA_INV_POLY_SIZE]; 324 | for (int i = 0; i < SCARAMUZZA_INV_POLY_SIZE; i++) 325 | inv_poly[i] = params[5 + SCARAMUZZA_POLY_SIZE + i]; 326 | 327 | T norm_sqr = P_c[0] * P_c[0] + P_c[1] * P_c[1]; 328 | T norm = T(0.0); 329 | if (norm_sqr > T(0.0)) 330 | norm = sqrt(norm_sqr); 331 | 332 | T theta = atan2(-P_c[2], norm); 333 | T rho = T(0.0); 334 | T theta_i = T(1.0); 335 | 336 | for (int i = 0; i < SCARAMUZZA_INV_POLY_SIZE; i++) 337 | { 338 | rho += theta_i * inv_poly[i]; 339 | theta_i *= theta; 340 | } 341 | 342 | T invNorm = T(1.0) / norm; 343 | T xn[2] = {P_c[0] * invNorm * rho, P_c[1] * invNorm * rho}; 344 | 345 | p(0) = xn[0] * c + xn[1] * d + xc[0]; 346 | p(1) = xn[0] * e + xn[1] + xc[1]; 347 | } 348 | } // namespace camera_model 349 | 350 | #endif 351 | -------------------------------------------------------------------------------- /include/camera_model/chessboard/Chessboard.h: -------------------------------------------------------------------------------- 1 | #ifndef CHESSBOARD_H 2 | #define CHESSBOARD_H 3 | 4 | #include 5 | #include 6 | 7 | namespace camera_model 8 | { 9 | 10 | // forward declarations 11 | class ChessboardCorner; 12 | typedef boost::shared_ptr ChessboardCornerPtr; 13 | class ChessboardQuad; 14 | typedef boost::shared_ptr ChessboardQuadPtr; 15 | 16 | class Chessboard 17 | { 18 | public: 19 | Chessboard(cv::Size boardSize, cv::Mat& image); 20 | 21 | void findCorners(bool useOpenCV = false); 22 | const std::vector& getCorners(void) const; 23 | bool cornersFound(void) const; 24 | 25 | const cv::Mat& getImage(void) const; 26 | const cv::Mat& getSketch(void) const; 27 | 28 | private: 29 | bool findChessboardCorners(const cv::Mat& image, 30 | const cv::Size& patternSize, 31 | std::vector& corners, 32 | int flags, bool useOpenCV); 33 | 34 | bool findChessboardCornersImproved(const cv::Mat& image, 35 | const cv::Size& patternSize, 36 | std::vector& corners, 37 | int flags); 38 | 39 | void cleanFoundConnectedQuads(std::vector& quadGroup, cv::Size patternSize); 40 | 41 | void findConnectedQuads(std::vector& quads, 42 | std::vector& group, 43 | int group_idx, int dilation); 44 | 45 | // int checkQuadGroup(std::vector& quadGroup, 46 | // std::vector& outCorners, 47 | // cv::Size patternSize); 48 | 49 | void labelQuadGroup(std::vector& quad_group, 50 | cv::Size patternSize, bool firstRun); 51 | 52 | void findQuadNeighbors(std::vector& quads, int dilation); 53 | 54 | int augmentBestRun(std::vector& candidateQuads, int candidateDilation, 55 | std::vector& existingQuads, int existingDilation); 56 | 57 | void generateQuads(std::vector& quads, 58 | cv::Mat& image, int flags, 59 | int dilation, bool firstRun); 60 | 61 | bool checkQuadGroup(std::vector& quads, 62 | std::vector& corners, 63 | cv::Size patternSize); 64 | 65 | void getQuadrangleHypotheses(const std::vector< std::vector >& contours, 66 | std::vector< std::pair >& quads, 67 | int classId) const; 68 | 69 | bool checkChessboard(const cv::Mat& image, cv::Size patternSize) const; 70 | 71 | bool checkBoardMonotony(std::vector& corners, 72 | cv::Size patternSize); 73 | 74 | bool matchCorners(ChessboardQuadPtr& quad1, int corner1, 75 | ChessboardQuadPtr& quad2, int corner2) const; 76 | 77 | cv::Mat mImage; 78 | cv::Mat mSketch; 79 | std::vector mCorners; 80 | cv::Size mBoardSize; 81 | bool mCornersFound; 82 | }; 83 | 84 | } 85 | 86 | #endif 87 | -------------------------------------------------------------------------------- /include/camera_model/chessboard/ChessboardCorner.h: -------------------------------------------------------------------------------- 1 | #ifndef CHESSBOARDCORNER_H 2 | #define CHESSBOARDCORNER_H 3 | 4 | #include 5 | #include 6 | 7 | namespace camera_model 8 | { 9 | 10 | class ChessboardCorner; 11 | typedef boost::shared_ptr ChessboardCornerPtr; 12 | 13 | class ChessboardCorner 14 | { 15 | public: 16 | ChessboardCorner() : row(0), column(0), needsNeighbor(true), count(0) {} 17 | 18 | float meanDist(int &n) const 19 | { 20 | float sum = 0; 21 | n = 0; 22 | for (int i = 0; i < 4; ++i) 23 | { 24 | if (neighbors[i].get()) 25 | { 26 | float dx = neighbors[i]->pt.x - pt.x; 27 | float dy = neighbors[i]->pt.y - pt.y; 28 | sum += sqrt(dx*dx + dy*dy); 29 | n++; 30 | } 31 | } 32 | return sum / std::max(n, 1); 33 | } 34 | 35 | cv::Point2f pt; // X and y coordinates 36 | int row; // Row and column of the corner 37 | int column; // in the found pattern 38 | bool needsNeighbor; // Does the corner require a neighbor? 39 | int count; // number of corner neighbors 40 | ChessboardCornerPtr neighbors[4]; // pointer to all corner neighbors 41 | }; 42 | 43 | } 44 | 45 | #endif 46 | -------------------------------------------------------------------------------- /include/camera_model/chessboard/ChessboardQuad.h: -------------------------------------------------------------------------------- 1 | #ifndef CHESSBOARDQUAD_H 2 | #define CHESSBOARDQUAD_H 3 | 4 | #include 5 | 6 | #include "camera_model/chessboard/ChessboardCorner.h" 7 | 8 | namespace camera_model 9 | { 10 | 11 | class ChessboardQuad; 12 | typedef boost::shared_ptr ChessboardQuadPtr; 13 | 14 | class ChessboardQuad 15 | { 16 | public: 17 | ChessboardQuad() : count(0), group_idx(-1), edge_len(FLT_MAX), labeled(false) {} 18 | 19 | int count; // Number of quad neighbors 20 | int group_idx; // Quad group ID 21 | float edge_len; // Smallest side length^2 22 | ChessboardCornerPtr corners[4]; // Coordinates of quad corners 23 | ChessboardQuadPtr neighbors[4]; // Pointers of quad neighbors 24 | bool labeled; // Has this corner been labeled? 25 | }; 26 | 27 | } 28 | 29 | #endif 30 | -------------------------------------------------------------------------------- /include/camera_model/chessboard/Spline.h: -------------------------------------------------------------------------------- 1 | /* dynamo:- Event driven molecular dynamics simulator 2 | http://www.marcusbannerman.co.uk/dynamo 3 | Copyright (C) 2011 Marcus N Campbell Bannerman 4 | 5 | This program is free software: you can redistribute it and/or 6 | modify it under the terms of the GNU General Public License 7 | version 3 as published by the Free Software Foundation. 8 | 9 | This program is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details. 13 | 14 | You should have received a copy of the GNU General Public License 15 | along with this program. If not, see . 16 | */ 17 | 18 | #pragma once 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | namespace ublas = boost::numeric::ublas; 28 | 29 | class Spline : private std::vector > 30 | { 31 | public: 32 | //The boundary conditions available 33 | enum BC_type { 34 | FIXED_1ST_DERIV_BC, 35 | FIXED_2ND_DERIV_BC, 36 | PARABOLIC_RUNOUT_BC 37 | }; 38 | 39 | enum Spline_type { 40 | LINEAR, 41 | CUBIC 42 | }; 43 | 44 | //Constructor takes the boundary conditions as arguments, this 45 | //sets the first derivative (gradient) at the lower and upper 46 | //end points 47 | Spline(): 48 | _valid(false), 49 | _BCLow(FIXED_2ND_DERIV_BC), _BCHigh(FIXED_2ND_DERIV_BC), 50 | _BCLowVal(0), _BCHighVal(0), 51 | _type(CUBIC) 52 | {} 53 | 54 | typedef std::vector > base; 55 | typedef base::const_iterator const_iterator; 56 | 57 | //Standard STL read-only container stuff 58 | const_iterator begin() const { return base::begin(); } 59 | const_iterator end() const { return base::end(); } 60 | void clear() { _valid = false; base::clear(); _data.clear(); } 61 | size_t size() const { return base::size(); } 62 | size_t max_size() const { return base::max_size(); } 63 | size_t capacity() const { return base::capacity(); } 64 | bool empty() const { return base::empty(); } 65 | 66 | //Add a point to the spline, and invalidate it so its 67 | //recalculated on the next access 68 | inline void addPoint(double x, double y) 69 | { 70 | _valid = false; 71 | base::push_back(std::pair(x,y)); 72 | } 73 | 74 | //Reset the boundary conditions 75 | inline void setLowBC(BC_type BC, double val = 0) 76 | { _BCLow = BC; _BCLowVal = val; _valid = false; } 77 | 78 | inline void setHighBC(BC_type BC, double val = 0) 79 | { _BCHigh = BC; _BCHighVal = val; _valid = false; } 80 | 81 | void setType(Spline_type type) { _type = type; _valid = false; } 82 | 83 | //Check if the spline has been calculated, then generate the 84 | //spline interpolated value 85 | double operator()(double xval) 86 | { 87 | if (!_valid) generate(); 88 | 89 | //Special cases when we're outside the range of the spline points 90 | if (xval <= x(0)) return lowCalc(xval); 91 | if (xval >= x(size()-1)) return highCalc(xval); 92 | 93 | //Check all intervals except the last one 94 | for (std::vector::const_iterator iPtr = _data.begin(); 95 | iPtr != _data.end()-1; ++iPtr) 96 | if ((xval >= iPtr->x) && (xval <= (iPtr+1)->x)) 97 | return splineCalc(iPtr, xval); 98 | 99 | return splineCalc(_data.end() - 1, xval); 100 | } 101 | 102 | private: 103 | 104 | ///////PRIVATE DATA MEMBERS 105 | struct SplineData { double x,a,b,c,d; }; 106 | //vector of calculated spline data 107 | std::vector _data; 108 | //Second derivative at each point 109 | ublas::vector _ddy; 110 | //Tracks whether the spline parameters have been calculated for 111 | //the current set of points 112 | bool _valid; 113 | //The boundary conditions 114 | BC_type _BCLow, _BCHigh; 115 | //The values of the boundary conditions 116 | double _BCLowVal, _BCHighVal; 117 | 118 | Spline_type _type; 119 | 120 | ///////PRIVATE FUNCTIONS 121 | //Function to calculate the value of a given spline at a point xval 122 | inline double splineCalc(std::vector::const_iterator i, double xval) 123 | { 124 | const double lx = xval - i->x; 125 | return ((i->a * lx + i->b) * lx + i->c) * lx + i->d; 126 | } 127 | 128 | inline double lowCalc(double xval) 129 | { 130 | const double lx = xval - x(0); 131 | 132 | if (_type == LINEAR) 133 | return lx * _BCHighVal + y(0); 134 | 135 | const double firstDeriv = (y(1) - y(0)) / h(0) - 2 * h(0) * (_data[0].b + 2 * _data[1].b) / 6; 136 | 137 | switch(_BCLow) 138 | { 139 | case FIXED_1ST_DERIV_BC: 140 | return lx * _BCLowVal + y(0); 141 | case FIXED_2ND_DERIV_BC: 142 | return lx * lx * _BCLowVal + firstDeriv * lx + y(0); 143 | case PARABOLIC_RUNOUT_BC: 144 | return lx * lx * _ddy[0] + lx * firstDeriv + y(0); 145 | } 146 | throw std::runtime_error("Unknown BC"); 147 | } 148 | 149 | inline double highCalc(double xval) 150 | { 151 | const double lx = xval - x(size() - 1); 152 | 153 | if (_type == LINEAR) 154 | return lx * _BCHighVal + y(size() - 1); 155 | 156 | const double firstDeriv = 2 * h(size() - 2) * (_ddy[size() - 2] + 2 * _ddy[size() - 1]) / 6 + (y(size() - 1) - y(size() - 2)) / h(size() - 2); 157 | 158 | switch(_BCHigh) 159 | { 160 | case FIXED_1ST_DERIV_BC: 161 | return lx * _BCHighVal + y(size() - 1); 162 | case FIXED_2ND_DERIV_BC: 163 | return lx * lx * _BCHighVal + firstDeriv * lx + y(size() - 1); 164 | case PARABOLIC_RUNOUT_BC: 165 | return lx * lx * _ddy[size()-1] + lx * firstDeriv + y(size() - 1); 166 | } 167 | throw std::runtime_error("Unknown BC"); 168 | } 169 | 170 | //These just provide access to the point data in a clean way 171 | inline double x(size_t i) const { return operator[](i).first; } 172 | inline double y(size_t i) const { return operator[](i).second; } 173 | inline double h(size_t i) const { return x(i+1) - x(i); } 174 | 175 | //Invert a arbitrary matrix using the boost ublas library 176 | template 177 | bool InvertMatrix(ublas::matrix A, 178 | ublas::matrix& inverse) 179 | { 180 | using namespace ublas; 181 | 182 | // create a permutation matrix for the LU-factorization 183 | permutation_matrix pm(A.size1()); 184 | 185 | // perform LU-factorization 186 | int res = lu_factorize(A,pm); 187 | if( res != 0 ) return false; 188 | 189 | // create identity matrix of "inverse" 190 | inverse.assign(ublas::identity_matrix(A.size1())); 191 | 192 | // backsubstitute to get the inverse 193 | lu_substitute(A, pm, inverse); 194 | 195 | return true; 196 | } 197 | 198 | //This function will recalculate the spline parameters and store 199 | //them in _data, ready for spline interpolation 200 | void generate() 201 | { 202 | if (size() < 2) 203 | throw std::runtime_error("Spline requires at least 2 points"); 204 | 205 | //If any spline points are at the same x location, we have to 206 | //just slightly seperate them 207 | { 208 | bool testPassed(false); 209 | while (!testPassed) 210 | { 211 | testPassed = true; 212 | std::sort(base::begin(), base::end()); 213 | 214 | for (base::iterator iPtr = base::begin(); iPtr != base::end() - 1; ++iPtr) 215 | if (iPtr->first == (iPtr+1)->first) 216 | { 217 | if ((iPtr+1)->first != 0) 218 | (iPtr+1)->first += (iPtr+1)->first 219 | * std::numeric_limits::epsilon() * 10; 220 | else 221 | (iPtr+1)->first = std::numeric_limits::epsilon() * 10; 222 | testPassed = false; 223 | break; 224 | } 225 | } 226 | } 227 | 228 | const size_t e = size() - 1; 229 | 230 | switch (_type) 231 | { 232 | case LINEAR: 233 | { 234 | _data.resize(e); 235 | for (size_t i(0); i < e; ++i) 236 | { 237 | _data[i].x = x(i); 238 | _data[i].a = 0; 239 | _data[i].b = 0; 240 | _data[i].c = (y(i+1) - y(i)) / (x(i+1) - x(i)); 241 | _data[i].d = y(i); 242 | } 243 | break; 244 | } 245 | case CUBIC: 246 | { 247 | ublas::matrix A(size(), size()); 248 | for (size_t yv(0); yv <= e; ++yv) 249 | for (size_t xv(0); xv <= e; ++xv) 250 | A(xv,yv) = 0; 251 | 252 | for (size_t i(1); i < e; ++i) 253 | { 254 | A(i-1,i) = h(i-1); 255 | A(i,i) = 2 * (h(i-1) + h(i)); 256 | A(i+1,i) = h(i); 257 | } 258 | 259 | ublas::vector C(size()); 260 | for (size_t xv(0); xv <= e; ++xv) 261 | C(xv) = 0; 262 | 263 | for (size_t i(1); i < e; ++i) 264 | C(i) = 6 * 265 | ((y(i+1) - y(i)) / h(i) 266 | - (y(i) - y(i-1)) / h(i-1)); 267 | 268 | //Boundary conditions 269 | switch(_BCLow) 270 | { 271 | case FIXED_1ST_DERIV_BC: 272 | C(0) = 6 * ((y(1) - y(0)) / h(0) - _BCLowVal); 273 | A(0,0) = 2 * h(0); 274 | A(1,0) = h(0); 275 | break; 276 | case FIXED_2ND_DERIV_BC: 277 | C(0) = _BCLowVal; 278 | A(0,0) = 1; 279 | break; 280 | case PARABOLIC_RUNOUT_BC: 281 | C(0) = 0; A(0,0) = 1; A(1,0) = -1; 282 | break; 283 | } 284 | 285 | switch(_BCHigh) 286 | { 287 | case FIXED_1ST_DERIV_BC: 288 | C(e) = 6 * (_BCHighVal - (y(e) - y(e-1)) / h(e-1)); 289 | A(e,e) = 2 * h(e - 1); 290 | A(e-1,e) = h(e - 1); 291 | break; 292 | case FIXED_2ND_DERIV_BC: 293 | C(e) = _BCHighVal; 294 | A(e,e) = 1; 295 | break; 296 | case PARABOLIC_RUNOUT_BC: 297 | C(e) = 0; A(e,e) = 1; A(e-1,e) = -1; 298 | break; 299 | } 300 | 301 | ublas::matrix AInv(size(), size()); 302 | InvertMatrix(A,AInv); 303 | 304 | _ddy = ublas::prod(C, AInv); 305 | 306 | _data.resize(size()-1); 307 | for (size_t i(0); i < e; ++i) 308 | { 309 | _data[i].x = x(i); 310 | _data[i].a = (_ddy(i+1) - _ddy(i)) / (6 * h(i)); 311 | _data[i].b = _ddy(i) / 2; 312 | _data[i].c = (y(i+1) - y(i)) / h(i) - _ddy(i+1) * h(i) / 6 - _ddy(i) * h(i) / 3; 313 | _data[i].d = y(i); 314 | } 315 | } 316 | } 317 | _valid = true; 318 | } 319 | }; 320 | -------------------------------------------------------------------------------- /include/camera_model/gpl/EigenQuaternionParameterization.h: -------------------------------------------------------------------------------- 1 | #ifndef EIGENQUATERNIONPARAMETERIZATION_H 2 | #define EIGENQUATERNIONPARAMETERIZATION_H 3 | 4 | #include "ceres/local_parameterization.h" 5 | 6 | namespace camera_model 7 | { 8 | 9 | class EigenQuaternionParameterization : public ceres::LocalParameterization 10 | { 11 | public: 12 | virtual ~EigenQuaternionParameterization() {} 13 | virtual bool Plus(const double* x, 14 | const double* delta, 15 | double* x_plus_delta) const; 16 | virtual bool ComputeJacobian(const double* x, 17 | double* jacobian) const; 18 | virtual int GlobalSize() const { return 4; } 19 | virtual int LocalSize() const { return 3; } 20 | 21 | private: 22 | template 23 | void EigenQuaternionProduct(const T z[4], const T w[4], T zw[4]) const; 24 | }; 25 | 26 | 27 | template 28 | void 29 | EigenQuaternionParameterization::EigenQuaternionProduct(const T z[4], const T w[4], T zw[4]) const 30 | { 31 | zw[0] = z[3] * w[0] + z[0] * w[3] + z[1] * w[2] - z[2] * w[1]; 32 | zw[1] = z[3] * w[1] - z[0] * w[2] + z[1] * w[3] + z[2] * w[0]; 33 | zw[2] = z[3] * w[2] + z[0] * w[1] - z[1] * w[0] + z[2] * w[3]; 34 | zw[3] = z[3] * w[3] - z[0] * w[0] - z[1] * w[1] - z[2] * w[2]; 35 | } 36 | 37 | } 38 | 39 | #endif 40 | 41 | -------------------------------------------------------------------------------- /include/camera_model/gpl/EigenUtils.h: -------------------------------------------------------------------------------- 1 | #ifndef EIGENUTILS_H 2 | #define EIGENUTILS_H 3 | 4 | #include 5 | 6 | #include "ceres/rotation.h" 7 | #include "camera_model/gpl/gpl.h" 8 | 9 | namespace camera_model 10 | { 11 | 12 | // Returns the 3D cross product skew symmetric matrix of a given 3D vector 13 | template 14 | Eigen::Matrix skew(const Eigen::Matrix& vec) 15 | { 16 | return (Eigen::Matrix() << T(0), -vec(2), vec(1), 17 | vec(2), T(0), -vec(0), 18 | -vec(1), vec(0), T(0)).finished(); 19 | } 20 | 21 | template 22 | typename Eigen::MatrixBase::PlainObject sqrtm(const Eigen::MatrixBase& A) 23 | { 24 | Eigen::SelfAdjointEigenSolver es(A); 25 | 26 | return es.operatorSqrt(); 27 | } 28 | 29 | template 30 | Eigen::Matrix AngleAxisToRotationMatrix(const Eigen::Matrix& rvec) 31 | { 32 | T angle = rvec.norm(); 33 | if (angle == T(0)) 34 | { 35 | return Eigen::Matrix::Identity(); 36 | } 37 | 38 | Eigen::Matrix axis; 39 | axis = rvec.normalized(); 40 | 41 | Eigen::Matrix rmat; 42 | rmat = Eigen::AngleAxis(angle, axis); 43 | 44 | return rmat; 45 | } 46 | 47 | template 48 | Eigen::Quaternion AngleAxisToQuaternion(const Eigen::Matrix& rvec) 49 | { 50 | Eigen::Matrix rmat = AngleAxisToRotationMatrix(rvec); 51 | 52 | return Eigen::Quaternion(rmat); 53 | } 54 | 55 | template 56 | void AngleAxisToQuaternion(const Eigen::Matrix& rvec, T* q) 57 | { 58 | Eigen::Quaternion quat = AngleAxisToQuaternion(rvec); 59 | 60 | q[0] = quat.x(); 61 | q[1] = quat.y(); 62 | q[2] = quat.z(); 63 | q[3] = quat.w(); 64 | } 65 | 66 | template 67 | Eigen::Matrix RotationToAngleAxis(const Eigen::Matrix & rmat) 68 | { 69 | Eigen::AngleAxis angleaxis; 70 | angleaxis.fromRotationMatrix(rmat); 71 | return angleaxis.angle() * angleaxis.axis(); 72 | 73 | } 74 | 75 | template 76 | void QuaternionToAngleAxis(const T* const q, Eigen::Matrix& rvec) 77 | { 78 | Eigen::Quaternion quat(q[3], q[0], q[1], q[2]); 79 | 80 | Eigen::Matrix rmat = quat.toRotationMatrix(); 81 | 82 | Eigen::AngleAxis angleaxis; 83 | angleaxis.fromRotationMatrix(rmat); 84 | 85 | rvec = angleaxis.angle() * angleaxis.axis(); 86 | } 87 | 88 | template 89 | Eigen::Matrix QuaternionToRotation(const T* const q) 90 | { 91 | T R[9]; 92 | ceres::QuaternionToRotation(q, R); 93 | 94 | Eigen::Matrix rmat; 95 | for (int i = 0; i < 3; ++i) 96 | { 97 | for (int j = 0; j < 3; ++j) 98 | { 99 | rmat(i,j) = R[i * 3 + j]; 100 | } 101 | } 102 | 103 | return rmat; 104 | } 105 | 106 | template 107 | void QuaternionToRotation(const T* const q, T* rot) 108 | { 109 | ceres::QuaternionToRotation(q, rot); 110 | } 111 | 112 | template 113 | Eigen::Matrix QuaternionMultMatLeft(const Eigen::Quaternion& q) 114 | { 115 | return (Eigen::Matrix() << q.w(), -q.z(), q.y(), q.x(), 116 | q.z(), q.w(), -q.x(), q.y(), 117 | -q.y(), q.x(), q.w(), q.z(), 118 | -q.x(), -q.y(), -q.z(), q.w()).finished(); 119 | } 120 | 121 | template 122 | Eigen::Matrix QuaternionMultMatRight(const Eigen::Quaternion& q) 123 | { 124 | return (Eigen::Matrix() << q.w(), q.z(), -q.y(), q.x(), 125 | -q.z(), q.w(), q.x(), q.y(), 126 | q.y(), -q.x(), q.w(), q.z(), 127 | -q.x(), -q.y(), -q.z(), q.w()).finished(); 128 | } 129 | 130 | /// @param theta - rotation about screw axis 131 | /// @param d - projection of tvec on the rotation axis 132 | /// @param l - screw axis direction 133 | /// @param m - screw axis moment 134 | template 135 | void AngleAxisAndTranslationToScrew(const Eigen::Matrix& rvec, 136 | const Eigen::Matrix& tvec, 137 | T& theta, T& d, 138 | Eigen::Matrix& l, 139 | Eigen::Matrix& m) 140 | { 141 | 142 | theta = rvec.norm(); 143 | if (theta == 0) 144 | { 145 | l.setZero(); 146 | m.setZero(); 147 | std::cout << "Warning: Undefined screw! Returned 0. " << std::endl; 148 | } 149 | 150 | l = rvec.normalized(); 151 | 152 | Eigen::Matrix t = tvec; 153 | 154 | d = t.transpose() * l; 155 | 156 | // point on screw axis - projection of origin on screw axis 157 | Eigen::Matrix c; 158 | c = 0.5 * (t - d * l + (1.0 / tan(theta / 2.0) * l).cross(t)); 159 | 160 | // c and hence the screw axis is not defined if theta is either 0 or M_PI 161 | m = c.cross(l); 162 | } 163 | 164 | template 165 | Eigen::Matrix RPY2mat(T roll, T pitch, T yaw) 166 | { 167 | Eigen::Matrix m; 168 | 169 | T cr = cos(roll); 170 | T sr = sin(roll); 171 | T cp = cos(pitch); 172 | T sp = sin(pitch); 173 | T cy = cos(yaw); 174 | T sy = sin(yaw); 175 | 176 | m(0,0) = cy * cp; 177 | m(0,1) = cy * sp * sr - sy * cr; 178 | m(0,2) = cy * sp * cr + sy * sr; 179 | m(1,0) = sy * cp; 180 | m(1,1) = sy * sp * sr + cy * cr; 181 | m(1,2) = sy * sp * cr - cy * sr; 182 | m(2,0) = - sp; 183 | m(2,1) = cp * sr; 184 | m(2,2) = cp * cr; 185 | return m; 186 | } 187 | 188 | template 189 | void mat2RPY(const Eigen::Matrix& m, T& roll, T& pitch, T& yaw) 190 | { 191 | roll = atan2(m(2,1), m(2,2)); 192 | pitch = atan2(-m(2,0), sqrt(m(2,1) * m(2,1) + m(2,2) * m(2,2))); 193 | yaw = atan2(m(1,0), m(0,0)); 194 | } 195 | 196 | template 197 | Eigen::Matrix homogeneousTransform(const Eigen::Matrix& R, const Eigen::Matrix& t) 198 | { 199 | Eigen::Matrix H; 200 | H.setIdentity(); 201 | 202 | H.block(0,0,3,3) = R; 203 | H.block(0,3,3,1) = t; 204 | 205 | return H; 206 | } 207 | 208 | template 209 | Eigen::Matrix poseWithCartesianTranslation(const T* const q, const T* const p) 210 | { 211 | Eigen::Matrix pose = Eigen::Matrix::Identity(); 212 | 213 | T rotation[9]; 214 | ceres::QuaternionToRotation(q, rotation); 215 | for (int i = 0; i < 3; ++i) 216 | { 217 | for (int j = 0; j < 3; ++j) 218 | { 219 | pose(i,j) = rotation[i * 3 + j]; 220 | } 221 | } 222 | 223 | pose(0,3) = p[0]; 224 | pose(1,3) = p[1]; 225 | pose(2,3) = p[2]; 226 | 227 | return pose; 228 | } 229 | 230 | template 231 | Eigen::Matrix poseWithSphericalTranslation(const T* const q, const T* const p, const T scale = T(1.0)) 232 | { 233 | Eigen::Matrix pose = Eigen::Matrix::Identity(); 234 | 235 | T rotation[9]; 236 | ceres::QuaternionToRotation(q, rotation); 237 | for (int i = 0; i < 3; ++i) 238 | { 239 | for (int j = 0; j < 3; ++j) 240 | { 241 | pose(i,j) = rotation[i * 3 + j]; 242 | } 243 | } 244 | 245 | T theta = p[0]; 246 | T phi = p[1]; 247 | pose(0,3) = sin(theta) * cos(phi) * scale; 248 | pose(1,3) = sin(theta) * sin(phi) * scale; 249 | pose(2,3) = cos(theta) * scale; 250 | 251 | return pose; 252 | } 253 | 254 | // Returns the Sampson error of a given essential matrix and 2 image points 255 | template 256 | T sampsonError(const Eigen::Matrix& E, 257 | const Eigen::Matrix& p1, 258 | const Eigen::Matrix& p2) 259 | { 260 | Eigen::Matrix Ex1 = E * p1; 261 | Eigen::Matrix Etx2 = E.transpose() * p2; 262 | 263 | T x2tEx1 = p2.dot(Ex1); 264 | 265 | // compute Sampson error 266 | T err = square(x2tEx1) / (square(Ex1(0,0)) + square(Ex1(1,0)) + square(Etx2(0,0)) + square(Etx2(1,0))); 267 | 268 | return err; 269 | } 270 | 271 | // Returns the Sampson error of a given rotation/translation and 2 image points 272 | template 273 | T sampsonError(const Eigen::Matrix& R, 274 | const Eigen::Matrix& t, 275 | const Eigen::Matrix& p1, 276 | const Eigen::Matrix& p2) 277 | { 278 | // construct essential matrix 279 | Eigen::Matrix E = skew(t) * R; 280 | 281 | Eigen::Matrix Ex1 = E * p1; 282 | Eigen::Matrix Etx2 = E.transpose() * p2; 283 | 284 | T x2tEx1 = p2.dot(Ex1); 285 | 286 | // compute Sampson error 287 | T err = square(x2tEx1) / (square(Ex1(0,0)) + square(Ex1(1,0)) + square(Etx2(0,0)) + square(Etx2(1,0))); 288 | 289 | return err; 290 | } 291 | 292 | // Returns the Sampson error of a given rotation/translation and 2 image points 293 | template 294 | T sampsonError(const Eigen::Matrix& H, 295 | const Eigen::Matrix& p1, 296 | const Eigen::Matrix& p2) 297 | { 298 | Eigen::Matrix R = H.block(0, 0, 3, 3); 299 | Eigen::Matrix t = H.block(0, 3, 3, 1); 300 | 301 | return sampsonError(R, t, p1, p2); 302 | } 303 | 304 | template 305 | Eigen::Matrix 306 | transformPoint(const Eigen::Matrix& H, const Eigen::Matrix& P) 307 | { 308 | Eigen::Matrix P_trans = H.block(0, 0, 3, 3) * P + H.block(0, 3, 3, 1); 309 | 310 | return P_trans; 311 | } 312 | 313 | template 314 | Eigen::Matrix 315 | estimate3DRigidTransform(const std::vector, Eigen::aligned_allocator > >& points1, 316 | const std::vector, Eigen::aligned_allocator > >& points2) 317 | { 318 | // compute centroids 319 | Eigen::Matrix c1, c2; 320 | c1.setZero(); c2.setZero(); 321 | 322 | for (size_t i = 0; i < points1.size(); ++i) 323 | { 324 | c1 += points1.at(i); 325 | c2 += points2.at(i); 326 | } 327 | 328 | c1 /= points1.size(); 329 | c2 /= points1.size(); 330 | 331 | Eigen::Matrix X(3, points1.size()); 332 | Eigen::Matrix Y(3, points1.size()); 333 | for (size_t i = 0; i < points1.size(); ++i) 334 | { 335 | X.col(i) = points1.at(i) - c1; 336 | Y.col(i) = points2.at(i) - c2; 337 | } 338 | 339 | Eigen::Matrix H = X * Y.transpose(); 340 | 341 | Eigen::JacobiSVD< Eigen::Matrix > svd(H, Eigen::ComputeFullU | Eigen::ComputeFullV); 342 | 343 | Eigen::Matrix U = svd.matrixU(); 344 | Eigen::Matrix V = svd.matrixV(); 345 | if (U.determinant() * V.determinant() < 0.0) 346 | { 347 | V.col(2) *= -1.0; 348 | } 349 | 350 | Eigen::Matrix R = V * U.transpose(); 351 | Eigen::Matrix t = c2 - R * c1; 352 | 353 | return homogeneousTransform(R, t); 354 | } 355 | 356 | template 357 | Eigen::Matrix 358 | estimate3DRigidSimilarityTransform(const std::vector, Eigen::aligned_allocator > >& points1, 359 | const std::vector, Eigen::aligned_allocator > >& points2) 360 | { 361 | // compute centroids 362 | Eigen::Matrix c1, c2; 363 | c1.setZero(); c2.setZero(); 364 | 365 | for (size_t i = 0; i < points1.size(); ++i) 366 | { 367 | c1 += points1.at(i); 368 | c2 += points2.at(i); 369 | } 370 | 371 | c1 /= points1.size(); 372 | c2 /= points1.size(); 373 | 374 | Eigen::Matrix X(3, points1.size()); 375 | Eigen::Matrix Y(3, points1.size()); 376 | for (size_t i = 0; i < points1.size(); ++i) 377 | { 378 | X.col(i) = points1.at(i) - c1; 379 | Y.col(i) = points2.at(i) - c2; 380 | } 381 | 382 | Eigen::Matrix H = X * Y.transpose(); 383 | 384 | Eigen::JacobiSVD< Eigen::Matrix > svd(H, Eigen::ComputeFullU | Eigen::ComputeFullV); 385 | 386 | Eigen::Matrix U = svd.matrixU(); 387 | Eigen::Matrix V = svd.matrixV(); 388 | if (U.determinant() * V.determinant() < 0.0) 389 | { 390 | V.col(2) *= -1.0; 391 | } 392 | 393 | Eigen::Matrix R = V * U.transpose(); 394 | 395 | std::vector, Eigen::aligned_allocator > > rotatedPoints1(points1.size()); 396 | for (size_t i = 0; i < points1.size(); ++i) 397 | { 398 | rotatedPoints1.at(i) = R * (points1.at(i) - c1); 399 | } 400 | 401 | double sum_ss = 0.0, sum_tt = 0.0; 402 | for (size_t i = 0; i < points1.size(); ++i) 403 | { 404 | sum_ss += (points1.at(i) - c1).squaredNorm(); 405 | sum_tt += (points2.at(i) - c2).dot(rotatedPoints1.at(i)); 406 | } 407 | 408 | double scale = sum_tt / sum_ss; 409 | 410 | Eigen::Matrix sR = scale * R; 411 | Eigen::Matrix t = c2 - sR * c1; 412 | 413 | return homogeneousTransform(sR, t); 414 | } 415 | 416 | } 417 | 418 | #endif 419 | -------------------------------------------------------------------------------- /include/camera_model/gpl/gpl.h: -------------------------------------------------------------------------------- 1 | #ifndef GPL_H 2 | #define GPL_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace camera_model 9 | { 10 | 11 | template 12 | const T clamp(const T& v, const T& a, const T& b) 13 | { 14 | return std::min(b, std::max(a, v)); 15 | } 16 | 17 | double hypot3(double x, double y, double z); 18 | float hypot3f(float x, float y, float z); 19 | 20 | template 21 | const T normalizeTheta(const T& theta) 22 | { 23 | T normTheta = theta; 24 | 25 | while (normTheta < - M_PI) 26 | { 27 | normTheta += 2.0 * M_PI; 28 | } 29 | while (normTheta > M_PI) 30 | { 31 | normTheta -= 2.0 * M_PI; 32 | } 33 | 34 | return normTheta; 35 | } 36 | 37 | double d2r(double deg); 38 | float d2r(float deg); 39 | double r2d(double rad); 40 | float r2d(float rad); 41 | 42 | double sinc(double theta); 43 | 44 | template 45 | const T square(const T& x) 46 | { 47 | return x * x; 48 | } 49 | 50 | template 51 | const T cube(const T& x) 52 | { 53 | return x * x * x; 54 | } 55 | 56 | template 57 | const T random(const T& a, const T& b) 58 | { 59 | return static_cast(rand()) / RAND_MAX * (b - a) + a; 60 | } 61 | 62 | template 63 | const T randomNormal(const T& sigma) 64 | { 65 | T x1, x2, w; 66 | 67 | do 68 | { 69 | x1 = 2.0 * random(0.0, 1.0) - 1.0; 70 | x2 = 2.0 * random(0.0, 1.0) - 1.0; 71 | w = x1 * x1 + x2 * x2; 72 | } 73 | while (w >= 1.0 || w == 0.0); 74 | 75 | w = sqrt((-2.0 * log(w)) / w); 76 | 77 | return x1 * w * sigma; 78 | } 79 | 80 | unsigned long long timeInMicroseconds(void); 81 | 82 | double timeInSeconds(void); 83 | 84 | void colorDepthImage(cv::Mat& imgDepth, 85 | cv::Mat& imgColoredDepth, 86 | float minRange, float maxRange); 87 | 88 | bool colormap(const std::string& name, unsigned char idx, 89 | float& r, float& g, float& b); 90 | 91 | std::vector bresLine(int x0, int y0, int x1, int y1); 92 | std::vector bresCircle(int x0, int y0, int r); 93 | 94 | void fitCircle(const std::vector& points, 95 | double& centerX, double& centerY, double& radius); 96 | 97 | std::vector intersectCircles(double x1, double y1, double r1, 98 | double x2, double y2, double r2); 99 | 100 | void LLtoUTM(double latitude, double longitude, 101 | double& utmNorthing, double& utmEasting, 102 | std::string& utmZone); 103 | void UTMtoLL(double utmNorthing, double utmEasting, 104 | const std::string& utmZone, 105 | double& latitude, double& longitude); 106 | 107 | long int timestampDiff(uint64_t t1, uint64_t t2); 108 | 109 | } 110 | 111 | #endif 112 | -------------------------------------------------------------------------------- /include/camera_model/sparse_graph/Transform.h: -------------------------------------------------------------------------------- 1 | #ifndef TRANSFORM_H 2 | #define TRANSFORM_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace camera_model 9 | { 10 | 11 | class Transform 12 | { 13 | public: 14 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 15 | 16 | Transform(); 17 | Transform(const Eigen::Matrix4d& H); 18 | 19 | Eigen::Quaterniond& rotation(void); 20 | const Eigen::Quaterniond& rotation(void) const; 21 | double* rotationData(void); 22 | const double* const rotationData(void) const; 23 | 24 | Eigen::Vector3d& translation(void); 25 | const Eigen::Vector3d& translation(void) const; 26 | double* translationData(void); 27 | const double* const translationData(void) const; 28 | 29 | Eigen::Matrix4d toMatrix(void) const; 30 | 31 | private: 32 | Eigen::Quaterniond m_q; 33 | Eigen::Vector3d m_t; 34 | }; 35 | 36 | } 37 | 38 | #endif 39 | -------------------------------------------------------------------------------- /src/calib/CameraCalibration.cc: -------------------------------------------------------------------------------- 1 | #include "camera_model/calib/CameraCalibration.h" 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | #include "camera_model/camera_models/CameraFactory.h" 15 | #include "camera_model/sparse_graph/Transform.h" 16 | #include "camera_model/gpl/EigenQuaternionParameterization.h" 17 | #include "camera_model/gpl/EigenUtils.h" 18 | #include "camera_model/camera_models/CostFunctionFactory.h" 19 | 20 | #include "ceres/ceres.h" 21 | namespace camera_model 22 | { 23 | 24 | CameraCalibration::CameraCalibration() 25 | : m_boardSize(cv::Size(0,0)) 26 | , m_squareSize(0.0f) 27 | , m_verbose(false) 28 | { 29 | 30 | } 31 | 32 | CameraCalibration::CameraCalibration(const Camera::ModelType modelType, 33 | const std::string& cameraName, 34 | const cv::Size& imageSize, 35 | const cv::Size& boardSize, 36 | float squareSize) 37 | : m_boardSize(boardSize) 38 | , m_squareSize(squareSize) 39 | , m_verbose(false) 40 | { 41 | m_camera = CameraFactory::instance()->generateCamera(modelType, cameraName, imageSize); 42 | } 43 | 44 | void 45 | CameraCalibration::clear(void) 46 | { 47 | m_imagePoints.clear(); 48 | m_scenePoints.clear(); 49 | } 50 | 51 | void 52 | CameraCalibration::addChessboardData(const std::vector& corners) 53 | { 54 | m_imagePoints.push_back(corners); 55 | 56 | std::vector scenePointsInView; 57 | for (int i = 0; i < m_boardSize.height; ++i) 58 | { 59 | for (int j = 0; j < m_boardSize.width; ++j) 60 | { 61 | scenePointsInView.push_back(cv::Point3f(i * m_squareSize, j * m_squareSize, 0.0)); 62 | } 63 | } 64 | m_scenePoints.push_back(scenePointsInView); 65 | } 66 | 67 | void 68 | CameraCalibration::addCornersData(const std::vector &corners, const std::vector& scenePoints) 69 | { 70 | m_imagePoints.push_back(corners); 71 | m_scenePoints.push_back(scenePoints); 72 | } 73 | 74 | bool 75 | CameraCalibration::calibrate(void) 76 | { 77 | int imageCount = m_imagePoints.size(); 78 | 79 | for (auto m : m_imagePoints) 80 | std::cout << "m_imagePoints: " << m << std::endl; 81 | 82 | for (auto m : m_scenePoints) 83 | std::cout << "m_scenePoints: " << m << std::endl; 84 | // compute intrinsic camera parameters and extrinsic parameters for each of the views 85 | std::vector rvecs; 86 | std::vector tvecs; 87 | bool ret = calibrateHelper(m_camera, rvecs, tvecs); 88 | 89 | m_cameraPoses = cv::Mat(imageCount, 6, CV_64F); 90 | for (int i = 0; i < imageCount; ++i) 91 | { 92 | m_cameraPoses.at(i,0) = rvecs.at(i).at(0); 93 | m_cameraPoses.at(i,1) = rvecs.at(i).at(1); 94 | m_cameraPoses.at(i,2) = rvecs.at(i).at(2); 95 | m_cameraPoses.at(i,3) = tvecs.at(i).at(0); 96 | m_cameraPoses.at(i,4) = tvecs.at(i).at(1); 97 | m_cameraPoses.at(i,5) = tvecs.at(i).at(2); 98 | } 99 | 100 | // Compute measurement covariance. 101 | std::vector > errVec(m_imagePoints.size()); 102 | Eigen::Vector2d errSum = Eigen::Vector2d::Zero(); 103 | size_t errCount = 0; 104 | for (size_t i = 0; i < m_imagePoints.size(); ++i) 105 | { 106 | std::vector estImagePoints; 107 | m_camera->projectPoints(m_scenePoints.at(i), rvecs.at(i), tvecs.at(i), 108 | estImagePoints); 109 | 110 | for (size_t j = 0; j < m_imagePoints.at(i).size(); ++j) 111 | { 112 | cv::Point2f pObs = m_imagePoints.at(i).at(j); 113 | cv::Point2f pEst = estImagePoints.at(j); 114 | 115 | cv::Point2f err = pObs - pEst; 116 | 117 | errVec.at(i).push_back(err); 118 | 119 | errSum += Eigen::Vector2d(err.x, err.y); 120 | } 121 | 122 | errCount += m_imagePoints.at(i).size(); 123 | } 124 | 125 | Eigen::Vector2d errMean = errSum / static_cast(errCount); 126 | 127 | Eigen::Matrix2d measurementCovariance = Eigen::Matrix2d::Zero(); 128 | for (size_t i = 0; i < errVec.size(); ++i) 129 | { 130 | for (size_t j = 0; j < errVec.at(i).size(); ++j) 131 | { 132 | cv::Point2f err = errVec.at(i).at(j); 133 | double d0 = err.x - errMean(0); 134 | double d1 = err.y - errMean(1); 135 | 136 | measurementCovariance(0,0) += d0 * d0; 137 | measurementCovariance(0,1) += d0 * d1; 138 | measurementCovariance(1,1) += d1 * d1; 139 | } 140 | } 141 | measurementCovariance /= static_cast(errCount); 142 | measurementCovariance(1,0) = measurementCovariance(0,1); 143 | 144 | m_measurementCovariance = measurementCovariance; 145 | 146 | return ret; 147 | } 148 | 149 | int 150 | CameraCalibration::sampleCount(void) const 151 | { 152 | return m_imagePoints.size(); 153 | } 154 | 155 | std::vector >& 156 | CameraCalibration::imagePoints(void) 157 | { 158 | return m_imagePoints; 159 | } 160 | 161 | const std::vector >& 162 | CameraCalibration::imagePoints(void) const 163 | { 164 | return m_imagePoints; 165 | } 166 | 167 | std::vector >& 168 | CameraCalibration::scenePoints(void) 169 | { 170 | return m_scenePoints; 171 | } 172 | 173 | const std::vector >& 174 | CameraCalibration::scenePoints(void) const 175 | { 176 | return m_scenePoints; 177 | } 178 | 179 | CameraPtr& 180 | CameraCalibration::camera(void) 181 | { 182 | return m_camera; 183 | } 184 | 185 | const CameraConstPtr 186 | CameraCalibration::camera(void) const 187 | { 188 | return m_camera; 189 | } 190 | 191 | Eigen::Matrix2d& 192 | CameraCalibration::measurementCovariance(void) 193 | { 194 | return m_measurementCovariance; 195 | } 196 | 197 | const Eigen::Matrix2d& 198 | CameraCalibration::measurementCovariance(void) const 199 | { 200 | return m_measurementCovariance; 201 | } 202 | 203 | cv::Mat& 204 | CameraCalibration::cameraPoses(void) 205 | { 206 | return m_cameraPoses; 207 | } 208 | 209 | const cv::Mat& 210 | CameraCalibration::cameraPoses(void) const 211 | { 212 | return m_cameraPoses; 213 | } 214 | 215 | void 216 | CameraCalibration::drawResults(std::vector& images) const 217 | { 218 | std::vector rvecs, tvecs; 219 | 220 | for (size_t i = 0; i < images.size(); ++i) 221 | { 222 | cv::Mat rvec(3, 1, CV_64F); 223 | rvec.at(0) = m_cameraPoses.at(i,0); 224 | rvec.at(1) = m_cameraPoses.at(i,1); 225 | rvec.at(2) = m_cameraPoses.at(i,2); 226 | 227 | cv::Mat tvec(3, 1, CV_64F); 228 | tvec.at(0) = m_cameraPoses.at(i,3); 229 | tvec.at(1) = m_cameraPoses.at(i,4); 230 | tvec.at(2) = m_cameraPoses.at(i,5); 231 | 232 | rvecs.push_back(rvec); 233 | tvecs.push_back(tvec); 234 | } 235 | 236 | int drawShiftBits = 4; 237 | int drawMultiplier = 1 << drawShiftBits; 238 | 239 | cv::Scalar green(0, 255, 0); 240 | cv::Scalar red(0, 0, 255); 241 | 242 | for (size_t i = 0; i < images.size(); ++i) 243 | { 244 | cv::Mat& image = images.at(i); 245 | if (image.channels() == 1) 246 | { 247 | cv::cvtColor(image, image, CV_GRAY2RGB); 248 | } 249 | 250 | std::vector estImagePoints; 251 | m_camera->projectPoints(m_scenePoints.at(i), rvecs.at(i), tvecs.at(i), 252 | estImagePoints); 253 | 254 | float errorSum = 0.0f; 255 | float errorMax = std::numeric_limits::min(); 256 | 257 | for (size_t j = 0; j < m_imagePoints.at(i).size(); ++j) 258 | { 259 | cv::Point2f pObs = m_imagePoints.at(i).at(j); 260 | cv::Point2f pEst = estImagePoints.at(j); 261 | 262 | cv::circle(image, 263 | cv::Point(cvRound(pObs.x * drawMultiplier), 264 | cvRound(pObs.y * drawMultiplier)), 265 | 5, green, 2, CV_AA, drawShiftBits); 266 | 267 | cv::circle(image, 268 | cv::Point(cvRound(pEst.x * drawMultiplier), 269 | cvRound(pEst.y * drawMultiplier)), 270 | 5, red, 2, CV_AA, drawShiftBits); 271 | 272 | float error = cv::norm(pObs - pEst); 273 | 274 | errorSum += error; 275 | if (error > errorMax) 276 | { 277 | errorMax = error; 278 | } 279 | } 280 | 281 | std::ostringstream oss; 282 | oss << "Reprojection error: avg = " << errorSum / m_imagePoints.at(i).size() 283 | << " max = " << errorMax; 284 | 285 | cv::putText(image, oss.str(), cv::Point(10, image.rows - 10), 286 | cv::FONT_HERSHEY_COMPLEX, 0.5, cv::Scalar(255, 255, 255), 287 | 1, CV_AA); 288 | } 289 | } 290 | 291 | void 292 | CameraCalibration::writeParams(const std::string& filename) const 293 | { 294 | m_camera->writeParametersToYamlFile(filename); 295 | } 296 | 297 | bool 298 | CameraCalibration::writeChessboardData(const std::string& filename) const 299 | { 300 | std::ofstream ofs(filename.c_str(), std::ios::out | std::ios::binary); 301 | if (!ofs.is_open()) 302 | { 303 | return false; 304 | } 305 | 306 | writeData(ofs, m_boardSize.width); 307 | writeData(ofs, m_boardSize.height); 308 | writeData(ofs, m_squareSize); 309 | 310 | writeData(ofs, m_measurementCovariance(0,0)); 311 | writeData(ofs, m_measurementCovariance(0,1)); 312 | writeData(ofs, m_measurementCovariance(1,0)); 313 | writeData(ofs, m_measurementCovariance(1,1)); 314 | 315 | writeData(ofs, m_cameraPoses.rows); 316 | writeData(ofs, m_cameraPoses.cols); 317 | writeData(ofs, m_cameraPoses.type()); 318 | for (int i = 0; i < m_cameraPoses.rows; ++i) 319 | { 320 | for (int j = 0; j < m_cameraPoses.cols; ++j) 321 | { 322 | writeData(ofs, m_cameraPoses.at(i,j)); 323 | } 324 | } 325 | 326 | writeData(ofs, m_imagePoints.size()); 327 | for (size_t i = 0; i < m_imagePoints.size(); ++i) 328 | { 329 | writeData(ofs, m_imagePoints.at(i).size()); 330 | for (size_t j = 0; j < m_imagePoints.at(i).size(); ++j) 331 | { 332 | const cv::Point2f& ipt = m_imagePoints.at(i).at(j); 333 | 334 | writeData(ofs, ipt.x); 335 | writeData(ofs, ipt.y); 336 | } 337 | } 338 | 339 | writeData(ofs, m_scenePoints.size()); 340 | for (size_t i = 0; i < m_scenePoints.size(); ++i) 341 | { 342 | writeData(ofs, m_scenePoints.at(i).size()); 343 | for (size_t j = 0; j < m_scenePoints.at(i).size(); ++j) 344 | { 345 | const cv::Point3f& spt = m_scenePoints.at(i).at(j); 346 | 347 | writeData(ofs, spt.x); 348 | writeData(ofs, spt.y); 349 | writeData(ofs, spt.z); 350 | } 351 | } 352 | 353 | return true; 354 | } 355 | 356 | bool 357 | CameraCalibration::readChessboardData(const std::string& filename) 358 | { 359 | std::ifstream ifs(filename.c_str(), std::ios::in | std::ios::binary); 360 | if (!ifs.is_open()) 361 | { 362 | return false; 363 | } 364 | 365 | readData(ifs, m_boardSize.width); 366 | readData(ifs, m_boardSize.height); 367 | readData(ifs, m_squareSize); 368 | 369 | readData(ifs, m_measurementCovariance(0,0)); 370 | readData(ifs, m_measurementCovariance(0,1)); 371 | readData(ifs, m_measurementCovariance(1,0)); 372 | readData(ifs, m_measurementCovariance(1,1)); 373 | 374 | int rows, cols, type; 375 | readData(ifs, rows); 376 | readData(ifs, cols); 377 | readData(ifs, type); 378 | m_cameraPoses = cv::Mat(rows, cols, type); 379 | 380 | for (int i = 0; i < m_cameraPoses.rows; ++i) 381 | { 382 | for (int j = 0; j < m_cameraPoses.cols; ++j) 383 | { 384 | readData(ifs, m_cameraPoses.at(i,j)); 385 | } 386 | } 387 | 388 | size_t nImagePointSets; 389 | readData(ifs, nImagePointSets); 390 | 391 | m_imagePoints.clear(); 392 | m_imagePoints.resize(nImagePointSets); 393 | for (size_t i = 0; i < m_imagePoints.size(); ++i) 394 | { 395 | size_t nImagePoints; 396 | readData(ifs, nImagePoints); 397 | m_imagePoints.at(i).resize(nImagePoints); 398 | 399 | for (size_t j = 0; j < m_imagePoints.at(i).size(); ++j) 400 | { 401 | cv::Point2f& ipt = m_imagePoints.at(i).at(j); 402 | readData(ifs, ipt.x); 403 | readData(ifs, ipt.y); 404 | } 405 | } 406 | 407 | size_t nScenePointSets; 408 | readData(ifs, nScenePointSets); 409 | 410 | m_scenePoints.clear(); 411 | m_scenePoints.resize(nScenePointSets); 412 | for (size_t i = 0; i < m_scenePoints.size(); ++i) 413 | { 414 | size_t nScenePoints; 415 | readData(ifs, nScenePoints); 416 | m_scenePoints.at(i).resize(nScenePoints); 417 | 418 | for (size_t j = 0; j < m_scenePoints.at(i).size(); ++j) 419 | { 420 | cv::Point3f& spt = m_scenePoints.at(i).at(j); 421 | readData(ifs, spt.x); 422 | readData(ifs, spt.y); 423 | readData(ifs, spt.z); 424 | } 425 | } 426 | 427 | return true; 428 | } 429 | 430 | void 431 | CameraCalibration::setVerbose(bool verbose) 432 | { 433 | m_verbose = verbose; 434 | } 435 | 436 | bool 437 | CameraCalibration::calibrateHelper(CameraPtr& camera, 438 | std::vector& rvecs, std::vector& tvecs) const 439 | { 440 | rvecs.assign(m_scenePoints.size(), cv::Mat()); 441 | tvecs.assign(m_scenePoints.size(), cv::Mat()); 442 | 443 | // STEP 1: Estimate intrinsics 444 | camera->estimateIntrinsics(m_boardSize, m_scenePoints, m_imagePoints); 445 | 446 | // STEP 2: Estimate extrinsics 447 | for (size_t i = 0; i < m_scenePoints.size(); ++i) 448 | { 449 | camera->estimateExtrinsics(m_scenePoints.at(i), m_imagePoints.at(i), rvecs.at(i), tvecs.at(i)); 450 | } 451 | 452 | if (m_verbose) 453 | { 454 | std::cout << "[" << camera->cameraName() << "] " 455 | << "# INFO: " << "Initial reprojection error: " 456 | << std::fixed << std::setprecision(3) 457 | << camera->reprojectionError(m_scenePoints, m_imagePoints, rvecs, tvecs) 458 | << " pixels" << std::endl; 459 | } 460 | 461 | // STEP 3: optimization using ceres 462 | optimize(camera, rvecs, tvecs); 463 | 464 | if (m_verbose) 465 | { 466 | double err = camera->reprojectionError(m_scenePoints, m_imagePoints, rvecs, tvecs); 467 | std::cout << "[" << camera->cameraName() << "] " << "# INFO: Final reprojection error: " 468 | << err << " pixels" << std::endl; 469 | std::cout << "[" << camera->cameraName() << "] " << "# INFO: " 470 | << camera->parametersToString() << std::endl; 471 | } 472 | 473 | return true; 474 | } 475 | 476 | void 477 | CameraCalibration::optimize(CameraPtr& camera, 478 | std::vector& rvecs, std::vector& tvecs) const 479 | { 480 | // Use ceres to do optimization 481 | ceres::Problem problem; 482 | 483 | std::vector > transformVec(rvecs.size()); 484 | for (size_t i = 0; i < rvecs.size(); ++i) 485 | { 486 | Eigen::Vector3d rvec; 487 | cv::cv2eigen(rvecs.at(i), rvec); 488 | 489 | transformVec.at(i).rotation() = Eigen::AngleAxisd(rvec.norm(), rvec.normalized()); 490 | transformVec.at(i).translation() << tvecs[i].at(0), 491 | tvecs[i].at(1), 492 | tvecs[i].at(2); 493 | } 494 | 495 | std::vector intrinsicCameraParams; 496 | m_camera->writeParameters(intrinsicCameraParams); 497 | 498 | // create residuals for each observation 499 | for (size_t i = 0; i < m_imagePoints.size(); ++i) 500 | { 501 | for (size_t j = 0; j < m_imagePoints.at(i).size(); ++j) 502 | { 503 | const cv::Point3f& spt = m_scenePoints.at(i).at(j); 504 | const cv::Point2f& ipt = m_imagePoints.at(i).at(j); 505 | 506 | ceres::CostFunction* costFunction = 507 | CostFunctionFactory::instance()->generateCostFunction(camera, 508 | Eigen::Vector3d(spt.x, spt.y, spt.z), 509 | Eigen::Vector2d(ipt.x, ipt.y), 510 | CAMERA_INTRINSICS | CAMERA_POSE); 511 | 512 | ceres::LossFunction* lossFunction = new ceres::CauchyLoss(1.0); 513 | problem.AddResidualBlock(costFunction, lossFunction, 514 | intrinsicCameraParams.data(), 515 | transformVec.at(i).rotationData(), 516 | transformVec.at(i).translationData()); 517 | } 518 | 519 | ceres::LocalParameterization* quaternionParameterization = 520 | new EigenQuaternionParameterization; 521 | 522 | problem.SetParameterization(transformVec.at(i).rotationData(), 523 | quaternionParameterization); 524 | } 525 | 526 | std::cout << "begin ceres" << std::endl; 527 | ceres::Solver::Options options; 528 | options.max_num_iterations = 1000; 529 | options.num_threads = 1; 530 | 531 | if (m_verbose) 532 | { 533 | options.minimizer_progress_to_stdout = true; 534 | } 535 | 536 | ceres::Solver::Summary summary; 537 | ceres::Solve(options, &problem, &summary); 538 | std::cout << "end ceres" << std::endl; 539 | 540 | if (m_verbose) 541 | { 542 | std::cout << summary.FullReport() << std::endl; 543 | } 544 | 545 | camera->readParameters(intrinsicCameraParams); 546 | 547 | for (size_t i = 0; i < rvecs.size(); ++i) 548 | { 549 | Eigen::AngleAxisd aa(transformVec.at(i).rotation()); 550 | 551 | Eigen::Vector3d rvec = aa.angle() * aa.axis(); 552 | cv::eigen2cv(rvec, rvecs.at(i)); 553 | 554 | cv::Mat& tvec = tvecs.at(i); 555 | tvec.at(0) = transformVec.at(i).translation()(0); 556 | tvec.at(1) = transformVec.at(i).translation()(1); 557 | tvec.at(2) = transformVec.at(i).translation()(2); 558 | } 559 | } 560 | 561 | template 562 | void 563 | CameraCalibration::readData(std::ifstream& ifs, T& data) const 564 | { 565 | char* buffer = new char[sizeof(T)]; 566 | 567 | ifs.read(buffer, sizeof(T)); 568 | 569 | data = *(reinterpret_cast(buffer)); 570 | 571 | delete buffer; 572 | } 573 | 574 | template 575 | void 576 | CameraCalibration::writeData(std::ofstream& ofs, T data) const 577 | { 578 | char* pData = reinterpret_cast(&data); 579 | 580 | ofs.write(pData, sizeof(T)); 581 | } 582 | 583 | } 584 | -------------------------------------------------------------------------------- /src/calib/StereoCameraCalibration.cc: -------------------------------------------------------------------------------- 1 | #include "camera_model/calib/StereoCameraCalibration.h" 2 | 3 | #include 4 | #include 5 | 6 | #include "ceres/ceres.h" 7 | #include "camera_model/gpl/EigenQuaternionParameterization.h" 8 | #include "camera_model/gpl/EigenUtils.h" 9 | #include "camera_model/camera_models/CameraFactory.h" 10 | #include "camera_model/camera_models/CostFunctionFactory.h" 11 | 12 | namespace camera_model 13 | { 14 | 15 | StereoCameraCalibration::StereoCameraCalibration(Camera::ModelType modelType, 16 | const std::string& cameraLeftName, 17 | const std::string& cameraRightName, 18 | const cv::Size& imageSize, 19 | const cv::Size& boardSize, 20 | float squareSize) 21 | : m_calibLeft(modelType, cameraLeftName, imageSize, boardSize, squareSize) 22 | , m_calibRight(modelType, cameraRightName, imageSize, boardSize, squareSize) 23 | , m_verbose(false) 24 | { 25 | 26 | } 27 | 28 | void 29 | StereoCameraCalibration::clear(void) 30 | { 31 | m_calibLeft.clear(); 32 | m_calibRight.clear(); 33 | } 34 | 35 | void 36 | StereoCameraCalibration::addChessboardData(const std::vector& cornersLeft, 37 | const std::vector& cornersRight) 38 | { 39 | m_calibLeft.addChessboardData(cornersLeft); 40 | m_calibRight.addChessboardData(cornersRight); 41 | } 42 | 43 | void StereoCameraCalibration::addCornersData(const std::vector &cornersLeft, 44 | const std::vector &cornersRight, 45 | const std::vector &scenePoints) 46 | { 47 | m_calibLeft.addCornersData(cornersLeft, scenePoints); 48 | m_calibRight.addCornersData(cornersRight, scenePoints); 49 | } 50 | 51 | bool 52 | StereoCameraCalibration::calibrate(void) 53 | { 54 | // calibrate cameras individually 55 | if (!m_calibLeft.calibrate()) 56 | { 57 | return false; 58 | } 59 | if (!m_calibRight.calibrate()) 60 | { 61 | return false; 62 | } 63 | std::cout << __FUNCTION__ << " " << __LINE__ << std::endl; 64 | // perform stereo calibration 65 | int imageCount = imagePointsLeft().size(); 66 | 67 | // find best estimate for initial transform from left camera frame to right camera frame 68 | double minReprojErr = std::numeric_limits::max(); 69 | for (int i = 0; i < imageCount; ++i) 70 | { 71 | Eigen::Vector3d rvec; 72 | rvec << m_calibLeft.cameraPoses().at(i,0), 73 | m_calibLeft.cameraPoses().at(i,1), 74 | m_calibLeft.cameraPoses().at(i,2); 75 | 76 | Eigen::Quaterniond q_l = AngleAxisToQuaternion(rvec); 77 | 78 | Eigen::Vector3d t_l; 79 | t_l << m_calibLeft.cameraPoses().at(i,3), 80 | m_calibLeft.cameraPoses().at(i,4), 81 | m_calibLeft.cameraPoses().at(i,5); 82 | 83 | rvec << m_calibRight.cameraPoses().at(i,0), 84 | m_calibRight.cameraPoses().at(i,1), 85 | m_calibRight.cameraPoses().at(i,2); 86 | 87 | Eigen::Quaterniond q_r = AngleAxisToQuaternion(rvec); 88 | 89 | Eigen::Vector3d t_r; 90 | t_r << m_calibRight.cameraPoses().at(i,3), 91 | m_calibRight.cameraPoses().at(i,4), 92 | m_calibRight.cameraPoses().at(i,5); 93 | 94 | Eigen::Quaterniond q_l_r = q_r * q_l.conjugate(); 95 | Eigen::Vector3d t_l_r = -q_l_r.toRotationMatrix() * t_l + t_r; 96 | 97 | std::vector rvecs(imageCount); 98 | std::vector tvecs(imageCount); 99 | 100 | for (int j = 0; j < imageCount; ++j) 101 | { 102 | rvec << m_calibLeft.cameraPoses().at(j,0), 103 | m_calibLeft.cameraPoses().at(j,1), 104 | m_calibLeft.cameraPoses().at(j,2); 105 | 106 | q_l = AngleAxisToQuaternion(rvec); 107 | 108 | t_l << m_calibLeft.cameraPoses().at(j,3), 109 | m_calibLeft.cameraPoses().at(j,4), 110 | m_calibLeft.cameraPoses().at(j,5); 111 | 112 | Eigen::Quaterniond q_r = q_l_r * q_l; 113 | Eigen::Vector3d t_r = q_l_r.toRotationMatrix() * t_l + t_l_r; 114 | 115 | QuaternionToAngleAxis(q_r.coeffs().data(), rvec); 116 | cv::eigen2cv(rvec, rvecs.at(j)); 117 | 118 | cv::eigen2cv(t_r, tvecs.at(j)); 119 | } 120 | 121 | double reprojErr = cameraRight()->reprojectionError(scenePoints(), 122 | imagePointsRight(), 123 | rvecs, tvecs); 124 | 125 | if (reprojErr < minReprojErr) 126 | { 127 | minReprojErr = reprojErr; 128 | m_q = q_l_r; 129 | m_t = t_l_r; 130 | } 131 | } 132 | std::cout << __FUNCTION__ << " " << __LINE__ << std::endl; 133 | std::vector rvecsL(imageCount); 134 | std::vector tvecsL(imageCount); 135 | std::vector rvecsR(imageCount); 136 | std::vector tvecsR(imageCount); 137 | 138 | std::cout << __FUNCTION__ << " " << __LINE__ << std::endl; 139 | double* extrinsicCameraLParams[scenePoints().size()]; 140 | for (int i = 0; i < imageCount; ++i) 141 | { 142 | extrinsicCameraLParams[i] = new double[7]; 143 | 144 | Eigen::Vector3d rvecL(m_calibLeft.cameraPoses().at(i,0), 145 | m_calibLeft.cameraPoses().at(i,1), 146 | m_calibLeft.cameraPoses().at(i,2)); 147 | 148 | AngleAxisToQuaternion(rvecL, extrinsicCameraLParams[i]); 149 | 150 | extrinsicCameraLParams[i][4] = m_calibLeft.cameraPoses().at(i,3); 151 | extrinsicCameraLParams[i][5] = m_calibLeft.cameraPoses().at(i,4); 152 | extrinsicCameraLParams[i][6] = m_calibLeft.cameraPoses().at(i,5); 153 | 154 | cv::eigen2cv(rvecL, rvecsL.at(i)); 155 | 156 | Eigen::Vector3d tvecL; 157 | tvecL << m_calibLeft.cameraPoses().at(i,3), 158 | m_calibLeft.cameraPoses().at(i,4), 159 | m_calibLeft.cameraPoses().at(i,5); 160 | 161 | cv::eigen2cv(tvecL, tvecsL.at(i)); 162 | 163 | Eigen::Quaterniond q_r = m_q * AngleAxisToQuaternion(rvecL); 164 | Eigen::Vector3d t_r = m_q.toRotationMatrix() * tvecL + m_t; 165 | 166 | Eigen::Vector3d rvecR; 167 | QuaternionToAngleAxis(q_r.coeffs().data(), rvecR); 168 | 169 | cv::eigen2cv(rvecR, rvecsR.at(i)); 170 | cv::eigen2cv(t_r, tvecsR.at(i)); 171 | } 172 | 173 | if (m_verbose) 174 | { 175 | double roll, pitch, yaw; 176 | mat2RPY(m_q.toRotationMatrix(), roll, pitch, yaw); 177 | 178 | std::cout << "[stereo]" << "# INFO: Initial extrinsics: " << std::endl 179 | << "r: " << roll << " p: " << pitch << " yaw: " << yaw << std::endl 180 | << "x: " << m_t(0) << " y: " << m_t(1) << " z: " << m_t(2) << std::endl; 181 | 182 | double err = cameraLeft()->reprojectionError(scenePoints(), imagePointsLeft(), rvecsL, tvecsL); 183 | std::cout << "[" << cameraLeft()->cameraName() << "] " << "# INFO: Initial reprojection error: " 184 | << err << " pixels" << std::endl; 185 | 186 | err = cameraRight()->reprojectionError(scenePoints(), imagePointsRight(), rvecsR, tvecsR); 187 | std::cout << "[" << cameraRight()->cameraName() << "] " << "# INFO: Initial reprojection error: " 188 | << err << " pixels" << std::endl; 189 | } 190 | 191 | std::vector intrinsicCameraLParams; 192 | cameraLeft()->writeParameters(intrinsicCameraLParams); 193 | 194 | std::vector intrinsicCameraRParams; 195 | cameraRight()->writeParameters(intrinsicCameraRParams); 196 | 197 | ceres::Problem problem; 198 | 199 | for (int i = 0; i < imageCount; ++i) 200 | { 201 | for (size_t j = 0; j < scenePoints().at(i).size(); ++j) 202 | { 203 | const cv::Point3f& spt = scenePoints().at(i).at(j); 204 | const cv::Point2f& iptL = imagePointsLeft().at(i).at(j); 205 | const cv::Point2f& iptR = imagePointsRight().at(i).at(j); 206 | 207 | ceres::CostFunction* costFunction = 208 | CostFunctionFactory::instance()->generateCostFunction(cameraLeft(), 209 | cameraRight(), 210 | Eigen::Vector3d(spt.x, spt.y, spt.z), 211 | Eigen::Vector2d(iptL.x, iptL.y), 212 | Eigen::Vector2d(iptR.x, iptR.y)); 213 | 214 | ceres::LossFunction* lossFunction = new ceres::CauchyLoss(1.0); 215 | problem.AddResidualBlock(costFunction, lossFunction, 216 | intrinsicCameraLParams.data(), 217 | intrinsicCameraRParams.data(), 218 | extrinsicCameraLParams[i], 219 | extrinsicCameraLParams[i] + 4, 220 | m_q.coeffs().data(), 221 | m_t.data()); 222 | } 223 | } 224 | 225 | for (int i = 0; i < imageCount; ++i) 226 | { 227 | ceres::LocalParameterization* quaternionParameterization = 228 | new EigenQuaternionParameterization; 229 | 230 | problem.SetParameterization(extrinsicCameraLParams[i], 231 | quaternionParameterization); 232 | } 233 | 234 | ceres::LocalParameterization* quaternionParameterization = 235 | new EigenQuaternionParameterization; 236 | 237 | problem.SetParameterization(m_q.coeffs().data(), 238 | quaternionParameterization); 239 | 240 | ceres::Solver::Options options; 241 | options.max_num_iterations = 1000; 242 | options.num_threads = 8; 243 | 244 | if (m_verbose) 245 | { 246 | options.minimizer_progress_to_stdout = true; 247 | } 248 | 249 | ceres::Solver::Summary summary; 250 | ceres::Solve(options, &problem, &summary); 251 | 252 | if (m_verbose) 253 | { 254 | std::cout << summary.FullReport() << "\n"; 255 | } 256 | 257 | cameraLeft()->readParameters(intrinsicCameraLParams); 258 | cameraRight()->readParameters(intrinsicCameraRParams); 259 | 260 | for (int i = 0; i < imageCount; ++i) 261 | { 262 | Eigen::Vector3d rvecL; 263 | QuaternionToAngleAxis(extrinsicCameraLParams[i], rvecL); 264 | 265 | m_calibLeft.cameraPoses().at(i,0) = rvecL(0); 266 | m_calibLeft.cameraPoses().at(i,1) = rvecL(1); 267 | m_calibLeft.cameraPoses().at(i,2) = rvecL(2); 268 | m_calibLeft.cameraPoses().at(i,3) = extrinsicCameraLParams[i][4]; 269 | m_calibLeft.cameraPoses().at(i,4) = extrinsicCameraLParams[i][5]; 270 | m_calibLeft.cameraPoses().at(i,5) = extrinsicCameraLParams[i][6]; 271 | 272 | cv::eigen2cv(rvecL, rvecsL.at(i)); 273 | 274 | Eigen::Vector3d tvecL; 275 | tvecL << extrinsicCameraLParams[i][4], 276 | extrinsicCameraLParams[i][5], 277 | extrinsicCameraLParams[i][6]; 278 | 279 | cv::eigen2cv(tvecL, tvecsL.at(i)); 280 | 281 | Eigen::Quaterniond q_r = m_q * AngleAxisToQuaternion(rvecL); 282 | Eigen::Vector3d t_r = m_q.toRotationMatrix() * tvecL + m_t; 283 | 284 | Eigen::Vector3d rvecR; 285 | QuaternionToAngleAxis(q_r.coeffs().data(), rvecR); 286 | 287 | m_calibRight.cameraPoses().at(i,0) = rvecR(0); 288 | m_calibRight.cameraPoses().at(i,1) = rvecR(1); 289 | m_calibRight.cameraPoses().at(i,2) = rvecR(2); 290 | m_calibRight.cameraPoses().at(i,3) = t_r(0); 291 | m_calibRight.cameraPoses().at(i,4) = t_r(1); 292 | m_calibRight.cameraPoses().at(i,5) = t_r(2); 293 | 294 | cv::eigen2cv(rvecR, rvecsR.at(i)); 295 | cv::eigen2cv(t_r, tvecsR.at(i)); 296 | } 297 | 298 | if (m_verbose) 299 | { 300 | double roll, pitch, yaw; 301 | mat2RPY(m_q.toRotationMatrix(), roll, pitch, yaw); 302 | 303 | std::cout << "[stereo]" << "# INFO: Final extrinsics: " << std::endl 304 | << "r: " << roll << " p: " << pitch << " yaw: " << yaw << std::endl 305 | << "x: " << m_t(0) << " y: " << m_t(1) << " z: " << m_t(2) << std::endl; 306 | 307 | double err = cameraLeft()->reprojectionError(scenePoints(), imagePointsLeft(), rvecsL, tvecsL); 308 | std::cout << "[" << cameraLeft()->cameraName() << "] " << "# INFO: Final reprojection error: " 309 | << err << " pixels" << std::endl; 310 | std::cout << "[" << cameraLeft()->cameraName() << "] " << "# INFO: " 311 | << cameraLeft()->parametersToString() << std::endl; 312 | 313 | err = cameraRight()->reprojectionError(scenePoints(), imagePointsRight(), rvecsR, tvecsR); 314 | std::cout << "[" << cameraRight()->cameraName() << "] " << "# INFO: Final reprojection error: " 315 | << err << " pixels" << std::endl; 316 | std::cout << "[" << cameraRight()->cameraName() << "] " << "# INFO: " 317 | << cameraRight()->parametersToString() << std::endl; 318 | } 319 | 320 | return true; 321 | } 322 | 323 | int 324 | StereoCameraCalibration::sampleCount(void) const 325 | { 326 | return m_calibLeft.sampleCount(); 327 | } 328 | 329 | const std::vector >& 330 | StereoCameraCalibration::imagePointsLeft(void) const 331 | { 332 | return m_calibLeft.imagePoints(); 333 | } 334 | 335 | const std::vector >& 336 | StereoCameraCalibration::imagePointsRight(void) const 337 | { 338 | return m_calibRight.imagePoints(); 339 | } 340 | 341 | const std::vector >& 342 | StereoCameraCalibration::scenePoints(void) const 343 | { 344 | return m_calibLeft.scenePoints(); 345 | } 346 | 347 | CameraPtr& 348 | StereoCameraCalibration::cameraLeft(void) 349 | { 350 | return m_calibLeft.camera(); 351 | } 352 | 353 | const CameraConstPtr 354 | StereoCameraCalibration::cameraLeft(void) const 355 | { 356 | return m_calibLeft.camera(); 357 | } 358 | 359 | CameraPtr& 360 | StereoCameraCalibration::cameraRight(void) 361 | { 362 | return m_calibRight.camera(); 363 | } 364 | 365 | const CameraConstPtr 366 | StereoCameraCalibration::cameraRight(void) const 367 | { 368 | return m_calibRight.camera(); 369 | } 370 | 371 | void 372 | StereoCameraCalibration::drawResults(std::vector& imagesLeft, 373 | std::vector& imagesRight) const 374 | { 375 | m_calibLeft.drawResults(imagesLeft); 376 | m_calibRight.drawResults(imagesRight); 377 | } 378 | 379 | void 380 | StereoCameraCalibration::writeParams(const std::string& directory) const 381 | { 382 | if (!boost::filesystem::exists(directory)) 383 | { 384 | boost::filesystem::create_directory(directory); 385 | } 386 | 387 | cameraLeft()->writeParametersToYamlFile(directory + "/camera_left.yaml"); 388 | cameraRight()->writeParametersToYamlFile(directory + "/camera_right.yaml"); 389 | 390 | cv::FileStorage fs(directory + "/extrinsics.yaml", cv::FileStorage::WRITE); 391 | 392 | fs << "transform"; 393 | fs << "{" << "q_x" << m_q.x() 394 | << "q_y" << m_q.y() 395 | << "q_z" << m_q.z() 396 | << "q_w" << m_q.w() 397 | << "t_x" << m_t(0) 398 | << "t_y" << m_t(1) 399 | << "t_z" << m_t(2) << "}"; 400 | 401 | fs.release(); 402 | } 403 | 404 | void 405 | StereoCameraCalibration::setVerbose(bool verbose) 406 | { 407 | m_verbose = verbose; 408 | m_calibLeft.setVerbose(verbose); 409 | m_calibRight.setVerbose(verbose); 410 | } 411 | 412 | } 413 | -------------------------------------------------------------------------------- /src/camera_models/Camera.cc: -------------------------------------------------------------------------------- 1 | #include "camera_model/camera_models/Camera.h" 2 | #include "camera_model/camera_models/ScaramuzzaCamera.h" 3 | 4 | #include 5 | 6 | namespace camera_model 7 | { 8 | 9 | Camera::Parameters::Parameters(ModelType modelType) 10 | : m_modelType(modelType) 11 | , m_imageWidth(0) 12 | , m_imageHeight(0) 13 | { 14 | switch (modelType) 15 | { 16 | case KANNALA_BRANDT: 17 | m_nIntrinsics = 8; 18 | break; 19 | case PINHOLE: 20 | m_nIntrinsics = 8; 21 | break; 22 | case SCARAMUZZA: 23 | m_nIntrinsics = SCARAMUZZA_CAMERA_NUM_PARAMS; 24 | break; 25 | case MEI: 26 | default: 27 | m_nIntrinsics = 9; 28 | } 29 | } 30 | 31 | Camera::Parameters::Parameters(ModelType modelType, 32 | const std::string& cameraName, 33 | int w, int h) 34 | : m_modelType(modelType) 35 | , m_cameraName(cameraName) 36 | , m_imageWidth(w) 37 | , m_imageHeight(h) 38 | { 39 | switch (modelType) 40 | { 41 | case KANNALA_BRANDT: 42 | m_nIntrinsics = 8; 43 | break; 44 | case PINHOLE: 45 | m_nIntrinsics = 8; 46 | break; 47 | case SCARAMUZZA: 48 | m_nIntrinsics = SCARAMUZZA_CAMERA_NUM_PARAMS; 49 | break; 50 | case MEI: 51 | default: 52 | m_nIntrinsics = 9; 53 | } 54 | } 55 | 56 | Camera::ModelType& 57 | Camera::Parameters::modelType(void) 58 | { 59 | return m_modelType; 60 | } 61 | 62 | std::string& 63 | Camera::Parameters::cameraName(void) 64 | { 65 | return m_cameraName; 66 | } 67 | 68 | int& 69 | Camera::Parameters::imageWidth(void) 70 | { 71 | return m_imageWidth; 72 | } 73 | 74 | int& 75 | Camera::Parameters::imageHeight(void) 76 | { 77 | return m_imageHeight; 78 | } 79 | 80 | Camera::ModelType 81 | Camera::Parameters::modelType(void) const 82 | { 83 | return m_modelType; 84 | } 85 | 86 | const std::string& 87 | Camera::Parameters::cameraName(void) const 88 | { 89 | return m_cameraName; 90 | } 91 | 92 | int 93 | Camera::Parameters::imageWidth(void) const 94 | { 95 | return m_imageWidth; 96 | } 97 | 98 | int 99 | Camera::Parameters::imageHeight(void) const 100 | { 101 | return m_imageHeight; 102 | } 103 | 104 | int 105 | Camera::Parameters::nIntrinsics(void) const 106 | { 107 | return m_nIntrinsics; 108 | } 109 | 110 | cv::Mat& 111 | Camera::mask(void) 112 | { 113 | return m_mask; 114 | } 115 | 116 | const cv::Mat& 117 | Camera::mask(void) const 118 | { 119 | return m_mask; 120 | } 121 | 122 | void 123 | Camera::estimateExtrinsics(const std::vector& objectPoints, 124 | const std::vector& imagePoints, 125 | cv::Mat& rvec, cv::Mat& tvec) const 126 | { 127 | std::vector Ms(imagePoints.size()); 128 | for (size_t i = 0; i < Ms.size(); ++i) 129 | { 130 | Eigen::Vector3d P; 131 | liftProjective(Eigen::Vector2d(imagePoints.at(i).x, imagePoints.at(i).y), P); 132 | 133 | P /= P(2); 134 | 135 | Ms.at(i).x = P(0); 136 | Ms.at(i).y = P(1); 137 | } 138 | 139 | // assume unit focal length, zero principal point, and zero distortion 140 | cv::solvePnP(objectPoints, Ms, cv::Mat::eye(3, 3, CV_64F), cv::noArray(), rvec, tvec); 141 | } 142 | 143 | double 144 | Camera::reprojectionDist(const Eigen::Vector3d& P1, const Eigen::Vector3d& P2) const 145 | { 146 | Eigen::Vector2d p1, p2; 147 | 148 | spaceToPlane(P1, p1); 149 | spaceToPlane(P2, p2); 150 | 151 | return (p1 - p2).norm(); 152 | } 153 | 154 | double 155 | Camera::reprojectionError(const std::vector< std::vector >& objectPoints, 156 | const std::vector< std::vector >& imagePoints, 157 | const std::vector& rvecs, 158 | const std::vector& tvecs, 159 | cv::OutputArray _perViewErrors) const 160 | { 161 | int imageCount = objectPoints.size(); 162 | size_t pointsSoFar = 0; 163 | double totalErr = 0.0; 164 | 165 | bool computePerViewErrors = _perViewErrors.needed(); 166 | cv::Mat perViewErrors; 167 | if (computePerViewErrors) 168 | { 169 | _perViewErrors.create(imageCount, 1, CV_64F); 170 | perViewErrors = _perViewErrors.getMat(); 171 | } 172 | 173 | for (int i = 0; i < imageCount; ++i) 174 | { 175 | size_t pointCount = imagePoints.at(i).size(); 176 | 177 | pointsSoFar += pointCount; 178 | 179 | std::vector estImagePoints; 180 | projectPoints(objectPoints.at(i), rvecs.at(i), tvecs.at(i), 181 | estImagePoints); 182 | 183 | double err = 0.0; 184 | for (size_t j = 0; j < imagePoints.at(i).size(); ++j) 185 | { 186 | err += cv::norm(imagePoints.at(i).at(j) - estImagePoints.at(j)); 187 | } 188 | 189 | if (computePerViewErrors) 190 | { 191 | perViewErrors.at(i) = err / pointCount; 192 | } 193 | 194 | totalErr += err; 195 | } 196 | 197 | return totalErr / pointsSoFar; 198 | } 199 | 200 | double 201 | Camera::reprojectionError(const Eigen::Vector3d& P, 202 | const Eigen::Quaterniond& camera_q, 203 | const Eigen::Vector3d& camera_t, 204 | const Eigen::Vector2d& observed_p) const 205 | { 206 | Eigen::Vector3d P_cam = camera_q.toRotationMatrix() * P + camera_t; 207 | 208 | Eigen::Vector2d p; 209 | spaceToPlane(P_cam, p); 210 | 211 | return (p - observed_p).norm(); 212 | } 213 | 214 | void 215 | Camera::projectPoints(const std::vector& objectPoints, 216 | const cv::Mat& rvec, 217 | const cv::Mat& tvec, 218 | std::vector& imagePoints) const 219 | { 220 | // project 3D object points to the image plane 221 | imagePoints.reserve(objectPoints.size()); 222 | 223 | //double 224 | cv::Mat R0; 225 | cv::Rodrigues(rvec, R0); 226 | 227 | Eigen::MatrixXd R(3,3); 228 | R << R0.at(0,0), R0.at(0,1), R0.at(0,2), 229 | R0.at(1,0), R0.at(1,1), R0.at(1,2), 230 | R0.at(2,0), R0.at(2,1), R0.at(2,2); 231 | 232 | Eigen::Vector3d t; 233 | t << tvec.at(0), tvec.at(1), tvec.at(2); 234 | 235 | for (size_t i = 0; i < objectPoints.size(); ++i) 236 | { 237 | const cv::Point3f& objectPoint = objectPoints.at(i); 238 | 239 | // Rotate and translate 240 | Eigen::Vector3d P; 241 | P << objectPoint.x, objectPoint.y, objectPoint.z; 242 | 243 | P = R * P + t; 244 | 245 | Eigen::Vector2d p; 246 | spaceToPlane(P, p); 247 | 248 | imagePoints.push_back(cv::Point2f(p(0), p(1))); 249 | } 250 | } 251 | 252 | } 253 | -------------------------------------------------------------------------------- /src/camera_models/CameraFactory.cc: -------------------------------------------------------------------------------- 1 | #include "camera_model/camera_models/CameraFactory.h" 2 | 3 | #include 4 | 5 | 6 | #include "camera_model/camera_models/CataCamera.h" 7 | #include "camera_model/camera_models/EquidistantCamera.h" 8 | #include "camera_model/camera_models/PinholeCamera.h" 9 | #include "camera_model/camera_models/ScaramuzzaCamera.h" 10 | 11 | #include "ceres/ceres.h" 12 | 13 | namespace camera_model 14 | { 15 | 16 | boost::shared_ptr CameraFactory::m_instance; 17 | 18 | CameraFactory::CameraFactory() 19 | { 20 | 21 | } 22 | 23 | boost::shared_ptr 24 | CameraFactory::instance(void) 25 | { 26 | if (m_instance.get() == 0) 27 | { 28 | m_instance.reset(new CameraFactory); 29 | } 30 | 31 | return m_instance; 32 | } 33 | 34 | CameraPtr 35 | CameraFactory::generateCamera(Camera::ModelType modelType, 36 | const std::string& cameraName, 37 | cv::Size imageSize) const 38 | { 39 | switch (modelType) 40 | { 41 | case Camera::KANNALA_BRANDT: 42 | { 43 | EquidistantCameraPtr camera(new EquidistantCamera); 44 | 45 | EquidistantCamera::Parameters params = camera->getParameters(); 46 | params.cameraName() = cameraName; 47 | params.imageWidth() = imageSize.width; 48 | params.imageHeight() = imageSize.height; 49 | camera->setParameters(params); 50 | return camera; 51 | } 52 | case Camera::PINHOLE: 53 | { 54 | PinholeCameraPtr camera(new PinholeCamera); 55 | 56 | PinholeCamera::Parameters params = camera->getParameters(); 57 | params.cameraName() = cameraName; 58 | params.imageWidth() = imageSize.width; 59 | params.imageHeight() = imageSize.height; 60 | camera->setParameters(params); 61 | return camera; 62 | } 63 | case Camera::SCARAMUZZA: 64 | { 65 | OCAMCameraPtr camera(new OCAMCamera); 66 | 67 | OCAMCamera::Parameters params = camera->getParameters(); 68 | params.cameraName() = cameraName; 69 | params.imageWidth() = imageSize.width; 70 | params.imageHeight() = imageSize.height; 71 | camera->setParameters(params); 72 | return camera; 73 | } 74 | case Camera::MEI: 75 | default: 76 | { 77 | CataCameraPtr camera(new CataCamera); 78 | 79 | CataCamera::Parameters params = camera->getParameters(); 80 | params.cameraName() = cameraName; 81 | params.imageWidth() = imageSize.width; 82 | params.imageHeight() = imageSize.height; 83 | camera->setParameters(params); 84 | return camera; 85 | } 86 | } 87 | } 88 | 89 | CameraPtr 90 | CameraFactory::generateCameraFromYamlFile(const std::string& filename) 91 | { 92 | cv::FileStorage fs(filename, cv::FileStorage::READ); 93 | 94 | if (!fs.isOpened()) 95 | { 96 | return CameraPtr(); 97 | } 98 | 99 | Camera::ModelType modelType = Camera::MEI; 100 | if (!fs["model_type"].isNone()) 101 | { 102 | std::string sModelType; 103 | fs["model_type"] >> sModelType; 104 | 105 | if (boost::iequals(sModelType, "kannala_brandt")) 106 | { 107 | modelType = Camera::KANNALA_BRANDT; 108 | } 109 | else if (boost::iequals(sModelType, "mei")) 110 | { 111 | modelType = Camera::MEI; 112 | } 113 | else if (boost::iequals(sModelType, "scaramuzza")) 114 | { 115 | modelType = Camera::SCARAMUZZA; 116 | } 117 | else if (boost::iequals(sModelType, "pinhole")) 118 | { 119 | modelType = Camera::PINHOLE; 120 | } 121 | else 122 | { 123 | std::cerr << "# ERROR: Unknown camera model: " << sModelType << std::endl; 124 | return CameraPtr(); 125 | } 126 | } 127 | 128 | switch (modelType) 129 | { 130 | case Camera::KANNALA_BRANDT: 131 | { 132 | EquidistantCameraPtr camera(new EquidistantCamera); 133 | 134 | EquidistantCamera::Parameters params = camera->getParameters(); 135 | params.readFromYamlFile(filename); 136 | camera->setParameters(params); 137 | return camera; 138 | } 139 | case Camera::PINHOLE: 140 | { 141 | PinholeCameraPtr camera(new PinholeCamera); 142 | 143 | PinholeCamera::Parameters params = camera->getParameters(); 144 | params.readFromYamlFile(filename); 145 | camera->setParameters(params); 146 | return camera; 147 | } 148 | case Camera::SCARAMUZZA: 149 | { 150 | OCAMCameraPtr camera(new OCAMCamera); 151 | 152 | OCAMCamera::Parameters params = camera->getParameters(); 153 | params.readFromYamlFile(filename); 154 | camera->setParameters(params); 155 | return camera; 156 | } 157 | case Camera::MEI: 158 | default: 159 | { 160 | CataCameraPtr camera(new CataCamera); 161 | 162 | CataCamera::Parameters params = camera->getParameters(); 163 | params.readFromYamlFile(filename); 164 | camera->setParameters(params); 165 | return camera; 166 | } 167 | } 168 | 169 | return CameraPtr(); 170 | } 171 | 172 | } 173 | 174 | -------------------------------------------------------------------------------- /src/gpl/EigenQuaternionParameterization.cc: -------------------------------------------------------------------------------- 1 | #include "camera_model/gpl/EigenQuaternionParameterization.h" 2 | 3 | #include 4 | 5 | namespace camera_model 6 | { 7 | 8 | bool 9 | EigenQuaternionParameterization::Plus(const double* x, 10 | const double* delta, 11 | double* x_plus_delta) const 12 | { 13 | const double norm_delta = 14 | sqrt(delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2]); 15 | if (norm_delta > 0.0) 16 | { 17 | const double sin_delta_by_delta = (sin(norm_delta) / norm_delta); 18 | double q_delta[4]; 19 | q_delta[0] = sin_delta_by_delta * delta[0]; 20 | q_delta[1] = sin_delta_by_delta * delta[1]; 21 | q_delta[2] = sin_delta_by_delta * delta[2]; 22 | q_delta[3] = cos(norm_delta); 23 | EigenQuaternionProduct(q_delta, x, x_plus_delta); 24 | } 25 | else 26 | { 27 | for (int i = 0; i < 4; ++i) 28 | { 29 | x_plus_delta[i] = x[i]; 30 | } 31 | } 32 | return true; 33 | } 34 | 35 | bool 36 | EigenQuaternionParameterization::ComputeJacobian(const double* x, 37 | double* jacobian) const 38 | { 39 | jacobian[0] = x[3]; jacobian[1] = x[2]; jacobian[2] = -x[1]; // NOLINT 40 | jacobian[3] = -x[2]; jacobian[4] = x[3]; jacobian[5] = x[0]; // NOLINT 41 | jacobian[6] = x[1]; jacobian[7] = -x[0]; jacobian[8] = x[3]; // NOLINT 42 | jacobian[9] = -x[0]; jacobian[10] = -x[1]; jacobian[11] = -x[2]; // NOLINT 43 | return true; 44 | } 45 | 46 | } 47 | -------------------------------------------------------------------------------- /src/sparse_graph/Transform.cc: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | namespace camera_model 4 | { 5 | 6 | Transform::Transform() 7 | { 8 | m_q.setIdentity(); 9 | m_t.setZero(); 10 | } 11 | 12 | Transform::Transform(const Eigen::Matrix4d& H) 13 | { 14 | m_q = Eigen::Quaterniond(H.block<3,3>(0,0)); 15 | m_t = H.block<3,1>(0,3); 16 | } 17 | 18 | Eigen::Quaterniond& 19 | Transform::rotation(void) 20 | { 21 | return m_q; 22 | } 23 | 24 | const Eigen::Quaterniond& 25 | Transform::rotation(void) const 26 | { 27 | return m_q; 28 | } 29 | 30 | double* 31 | Transform::rotationData(void) 32 | { 33 | return m_q.coeffs().data(); 34 | } 35 | 36 | const double* const 37 | Transform::rotationData(void) const 38 | { 39 | return m_q.coeffs().data(); 40 | } 41 | 42 | Eigen::Vector3d& 43 | Transform::translation(void) 44 | { 45 | return m_t; 46 | } 47 | 48 | const Eigen::Vector3d& 49 | Transform::translation(void) const 50 | { 51 | return m_t; 52 | } 53 | 54 | double* 55 | Transform::translationData(void) 56 | { 57 | return m_t.data(); 58 | } 59 | 60 | const double* const 61 | Transform::translationData(void) const 62 | { 63 | return m_t.data(); 64 | } 65 | 66 | Eigen::Matrix4d 67 | Transform::toMatrix(void) const 68 | { 69 | Eigen::Matrix4d H; 70 | H.setIdentity(); 71 | H.block<3,3>(0,0) = m_q.toRotationMatrix(); 72 | H.block<3,1>(0,3) = m_t; 73 | 74 | return H; 75 | } 76 | 77 | } 78 | --------------------------------------------------------------------------------