├── .gitignore ├── CMakeLists.txt ├── README.md ├── applications ├── CMakeLists.txt ├── calib │ ├── CMakeLists.txt │ ├── GetPot │ ├── calibu-target.pdf │ └── main.cpp ├── grid-gen │ ├── CMakeLists.txt │ └── main.cpp └── modelio │ ├── CMakeLists.txt │ └── main.cpp ├── cmake_modules ├── .gitignore ├── FindCeresSolver.cmake ├── FindEigen3.cmake ├── FindGLog.cmake ├── FindMATLAB.cmake ├── FindPackage.cmake.in ├── FindPkgMacros.cmake ├── FindTinyXML2.cmake ├── LICENSE.md ├── LibFindMacros.cmake ├── PackageConfig.cmake.in ├── PackageConfigVersion.cmake.in ├── PkgConfig.pc.in ├── README.md ├── SetPlatformVars.cmake ├── cmake_uninstall.cmake.in └── install_package.cmake ├── include └── calibu │ ├── Calibu.h │ ├── Platform.h │ ├── calib │ ├── AutoDiffArrayCostFunction.h │ ├── Calibrator.h │ ├── CostFunctionAndParams.h │ ├── LocalParamSe3.h │ ├── ReprojectionCost.h │ └── ReprojectionCostFunctor.h │ ├── cam │ ├── camera_crtp.h │ ├── camera_crtp_impl.h │ ├── camera_models_crtp.h │ ├── camera_models_kb4.h │ ├── camera_models_poly.h │ ├── camera_models_rational.h │ ├── camera_rig.h │ ├── camera_utils.h │ ├── camera_xml.h │ ├── rectify_crtp.h │ └── stereo_rectify.h │ ├── conics │ ├── Conic.h │ ├── ConicFinder.h │ └── FindConics.h │ ├── exception.h │ ├── gl │ └── Drawing.h │ ├── image │ ├── AdaptiveThreshold.h │ ├── Gradient.h │ ├── ImageProcessing.h │ ├── IntegralImage.h │ └── Label.h │ ├── pcalib │ ├── base64.h │ ├── pcalib.h │ ├── pcalib_xml.h │ ├── response.h │ ├── response_impl.h │ ├── response_linear.h │ ├── response_poly.h │ ├── vignetting.h │ ├── vignetting_dense.h │ ├── vignetting_impl.h │ ├── vignetting_poly.h │ └── vignetting_uniform.h │ ├── pose │ ├── Pnp.h │ ├── Ransac.h │ └── Tracker.h │ ├── target │ ├── GridDefinitions.h │ ├── Hungarian.h │ ├── LineGroup.h │ ├── RandomGrid.h │ ├── Target.h │ └── TargetGridDot.h │ └── utils │ ├── PlaneBasis.h │ ├── Range.h │ ├── Rectangle.h │ ├── StreamOperatorsEigen.h │ ├── StreamOperatorsSophus.h │ └── Utils.h ├── matlab ├── CMakeLists.txt ├── calibu_mex.cpp ├── calibu_rig.m ├── class_handle.hpp └── mex.cmake ├── src ├── CMakeLists.txt ├── cam │ ├── CameraXml.cpp │ ├── StereoRectify.cpp │ └── rectify_crtp.cpp ├── config.h.in ├── conics │ ├── Conic.cpp │ ├── ConicFinder.cpp │ └── FindConics.cpp ├── image │ ├── ImageProcessing.cpp │ └── Label.cpp ├── pcalib │ ├── base64.cpp │ └── pcalib_xml.cpp ├── pose │ ├── Pnp.cpp │ └── Tracker.cpp ├── target │ ├── Hungarian.cpp │ ├── RandomGrid.cpp │ └── TargetGridDot.cpp └── utils │ └── Utils.cpp └── tests ├── CMakeLists.txt ├── base64_test.cpp ├── exception_test.cpp ├── pcalib_xml_test.cpp ├── response_linear_test.cpp ├── response_poly_test.cpp ├── tests.cpp ├── vignetting_dense_test.cpp ├── vignetting_poly_test.cpp └── vignetting_uniform_test.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | #CMake-generated files 2 | CMakeCache.txt 3 | CalibuConfig.cmake 4 | CalibuConfigVersion.cmake 5 | CalibuTargets.cmake 6 | FindCalibu.cmake 7 | Makefile 8 | CMakeFiles/ 9 | cmake_install.cmake 10 | cmake_uninstall.cmake 11 | install_manifest.txt 12 | 13 | #Generated project files 14 | include/calibu/config.h 15 | 16 | #eclipse-specific 17 | .project 18 | .cproject 19 | .settings/ 20 | 21 | Calibu.pc 22 | 23 | #debian packages 24 | *.deb 25 | description-pak 26 | doc-pak/ 27 | 28 | #generated binaries 29 | libcalibu.so 30 | grid_gen 31 | modelio 32 | cmod 33 | 34 | target_A4.eps 35 | CMakeLists.txt.user 36 | debug 37 | release 38 | build 39 | 40 | *~ 41 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | Calibu 2 | ====== 3 | Calibration library. Calibu is a framework to represent cameras and compute 4 | camera geometry and photometric model parameters. It may be used in conjunction 5 | with a calibration application e.g. vicalib (github.com/arpg/vicalib) and pcalib 6 | (github.com/aprg/pcalib) to calibrate cameras. 7 | 8 | Installation 9 | --- 10 | To install calibu, please follow the installation instructions at: 11 | https://github.com/arpg/Documentation/tree/master/Installation up to the 12 | "Calibu" step. 13 | 14 | Geometric calibration 15 | --- 16 | To calibrate cameras, please follow the installation instructions above up to 17 | the "vicalib" step. Then, follow the instructions at: 18 | https://github.com/arpg/Documentation/tree/master/Calibration to learn how to 19 | use vicalib. 20 | 21 | Photometric calibration 22 | --- 23 | To calibrate cameras, please follow the installation instructions above up to 24 | the "pcalib" step. Then, follow the instructions at: 25 | https://github.com/arpg/Documentation/tree/master/Calibration to learn how to 26 | use pcalib. 27 | 28 | -------------------------------------------------------------------------------- /applications/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | if( NOT ANDROID ) 2 | add_subdirectory( calib ) 3 | add_subdirectory( modelio ) 4 | add_subdirectory( grid-gen ) 5 | endif() 6 | 7 | -------------------------------------------------------------------------------- /applications/calib/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | include_directories( ${Calibu_INCLUDE_DIRS} ) 2 | 3 | # git://github.com/stevenlovegrove/Pangolin.git 4 | find_package( Pangolin 0.1 QUIET ) 5 | include_directories( ${Pangolin_INCLUDE_DIRS} ) 6 | 7 | # git clone https://ceres-solver.googlesource.com/ceres-solver 8 | list(APPEND SEARCH_HEADERS ${EIGEN3_INCLUDE_DIR}) # Help Ceres find Eigen 9 | find_package( Ceres 1.6.0 QUIET ) 10 | include_directories( ${CERES_INCLUDES} ) 11 | 12 | # Check that OPENCV is available 13 | set(NO_OPENCV_GPU TRUE) 14 | find_package( OpenCV QUIET ) 15 | find_package( CVars QUIET ) 16 | 17 | option(BUILD_CALIBGRID OFF "Toggle build calibgrid.") 18 | if( Pangolin_FOUND AND Ceres_FOUND AND OpenCV_FOUND AND CVars_FOUND AND BUILD_CALIBGRID) 19 | add_executable( calibgrid main.cpp ) 20 | target_link_libraries( calibgrid 21 | ${CERES_LIBRARIES} 22 | ${Pangolin_LIBRARIES} 23 | ${Calibu_LIBRARIES} 24 | ${CVars_LIBRARIES} 25 | calibu ) 26 | 27 | install(TARGETS calibgrid EXPORT CalibuTargets RUNTIME 28 | DESTINATION ${CMAKE_INSTALL_PREFIX}/bin) 29 | elseif(BUILD_CALIBGRID) 30 | #report what's missing 31 | set(WARNING_MSG "calibgrid dependencies not met:") 32 | foreach(dep Pangolin Ceres OpenCV CVars) 33 | if(NOT ${dep}_FOUND) 34 | set(WARNING_MSG "${WARNING_MSG} ${dep}") 35 | endif() 36 | endforeach() 37 | set(WARNING_MSG "${WARNING_MSG}. Skipping calibgrid during build.") 38 | message(WARNING ${WARNING_MSG}) 39 | endif() 40 | -------------------------------------------------------------------------------- /applications/calib/calibu-target.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/arpg/Calibu/d40f0e7fd5848a748799185348de6fe94a6e12db/applications/calib/calibu-target.pdf -------------------------------------------------------------------------------- /applications/grid-gen/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | include_directories(${Calibu_INCLUDE_DIRS}) 2 | add_executable( grid_gen main.cpp ) 3 | target_link_libraries( grid_gen ${Calibu_LIBRARIES} calibu ) 4 | 5 | -------------------------------------------------------------------------------- /applications/grid-gen/main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | 12 | using namespace calibu; 13 | 14 | bool should_run = true; 15 | 16 | void UserQuit(int) 17 | { 18 | should_run = false; 19 | } 20 | 21 | int main( int, char** ) 22 | { 23 | signal(SIGABRT,UserQuit); 24 | signal(SIGTERM,UserQuit); 25 | 26 | const int PR = 10; 27 | const int PC = 19; 28 | 29 | uint32_t seed = FindBestSeed(PR, PC, should_run); // 14 30 | const std::array PG = MakePatternGroup(PR, PC, seed); 31 | 32 | std::cout << PG[0] << std::endl; 33 | std::cout << "Best seed: " << seed << std::endl; 34 | std::cout << "Min unique window size: " << AutoCorrelationMinArea(PG) << std::endl; 35 | 36 | const int auto_corr = AutoCorrelation(PG,5,5); 37 | std::cout << "Bad auto correlation matches: " << auto_corr << std::endl; 38 | 39 | //SaveEPS("target.eps", PG[0], Eigen::Vector2d(0,0), 10.0, 2.0, 3.0, 4.0 ); 40 | 41 | return 0; 42 | } 43 | -------------------------------------------------------------------------------- /applications/modelio/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | include_directories( ${Calibu_INCLUDE_DIRS} ) 2 | 3 | add_executable( modelio main.cpp ) 4 | target_link_libraries( modelio ${Calibu_LIBRARIES} calibu ) 5 | -------------------------------------------------------------------------------- /applications/modelio/main.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Example demonstrating camera model reading and writing. 3 | */ 4 | 5 | #include 6 | #include 7 | 8 | using namespace calibu; 9 | 10 | 11 | void Test1() 12 | { 13 | // Tests the rig read/write methods. 14 | std::shared_ptr> rig = calibu::ReadXmlRig("cameras_in.xml"); 15 | for(size_t ii=0; ii< rig->cameras_.size(); ++ii) { 16 | std::shared_ptr> cam = rig->cameras_[ii]; 17 | cam->PrintInfo(); 18 | std::cout << " Params = \n" << cam->K().transpose() << std::endl; 19 | std::cout << " T_wc = \n" << cam->Pose().matrix3x4() << std::endl; 20 | } 21 | 22 | WriteXmlRig("cameras_out.xml", rig); 23 | } 24 | 25 | void Test2() 26 | { 27 | // Tests the camera projection methods. 28 | std::shared_ptr> rig = ReadXmlRig("cameras_in.xml"); 29 | for( size_t ii=0; ii < rig->cameras_.size(); ++ii) { 30 | std::shared_ptr> cam = rig->cameras_[ii]; 31 | 32 | Eigen::Vector2d p; 33 | Eigen::Vector3d P; 34 | double d = 10; 35 | P << 1,1,d; 36 | 37 | // project and lift into the z=1 plane 38 | p = cam->Project( P ); 39 | 40 | // unproject form the image back into 3D 41 | Eigen::Vector3d P2; 42 | P2 = d*cam->Unproject( p ); 43 | 44 | std::cout << "These vectors should match: \n" << 45 | P << "\n" << 46 | P2 << std::endl; 47 | } 48 | } 49 | 50 | int main( int argc, char* argv[] ) 51 | { 52 | std::cout << "Running Test1(). \n" << std::endl; 53 | Test1(); 54 | std::cout << "Running Test2(). \n" << std::endl; 55 | Test2(); 56 | return 0; 57 | } 58 | -------------------------------------------------------------------------------- /cmake_modules/.gitignore: -------------------------------------------------------------------------------- 1 | *~ 2 | -------------------------------------------------------------------------------- /cmake_modules/FindEigen3.cmake: -------------------------------------------------------------------------------- 1 | # - Try to find Eigen3 lib 2 | # Once done this will define 3 | # 4 | # EIGEN3_FOUND - system has eigen lib 5 | # EIGEN3_INCLUDE_DIR - the eigen include directory 6 | 7 | # Copyright (c) 2006, 2007 Montel Laurent, 8 | # Redistribution and use is allowed according to the terms of the BSD license. 9 | # For details see the accompanying COPYING-CMAKE-SCRIPTS file. 10 | 11 | if (EIGEN3_INCLUDE_DIR) 12 | 13 | # in cache already 14 | set(EIGEN3_FOUND TRUE) 15 | 16 | else (EIGEN3_INCLUDE_DIR) 17 | 18 | find_path(EIGEN3_INCLUDE_DIR NAMES Eigen/Core 19 | PATHS 20 | ${INCLUDE_INSTALL_DIR} 21 | ${KDE4_INCLUDE_DIR} 22 | PATH_SUFFIXES eigen3 23 | ) 24 | 25 | include(FindPackageHandleStandardArgs) 26 | find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR ) 27 | 28 | 29 | mark_as_advanced(EIGEN3_INCLUDE_DIR) 30 | 31 | endif(EIGEN3_INCLUDE_DIR) 32 | 33 | -------------------------------------------------------------------------------- /cmake_modules/FindGLog.cmake: -------------------------------------------------------------------------------- 1 | # - Try to find glog (or miniglog for Android) 2 | # Once done, this will define 3 | # 4 | # GLog_FOUND - system has glog 5 | # GLog_INCLUDE_DIRS - the glog include directories 6 | # GLog_LIBRARIES - link these to use glog 7 | 8 | # Find header and lib 9 | find_path(GLog_INCLUDE_DIR NAMES glog/logging.h) 10 | find_library(GLog_LIBRARIES NAMES glog) 11 | 12 | include(FindPackageHandleStandardArgs) 13 | find_package_handle_standard_args(GLog DEFAULT_MSG GLog_INCLUDE_DIR GLog_LIBRARIES) 14 | set(GLOG_INCLUDE_DIRS ${GLog_INCLUDE_DIRS}) 15 | set(GLOG_LIBRARIES ${GLog_LIBRARIES}) 16 | -------------------------------------------------------------------------------- /cmake_modules/FindPackage.cmake.in: -------------------------------------------------------------------------------- 1 | 2 | SET( @PACKAGE_PKG_NAME@_LIBRARIES @PACKAGE_LINK_LIBS@ CACHE INTERNAL "@PACKAGE_PKG_NAME@ libraries" FORCE ) 3 | SET( @PACKAGE_PKG_NAME@_INCLUDE_DIRS @PACKAGE_INCLUDE_DIRS@ CACHE INTERNAL "@PACKAGE_PKG_NAME@ include directories" FORCE ) 4 | SET( @PACKAGE_PKG_NAME@_LIBRARY_DIRS @PACKAGE_LINK_DIRS@ CACHE INTERNAL "@PACKAGE_PKG_NAME@ library directories" FORCE ) 5 | 6 | mark_as_advanced( @PACKAGE_PKG_NAME@_LIBRARIES ) 7 | mark_as_advanced( @PACKAGE_PKG_NAME@_LIBRARY_DIRS ) 8 | mark_as_advanced( @PACKAGE_PKG_NAME@_INCLUDE_DIRS ) 9 | 10 | 11 | -------------------------------------------------------------------------------- /cmake_modules/FindPkgMacros.cmake: -------------------------------------------------------------------------------- 1 | #------------------------------------------------------------------- 2 | # This file is part of the CMake build system for OGRE 3 | # (Object-oriented Graphics Rendering Engine) 4 | # For the latest info, see http://www.ogre3d.org/ 5 | # 6 | # The contents of this file are placed in the public domain. Feel 7 | # free to make use of it in any way you like. 8 | #------------------------------------------------------------------- 9 | 10 | ################################################################## 11 | # Provides some common functionality for the FindPackage modules 12 | ################################################################## 13 | 14 | # Begin processing of package 15 | macro(findpkg_begin PREFIX) 16 | if (NOT ${PREFIX}_FIND_QUIETLY) 17 | message(STATUS "Looking for ${PREFIX}...") 18 | endif () 19 | endmacro(findpkg_begin) 20 | 21 | # Display a status message unless FIND_QUIETLY is set 22 | macro(pkg_message PREFIX) 23 | if (NOT ${PREFIX}_FIND_QUIETLY) 24 | message(STATUS ${ARGN}) 25 | endif () 26 | endmacro(pkg_message) 27 | 28 | # Get environment variable, define it as ENV_$var and make sure backslashes are converted to forward slashes 29 | macro(getenv_path VAR) 30 | set(ENV_${VAR} $ENV{${VAR}}) 31 | # replace won't work if var is blank 32 | if (ENV_${VAR}) 33 | string( REGEX REPLACE "\\\\" "/" ENV_${VAR} ${ENV_${VAR}} ) 34 | endif () 35 | endmacro(getenv_path) 36 | 37 | # Construct search paths for includes and libraries from a PREFIX_PATH 38 | macro(create_search_paths PREFIX) 39 | foreach(dir ${${PREFIX}_PREFIX_PATH}) 40 | set(${PREFIX}_INC_SEARCH_PATH ${${PREFIX}_INC_SEARCH_PATH} 41 | ${dir}/include ${dir}/include/${PREFIX} ${dir}/Headers) 42 | set(${PREFIX}_LIB_SEARCH_PATH ${${PREFIX}_LIB_SEARCH_PATH} 43 | ${dir}/lib ${dir}/lib/${PREFIX} ${dir}/Libs) 44 | set(${PREFIX}_BIN_SEARCH_PATH ${${PREFIX}_BIN_SEARCH_PATH} 45 | ${dir}/bin) 46 | endforeach(dir) 47 | set(${PREFIX}_FRAMEWORK_SEARCH_PATH ${${PREFIX}_PREFIX_PATH}) 48 | endmacro(create_search_paths) 49 | 50 | # clear cache variables if a certain variable changed 51 | macro(clear_if_changed TESTVAR) 52 | # test against internal check variable 53 | if (NOT "${${TESTVAR}}" STREQUAL "${${TESTVAR}_INT_CHECK}") 54 | message(STATUS "${TESTVAR} changed.") 55 | foreach(var ${ARGN}) 56 | set(${var} "NOTFOUND" CACHE STRING "x" FORCE) 57 | endforeach(var) 58 | endif () 59 | set(${TESTVAR}_INT_CHECK ${${TESTVAR}} CACHE INTERNAL "x" FORCE) 60 | endmacro(clear_if_changed) 61 | 62 | # Try to get some hints from pkg-config, if available 63 | macro(use_pkgconfig PREFIX PKGNAME) 64 | find_package(PkgConfig) 65 | if (PKG_CONFIG_FOUND) 66 | pkg_check_modules(${PREFIX} ${PKGNAME}) 67 | endif () 68 | endmacro (use_pkgconfig) 69 | 70 | # Couple a set of release AND debug libraries (or frameworks) 71 | macro(make_library_set PREFIX) 72 | if (${PREFIX}_FWK) 73 | set(${PREFIX} ${${PREFIX}_FWK}) 74 | elseif (${PREFIX}_REL AND ${PREFIX}_DBG) 75 | set(${PREFIX} optimized ${${PREFIX}_REL} debug ${${PREFIX}_DBG}) 76 | elseif (${PREFIX}_REL) 77 | set(${PREFIX} ${${PREFIX}_REL}) 78 | elseif (${PREFIX}_DBG) 79 | set(${PREFIX} ${${PREFIX}_DBG}) 80 | endif () 81 | endmacro(make_library_set) 82 | 83 | # Generate debug names from given release names 84 | macro(get_debug_names PREFIX) 85 | foreach(i ${${PREFIX}}) 86 | set(${PREFIX}_DBG ${${PREFIX}_DBG} ${i}d ${i}D ${i}_d ${i}_D ${i}_debug ${i}) 87 | endforeach(i) 88 | endmacro(get_debug_names) 89 | 90 | # Add the parent dir from DIR to VAR 91 | macro(add_parent_dir VAR DIR) 92 | get_filename_component(${DIR}_TEMP "${${DIR}}/.." ABSOLUTE) 93 | set(${VAR} ${${VAR}} ${${DIR}_TEMP}) 94 | endmacro(add_parent_dir) 95 | 96 | # Do the final processing for the package find. 97 | macro(findpkg_finish PREFIX) 98 | # skip if already processed during this run 99 | if (NOT ${PREFIX}_FOUND) 100 | if (${PREFIX}_INCLUDE_DIR AND ${PREFIX}_LIBRARY) 101 | set(${PREFIX}_FOUND TRUE) 102 | set(${PREFIX}_INCLUDE_DIRS ${${PREFIX}_INCLUDE_DIR}) 103 | set(${PREFIX}_LIBRARIES ${${PREFIX}_LIBRARY}) 104 | if (NOT ${PREFIX}_FIND_QUIETLY) 105 | message(STATUS "Found ${PREFIX}: ${${PREFIX}_LIBRARIES}") 106 | endif () 107 | else () 108 | if (NOT ${PREFIX}_FIND_QUIETLY) 109 | message(STATUS "Could not locate ${PREFIX}") 110 | endif () 111 | if (${PREFIX}_FIND_REQUIRED) 112 | message(FATAL_ERROR "Required library ${PREFIX} not found! Install the library (including dev packages) and try again. If the library is already installed, set the missing variables manually in cmake.") 113 | endif () 114 | endif () 115 | 116 | mark_as_advanced(${PREFIX}_INCLUDE_DIR ${PREFIX}_LIBRARY ${PREFIX}_LIBRARY_REL ${PREFIX}_LIBRARY_DBG ${PREFIX}_LIBRARY_FWK) 117 | endif () 118 | endmacro(findpkg_finish) 119 | 120 | 121 | # Slightly customised framework finder 122 | MACRO(findpkg_framework fwk) 123 | IF(APPLE) 124 | SET(${fwk}_FRAMEWORK_PATH 125 | ${${fwk}_FRAMEWORK_SEARCH_PATH} 126 | ${CMAKE_FRAMEWORK_PATH} 127 | ~/Library/Frameworks 128 | /Library/Frameworks 129 | /System/Library/Frameworks 130 | /Network/Library/Frameworks 131 | /Developer/Platforms/iPhoneOS.platform/Developer/SDKs/iPhoneOS3.0.sdk/System/Library/Frameworks/ 132 | ) 133 | FOREACH(dir ${${fwk}_FRAMEWORK_PATH}) 134 | SET(fwkpath ${dir}/${fwk}.framework) 135 | IF(EXISTS ${fwkpath}) 136 | SET(${fwk}_FRAMEWORK_INCLUDES ${${fwk}_FRAMEWORK_INCLUDES} 137 | ${fwkpath}/Headers ${fwkpath}/PrivateHeaders) 138 | if (NOT ${fwk}_LIBRARY_FWK) 139 | SET(${fwk}_LIBRARY_FWK "-framework ${fwk}") 140 | endif () 141 | ENDIF(EXISTS ${fwkpath}) 142 | ENDFOREACH(dir) 143 | ENDIF(APPLE) 144 | ENDMACRO(findpkg_framework) 145 | -------------------------------------------------------------------------------- /cmake_modules/FindTinyXML2.cmake: -------------------------------------------------------------------------------- 1 | # FractalImages 2 | # Copyright (C) 2012 Sven Hertle 3 | # 4 | # This program is free software; you can redistribute it and/or modify it under 5 | # the terms of the GNU General Public License as published by the Free Software 6 | # Foundation; either version 3 of the License, or (at your option) any later 7 | # version. 8 | # 9 | # This program is distributed in the hope that it will be useful, but WITHOUT ANY 10 | # WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A 11 | # PARTICULAR PURPOSE. See the GNU General Public License for more details. 12 | # 13 | # You should have received a copy of the GNU General Public License along with 14 | # this program; if not, see . 15 | 16 | # Find TinyXML2 17 | # 18 | # TinyXML2_FOUND True if TinyXML2 was found 19 | # TinyXML2_INCLUDE_DIR Directory with headers 20 | # TinyXML2_LIBRARIES List of libraries 21 | # 22 | 23 | find_path(TinyXML2_INCLUDE_DIR "tinyxml2.h") 24 | 25 | find_library(TinyXML2_LIBRARIES NAMES "tinyxml2") 26 | 27 | include(FindPackageHandleStandardArgs) 28 | find_package_handle_standard_args("TinyXML2" DEFAULT_MSG TinyXML2_INCLUDE_DIR TinyXML2_LIBRARIES) 29 | set(TinyXML2_FOUND ${TINYXML2_FOUND} CACHE BOOL "TinyXML2 was found or not" FORCE) 30 | 31 | mark_as_advanced(TinyXML2_INCLUDE_DIR TinyXML2_LIBRARIES) 32 | 33 | message(STATUS "TinyXML2_INCLUDE_DIR: ${TinyXML2_INCLUDE_DIR}") 34 | message(STATUS "TinyXML2_LIBRARIES: ${TinyXML2_LIBRARIES}") 35 | -------------------------------------------------------------------------------- /cmake_modules/LibFindMacros.cmake: -------------------------------------------------------------------------------- 1 | # Works the same as find_package, but forwards the "REQUIRED" and "QUIET" arguments 2 | # used for the current package. For this to work, the first parameter must be the 3 | # prefix of the current package, then the prefix of the new package etc, which are 4 | # passed to find_package. 5 | macro (libfind_package PREFIX) 6 | set (LIBFIND_PACKAGE_ARGS ${ARGN}) 7 | if (${PREFIX}_FIND_QUIETLY) 8 | set (LIBFIND_PACKAGE_ARGS ${LIBFIND_PACKAGE_ARGS} QUIET) 9 | endif (${PREFIX}_FIND_QUIETLY) 10 | if (${PREFIX}_FIND_REQUIRED) 11 | set (LIBFIND_PACKAGE_ARGS ${LIBFIND_PACKAGE_ARGS} REQUIRED) 12 | endif (${PREFIX}_FIND_REQUIRED) 13 | find_package(${LIBFIND_PACKAGE_ARGS}) 14 | endmacro (libfind_package) 15 | 16 | # CMake developers made the UsePkgConfig system deprecated in the same release (2.6) 17 | # where they added pkg_check_modules. Consequently I need to support both in my scripts 18 | # to avoid those deprecated warnings. Here's a helper that does just that. 19 | # Works identically to pkg_check_modules, except that no checks are needed prior to use. 20 | macro (libfind_pkg_check_modules PREFIX PKGNAME) 21 | if (${CMAKE_MAJOR_VERSION} EQUAL 2 AND ${CMAKE_MINOR_VERSION} EQUAL 4) 22 | include(UsePkgConfig) 23 | pkgconfig(${PKGNAME} ${PREFIX}_INCLUDE_DIRS ${PREFIX}_LIBRARY_DIRS ${PREFIX}_LDFLAGS ${PREFIX}_CFLAGS) 24 | else (${CMAKE_MAJOR_VERSION} EQUAL 2 AND ${CMAKE_MINOR_VERSION} EQUAL 4) 25 | find_package(PkgConfig) 26 | if (PKG_CONFIG_FOUND) 27 | pkg_check_modules(${PREFIX} ${PKGNAME}) 28 | endif (PKG_CONFIG_FOUND) 29 | endif (${CMAKE_MAJOR_VERSION} EQUAL 2 AND ${CMAKE_MINOR_VERSION} EQUAL 4) 30 | endmacro (libfind_pkg_check_modules) 31 | 32 | # Do the final processing once the paths have been detected. 33 | # If include dirs are needed, ${PREFIX}_PROCESS_INCLUDES should be set to contain 34 | # all the variables, each of which contain one include directory. 35 | # Ditto for ${PREFIX}_PROCESS_LIBS and library files. 36 | # Will set ${PREFIX}_FOUND, ${PREFIX}_INCLUDE_DIRS and ${PREFIX}_LIBRARIES. 37 | # Also handles errors in case library detection was required, etc. 38 | macro (libfind_process PREFIX) 39 | # Skip processing if already processed during this run 40 | if (NOT ${PREFIX}_FOUND) 41 | # Start with the assumption that the library was found 42 | set (${PREFIX}_FOUND TRUE) 43 | 44 | # Process all includes and set _FOUND to false if any are missing 45 | foreach (i ${${PREFIX}_PROCESS_INCLUDES}) 46 | if (${i}) 47 | set (${PREFIX}_INCLUDE_DIRS ${${PREFIX}_INCLUDE_DIRS} ${${i}}) 48 | mark_as_advanced(${i}) 49 | else (${i}) 50 | set (${PREFIX}_FOUND FALSE) 51 | endif (${i}) 52 | endforeach (i) 53 | 54 | # Process all libraries and set _FOUND to false if any are missing 55 | foreach (i ${${PREFIX}_PROCESS_LIBS}) 56 | if (${i}) 57 | set (${PREFIX}_LIBRARIES ${${PREFIX}_LIBRARIES} ${${i}}) 58 | mark_as_advanced(${i}) 59 | else (${i}) 60 | set (${PREFIX}_FOUND FALSE) 61 | endif (${i}) 62 | endforeach (i) 63 | 64 | # Print message and/or exit on fatal error 65 | if (${PREFIX}_FOUND) 66 | if (NOT ${PREFIX}_FIND_QUIETLY) 67 | message (STATUS "Found ${PREFIX} ${${PREFIX}_VERSION}") 68 | endif (NOT ${PREFIX}_FIND_QUIETLY) 69 | else (${PREFIX}_FOUND) 70 | if (${PREFIX}_FIND_REQUIRED) 71 | foreach (i ${${PREFIX}_PROCESS_INCLUDES} ${${PREFIX}_PROCESS_LIBS}) 72 | message("${i}=${${i}}") 73 | endforeach (i) 74 | message (FATAL_ERROR "Required library ${PREFIX} NOT FOUND.\nInstall the library (dev version) and try again. If the library is already installed, use ccmake to set the missing variables manually.") 75 | endif (${PREFIX}_FIND_REQUIRED) 76 | endif (${PREFIX}_FOUND) 77 | endif (NOT ${PREFIX}_FOUND) 78 | endmacro (libfind_process) 79 | 80 | macro(libfind_library PREFIX basename) 81 | set(TMP "") 82 | if(MSVC80) 83 | set(TMP -vc80) 84 | endif(MSVC80) 85 | if(MSVC90) 86 | set(TMP -vc90) 87 | endif(MSVC90) 88 | set(${PREFIX}_LIBNAMES ${basename}${TMP}) 89 | if(${ARGC} GREATER 2) 90 | set(${PREFIX}_LIBNAMES ${basename}${TMP}-${ARGV2}) 91 | string(REGEX REPLACE "\\." "_" TMP ${${PREFIX}_LIBNAMES}) 92 | set(${PREFIX}_LIBNAMES ${${PREFIX}_LIBNAMES} ${TMP}) 93 | endif(${ARGC} GREATER 2) 94 | find_library(${PREFIX}_LIBRARY 95 | NAMES ${${PREFIX}_LIBNAMES} 96 | PATHS ${${PREFIX}_PKGCONF_LIBRARY_DIRS} 97 | ) 98 | endmacro(libfind_library) 99 | 100 | -------------------------------------------------------------------------------- /cmake_modules/PackageConfig.cmake.in: -------------------------------------------------------------------------------- 1 | SET( @PACKAGE_PKG_NAME@_LIBRARIES "@PACKAGE_LINK_LIBS@" CACHE INTERNAL "@PACKAGE_PKG_NAME@ libraries" FORCE ) 2 | SET( @PACKAGE_PKG_NAME@_INCLUDE_DIRS @PACKAGE_INCLUDE_DIRS@ CACHE INTERNAL "@PACKAGE_PKG_NAME@ include directories" FORCE ) 3 | SET( @PACKAGE_PKG_NAME@_LIBRARY_DIRS @PACKAGE_LINK_DIRS@ CACHE INTERNAL "@PACKAGE_PKG_NAME@ library directories" FORCE ) 4 | 5 | mark_as_advanced( @PACKAGE_PKG_NAME@_LIBRARIES ) 6 | mark_as_advanced( @PACKAGE_PKG_NAME@_LIBRARY_DIRS ) 7 | mark_as_advanced( @PACKAGE_PKG_NAME@_INCLUDE_DIRS ) 8 | 9 | 10 | 11 | # Compute paths 12 | get_filename_component( PACKAGE_CMAKE_DIR "${CMAKE_CURRENT_LIST_FILE}" PATH ) 13 | 14 | # This file, when used for INSTALLED code, does not use Targets... sigh. 15 | ## Library dependencies (contains definitions for IMPORTED targets) 16 | #if(NOT TARGET "@PACKAGE_PKG_NAME@_LIBRARIES" AND NOT "@PACKAGE_PKG_NAME@_BINARY_DIR") 17 | # include( "${PACKAGE_CMAKE_DIR}/@PACKAGE_PKG_NAME@Targets.cmake" ) 18 | # include( "${PACKAGE_CMAKE_DIR}/@PACKAGE_PKG_NAME@ConfigVersion.cmake" ) 19 | #endif() 20 | 21 | #SET(@PACKAGE_PKG_NAME@_LIBRARIES @PACKAGE_LIBRARIES@) 22 | #SET(@PACKAGE_PKG_NAME@_LIBRARY @PACKAGE_LIBRARY@) 23 | #SET(@PACKAGE_PKG_NAME@_INCLUDE_DIRS @PACKAGE_INCLUDE_DIRS@) 24 | #SET(@PACKAGE_PKG_NAME@_LINK_DIRS @PACKAGE_LINK_DIRS@) 25 | -------------------------------------------------------------------------------- /cmake_modules/PackageConfigVersion.cmake.in: -------------------------------------------------------------------------------- 1 | set(PACKAGE_VERSION "@PACKAGE_VERSION@") 2 | 3 | # Check build type is valid 4 | if( "System:${CMAKE_SYSTEM_NAME},Android:${ANDROID},iOS:${IOS}" STREQUAL 5 | "System:@CMAKE_SYSTEM_NAME@,Android:@ANDROID@,iOS:@IOS@" ) 6 | # Check whether the requested PACKAGE_FIND_VERSION is compatible 7 | if("${PACKAGE_VERSION}" VERSION_LESS "${PACKAGE_FIND_VERSION}") 8 | set(PACKAGE_VERSION_COMPATIBLE FALSE) 9 | else() 10 | set(PACKAGE_VERSION_COMPATIBLE TRUE) 11 | if ("${PACKAGE_VERSION}" VERSION_EQUAL "${PACKAGE_FIND_VERSION}") 12 | set(PACKAGE_VERSION_EXACT TRUE) 13 | endif() 14 | endif() 15 | else() 16 | set(PACKAGE_VERSION_COMPATIBLE FALSE) 17 | endif() 18 | -------------------------------------------------------------------------------- /cmake_modules/PkgConfig.pc.in: -------------------------------------------------------------------------------- 1 | prefix=@CMAKE_INSTALL_PREFIX@ 2 | exec_prefix=${prefix} 3 | libdir=${exec_prefix}/lib 4 | includedir=${prefix}/include 5 | 6 | Name: @PACKAGE_PKG_NAME@ 7 | Description: @PACKAGE_DESCRIPTION@ 8 | Version: @PACKAGE_VERSION@ 9 | Cflags: @PACKAGE_CFLAGS@ 10 | Libs: -L${libdir} @PACKAGE_LIBS@ @PACKAGE_LIB_LINK@ 11 | 12 | -------------------------------------------------------------------------------- /cmake_modules/README.md: -------------------------------------------------------------------------------- 1 | rpg-cmake 2 | ========= 3 | 4 | RPG's collection of reusable CMake scripts. 5 | -------------------------------------------------------------------------------- /cmake_modules/SetPlatformVars.cmake: -------------------------------------------------------------------------------- 1 | ## Compiler configuration 2 | IF(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_COMPILER_IS_GNUCC) 3 | SET(_GCC_ 1) 4 | ENDIF() 5 | 6 | IF(MSVC) 7 | SET(_MSVC_ 1) 8 | ENDIF() 9 | 10 | ## Platform configuration 11 | IF(WIN32 OR WIN64) 12 | SET(_WIN_ 1) 13 | ENDIF() 14 | 15 | IF(UNIX) 16 | SET(_UNIX_ 1) 17 | ENDIF() 18 | 19 | IF(${CMAKE_SYSTEM_NAME} MATCHES "Darwin") 20 | SET(_OSX_ 1) 21 | ENDIF() 22 | 23 | IF(${CMAKE_SYSTEM_NAME} MATCHES "Linux") 24 | SET(_LINUX_ 1) 25 | ENDIF() 26 | 27 | IF(ANDROID) 28 | SET(_ANDROID_ 1) 29 | ENDIF() 30 | -------------------------------------------------------------------------------- /cmake_modules/cmake_uninstall.cmake.in: -------------------------------------------------------------------------------- 1 | ## A simple uninstall script. 2 | ## Alternatively UNIX users can run/sudo `xargs rm < install_manifest.txt` in the build directory. 3 | 4 | set(unfile ${CMAKE_CURRENT_BINARY_DIR}/cmake_uninstall.cmake) 5 | file(WRITE ${unfile} "IF(NOT EXISTS \"install_manifest.txt\")\n") 6 | file(APPEND ${unfile} "MESSAGE(\"FATAL_ERROR Cannot find \\\"install manifest\\\": install_manifest.txt\")\n") 7 | file(APPEND ${unfile} "ENDIF(NOT EXISTS \"install_manifest.txt\")\n") 8 | file(APPEND ${unfile} "FILE(READ \"install_manifest.txt\" files)\n") 9 | file(APPEND ${unfile} "STRING(REGEX REPLACE \"\\n\" \";\" files \"\${files}\")\n") 10 | file(APPEND ${unfile} "FOREACH(file \${files})\n") 11 | file(APPEND ${unfile} " MESSAGE(STATUS \"Uninstalling \\\"\${file}\\\"\")\n") 12 | file(APPEND ${unfile} " IF(EXISTS \"\${file}\")\n") 13 | file(APPEND ${unfile} " EXEC_PROGRAM(\n") 14 | file(APPEND ${unfile} " \"\${CMAKE_COMMAND}\" ARGS \"-E remove \\\"\${file}\\\"\"\n") 15 | file(APPEND ${unfile} " OUTPUT_VARIABLE rm_out\n") 16 | file(APPEND ${unfile} " RETURN_VALUE rm_retval\n") 17 | file(APPEND ${unfile} " )\n") 18 | file(APPEND ${unfile} " IF(\"\${rm_retval}\" STREQUAL 0\)\n") 19 | file(APPEND ${unfile} " ELSE(\"\${rm_retval}\" STREQUAL 0\)\n") 20 | file(APPEND ${unfile} " MESSAGE(FATAL_ERROR \"Problem when removing \\\"\${file}\\\"\")\n") 21 | file(APPEND ${unfile} " ENDIF(\"\${rm_retval}\" STREQUAL 0)\n") 22 | file(APPEND ${unfile} " ELSE(EXISTS \"\${file}\")\n") 23 | file(APPEND ${unfile} " MESSAGE(STATUS \"File \\\"\${file}\\\" does not exist. \")\n") 24 | file(APPEND ${unfile} " ENDIF(EXISTS \"\${file}\")\n") 25 | file(APPEND ${unfile} "ENDFOREACH(file)\n") 26 | 27 | -------------------------------------------------------------------------------- /include/calibu/Calibu.h: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of the Calibu Project. 3 | https://github.com/arpg/Calibu 4 | 5 | Copyright (C) 2013 George Washington University, 6 | Steven Lovegrove 7 | Gabe Sibley 8 | 9 | Copyright (C) 2015 University of Colorado, 10 | Christoffer Heckman 11 | 12 | Licensed under the Apache License, Version 2.0 (the "License"); 13 | you may not use this file except in compliance with the License. 14 | You may obtain a copy of the License at 15 | 16 | http://www.apache.org/licenses/LICENSE-2.0 17 | 18 | Unless required by applicable law or agreed to in writing, software 19 | distributed under the License is distributed on an "AS IS" BASIS, 20 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 21 | See the License for the specific language governing permissions and 22 | limitations under the License. 23 | */ 24 | 25 | #pragma once 26 | 27 | #include 28 | 29 | #include 30 | #include 31 | 32 | -------------------------------------------------------------------------------- /include/calibu/Platform.h: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of the Calibu Project. 3 | https://github.com/gwu-robotics/Calibu 4 | 5 | Copyright (C) 2014 George Washington University, 6 | Steven Lovegrove 7 | 8 | Licensed under the Apache License, Version 2.0 (the "License"); 9 | you may not use this file except in compliance with the License. 10 | You may obtain a copy of the License at 11 | 12 | http://www.apache.org/licenses/LICENSE-2.0 13 | 14 | Unless required by applicable law or agreed to in writing, software 15 | distributed under the License is distributed on an "AS IS" BASIS, 16 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 17 | See the License for the specific language governing permissions and 18 | limitations under the License. 19 | */ 20 | 21 | #pragma once 22 | 23 | #include 24 | 25 | #ifdef _MSVC_ 26 | # include 27 | #else 28 | # define CALIBU_EXPORT 29 | #endif // _MSVC_ 30 | -------------------------------------------------------------------------------- /include/calibu/calib/AutoDiffArrayCostFunction.h: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of the Calibu Project. 3 | https://github.com/gwu-robotics/Calibu 4 | 5 | Copyright (C) 2013 George Washington University, 6 | Steven Lovegrove, 7 | 8 | Licensed under the Apache License, Version 2.0 (the "License"); 9 | you may not use this file except in compliance with the License. 10 | You may obtain a copy of the License at 11 | 12 | http://www.apache.org/licenses/LICENSE-2.0 13 | 14 | Unless required by applicable law or agreed to in writing, software 15 | distributed under the License is distributed on an "AS IS" BASIS, 16 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 17 | See the License for the specific language governing permissions and 18 | limitations under the License. 19 | */ 20 | 21 | #pragma once 22 | 23 | #include 24 | 25 | #include 26 | 27 | namespace ceres 28 | { 29 | 30 | // Sum group of variadic compile time values. 31 | template 32 | struct SumParams; 33 | 34 | template 35 | struct SumParams

{ 36 | static const int value = P; 37 | }; 38 | 39 | template 40 | struct SumParams 41 | { 42 | static const int value = P + SumParams::value; 43 | }; 44 | 45 | // Sum first N variadic compile time values 46 | template 47 | struct SumParamsN; 48 | 49 | template 50 | struct SumParamsN 51 | { 52 | static const int value = P + std::conditional, SumParamsN >::value; 53 | }; 54 | 55 | // Take Nth variadic compile time value 56 | template 57 | struct ParamsN; 58 | 59 | template 60 | struct ParamsN 61 | { 62 | static const int value = std::conditional, ParamsN >::value; 63 | }; 64 | 65 | template 66 | struct Take1stOrderPart; 67 | 68 | template 69 | struct Take1stOrderPart 70 | { 71 | // Base case - do nothing 72 | static inline void go(const int, const JetT*, T** ){} 73 | }; 74 | 75 | 76 | template 77 | struct Take1stOrderPart 78 | { 79 | static inline void go(const int M, const JetT *src, T** dst) 80 | { 81 | if(dst[0]) { 82 | internal::Take1stOrderPart(M, src, dst[0]); 83 | } 84 | Take1stOrderPart::go(M,src,dst+1); 85 | } 86 | }; 87 | 88 | 89 | template 90 | class AutoDiffArrayCostFunction : 91 | public CostBase 92 | { 93 | public: 94 | 95 | static const unsigned int num_residuals = NumResiduals; 96 | static const unsigned int num_blocks = sizeof...(Blocks); 97 | static const unsigned int num_params = SumParams::value; 98 | const unsigned int num_params_in_block[num_blocks] = {Blocks...}; 99 | 100 | typedef double T; 101 | typedef Jet JetT; 102 | 103 | AutoDiffArrayCostFunction() 104 | { 105 | // Set up residuals / params in base class 106 | CostBase::set_num_residuals(num_residuals); 107 | 108 | for(unsigned int p=0; p < num_blocks; p++) { 109 | CostBase::mutable_parameter_block_sizes()->push_back( num_params_in_block[p] ); 110 | } 111 | } 112 | 113 | virtual ~AutoDiffArrayCostFunction() {} 114 | 115 | inline const Derived& derived() const { 116 | return *static_cast(this); 117 | } 118 | 119 | virtual bool Evaluate(T const* const* parameters, 120 | T* residuals, 121 | T** jacobians) const 122 | { 123 | if (!jacobians) { 124 | return derived().template Evaluate(parameters, residuals); 125 | }else{ 126 | JetT jet_residuals[num_residuals]; 127 | 128 | JetT x[num_params]; 129 | int jet_start[num_blocks]; 130 | JetT* jet_parameters[num_blocks]; 131 | 132 | int jetb = 0; 133 | for(unsigned int b=0; b < num_blocks; ++b) { 134 | jet_start[b] = jetb; 135 | jet_parameters[b] = x + jet_start[b]; 136 | ceres::internal::Make1stOrderPerturbation(jetb, num_params_in_block[b], parameters[b], jet_parameters[b] ); 137 | jetb += num_params_in_block[b]; 138 | } 139 | 140 | if(!derived().template Evaluate(jet_parameters, jet_residuals)) { 141 | return false; 142 | } 143 | 144 | internal::Take0thOrderPart(num_residuals, jet_residuals, residuals); 145 | 146 | Take1stOrderPart::go(num_residuals, jet_residuals, jacobians); 147 | 148 | return true; 149 | } 150 | } 151 | }; 152 | 153 | } 154 | -------------------------------------------------------------------------------- /include/calibu/calib/CostFunctionAndParams.h: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of the Calibu Project. 3 | https://github.com/gwu-robotics/Calibu 4 | 5 | Copyright (C) 2013 George Washington University, 6 | Steven Lovegrove 7 | 8 | Licensed under the Apache License, Version 2.0 (the "License"); 9 | you may not use this file except in compliance with the License. 10 | You may obtain a copy of the License at 11 | 12 | http://www.apache.org/licenses/LICENSE-2.0 13 | 14 | Unless required by applicable law or agreed to in writing, software 15 | distributed under the License is distributed on an "AS IS" BASIS, 16 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 17 | See the License for the specific language governing permissions and 18 | limitations under the License. 19 | */ 20 | 21 | #pragma once 22 | 23 | #include 24 | #include 25 | 26 | namespace calibu 27 | { 28 | 29 | class CostFunctionAndParams 30 | { 31 | public: 32 | virtual ~CostFunctionAndParams() 33 | { 34 | } 35 | 36 | ceres::CostFunction*& Cost() 37 | { 38 | return m_cost; 39 | } 40 | 41 | std::vector& Params() 42 | { 43 | return m_params; 44 | } 45 | 46 | ceres::LossFunction*& Loss() 47 | { 48 | return m_loss_func; 49 | } 50 | 51 | protected: 52 | ceres::CostFunction* m_cost; 53 | std::vector m_params; 54 | ceres::LossFunction* m_loss_func; 55 | }; 56 | 57 | } 58 | -------------------------------------------------------------------------------- /include/calibu/calib/LocalParamSe3.h: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of the Calibu Project. 3 | https://github.com/gwu-robotics/Calibu 4 | 5 | Copyright (C) 2013 George Washington University, 6 | Alonso Patron, 7 | Steven Lovegrove 8 | 9 | Licensed under the Apache License, Version 2.0 (the "License"); 10 | you may not use this file except in compliance with the License. 11 | You may obtain a copy of the License at 12 | 13 | http://www.apache.org/licenses/LICENSE-2.0 14 | 15 | Unless required by applicable law or agreed to in writing, software 16 | distributed under the License is distributed on an "AS IS" BASIS, 17 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 18 | See the License for the specific language governing permissions and 19 | limitations under the License. 20 | */ 21 | 22 | #pragma once 23 | 24 | #include 25 | #include 26 | 27 | namespace calibu 28 | { 29 | 30 | class LocalParameterizationSe3 : public ceres::LocalParameterization { 31 | public: 32 | virtual ~LocalParameterizationSe3() {} 33 | virtual bool Plus(const double* x, const double* delta, double* x_plus_delta) const 34 | { 35 | const Eigen::Map T(x); 36 | const Eigen::Map > dx(delta); 37 | Eigen::Map Tdx(x_plus_delta); 38 | Tdx = T * Sophus::SE3d::exp(dx); 39 | return true; 40 | } 41 | 42 | virtual bool ComputeJacobian(const double* x, double* jacobian) const 43 | { 44 | // Largely zeroes. 45 | memset(jacobian,0, sizeof(double)*7*6); 46 | 47 | // Elements of quaternion 48 | const double q1 = x[0]; 49 | const double q2 = x[1]; 50 | const double q3 = x[2]; 51 | const double q0 = x[3]; 52 | 53 | // Common terms 54 | const double half_q0 = 0.5*q0; 55 | const double half_q1 = 0.5*q1; 56 | const double half_q2 = 0.5*q2; 57 | const double half_q3 = 0.5*q3; 58 | 59 | const double q1_sq = q1*q1; 60 | const double q2_sq = q2*q2; 61 | const double q3_sq = q3*q3; 62 | 63 | // d output_quaternion / d update 64 | jacobian[3] = half_q0; 65 | jacobian[4] = -half_q3; 66 | jacobian[5] = half_q2; 67 | 68 | jacobian[9] = half_q3; 69 | jacobian[10] = half_q0; 70 | jacobian[11] = -half_q1; 71 | 72 | jacobian[15] = -half_q2; 73 | jacobian[16] = half_q1; 74 | jacobian[17] = half_q0; 75 | 76 | jacobian[21] = -half_q1; 77 | jacobian[22] = -half_q2; 78 | jacobian[23] = -half_q3; 79 | 80 | // d output_translation / d update 81 | jacobian[24] = 1.0 - 2.0 * (q2_sq + q3_sq); 82 | jacobian[25] = 2.0 * (q1*q2 - q0*q3); 83 | jacobian[26] = 2.0 * (q1*q3 + q0*q2); 84 | 85 | jacobian[30] = 2.0 * (q1*q2 + q0*q3); 86 | jacobian[31] = 1.0 - 2.0 * (q1_sq + q3_sq); 87 | jacobian[32] = 2.0 * (q2*q3 - q0*q1); 88 | 89 | jacobian[36] = 2.0 * (q1*q3 - q0*q2); 90 | jacobian[37] = 2.0 * (q2*q3 + q0*q1); 91 | jacobian[38] = 1.0 - 2.0 * (q1_sq + q2_sq); 92 | 93 | return true; 94 | } 95 | 96 | virtual int GlobalSize() const { return 7; } 97 | virtual int LocalSize() const { return 6; } 98 | }; 99 | 100 | } 101 | -------------------------------------------------------------------------------- /include/calibu/calib/ReprojectionCost.h: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of the Calibu Project. 3 | https://github.com/gwu-robotics/Calibu 4 | 5 | Copyright (C) 2013 George Washington University 6 | Steven Lovegrove 7 | 8 | Licensed under the Apache License, Version 2.0 (the "License"); 9 | you may not use this file except in compliance with the License. 10 | You may obtain a copy of the License at 11 | 12 | http://www.apache.org/licenses/LICENSE-2.0 13 | 14 | Unless required by applicable law or agreed to in writing, software 15 | distributed under the License is distributed on an "AS IS" BASIS, 16 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 17 | See the License for the specific language governing permissions and 18 | limitations under the License. 19 | */ 20 | 21 | #pragma once 22 | 23 | #include 24 | #include 25 | 26 | namespace calibu 27 | { 28 | 29 | // Parameter block 0: T_kw // keyframe 30 | // Parameter block 1: T_ck // keyframe to cam 31 | // Parameter block 2: fu,fv,u0,v0,w 32 | template 33 | struct ReprojectionCost 34 | : public ceres::AutoDiffArrayCostFunction< 35 | ceres::CostFunction, ReprojectionCost, 36 | 2, Sophus::SE3d::num_parameters, Sophus::SE3d::num_parameters, 37 | ProjModel::NUM_PARAMS> 38 | { 39 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 40 | ReprojectionCost(const Eigen::Vector3d& Pw, const Eigen::Vector2d& pc) 41 | : m_Pw(Pw), m_pc(pc) 42 | { 43 | } 44 | 45 | template 46 | bool Evaluate(T const* const* parameters, T* residuals) const 47 | { 48 | Eigen::Map > r(residuals); 49 | const Eigen::Map > T_kw(parameters[0]); 50 | const Eigen::Map > T_ck(parameters[1]); 51 | T const* camparam = parameters[2]; 52 | 53 | const Eigen::Matrix Pc = T_ck * (T_kw * m_Pw.cast()); 54 | const Eigen::Matrix pc = ProjModel::template Map(Project(Pc), camparam); 55 | r = pc - m_pc.cast(); 56 | return true; 57 | } 58 | 59 | Eigen::Vector3d m_Pw; 60 | Eigen::Vector2d m_pc; 61 | }; 62 | 63 | } 64 | -------------------------------------------------------------------------------- /include/calibu/calib/ReprojectionCostFunctor.h: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of the Calibu Project. 3 | https://github.com/gwu-robotics/Calibu 4 | 5 | Copyright (C) 2013 George Washington University 6 | Steven Lovegrove 7 | 8 | Licensed under the Apache License, Version 2.0 (the "License"); 9 | you may not use this file except in compliance with the License. 10 | You may obtain a copy of the License at 11 | 12 | http://www.apache.org/licenses/LICENSE-2.0 13 | 14 | Unless required by applicable law or agreed to in writing, software 15 | distributed under the License is distributed on an "AS IS" BASIS, 16 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 17 | See the License for the specific language governing permissions and 18 | limitations under the License. 19 | */ 20 | 21 | #pragma once 22 | 23 | #include 24 | #include 25 | 26 | #include 27 | 28 | namespace calibu 29 | { 30 | 31 | // Parameter block 0: T_kw // keyframe 32 | // Parameter block 1: T_ck // keyframe to cam 33 | // Parameter block 2: fu,fv,u0,v0,w 34 | template 35 | struct ReprojectionCostFunctor 36 | { 37 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 38 | ReprojectionCostFunctor(const Eigen::Vector3d& Pw, 39 | const Eigen::Vector2d& pc) 40 | : m_Pw(Pw), m_pc(pc) 41 | { 42 | } 43 | 44 | template 45 | bool operator()( 46 | const T* const pT_kw, const T* const pT_ck, const T* const camparam, 47 | T* residuals 48 | ) const 49 | { 50 | Eigen::Map > r(residuals); 51 | const Eigen::Map > T_kw(pT_kw); 52 | const Eigen::Map > T_ck(pT_ck); 53 | 54 | const Eigen::Matrix Pc = T_ck * (T_kw * m_Pw.cast()); 55 | Eigen::Matrix pc; 56 | CameraInt::Project(Pc.data(), camparam, pc.data()); 57 | r = pc - m_pc.cast(); 58 | return true; 59 | } 60 | 61 | Eigen::Vector3d m_Pw; 62 | Eigen::Vector2d m_pc; 63 | }; 64 | 65 | } 66 | -------------------------------------------------------------------------------- /include/calibu/cam/camera_crtp_impl.h: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of the Calibu Project. 3 | https://github.com/arpg/Calibu 4 | 5 | Copyright (C) 2013 George Washington University, 6 | Steven Lovegrove, 7 | Nima Keivan 8 | Jack Morrison 9 | Christoffer Heckman 10 | Gabe Sibley 11 | 12 | Licensed under the Apache License, Version 2.0 (the "License"); 13 | you may not use this file except in compliance with the License. 14 | You may obtain a copy of the License at 15 | 16 | http://www.apache.org/licenses/LICENSE-2.0 17 | 18 | Unless required by applicable law or agreed to in writing, software 19 | distributed under the License is distributed on an "AS IS" BASIS, 20 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 21 | See the License for the specific language governing permissions and 22 | limitations under the License. 23 | */ 24 | #pragma once 25 | #include 26 | 27 | /** 28 | * Avoids copying inherited function implementations for all camera models. 29 | * 30 | * Requires the following functions in the derived class: 31 | * - static void Scale(const T* scale, const T* params) 32 | * - static void SetParams() 33 | * - static void Unproject(const T* pix, const T* params, T* ray) { 34 | * - static void Project(const T* ray, const T* params, T* pix) { 35 | * - static void dProject_dray(const T* ray, const T* params, T* j) { 36 | * - static void dProject_dparams(const T* ray, const T* params, T* j) 37 | * - static void dUnproject_dparams(const T* pix, const T* params, T* j) 38 | */ 39 | namespace calibu { 40 | template 41 | class CameraImpl : public CameraInterface { 42 | typedef typename CameraInterface::Vec2t Vec2t; 43 | typedef typename CameraInterface::Vec3t Vec3t; 44 | typedef typename CameraInterface::SE3t SE3t; 45 | 46 | public: 47 | static constexpr int kParamSize = ParamSize; 48 | 49 | CameraImpl() {} 50 | virtual ~CameraImpl() {} 51 | CameraImpl(const Eigen::VectorXd& params, Eigen::Vector2i& image_size) : 52 | CameraInterface(params, image_size) { 53 | } 54 | 55 | void 56 | Scale(const Scalar& s) override { 57 | Derived::Scale( s, this->params_.data() ); 58 | } 59 | 60 | void 61 | PrintInfo() const override { 62 | printf("camera model info:\n" ); 63 | printf(" Right = [%d; %d; %d] unit vector\n", (int)this->rdf_(0,0), (int)this->rdf_(0,1), (int)this->rdf_(0,2) ); 64 | printf(" Down = [%d; %d; %d] unit vector\n", (int)this->rdf_(1,0), (int)this->rdf_(1,1), (int)this->rdf_(1,2) ); 65 | printf(" Forward = [%d; %d; %d] unit vector\n", (int)this->rdf_(2,0), (int)this->rdf_(2,1), (int)this->rdf_(2,2) ); 66 | printf(" Width = %d pixels\n", (int) this->image_size_[0]); 67 | printf(" Height = %d pixels\n", (int) this->image_size_[1]); 68 | 69 | // Assume we're respecting parameter ordering. 70 | printf(" Horiz Center = %.3f pixels\n", this->params_[2] ); 71 | printf(" Vert Center = %.3f pixels\n", this->params_[3] ); 72 | printf(" Horiz FOV = %.3f degrees\n", 180.0*2.0*atan2( this->image_size_[0]/2, this->params_[0] )/M_PI ); 73 | printf(" Vert FOV = %.3f degrees\n", 180.0*2.0*atan2( this->image_size_[1]/2, this->params_[1] )/M_PI ); 74 | } 75 | 76 | /** Model-dependent operations. */ 77 | Eigen::Matrix 78 | K() const override { 79 | Eigen::Matrix Kmat; 80 | Derived::K( this->params_.data() , Kmat.data()); 81 | return Kmat; 82 | } 83 | 84 | Vec3t 85 | Unproject(const Vec2t& pix) const override { 86 | Vec3t ray; 87 | Derived::Unproject(pix.data(), this->params_.data(), ray.data()); 88 | return ray; 89 | } 90 | 91 | Vec2t 92 | Project(const Vec3t& ray) const override { 93 | Vec2t pix; 94 | Derived::Project(ray.data(), this->params_.data(), pix.data()); 95 | return pix; 96 | } 97 | 98 | Eigen::Matrix 99 | dProject_dparams(const Vec3t& ray) const override { 100 | Eigen::Matrix j; 101 | Derived::dProject_dparams(ray.data(), this->params_.data(), j.data()); 102 | return j; 103 | } 104 | 105 | Eigen::Matrix 106 | dUnproject_dparams(const Vec2t& pix) const override { 107 | Eigen::Matrix j; 108 | Derived::dUnproject_dparams(pix.data(), this->params_.data(), j.data()); 109 | return j; 110 | } 111 | 112 | Eigen::Matrix 113 | dProject_dray(const Vec3t& ray) const override { 114 | Eigen::Matrix j; 115 | Derived::dProject_dray(ray.data(), this->params_.data(), j.data()); 116 | return j; 117 | } 118 | }; // public CameraInterface 119 | } // namespace calibu 120 | -------------------------------------------------------------------------------- /include/calibu/cam/camera_models_crtp.h: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of the Calibu Project. 3 | https://github.com/arpg/Calibu 4 | 5 | Copyright (C) 2015 6 | Steven Lovegrove, 7 | Nima Keivan 8 | Christoffer Heckman, 9 | Gabe Sibley, 10 | University of Colorado at Boulder, 11 | George Washington University. 12 | 13 | Licensed under the Apache License, Version 2.0 (the "License"); 14 | you may not use this file except in compliance with the License. 15 | You may obtain a copy of the License at 16 | 17 | http://www.apache.org/licenses/LICENSE-2.0 18 | 19 | Unless required by applicable law or agreed to in writing, software 20 | distributed under the License is distributed on an "AS IS" BASIS, 21 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 22 | See the License for the specific language governing permissions and 23 | limitations under the License. 24 | */ 25 | 26 | /** 27 | * This file defines various camera models via including their files. 28 | * 29 | * 30 | * NOTES ON RADIAL DISTORTION MODELS (Poly2, Poly3, Rational3, etc.) 31 | * Generalized radial distortion model: 32 | * x_d = f(r_u)*x_u (1) 33 | * y_d = f(r_u)*y_u 34 | * r_d = sqrt(x_d^2 + y_d^2) 35 | * r_d = sqrt((f(r_u)*x_u)^2 + (f(r_u)*y_u)^2) 36 | * r_d = sqrt((f(r_u))^2*(x_u^2*y_u^2)) (2) 37 | * Recall that r_u = x_u^2*y_u^2 (3) 38 | * So, if we want to consider purely the radii, substitute (3) into (2): 39 | * r_d = r_u*f(r_u) (4) 40 | * here, r_d is known, so if we find minimum of (r_u*f(r_u)-r_d)^2 we are effectively finding 41 | * the zeroes of equation (4), and several models use the Newton method to approximate these quickly. 42 | * Using the found r_u, we use r_u / r_d = 1 / f(r_u) to compute the factor, 43 | * so we can compute x_u = (r_u / r_d) * x_d. 44 | */ 45 | 46 | #pragma once 47 | #include 48 | #include 49 | #include 50 | #include 51 | 52 | /// Add camera model templates. 53 | #include 54 | #include 55 | #include 56 | -------------------------------------------------------------------------------- /include/calibu/cam/camera_models_kb4.h: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of the Calibu Project. 3 | https://github.com/arpg/Calibu 4 | 5 | Copyright (C) 2015 6 | Steven Lovegrove, 7 | Nima Keivan 8 | Christoffer Heckman, 9 | Gabe Sibley, 10 | University of Colorado at Boulder, 11 | George Washington University. 12 | 13 | Licensed under the Apache License, Version 2.0 (the "License"); 14 | you may not use this file except in compliance with the License. 15 | You may obtain a copy of the License at 16 | 17 | http://www.apache.org/licenses/LICENSE-2.0 18 | 19 | Unless required by applicable law or agreed to in writing, software 20 | distributed under the License is distributed on an "AS IS" BASIS, 21 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 22 | See the License for the specific language governing permissions and 23 | limitations under the License. 24 | */ 25 | 26 | #pragma once 27 | #include 28 | #include 29 | #include 30 | #include 31 | 32 | namespace calibu { 33 | 34 | /** Kannala-Brandt camera model. 35 | * Kannala and Brandt Like 'Generic' Projection Model 36 | * http://ieeexplore.ieee.org/iel5/34/34427/01642666.pdf 37 | * http://april.eecs.umich.edu/wiki/index.php/Camera_suite 38 | */ 39 | template 40 | class KannalaBrandtCamera : public CameraImpl > { 41 | typedef CameraImpl > Base; 42 | public: 43 | using Base::Base; 44 | 45 | static constexpr int NumParams = 8; 46 | 47 | template 48 | static void Scale( const double s, T* params ) { 49 | CameraUtils::Scale( s, params ); 50 | } 51 | 52 | // NOTE: We ASSUME the first four entries of params_ to be fu, fv, sx 53 | // and sy. If your camera model doesn't respect this ordering, then evaluating 54 | // K for it will result in an incorrect matrix. 55 | template 56 | static void K( const T* params , T* Kmat) { 57 | CameraUtils::K( params , Kmat); 58 | } 59 | 60 | template 61 | static void Unproject(const T* pix, const T* params, T* ray) { 62 | 63 | const T fu = params[0]; 64 | const T fv = params[1]; 65 | const T u0 = params[2]; 66 | const T v0 = params[3]; 67 | 68 | const T k0 = params[4]; 69 | const T k1 = params[5]; 70 | const T k2 = params[6]; 71 | const T k3 = params[7]; 72 | 73 | const T un = pix[0] - u0; 74 | const T vn = pix[1] - v0; 75 | const T psi = atan2( fu*vn, fv*un ); 76 | 77 | const T rth = un / (fu * cos(psi) ); 78 | 79 | // Use Newtons method to solve for theta. 80 | T th = rth; 81 | for (int i=0; i<5; i++) 82 | { 83 | // f = (th + k0*th**3 + k1*th**5 + k2*th**7 + k3*th**9 - rth)^2 84 | const T th2 = th*th; 85 | const T th3 = th2*th; 86 | const T th4 = th2*th2; 87 | const T th6 = th4*th2; 88 | const T x0 = k0*th3 + k1*th4*th + k2*th6*th + k3*th6*th3 - rth + th; 89 | const T x1 = 3*k0*th2 + 5*k1*th4 + 7*k2*th6 + 9*k3*th6*th2 + 1; 90 | const T d = 2*x0*x1; 91 | const T d2 = 4*th*x0*(3*k0 + 10*k1*th2 + 21*k2*th4 + 36*k3*th6) + 2*x1*x1; 92 | const T delta = d / d2; 93 | th -= delta; 94 | } 95 | 96 | ray[0] = sin(th)*cos(psi); 97 | ray[1] = sin(th)*sin(psi); 98 | ray[2] = cos(th); 99 | } 100 | 101 | template 102 | static void Project(const T* ray, const T* params, T* pix) { 103 | const T fu = params[0]; 104 | const T fv = params[1]; 105 | const T u0 = params[2]; 106 | const T v0 = params[3]; 107 | 108 | const T k0 = params[4]; 109 | const T k1 = params[5]; 110 | const T k2 = params[6]; 111 | const T k3 = params[7]; 112 | 113 | const T Xsq_plus_Ysq = ray[0]*ray[0]+ray[1]*ray[1]; 114 | const T theta = atan2( sqrt(Xsq_plus_Ysq), ray[2] ); 115 | const T psi = atan2( ray[1], ray[0] ); 116 | 117 | const T theta2 = theta*theta; 118 | const T theta3 = theta2*theta; 119 | const T theta5 = theta3*theta2; 120 | const T theta7 = theta5*theta2; 121 | const T theta9 = theta7*theta2; 122 | const T r = theta + k0*theta3 + k1*theta5 + k2*theta7 + k3*theta9; 123 | 124 | pix[0] = fu*r*cos(psi) + u0; 125 | pix[1] = fv*r*sin(psi) + v0; 126 | } 127 | 128 | template 129 | static void dProject_dparams(const T* ray, const T* params, T* j) { 130 | T pix[2]; 131 | CameraUtils::Dehomogenize(ray, pix); 132 | CameraUtils::dMultK_dparams(params, pix, j); 133 | } 134 | 135 | template 136 | static void dUnproject_dparams(const T* pix, const T* params, T* j) { 137 | CameraUtils::dMultInvK_dparams(params, pix, j); 138 | } 139 | 140 | template 141 | static void dProject_dray(const T* ray, const T* params, T* j) { 142 | const T fu = params[0]; 143 | const T fv = params[1]; 144 | const T k0 = params[4]; 145 | const T k1 = params[5]; 146 | const T k2 = params[6]; 147 | const T k3 = params[7]; 148 | 149 | const T x0 = ray[0]*ray[0]; 150 | const T x1 = ray[1]*ray[1]; 151 | const T x2 = x0 + x1; 152 | const T x3 = sqrt(x2); 153 | const T x4 = std::pow(x2,3.0/2.0); 154 | const T x5 = ray[2]*ray[2] + x2; 155 | const T a = atan2(x3, ray[2]); 156 | const T a2 = a*a; 157 | const T a4 = a2*a2; 158 | const T a6 = a4*a2; 159 | const T a8 = a4*a4; 160 | const T x11 = k0*a2 + k1*a4 + k2*a6 + k3*a8 + 1; 161 | const T x12 = 3*k0*a2 + 5*k1*a4 + 7*k2*a6 + 9*k3*a8 + 1; 162 | const T x13 = ray[2]*x12/(x2*x5) - x11*a/x4; 163 | const T x14 = ray[0]*fu; 164 | const T x15 = ray[1]*fv; 165 | const T x16 = -1/x4; 166 | const T x17 = x11*a; 167 | const T x18 = -x12/x5; 168 | const T x19 = x17/x3; 169 | const T x20 = ray[2]*x12/(x2*x5); 170 | 171 | j[0] = fu*(x0*x16*x17 + x0*x20 + x19); 172 | j[1] = ray[1]*x13*x14; 173 | j[2] = x14*x18; 174 | j[3] = ray[0]*x13*x15; 175 | j[4] = fv*(x1*x16*x17 + x1*x20 + x19); 176 | j[5] = x15*x18; 177 | } 178 | }; 179 | } 180 | -------------------------------------------------------------------------------- /include/calibu/cam/camera_rig.h: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of the Calibu Project. 3 | https://github.com/gwu-robotics/Calibu 4 | 5 | Copyright (C) 2013 George Washington University, 6 | Steven Lovegrove, 7 | Gabe Sibley 8 | 9 | Licensed under the Apache License, Version 2.0 (the "License"); 10 | you may not use this file except in compliance with the License. 11 | You may obtain a copy of the License at 12 | 13 | http://www.apache.org/licenses/LICENSE-2.0 14 | 15 | Unless required by applicable law or agreed to in writing, software 16 | distributed under the License is distributed on an "AS IS" BASIS, 17 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 18 | See the License for the specific language governing permissions and 19 | limitations under the License. 20 | */ 21 | 22 | #pragma once 23 | 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | 31 | namespace calibu 32 | { 33 | 34 | ////////////////////////////////////////////////////////////////////////////// 35 | 36 | static const Sophus::SO3d RdfVision = 37 | Sophus::SO3d( (Eigen::Matrix3d() << 1,0,0, 0,1,0, 0,0,1).finished() ); 38 | 39 | static const Sophus::SO3d RdfRobotics = 40 | Sophus::SO3d( (Eigen::Matrix3d() << 0,0,1, 1,0,0, 0,1,0).finished() ); 41 | 42 | // T_2b_1b = T_ba * T_2a_1a * T_ab 43 | template 44 | inline Sophus::SE3Group ToCoordinateConvention( 45 | const Sophus::SE3Group& T_2a_1a, 46 | const Sophus::SO3Group& R_ba 47 | ) 48 | { 49 | Sophus::SE3Group T_2b_1b; 50 | T_2b_1b.so3() = R_ba * T_2a_1a.so3() * R_ba.inverse(); 51 | T_2b_1b.translation() = R_ba * T_2a_1a.translation(); 52 | return T_2b_1b; 53 | } 54 | 55 | template 56 | inline std::shared_ptr> ToCoordinateConvention( 57 | const std::shared_ptr>& rig, 58 | const Sophus::SO3Group& rdf 59 | ) 60 | { 61 | std::shared_ptr> ret(new calibu::Rig()); 62 | *ret = *rig; 63 | for(size_t c=0; cNumCams(); ++c) { 64 | const Sophus::SO3Group M = 65 | rdf * Sophus::SO3Group(rig->cameras_[c]->RDF()).inverse(); 66 | ret->cameras_[c]->SetPose(ToCoordinateConvention(rig->cameras_[c]->Pose(), M)); 67 | ret->cameras_[c]->SetRDF(rdf.matrix()); 68 | } 69 | return ret; 70 | } 71 | 72 | } 73 | -------------------------------------------------------------------------------- /include/calibu/cam/camera_xml.h: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of the Calibu Project. 3 | https://github.com/arpg/Calibu 4 | 5 | Copyright (C) 2015 University of Colorado at Boulder, 6 | Steven Lovegrove, 7 | Christoffer Heckman, 8 | Nima Keivan, 9 | Gabe Sibley 10 | 11 | Licensed under the Apache License, Version 2.0 (the "License"); 12 | you may not use this file except in compliance with the License. 13 | You may obtain a copy of the License at 14 | 15 | http://www.apache.org/licenses/LICENSE-2.0 16 | 17 | Unless required by applicable law or agreed to in writing, software 18 | distributed under the License is distributed on an "AS IS" BASIS, 19 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 20 | See the License for the specific language governing permissions and 21 | limitations under the License. 22 | */ 23 | 24 | #pragma once 25 | 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | 32 | namespace calibu 33 | { 34 | 35 | const std::string NODE_RIG = "rig"; 36 | const std::string NODE_CAM_POSE = "camera"; 37 | const std::string NODE_CAM = "camera_model"; 38 | const std::string NODE_POSE = "pose"; 39 | 40 | typedef CameraInterface CameraInterfaced; 41 | typedef Rig Rigd; 42 | 43 | /////////////////////////////////////////////////////////////////////////////// 44 | template inline 45 | void StrToVal( T& t, const std::string& sValue ) 46 | { 47 | std::istringstream iss( sValue ); 48 | iss >> t; 49 | } 50 | 51 | /////////////////////////////////////////////////////////////////////////////// 52 | template inline 53 | T StrToVal( const std::string& sValue, const T& default_val = T() ) 54 | { 55 | T t = default_val; 56 | std::istringstream iss( sValue ); 57 | iss >> t; 58 | return t; 59 | } 60 | 61 | /////////////////////////////////////////////////////////////////////////////// 62 | template inline 63 | std::string ValToStr( const T& t ) 64 | { 65 | std::ostringstream oss; 66 | oss << t; 67 | return oss.str(); 68 | } 69 | 70 | /////////////////////////////////////////////////////////////////////////////// 71 | CALIBU_EXPORT 72 | std::string CameraType( const std::string& sType ); 73 | 74 | CALIBU_EXPORT 75 | std::string IndentStr(int indent); 76 | 77 | CALIBU_EXPORT 78 | std::string AttribOpen(const std::string& attrib); 79 | 80 | CALIBU_EXPORT 81 | std::string AttribClose(const std::string& attrib); 82 | 83 | CALIBU_EXPORT 84 | void WriteXmlSE3(std::ostream& out, const Sophus::SE3d& t_rc, int indent); 85 | 86 | 87 | CALIBU_EXPORT 88 | void WriteXmlCamera(std::ostream& out, const std::shared_ptr cam, 89 | int indent = 0); 90 | 91 | CALIBU_EXPORT 92 | void WriteXmlCamera(const std::string& filename, 93 | const std::shared_ptr cam); 94 | 95 | 96 | //////////////////////////////////////////////////////////////////////////// 97 | inline void WriteXmlCameraAndTransformWithLut( 98 | std::ostream& out, 99 | std::string& sLutXmlElement, 100 | const std::shared_ptr cam, 101 | int indent = 0 102 | ) 103 | { 104 | const std::string dd = IndentStr(indent); 105 | out << dd << AttribOpen(NODE_CAM_POSE) << std::endl; 106 | WriteXmlCamera(out, cam, indent+4); 107 | WriteXmlSE3(out,cam->Pose(),indent+4); 108 | 109 | out << dd << sLutXmlElement << std::endl; // LUT, if there is one! 110 | out << dd << AttribClose(NODE_CAM_POSE) << std::endl; 111 | } 112 | 113 | 114 | /////////////////////////////////////////////////////////////////////////////// 115 | CALIBU_EXPORT 116 | std::shared_ptr ReadXmlCamera( 117 | const std::string& filename, const std::shared_ptr cam); 118 | 119 | //////////////////////////////////////////////////////////////////////// 120 | 121 | CALIBU_EXPORT 122 | void WriteXmlSE3(std::ostream& out, const Sophus::SE3d& t_rc, int indent = 0); 123 | 124 | CALIBU_EXPORT 125 | void WriteXmlSE3(const std::string& filename, const Sophus::SE3d& t_rc); 126 | 127 | CALIBU_EXPORT 128 | Sophus::SE3d ReadXmlSE3(const std::string& filename); 129 | 130 | //////////////////////////////////////////////////////////////////////// 131 | 132 | CALIBU_EXPORT 133 | void WriteXmlCameraAndTransform(std::ostream& out, 134 | const std::shared_ptr cop, 135 | int indent = 0); 136 | 137 | CALIBU_EXPORT 138 | void WriteXmlCameraAndTransform(const std::string& filename, 139 | const std::shared_ptr cop); 140 | 141 | CALIBU_EXPORT 142 | std::shared_ptr ReadXmlCameraAndTransform( 143 | const std::string& filename, const std::shared_ptr cop); 144 | 145 | CALIBU_EXPORT 146 | std::shared_ptr ReadXmlCameraAndTransform( 147 | const std::string& filename); 148 | 149 | //////////////////////////////////////////////////////////////////////// 150 | CALIBU_EXPORT 151 | void WriteXmlRig(std::ostream& out, const std::shared_ptr rig, int indent = 0); 152 | 153 | CALIBU_EXPORT 154 | void WriteXmlRig(const std::string& filename, const std::shared_ptr rig); 155 | 156 | CALIBU_EXPORT 157 | std::shared_ptr ReadXmlRig(const std::string& filename); 158 | 159 | CALIBU_EXPORT 160 | std::shared_ptr ReadXmlRigFromString(const std::string& modelXML); 161 | } // namespace calibu 162 | -------------------------------------------------------------------------------- /include/calibu/cam/stereo_rectify.h: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of the Calibu Project. 3 | https://github.com/arpg/Calibu 4 | 5 | Copyright (C) 2013 George Washington University, 6 | Steven Lovegrove, 7 | Gabe Sibley 8 | 9 | Licensed under the Apache License, Version 2.0 (the "License"); 10 | you may not use this file except in compliance with the License. 11 | You may obtain a copy of the License at 12 | 13 | http://www.apache.org/licenses/LICENSE-2.0 14 | 15 | Unless required by applicable law or agreed to in writing, software 16 | distributed under the License is distributed on an "AS IS" BASIS, 17 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 18 | See the License for the specific language governing permissions and 19 | limitations under the License. 20 | */ 21 | 22 | #pragma once 23 | 24 | #include 25 | 26 | #include 27 | #include 28 | #include 29 | #include 30 | 31 | namespace calibu 32 | { 33 | 34 | /// Create left and right camera lookup tables from left and right camera models, 35 | /// and output their new intrinsics and extrinsics. 36 | /// Returns: New camera rig (intrinsics same for both cameras) 37 | /// T_nr_nl: New scanline rectified extrinsics considering rotation applied in lookup tables. 38 | CALIBU_EXPORT 39 | std::shared_ptr > CreateScanlineRectifiedLookupAndCameras( 40 | const Sophus::SE3d& T_rl, 41 | const std::shared_ptr> cam_left, 42 | const std::shared_ptr> cam_right, 43 | Sophus::SE3d& T_nr_nl, 44 | LookupTable& left_lut, 45 | LookupTable& right_lut 46 | ); 47 | 48 | } 49 | 50 | -------------------------------------------------------------------------------- /include/calibu/conics/Conic.h: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of the Calibu Project. 3 | https://github.com/gwu-robotics/Calibu 4 | 5 | Copyright (C) 2013 George Washington University, 6 | Steven Lovegrove 7 | 8 | Licensed under the Apache License, Version 2.0 (the "License"); 9 | you may not use this file except in compliance with the License. 10 | You may obtain a copy of the License at 11 | 12 | http://www.apache.org/licenses/LICENSE-2.0 13 | 14 | Unless required by applicable law or agreed to in writing, software 15 | distributed under the License is distributed on an "AS IS" BASIS, 16 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 17 | See the License for the specific language governing permissions and 18 | limitations under the License. 19 | */ 20 | 21 | #pragma once 22 | 23 | #include 24 | #include 25 | 26 | #include 27 | #include 28 | 29 | #include 30 | #include 31 | #include 32 | 33 | namespace calibu { 34 | 35 | struct Conic { 36 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 37 | IRectangle bbox; 38 | 39 | // quadratic form: x'*C*x = 0 with x = (x1,x2,1) are points on the ellipse 40 | Eigen::Matrix3d C; 41 | 42 | // l:=C*x is tangent line throught the point x. The dual of C is adj(C). 43 | // For lines through C it holds: l'*adj(C)*l = 0. 44 | // If C has full rank it holds up to scale: adj(C) = C^{-1} 45 | Eigen::Matrix3d Dual; 46 | 47 | // center (c1,c2) 48 | Eigen::Vector2d center; 49 | }; 50 | 51 | CALIBU_EXPORT 52 | double Distance( const Conic& c1, const Conic& c2, double circle_radius ); 53 | 54 | CALIBU_EXPORT 55 | std::array, 2 > PlaneFromConic( 56 | const Conic& c, double plane_circle_radius, const Eigen::Matrix3d& K 57 | ); 58 | 59 | CALIBU_EXPORT 60 | std::pair PlaneFromConics( 61 | const std::vector >& conics, 62 | double plane_circle_radius, 63 | const Eigen::Matrix3d& K, double inlier_threshold 64 | ); 65 | CALIBU_EXPORT 66 | Conic UnmapConic( const Conic& c, const std::shared_ptr> cam ); 67 | 68 | /** Returns the major and minor axes lengths of the conic */ 69 | CALIBU_EXPORT 70 | Eigen::Vector2d GetAxesLengths(const Conic& c); 71 | 72 | } 73 | -------------------------------------------------------------------------------- /include/calibu/conics/ConicFinder.h: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of the Calibu Project. 3 | https://github.com/gwu-robotics/Calibu 4 | 5 | Copyright (C) 2013 George Washington University, 6 | Steven Lovegrove 7 | 8 | Licensed under the Apache License, Version 2.0 (the "License"); 9 | you may not use this file except in compliance with the License. 10 | You may obtain a copy of the License at 11 | 12 | http://www.apache.org/licenses/LICENSE-2.0 13 | 14 | Unless required by applicable law or agreed to in writing, software 15 | distributed under the License is distributed on an "AS IS" BASIS, 16 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 17 | See the License for the specific language governing permissions and 18 | limitations under the License. 19 | */ 20 | 21 | #pragma once 22 | 23 | #include 24 | #include 25 | #include 26 | 27 | #include 28 | #include 29 | #include 30 | 31 | namespace calibu { 32 | 33 | struct ParamsConicFinder 34 | { 35 | ParamsConicFinder() : 36 | conic_min_area(25), 37 | conic_max_area(4E4), 38 | conic_min_density(0.4), 39 | conic_min_aspect(0.1) 40 | { 41 | 42 | } 43 | 44 | float conic_min_area; 45 | float conic_max_area; 46 | float conic_min_density; 47 | float conic_min_aspect; 48 | }; 49 | 50 | CALIBU_EXPORT 51 | class ConicFinder 52 | { 53 | public: 54 | ConicFinder(); 55 | void Find(const ImageProcessing& imgs); 56 | 57 | inline const std::vector >& 58 | Conics() const { 59 | return conics; 60 | } 61 | 62 | ParamsConicFinder& Params() { 63 | return params; 64 | } 65 | 66 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 67 | 68 | protected: 69 | // Output of this class 70 | std::vector candidates; 71 | std::vector > conics; 72 | 73 | ParamsConicFinder params; 74 | }; 75 | 76 | } 77 | -------------------------------------------------------------------------------- /include/calibu/conics/FindConics.h: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of the Calibu Project. 3 | https://github.com/gwu-robotics/Calibu 4 | 5 | Copyright (C) 2013 George Washington University, 6 | Steven Lovegrove 7 | 8 | Licensed under the Apache License, Version 2.0 (the "License"); 9 | you may not use this file except in compliance with the License. 10 | You may obtain a copy of the License at 11 | 12 | http://www.apache.org/licenses/LICENSE-2.0 13 | 14 | Unless required by applicable law or agreed to in writing, software 15 | distributed under the License is distributed on an "AS IS" BASIS, 16 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 17 | See the License for the specific language governing permissions and 18 | limitations under the License. 19 | */ 20 | 21 | #pragma once 22 | 23 | #include 24 | 25 | #include 26 | #include 27 | #include 28 | 29 | namespace calibu { 30 | 31 | template 32 | Eigen::Matrix3d FindEllipse( 33 | const int w, const int /*h*/, 34 | const TdI* dI, 35 | const IRectangle& r, 36 | double& /*residual*/ 37 | ); 38 | 39 | CALIBU_EXPORT 40 | void FindCandidateConicsFromLabels( 41 | unsigned w, unsigned h, 42 | const std::vector& labels, 43 | std::vector& candidates, 44 | float min_area, 45 | float max_area, 46 | float min_density, 47 | float min_aspect 48 | ); 49 | 50 | template 51 | void FindConics( 52 | const int w, const int h, 53 | const std::vector& candidates, 54 | const TdI* dI, 55 | std::vector >& conics 56 | ); 57 | 58 | } 59 | -------------------------------------------------------------------------------- /include/calibu/exception.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #define CALIBU_THROW(desc) \ 7 | throw ::calibu::Exception(__LINE__, __FILE__, desc) 8 | 9 | #define CALIBU_ASSERT_DESC(cond, desc) \ 10 | if (!(cond)) CALIBU_THROW(desc) 11 | 12 | #define CALIBU_ASSERT(cond) \ 13 | CALIBU_ASSERT_DESC(cond, "assertion failed: " #cond) 14 | 15 | #ifdef NDEBUG 16 | #define CALIBU_DEBUG_DESC(cond, desc) 17 | #define CALIBU_DEBUG(cond) 18 | #else 19 | #define CALIBU_DEBUG_DESC CALIBU_ASSERT_DESC 20 | #define CALIBU_DEBUG CALIBU_ASSERT 21 | #endif 22 | 23 | namespace calibu 24 | { 25 | 26 | /** 27 | * Respresents a generic exception thrown from the calibu framework. An 28 | * Exception object can be created directly or via one of the macros 29 | * defined in calibu/exception.h 30 | */ 31 | class Exception : public std::exception 32 | { 33 | public: 34 | 35 | /** 36 | * Creates an exception with the given metadata 37 | * @param line line the error occurred on 38 | * @param file file the error occurred in 39 | * @param desc description of the error 40 | */ 41 | Exception(int line, const std::string& file, const std::string& desc) : 42 | line_(line), 43 | file_(file), 44 | desc_(desc) 45 | { 46 | Initialize(); 47 | } 48 | 49 | /** 50 | * Returns the error line number 51 | * @return line the error occurred on 52 | */ 53 | inline int line() const 54 | { 55 | return line_; 56 | } 57 | 58 | /** 59 | * Returns the error source file 60 | * @return file the error occurred in 61 | */ 62 | inline const std::string& file() const 63 | { 64 | return file_; 65 | } 66 | 67 | /** 68 | * Returns the error description 69 | * @return description of the error 70 | */ 71 | inline const std::string& desc() const 72 | { 73 | return desc_; 74 | } 75 | 76 | /** 77 | * Returns a formatted string giving the full error message 78 | * @return full error message 79 | */ 80 | inline const char* what() const throw() override 81 | { 82 | return what_.c_str(); 83 | } 84 | 85 | private: 86 | 87 | /** 88 | * Constructs the formated string from the given metadata 89 | */ 90 | void Initialize() 91 | { 92 | what_ = file_ + "(" + std::to_string(line_) + "): " + desc_; 93 | } 94 | 95 | protected: 96 | 97 | /** Line the error occurred on */ 98 | int line_; 99 | 100 | /** File the error occurred in */ 101 | std::string file_; 102 | 103 | /** Description of the error */ 104 | std::string desc_; 105 | 106 | /** Full error message */ 107 | std::string what_; 108 | }; 109 | 110 | } // namespace calibu -------------------------------------------------------------------------------- /include/calibu/gl/Drawing.h: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of the Calibu Project. 3 | https://github.com/gwu-robotics/Calibu 4 | 5 | Copyright (C) 2013 George Washington University, 6 | Steven Lovegrove 7 | 8 | Licensed under the Apache License, Version 2.0 (the "License"); 9 | you may not use this file except in compliance with the License. 10 | You may obtain a copy of the License at 11 | 12 | http://www.apache.org/licenses/LICENSE-2.0 13 | 14 | Unless required by applicable law or agreed to in writing, software 15 | distributed under the License is distributed on an "AS IS" BASIS, 16 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 17 | See the License for the specific language governing permissions and 18 | limitations under the License. 19 | */ 20 | 21 | #pragma once 22 | 23 | #include 24 | 25 | #include 26 | 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | 33 | namespace calibu { 34 | 35 | inline void glDrawRectPerimeter( const IRectangle& r ) 36 | { 37 | pangolin::glDrawRectPerimeter(r.x1,r.y1, r.x2, r.y2); 38 | } 39 | 40 | inline void glDrawPlane(const Eigen::Vector3d& nd_w, float scale, int grid) 41 | { 42 | const Sophus::SE3d T_wn = PlaneBasis_wp(nd_w); 43 | pangolin::glSetFrameOfReference(T_wn.matrix()); 44 | pangolin::glDraw_z0(scale,grid); 45 | pangolin::glUnsetFrameOfReference(); 46 | } 47 | 48 | inline void glDrawPlane(const Eigen::Vector4d& N_w, float scale, int grid) 49 | { 50 | // TODO: Verify that this works. 51 | const Eigen::Vector3d nd_w = N_w.head<3>() / N_w(3); 52 | glDrawPlane(nd_w, scale, grid); 53 | } 54 | 55 | inline void glDrawTarget( const TargetInterface& t, const Eigen::Vector2d& offset, double scale, double sat, double val ) 56 | { 57 | 58 | for( unsigned int i=0; i& map, const TargetInterface& target, const Eigen::Vector2d& offset, double scale, double sat, double val ) 68 | { 69 | const double r = target.CircleRadius() * scale; 70 | 71 | for( unsigned int i=0; i= 0 ) 75 | { 76 | const Eigen::Vector2d p = target.Circles2D()[t] * scale + offset; 77 | pangolin::glColorBin(t,target.Circles2D().size(),sat,val); 78 | pangolin::glDrawCircle(p,r); 79 | } 80 | } 81 | } 82 | 83 | } 84 | -------------------------------------------------------------------------------- /include/calibu/image/AdaptiveThreshold.h: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of the Calibu Project. 3 | https://github.com/gwu-robotics/Calibu 4 | 5 | Copyright (C) 2013 George Washington University, 6 | Steven Lovegrove 7 | 8 | Licensed under the Apache License, Version 2.0 (the "License"); 9 | you may not use this file except in compliance with the License. 10 | You may obtain a copy of the License at 11 | 12 | http://www.apache.org/licenses/LICENSE-2.0 13 | 14 | Unless required by applicable law or agreed to in writing, software 15 | distributed under the License is distributed on an "AS IS" BASIS, 16 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 17 | See the License for the specific language governing permissions and 18 | limitations under the License. 19 | */ 20 | 21 | #pragma once 22 | 23 | #include 24 | 25 | #include 26 | 27 | namespace calibu { 28 | 29 | template 30 | void AdaptiveThreshold( int w, int h, const TI* I, const TintI* intI, Tout* out, float threshold, int rad, Tout pass, Tout fail ) 31 | { 32 | // Adaptive Thresholding Using the Integral Image 33 | // Derek Bradley, Gerhard Roth 34 | 35 | for ( int j=0; j 56 | void AdaptiveThreshold( int w, int h, const TI* I, const TintI* intI, Tout* out, float threshold, int rad, int min_diff, Tout pass, Tout fail ) 57 | { 58 | // Adaptive Thresholding Using the Integral Image 59 | // Derek Bradley, Gerhard Roth 60 | 61 | // With min diff trick to make it less sensitive in homogeneous regions: 62 | // http://homepages.inf.ed.ac.uk/rbf/HIPR2/adpthrsh.htm 63 | 64 | for ( int j=0; j 24 | 25 | namespace calibu { 26 | 27 | // Influenced by libCVD gradient method 28 | template 29 | void gradient(const int w, const int h, const TI* I, TD* grad) 30 | { 31 | const TI* pI = I + w + 1; 32 | const TI* pEnd = I + w*h - w - 1; 33 | TD* pOut = grad + w + 1; 34 | 35 | while (pI != pEnd) { 36 | (*pOut)[0] = *(pI+1) - *(pI-1); 37 | (*pOut)[1] = *(pI+w) - *(pI-w); 38 | pI++; 39 | pOut++; 40 | } 41 | } 42 | 43 | } 44 | -------------------------------------------------------------------------------- /include/calibu/image/ImageProcessing.h: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of the Calibu Project. 3 | https://github.com/gwu-robotics/Calibu 4 | 5 | Copyright (C) 2013 George Washington University, 6 | Steven Lovegrove 7 | 8 | Licensed under the Apache License, Version 2.0 (the "License"); 9 | you may not use this file except in compliance with the License. 10 | You may obtain a copy of the License at 11 | 12 | http://www.apache.org/licenses/LICENSE-2.0 13 | 14 | Unless required by applicable law or agreed to in writing, software 15 | distributed under the License is distributed on an "AS IS" BASIS, 16 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 17 | See the License for the specific language governing permissions and 18 | limitations under the License. 19 | */ 20 | 21 | #pragma once 22 | 23 | #include 24 | #include 25 | 26 | #include 27 | #include 28 | 29 | namespace calibu { 30 | 31 | struct ParamsImageProcessing { 32 | ParamsImageProcessing() : at_threshold(0.7), 33 | at_window_ratio(3), 34 | black_on_white(true) {} 35 | float at_threshold; 36 | int at_window_ratio; 37 | bool black_on_white; 38 | }; 39 | 40 | CALIBU_EXPORT 41 | class ImageProcessing { 42 | public: 43 | ImageProcessing(int maxWidth, int maxHeight); 44 | ~ImageProcessing(); 45 | 46 | void Process(const unsigned char* greyscale_image, size_t w, size_t h, size_t pitch); 47 | 48 | inline int Width() const { return width; } 49 | inline int Height() const { return height; } 50 | 51 | inline const unsigned char* Img() const { return &I[0]; } 52 | inline const Eigen::Vector2f* ImgDeriv() const { return &dI[0]; } 53 | inline const unsigned char* ImgThresh() const { return &tI[0]; } 54 | inline const std::vector& Labels() const { return labels; } 55 | 56 | ParamsImageProcessing& Params() { return params; } 57 | 58 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 59 | 60 | protected: 61 | void AllocateImageData(int maxPixels); 62 | void DeallocateImageData(); 63 | 64 | int width, height; 65 | 66 | // Images owned by this class 67 | std::vector I; 68 | std::vector intI; 69 | std::vector > dI; 70 | std::vector lI; 71 | std::vector tI; 72 | 73 | std::vector labels; 74 | ParamsImageProcessing params; 75 | }; 76 | 77 | } 78 | -------------------------------------------------------------------------------- /include/calibu/image/IntegralImage.h: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of the Calibu Project. 3 | https://github.com/gwu-robotics/Calibu 4 | 5 | Copyright (C) 2013 George Washington University, 6 | Steven Lovegrove 7 | 8 | Licensed under the Apache License, Version 2.0 (the "License"); 9 | you may not use this file except in compliance with the License. 10 | You may obtain a copy of the License at 11 | 12 | http://www.apache.org/licenses/LICENSE-2.0 13 | 14 | Unless required by applicable law or agreed to in writing, software 15 | distributed under the License is distributed on an "AS IS" BASIS, 16 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 17 | See the License for the specific language governing permissions and 18 | limitations under the License. 19 | */ 20 | 21 | #pragma once 22 | 23 | #include 24 | 25 | namespace calibu { 26 | 27 | // Influenced by libCVD integral_image 28 | template 29 | void integral_image(const int w, const int h, const TI* in, TO* out) 30 | { 31 | out[0] = in[0]; 32 | 33 | //Do the first row. 34 | for(int x=1; x < w; x++) 35 | out[x] =out[x-1] + in[x]; 36 | 37 | //Do the first column. 38 | for(int y=1; y < h; y++) 39 | out[y*w] =out[(y-1)*w] + in[y*w]; 40 | 41 | //Do the remainder of the image 42 | for(int y=1; y < h; y++) { 43 | TO sum = in[y*w]; 44 | 45 | for(int x=1; x < w; x++) { 46 | sum += in[y*w+x]; 47 | out[y*w+x] = sum + out[(y-1)*w+x]; 48 | } 49 | } 50 | } 51 | 52 | } 53 | -------------------------------------------------------------------------------- /include/calibu/image/Label.h: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of the Calibu Project. 3 | https://github.com/gwu-robotics/Calibu 4 | 5 | Copyright (C) 2013 George Washington University, 6 | Steven Lovegrove 7 | 8 | Licensed under the Apache License, Version 2.0 (the "License"); 9 | you may not use this file except in compliance with the License. 10 | You may obtain a copy of the License at 11 | 12 | http://www.apache.org/licenses/LICENSE-2.0 13 | 14 | Unless required by applicable law or agreed to in writing, software 15 | distributed under the License is distributed on an "AS IS" BASIS, 16 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 17 | See the License for the specific language governing permissions and 18 | limitations under the License. 19 | */ 20 | 21 | #pragma once 22 | 23 | #include 24 | #include 25 | 26 | #include 27 | 28 | namespace calibu { 29 | 30 | struct PixelClass 31 | { 32 | int equiv; 33 | IRectangle bbox; 34 | int size; 35 | }; 36 | 37 | CALIBU_EXPORT 38 | void Label( 39 | int w, int h, 40 | const unsigned char* I, 41 | short* label, 42 | std::vector& labels, 43 | unsigned char passval 44 | ); 45 | 46 | } 47 | -------------------------------------------------------------------------------- /include/calibu/pcalib/pcalib.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | namespace calibu 9 | { 10 | 11 | /** 12 | * Represents the photometric calibration of a single camera. 13 | * It is a collection of camera responses and attenuation factors (vignetting). 14 | * If multiple response models are provided, it is assumed each response 15 | * pertains to each color channel. If only one response is given, then it 16 | * pertains to all color channels. The same is true for vignetting. 17 | */ 18 | template 19 | struct PhotoCamera 20 | { 21 | /** 22 | * Clears the response and vignetting vectors 23 | */ 24 | inline void Clear() 25 | { 26 | responses.clear(); 27 | vignettings.clear(); 28 | } 29 | 30 | /** Ordered list of response models */ 31 | std::vector>> responses; 32 | 33 | /** Ordered list of vignetting models */ 34 | std::vector>> vignettings; 35 | }; 36 | 37 | /** 38 | * Represents a collection photometric camera calibrations 39 | * The order of cameras is assumed to align with input camera streams order. 40 | */ 41 | template 42 | struct PhotoRig 43 | { 44 | /** 45 | * Clears camera vector 46 | */ 47 | inline void Clear() 48 | { 49 | cameras.clear(); 50 | } 51 | 52 | /** Ordered list of photometric camera calibrations */ 53 | std::vector>> cameras; 54 | }; 55 | 56 | typedef PhotoCamera PhotoCamerad; 57 | 58 | typedef PhotoRig PhotoRigd; 59 | 60 | } // namespace calibu -------------------------------------------------------------------------------- /include/calibu/pcalib/response.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | namespace calibu 8 | { 9 | 10 | template 11 | class Response 12 | { 13 | public: 14 | 15 | /** 16 | * Create inverse-response model 17 | */ 18 | Response() 19 | { 20 | } 21 | 22 | virtual ~Response() 23 | { 24 | } 25 | 26 | /** 27 | * Returns name of the response model type. This will be a unique name for 28 | * each response model implementation. 29 | * @return model type name 30 | */ 31 | inline const std::string& Type() const 32 | { 33 | return type_; 34 | } 35 | 36 | /** 37 | * Returns the number of parameters in the response model 38 | * @return number of model parameters 39 | */ 40 | inline int NumParams() const 41 | { 42 | return params_.size(); 43 | } 44 | 45 | /** 46 | * Determines if given intensity value is within value response range 47 | * @param value intensity value in question 48 | * @return true if within range 49 | */ 50 | inline bool InRange(double value) const 51 | { 52 | return value >= range_[0] && value <= range_[1]; 53 | } 54 | 55 | /** 56 | * Returns the modeled pixel intensity range. The minimum intensity is 57 | * stored at index-0 and the maximum at index-1. These values can be used to 58 | * adapt the current model, when processing images with different formats. 59 | * @return pixel intensity range 60 | */ 61 | inline const Eigen::Vector2d& GetRange() const 62 | { 63 | return range_; 64 | } 65 | 66 | /** 67 | * Sets the minimum and maximum pixel intensities. For example, the standard 68 | * unsigned, 8-bit color, a range of [0..255] would be used. In the case of 69 | * 32-bit floating point color, a range of [0..1] could be used. 70 | * @param min minimum intensity value 71 | * @param max maximum intensity value 72 | */ 73 | inline void SetRange(double min, double max) 74 | { 75 | SetRange(Eigen::Vector2d(min, max)); 76 | } 77 | 78 | /** 79 | * Sets the minimum and maximum pixel intensities. For example, the standard 80 | * unsigned, 8-bit color, a range of [0..255] would be used. In the case of 81 | * 32-bit floating point color, a range of [0..1] could be used. The minimum 82 | * intensity is stored at index-0 and the maximum at index-1. 83 | * @param range 2D vector specifying min and max intensities, respectively 84 | */ 85 | inline void SetRange(const Eigen::Vector2d& range) 86 | { 87 | range_ = range; 88 | } 89 | 90 | /** 91 | * Returns a constant reference to the model parameters 92 | * @return current model parameters 93 | */ 94 | inline const Eigen::VectorXd& GetParams() const 95 | { 96 | return params_; 97 | } 98 | 99 | /** 100 | * Assigns a new set of model parameters 101 | * @param params new model parameters to be assigned 102 | */ 103 | inline void SetParams(const Eigen::VectorXd& params) 104 | { 105 | CALIBU_ASSERT_DESC(params.size() == params_.size(), "invalid param count"); 106 | params_ = params; 107 | } 108 | 109 | /** 110 | * Evaluates the inverse-response for the given pixel intensity. 111 | * @param value pixel intensity 112 | * @return evaluated inverse-response 113 | */ 114 | virtual Scalar operator()(Scalar value) const = 0; 115 | 116 | /** 117 | * Resets the model parameters, which results in a linear response. 118 | */ 119 | virtual void Reset() = 0; 120 | 121 | protected: 122 | 123 | /** Response type name */ 124 | std::string type_; 125 | 126 | /** Pixel intensity range */ 127 | Eigen::Vector2d range_; 128 | 129 | /** Response model parameter vector */ 130 | Eigen::VectorXd params_; 131 | }; 132 | 133 | } // namespace calibu -------------------------------------------------------------------------------- /include/calibu/pcalib/response_impl.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | namespace calibu 7 | { 8 | 9 | /** 10 | * Inverse camera response implementation template, which implements the 11 | * Response interface by calling static functions provided by derived classes. 12 | */ 13 | template 14 | class ResponseImpl : public Response 15 | { 16 | public: 17 | 18 | /** 19 | * Create inverse-response model 20 | */ 21 | ResponseImpl() 22 | { 23 | Initialize(); 24 | } 25 | 26 | virtual ~ResponseImpl() 27 | { 28 | } 29 | 30 | /** 31 | * Evaluates the inverse-response for the given pixel intensity. 32 | * @param value pixel intensity 33 | * @return evaluated inverse-response 34 | */ 35 | Scalar operator()(Scalar value) const override 36 | { 37 | CALIBU_DEBUG_DESC(this->InRange(value), "invalid intensity value"); 38 | return Derived::GetResponse(this->params_.data(), value); 39 | } 40 | 41 | /** 42 | * Resets the model parameters, which results in a linear response. 43 | */ 44 | virtual void Reset() override 45 | { 46 | Derived::ResetParameters(this->params_.data()); 47 | } 48 | 49 | private: 50 | 51 | /** 52 | * Initializes member variables shared by all derived classes 53 | */ 54 | void Initialize() 55 | { 56 | this->range_ = Eigen::Vector2d(0, 1); 57 | this->type_ = std::string(Derived::type); 58 | this->params_.resize(Derived::param_count); 59 | Derived::ResetParameters(this->params_.data()); 60 | } 61 | }; 62 | 63 | } // namespace calibu -------------------------------------------------------------------------------- /include/calibu/pcalib/response_linear.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace calibu 6 | { 7 | 8 | /** 9 | * Linear camera response model, used in the trivial case where the 10 | * image intensities are always proportional to scene irradiance. 11 | */ 12 | template 13 | class LinearResponse : public ResponseImpl> 14 | { 15 | public: 16 | 17 | /** Number of parameters used for model */ 18 | static const int param_count = 0; 19 | 20 | /** Unique vignetting type name */ 21 | static constexpr const char* type = "linear"; 22 | 23 | public: 24 | 25 | /** 26 | * Create inverse-response model 27 | */ 28 | LinearResponse() 29 | { 30 | } 31 | 32 | virtual ~LinearResponse() 33 | { 34 | } 35 | 36 | /** 37 | * Evaluates the inverse-response for the given pixel intensity. 38 | * As this model served in the trivial case where the image is uniformly 39 | * attenuated, an attenuation factor of one is always returned, regardless 40 | * of the privided arguments. 41 | * @param params model parameters used for evaluation 42 | * @param value pixel intensity 43 | * @return evaluated inverse-response 44 | */ 45 | template 46 | static inline T GetResponse(const T*, T value) 47 | { 48 | return value; 49 | } 50 | 51 | /** 52 | * Resets the model parameters, which results in a linear response. 53 | * For this specific model, this function does nothing 54 | * @param params parameter vector to be reset 55 | * @param range min and max range of image intensities 56 | */ 57 | template 58 | static inline void ResetParameters(T*) 59 | { 60 | } 61 | }; 62 | 63 | } // namespace calibu -------------------------------------------------------------------------------- /include/calibu/pcalib/response_poly.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace calibu 6 | { 7 | 8 | /** 9 | * 3rd order polynomial inverse-response model. 10 | */ 11 | template 12 | class Poly3Response : public ResponseImpl> 13 | { 14 | public: 15 | 16 | /** Number of parameters used for model */ 17 | static const int param_count = 3; 18 | 19 | /** Unique inverse-response type name */ 20 | static constexpr const char* type = "poly3"; 21 | 22 | public: 23 | 24 | /** 25 | * Create inverse-response model 26 | */ 27 | Poly3Response() 28 | { 29 | } 30 | 31 | virtual ~Poly3Response() 32 | { 33 | } 34 | 35 | /** 36 | * Evaluates the inverse-response for the given pixel intensity. 37 | * @param params model parameters used for evaluation 38 | * @param value pixel intensity 39 | * @return evaluated inverse-response 40 | */ 41 | template 42 | static inline T GetResponse(const T* params, T value) 43 | { 44 | // initialize attenuation 45 | 46 | T pow = value; 47 | T result = T(0); 48 | 49 | // add each term of the polynomial 50 | 51 | for (int i = 0; i < param_count; ++i) 52 | { 53 | result += pow * params[i]; 54 | pow *= value; 55 | } 56 | 57 | return result; 58 | } 59 | 60 | /** 61 | * Resets the model parameters, which results in a linear response. 62 | * @param params parameter vector to be reset 63 | * @param range min and max range of image intensities 64 | */ 65 | template 66 | static inline void ResetParameters(T* params) 67 | { 68 | Eigen::Map x(params); 69 | x = Eigen::Vector3d(1, 0, 0); 70 | } 71 | }; 72 | 73 | /** 74 | * 4th order polynomial inverse-response model. 75 | */ 76 | template 77 | class Poly4Response : public ResponseImpl> 78 | { 79 | public: 80 | 81 | /** Number of parameters used for model */ 82 | static const int param_count = 4; 83 | 84 | /** Unique inverse-response type name */ 85 | static constexpr const char* type = "poly4"; 86 | 87 | public: 88 | 89 | /** 90 | * Create inverse-response model 91 | */ 92 | Poly4Response() 93 | { 94 | } 95 | 96 | virtual ~Poly4Response() 97 | { 98 | } 99 | 100 | /** 101 | * Evaluates the inverse-response for the given pixel intensity. 102 | * @param params model parameters used for evaluation 103 | * @param value pixel intensity 104 | * @return evaluated inverse-response 105 | */ 106 | template 107 | static inline T GetResponse(const T* params, T value) 108 | { 109 | // initialize attenuation 110 | 111 | T pow = value; 112 | T result = T(0); 113 | 114 | // add each term of the polynomial 115 | 116 | for (int i = 0; i < param_count; ++i) 117 | { 118 | result += pow * params[i]; 119 | pow *= value; 120 | } 121 | 122 | return result; 123 | } 124 | 125 | /** 126 | * Resets the model parameters, which results in a linear response. 127 | * @param params parameter vector to be reset 128 | * @param range min and max range of image intensities 129 | */ 130 | template 131 | static inline void ResetParameters(T* params) 132 | { 133 | Eigen::Map x(params); 134 | x = Eigen::Vector4d(1, 0, 0, 0); 135 | } 136 | }; 137 | 138 | } // namespace calibu -------------------------------------------------------------------------------- /include/calibu/pcalib/vignetting.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | namespace calibu 8 | { 9 | 10 | /** 11 | * Abstract vignetting interface, which models image attenuation. 12 | */ 13 | template 14 | class Vignetting 15 | { 16 | public: 17 | 18 | /** 19 | * Create vignetting model for given image resolution 20 | * @param width image width 21 | * @param height image height 22 | */ 23 | Vignetting(int width, int height) : 24 | width_(width), 25 | height_(height) 26 | { 27 | } 28 | 29 | virtual ~Vignetting() 30 | { 31 | } 32 | 33 | /** 34 | * Returns the image width of vignetting model 35 | * @return image width 36 | */ 37 | inline int Width() const 38 | { 39 | return width_; 40 | } 41 | 42 | /** 43 | * Returns the image height of vignetting model 44 | * @return image height 45 | */ 46 | inline int Height() const 47 | { 48 | return height_; 49 | } 50 | 51 | /** 52 | * Returns name of the vignetting model type. This will be a unique name for 53 | * each vignetting model implementation. 54 | * @return model type name 55 | */ 56 | inline const std::string& Type() const 57 | { 58 | return type_; 59 | } 60 | 61 | /** 62 | * Returns the number of parameters in the vignetting model 63 | * @return number of model parameters 64 | */ 65 | inline int NumParams() const 66 | { 67 | return params_.size(); 68 | } 69 | 70 | /** 71 | * Returns a constant reference to the model parameters 72 | * @return current model parameters 73 | */ 74 | inline const Eigen::VectorXd& GetParams() const 75 | { 76 | return params_; 77 | } 78 | 79 | /** 80 | * Assigns a new set of model parameters 81 | * @param params new model parameters to be assigned 82 | */ 83 | inline void SetParams(const Eigen::VectorXd& params) 84 | { 85 | CALIBU_ASSERT_DESC(params.size() == params_.size(), "invalid param count"); 86 | params_ = params; 87 | } 88 | 89 | /** 90 | * Evaluates the attenuation at the specified point in the image. 91 | * @param u horizontal image coordinate for point being evaluated 92 | * @param v vertical image coordinate for point being evaluated 93 | * @return evaluated attenuation factor at image position 94 | */ 95 | virtual Scalar operator()(Scalar u, Scalar v) const = 0; 96 | 97 | /** 98 | * Resets the model parameters, which results in uniform attenuation. 99 | */ 100 | virtual void Reset() = 0; 101 | 102 | protected: 103 | 104 | /** Image width of vignetting model */ 105 | int width_; 106 | 107 | /** Image height of vignetting model */ 108 | int height_; 109 | 110 | /** Vignetting type name */ 111 | std::string type_; 112 | 113 | /** Vignetting model parameter vector */ 114 | Eigen::VectorXd params_; 115 | }; 116 | 117 | } // namespace calibu -------------------------------------------------------------------------------- /include/calibu/pcalib/vignetting_dense.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace calibu 6 | { 7 | 8 | /** 9 | * Dense vignetting model, with one attenuation factor specified per pixel. 10 | * The 1D parameter vector, representing the 2D matrix of attenuation factors, 11 | * is organized in row-major order. 12 | */ 13 | template 14 | class DenseVignetting : public VignettingImpl> 15 | { 16 | public: 17 | 18 | /** Unique vignetting type name */ 19 | static constexpr const char* type = "dense"; 20 | 21 | public: 22 | 23 | /** 24 | * Create vignetting model for given image resolution 25 | * @param width image width 26 | * @param height image height 27 | */ 28 | DenseVignetting(int width, int height) : 29 | VignettingImpl>(width, height) 30 | { 31 | } 32 | 33 | virtual ~DenseVignetting() 34 | { 35 | } 36 | 37 | /** 38 | * Evaluates the attenuation at the specified point in the image. 39 | * The attuentation factor is retrieved from a look up table, using bilinear 40 | * interpolation. If the point is outside the image an attenuation factor of 41 | * the closest pixel inside the image will be used. 42 | * @param params model parameters used for evaluation 43 | * @param u horizontal image coordinate for point being evaluated 44 | * @param v vertical image coordinate for point being evaluated 45 | * @param width image width of model 46 | * @param height image height of model 47 | * @return evaluated attenuation factor at image position 48 | */ 49 | template 50 | static inline T GetAttenuation(const T* params, double u, double v, 51 | int width, int height) 52 | { 53 | // initialize attenuation 54 | 55 | T result = T(0); 56 | 57 | // force point inside image 58 | 59 | u = std::max(0.5, std::min(width - 0.5, u)); 60 | v = std::max(0.5, std::min(height - 0.5, v)); 61 | 62 | // compute values and weigths for bilinear interpolation 63 | 64 | const int x0 = u - 0.5; 65 | const int y0 = v - 0.5; 66 | const T wx1 = T(u - x0 - 0.5); 67 | const T wy1 = T(v - y0 - 0.5); 68 | const T wx0 = T(1) - wx1; 69 | const T wy0 = T(1) - wy1; 70 | 71 | // perform final bilinear interpolation of all attenuation factors 72 | 73 | result += wy0 * wx0 * params[(y0 + 0) * width + (x0 + 0)]; 74 | result += wy0 * wx1 * params[(y0 + 0) * width + (x0 + 1)]; 75 | result += wy1 * wx0 * params[(y0 + 1) * width + (x0 + 0)]; 76 | result += wy1 * wx1 * params[(y0 + 1) * width + (x0 + 1)]; 77 | 78 | return result; 79 | } 80 | 81 | /** 82 | * Resets the model parameters, which results in uniform attenuation 83 | * @param params parameter vector to be reset 84 | * @param width image width of model 85 | * @param height image height of model 86 | */ 87 | static inline void ResetParameters(double* params, int width, int height) 88 | { 89 | const int count = GetNumParams(width, height); 90 | Eigen::Map x(params, count); 91 | x.setOnes(); 92 | } 93 | 94 | /** 95 | * Returns the number of parameters needed for the specified image size 96 | * @param width image width of model 97 | * @param height image height of model 98 | * @return number of parameters of the model 99 | */ 100 | static inline int GetNumParams(int width, int height) 101 | { 102 | return width * height; 103 | } 104 | }; 105 | 106 | } // namespace calibu -------------------------------------------------------------------------------- /include/calibu/pcalib/vignetting_impl.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace calibu 6 | { 7 | 8 | /** 9 | * Vignetting implementation template, which implements the Vignetting interface 10 | * by calling static functions provided by derived classes. 11 | */ 12 | template 13 | class VignettingImpl : public Vignetting 14 | { 15 | public: 16 | 17 | /** 18 | * Create vignetting model for given image resolution 19 | * @param width image width 20 | * @param height image height 21 | */ 22 | VignettingImpl(int width, int height) : 23 | Vignetting(width, height) 24 | { 25 | Initialize(); 26 | } 27 | 28 | virtual ~VignettingImpl() 29 | { 30 | } 31 | 32 | /** 33 | * Evaluates the attenuation at the specified point in the image. 34 | * @param u horizontal image coordinate for point being evaluated 35 | * @param v vertical image coordinate for point being evaluated 36 | * @return evaluated attenuation factor at image position 37 | */ 38 | Scalar operator()(Scalar u, Scalar v) const override 39 | { 40 | return Derived::GetAttenuation(this->params_.data(), u, v, 41 | this->width_, this->height_); 42 | } 43 | 44 | /** 45 | * Resets the model parameters, which results in uniform attenuation. 46 | */ 47 | virtual void Reset() override 48 | { 49 | Derived::ResetParameters(this->params_.data(), 50 | this->width_, this->height_); 51 | } 52 | 53 | private: 54 | 55 | /** 56 | * Initializes member variables shared by all derived classes 57 | */ 58 | void Initialize() 59 | { 60 | const int w = this->width_; 61 | const int h = this->height_; 62 | this->type_ = std::string(Derived::type); 63 | this->params_.resize(Derived::GetNumParams(w, h)); 64 | Derived::ResetParameters(this->params_.data(), w, h); 65 | } 66 | }; 67 | 68 | } // namespace calibu -------------------------------------------------------------------------------- /include/calibu/pcalib/vignetting_poly.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace calibu 6 | { 7 | 8 | /** 9 | * 6th order even polynomial vignetting model as described in the work: 10 | * Alexandrov, S. V., Prankl, J., Zillich, M., & Vincze, M. (2016). 11 | * Calibration and correction of vignetting effects with an application to 3D 12 | * mapping. International Conference on Intelligent Robots and Systems 13 | */ 14 | template 15 | class EvenPoly6Vignetting : 16 | public VignettingImpl> 17 | { 18 | public: 19 | 20 | /** Unique vignetting type name */ 21 | static constexpr const char* type = "epoly6"; 22 | 23 | public: 24 | 25 | /** 26 | * Create vignetting model for given image resolution 27 | * @param width image width 28 | * @param height image height 29 | */ 30 | EvenPoly6Vignetting(int width, int height) : 31 | VignettingImpl>(width, height) 32 | { 33 | } 34 | 35 | virtual ~EvenPoly6Vignetting() 36 | { 37 | } 38 | 39 | /** 40 | * Evaluates the attenuation at the specified point in the image. 41 | * The evaluated point radius is computed relative to the image diagonal, 42 | * so a point at any corner of the image would have a radius of one. 43 | * @param params model parameters used for evaluation 44 | * @param u horizontal image coordinate for point being evaluated 45 | * @param v vertical image coordinate for point being evaluated 46 | * @param width image width of model 47 | * @param height image height of model 48 | * @return evaluated attenuation factor at image position 49 | */ 50 | template 51 | static inline T GetAttenuation(const T* params, double u, double v, 52 | int width, int height) 53 | { 54 | // initialize attenuation 55 | 56 | T result = T(1); 57 | const Eigen::Vector2d point(u, v); 58 | const Eigen::Vector2d size(width, height); 59 | const Eigen::Vector2d center = 0.5 * size; 60 | const double radius = (point - center).norm(); 61 | const double max_radius = center.norm(); 62 | const double ratio = radius / max_radius; 63 | const T rr = ratio * ratio; 64 | T pow = rr; 65 | 66 | // add each term of the polynomial 67 | 68 | for (int i = 0; i < 3; ++i) 69 | { 70 | result += pow * params[i]; 71 | pow *= rr; 72 | } 73 | 74 | return result; 75 | } 76 | 77 | /** 78 | * Resets the model parameters, which results in uniform attenuation 79 | * @param params parameter vector to be reset 80 | * @param width image width of model 81 | * @param height image height of model 82 | */ 83 | static inline void ResetParameters(double* params, int, int) 84 | { 85 | Eigen::Map x(params); 86 | x = Eigen::Vector3d(0, 0, 0); 87 | } 88 | 89 | /** 90 | * Returns the number of parameters needed for the specified image size 91 | * @param width image width of model 92 | * @param height image height of model 93 | * @return number of parameters of the model 94 | */ 95 | static inline int GetNumParams(int, int) 96 | { 97 | return 3; 98 | } 99 | }; 100 | 101 | } // namespace calibu -------------------------------------------------------------------------------- /include/calibu/pcalib/vignetting_uniform.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace calibu 6 | { 7 | 8 | /** 9 | * Uniform vignetting model, used in the trivial case where a camera has 10 | * effectively no attenuation 11 | */ 12 | template 13 | class UniformVignetting : 14 | public VignettingImpl> 15 | { 16 | public: 17 | 18 | /** Unique vignetting type name */ 19 | static constexpr const char* type = "uniform"; 20 | 21 | public: 22 | 23 | /** 24 | * Create vignetting model for given image resolution 25 | * @param width image width 26 | * @param height image height 27 | */ 28 | UniformVignetting(int width, int height) : 29 | VignettingImpl>(width, height) 30 | { 31 | } 32 | 33 | virtual ~UniformVignetting() 34 | { 35 | } 36 | 37 | /** 38 | * Evaluates the attenuation at the specified point in the image. 39 | * As this model served in the trivial case where the image is uniformly 40 | * attenuated, an attenuation factor of one is always returned, regardless 41 | * of the privided arguments. 42 | * @param params model parameters used for evaluation 43 | * @param u horizontal image coordinate for point being evaluated 44 | * @param v vertical image coordinate for point being evaluated 45 | * @param width image width of model 46 | * @param height image height of model 47 | * @return evaluated attenuation factor at image position 48 | */ 49 | template 50 | static inline T GetAttenuation(const T*, double, double, int, int) 51 | { 52 | return T(1); 53 | } 54 | 55 | /** 56 | * Resets the model parameters, which results in uniform attenuation. 57 | * For this specific model, this function does nothing 58 | * @param params parameter vector to be reset 59 | * @param width image width of model 60 | * @param height image height of model 61 | */ 62 | static inline void ResetParameters(double*, int, int) 63 | { 64 | } 65 | 66 | /** 67 | * Returns the number of parameters needed for the specified image size. 68 | * This specific model has no parameters, and will always return zero. 69 | * @param width image width of model 70 | * @param height image height of model 71 | * @return number of parameters of the model 72 | */ 73 | static inline int GetNumParams(int, int) 74 | { 75 | return 0; 76 | } 77 | }; 78 | 79 | } // namespace calibu -------------------------------------------------------------------------------- /include/calibu/pose/Pnp.h: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of the Calibu Project. 3 | https://github.com/gwu-robotics/Calibu 4 | 5 | Copyright (C) 2013 George Washington University, 6 | Hauke Strasdat, 7 | Steven Lovegrove 8 | 9 | Licensed under the Apache License, Version 2.0 (the "License"); 10 | you may not use this file except in compliance with the License. 11 | You may obtain a copy of the License at 12 | 13 | http://www.apache.org/licenses/LICENSE-2.0 14 | 15 | Unless required by applicable law or agreed to in writing, software 16 | distributed under the License is distributed on an "AS IS" BASIS, 17 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 18 | See the License for the specific language governing permissions and 19 | limitations under the License. 20 | */ 21 | 22 | #pragma once 23 | 24 | #include 25 | #include 26 | #include 27 | 28 | namespace calibu { 29 | 30 | std::vector PosePnPRansac( 31 | const std::shared_ptr> cam, 32 | const std::vector > & img_pts, 33 | const std::vector > & ideal_pts, 34 | const std::vector & candidate_map, 35 | int robust_3pt_its, 36 | float robust_3pt_tol, 37 | Sophus::SE3d * T 38 | ); 39 | 40 | double ReprojectionErrorRMS( 41 | const std::shared_ptr> cam, 42 | const Sophus::SE3d& T_cw, 43 | const std::vector >& pts3d, 44 | const std::vector >& pts2d, 45 | const std::vector& map2d_3d 46 | ); 47 | 48 | int CountInliers(const std::vector & conics_target_map); 49 | 50 | } 51 | -------------------------------------------------------------------------------- /include/calibu/pose/Ransac.h: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of the Calibu Project. 3 | https://github.com/gwu-robotics/Calibu 4 | 5 | Copyright (C) 2013 George Washington University, 6 | Steven Lovegrove 7 | 8 | Licensed under the Apache License, Version 2.0 (the "License"); 9 | you may not use this file except in compliance with the License. 10 | You may obtain a copy of the License at 11 | 12 | http://www.apache.org/licenses/LICENSE-2.0 13 | 14 | Unless required by applicable law or agreed to in writing, software 15 | distributed under the License is distributed on an "AS IS" BASIS, 16 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 17 | See the License for the specific language governing permissions and 18 | limitations under the License. 19 | */ 20 | 21 | #pragma once 22 | 23 | #include 24 | 25 | #include 26 | #include 27 | #include 28 | 29 | namespace calibu { 30 | 31 | template 32 | class Ransac 33 | { 34 | public: 35 | typedef double(*CostFunction)(const Model& model, int element_index, Data); 36 | typedef Model(*ModelFunction)(const std::vector& element_indices, Data); 37 | 38 | Ransac(ModelFunction mf, CostFunction cf, Data data) 39 | : mf(mf), cf(cf), data(data) 40 | { 41 | } 42 | 43 | Model Compute(unsigned int num_elements, std::vector& inliers, int iterations, double max_datum_fit_error, unsigned int min_consensus_size) 44 | { 45 | // http://en.wikipedia.org/wiki/RANSAC 46 | if (num_elements < minimum_set_size) 47 | return Model(); 48 | 49 | Model best_model; 50 | std::vector best_consensus_set; 51 | double best_error = std::numeric_limits::max(); 52 | 53 | for (int k = 0; k < iterations; ++k) 54 | { 55 | std::vector maybe_inliers; 56 | SelectCandidates(maybe_inliers, num_elements); 57 | Model maybe_model = (*mf)(maybe_inliers, data); 58 | std::vector consensus_set(maybe_inliers); 59 | 60 | for (unsigned int i = 0; i= min_consensus_size) 71 | { 72 | Model better_model = (*mf)(consensus_set, data); 73 | double this_error = 0; 74 | for (unsigned int i = 0; i< consensus_set.size(); ++i) 75 | { 76 | double err = (*cf)(better_model, consensus_set[i], data); 77 | this_error += err*err; 78 | } 79 | this_error /= consensus_set.size(); 80 | if (this_error < best_error) 81 | { 82 | best_model = better_model; 83 | best_consensus_set = consensus_set; 84 | best_error = this_error; 85 | } 86 | } 87 | } 88 | 89 | inliers = best_consensus_set; 90 | return best_model; 91 | } 92 | 93 | protected: 94 | void SelectCandidates(std::vector& set, int num_elements) 95 | { 96 | while (set.size() < minimum_set_size) 97 | { 98 | const int i = rand() % num_elements; 99 | if (find(set.begin(), set.end(), i) == set.end()) 100 | set.push_back(i); 101 | } 102 | } 103 | 104 | ModelFunction mf; 105 | CostFunction cf; 106 | Data data; 107 | }; 108 | 109 | } 110 | -------------------------------------------------------------------------------- /include/calibu/pose/Tracker.h: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of the Calibu Project. 3 | https://github.com/gwu-robotics/Calibu 4 | 5 | Copyright (C) 2013 George Washington University, 6 | Steven Lovegrove, 7 | Hauke Strasdat 8 | 9 | Licensed under the Apache License, Version 2.0 (the "License"); 10 | you may not use this file except in compliance with the License. 11 | You may obtain a copy of the License at 12 | 13 | http://www.apache.org/licenses/LICENSE-2.0 14 | 15 | Unless required by applicable law or agreed to in writing, software 16 | distributed under the License is distributed on an "AS IS" BASIS, 17 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 18 | See the License for the specific language governing permissions and 19 | limitations under the License. 20 | */ 21 | 22 | #pragma once 23 | 24 | #include 25 | #include 26 | #include 27 | 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | 34 | namespace calibu { 35 | 36 | struct ParamsTracker 37 | { 38 | ParamsTracker () : 39 | robust_3pt_inlier_tol(1.5), 40 | robust_3pt_its(100), 41 | inlier_num_required(10), 42 | max_rms(3.0) {} 43 | 44 | double robust_3pt_inlier_tol; 45 | int robust_3pt_its; 46 | int inlier_num_required; 47 | double max_rms; 48 | }; 49 | 50 | class Tracker 51 | { 52 | public: 53 | Tracker(TargetInterface& target, int w, int h); 54 | 55 | bool ProcessFrame( std::shared_ptr> cam, 56 | const unsigned char *I, size_t w, size_t h, size_t pitch ); 57 | 58 | const TargetInterface& Target() const { 59 | return target; 60 | } 61 | 62 | const ConicFinder& GetConicFinder() const { 63 | return conic_finder; 64 | } 65 | 66 | const ImageProcessing& Images() const { 67 | return imgs; 68 | } 69 | 70 | const std::vector& ConicsTargetMap() const{ 71 | return conics_target_map; 72 | } 73 | 74 | const Sophus::SE3d& PoseT_gw() const 75 | { 76 | return T_gw; 77 | } 78 | 79 | protected: 80 | // Target 81 | TargetInterface& target; 82 | ImageProcessing imgs; 83 | ConicFinder conic_finder; 84 | 85 | // Hypothesis conics 86 | std::vector conics_target_map; 87 | std::vector conics_candidate_map_first_pass; 88 | std::vector conics_candidate_map_second_pass; 89 | 90 | // Last good pose 91 | Sophus::SE3d T_gw; 92 | std::clock_t last_good; 93 | int good_frames; 94 | 95 | // Pose hypothesis 96 | Sophus::SE3d T_hw; 97 | 98 | ParamsTracker params; 99 | }; 100 | 101 | } 102 | -------------------------------------------------------------------------------- /include/calibu/target/Hungarian.h: -------------------------------------------------------------------------------- 1 | /******************************************************************** 2 | ******************************************************************** 3 | ** 4 | ** libhungarian by Cyrill Stachniss, 2004 5 | ** 6 | ** 7 | ** Solving the Minimum Assignment Problem using the 8 | ** Hungarian Method. 9 | ** 10 | ** ** This file may be freely copied and distributed! ** 11 | ** 12 | ** Parts of the used code was originally provided by the 13 | ** "Stanford GraphGase", but I made changes to this code. 14 | ** As asked by the copyright node of the "Stanford GraphGase", 15 | ** I hereby proclaim that this file are *NOT* part of the 16 | ** "Stanford GraphGase" distrubition! 17 | ** 18 | ** This file is distributed in the hope that it will be useful, 19 | ** but WITHOUT ANY WARRANTY; without even the implied 20 | ** warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR 21 | ** PURPOSE. 22 | ** 23 | ******************************************************************** 24 | ********************************************************************/ 25 | 26 | #ifndef HUNGARIAN_H 27 | #define HUNGARIAN_H 28 | 29 | #define HUNGARIAN_NOT_ASSIGNED 0 30 | #define HUNGARIAN_ASSIGNED 1 31 | 32 | #define HUNGARIAN_MODE_MINIMIZE_COST 0 33 | #define HUNGARIAN_MODE_MAXIMIZE_UTIL 1 34 | 35 | typedef struct { 36 | int num_rows; 37 | int num_cols; 38 | double** cost; 39 | int* row_to_col_map; 40 | } hungarian_problem_t; 41 | 42 | /** This method initialize the hungarian_problem structure and init 43 | * the cost matrices (missing lines or columns are filled with 0). 44 | * It returns the size of the quadratic(!) assignment matrix. **/ 45 | int hungarian_init(hungarian_problem_t* p, 46 | double** cost_matrix, 47 | int rows, 48 | int cols, 49 | int mode = HUNGARIAN_MODE_MINIMIZE_COST); 50 | 51 | /** Free the memory allocated by init. **/ 52 | void hungarian_free(hungarian_problem_t* p); 53 | 54 | /** This method computes the optimal assignment. **/ 55 | void hungarian_solve(hungarian_problem_t* p); 56 | 57 | /** Print the cost matrix. **/ 58 | void hungarian_print_costmatrix(hungarian_problem_t* p); 59 | 60 | #endif 61 | 62 | 63 | 64 | -------------------------------------------------------------------------------- /include/calibu/target/LineGroup.h: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of the Calibu Project. 3 | https://github.com/gwu-robotics/Calibu 4 | 5 | Copyright (C) 2013 George Washington University, 6 | Steven Lovegrove 7 | 8 | Licensed under the Apache License, Version 2.0 (the "License"); 9 | you may not use this file except in compliance with the License. 10 | You may obtain a copy of the License at 11 | 12 | http://www.apache.org/licenses/LICENSE-2.0 13 | 14 | Unless required by applicable law or agreed to in writing, software 15 | distributed under the License is distributed on an "AS IS" BASIS, 16 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 17 | See the License for the specific language governing permissions and 18 | limitations under the License. 19 | */ 20 | 21 | #pragma once 22 | 23 | #include 24 | 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | 31 | #include 32 | #include 33 | 34 | namespace calibu { 35 | 36 | const static int GRID_INVALID = std::numeric_limits::min(); 37 | 38 | struct Triple; 39 | 40 | struct Vertex 41 | { 42 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 43 | inline Vertex(size_t id, const Conic& c) 44 | : id(id), conic(c), pc(c.center), pg(GRID_INVALID,GRID_INVALID), area(0.0), value(-1) 45 | { 46 | } 47 | 48 | inline bool HasGridPosition() 49 | { 50 | return pg(0) != GRID_INVALID; 51 | } 52 | 53 | size_t id; 54 | Conic conic; 55 | Eigen::Vector2d pc; 56 | Eigen::Vector2i pg; 57 | std::vector triples; 58 | std::set neighbours; 59 | double area; 60 | int value; 61 | }; 62 | 63 | struct Triple 64 | { 65 | inline Triple(Vertex& o1, Vertex& c, Vertex& o2) 66 | { 67 | vs = {&o1, &c, &o2}; 68 | } 69 | 70 | Triple(const Triple& triple) = default; 71 | 72 | inline Vertex& Center() { return *vs[1]; } 73 | inline const Vertex& Center() const { return *vs[1]; } 74 | 75 | inline Vertex& Neighbour(size_t i) { return i ? *vs[2] : *vs[0]; } 76 | inline const Vertex& Neighbour(size_t i) const { return i ? *vs[2] : *vs[0]; } 77 | 78 | inline Vertex& OtherNeighbour(size_t i) { return i ? *vs[0] : *vs[2]; } 79 | inline const Vertex& OtherNeighbour(size_t i) const { return i ? *vs[0] : *vs[2]; } 80 | 81 | inline Vertex& Vert(size_t i) { return *vs[i]; } 82 | inline const Vertex& Vert(size_t i) const { return *vs[i]; } 83 | 84 | inline Eigen::Vector2d Dir() const 85 | { 86 | return vs[2]->pc - vs[0]->pc; 87 | } 88 | 89 | inline bool Contains(const Vertex& v) const 90 | { 91 | const bool found = std::find(vs.begin(), vs.end(), &v) != vs.end(); 92 | return found; 93 | } 94 | 95 | inline bool In(const std::set bag) const 96 | { 97 | return bag.find(vs[0]) != bag.end() && bag.find(vs[2]) != bag.end(); 98 | } 99 | 100 | inline void Reverse() 101 | { 102 | std::swap(vs[0], vs[2]); 103 | } 104 | 105 | // Colinear sequence of vertices, v[0], v[1], v[2]. v[1] is center 106 | std::vector vs; 107 | }; 108 | 109 | inline bool operator==(const Vertex& lhs, const Vertex& rhs) 110 | { 111 | return lhs.id == rhs.id; 112 | } 113 | 114 | inline bool AreCollinear(const Triple& t1, const Triple& t2) 115 | { 116 | return t1.Contains(t2.Center()) && t2.Contains(t1.Center()); 117 | } 118 | 119 | inline double Distance(const Vertex& v1, const Vertex& v2) 120 | { 121 | return (v2.pc - v1.pc).norm(); 122 | } 123 | 124 | inline std::ostream& operator<<(std::ostream& os, const Vertex& v) 125 | { 126 | os << "(" << v.pg.transpose() << ")"; 127 | return os; 128 | } 129 | 130 | inline std::ostream& operator<<(std::ostream& os, const Triple& t) 131 | { 132 | os << t.Vert(0) << " - " << t.Vert(1) << " - " << t.Vert(2); 133 | return os; 134 | } 135 | 136 | struct LineGroup 137 | { 138 | LineGroup(const Triple& o) 139 | : ops({o.vs[0]->id, o.vs[1]->id, o.vs[2]->id}) 140 | { 141 | } 142 | 143 | bool Merge(LineGroup& o) 144 | { 145 | if(last() == o.second() && pen() == o.first() ) { 146 | ops.insert(ops.end(), std::next(o.ops.begin(),2), o.ops.end() ); 147 | o.ops.clear(); 148 | return true; 149 | }else if(o.last() == second() && o.pen() == first() ) { 150 | ops.insert(ops.begin(), o.ops.begin(), std::prev(o.ops.end(),2) ); 151 | o.ops.clear(); 152 | return true; 153 | }else if( last() == o.pen() && pen() == o.last() ) { 154 | ops.insert(ops.end(), std::next(o.ops.rbegin(),2), o.ops.rend() ); 155 | o.ops.clear(); 156 | return true; 157 | }else if( first() == o.second() && second() == o.first() ) { 158 | ops.insert(ops.begin(), o.ops.rbegin(), std::prev(o.ops.rend(),2) ); 159 | o.ops.clear(); 160 | return true; 161 | } 162 | return false; 163 | } 164 | 165 | void Reverse() 166 | { 167 | std::list rev_ops; 168 | rev_ops.insert(rev_ops.begin(), ops.rbegin(), ops.rend()); 169 | ops = rev_ops; 170 | } 171 | 172 | size_t first() { return *ops.begin(); } 173 | size_t last() { return *ops.rbegin(); } 174 | size_t second() { return *std::next(ops.begin()); } 175 | size_t pen() { return *std::next(ops.rbegin()); } 176 | 177 | std::list ops; 178 | double theta; 179 | int k; 180 | }; 181 | 182 | } 183 | -------------------------------------------------------------------------------- /include/calibu/target/RandomGrid.h: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of the Calibu Project. 3 | https://github.com/gwu-robotics/Calibu 4 | 5 | Copyright (C) 2013 George Washington University, 6 | Steven Lovegrove 7 | 8 | Licensed under the Apache License, Version 2.0 (the "License"); 9 | you may not use this file except in compliance with the License. 10 | You may obtain a copy of the License at 11 | 12 | http://www.apache.org/licenses/LICENSE-2.0 13 | 14 | Unless required by applicable law or agreed to in writing, software 15 | distributed under the License is distributed on an "AS IS" BASIS, 16 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 17 | See the License for the specific language governing permissions and 18 | limitations under the License. 19 | */ 20 | 21 | #pragma once 22 | 23 | #include 24 | 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | 34 | namespace calibu 35 | { 36 | 37 | CALIBU_EXPORT 38 | void SaveEPS( 39 | std::string filename, const Eigen::MatrixXi& M, 40 | const Eigen::Vector2d& offset, double grid_spacing, 41 | double rad0, double rad1, double pts_per_unit, 42 | unsigned char id 43 | ); 44 | 45 | CALIBU_EXPORT 46 | Eigen::MatrixXi MakePattern(int r, int c, uint32_t seed = 0); 47 | 48 | CALIBU_EXPORT 49 | std::array MakePatternGroup(int r, int c, uint32_t seed); 50 | 51 | CALIBU_EXPORT 52 | std::array FillGroup(const Eigen::MatrixXi& m); 53 | 54 | CALIBU_EXPORT 55 | int HammingDistance(const Eigen::MatrixXi& M, const Eigen::MatrixXi& m, int r, int c); 56 | 57 | CALIBU_EXPORT 58 | int NumExactMatches(const Eigen::MatrixXi& M, const Eigen::MatrixXi& m, int& best_score, int& best_r, int& best_c); 59 | 60 | CALIBU_EXPORT 61 | int NumExactMatches(const std::array& PG, const Eigen::MatrixXi& m, int& best_score, int& best_g, int& best_r, int& best_c); 62 | 63 | CALIBU_EXPORT 64 | int AutoCorrelation(const std::array& PG, int minr = 2, int minc = 2); 65 | 66 | CALIBU_EXPORT 67 | int AutoCorrelationMinArea(const std::array& PG); 68 | 69 | CALIBU_EXPORT 70 | int SeedScore(uint32_t seed, int r, int c); 71 | 72 | CALIBU_EXPORT 73 | uint32_t FindBestSeed(int r, int c, bool& should_run); 74 | 75 | CALIBU_EXPORT 76 | void PrintPattern(const Eigen::MatrixXi& M); 77 | 78 | } 79 | -------------------------------------------------------------------------------- /include/calibu/target/Target.h: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of the Calibu Project. 3 | https://github.com/gwu-robotics/Calibu 4 | 5 | Copyright (C) 2013 George Washington University, 6 | Steven Lovegrove 7 | 8 | Licensed under the Apache License, Version 2.0 (the "License"); 9 | you may not use this file except in compliance with the License. 10 | You may obtain a copy of the License at 11 | 12 | http://www.apache.org/licenses/LICENSE-2.0 13 | 14 | Unless required by applicable law or agreed to in writing, software 15 | distributed under the License is distributed on an "AS IS" BASIS, 16 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 17 | See the License for the specific language governing permissions and 18 | limitations under the License. 19 | */ 20 | 21 | #pragma once 22 | 23 | #include 24 | #include 25 | #include 26 | #include 27 | 28 | #include 29 | #include 30 | #include 31 | #include 32 | 33 | namespace calibu 34 | { 35 | 36 | CALIBU_EXPORT 37 | class TargetInterface 38 | { 39 | public: 40 | 41 | // Find target given conic observations 42 | // Returns true on success, false on failure 43 | 44 | // Assume approximately known camera and pose 45 | virtual bool FindTarget( 46 | const Sophus::SE3d& T_cw, 47 | const std::shared_ptr> cam, 48 | const ImageProcessing& images, 49 | const std::vector >& conics, 50 | std::vector& conics_target_map 51 | ) = 0; 52 | 53 | // Assume approximately known camera 54 | virtual bool FindTarget( 55 | const std::shared_ptr> cam, 56 | const ImageProcessing& images, 57 | const std::vector >& conics, 58 | std::vector& conics_target_map 59 | ) = 0; 60 | 61 | // Only observations known 62 | virtual bool FindTarget( 63 | const ImageProcessing& images, 64 | const std::vector >& conics, 65 | std::vector& conics_target_map 66 | ) = 0; 67 | 68 | // Return circle radius 69 | virtual double CircleRadius() const = 0; 70 | virtual double CircleRadius(uint32_t circle_index) const = 0; 71 | 72 | // Return canonical set of known 2D/3D points. 73 | virtual const std::vector >& Circles2D() const = 0; 75 | virtual const std::vector >& Circles3D() const = 0; 77 | virtual const std::vector >& Code3D() const = 0; 79 | }; 80 | 81 | } 82 | -------------------------------------------------------------------------------- /include/calibu/utils/PlaneBasis.h: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of the Calibu Project. 3 | https://github.com/gwu-robotics/Calibu 4 | 5 | Copyright (C) 2013 George Washington University, 6 | Steven Lovegrove 7 | 8 | Licensed under the Apache License, Version 2.0 (the "License"); 9 | you may not use this file except in compliance with the License. 10 | You may obtain a copy of the License at 11 | 12 | http://www.apache.org/licenses/LICENSE-2.0 13 | 14 | Unless required by applicable law or agreed to in writing, software 15 | distributed under the License is distributed on an "AS IS" BASIS, 16 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 17 | See the License for the specific language governing permissions and 18 | limitations under the License. 19 | */ 20 | 21 | #pragma once 22 | 23 | #include 24 | #include 25 | 26 | namespace calibu 27 | { 28 | 29 | /// Adapted from TooN so3.h: 30 | /// creates an SO3 as a rotation that takes Vector a into the direction of Vector b 31 | /// with the rotation axis along a ^ b. If |a ^ b| == 0, it creates the identity rotation. 32 | /// An assertion will fail if Vector a and Vector b are in exactly opposite directions. 33 | /// @param a source Vector 34 | /// @param b target Vector 35 | inline Sophus::SO3d Rotation(const Eigen::Vector3d& a, const Eigen::Vector3d& b) 36 | { 37 | Eigen::Vector3d n = a.cross(b); 38 | 39 | if(n.squaredNorm() == 0) { 40 | //check that the vectors are in the same direction if cross product is 0. If not, 41 | //this means that the rotation is 180 degrees, which leads to an ambiguity in the rotation axis. 42 | // assert(a.dot(b)>=0); 43 | return Sophus::SO3d(); 44 | } 45 | 46 | n.normalize(); 47 | Eigen::Matrix3d R1; 48 | R1.col(0) = a.normalized(); 49 | R1.col(1) = n; 50 | R1.col(2) = n.cross(R1.col(0)); 51 | 52 | Eigen::Matrix3d M; 53 | M.col(0) = b.normalized(); 54 | M.col(1) = n; 55 | M.col(2) = n.cross(M.col(0)); 56 | M = M * R1.transpose(); 57 | 58 | return Sophus::SO3d(M); 59 | } 60 | 61 | inline Sophus::SE3d PlaneBasis_wp(const Eigen::Vector3d& nd_w) 62 | { 63 | const double d = 1.0 / nd_w.norm(); 64 | const Eigen::Vector3d n = d * nd_w; 65 | const Sophus::SO3d R_wn = Rotation(Eigen::Vector3d(0,0,-1),n); 66 | return Sophus::SE3d(R_wn, -d*n); 67 | } 68 | 69 | } 70 | -------------------------------------------------------------------------------- /include/calibu/utils/Range.h: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of the Calibu Project. 3 | https://github.com/gwu-robotics/Calibu 4 | 5 | Copyright (C) 2013 George Washington University, 6 | Steven Lovegrove 7 | 8 | Licensed under the Apache License, Version 2.0 (the "License"); 9 | you may not use this file except in compliance with the License. 10 | You may obtain a copy of the License at 11 | 12 | http://www.apache.org/licenses/LICENSE-2.0 13 | 14 | Unless required by applicable law or agreed to in writing, software 15 | distributed under the License is distributed on an "AS IS" BASIS, 16 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 17 | See the License for the specific language governing permissions and 18 | limitations under the License. 19 | */ 20 | 21 | #pragma once 22 | 23 | #include 24 | 25 | namespace calibu 26 | { 27 | 28 | /// Represet min/max range. 29 | struct Range 30 | { 31 | static Range Open() 32 | { 33 | return Range(-std::numeric_limits::max(), 34 | std::numeric_limits::max()); 35 | } 36 | 37 | static Range Closed() 38 | { 39 | return Range(std::numeric_limits::max(), 40 | -std::numeric_limits::max()); 41 | } 42 | 43 | inline Range() 44 | : minr(std::numeric_limits::max()), 45 | maxr(-std::numeric_limits::max()) 46 | { 47 | // Empty range 48 | } 49 | 50 | inline Range(double minr, double maxr) 51 | : minr(minr), maxr(maxr) 52 | { 53 | } 54 | 55 | inline bool Empty() const 56 | { 57 | return maxr <= minr; 58 | } 59 | 60 | // Expand range to include v 61 | inline void Insert(double v) 62 | { 63 | minr = std::min(minr,v); 64 | maxr = std::max(maxr,v); 65 | } 66 | 67 | // Enforce that range is greater than v 68 | inline void ExcludeLessThan(double v) 69 | { 70 | minr = std::max(minr,v); 71 | } 72 | 73 | // Enforce that range is less than v 74 | inline void ExcludeGreaterThan(double v) 75 | { 76 | maxr = std::min(maxr,v); 77 | } 78 | 79 | inline double Size() const 80 | { 81 | return maxr - minr; 82 | } 83 | 84 | double minr; 85 | double maxr; 86 | }; 87 | 88 | inline Range Union(const Range& lhs, const Range& rhs) 89 | { 90 | return Range( std::min(lhs.minr, rhs.minr), std::max(lhs.maxr, rhs.maxr) ); 91 | } 92 | 93 | inline Range Intersection(const Range& lhs, const Range& rhs) 94 | { 95 | return Range( std::max(lhs.minr, rhs.minr), std::min(lhs.maxr, rhs.maxr) ); 96 | } 97 | 98 | inline std::ostream& operator<<(std::ostream& os, const Range& r) 99 | { 100 | os << "[" << r.minr << ", " << r.maxr << "]"; 101 | return os; 102 | } 103 | 104 | } 105 | -------------------------------------------------------------------------------- /include/calibu/utils/Rectangle.h: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of the Calibu Project. 3 | https://github.com/gwu-robotics/Calibu 4 | 5 | Copyright (C) 2013 George Washington University, 6 | Hauke Strasdat, 7 | Steven Lovegrove 8 | 9 | Licensed under the Apache License, Version 2.0 (the "License"); 10 | you may not use this file except in compliance with the License. 11 | You may obtain a copy of the License at 12 | 13 | http://www.apache.org/licenses/LICENSE-2.0 14 | 15 | Unless required by applicable law or agreed to in writing, software 16 | distributed under the License is distributed on an "AS IS" BASIS, 17 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 18 | See the License for the specific language governing permissions and 19 | limitations under the License. 20 | */ 21 | 22 | #pragma once 23 | 24 | #include 25 | 26 | #include 27 | 28 | namespace calibu { 29 | 30 | class Rectangle 31 | { 32 | public: 33 | Rectangle(){} 34 | 35 | Rectangle(const Rectangle & r) 36 | { 37 | this->x1 = r.x1; 38 | this->x2 = r.x2; 39 | this->y2 = r.y2; 40 | this->y1 = r.y1; 41 | } 42 | 43 | Rectangle(double x1, double y1, double x2, double y2) 44 | { 45 | this->x1 = x1; 46 | this->x2 = x2; 47 | this->y2 = y2; 48 | this->y1 = y1; 49 | } 50 | 51 | Rectangle(const Eigen::Vector2d & tl, const Eigen::Vector2d & br) 52 | { 53 | this->x1 = tl[0]; 54 | this->x2 = br[0]; 55 | this->y2 = br[1]; 56 | this->y1 = tl[1]; 57 | } 58 | 59 | double x1, y1, x2, y2; 60 | 61 | double Width() const 62 | { 63 | return std::max(0.,x2-x1); 64 | } 65 | 66 | double Height() const 67 | { 68 | return std::max(0.,y2-y1); 69 | } 70 | 71 | bool intersectsWith(const Rectangle & other) const 72 | { 73 | if(y2 <= other.y1) return false; 74 | if(y1 >= other.y2) return false; 75 | if(x2 <= other.x1) return false; 76 | if(x1 >= other.x2) return false; 77 | return true; 78 | } 79 | 80 | bool contains(const Rectangle & other) const 81 | { 82 | if( y1 > other.y1) return false; 83 | if( x1 > other.x1) return false; 84 | if( x2 < other.x2) return false; 85 | if(y2 < other.y2) return false; 86 | return true; 87 | } 88 | 89 | bool contains(const Eigen::Vector2d & v) const 90 | { 91 | if( y1 > v[1]) return false; 92 | if( x1 > v[0]) return false; 93 | if( x2 < v[0]) return false; 94 | if(y2 < v[1]) return false; 95 | return true; 96 | } 97 | }; 98 | 99 | 100 | class IRectangle{ 101 | public: 102 | IRectangle(){} 103 | 104 | IRectangle(const IRectangle & r) 105 | { 106 | this->x1 = r.x1; 107 | this->x2 = r.x2; 108 | this->y2 = r.y2; 109 | this->y1 = r.y1; 110 | } 111 | 112 | IRectangle(int x1, int y1, int x2, int y2) 113 | { 114 | this->x1 = x1; 115 | this->x2 = x2; 116 | this->y2 = y2; 117 | this->y1 = y1; 118 | } 119 | 120 | int x1, y1, x2, y2; 121 | 122 | int Width() const 123 | { 124 | return std::max(0,x2+1-x1); 125 | } 126 | 127 | int Height() const 128 | { 129 | return std::max(0,y2+1-y1); 130 | } 131 | 132 | int Area() const 133 | { 134 | return Width()*Height(); 135 | } 136 | 137 | bool IntersectsWith(const IRectangle & other) const 138 | { 139 | if(y2 < other.y1) return false; 140 | if(y1 > other.y2) return false; 141 | if(x2 < other.x1) return false; 142 | if(x1 > other.x2) return false; 143 | return true; 144 | } 145 | 146 | bool Contains(const IRectangle & other) const 147 | { 148 | if(y1 >= other.y1) return false; 149 | if(x1 >= other.x1) return false; 150 | if(x2 <= other.x2) return false; 151 | if(y2 <= other.y2) return false; 152 | return true; 153 | } 154 | 155 | void Insert( int x, int y) 156 | { 157 | x1 = std::min(x1,x); 158 | x2 = std::max(x2,x); 159 | y1 = std::min(y1,y); 160 | y2 = std::max(y2,y); 161 | } 162 | 163 | void Insert( const IRectangle& d) 164 | { 165 | x1 = std::min(x1,d.x1); 166 | x2 = std::max(x2,d.x2); 167 | y1 = std::min(y1,d.y1); 168 | y2 = std::max(y2,d.y2); 169 | } 170 | 171 | IRectangle Grow( int r ) const 172 | { 173 | IRectangle ret(*this); 174 | ret.x1 -= r; 175 | ret.x2 += r; 176 | ret.y1 -= r; 177 | ret.y2 += r; 178 | return ret; 179 | } 180 | 181 | IRectangle Clamp( int minx, int miny, int maxx, int maxy ) const 182 | { 183 | IRectangle ret(*this); 184 | ret.x1 = std::max(minx,ret.x1); 185 | ret.y1 = std::max(miny,ret.y1); 186 | ret.x2 = std::min(maxx,ret.x2); 187 | ret.y2 = std::min(maxy,ret.y2); 188 | return ret; 189 | } 190 | 191 | bool Contains( int x, int y ) const 192 | { 193 | return x1 <= x && x <= x2 && y1 <= y && y <= y2; 194 | } 195 | 196 | bool Contains( const Eigen::Vector2d& p ) const 197 | { 198 | return Contains(p[0],p[1]); 199 | } 200 | 201 | Eigen::Vector2d Center() const 202 | { 203 | return Eigen::Vector2d((x2+x1)/2.0, (y2+y1)/2.0); 204 | } 205 | 206 | }; 207 | 208 | } 209 | -------------------------------------------------------------------------------- /include/calibu/utils/StreamOperatorsEigen.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include 6 | 7 | //////////////////////////////////////////////////////////////////////////// 8 | // Overloading stream operators for Eigen 9 | namespace calibu 10 | { 11 | 12 | template 13 | inline std::ostream& operator<<( std::ostream& Stream, const Eigen::Matrix& Mat ) 14 | { 15 | unsigned int nRows = Mat.rows(); 16 | unsigned int nCols = Mat.cols(); 17 | 18 | Stream << "[ "; 19 | 20 | for( unsigned int ii = 0; ii < nRows-1; ii++ ) { 21 | for( unsigned int jj = 0; jj < nCols-1; jj++ ) { 22 | Stream << Mat(ii, jj); 23 | Stream << ", "; 24 | } 25 | Stream << Mat(ii, nCols-1); 26 | Stream << "; "; 27 | } 28 | for( unsigned int jj = 0; jj < nCols-1; jj++ ) { 29 | Stream << Mat(nRows-1, jj); 30 | Stream << ", "; 31 | } 32 | Stream << Mat(nRows-1, nCols-1); 33 | Stream << " ]"; 34 | 35 | return Stream; 36 | } 37 | 38 | //////////////////////////////////////////////////////////////////////////// 39 | template 40 | inline std::istream& operator>>( std::istream& Stream, Eigen::Matrix& Mat ) 41 | { 42 | 43 | unsigned int nRows = Mat.rows(); 44 | unsigned int nCols = Mat.cols(); 45 | char str[256]; 46 | 47 | Stream.getline(str, 255, '['); 48 | if( Stream.gcount() == 0 ) { 49 | return Stream; 50 | } 51 | for( unsigned int ii = 0; ii < nRows-1; ii++ ) { 52 | for( unsigned int jj = 0; jj < nCols-1; jj++ ) { 53 | Stream.getline(str, 255, ','); 54 | Mat(ii, jj) = std::strtod(str, NULL); 55 | } 56 | Stream.getline(str, 255, ';'); 57 | Mat(ii, nCols-1) = std::strtod(str, NULL); 58 | } 59 | for( unsigned int jj = 0; jj < nCols-1; jj++ ) { 60 | Stream.getline(str, 255, ','); 61 | Mat(nRows-1, jj) = std::strtod(str, NULL); 62 | } 63 | Stream.getline(str, 255, ']'); 64 | Mat(nRows-1, nCols-1) = std::strtod(str, NULL); 65 | return Stream; 66 | } 67 | 68 | } 69 | 70 | namespace std { 71 | using calibu::operator<<; 72 | using calibu::operator>>; 73 | } 74 | -------------------------------------------------------------------------------- /include/calibu/utils/StreamOperatorsSophus.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | //////////////////////////////////////////////////////////////////////////// 8 | // Overloading stream operators for Sophus 9 | namespace calibu 10 | { 11 | 12 | //////////////////////////////////////////////////////////////////////////// 13 | inline std::ostream& operator<<( std::ostream& Stream, const Sophus::SO3d& R ) 14 | { 15 | Stream << R.unit_quaternion().coeffs(); 16 | return Stream; 17 | } 18 | 19 | //////////////////////////////////////////////////////////////////////////// 20 | inline std::ostream& operator<<( std::ostream& Stream, const Sophus::SE3d& T ) 21 | { 22 | Stream << "[" << T.so3() << "," << T.translation() << "]"; 23 | return Stream; 24 | } 25 | 26 | //////////////////////////////////////////////////////////////////////////// 27 | inline std::istream& operator>>( std::istream& Stream, Sophus::SO3d& R ) 28 | { 29 | Eigen::Matrix coeffs; 30 | Stream >> coeffs; 31 | R.setQuaternion(Eigen::Quaterniond(coeffs)); 32 | return Stream; 33 | } 34 | 35 | //////////////////////////////////////////////////////////////////////////// 36 | inline std::istream& operator>>( std::istream& Stream, Sophus::SE3d& T ) 37 | { 38 | char str[256]; 39 | 40 | Stream.getline(str, 255, '['); 41 | if( Stream.gcount() > 1 ) { 42 | return Stream; 43 | } 44 | Stream >> T.so3(); 45 | Stream.getline(str, 255, ','); 46 | Stream >> T.translation(); 47 | Stream.getline(str, 255, ']'); 48 | return Stream; 49 | } 50 | 51 | } 52 | -------------------------------------------------------------------------------- /include/calibu/utils/Utils.h: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of the Calibu Project. 3 | https://github.com/gwu-robotics/Calibu 4 | 5 | Copyright (C) 2013 George Washington University, 6 | Steven Lovegrove 7 | 8 | Licensed under the Apache License, Version 2.0 (the "License"); 9 | you may not use this file except in compliance with the License. 10 | You may obtain a copy of the License at 11 | 12 | http://www.apache.org/licenses/LICENSE-2.0 13 | 14 | Unless required by applicable law or agreed to in writing, software 15 | distributed under the License is distributed on an "AS IS" BASIS, 16 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 17 | See the License for the specific language governing permissions and 18 | limitations under the License. 19 | */ 20 | 21 | #pragma once 22 | 23 | #include 24 | 25 | #include 26 | #include 27 | #include 28 | 29 | namespace calibu { 30 | 31 | template 32 | bool is_nan(const Eigen::MatrixBase& x) 33 | { 34 | return !(x.array() == x.array()).all(); 35 | } 36 | 37 | template 38 | bool is_finite(const Eigen::MatrixBase& x) 39 | { 40 | return !is_nan( (x.array() - x.array()).matrix() ); 41 | } 42 | 43 | CALIBU_EXPORT 44 | Eigen::Matrix3d EstimateH_ba( 45 | const std::vector >& a, 46 | const std::vector >& b 47 | ); 48 | 49 | inline Eigen::Matrix3d SkewSym( const Eigen::Vector3d& A) 50 | { 51 | Eigen::Matrix3d R; 52 | R << 53 | 0, -A[2], A[1], 54 | A[2], 0, -A[0], 55 | -A[1], A[0], 0; 56 | return R; 57 | } 58 | 59 | inline Eigen::Matrix4d SymmetryTransform( const Eigen::Vector4d& N ) 60 | { 61 | // Compute Symmetry transformation S in ss(3) induced by plane N 62 | // "The top-left 3 × 3 sub-matrix of any element in ss(3) is always a House-holder matrix" 63 | // therefore S in ss(3) is involutionary: S^{-1} = S 64 | // T = S1.S2 where S1,S2 in ss(3) and T in SE(3) 65 | 66 | Eigen::Matrix4d S = Eigen::Matrix4d::Identity(); 67 | const Eigen::Vector3d n = N.head<3>(); 68 | const double d = -N[3]; 69 | S.block<3,3>(0,0) = (Eigen::Matrix3d::Identity()) - 2 * n * n.transpose(); 70 | S.block<3,1>(0,3) = 2 * d * n; 71 | return S; 72 | } 73 | 74 | } 75 | -------------------------------------------------------------------------------- /matlab/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.8) 2 | 3 | include(mex.cmake) # Load add_mex macro. 4 | 5 | # List external library dependencies. 6 | set(CMAKE_CXX_FLAGS "-std=c++11 -Wall ${CMAKE_CXX_FLAGS}") 7 | set(MEX_LDFLAGS "-Wl,-rpath,${Calibu_DIR} -L${Calibu_DIR}") 8 | set(MEX_LINK_LIBS "${Calibu_LIBRARIES};calibu" 9 | CACHE STRING "External libraries to link to this mex file." 10 | FORCE) 11 | 12 | # Invoke macro to generate calibu wrapper. 13 | add_mex(calibu_mex calibu_mex.cpp) 14 | 15 | #add_dependencies(calibu_mex calibu) 16 | 17 | -------------------------------------------------------------------------------- /matlab/calibu_rig.m: -------------------------------------------------------------------------------- 1 | classdef calibu_rig 2 | %CALIBU_RIG Wrapper for Calibu rig class. 3 | % Allows projecting, unprojecting and transferring 2d/3d from 4 | % all supported camera models. 5 | 6 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 7 | properties 8 | cpp_calibu_rig_ptr_; 9 | end 10 | 11 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 12 | methods 13 | 14 | %%% Constructor. 15 | function obj = calibu_rig(filename) 16 | obj.cpp_calibu_rig_ptr_ = calibu_mex('new', filename); 17 | end 18 | 19 | %%% Destructor. 20 | function delete(obj) 21 | calibu_mex('delete', obj.cpp_calibu_rig_ptr_); 22 | end 23 | 24 | %%% Project. 25 | function [pixel_coordinate] = project(obj, camera_id, point) 26 | pixel_coordinate = calibu_mex('project', obj.cpp_calibu_rig_ptr_, ... 27 | camera_id, point); 28 | end 29 | 30 | %%% Project Points. 31 | function [pixel_coordinate] = project_points(obj, camera_id, points) 32 | pixel_coordinate = calibu_mex('project_points', obj.cpp_calibu_rig_ptr_, ... 33 | camera_id, length(points), points); 34 | end 35 | 36 | %%% Unproject. 37 | function [ray] = unproject(obj, camera_id, pixel_coordinate) 38 | ray = calibu_mex('unproject', obj.cpp_calibu_rig_ptr_, ... 39 | camera_id, pixel_coordinate); 40 | end 41 | 42 | %%% Unproject Pixels. 43 | function [rays] = unproject_pixels(obj, camera_id, pixels) 44 | rays = calibu_mex('unproject_pixels', obj.cpp_calibu_rig_ptr_, ... 45 | camera_id, length(pixels), pixels); 46 | end 47 | 48 | %%% Transfer3d. 49 | function [pixel_coordinate] = transfer_3d(obj, camera_id, Tab, ray, rho) 50 | pixel_coordinate = calibu_mex('transfer_3d', ... 51 | obj.cpp_calibu_rig_ptr_, ... 52 | camera_id, Tab, ray, rho); 53 | end 54 | 55 | %%% Get K. 56 | function [K] = get_K(obj, camera_id) 57 | K = calibu_mex('get_K', obj.cpp_calibu_rig_ptr_, camera_id); 58 | end 59 | 60 | %%% Get Trc. 61 | function [Trc] = get_Trc(obj, camera_id) 62 | Trc = calibu_mex('get_Trc', obj.cpp_calibu_rig_ptr_, camera_id); 63 | end 64 | 65 | end 66 | 67 | end 68 | 69 | -------------------------------------------------------------------------------- /matlab/class_handle.hpp: -------------------------------------------------------------------------------- 1 | #ifndef __CLASS_HANDLE_HPP__ 2 | #define __CLASS_HANDLE_HPP__ 3 | 4 | #include "mex.h" 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | 11 | #define CLASS_HANDLE_SIGNATURE 0xFF00F0A5 12 | template class class_handle 13 | { 14 | public: 15 | class_handle(base *ptr) : name_m(typeid(base).name()), ptr_m(ptr) { signature_m = CLASS_HANDLE_SIGNATURE; } 16 | ~class_handle() { signature_m = 0; delete ptr_m; } 17 | bool isValid() { return ((signature_m == CLASS_HANDLE_SIGNATURE) && !strcmp(name_m.c_str(), typeid(base).name())); } 18 | base *ptr() { return ptr_m; } 19 | 20 | private: 21 | uint32_t signature_m; 22 | std::string name_m; 23 | base *ptr_m; 24 | }; 25 | 26 | template inline mxArray *convertPtr2Mat(base *ptr) 27 | { 28 | mexLock(); 29 | mxArray *out = mxCreateNumericMatrix(1, 1, mxUINT64_CLASS, mxREAL); 30 | *((uint64_t *)mxGetData(out)) = reinterpret_cast(new class_handle(ptr)); 31 | return out; 32 | } 33 | 34 | template inline class_handle *convertMat2HandlePtr(const mxArray *in) 35 | { 36 | if (mxGetNumberOfElements(in) != 1 || mxGetClassID(in) != mxUINT64_CLASS || mxIsComplex(in)) 37 | mexErrMsgTxt("Input must be a real uint64 scalar."); 38 | class_handle *ptr = reinterpret_cast *>(*((uint64_t *)mxGetData(in))); 39 | if (!ptr->isValid()) 40 | mexErrMsgTxt("Handle not valid."); 41 | return ptr; 42 | } 43 | 44 | template inline base *convertMat2Ptr(const mxArray *in) 45 | { 46 | return convertMat2HandlePtr(in)->ptr(); 47 | } 48 | 49 | template inline void destroyObject(const mxArray *in) 50 | { 51 | delete convertMat2HandlePtr(in); 52 | mexUnlock(); 53 | } 54 | 55 | #endif // __CLASS_HANDLE_HPP__ 56 | -------------------------------------------------------------------------------- /src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ####################################################### 2 | ## Sources 3 | 4 | set( INC_DIR ${Calibu_SOURCE_DIR}/include/calibu ) 5 | 6 | set(HEADERS 7 | ${INC_DIR}/Calibu.h 8 | ${INC_DIR}/Platform.h 9 | ${INC_DIR}/calib/AutoDiffArrayCostFunction.h 10 | ${INC_DIR}/calib/Calibrator.h 11 | ${INC_DIR}/calib/CostFunctionAndParams.h 12 | ${INC_DIR}/calib/LocalParamSe3.h 13 | ${INC_DIR}/cam/camera_crtp.h 14 | ${INC_DIR}/cam/camera_crtp_impl.h 15 | ${INC_DIR}/cam/camera_models_crtp.h 16 | ${INC_DIR}/cam/camera_xml.h 17 | ${INC_DIR}/cam/camera_rig.h 18 | ${INC_DIR}/cam/rectify_crtp.h 19 | ${INC_DIR}/cam/stereo_rectify.h 20 | ${INC_DIR}/conics/Conic.h 21 | ${INC_DIR}/conics/ConicFinder.h 22 | ${INC_DIR}/conics/FindConics.h 23 | ${INC_DIR}/gl/Drawing.h 24 | ${INC_DIR}/image/AdaptiveThreshold.h 25 | ${INC_DIR}/image/Gradient.h 26 | ${INC_DIR}/image/ImageProcessing.h 27 | ${INC_DIR}/image/IntegralImage.h 28 | ${INC_DIR}/image/Label.h 29 | ${INC_DIR}/pose/Ransac.h 30 | ${INC_DIR}/target/Hungarian.h 31 | ${INC_DIR}/target/LineGroup.h 32 | ${INC_DIR}/target/RandomGrid.h 33 | ${INC_DIR}/target/Target.h 34 | ${INC_DIR}/target/TargetGridDot.h 35 | ${INC_DIR}/utils/Rectangle.h 36 | ${INC_DIR}/utils/Range.h 37 | ${INC_DIR}/utils/Utils.h 38 | ${INC_DIR}/utils/PlaneBasis.h 39 | ${INC_DIR}/utils/StreamOperatorsEigen.h 40 | ${INC_DIR}/utils/StreamOperatorsSophus.h 41 | ) 42 | 43 | set(SRC_DIR ${Calibu_SOURCE_DIR}/src) 44 | SET(SOURCES 45 | ${SRC_DIR}/cam/CameraXml.cpp 46 | ${SRC_DIR}/cam/rectify_crtp.cpp 47 | ${SRC_DIR}/cam/StereoRectify.cpp 48 | ${SRC_DIR}/conics/Conic.cpp 49 | ${SRC_DIR}/conics/ConicFinder.cpp 50 | ${SRC_DIR}/conics/FindConics.cpp 51 | ${SRC_DIR}/image/ImageProcessing.cpp 52 | ${SRC_DIR}/image/Label.cpp 53 | ${SRC_DIR}/target/Hungarian.cpp 54 | ${SRC_DIR}/target/RandomGrid.cpp 55 | ${SRC_DIR}/target/TargetGridDot.cpp 56 | ${SRC_DIR}/utils/Utils.cpp 57 | ${SRC_DIR}/utils/Xml.cpp 58 | ) 59 | 60 | ####################################################### 61 | ## Find required dependencies 62 | 63 | find_package( Eigen3 REQUIRED ) 64 | 65 | find_package( GLog REQUIRED ) 66 | set(HAVE_GLOG 1) 67 | list( APPEND LINK_LIBS ${GLog_LIBRARIES} ) 68 | list( APPEND CALIBU_INC ${Glog_INCLUDE_DIRS} ) 69 | 70 | # https://github.com/gwu-robotics/Sophus.git 71 | find_package( Sophus REQUIRED ) 72 | 73 | ## Apply project include directories 74 | list( APPEND CALIBU_INC "${CMAKE_CURRENT_SOURCE_DIR}/../include;${CMAKE_CURRENT_BINARY_DIR}/include" ) 75 | list( APPEND USER_INC ${EIGEN3_INCLUDE_DIR} ) 76 | list( APPEND USER_INC ${Sophus_INCLUDE_DIR} ) 77 | 78 | ####################################################### 79 | ## Find optional dependencies 80 | 81 | # OpenCV is required for PNP methods and calibration application 82 | find_package(OpenCV2) 83 | if( OpenCV2_FOUND ) 84 | set( HAVE_OPENCV 1 ) 85 | list( APPEND LINK_LIBS ${OpenCV2_LIBRARIES}) 86 | list( APPEND CALIBU_INC ${OpenCV2_INCLUDE_DIRS} ) 87 | list( APPEND HEADERS ${INC_DIR}/pose/Pnp.h ${INC_DIR}/pose/Tracker.h ) 88 | list( APPEND SOURCES ${SRC_DIR}/pose/Pnp.cpp ${SRC_DIR}/pose/Tracker.cpp ) 89 | endif() 90 | 91 | ####################################################### 92 | ## Setup and configure library 93 | 94 | # Include source/build directories first to preempt any installed Calibu 95 | include_directories( ${CALIBU_INC} ) 96 | include_directories( ${USER_INC} ) 97 | 98 | ## Generate symbol export helper header on MSVC 99 | IF(MSVC) 100 | string(TOUPPER ${LIBRARY_NAME} LIBRARY_NAME_CAPS) 101 | include(GenerateExportHeader) 102 | GENERATE_EXPORT_HEADER( ${LIBRARY_NAME} 103 | BASE_NAME ${LIBRARY_NAME_CAPS} 104 | EXPORT_MACRO_NAME ${LIBRARY_NAME_CAPS}_EXPORT 105 | EXPORT_FILE_NAME "${CMAKE_CURRENT_BINARY_DIR}/include/${LIBRARY_NAME}/${LIBRARY_NAME}_export.h" 106 | STATIC_DEFINE ${LIBRARY_NAME_CAPS}_BUILT_AS_STATIC 107 | ) 108 | ENDIF() 109 | 110 | ####################################################### 111 | ## Create configure file for inclusion in library 112 | 113 | # Platform configuration vars 114 | ## Compiler configuration 115 | include(SetPlatformVars) 116 | configure_file( 117 | "${CMAKE_CURRENT_SOURCE_DIR}/config.h.in" 118 | "${CMAKE_CURRENT_BINARY_DIR}/include/${LIBRARY_NAME}/config.h" 119 | ) 120 | 121 | list(APPEND CALIBU_INC ${CMAKE_CURRENT_BINARY_DIR}/include/) 122 | 123 | # Configure the include directories for export_package 124 | set(EXPORT_CALIBU_INC ${CALIBU_INC} ${USER_INC} PARENT_SCOPE) 125 | set(CALIBU_HEADERS ${HEADERS} PARENT_SCOPE) 126 | set(CALIBU_SOURCES ${SOURCES} PARENT_SCOPE) 127 | set(CALIBU_LINK_LIBS ${LINK_LIBS} PARENT_SCOPE) 128 | 129 | ####################################################### 130 | ## Install headers 131 | 132 | install(FILES "${CMAKE_CURRENT_BINARY_DIR}/include/${LIBRARY_NAME}/config.h" 133 | DESTINATION ${CMAKE_INSTALL_PREFIX}/include/${LIBRARY_NAME} 134 | ) 135 | install(DIRECTORY ${INC_DIR} 136 | DESTINATION ${CMAKE_INSTALL_PREFIX}/include 137 | ) 138 | -------------------------------------------------------------------------------- /src/cam/StereoRectify.cpp: -------------------------------------------------------------------------------- 1 | 2 | /* 3 | This file is part of the Calibu Project. 4 | https://github.com/gwu-robotics/Calibu 5 | 6 | Copyright (C) 2013 George Washington University, 7 | Steven Lovegrove, 8 | Gabe Sibley 9 | 10 | Licensed under the Apache License, Version 2.0 (the "License"); 11 | you may not use this file except in compliance with the License. 12 | You may obtain a copy of the License at 13 | 14 | http://www.apache.org/licenses/LICENSE-2.0 15 | 16 | Unless required by applicable law or agreed to in writing, software 17 | distributed under the License is distributed on an "AS IS" BASIS, 18 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 19 | See the License for the specific language governing permissions and 20 | limitations under the License. 21 | */ 22 | 23 | #include 24 | #include 25 | #include 26 | #include 27 | 28 | namespace calibu 29 | { 30 | 31 | std::shared_ptr> CreateScanlineRectifiedLookupAndCameras(const Sophus::SE3d& T_rl, 32 | const std::shared_ptr > cam_left, 33 | const std::shared_ptr > cam_right, 34 | Sophus::SE3d& T_nr_nl, 35 | LookupTable& left_lut, 36 | LookupTable& right_lut 37 | ) 38 | { 39 | const Sophus::SO3d R_rl = T_rl.so3(); 40 | const Sophus::SO3d R_lr = R_rl.inverse(); 41 | const Eigen::Vector3d l_r = T_rl.translation(); 42 | const Eigen::Vector3d r_l = - (R_lr * l_r); 43 | 44 | // Current up vector for each camera (in left FoR) 45 | const Eigen::Vector3d lup_l = Eigen::Vector3d(0,-1,0); 46 | const Eigen::Vector3d rup_l = R_lr * Eigen::Vector3d(0,-1,0); 47 | 48 | // Hypothetical fwd vector for each camera, perpendicular to baseline (in left FoR) 49 | const Eigen::Vector3d lfwd_l = (lup_l.cross(r_l)).normalized(); 50 | const Eigen::Vector3d rfwd_l = (rup_l.cross(r_l)).normalized(); 51 | 52 | // New fwd is average of left / right hypothetical baselines (also perpendicular to baseline) 53 | const Eigen::Vector3d avgfwd_l = lfwd_l + rfwd_l; 54 | 55 | // Define new basis (in left FoR); 56 | const Eigen::Vector3d x_l = r_l.normalized(); 57 | const Eigen::Vector3d z_l = avgfwd_l.normalized(); 58 | const Eigen::Vector3d y_l = z_l.cross(x_l).normalized(); 59 | 60 | // New orientation for both left and right cameras (expressed relative to original left) 61 | Eigen::Matrix3d Rnl_l; 62 | Rnl_l << x_l, y_l, z_l; 63 | 64 | // By definition, the right camera now lies exactly on the x-axis with the same orientation 65 | // as the left camera. 66 | T_nr_nl = Sophus::SE3d(Eigen::Matrix3d::Identity(), Eigen::Vector3d(-r_l.norm(),0,0) ); 67 | 68 | // Work out parameters of new linear camera 69 | const Range range_width = //Intersection( 70 | MinMaxRotatedCol(cam_left, Rnl_l); 71 | // MinMaxRotatedCol(cam_right, Rnl_l) 72 | // ); 73 | const Range range_height = //Intersection( 74 | MinMaxRotatedRow(cam_left, Rnl_l); 75 | // MinMaxRotatedRow(cam_right, Rnl_l) 76 | // ); 77 | 78 | // We want to map range width/height to image via K. 79 | const double fu = (cam_left->Width()-1) / range_width.Size(); 80 | const double fv = (cam_left->Height()-1) / range_height.Size(); 81 | const double u0 = -fu * range_width.minr; 82 | const double v0 = -fv * range_height.minr; 83 | 84 | // Setup new camera 85 | Eigen::Vector2i size_; 86 | Eigen::Matrix::NumParams> params_; 87 | size_ << cam_left->Width(), cam_left->Height(); 88 | params_ << fu, fv, u0, v0; 89 | 90 | std::shared_ptr> new_rig(new calibu::Rig()); 91 | 92 | std::shared_ptr> 93 | new_cam_left(new calibu::LinearCamera(params_, size_)); 94 | new_cam_left->SetPose(Sophus::SE3d()); 95 | std::shared_ptr> 96 | new_cam_right(new calibu::LinearCamera(params_, size_)); 97 | new_cam_left->SetPose(T_nr_nl); 98 | 99 | new_rig->AddCamera(new_cam_left); 100 | new_rig->AddCamera(new_cam_right); 101 | 102 | // Homographies which should be applied to left and right images to scan-line rectify them 103 | const Eigen::Matrix3d Rl_nlKlinv = Rnl_l.transpose() * new_cam_left->K().inverse(); 104 | const Eigen::Matrix3d Rr_nrKlinv = R_lr.inverse().matrix() * Rnl_l.transpose() * new_cam_left->K().inverse(); 105 | 106 | CreateLookupTable(cam_left, Rl_nlKlinv, left_lut); 107 | CreateLookupTable(cam_right, Rr_nrKlinv, right_lut); 108 | return new_rig; 109 | } 110 | 111 | } 112 | -------------------------------------------------------------------------------- /src/config.h.in: -------------------------------------------------------------------------------- 1 | #ifndef _CALIBU_CONFIG_H_ 2 | #define _CALIBU_CONFIG_H_ 3 | 4 | /// Version 5 | #define CALIBU_VERSION_MAJOR @CALIBU_VERSION_MAJOR@ 6 | #define CALIBU_VERSION_MINOR @CALIBU_VERSION_MINOR@ 7 | #define CALIBU_VERSION_STRING "@CALIBU_VERSION@" 8 | 9 | /// Platform 10 | #cmakedefine _UNIX_ 11 | #cmakedefine _WIN_ 12 | #cmakedefine _OSX_ 13 | #cmakedefine _LINUX_ 14 | #cmakedefine _ANDROID_ 15 | 16 | /// Compiler 17 | #cmakedefine _GCC_ 18 | #cmakedefine _MSVC_ 19 | 20 | /// Optional Libraries 21 | #cmakedefine HAVE_OPENCV 22 | 23 | 24 | #endif //_CALIBU_CONFIG_H_ 25 | -------------------------------------------------------------------------------- /src/conics/ConicFinder.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of the Calibu Project. 3 | https://github.com/gwu-robotics/Calibu 4 | 5 | Copyright (C) 2013 George Washington University, 6 | Steven Lovegrove 7 | 8 | Licensed under the Apache License, Version 2.0 (the "License"); 9 | you may not use this file except in compliance with the License. 10 | You may obtain a copy of the License at 11 | 12 | http://www.apache.org/licenses/LICENSE-2.0 13 | 14 | Unless required by applicable law or agreed to in writing, software 15 | distributed under the License is distributed on an "AS IS" BASIS, 16 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 17 | See the License for the specific language governing permissions and 18 | limitations under the License. 19 | */ 20 | 21 | 22 | 23 | #include 24 | #include 25 | #include 26 | 27 | namespace calibu { 28 | 29 | ConicFinder::ConicFinder() 30 | { 31 | } 32 | 33 | void ConicFinder::Find(const ImageProcessing& imgs) 34 | { 35 | candidates.clear(); 36 | conics.clear(); 37 | 38 | // Find candidate regions for conics 39 | FindCandidateConicsFromLabels( 40 | imgs.Width(), imgs.Height(), imgs.Labels(), candidates, 41 | params.conic_min_area, params.conic_max_area, 42 | params.conic_min_density, 43 | params.conic_min_aspect 44 | ); 45 | 46 | // Find conic parameters 47 | FindConics(imgs.Width(), imgs.Height(), candidates, imgs.ImgDeriv(), conics ); 48 | } 49 | 50 | } 51 | -------------------------------------------------------------------------------- /src/image/ImageProcessing.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of the Calibu Project. 3 | https://github.com/gwu-robotics/Calibu 4 | 5 | Copyright (C) 2013 George Washington University, 6 | Steven Lovegrove 7 | 8 | Licensed under the Apache License, Version 2.0 (the "License"); 9 | you may not use this file except in compliance with the License. 10 | You may obtain a copy of the License at 11 | 12 | http://www.apache.org/licenses/LICENSE-2.0 13 | 14 | Unless required by applicable law or agreed to in writing, software 15 | distributed under the License is distributed on an "AS IS" BASIS, 16 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 17 | See the License for the specific language governing permissions and 18 | limitations under the License. 19 | */ 20 | 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | namespace calibu { 28 | 29 | ImageProcessing::ImageProcessing(int maxWidth, int maxHeight) 30 | : width(maxWidth), height(maxHeight) { 31 | AllocateImageData(maxWidth*maxHeight); 32 | } 33 | 34 | ImageProcessing::~ImageProcessing() { 35 | DeallocateImageData(); 36 | } 37 | 38 | void ImageProcessing::AllocateImageData(int maxPixels) { 39 | I.resize(maxPixels); 40 | intI.resize(maxPixels); 41 | dI.resize(maxPixels); 42 | lI.resize(maxPixels); 43 | tI.resize(maxPixels); 44 | } 45 | 46 | void ImageProcessing::DeallocateImageData() {} 47 | 48 | void ImageProcessing::Process(const unsigned char* greyscale_image, 49 | size_t w, size_t h, size_t pitch) { 50 | width = w; 51 | height = h; 52 | 53 | size_t img_size = width * height * sizeof(unsigned char); 54 | if (img_size > I.size()) { 55 | AllocateImageData(img_size); 56 | } 57 | 58 | // Copy input image 59 | if(pitch > width*sizeof(unsigned char) ) { 60 | // Copy line by line 61 | for(int y=0; y < height; ++y) { 62 | memcpy(&I[y*width], greyscale_image+y*pitch, width * sizeof(unsigned char)); 63 | } 64 | }else{ 65 | memcpy(&I[0], greyscale_image, img_size); 66 | } 67 | 68 | // Process image 69 | gradient<>(width, height, &I[0], &dI[0]); 70 | integral_image(width, height, &I[0], &intI[0] ); 71 | 72 | // Threshold image 73 | AdaptiveThreshold( 74 | width, height, &I[0], &intI[0], &tI[0], params.at_threshold, 75 | width / params.at_window_ratio, 20, 76 | (unsigned char)0, (unsigned char)255 77 | ); 78 | 79 | // Label image (connected components) 80 | labels.clear(); 81 | Label(width, height, &tI[0], &lI[0], labels, 82 | params.black_on_white ? 0 : 255 ); 83 | } 84 | 85 | } 86 | -------------------------------------------------------------------------------- /src/image/Label.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of the Calibu Project. 3 | https://github.com/gwu-robotics/Calibu 4 | 5 | Copyright (C) 2013 George Washington University, 6 | Steven Lovegrove 7 | 8 | Licensed under the Apache License, Version 2.0 (the "License"); 9 | you may not use this file except in compliance with the License. 10 | You may obtain a copy of the License at 11 | 12 | http://www.apache.org/licenses/LICENSE-2.0 13 | 14 | Unless required by applicable law or agreed to in writing, software 15 | distributed under the License is distributed on an "AS IS" BASIS, 16 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 17 | See the License for the specific language governing permissions and 18 | limitations under the License. 19 | */ 20 | 21 | #include 22 | 23 | #include 24 | 25 | using namespace std; 26 | using namespace Eigen; 27 | 28 | namespace calibu { 29 | 30 | inline short RootLabel(vector& labels, short label ) 31 | { 32 | if( label >= 0 ) 33 | { 34 | while( labels[label].equiv >= 0) 35 | label = labels[label].equiv; 36 | } 37 | return label; 38 | } 39 | 40 | inline void AssignLabel( const int w, const int h, const unsigned char* I, short* label, vector& labels, int r, int c, unsigned char passval ) 41 | { 42 | if( I[r*w+c] == passval ) 43 | { 44 | short* labelr = label + r*w; 45 | short* labelrm1 = labelr - w; 46 | 47 | const short lup = r>0? RootLabel(labels,labelrm1[c]) : -1; 48 | const short lleft = c>0? RootLabel(labels,labelr[c-1]) : -1; 49 | 50 | if( lup >= 0 && lleft >= 0 && lup != lleft ) 51 | { 52 | // merge 53 | labelr[c] = lup; 54 | labels[lup].size += labels[lleft].size + 1; 55 | labels[lleft].equiv = lup; 56 | labels[lup].bbox.Insert(labels[lleft].bbox); 57 | }else if( lup >= 0 ) 58 | { 59 | // assign to lup 60 | labelr[c] = lup; 61 | ++labels[lup].size; 62 | labels[lup].bbox.Insert(c,r); 63 | }else if( lleft >= 0 ) 64 | { 65 | // assign to lleft 66 | labelr[c] = lleft; 67 | ++labels[lleft].size; 68 | labels[lleft].bbox.Insert(c,r); 69 | }else{ 70 | // new 71 | PixelClass pc = { -1, IRectangle(c, r, c, r), 1 }; 72 | labels.push_back( pc ); 73 | labelr[c] = labels.size()-1; 74 | } 75 | } 76 | } 77 | 78 | void Label( int w, int h, const unsigned char* I, short* label, vector& labels, unsigned char passval ) 79 | { 80 | std::fill(label,label+w*h, -1); 81 | 82 | for( int d=0; d < max(w,h); ++d ) 83 | { 84 | if( d 2 | #include 3 | 4 | namespace calibu 5 | { 6 | 7 | //////////////////////////////////////////////////////////////////////////////// 8 | 9 | const uint8_t Base64::encoding_map[] = 10 | "ABCDEFGHIJKLMNOPQRSTUVWXYZ" 11 | "abcdefghijklmnopqrstuvwxyz" 12 | "0123456789+/"; 13 | 14 | const uint8_t Base64::decoding_map[] = 15 | { 16 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 17 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 18 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,62, 0, 0, 0,63, 19 | 52,53,54,55,56,57,58,59,60,61, 0, 0, 0, 0, 0, 0, 20 | 0, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9,10,11,12,13,14, 21 | 15,16,17,18,19,20,21,22,23,24,25, 0, 0, 0, 0, 0, 22 | 0,26,27,28,29,30,31,32,33,34,35,36,37,38,39,40, 23 | 41,42,43,44,45,46,47,48,49,50,51, 0, 0, 0, 0, 0, 24 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 25 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 26 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 27 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 28 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 29 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 30 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 31 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 32 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 33 | }; 34 | 35 | //////////////////////////////////////////////////////////////////////////////// 36 | 37 | Base64Encoder::Base64Encoder() 38 | { 39 | } 40 | 41 | template <> 42 | Base64Encoder& Base64Encoder::operator<<(const Eigen::MatrixXd& value) 43 | { 44 | // allocate encoding buffer 45 | 46 | std::vector buffer(sizeof(uint64_t) * value.size()); 47 | uint64_t* pointer = reinterpret_cast(buffer.data()); 48 | 49 | // encode each cell of matrix 50 | 51 | for (int col = 0; col < value.cols(); ++col) 52 | { 53 | for (int row = 0; row < value.rows(); ++row) 54 | { 55 | *pointer = DoubleEncoder::Encode(value(row, col)); 56 | ++pointer; 57 | } 58 | } 59 | 60 | // write encoding to stream 61 | 62 | stream_ << Base64::Encode(buffer); 63 | 64 | return *this; 65 | } 66 | 67 | std::string Base64Encoder::str() const 68 | { 69 | return stream_.str(); 70 | } 71 | 72 | //////////////////////////////////////////////////////////////////////////////// 73 | 74 | Base64Decoder::Base64Decoder(const std::string data) : 75 | stream_(data) 76 | { 77 | } 78 | 79 | template <> 80 | Base64Decoder& Base64Decoder::operator>>(Eigen::MatrixXd& value) 81 | { 82 | const size_t bytes = sizeof(uint64_t) * value.size(); 83 | const size_t chars = Base64::GetEncodingSize(bytes); 84 | 85 | std::vector encoding(chars); 86 | stream_.read(encoding.data(), chars); 87 | 88 | const std::vector data = Base64::Decode(encoding); 89 | const uint64_t* pointer = reinterpret_cast(data.data()); 90 | 91 | // decode each cell of matrix 92 | 93 | for (int col = 0; col < value.cols(); ++col) 94 | { 95 | for (int row = 0; row < value.rows(); ++row) 96 | { 97 | value(row, col) = DoubleEncoder::Decode(*pointer); 98 | ++pointer; 99 | } 100 | } 101 | 102 | return *this; 103 | } 104 | 105 | //////////////////////////////////////////////////////////////////////////////// 106 | 107 | } // namespace calibu -------------------------------------------------------------------------------- /src/pose/Pnp.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of the Calibu Project. 3 | https://github.com/gwu-robotics/Calibu 4 | 5 | Copyright (C) 2013 George Washington University, 6 | Hauke Strasdat, 7 | Steven Lovegrove 8 | 9 | Licensed under the Apache License, Version 2.0 (the "License"); 10 | you may not use this file except in compliance with the License. 11 | You may obtain a copy of the License at 12 | 13 | http://www.apache.org/licenses/LICENSE-2.0 14 | 15 | Unless required by applicable law or agreed to in writing, software 16 | distributed under the License is distributed on an "AS IS" BASIS, 17 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 18 | See the License for the specific language governing permissions and 19 | limitations under the License. 20 | */ 21 | 22 | #include 23 | 24 | #include 25 | #include 26 | #include 27 | 28 | using namespace std; 29 | using namespace Eigen; 30 | 31 | namespace calibu { 32 | 33 | vector PosePnPRansac( 34 | const std::shared_ptr> cam, 35 | const std::vector >& img_pts, 36 | const std::vector >& ideal_pts, 37 | const vector & candidate_map, 38 | int robust_3pt_its, 39 | float robust_3pt_tol, 40 | Sophus::SE3d * T) { 41 | vector inlier_map(candidate_map.size(), -1); 42 | std::vector cv_obj; 43 | std::vector cv_img; 44 | std::vector idx_vec; 45 | cv::Mat cv_coeff; 46 | cv::Mat cv_rot(3,1,CV_64F); 47 | cv::Mat cv_trans(3,1,CV_64F); 48 | cv::Mat cv_K(3,3,CV_64F); 49 | 50 | // cv::eigen2cv(cam.K(), cv_K); 51 | cv::setIdentity(cv_K); 52 | 53 | for (size_t i = 0; i=0) 57 | { 58 | // TODO: This is really bad for cameras > 180 FOV 59 | // Replace with PNP for general camera. 60 | const Eigen::Vector3d img_center_pts = cam->Unproject(img_pts[i]); 61 | Eigen::Vector2d center; 62 | center << img_center_pts[0]/img_center_pts[2], img_center_pts[1]/img_center_pts[2]; 63 | // const Eigen::Vector2d center = cam.Unmap(img_pts[i]); 64 | const Eigen::Vector3d & c3d = ideal_pts[ideal_point_id]; 65 | cv_img.push_back(cv::Point2f(center.x(), center.y())); 66 | cv_obj.push_back(cv::Point3f(c3d.x(), c3d.y(), c3d.z())); 67 | idx_vec.push_back(i); 68 | } 69 | } 70 | 71 | std::vector cv_inliers; 72 | 73 | if(cv_img.size() < 4) 74 | return cv_inliers; 75 | 76 | if(robust_3pt_its > 0) { 77 | cv::solvePnPRansac(cv_obj, cv_img, cv_K, cv_coeff, cv_rot, cv_trans, 78 | false, robust_3pt_its, robust_3pt_tol / cam->K()(0,0), 60, cv_inliers); 79 | }else{ 80 | cv::solvePnP(cv_obj, cv_img, cv_K, cv_coeff, cv_rot, cv_trans, false); 81 | } 82 | 83 | Vector3d rot, trans; 84 | cv::cv2eigen(cv_rot, rot); 85 | cv::cv2eigen(cv_trans, trans); 86 | 87 | if(std::isnan(rot[0]) || std::isnan(rot[1]) || std::isnan(rot[2])) 88 | return inlier_map; 89 | 90 | for (size_t i = 0; i & conics_target_map) 101 | { 102 | int inliers =0; 103 | for (size_t i=0; i < conics_target_map.size(); ++i) 104 | { 105 | if( conics_target_map[i] >=0 ) 106 | { 107 | inliers++; 108 | } 109 | } 110 | return inliers; 111 | } 112 | 113 | double ReprojectionErrorRMS(const std::shared_ptr> cam, 114 | const Sophus::SE3d& T_cw, 115 | const std::vector >& pts3d, 117 | const std::vector >& pts2d, 119 | const vector& map2d_3d) 120 | { 121 | int n=0; 122 | double sse =0; 123 | for( unsigned i=0; i= 0 ) 127 | { 128 | const Vector2d t = cam->Project(T_cw * pts3d[ti]); 129 | Vector2d err = t - pts2d[i].head<2>(); 130 | sse += (err).squaredNorm(); 131 | ++n; 132 | } 133 | } 134 | return sqrt(sse / n); 135 | } 136 | 137 | } 138 | -------------------------------------------------------------------------------- /src/pose/Tracker.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of the Calibu Project. 3 | https://github.com/gwu-robotics/Calibu 4 | 5 | Copyright (C) 2013 George Washington University 6 | Steven Lovegrove, 7 | Hauke Strasdat 8 | 9 | Licensed under the Apache License, Version 2.0 (the "License"); 10 | you may not use this file except in compliance with the License. 11 | You may obtain a copy of the License at 12 | 13 | http://www.apache.org/licenses/LICENSE-2.0 14 | 15 | Unless required by applicable law or agreed to in writing, software 16 | distributed under the License is distributed on an "AS IS" BASIS, 17 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 18 | See the License for the specific language governing permissions and 19 | limitations under the License. 20 | */ 21 | 22 | #include 23 | #include 24 | #include 25 | 26 | #include 27 | 28 | using namespace std; 29 | using namespace Eigen; 30 | 31 | namespace calibu { 32 | 33 | Tracker::Tracker(TargetInterface& target, int w, int h) 34 | : target(target), imgs(w,h), 35 | last_good(0), good_frames(0) 36 | { 37 | 38 | } 39 | 40 | bool Tracker::ProcessFrame( 41 | std::shared_ptr> cam, 42 | const unsigned char* I, size_t w, size_t h, size_t pitch) 43 | { 44 | double rms = 0; 45 | 46 | imgs.Process(I, w, h, pitch ); 47 | conic_finder.Find(imgs); 48 | 49 | const std::vector >& conics = 50 | conic_finder.Conics(); 51 | 52 | // Generate map and point structures 53 | conics_target_map.clear(); 54 | conics_target_map.resize(conics.size(),-1); 55 | vector > ellipses; 56 | for( size_t i=0; i < conics.size(); ++i ) { 57 | ellipses.push_back(Vector2d(conics[i].center.x(),conics[i].center.y())); 58 | } 59 | 60 | // Undistort Conics 61 | vector > conics_camframe; 62 | for( unsigned int i=0; i> idcam(new LinearCamera()); 68 | 69 | target.FindTarget( idcam, imgs, conics_camframe, conics_target_map ); 70 | conics_candidate_map_first_pass = conics_target_map; 71 | int inliers = CountInliers(conics_candidate_map_first_pass); 72 | if (inliers=params.inlier_num_required) { 105 | T_gw = T_hw; 106 | return true; 107 | } 108 | printf("Failed: if( isfinite((double)rms) && rms < params.max_rms && inliers>=params.inlier_num_required) {\n"); 109 | return false; 110 | } 111 | 112 | } 113 | -------------------------------------------------------------------------------- /src/utils/Utils.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of the Calibu Project. 3 | https://github.com/gwu-robotics/Calibu 4 | 5 | Copyright (C) 2013 George Washington University, 6 | Steven Lovegrove, 7 | Gabe Sibley 8 | 9 | Licensed under the Apache License, Version 2.0 (the "License"); 10 | you may not use this file except in compliance with the License. 11 | You may obtain a copy of the License at 12 | 13 | http://www.apache.org/licenses/LICENSE-2.0 14 | 15 | Unless required by applicable law or agreed to in writing, software 16 | distributed under the License is distributed on an "AS IS" BASIS, 17 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 18 | See the License for the specific language governing permissions and 19 | limitations under the License. 20 | */ 21 | 22 | #include 23 | 24 | #include "assert.h" 25 | #include 26 | 27 | using namespace Eigen; 28 | 29 | namespace calibu { 30 | 31 | Eigen::Matrix3d EstimateH_ba( 32 | const std::vector >& a, 33 | const std::vector >& b 34 | ) 35 | { 36 | assert(a.size() == b.size()); 37 | 38 | // based on estimatehomography.m 39 | // George Vogiatzis and Carlos Hernández 40 | // http://george-vogiatzis.org/calib/ 41 | 42 | MatrixXd M(a.size()*2,9); 43 | 44 | for( unsigned int i=0; i< a.size(); ++i ) 45 | { 46 | const double u1 = a[i][0]; 47 | const double v1 = a[i][1]; 48 | const double u2 = b[i][0]; 49 | const double v2 = b[i][1]; 50 | 51 | M.block<2,9>(i*2,0) << 52 | u1, v1, 1, 0, 0, 0, -u1 * u2, -v1 * u2, -u2, 53 | 0, 0, 0, u1, v1, 1, -u1 * v2, -v1 * v2, -v2; 54 | } 55 | 56 | const Matrix Vt = 57 | Eigen::JacobiSVD(M, ComputeFullV).matrixV().transpose(); 58 | 59 | // return last row of svd.get_VT(), reshaped in to 3x3 60 | Matrix3d H; 61 | H.block<1,3>(0,0) = Vt.block<1,3>(8,0); 62 | H.block<1,3>(1,0) = Vt.block<1,3>(8,3); 63 | H.block<1,3>(2,0) = Vt.block<1,3>(8,6); 64 | 65 | H /= H(2,2); 66 | 67 | return H; 68 | } 69 | 70 | } 71 | -------------------------------------------------------------------------------- /tests/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | enable_testing() 2 | 3 | # find packages 4 | 5 | list(APPEND REQUIRED_INCLUDE_DIRS ${USER_INC}) 6 | list(APPEND REQUIRED_INCLUDE_DIRS ${CMAKE_SOURCE_DIR}/include) 7 | list(APPEND REQUIRED_INCLUDE_DIRS ${CMAKE_BINARY_DIR}/include) 8 | list(APPEND REQUIRED_INCLUDE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}) 9 | list(APPEND REQUIRED_INCLUDE_DIRS ${CMAKE_CURRENT_BINARY_DIR}) 10 | list(APPEND REQUIRED_LIBRARIES calibu) 11 | 12 | find_package(GTest REQUIRED) 13 | list(APPEND REQUIRED_INCLUDE_DIRS ${GTEST_INCLUDE_DIRS}) 14 | list(APPEND REQUIRED_LIBRARIES ${GTEST_LIBRARIES}) 15 | list(APPEND REQUIRED_LIBRARIES pthread) 16 | 17 | # define c++ sources 18 | 19 | set(CPP_SOURCES 20 | base64_test.cpp 21 | exception_test.cpp 22 | pcalib_xml_test.cpp 23 | response_linear_test.cpp 24 | response_poly_test.cpp 25 | vignetting_dense_test.cpp 26 | vignetting_poly_test.cpp 27 | vignetting_uniform_test.cpp 28 | ) 29 | 30 | # build executable 31 | 32 | include_directories(${REQUIRED_INCLUDE_DIRS}) 33 | add_executable(tests tests.cpp ${CPP_SOURCES}) 34 | target_link_libraries(tests ${REQUIRED_LIBRARIES}) -------------------------------------------------------------------------------- /tests/base64_test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | namespace calibu 5 | { 6 | namespace testing 7 | { 8 | 9 | TEST(DoubleEncoder, Mapping) 10 | { 11 | std::vector values; 12 | values.push_back(+0.0); 13 | values.push_back(-0.0); 14 | values.push_back(+1.0); 15 | values.push_back(-1.0); 16 | values.push_back(+1.357103); 17 | values.push_back(-1.357103); 18 | values.push_back(+8.03117E13); 19 | values.push_back(-8.03117E13); 20 | values.push_back(std::numeric_limits::max()); 21 | values.push_back(std::numeric_limits::min()); 22 | values.push_back(+std::numeric_limits::infinity()); 23 | values.push_back(-std::numeric_limits::infinity()); 24 | values.push_back(std::numeric_limits::quiet_NaN()); 25 | 26 | for (double expected : values) 27 | { 28 | const uint64_t encoding = DoubleEncoder::Encode(expected); 29 | const double found = DoubleEncoder::Decode(encoding); 30 | if (std::isnan(expected)) ASSERT_TRUE(std::isnan(found)); 31 | else ASSERT_DOUBLE_EQ(expected, found); 32 | } 33 | } 34 | 35 | TEST(Base64, Encode) 36 | { 37 | { 38 | std::vector data = { 255 }; 39 | const std::string found = Base64::Encode(data); 40 | const std::string expected = "/w"; 41 | ASSERT_EQ(expected, found); 42 | } 43 | 44 | { 45 | std::vector data = { 255, 126 }; 46 | const std::string found = Base64::Encode(data); 47 | const std::string expected = "/34"; 48 | ASSERT_EQ(expected, found); 49 | } 50 | 51 | { 52 | std::vector data = { 0, 1, 2 }; 53 | const std::string found = Base64::Encode(data); 54 | const std::string expected = "AAEC"; 55 | ASSERT_EQ(expected, found); 56 | } 57 | 58 | { 59 | std::vector data = { 0, 1, 2, 3, 4, 5 }; 60 | const std::string found = Base64::Encode(data); 61 | const std::string expected = "AAECAwQF"; 62 | ASSERT_EQ(expected, found); 63 | } 64 | } 65 | 66 | TEST(Base64, Decode) 67 | { 68 | { 69 | const std::string data = "/w"; 70 | std::vector expected = { 255, 0 }; 71 | const std::vector found = Base64::Decode(data); 72 | ASSERT_EQ(expected.size(), found.size()); 73 | 74 | for (size_t i = 0; i < expected.size(); ++i) 75 | { 76 | ASSERT_EQ(expected[i], found[i]); 77 | } 78 | } 79 | 80 | { 81 | const std::string data = "/34"; 82 | std::vector expected = { 255, 126, 0 }; 83 | const std::vector found = Base64::Decode(data); 84 | ASSERT_EQ(expected.size(), found.size()); 85 | 86 | for (size_t i = 0; i < expected.size(); ++i) 87 | { 88 | ASSERT_EQ(expected[i], found[i]); 89 | } 90 | } 91 | 92 | { 93 | const std::string data = "AAEC"; 94 | std::vector expected = { 0, 1, 2 }; 95 | const std::vector found = Base64::Decode(data); 96 | ASSERT_EQ(expected.size(), found.size()); 97 | 98 | for (size_t i = 0; i < expected.size(); ++i) 99 | { 100 | ASSERT_EQ(expected[i], found[i]); 101 | } 102 | } 103 | 104 | { 105 | const std::string data = "AAECAwQF"; 106 | std::vector expected = { 0, 1, 2, 3, 4, 5 }; 107 | const std::vector found = Base64::Decode(data); 108 | ASSERT_EQ(expected.size(), found.size()); 109 | 110 | for (size_t i = 0; i < expected.size(); ++i) 111 | { 112 | ASSERT_EQ(expected[i], found[i]); 113 | } 114 | } 115 | } 116 | 117 | TEST(Base64, Mapping) 118 | { 119 | std::vector expected(256); 120 | std::iota(expected.begin(), expected.end(), 0); 121 | const std::string encoding = Base64::Encode(expected); 122 | const std::vector found = Base64::Decode(encoding); 123 | 124 | ASSERT_LE(expected.size(), found.size()); 125 | 126 | for (size_t i = 0; i < expected.size(); ++i) 127 | { 128 | ASSERT_EQ(expected[i], found[i]); 129 | } 130 | } 131 | 132 | TEST(Base64Encode, Mapping) 133 | { 134 | Eigen::MatrixXd matrix(640, 480); 135 | matrix = Eigen::MatrixXd::Random(640, 480); 136 | 137 | Base64Encoder encoder; 138 | encoder << matrix; 139 | 140 | const std::string encoding = encoder.str(); 141 | 142 | Base64Decoder decoder(encoding); 143 | 144 | Eigen::MatrixXd found(640, 480); 145 | decoder >> found; 146 | 147 | for (int col = 0; col < matrix.cols(); ++col) 148 | { 149 | for (int row = 0; row < matrix.rows(); ++row) 150 | { 151 | ASSERT_DOUBLE_EQ(matrix(row, col), found(row, col)); 152 | } 153 | } 154 | } 155 | 156 | } // namespace testing 157 | 158 | } // namespace calibu -------------------------------------------------------------------------------- /tests/exception_test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | namespace calibu 5 | { 6 | namespace testing 7 | { 8 | 9 | TEST(Exception, Constructor) 10 | { 11 | const int expected_line = __LINE__; 12 | const std::string expected_file = __FILE__; 13 | const std::string expected_desc = "test description"; 14 | 15 | const std::string expected_what = expected_file + "(" + 16 | std::to_string(expected_line) + "): " + expected_desc; 17 | 18 | Exception exception(expected_line, expected_file, expected_desc); 19 | ASSERT_EQ(expected_line, exception.line()); 20 | ASSERT_EQ(expected_file, exception.file()); 21 | ASSERT_EQ(expected_desc, exception.desc()); 22 | ASSERT_EQ(expected_what, exception.what()); 23 | } 24 | 25 | TEST(Exception, Throw) 26 | { 27 | int expected_line; 28 | const std::string expected_file = __FILE__; 29 | const std::string expected_desc = "test description"; 30 | bool thrown = false; 31 | 32 | try 33 | { 34 | expected_line = __LINE__ + 1; 35 | CALIBU_THROW(expected_desc); 36 | } 37 | catch (const Exception& exception) 38 | { 39 | const std::string expected_what = expected_file + "(" + 40 | std::to_string(expected_line) + "): " + expected_desc; 41 | 42 | ASSERT_EQ(expected_line, exception.line()); 43 | ASSERT_EQ(expected_file, exception.file()); 44 | ASSERT_EQ(expected_desc, exception.desc()); 45 | ASSERT_EQ(expected_what, exception.what()); 46 | thrown = true; 47 | } 48 | 49 | ASSERT_TRUE(thrown); 50 | } 51 | 52 | TEST(Exception, AssertDescription) 53 | { 54 | int expected_line; 55 | const std::string expected_file = __FILE__; 56 | const std::string expected_desc = "test description"; 57 | bool thrown = false; 58 | 59 | try 60 | { 61 | expected_line = __LINE__ + 1; 62 | CALIBU_ASSERT_DESC(0 > 1, expected_desc); 63 | } 64 | catch (const Exception& exception) 65 | { 66 | const std::string expected_what = expected_file + "(" + 67 | std::to_string(expected_line) + "): " + expected_desc; 68 | 69 | ASSERT_EQ(expected_line, exception.line()); 70 | ASSERT_EQ(expected_file, exception.file()); 71 | ASSERT_EQ(expected_desc, exception.desc()); 72 | ASSERT_EQ(expected_what, exception.what()); 73 | thrown = true; 74 | } 75 | 76 | ASSERT_TRUE(thrown); 77 | ASSERT_NO_THROW(CALIBU_ASSERT_DESC(0 < 1, "")); 78 | } 79 | 80 | TEST(Exception, Assert) 81 | { 82 | int expected_line; 83 | const std::string expected_file = __FILE__; 84 | const std::string expected_desc = "assertion failed: 0 > 1"; 85 | bool thrown = false; 86 | 87 | try 88 | { 89 | expected_line = __LINE__ + 1; 90 | CALIBU_ASSERT(0 > 1); 91 | } 92 | catch (const Exception& exception) 93 | { 94 | const std::string expected_what = expected_file + "(" + 95 | std::to_string(expected_line) + "): " + expected_desc; 96 | 97 | ASSERT_EQ(expected_line, exception.line()); 98 | ASSERT_EQ(expected_file, exception.file()); 99 | ASSERT_EQ(expected_desc, exception.desc()); 100 | ASSERT_EQ(expected_what, exception.what()); 101 | thrown = true; 102 | } 103 | 104 | ASSERT_TRUE(thrown); 105 | ASSERT_NO_THROW(CALIBU_ASSERT(0 < 1)); 106 | } 107 | 108 | TEST(Exception, DebugDescription) 109 | { 110 | int expected_line; 111 | const std::string expected_file = __FILE__; 112 | const std::string expected_desc = "test description"; 113 | bool thrown = false; 114 | 115 | try 116 | { 117 | expected_line = __LINE__ + 1; 118 | CALIBU_DEBUG_DESC(0 > 1, expected_desc); 119 | } 120 | catch (const Exception& exception) 121 | { 122 | const std::string expected_what = expected_file + "(" + 123 | std::to_string(expected_line) + "): " + expected_desc; 124 | 125 | ASSERT_EQ(expected_line, exception.line()); 126 | ASSERT_EQ(expected_file, exception.file()); 127 | ASSERT_EQ(expected_desc, exception.desc()); 128 | ASSERT_EQ(expected_what, exception.what()); 129 | thrown = true; 130 | } 131 | 132 | 133 | #ifdef NDEBUG 134 | 135 | ASSERT_FALSE(thrown); 136 | 137 | int value = 0; 138 | CALIBU_DEBUG_DESC(++value, ""); 139 | ASSERT_EQ(0, value); 140 | 141 | #else 142 | 143 | ASSERT_TRUE(thrown); 144 | 145 | #endif 146 | 147 | ASSERT_NO_THROW(CALIBU_DEBUG_DESC(0 < 1, "")); 148 | } 149 | 150 | TEST(Exception, Debug) 151 | { 152 | int expected_line; 153 | const std::string expected_file = __FILE__; 154 | const std::string expected_desc = "assertion failed: 0 > 1"; 155 | bool thrown = false; 156 | 157 | try 158 | { 159 | expected_line = __LINE__ + 1; 160 | CALIBU_DEBUG(0 > 1); 161 | } 162 | catch (const Exception& exception) 163 | { 164 | const std::string expected_what = expected_file + "(" + 165 | std::to_string(expected_line) + "): " + expected_desc; 166 | 167 | ASSERT_EQ(expected_line, exception.line()); 168 | ASSERT_EQ(expected_file, exception.file()); 169 | ASSERT_EQ(expected_desc, exception.desc()); 170 | ASSERT_EQ(expected_what, exception.what()); 171 | thrown = true; 172 | } 173 | 174 | 175 | #ifdef NDEBUG 176 | 177 | ASSERT_FALSE(thrown); 178 | 179 | int value = 0; 180 | CALIBU_DEBUG(++value); 181 | ASSERT_EQ(0, value); 182 | 183 | #else 184 | 185 | ASSERT_TRUE(thrown); 186 | 187 | #endif 188 | 189 | ASSERT_NO_THROW(CALIBU_DEBUG(0 < 1)); 190 | } 191 | 192 | } // namespace testing 193 | 194 | } // namespace calibu -------------------------------------------------------------------------------- /tests/pcalib_xml_test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | namespace calibu 10 | { 11 | namespace testing 12 | { 13 | 14 | TEST(PhotoRigXml, ReadWrite) 15 | { 16 | PhotoRigd found; 17 | PhotoRigd expected; 18 | 19 | for (int i = 0; i < 3; ++i) 20 | { 21 | std::shared_ptr camera; 22 | camera = std::make_shared(); 23 | expected.cameras.push_back(camera); 24 | 25 | { 26 | std::shared_ptr> response; 27 | response = std::make_shared>(); 28 | response->SetParams(Eigen::Vector3d(+0.1, -0.2, +0.3)); 29 | response->SetRange(0, 255); 30 | camera->responses.push_back(response); 31 | } 32 | 33 | { 34 | std::shared_ptr> response; 35 | response = std::make_shared>(); 36 | response->SetParams(Eigen::Vector4d(0.0, +0.1, -0.2, +0.3)); 37 | response->SetRange(0, 65535); 38 | camera->responses.push_back(response); 39 | } 40 | 41 | { 42 | std::shared_ptr> response; 43 | response = std::make_shared>(); 44 | response->SetRange(0, 1); 45 | camera->responses.push_back(response); 46 | } 47 | 48 | { 49 | std::shared_ptr> vignetting; 50 | vignetting = std::make_shared>(640, 480); 51 | camera->vignettings.push_back(vignetting); 52 | } 53 | 54 | { 55 | std::shared_ptr> vignetting; 56 | vignetting = std::make_shared>(640, 480); 57 | vignetting->SetParams(Eigen::Vector3d(+0.1, -0.2, +0.3)); 58 | camera->vignettings.push_back(vignetting); 59 | } 60 | 61 | { 62 | Eigen::VectorXd params(640 * 480); 63 | 64 | for (int i = 0; i < params.size(); ++i) 65 | { 66 | params[i] = 0.01 * i; 67 | } 68 | 69 | std::shared_ptr> vignetting; 70 | vignetting = std::make_shared>(640, 480); 71 | vignetting->SetParams(params); 72 | camera->vignettings.push_back(vignetting); 73 | } 74 | } 75 | 76 | const std::string filename = "test_pcalib.xml"; 77 | PhotoRigWriter writer(filename); 78 | writer.Write(expected); 79 | 80 | PhotoRigReader reader(filename); 81 | reader.Read(found); 82 | 83 | // std::remove(filename.c_str()); 84 | 85 | ASSERT_EQ(expected.cameras.size(), found.cameras.size()); 86 | 87 | for (size_t j = 0; j < expected.cameras.size(); ++j) 88 | { 89 | std::shared_ptr fcamera = found.cameras[j]; 90 | std::shared_ptr ecamera = expected.cameras[j]; 91 | 92 | ASSERT_EQ(ecamera->responses.size(), fcamera->responses.size()); 93 | 94 | for (size_t i = 0; i < ecamera->responses.size(); ++i) 95 | { 96 | std::shared_ptr> e = ecamera->responses[i]; 97 | std::shared_ptr> f = fcamera->responses[i]; 98 | 99 | ASSERT_EQ(e->Type(), f->Type()); 100 | ASSERT_NEAR(e->GetRange()[0], f->GetRange()[0], 1E-10); 101 | ASSERT_NEAR(f->GetRange()[1], f->GetRange()[1], 1E-10); 102 | 103 | const Eigen::VectorXd& eparams = e->GetParams(); 104 | const Eigen::VectorXd& fparams = f->GetParams(); 105 | 106 | ASSERT_EQ(eparams.size(), fparams.size()); 107 | 108 | for (int j = 0; j < eparams.size(); ++j) 109 | { 110 | ASSERT_NEAR(eparams[j], fparams[j], 1E-10); 111 | } 112 | } 113 | 114 | ASSERT_EQ(ecamera->vignettings.size(), fcamera->vignettings.size()); 115 | 116 | for (size_t i = 0; i < ecamera->vignettings.size(); ++i) 117 | { 118 | std::shared_ptr> e = ecamera->vignettings[i]; 119 | std::shared_ptr> f = fcamera->vignettings[i]; 120 | 121 | ASSERT_EQ(e->Type(), f->Type()); 122 | ASSERT_EQ(e->Width(), f->Width()); 123 | ASSERT_EQ(e->Height(), f->Height()); 124 | 125 | const Eigen::VectorXd& eparams = e->GetParams(); 126 | const Eigen::VectorXd& fparams = f->GetParams(); 127 | 128 | ASSERT_EQ(eparams.size(), fparams.size()); 129 | 130 | for (int j = 0; j < eparams.size(); ++j) 131 | { 132 | ASSERT_NEAR(eparams[j], fparams[j], 1E-10); 133 | } 134 | } 135 | } 136 | } 137 | 138 | } // namespace testing 139 | 140 | } // namespace calibu -------------------------------------------------------------------------------- /tests/response_linear_test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | namespace calibu 5 | { 6 | namespace testing 7 | { 8 | 9 | TEST(LinearResponse, Constructor) 10 | { 11 | LinearResponse response; 12 | ASSERT_EQ(0, response.NumParams()); 13 | ASSERT_EQ("linear", response.Type()); 14 | ASSERT_EQ(0, response.GetRange()[0]); 15 | ASSERT_EQ(1, response.GetRange()[1]); 16 | ASSERT_EQ(0, response.GetParams().size()); 17 | } 18 | 19 | TEST(LinearResponse, Range) 20 | { 21 | LinearResponse response; 22 | ASSERT_EQ(0, response.GetRange()[0]); 23 | ASSERT_EQ(1, response.GetRange()[1]); 24 | ASSERT_TRUE(response.InRange(0.0)); 25 | ASSERT_TRUE(response.InRange(0.5)); 26 | ASSERT_TRUE(response.InRange(1.0)); 27 | ASSERT_FALSE(response.InRange(-0.01)); 28 | ASSERT_FALSE(response.InRange(+1.01)); 29 | 30 | response.SetRange(0, 255); 31 | ASSERT_EQ(0, response.GetRange()[0]); 32 | ASSERT_EQ(255, response.GetRange()[1]); 33 | ASSERT_TRUE(response.InRange(0.0)); 34 | ASSERT_TRUE(response.InRange(127.5)); 35 | ASSERT_TRUE(response.InRange(255.0)); 36 | ASSERT_FALSE(response.InRange(-0.01)); 37 | ASSERT_FALSE(response.InRange(+255.01)); 38 | 39 | response.SetRange(Eigen::Vector2d(100, 65535)); 40 | ASSERT_EQ(100, response.GetRange()[0]); 41 | ASSERT_EQ(65535, response.GetRange()[1]); 42 | ASSERT_TRUE(response.InRange(100.0)); 43 | ASSERT_TRUE(response.InRange(32717.5)); 44 | ASSERT_TRUE(response.InRange(65535.0)); 45 | ASSERT_FALSE(response.InRange(99.99)); 46 | ASSERT_FALSE(response.InRange(65535.01)); 47 | } 48 | 49 | TEST(LinearResponse, Params) 50 | { 51 | LinearResponse response; 52 | ASSERT_EQ(0, response.GetParams().size()); 53 | ASSERT_THROW(response.SetParams(Eigen::Vector2d()), Exception); 54 | } 55 | 56 | TEST(LinearResponse, Response) 57 | { 58 | LinearResponse response; 59 | double* params = nullptr; 60 | 61 | response.SetRange(0, 255); 62 | 63 | for (int i = 0; i < 255; ++i) 64 | { 65 | const double x = i; 66 | ASSERT_DOUBLE_EQ(x, response(x)); 67 | ASSERT_DOUBLE_EQ(x, LinearResponse::GetResponse(params, x)); 68 | } 69 | 70 | #ifndef NDEBUG 71 | ASSERT_THROW(response(-1), Exception); 72 | ASSERT_THROW(response(256), Exception); 73 | #endif 74 | 75 | response.SetRange(0, 1); 76 | 77 | for (int i = 0; i < 100; ++i) 78 | { 79 | const double x = i / 99.0; 80 | ASSERT_DOUBLE_EQ(x, response(x)); 81 | ASSERT_DOUBLE_EQ(x, LinearResponse::GetResponse(params, x)); 82 | } 83 | 84 | #ifndef NDEBUG 85 | ASSERT_THROW(response(-0.1), Exception); 86 | ASSERT_THROW(response(1.1), Exception); 87 | #endif 88 | } 89 | 90 | TEST(LinearResponse, Reset) 91 | { 92 | LinearResponse response; 93 | response.SetRange(0, 255); 94 | response.Reset(); 95 | 96 | for (int i = 0; i < 255; ++i) 97 | { 98 | ASSERT_DOUBLE_EQ(i, response(i)); 99 | } 100 | } 101 | 102 | } // namespace testing 103 | 104 | } // namespace calibu -------------------------------------------------------------------------------- /tests/tests.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | int main(int argc, char** argv) 4 | { 5 | testing::InitGoogleTest(&argc, argv); 6 | return RUN_ALL_TESTS(); 7 | } -------------------------------------------------------------------------------- /tests/vignetting_dense_test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | namespace calibu 5 | { 6 | namespace testing 7 | { 8 | 9 | TEST(DenseVignetting, Constructor) 10 | { 11 | { 12 | const int w = 640; 13 | const int h = 480; 14 | DenseVignetting vignetting(w, h); 15 | ASSERT_EQ(w, vignetting.Width()); 16 | ASSERT_EQ(h, vignetting.Height()); 17 | ASSERT_EQ("dense", vignetting.Type()); 18 | ASSERT_EQ(w * h, vignetting.NumParams()); 19 | ASSERT_EQ(w * h, vignetting.GetParams().size()); 20 | } 21 | 22 | { 23 | const int w = 320; 24 | const int h = 240; 25 | DenseVignetting vignetting(w, h); 26 | ASSERT_EQ(w, vignetting.Width()); 27 | ASSERT_EQ(h, vignetting.Height()); 28 | ASSERT_EQ("dense", vignetting.Type()); 29 | ASSERT_EQ(w * h, vignetting.NumParams()); 30 | ASSERT_EQ(w * h, vignetting.GetParams().size()); 31 | } 32 | } 33 | 34 | TEST(DenseVignetting, Params) 35 | { 36 | const int w = 320; 37 | const int h = 240; 38 | DenseVignetting vignetting(w, h); 39 | Eigen::VectorXd params = vignetting.GetParams(); 40 | 41 | ASSERT_EQ(w * h, params.size()); 42 | 43 | for (int i = 0; i < params.size(); ++i) 44 | { 45 | ASSERT_DOUBLE_EQ(1, params[i]); 46 | } 47 | 48 | for (int i = 0; i < params.size(); ++i) 49 | { 50 | params[i] = 0.1 * i; 51 | } 52 | 53 | vignetting.SetParams(params); 54 | params = vignetting.GetParams(); 55 | 56 | for (int i = 0; i < params.size(); ++i) 57 | { 58 | ASSERT_DOUBLE_EQ(0.1 * i, params[i]); 59 | } 60 | 61 | ASSERT_THROW(vignetting.SetParams(Eigen::Vector2d()), Exception); 62 | ASSERT_THROW(vignetting.SetParams(Eigen::Vector4d()), Exception); 63 | } 64 | 65 | TEST(DenseVignetting, Attenuation) 66 | { 67 | const int w = 320; 68 | const int h = 240; 69 | Eigen::VectorXd params(w * h); 70 | DenseVignetting vignetting(w, h); 71 | 72 | for (int i = 0; i < params.size(); ++i) 73 | { 74 | params[i] = 0.1 * i; 75 | } 76 | 77 | vignetting.SetParams(params); 78 | 79 | for (int y = 0; y < h; ++y) 80 | { 81 | for (int x = 0; x < w; ++x) 82 | { 83 | const double u = x + 0.5; 84 | const double v = y + 0.5; 85 | const int index = y * w + x; 86 | const double expected = params[index]; 87 | 88 | ASSERT_DOUBLE_EQ(expected, vignetting(u, v)); 89 | 90 | ASSERT_DOUBLE_EQ(expected, DenseVignetting::GetAttenuation( 91 | params.data(), u, v, w, h)); 92 | } 93 | } 94 | } 95 | 96 | TEST(DenseVignetting, Reset) 97 | { 98 | const int w = 320; 99 | const int h = 240; 100 | Eigen::VectorXd params(w * h); 101 | DenseVignetting vignetting(w, h); 102 | 103 | for (int i = 0; i < params.size(); ++i) 104 | { 105 | params[i] = 0.1 * i; 106 | } 107 | 108 | vignetting.SetParams(params); 109 | vignetting.Reset(); 110 | 111 | for (int y = 0; y < h; ++y) 112 | { 113 | for (int x = 0; x < w; ++x) 114 | { 115 | const double u = x + 0.5; 116 | const double v = y + 0.5; 117 | ASSERT_DOUBLE_EQ(1, vignetting(u, v)); 118 | } 119 | } 120 | 121 | DenseVignetting::ResetParameters(params.data(), w, h); 122 | 123 | for (int i = 0; i < params.size(); ++i) 124 | { 125 | ASSERT_DOUBLE_EQ(1, params[i]); 126 | } 127 | } 128 | 129 | } // namespace testing 130 | 131 | } -------------------------------------------------------------------------------- /tests/vignetting_poly_test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | namespace calibu 5 | { 6 | namespace testing 7 | { 8 | 9 | TEST(EvenPoly6Vignetting, Constructor) 10 | { 11 | { 12 | const int w = 640; 13 | const int h = 480; 14 | EvenPoly6Vignetting vignetting(w, h); 15 | ASSERT_EQ(w, vignetting.Width()); 16 | ASSERT_EQ(h, vignetting.Height()); 17 | ASSERT_EQ("epoly6", vignetting.Type()); 18 | ASSERT_EQ(3, vignetting.NumParams()); 19 | ASSERT_EQ(3, vignetting.GetParams().size()); 20 | } 21 | 22 | { 23 | const int w = 320; 24 | const int h = 240; 25 | EvenPoly6Vignetting vignetting(w, h); 26 | ASSERT_EQ(w, vignetting.Width()); 27 | ASSERT_EQ(h, vignetting.Height()); 28 | ASSERT_EQ("epoly6", vignetting.Type()); 29 | ASSERT_EQ(3, vignetting.NumParams()); 30 | ASSERT_EQ(3, vignetting.GetParams().size()); 31 | } 32 | } 33 | 34 | TEST(EvenPoly6Vignetting, Params) 35 | { 36 | const int w = 640; 37 | const int h = 480; 38 | EvenPoly6Vignetting vignetting(w, h); 39 | Eigen::VectorXd params = vignetting.GetParams(); 40 | 41 | ASSERT_EQ(3, params.size()); 42 | ASSERT_DOUBLE_EQ(0, params[0]); 43 | ASSERT_DOUBLE_EQ(0, params[1]); 44 | ASSERT_DOUBLE_EQ(0, params[2]); 45 | 46 | params = Eigen::Vector3d(0.5, -0.1, 2.1); 47 | vignetting.SetParams(params); 48 | params = vignetting.GetParams(); 49 | 50 | ASSERT_DOUBLE_EQ( 0.5, params[0]); 51 | ASSERT_DOUBLE_EQ(-0.1, params[1]); 52 | ASSERT_DOUBLE_EQ( 2.1, params[2]); 53 | 54 | ASSERT_THROW(vignetting.SetParams(Eigen::Vector2d()), Exception); 55 | ASSERT_THROW(vignetting.SetParams(Eigen::Vector4d()), Exception); 56 | } 57 | 58 | TEST(EvenPoly6Vignetting, Attenuation) 59 | { 60 | const int w = 320; 61 | const int h = 240; 62 | EvenPoly6Vignetting vignetting(w, h); 63 | Eigen::Vector3d params; 64 | 65 | double* c = params.data(); 66 | const Eigen::Vector2d size(w, h); 67 | const Eigen::Vector2d center = 0.5 * size; 68 | const double max_radius = center.norm(); 69 | 70 | params = Eigen::Vector3d(0.5, -0.1, 2.1); 71 | vignetting.SetParams(params); 72 | 73 | for (int y = 0; y < h; ++y) 74 | { 75 | for (int x = 0; x < w; ++x) 76 | { 77 | const double u = x + 0.5; 78 | const double v = y + 0.5; 79 | const Eigen::Vector2d point(u, v); 80 | const double radius = (point - center).norm(); 81 | const double ratio = radius / max_radius; 82 | 83 | const double x1 = ratio * ratio; 84 | const double x2 = x1 * x1; 85 | const double x3 = x2 * x1; 86 | const double expected = 1 + c[0] * x1 + c[1] * x2 + c[2] * x3; 87 | 88 | ASSERT_DOUBLE_EQ(expected, vignetting(u, v)); 89 | 90 | ASSERT_DOUBLE_EQ(expected, EvenPoly6Vignetting::GetAttenuation( 91 | c, u, v, w, h)); 92 | } 93 | } 94 | } 95 | 96 | TEST(EvenPoly6Vignetting, Reset) 97 | { 98 | const int w = 320; 99 | const int h = 240; 100 | EvenPoly6Vignetting vignetting(w, h); 101 | Eigen::Vector3d params(0.5, -0.1, 2.1); 102 | vignetting.SetParams(params); 103 | vignetting.Reset(); 104 | 105 | for (int y = 0; y < h; ++y) 106 | { 107 | for (int x = 0; x < w; ++x) 108 | { 109 | ASSERT_DOUBLE_EQ(1, vignetting(x, y)); 110 | } 111 | } 112 | 113 | EvenPoly6Vignetting::ResetParameters(params.data(), w, h); 114 | ASSERT_DOUBLE_EQ(0, params[0]); 115 | ASSERT_DOUBLE_EQ(0, params[1]); 116 | ASSERT_DOUBLE_EQ(0, params[2]); 117 | } 118 | 119 | } // namespace testing 120 | 121 | } // namespace calibu -------------------------------------------------------------------------------- /tests/vignetting_uniform_test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | namespace calibu 5 | { 6 | namespace testing 7 | { 8 | 9 | TEST(UniformVignetting, Constructor) 10 | { 11 | { 12 | const int w = 640; 13 | const int h = 480; 14 | UniformVignetting vignetting(w, h); 15 | ASSERT_EQ(w, vignetting.Width()); 16 | ASSERT_EQ(h, vignetting.Height()); 17 | ASSERT_EQ("uniform", vignetting.Type()); 18 | ASSERT_EQ(0, vignetting.NumParams()); 19 | ASSERT_EQ(0, vignetting.GetParams().size()); 20 | } 21 | 22 | { 23 | const int w = 320; 24 | const int h = 240; 25 | UniformVignetting vignetting(w, h); 26 | ASSERT_EQ(w, vignetting.Width()); 27 | ASSERT_EQ(h, vignetting.Height()); 28 | ASSERT_EQ("uniform", vignetting.Type()); 29 | ASSERT_EQ(0, vignetting.NumParams()); 30 | ASSERT_EQ(0, vignetting.GetParams().size()); 31 | } 32 | } 33 | 34 | TEST(UniformVignetting, Params) 35 | { 36 | const int w = 640; 37 | const int h = 480; 38 | UniformVignetting vignetting(w, h); 39 | ASSERT_EQ(0, vignetting.GetParams().size()); 40 | ASSERT_THROW(vignetting.SetParams(Eigen::Vector2d()), Exception); 41 | } 42 | 43 | TEST(UniformVignetting, Attenuation) 44 | { 45 | const int w = 320; 46 | const int h = 240; 47 | UniformVignetting vignetting(w, h); 48 | const double* params = nullptr; 49 | 50 | for (int y = 0; y < h; ++y) 51 | { 52 | for (int x = 0; x < w; ++x) 53 | { 54 | ASSERT_DOUBLE_EQ(1, vignetting(x, y)); 55 | 56 | ASSERT_DOUBLE_EQ(1, UniformVignetting::GetAttenuation( 57 | params, x, y, w, h)); 58 | } 59 | } 60 | } 61 | 62 | TEST(UniformVignetting, Reset) 63 | { 64 | const int w = 320; 65 | const int h = 240; 66 | UniformVignetting vignetting(w, h); 67 | 68 | for (int y = 0; y < h; ++y) 69 | { 70 | for (int x = 0; x < w; ++x) 71 | { 72 | ASSERT_DOUBLE_EQ(1, vignetting(x, y)); 73 | } 74 | } 75 | } 76 | 77 | } // namespace testing 78 | 79 | } // namespace calibu --------------------------------------------------------------------------------