├── .github └── workflows │ ├── macos-dep-src.yml │ └── ubuntu-dep-src.yml ├── .gitignore ├── CMakeLists.txt ├── LICENSE ├── README.md ├── doc ├── Doxyfile.cmake ├── image │ ├── img-initClickTemplateTracker.png │ ├── img-teabox-cao.jpg │ ├── img-teabox-click.jpg │ └── img-template-tracker.jpg ├── mainpage.doc.cmake ├── references.bib ├── tutorial-homography-opencv.doc ├── tutorial-homography-visp.doc ├── tutorial-pose-dementhon-opencv.doc ├── tutorial-pose-dementhon-visp.doc ├── tutorial-pose-dlt-opencv.doc ├── tutorial-pose-dlt-planar-opencv.doc ├── tutorial-pose-dlt-planar-visp.doc ├── tutorial-pose-dlt-visp.doc ├── tutorial-pose-gauss-newton-opencv.doc ├── tutorial-pose-gauss-newton-visp.doc ├── tutorial-pose-mbt-visp.doc └── tutorial-template-matching-visp.doc ├── opencv ├── CMakeLists.txt ├── homography-basis │ ├── CMakeLists.txt │ ├── homography-dlt-opencv.cpp │ └── pose-from-homography-dlt-opencv.cpp └── pose-basis │ ├── CMakeLists.txt │ ├── pose-dementhon-opencv.cpp │ ├── pose-dlt-opencv.cpp │ └── pose-gauss-newton-opencv.cpp └── visp ├── CMakeLists.txt ├── homography-basis ├── CMakeLists.txt ├── homography-dlt-visp.cpp └── pose-from-homography-dlt-visp.cpp ├── pose-basis ├── CMakeLists.txt ├── pose-dementhon-visp.cpp ├── pose-dlt-visp.cpp └── pose-gauss-newton-visp.cpp ├── pose-mbt ├── CMakeLists.txt ├── pose-mbt-visp.cpp ├── teabox.cao ├── teabox.init ├── teabox.mpg ├── teabox.ppm ├── teabox.wrl └── teabox.xml └── template-matching ├── CMakeLists.txt ├── bruegel.mpg └── template-matching-visp.cpp /.github/workflows/macos-dep-src.yml: -------------------------------------------------------------------------------- 1 | name: MacOS-dep-sec 2 | 3 | # https://www.jeffgeerling.com/blog/2020/running-github-actions-workflow-on-schedule-and-other-events 4 | on: 5 | push: 6 | pull_request: 7 | schedule: 8 | - cron: '0 2 * * SUN' 9 | 10 | jobs: 11 | build-macos-dep-sec: 12 | runs-on: ${{ matrix.os }} 13 | strategy: 14 | fail-fast: false 15 | matrix: 16 | os: [macos-10.15, macos-11.0] 17 | 18 | steps: 19 | # https://github.com/marketplace/actions/cancel-workflow-action 20 | - name: Cancel Previous Runs 21 | uses: styfle/cancel-workflow-action@0.5.0 22 | with: 23 | access_token: ${{ github.token }} 24 | 25 | - name: Checkout repository 26 | uses: actions/checkout@v2 27 | 28 | - name: Print system information 29 | run: sysctl -a | grep machdep.cpu 30 | 31 | - name: Print OS information 32 | run: system_profiler SPSoftwareDataType 33 | 34 | - name: Install dependencies 35 | run: | 36 | brew update && brew upgrade 37 | brew install libpng libjpeg libdc1394 lapack openblas eigen opencv doxygen 38 | 39 | # Openblas location is exported explicitly because openblas is keg-only, 40 | # which means it was not symlinked into /usr/local/. 41 | - name: Clone and configure ViSP 42 | run: | 43 | git clone --depth 1 https://github.com/lagadic/visp.git ${HOME}/visp 44 | cd ${HOME}/visp 45 | export LDFLAGS="-L/usr/local/opt/openblas/lib" 46 | export CPPFLAGS="-I/usr/local/opt/openblas/include" 47 | mkdir build && cd build 48 | cmake .. -DCMAKE_FIND_FRAMEWORK=LAST -DBUILD_DEMOS=OFF -DBUILD_EXAMPLES=OFF -DBUILD_TESTS=OFF -DBUILD_TUTORIALS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=$(pwd)/install 49 | cat ViSP-third-party.txt 50 | 51 | - name: Build and install ViSP 52 | run: | 53 | cd ${HOME}/visp/build 54 | make -j2 install 55 | echo "VISP_DIR=$(pwd)/install" >> $GITHUB_ENV 56 | echo $VISP_DIR 57 | 58 | - name: Configure CMake and build camera_localization 59 | run: | 60 | mkdir build && cd build 61 | cmake .. -DCMAKE_BUILD_TYPE=Release 62 | make -j2 63 | make doc 64 | -------------------------------------------------------------------------------- /.github/workflows/ubuntu-dep-src.yml: -------------------------------------------------------------------------------- 1 | name: Ubuntu-dep-src 2 | 3 | # https://www.jeffgeerling.com/blog/2020/running-github-actions-workflow-on-schedule-and-other-events 4 | on: 5 | push: 6 | pull_request: 7 | schedule: 8 | - cron: '0 2 * * SUN' 9 | 10 | jobs: 11 | build-ubuntu-dep-src: 12 | runs-on: ${{ matrix.os }} 13 | strategy: 14 | fail-fast: false 15 | matrix: 16 | os: [ubuntu-16.04, ubuntu-18.04, ubuntu-20.04] 17 | 18 | steps: 19 | # https://github.com/marketplace/actions/cancel-workflow-action 20 | - name: Cancel Previous Runs 21 | uses: styfle/cancel-workflow-action@0.5.0 22 | with: 23 | access_token: ${{ github.token }} 24 | 25 | - name: Checkout repository 26 | uses: actions/checkout@v2 27 | 28 | - name: Print system information 29 | run: lscpu 30 | 31 | - name: Print OS information 32 | run: lsb_release -a 33 | 34 | - name: Install dependencies 35 | run: | 36 | sudo apt-get update && sudo apt-get install -y libx11-dev libdc1394-22-dev libv4l-dev gfortran liblapack-dev libopenblas-dev libeigen3-dev libopencv-dev 37 | sudo apt-get update && sudo apt-get install -y doxygen graphviz texlive-latex-base texlive-latex-extra 38 | 39 | - name: Build ViSP from source 40 | run: | 41 | git clone --depth 1 https://github.com/lagadic/visp.git ${HOME}/visp 42 | cd ${HOME}/visp 43 | mkdir build && cd build && mkdir install 44 | cmake .. -DBUILD_DEMOS=OFF -DBUILD_EXAMPLES=OFF -DBUILD_TESTS=OFF -DBUILD_TUTORIALS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=$(pwd)/install 45 | cat ViSP-third-party.txt 46 | make -j2 install 47 | echo "VISP_DIR=$(pwd)/install" >> $GITHUB_ENV 48 | echo $VISP_DIR 49 | 50 | - name: Configure CMake and build camera_localization 51 | run: | 52 | mkdir build && cd build 53 | cmake .. -DCMAKE_BUILD_TYPE=Release 54 | make -j2 55 | make doc 56 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | *~ 2 | */CMakeFiles 3 | CMakeCache.txt 4 | .DS_Store 5 | CMakeLists.txt.user* 6 | build 7 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10) 2 | project(camera-localization) 3 | 4 | option(WITH_OPENCV "Implementation using OpenCV structures" ON) 5 | option(WITH_VISP "Implementation using OpenCV structures" ON) 6 | 7 | #---------------------------------------------------------------------- 8 | # Try to find doxygen for documentation generation 9 | # Use "make doc" target to generate the documentation 10 | #---------------------------------------------------------------------- 11 | find_package(Doxygen) 12 | if(DOXYGEN_FOUND) 13 | configure_file(doc/Doxyfile.cmake 14 | ${PROJECT_BINARY_DIR}/Doxyfile 15 | @ONLY ) 16 | 17 | configure_file(doc/mainpage.doc.cmake 18 | ${PROJECT_BINARY_DIR}/doc/mainpage.doc 19 | @ONLY ) 20 | 21 | add_custom_target(doc ${DOXYGEN_EXECUTABLE} ${PROJECT_BINARY_DIR}/Doxyfile) 22 | endif() 23 | 24 | #---------------------------------------------------------------------- 25 | # Propagate in sub directories 26 | #---------------------------------------------------------------------- 27 | if(WITH_VISP) 28 | add_subdirectory(visp) 29 | endif() 30 | 31 | if(WITH_OPENCV) 32 | add_subdirectory(opencv) 33 | endif() 34 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | By downloading, copying, installing or using the software you agree to this license. 2 | If you do not agree to this license, do not download, install, 3 | copy or use the software. 4 | 5 | 6 | License Agreement 7 | For Open Source Computer Vision Library 8 | (3-clause BSD License) 9 | 10 | Copyright (C) 2000-2015, Intel Corporation, all rights reserved. 11 | Copyright (C) 2009-2011, Willow Garage Inc., all rights reserved. 12 | Copyright (C) 2009-2015, NVIDIA Corporation, all rights reserved. 13 | Copyright (C) 2010-2013, Advanced Micro Devices, Inc., all rights reserved. 14 | Copyright (C) 2015, OpenCV Foundation, all rights reserved. 15 | Copyright (C) 2015, Itseez Inc., all rights reserved. 16 | Third party copyrights are property of their respective owners. 17 | 18 | Redistribution and use in source and binary forms, with or without modification, 19 | are permitted provided that the following conditions are met: 20 | 21 | * Redistributions of source code must retain the above copyright notice, 22 | this list of conditions and the following disclaimer. 23 | 24 | * Redistributions in binary form must reproduce the above copyright notice, 25 | this list of conditions and the following disclaimer in the documentation 26 | and/or other materials provided with the distribution. 27 | 28 | * Neither the names of the copyright holders nor the names of the contributors 29 | may be used to endorse or promote products derived from this software 30 | without specific prior written permission. 31 | 32 | This software is provided by the copyright holders and contributors "as is" and 33 | any express or implied warranties, including, but not limited to, the implied 34 | warranties of merchantability and fitness for a particular purpose are disclaimed. 35 | In no event shall copyright holders or contributors be liable for any direct, 36 | indirect, incidental, special, exemplary, or consequential damages 37 | (including, but not limited to, procurement of substitute goods or services; 38 | loss of use, data, or profits; or business interruption) however caused 39 | and on any theory of liability, whether in contract, strict liability, 40 | or tort (including negligence or otherwise) arising in any way out of 41 | the use of this software, even if advised of the possibility of such damage. 42 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | camera_localization 2 | =================== 3 | 4 | The aim of this project is to provide researchers and practitioners with an almost comprehensive and consolidate 5 | introduction to effective tools to facilitate research in augmented reality. It is also dedicated to academics involved in teaching augmented reality at the undergraduate and graduate level. 6 | 7 | For most of the presented approaches presented in the paper [[pdf](http://rainbow-doc.irisa.fr/pdf/2016_ieeetvcg_marchand.pdf)]: 8 | 9 | E. Marchand, H. Uchiyama, F. Spindler. Pose estimation for augmented reality: a hands-on survey. IEEE Trans. on Visualization and Computer Graphics, 22(12):2633-2651, December 2016. 10 | 11 | we provide the code of short examples. This should allow developers to easily bridge the gap between theoretical aspects and practice. These examples have been written using either OpenCV or ViSP developed at Inria. 12 | 13 | 14 | The full documentation of this project is available from . 15 | -------------------------------------------------------------------------------- /doc/Doxyfile.cmake: -------------------------------------------------------------------------------- 1 | # Doxyfile 1.8.6 2 | 3 | #--------------------------------------------------------------------------- 4 | # Project related configuration options 5 | #--------------------------------------------------------------------------- 6 | DOXYFILE_ENCODING = UTF-8 7 | PROJECT_NAME = "Pose estimation for augmented reality" 8 | PROJECT_NUMBER = 9 | PROJECT_BRIEF = 10 | PROJECT_LOGO = 11 | OUTPUT_DIRECTORY = 12 | CREATE_SUBDIRS = NO 13 | OUTPUT_LANGUAGE = English 14 | BRIEF_MEMBER_DESC = YES 15 | REPEAT_BRIEF = YES 16 | ABBREVIATE_BRIEF = 17 | ALWAYS_DETAILED_SEC = NO 18 | INLINE_INHERITED_MEMB = NO 19 | FULL_PATH_NAMES = YES 20 | STRIP_FROM_PATH = 21 | STRIP_FROM_INC_PATH = 22 | SHORT_NAMES = NO 23 | JAVADOC_AUTOBRIEF = NO 24 | QT_AUTOBRIEF = NO 25 | MULTILINE_CPP_IS_BRIEF = NO 26 | INHERIT_DOCS = YES 27 | SEPARATE_MEMBER_PAGES = NO 28 | TAB_SIZE = 4 29 | ALIASES = 30 | TCL_SUBST = 31 | OPTIMIZE_OUTPUT_FOR_C = NO 32 | OPTIMIZE_OUTPUT_JAVA = NO 33 | OPTIMIZE_FOR_FORTRAN = NO 34 | OPTIMIZE_OUTPUT_VHDL = NO 35 | EXTENSION_MAPPING = 36 | MARKDOWN_SUPPORT = YES 37 | AUTOLINK_SUPPORT = YES 38 | BUILTIN_STL_SUPPORT = NO 39 | CPP_CLI_SUPPORT = NO 40 | SIP_SUPPORT = NO 41 | IDL_PROPERTY_SUPPORT = YES 42 | DISTRIBUTE_GROUP_DOC = NO 43 | SUBGROUPING = YES 44 | INLINE_GROUPED_CLASSES = NO 45 | INLINE_SIMPLE_STRUCTS = NO 46 | TYPEDEF_HIDES_STRUCT = NO 47 | LOOKUP_CACHE_SIZE = 0 48 | #--------------------------------------------------------------------------- 49 | # Build related configuration options 50 | #--------------------------------------------------------------------------- 51 | EXTRACT_ALL = NO 52 | EXTRACT_PRIVATE = NO 53 | EXTRACT_PACKAGE = NO 54 | EXTRACT_STATIC = NO 55 | EXTRACT_LOCAL_CLASSES = YES 56 | EXTRACT_LOCAL_METHODS = NO 57 | EXTRACT_ANON_NSPACES = NO 58 | HIDE_UNDOC_MEMBERS = NO 59 | HIDE_UNDOC_CLASSES = NO 60 | HIDE_FRIEND_COMPOUNDS = NO 61 | HIDE_IN_BODY_DOCS = NO 62 | INTERNAL_DOCS = NO 63 | CASE_SENSE_NAMES = YES 64 | HIDE_SCOPE_NAMES = NO 65 | SHOW_INCLUDE_FILES = YES 66 | SHOW_GROUPED_MEMB_INC = NO 67 | FORCE_LOCAL_INCLUDES = NO 68 | INLINE_INFO = YES 69 | SORT_MEMBER_DOCS = YES 70 | SORT_BRIEF_DOCS = NO 71 | SORT_MEMBERS_CTORS_1ST = NO 72 | SORT_GROUP_NAMES = NO 73 | SORT_BY_SCOPE_NAME = NO 74 | STRICT_PROTO_MATCHING = NO 75 | GENERATE_TODOLIST = YES 76 | GENERATE_TESTLIST = YES 77 | GENERATE_BUGLIST = YES 78 | GENERATE_DEPRECATEDLIST= YES 79 | ENABLED_SECTIONS = 80 | MAX_INITIALIZER_LINES = 30 81 | SHOW_USED_FILES = YES 82 | SHOW_FILES = YES 83 | SHOW_NAMESPACES = YES 84 | FILE_VERSION_FILTER = 85 | LAYOUT_FILE = 86 | CITE_BIB_FILES = "@PROJECT_SOURCE_DIR@/doc/references.bib" 87 | #--------------------------------------------------------------------------- 88 | # Configuration options related to warning and progress messages 89 | #--------------------------------------------------------------------------- 90 | QUIET = NO 91 | WARNINGS = YES 92 | WARN_IF_UNDOCUMENTED = YES 93 | WARN_IF_DOC_ERROR = YES 94 | WARN_NO_PARAMDOC = NO 95 | WARN_FORMAT = "$file:$line: $text" 96 | WARN_LOGFILE = warning.log 97 | #--------------------------------------------------------------------------- 98 | # Configuration options related to the input files 99 | #--------------------------------------------------------------------------- 100 | INPUT = @PROJECT_SOURCE_DIR@/visp/pose-basis \ 101 | @PROJECT_SOURCE_DIR@/visp/homography-basis \ 102 | @PROJECT_SOURCE_DIR@/visp/pose-mbt \ 103 | @PROJECT_SOURCE_DIR@/visp/template-matching \ 104 | @PROJECT_SOURCE_DIR@/opencv/pose-basis \ 105 | @PROJECT_SOURCE_DIR@/opencv/homography-basis \ 106 | @PROJECT_SOURCE_DIR@/doc \ 107 | @PROJECT_BINARY_DIR@/doc 108 | INPUT_ENCODING = UTF-8 109 | FILE_PATTERNS = *.cpp *.doc 110 | RECURSIVE = YES 111 | EXCLUDE = 112 | EXCLUDE_SYMLINKS = NO 113 | EXCLUDE_PATTERNS = 114 | EXCLUDE_SYMBOLS = 115 | EXAMPLE_PATH = @PROJECT_SOURCE_DIR@/visp/pose-basis \ 116 | @PROJECT_SOURCE_DIR@/visp/homography-basis \ 117 | @PROJECT_SOURCE_DIR@/visp/pose-mbt \ 118 | @PROJECT_SOURCE_DIR@/visp/template-matching \ 119 | @PROJECT_SOURCE_DIR@/opencv/pose-basis \ 120 | @PROJECT_SOURCE_DIR@/opencv/homography-basis 121 | EXAMPLE_PATTERNS = *.cpp 122 | EXAMPLE_RECURSIVE = YES 123 | IMAGE_PATH = @PROJECT_SOURCE_DIR@/doc/image 124 | INPUT_FILTER = 125 | FILTER_PATTERNS = 126 | FILTER_SOURCE_FILES = NO 127 | FILTER_SOURCE_PATTERNS = 128 | USE_MDFILE_AS_MAINPAGE = 129 | #--------------------------------------------------------------------------- 130 | # Configuration options related to source browsing 131 | #--------------------------------------------------------------------------- 132 | SOURCE_BROWSER = NO 133 | INLINE_SOURCES = NO 134 | STRIP_CODE_COMMENTS = YES 135 | REFERENCED_BY_RELATION = NO 136 | REFERENCES_RELATION = NO 137 | REFERENCES_LINK_SOURCE = YES 138 | SOURCE_TOOLTIPS = YES 139 | USE_HTAGS = NO 140 | VERBATIM_HEADERS = YES 141 | #--------------------------------------------------------------------------- 142 | # Configuration options related to the alphabetical class index 143 | #--------------------------------------------------------------------------- 144 | ALPHABETICAL_INDEX = YES 145 | COLS_IN_ALPHA_INDEX = 5 146 | IGNORE_PREFIX = 147 | #--------------------------------------------------------------------------- 148 | # Configuration options related to the HTML output 149 | #--------------------------------------------------------------------------- 150 | GENERATE_HTML = YES 151 | HTML_OUTPUT = doc/html 152 | HTML_FILE_EXTENSION = .html 153 | HTML_HEADER = 154 | HTML_FOOTER = 155 | HTML_STYLESHEET = 156 | HTML_EXTRA_STYLESHEET = 157 | HTML_EXTRA_FILES = 158 | HTML_COLORSTYLE_HUE = 220 159 | HTML_COLORSTYLE_SAT = 100 160 | HTML_COLORSTYLE_GAMMA = 80 161 | HTML_TIMESTAMP = YES 162 | HTML_DYNAMIC_SECTIONS = NO 163 | HTML_INDEX_NUM_ENTRIES = 100 164 | GENERATE_DOCSET = NO 165 | DOCSET_FEEDNAME = "Doxygen generated docs" 166 | DOCSET_BUNDLE_ID = org.doxygen.Project 167 | DOCSET_PUBLISHER_ID = org.doxygen.Publisher 168 | DOCSET_PUBLISHER_NAME = Publisher 169 | GENERATE_HTMLHELP = NO 170 | CHM_FILE = 171 | HHC_LOCATION = 172 | GENERATE_CHI = NO 173 | CHM_INDEX_ENCODING = 174 | BINARY_TOC = NO 175 | TOC_EXPAND = NO 176 | GENERATE_QHP = NO 177 | QCH_FILE = 178 | QHP_NAMESPACE = org.doxygen.Project 179 | QHP_VIRTUAL_FOLDER = doc 180 | QHP_CUST_FILTER_NAME = 181 | QHP_CUST_FILTER_ATTRS = 182 | QHP_SECT_FILTER_ATTRS = 183 | QHG_LOCATION = 184 | GENERATE_ECLIPSEHELP = NO 185 | ECLIPSE_DOC_ID = org.doxygen.Project 186 | DISABLE_INDEX = NO 187 | GENERATE_TREEVIEW = NO 188 | ENUM_VALUES_PER_LINE = 4 189 | TREEVIEW_WIDTH = 250 190 | EXT_LINKS_IN_WINDOW = NO 191 | FORMULA_FONTSIZE = 10 192 | FORMULA_TRANSPARENT = YES 193 | USE_MATHJAX = NO 194 | MATHJAX_FORMAT = HTML-CSS 195 | MATHJAX_RELPATH = http://cdn.mathjax.org/mathjax/latest 196 | MATHJAX_EXTENSIONS = 197 | MATHJAX_CODEFILE = 198 | SEARCHENGINE = YES 199 | SERVER_BASED_SEARCH = NO 200 | EXTERNAL_SEARCH = NO 201 | SEARCHENGINE_URL = 202 | SEARCHDATA_FILE = searchdata.xml 203 | EXTERNAL_SEARCH_ID = 204 | EXTRA_SEARCH_MAPPINGS = 205 | #--------------------------------------------------------------------------- 206 | # Configuration options related to the LaTeX output 207 | #--------------------------------------------------------------------------- 208 | GENERATE_LATEX = YES 209 | LATEX_OUTPUT = latex 210 | LATEX_CMD_NAME = latex 211 | MAKEINDEX_CMD_NAME = makeindex 212 | COMPACT_LATEX = NO 213 | PAPER_TYPE = a4 214 | EXTRA_PACKAGES = 215 | LATEX_HEADER = 216 | LATEX_FOOTER = 217 | LATEX_EXTRA_FILES = 218 | PDF_HYPERLINKS = YES 219 | USE_PDFLATEX = YES 220 | LATEX_BATCHMODE = NO 221 | LATEX_HIDE_INDICES = NO 222 | LATEX_SOURCE_CODE = NO 223 | LATEX_BIB_STYLE = plain 224 | #--------------------------------------------------------------------------- 225 | # Configuration options related to the RTF output 226 | #--------------------------------------------------------------------------- 227 | GENERATE_RTF = NO 228 | RTF_OUTPUT = rtf 229 | COMPACT_RTF = NO 230 | RTF_HYPERLINKS = NO 231 | RTF_STYLESHEET_FILE = 232 | RTF_EXTENSIONS_FILE = 233 | #--------------------------------------------------------------------------- 234 | # Configuration options related to the man page output 235 | #--------------------------------------------------------------------------- 236 | GENERATE_MAN = NO 237 | MAN_OUTPUT = man 238 | MAN_EXTENSION = .3 239 | MAN_LINKS = NO 240 | #--------------------------------------------------------------------------- 241 | # Configuration options related to the XML output 242 | #--------------------------------------------------------------------------- 243 | GENERATE_XML = NO 244 | XML_OUTPUT = xml 245 | XML_SCHEMA = 246 | XML_DTD = 247 | XML_PROGRAMLISTING = YES 248 | #--------------------------------------------------------------------------- 249 | # Configuration options related to the DOCBOOK output 250 | #--------------------------------------------------------------------------- 251 | GENERATE_DOCBOOK = NO 252 | DOCBOOK_OUTPUT = docbook 253 | #--------------------------------------------------------------------------- 254 | # Configuration options for the AutoGen Definitions output 255 | #--------------------------------------------------------------------------- 256 | GENERATE_AUTOGEN_DEF = NO 257 | #--------------------------------------------------------------------------- 258 | # Configuration options related to the Perl module output 259 | #--------------------------------------------------------------------------- 260 | GENERATE_PERLMOD = NO 261 | PERLMOD_LATEX = NO 262 | PERLMOD_PRETTY = YES 263 | PERLMOD_MAKEVAR_PREFIX = 264 | #--------------------------------------------------------------------------- 265 | # Configuration options related to the preprocessor 266 | #--------------------------------------------------------------------------- 267 | ENABLE_PREPROCESSING = YES 268 | MACRO_EXPANSION = NO 269 | EXPAND_ONLY_PREDEF = NO 270 | SEARCH_INCLUDES = YES 271 | INCLUDE_PATH = 272 | INCLUDE_FILE_PATTERNS = 273 | PREDEFINED = 274 | EXPAND_AS_DEFINED = 275 | SKIP_FUNCTION_MACROS = YES 276 | #--------------------------------------------------------------------------- 277 | # Configuration options related to external references 278 | #--------------------------------------------------------------------------- 279 | TAGFILES = 280 | GENERATE_TAGFILE = 281 | ALLEXTERNALS = NO 282 | EXTERNAL_GROUPS = YES 283 | EXTERNAL_PAGES = YES 284 | PERL_PATH = /usr/bin/perl 285 | #--------------------------------------------------------------------------- 286 | # Configuration options related to the dot tool 287 | #--------------------------------------------------------------------------- 288 | CLASS_DIAGRAMS = YES 289 | MSCGEN_PATH = 290 | DIA_PATH = 291 | HIDE_UNDOC_RELATIONS = YES 292 | HAVE_DOT = NO 293 | DOT_NUM_THREADS = 0 294 | DOT_FONTNAME = Helvetica 295 | DOT_FONTSIZE = 10 296 | DOT_FONTPATH = 297 | CLASS_GRAPH = YES 298 | COLLABORATION_GRAPH = YES 299 | GROUP_GRAPHS = YES 300 | UML_LOOK = NO 301 | UML_LIMIT_NUM_FIELDS = 10 302 | TEMPLATE_RELATIONS = NO 303 | INCLUDE_GRAPH = YES 304 | INCLUDED_BY_GRAPH = YES 305 | CALL_GRAPH = NO 306 | CALLER_GRAPH = NO 307 | GRAPHICAL_HIERARCHY = YES 308 | DIRECTORY_GRAPH = YES 309 | DOT_IMAGE_FORMAT = png 310 | INTERACTIVE_SVG = NO 311 | DOT_PATH = 312 | DOTFILE_DIRS = 313 | MSCFILE_DIRS = 314 | DIAFILE_DIRS = 315 | DOT_GRAPH_MAX_NODES = 50 316 | MAX_DOT_GRAPH_DEPTH = 0 317 | DOT_TRANSPARENT = NO 318 | DOT_MULTI_TARGETS = YES 319 | GENERATE_LEGEND = YES 320 | DOT_CLEANUP = YES 321 | -------------------------------------------------------------------------------- /doc/image/img-initClickTemplateTracker.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lagadic/camera_localization/d4197a8320c974e51f47cb7c7adb4dc681c805d9/doc/image/img-initClickTemplateTracker.png -------------------------------------------------------------------------------- /doc/image/img-teabox-cao.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lagadic/camera_localization/d4197a8320c974e51f47cb7c7adb4dc681c805d9/doc/image/img-teabox-cao.jpg -------------------------------------------------------------------------------- /doc/image/img-teabox-click.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lagadic/camera_localization/d4197a8320c974e51f47cb7c7adb4dc681c805d9/doc/image/img-teabox-click.jpg -------------------------------------------------------------------------------- /doc/image/img-template-tracker.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lagadic/camera_localization/d4197a8320c974e51f47cb7c7adb4dc681c805d9/doc/image/img-template-tracker.jpg -------------------------------------------------------------------------------- /doc/mainpage.doc.cmake: -------------------------------------------------------------------------------- 1 | /*! 2 | 3 | \mainpage Pose estimation for augmented reality: a hands-on survey 4 | 5 | \tableofcontents 6 | 7 | \section intro_sec Introduction 8 | 9 | Augmented Reality (AR) has now progressed to the point where real-time applications are being considered and needed. At the same time it is important that synthetic elements are rendered and aligned in the scene in an accurate and visually acceptable way. In order to address these issues, real-time, robust and efficient tracking algorithms have to be considered. The tracking of objects in the scene amounts to calculating the location (or pose) between the camera and the scene. 10 | 11 | In the paper [pdf]: 12 | 13 | \code 14 | E. Marchand, H. Uchiyama and F. Spindler. Pose estimation for augmented reality: 15 | a hands-on survey. IEEE Trans. on Visualization and Computer Graphics, 22(12):2633-2651, December 2016. 16 | \endcode 17 | 18 | a brief but almost self contented introduction to the most important approaches dedicated to camera localization along with a survey of the extension that have been proposed in the recent years. We also try to link these methodological concepts to the main libraries and SDK available on the market. 19 | 20 | The aim of this paper is then to provide researchers and practitioners with an almost comprehensive and consolidate 21 | introduction to effective tools to facilitate research in augmented reality. It is also dedicated to academics involved in teaching augmented reality at the undergraduate and graduate level. 22 | 23 | For most of the presented approaches, we also provide links to code of short examples. This should allow readers to easily bridge the gap between theoretical aspects and practice. These examples have been written using OpenCV but also ViSP developed at Inria. 24 | This page contains the documentation of these documented source code proposed as a supplementary material of the paper. 25 | 26 | We hope this article and source code will be accessible and interesting to experts and students alike. 27 | 28 | \section install_sec Installation 29 | 30 | \subsection prereq_subsec Prerequisities 31 | 32 | The source code available from http://github.com/lagadic/camera_localization was designed to work with OpenCV or with ViSP. During CMake configuration the user can choose which of these two 3rd parties are used. 33 | Prior to build this project, the user has to download and install OpenCV and/or ViSP. 34 | For example under Ubuntu OpenCV installation is done using: 35 | \code 36 | sudo apt-get libopencv-dev 37 | \endcode 38 | while ViSP installation is performed using: 39 | \code 40 | sudo apt-get libvisp-dev 41 | \endcode 42 | 43 | 44 | \subsection build_subsec How to build 45 | 46 | Once ViSP is installed, download the lastest source code release from github . 47 | 48 | Unzip the archive: 49 | \code 50 | $ unzip camera_localization-2.0.1.zip 51 | \endcode 52 | 53 | or extract the code from tarball: 54 | \code 55 | $ tar xvzf camera_localization-2.0.1.tar.gz 56 | \endcode 57 | 58 | 59 | Using cmake run: 60 | 61 | \code 62 | $ cd camera_localization 63 | $ cmake . 64 | $ make 65 | \endcode 66 | 67 | To generate the documentation you can run: 68 | \code 69 | $ make doc 70 | \endcode 71 | 72 | \section pose_3d_model_sec Pose estimation relying on a 3D model 73 | 74 | \subsection pose_known_model_sec Pose estimation from a known model 75 | 76 | In this section we give base algorithm for camera localization. 77 | 78 | - Pose from Direct Linear Transform method using \ref tutorial-pose-dlt-opencv "OpenCV" or using \ref tutorial-pose-dlt-visp "ViSP"
In this first tutorial a simple solution known as Direct Linear Transform (DLT) \cite HZ01 \cite Sut74 based on the resolution of a linear system is considered to estimate the pose of the camera from at least 6 non coplanar points. 79 | 80 | - Pose from Dementhon's POSIT method using \ref tutorial-pose-dementhon-opencv "OpenCV" or using \ref tutorial-pose-dementhon-visp "ViSP"
In this second tutorial we give Dementhon's POSIT method \cite DD95 \cite ODD96 used to estimate the pose based on the resolution of a linear system introducing additional constraints on the rotation matrix. The pose is estimated from at least 4 non coplanar points. 81 | 82 | - Pose from homography estimation using \ref tutorial-pose-dlt-planar-opencv "OpenCV" or using \ref tutorial-pose-dlt-planar-visp "ViSP"
In this tutorial we explain how to decompose the homography to estimate the pose from at least 4 coplanar points. 83 | 84 | - Pose from a non-linear minimization method using \ref tutorial-pose-gauss-newton-opencv "OpenCV" or using \ref tutorial-pose-gauss-newton-visp "ViSP"
In this other tutorial we give a non-linear minimization method to estimate the pose from at least 4 points. This method requires an initialization of the pose to estimate. Depending on the points planarity, this initialization could be performed using one of the previous pose algorithms. 85 | 86 | \subsection pose_mbt_sec Extension to markerless model-based tracking 87 | 88 | - Pose from markerless model-based tracking using \ref tutorial-pose-mbt-visp "ViSP"
This tutorial focuses on markerless model-based tracking that allows to estimate the pose of the camera. 89 | 90 | \section motion_estimation_sec Pose estimation relying on an image model 91 | 92 | \subsection homography_from_point_sec Homography estimation 93 | 94 | - Homography estimation using \ref tutorial-homography-opencv "OpenCV" or using \ref tutorial-homography-visp "ViSP"
In this tutorial we describe the estimation of an homography using Direct Linear Transform (DLT) algorithm. At least 4 coplanar points are requested to achieve the estimation. 95 | 96 | \subsection homography_from_template_tracker_sec Direct motion estimation through template matching 97 | 98 | - Direct motion estimation through template matching using \ref tutorial-template-matching-visp "ViSP"
In this other tutorial we propose a direct motion estimation through template matching approach. Here the reference template should be planar. 99 | 100 | 101 | 102 | */ 103 | 104 | -------------------------------------------------------------------------------- /doc/references.bib: -------------------------------------------------------------------------------- 1 | @article{Sut74, 2 | title={Three-dimensional data input by tablet}, 3 | author={Sutherland, I.E}, 4 | journal={Proceedings of the IEEE}, 5 | volume={62}, 6 | number={4}, 7 | pages={453--461}, 8 | year={1974}, 9 | month = apr 10 | } 11 | 12 | @Book{HZ01, 13 | author = { Hartley, R. and Zisserman,A.}, 14 | title = { Multiple View Geometry in Computer Vision }, 15 | publisher = {Cambridge University Press}, 16 | year = 2001 17 | } 18 | 19 | @Article{DD95, 20 | author = {Dementhon, D. and Davis, L.}, 21 | title = {Model-Based Object Pose in 25 Lines of Codes}, 22 | journal = {Int. J. of Computer Vision}, 23 | year = 1995, 24 | volume = 15, 25 | pages = {123--141}, 26 | keyword = {pose} 27 | } 28 | 29 | @article{ODD96, 30 | AUTHOR = {Oberkampf, D. and Dementhon, D.F. and Davis, L.S.}, 31 | TITLE = {Iterative Pose Estimation Using Coplanar Feature 32 | Points}, 33 | JOURNAL = {Computer Vision and Image Understanding}, 34 | VOLUME = 63, 35 | YEAR = 1996, 36 | NUMBER = 3, 37 | MONTH = may, 38 | PAGES = {495-511} 39 | } 40 | 41 | @article{CH06, 42 | Author = {Chaumette, F. and Hutchinson, S.}, 43 | Title = {Visual Servo Control, {P}art~{I}: Basic Approaches}, 44 | Journal = {IEEE Robotics and Automation Magazine}, 45 | Volume = { 13}, 46 | Number = 4, 47 | Pages = {82--90}, 48 | Month = {December}, 49 | Year = 2006 50 | } 51 | 52 | @article{Baker04a, 53 | author = {Baker, S. and Matthews, I.}, 54 | title = { Lucas-kanade 20 years on: A unifying framework}, 55 | journal = {Int. Journal of Computer Vision}, 56 | year = 2004, 57 | volume = 56, 58 | number = 3, 59 | pages = {221-255} 60 | } 61 | @inproceedings{Irani98a, 62 | title = {Robust multi-sensor image alignment}, 63 | author = {Irani, M. and Anandan, P.}, 64 | booktitle = {IEEE Int. Conf. on Computer Vision, ICCV'98}, 65 | pages = {959-966}, 66 | address = {Bombay, India}, 67 | year = 1998 68 | } -------------------------------------------------------------------------------- /doc/tutorial-homography-opencv.doc: -------------------------------------------------------------------------------- 1 | /** 2 | 3 | \page tutorial-homography-opencv Homography estimation 4 | \tableofcontents 5 | 6 | \section intro_homography_cv Introduction 7 | 8 | The estimation of an homography from coplanar points can be easily and precisely achieved using a Direct Linear Transform algorithm \cite HZ01 \cite Sut74 based on the resolution of a linear system. 9 | 10 | \section homography_code_cv Source code 11 | 12 | The following source code that uses OpenCV is also available in \ref homography-dlt-opencv.cpp file. It allows to estimate the homography between matched coplanar points. At least 4 points are required. 13 | 14 | \include homography-dlt-opencv.cpp 15 | 16 | \section homography_explained_cv Source code explained 17 | 18 | First of all we inlude OpenCV headers that are requested to manipulate vectors and matrices. 19 | 20 | \snippet homography-dlt-opencv.cpp Include 21 | 22 | Then we introduce the function that does the homography estimation. 23 | \snippet homography-dlt-opencv.cpp Estimation function 24 | 25 | From a vector of planar points \f${\bf x_1} = (x_1, y_1, 1)^T\f$ in image \f$I_1\f$ and a vector of matched points \f${\bf x_2} = (x_2, y_2, 1)^T\f$ in image \f$I_2\f$ it allows to estimate the homography \f$\bf {^2}H_1\f$: 26 | 27 | \f[\bf x_2 = {^2}H_1 x_1\f] 28 | 29 | The implementation of the Direct Linear Transform algorithm to estimate \f$\bf {^2}H_1\f$ is done next. First, for each point we update the values of matrix A using equation (23). Then we solve the system \f${\bf Ah}=0\f$ using a Singular Value Decomposition of \f$\bf A\f$. Finaly, we determine the smallest eigen value that allows to identify the eigen vector that corresponds to the solution \f$\bf h\f$. 30 | 31 | \snippet homography-dlt-opencv.cpp DLT 32 | 33 | From now the resulting eigen vector \f$\bf h\f$ that corresponds to the minimal 34 | eigen value of matrix \f$\bf A\f$ is used to update the homography \f$\bf {^2}H_1\f$. 35 | 36 | \snippet homography-dlt-opencv.cpp Update homography matrix 37 | 38 | Finally we define the main function in which we will initialize the input data before calling the previous function. 39 | 40 | \snippet homography-dlt-opencv.cpp Main function 41 | 42 | First in the main we create the data structures that will contain the 3D points coordinates \e wX in the world frame, their coordinates in the camera frame 1 \e c1X and 2 \e c2X and their coordinates in the image plane \e x1 and \e x2 obtained after perspective projection. Note here that at least 4 coplanar points are requested to estimate the 8 parameters of the homography. 43 | 44 | \snippet homography-dlt-opencv.cpp Create data structures 45 | 46 | For our simulation we then initialize the input data from a ground truth pose that corresponds to the pose of the camera in frame 1 with respect to the object frame; in \e c1tw_truth for the translation vector and in \e c1Rw_truth for the rotation matrix. 47 | For each point, we compute their coordinates in the camera frame 1 \e c1X = (c1X, c1Y, c1Z). These values are then used to compute their coordinates in the image plane \e x1 = (x1, y1) using perspective projection. 48 | 49 | Thanks to the ground truth transformation between camera frame 2 and camera frame 1 set in \e c2tc1 for the translation vector and in \e c2Rc1 for the rotation matrix, we compute also the coordinates of the points in camera frame 2 \e c2X = (c2X, c2Y, c2Z) and their corresponding coordinates \e x2 = (x2, y2) in the image plane. 50 | \snippet homography-dlt-opencv.cpp Simulation 51 | 52 | From here we have initialized \f${\bf x_1} = (x1,y1,1)^T\f$ and \f${\bf x_2} = (x2,y2,1)^T\f$. We are now ready to call the function that does the homography estimation. 53 | 54 | \snippet homography-dlt-opencv.cpp Call function 55 | 56 | \section homography_result_cv Resulting homography estimation 57 | 58 | If you run the previous code, it we produce the following result that shows that the estimated pose is equal to the ground truth one used to generate the input data: 59 | 60 | \code 61 | 2H1 (computed with DLT): 62 | [0.5425233873981674, -0.04785624324415742, 0.03308292557420141; 63 | 0.0476448024215215, 0.5427592708789931, 0.005830349194436123; 64 | -0.02550335176952741, -0.005978041062955012, 0.6361649706821216] 65 | \endcode 66 | */ 67 | -------------------------------------------------------------------------------- /doc/tutorial-homography-visp.doc: -------------------------------------------------------------------------------- 1 | /** 2 | 3 | \page tutorial-homography-visp Homography estimation 4 | \tableofcontents 5 | 6 | \section intro_homography Introduction 7 | 8 | The estimation of an homography from coplanar points can be easily and precisely achieved using a Direct Linear Transform algorithm \cite HZ01 \cite Sut74 based on the resolution of a linear system. 9 | 10 | \section homography_code Source code 11 | 12 | The following source code that uses ViSP is also available in \ref homography-dlt-visp.cpp file. It allows to estimate the homography between matched coplanar points. At least 4 points are required. 13 | 14 | \include homography-dlt-visp.cpp 15 | 16 | \section homography_explained Source code explained 17 | 18 | First of all we inlude ViSP headers that are requested to manipulate vectors and matrices. 19 | 20 | \snippet homography-dlt-visp.cpp Include 21 | 22 | Then we introduce the function that does the homography estimation. 23 | \snippet homography-dlt-visp.cpp Estimation function 24 | 25 | From a vector of planar points \f${\bf x_1} = (x_1, y_1, 1)^T\f$ in image \f$I_1\f$ and a vector of matched points \f${\bf x_2} = (x_2, y_2, 1)^T\f$ in image \f$I_2\f$ it allows to estimate the homography \f$\bf {^2}H_1\f$: 26 | 27 | \f[\bf x_2 = {^2}H_1 x_1\f] 28 | 29 | The implementation of the Direct Linear Transform algorithm to estimate \f$\bf {^2}H_1\f$ is done next. First, for each point we update the values of matrix A using equation (23). Then we solve the system \f${\bf Ah}=0\f$ using a Singular Value Decomposition of \f$\bf A\f$. Finaly, we determine the smallest eigen value that allows to identify the eigen vector that corresponds to the solution \f$\bf h\f$. 30 | 31 | \snippet homography-dlt-visp.cpp DLT 32 | 33 | From now the resulting eigen vector \f$\bf h\f$ that corresponds to the minimal 34 | eigen value of matrix \f$\bf A\f$ is used to update the homography \f$\bf {^2}H_1\f$. 35 | 36 | \snippet homography-dlt-visp.cpp Update homography matrix 37 | 38 | Finally we define the main function in which we will initialize the input data before calling the previous function. 39 | 40 | \snippet homography-dlt-visp.cpp Main function 41 | 42 | First in the main we create the data structures that will contain the 3D points coordinates \e wX in the world frame, their coordinates in the camera frame 1 \e c1X and 2 \e c2X and their coordinates in the image plane \e x1 and \e x2 obtained after perspective projection. Note here that at least 4 coplanar points are requested to estimate the 8 parameters of the homography. 43 | 44 | \snippet homography-dlt-visp.cpp Create data structures 45 | 46 | For our simulation we then initialize the input data from a ground truth pose \e c1Tw_truth that corresponds to the pose of the camera in frame 1 with respect to the object frame. 47 | For each point, we compute their coordinates in the camera frame 1 \e c1X = (c1X, c1Y, c1Z). These values are then used to compute their coordinates in the image plane \e x1 = (x1, y1) using perspective projection. 48 | 49 | Thanks to the ground truth transformation \e c2Tc1 between camera frame 2 and camera frame 1, we compute also the coordinates of the points in camera frame 2 \e c2X = (c2X, c2Y, c2Z) and their corresponding coordinates \e x2 = (x2, y2) in the image plane. 50 | \snippet homography-dlt-visp.cpp Simulation 51 | 52 | From here we have initialized \f${\bf x_1} = (x1,y1,1)^T\f$ and \f${\bf x_2} = (x2,y2,1)^T\f$. We are now ready to call the function that does the homography estimation. 53 | 54 | \snippet homography-dlt-visp.cpp Call function 55 | 56 | \section homography_result Resulting homography estimation 57 | 58 | If you run the previous code, it we produce the following result that shows that the estimated pose is equal to the ground truth one used to generate the input data: 59 | 60 | \code 61 | 2H1 (computed with DLT): 62 | -0.5425233874 0.04785624324 -0.03308292557 63 | -0.04764480242 -0.5427592709 -0.005830349194 64 | 0.02550335177 0.005978041063 -0.6361649707 65 | \endcode 66 | */ 67 | -------------------------------------------------------------------------------- /doc/tutorial-pose-dementhon-opencv.doc: -------------------------------------------------------------------------------- 1 | /** 2 | 3 | \page tutorial-pose-dementhon-opencv Pose from Dementhon's POSIT method 4 | \tableofcontents 5 | 6 | \section intro_dementhon_cv Introduction 7 | 8 | An alternative and very elegant solution to pose estimation from points has been proposed in \cite DD95 \cite ODD96. This algorithm is called POSIT. 9 | 10 | \section dementhon_code_cv Source code 11 | 12 | The following source code that uses OpenCV is also available in \ref pose-dementhon-opencv.cpp file. It allows to compute the pose of the camera from points. 13 | 14 | \include pose-dementhon-opencv.cpp 15 | 16 | \section dementon_explained_cv Source code explained 17 | 18 | First of all we include OpenCV headers that are requested to manipulate vectors and matrices. 19 | 20 | \snippet pose-dementhon-opencv.cpp Include 21 | 22 | Then we introduce the function that does the pose estimation. It takes as input parameters \f${^w}{\bf X} = (X,Y,Z,1)^T\f$ the 3D coordinates of the points in the world frame and \f${\bf x} = (x,y,1)^T\f$ their normalized coordinates in the image plane. It returns the estimated pose in \e ctw for the translation vector and in \e cRw for the rotation matrix. 23 | 24 | \snippet pose-dementhon-opencv.cpp Estimation function 25 | 26 | The implementation of the POSIT algorithm is done next. 27 | \snippet pose-dementhon-opencv.cpp POSIT 28 | 29 | After a minimal number of iterations, all the parameters are estimated and can be used to update the value of the homogenous transformation, first in \e ctw for the translation, then in \e cRw for the rotation matrix. 30 | 31 | \snippet pose-dementhon-opencv.cpp Update homogeneous matrix 32 | 33 | Finally we define the main function in which we will initialize the input data before calling the previous function and computing the pose using Dementhon POSIT algorithm. 34 | 35 | \snippet pose-dementhon-opencv.cpp Main function 36 | 37 | First in the main we create the data structures that will contain the 3D points coordinates \e wX in the world frame, and their coordinates in the image plane \e x obtained after prerspective projection. Note here that at least 4 non coplanar points are requested to estimate the pose. 38 | 39 | \snippet pose-dementhon-opencv.cpp Create data structures 40 | 41 | For our simulation we then initialize the input data from a ground truth pose with the translation in \e ctw_truth and the rotation matrix in \e cRw_truth. 42 | For each point we set in \e wX[i] the 3D coordinates in the world frame (wX, wY, wZ, 1) and compute in \e cX their 3D coordinates (cX, cY, cZ, 1) in the camera frame. Then in \e x[i] we update their coordinates (x, y) in the image plane, obtained by perspective projection. 43 | 44 | \snippet pose-dementhon-opencv.cpp Simulation 45 | 46 | From here we have initialized \f${^w}{\bf X} = (X,Y,Z,1)^T\f$ and \f${\bf x} = (x,y,1)^T\f$. We are now ready to call the function that does the pose estimation. 47 | 48 | \snippet pose-dementhon-opencv.cpp Call function 49 | 50 | \section dementhon_result_cv Resulting pose estimation 51 | 52 | If you run the previous code, it we produce the following result that shows that the estimated pose is very close to the ground truth one used to generate the input data: 53 | 54 | \code 55 | ctw (ground truth): 56 | [-0.1; 57 | 0.1; 58 | 0.5] 59 | ctw (computed with DLT): 60 | [-0.1070274014258891; 61 | 0.1741233539654255; 62 | 0.5236967119016803] 63 | cRw (ground truth): 64 | [0.7072945483755065, -0.7061704379962989, 0.03252282795827704; 65 | 0.7061704379962989, 0.7036809008245869, -0.07846338199958876; 66 | 0.03252282795827704, 0.07846338199958876, 0.9963863524490802] 67 | cRw (computed with DLT): 68 | [0.6172726698299887, -0.7813181606576134, 0.09228425059326956; 69 | 0.6906596219977137, 0.4249461799313228, -0.5851581245302429; 70 | 0.4179788297943932, 0.4249391234325819, 0.8019325685199985] 71 | \endcode 72 | 73 | */ 74 | -------------------------------------------------------------------------------- /doc/tutorial-pose-dementhon-visp.doc: -------------------------------------------------------------------------------- 1 | /** 2 | 3 | \page tutorial-pose-dementhon-visp Pose from Dementhon's POSIT method 4 | \tableofcontents 5 | 6 | \section intro_dementhon Introduction 7 | 8 | An alternative and very elegant solution to pose estimation from points has been proposed in \cite DD95 \cite ODD96. This algorithm is called POSIT. 9 | 10 | \section dementhon_code Source code 11 | 12 | The following source code that uses ViSP is also available in \ref pose-dementhon-visp.cpp file. It allows to compute the pose of the camera from points. 13 | 14 | \include pose-dementhon-visp.cpp 15 | 16 | \section dementon_explained Source code explained 17 | 18 | First of all we include ViSP headers that are requested to manipulate vectors and matrices. 19 | 20 | \snippet pose-dementhon-visp.cpp Include 21 | 22 | Then we introduce the function that does the pose estimation. It takes as input parameters \f${^w}{\bf X} = (X,Y,Z,1)^T\f$ the 3D coordinates of the points in the world frame and \f${\bf x} = (x,y,1)^T\f$ their normalized coordinates in the image plane. It returns the estimated pose as an homogeneous matrix transformation. 23 | 24 | \snippet pose-dementhon-visp.cpp Estimation function 25 | 26 | The implementation of the POSIT algorithm is done next. 27 | \snippet pose-dementhon-visp.cpp POSIT 28 | 29 | After a minimal number of iterations, all the parameters are estimated and can be used to update the value of the homogenous transformation. 30 | 31 | \snippet pose-dementhon-visp.cpp Update homogeneous matrix 32 | 33 | Finally we define the main function in which we will initialize the input data before calling the previous function and computing the pose using Dementhon POSIT algorithm. 34 | 35 | \snippet pose-dementhon-visp.cpp Main function 36 | 37 | First in the main we create the data structures that will contain the 3D points coordinates \e wX in the world frame, and their coordinates in the image plane \e x obtained after prerspective projection. Note here that at least 4 non coplanar points are requested to estimate the pose. 38 | 39 | \snippet pose-dementhon-visp.cpp Create data structures 40 | 41 | For our simulation we then initialize the input data from a ground truth pose \e cTw_truth. 42 | For each point we set in \e wX[i] the 3D coordinates in the world frame (wX, wY, wZ, 1) and compute in \e cX their 3D coordinates (cX, cY, cZ, 1) in the camera frame. Then in \e x[i] we update their coordinates (x, y) in the image plane, obtained by perspective projection. 43 | 44 | \snippet pose-dementhon-visp.cpp Simulation 45 | 46 | From here we have initialized \f${^w}{\bf X} = (X,Y,Z,1)^T\f$ and \f${\bf x} = (x,y,1)^T\f$. We are now ready to call the function that does the pose estimation. 47 | 48 | \snippet pose-dementhon-visp.cpp Call function 49 | 50 | \section dementhon_result Resulting pose estimation 51 | 52 | If you run the previous code, it we produce the following result that shows that the estimated pose is very close to the ground truth one used to generate the input data: 53 | 54 | \code 55 | cTw (ground truth): 56 | 0.7072945484 -0.706170438 0.03252282796 -0.1 57 | 0.706170438 0.7036809008 -0.078463382 0.1 58 | 0.03252282796 0.078463382 0.9963863524 0.5 59 | 0 0 0 1 60 | cTw (from Dementhon): 61 | 0.6172726698 -0.7813181607 0.09228425059 -0.1070274014 62 | 0.690659622 0.4249461799 -0.5851581245 0.174123354 63 | 0.4179788298 0.4249391234 0.8019325685 0.5236967119 64 | 0 0 0 1 65 | \endcode 66 | 67 | */ 68 | -------------------------------------------------------------------------------- /doc/tutorial-pose-dlt-opencv.doc: -------------------------------------------------------------------------------- 1 | /** 2 | 3 | \page tutorial-pose-dlt-opencv Pose from Direct Linear Transform method 4 | \tableofcontents 5 | 6 | \section intro_pose_dlt_cv Introduction 7 | 8 | Although PnP is intrinsically a non-linear problem, a simple solution known as Direct Linear Transform (DLT) \cite HZ01 \cite Sut74 based on the resolution of a linear system can be considered. 9 | 10 | \section dlt_code_cv Source code 11 | 12 | The following source code that uses OpenCV is also available in \ref pose-dlt-opencv.cpp file. It allows to compute the pose of the camera from points. 13 | 14 | \include pose-dlt-opencv.cpp 15 | 16 | \section dlt_explained_cv Source code explained 17 | 18 | First of all we include the header of the files that are requested to manipulate OpenCV vectors and matrices. 19 | 20 | \snippet pose-dlt-opencv.cpp Include 21 | 22 | Then we introduce the function that does the pose estimation. It takes as input parameters \f${^w}{\bf X} = (X,Y,Z,1)^T\f$ the 3D coordinates of the points in the world frame and \f${\bf x} = (x,y,1)^T\f$ their normalized coordinates in the image plane. It returns the estimated pose in \e ctw for the translation vector and in \e cRw for the rotation matrix. 23 | 24 | \snippet pose-dlt-opencv.cpp Estimation function 25 | 26 | The implementation of the Direct Linear Transform algorithm is done next. First, for each point we update the values of matrix A using equation (5). Then we solve the system Ah=0 using a Singular Value Decomposition of A. Finaly, we determine the smallest eigen value that allows to identify the eigen vector that corresponds to the solution. 27 | 28 | \snippet pose-dlt-opencv.cpp DLT 29 | 30 | From now the resulting eigen vector h that corresponds to the minimal 31 | eigen value of matrix A is used to update the translation vector \e ctw an the rotation matrix \e cRw. 32 | 33 | \snippet pose-dlt-opencv.cpp Update homogeneous matrix 34 | 35 | Finally we define the main function in which we will initialize the input data before calling the previous function and computing the pose using DLT algorithm. 36 | 37 | \snippet pose-dlt-opencv.cpp Main function 38 | 39 | First in the main we create the data structures that will contain the 3D points coordinates \e wX in the world frame, their coordinates in the image plane \e x obtained after prerspective projection. Note here that at least 4 non coplanar points are requested to estimate the pose. 40 | 41 | \snippet pose-dlt-opencv.cpp Create data structures 42 | 43 | For our simulation we then initialize the input data from a ground truth pose with the translation in \e ctw_truth and the rotation matrix in \e cRw_truth. 44 | For each point we set in \e wX[i] the 3D coordinates in the world frame (wX, wY, wZ, 1) and compute in \e cX their 3D coordinates (cX, cY, cZ, 1) in the camera frame. Then in \e x[i] we update their coordinates (x, y) in the image plane, obtained by perspective projection. 45 | 46 | \snippet pose-dlt-opencv.cpp Simulation 47 | 48 | From here we have initialized \f${^w}{\bf X} = (X,Y,Z,1)^T\f$ and \f${\bf x} = (x,y,1)^T\f$. We are now ready to call the function that does the pose estimation. 49 | 50 | \snippet pose-dlt-opencv.cpp Call function 51 | 52 | \section dlt_result_cv Resulting pose estimation 53 | 54 | If you run the previous code, it we produce the following result that shows that the estimated pose is equal to the ground truth one used to generate the input data: 55 | 56 | \code 57 | ctw (ground truth): 58 | [-0.1; 59 | 0.1; 60 | 1.2] 61 | ctw (computed with DLT): 62 | [-0.09999999999999899; 63 | 0.09999999999999894; 64 | 1.199999999999986] 65 | cRw (ground truth): 66 | [0.7072945483755065, -0.7061704379962989, 0.03252282795827704; 67 | 0.7061704379962989, 0.7036809008245869, -0.07846338199958876; 68 | 0.03252282795827704, 0.07846338199958876, 0.9963863524490802] 69 | cRw (computed with DLT): 70 | [0.7072945483754999, -0.7061704379962919, 0.03252282795827634; 71 | 0.7061704379962918, 0.70368090082458, -0.07846338199958748; 72 | 0.03252282795827235, 0.07846338199958545, 0.9963863524490808] 73 | \endcode 74 | 75 | */ 76 | -------------------------------------------------------------------------------- /doc/tutorial-pose-dlt-planar-opencv.doc: -------------------------------------------------------------------------------- 1 | /** 2 | 3 | \page tutorial-pose-dlt-planar-opencv Pose from homography estimation 4 | \tableofcontents 5 | 6 | \section intro_pose_dlt_cv_planar Introduction 7 | 8 | The homography can be decomposed to retrieve the pose. We consider here that all the points lie in the plane \f${^w}Z=0\f$. 9 | 10 | \section dlt_planar_code_cv Source code 11 | 12 | The following source code that uses OpenCV is also available in \ref pose-from-homography-dlt-opencv.cpp file. It allows to compute the pose of the camera from at least 4 coplanar points. 13 | 14 | \include pose-from-homography-dlt-opencv.cpp 15 | 16 | \section dlt_planar_explained_cv Source code explained 17 | 18 | First of all we include OpenCV headers that are requested to manipulate vectors and matrices. 19 | 20 | \snippet pose-from-homography-dlt-opencv.cpp Include 21 | 22 | Then we introduce the function that does the homography estimation from coplanar points. This function is detailed in \ref tutorial-homography-opencv. 23 | 24 | \snippet pose-from-homography-dlt-opencv.cpp Homography DLT function 25 | 26 | Then we introduce the function that does the pose from homography estimation. 27 | \snippet pose-from-homography-dlt-opencv.cpp Estimation function 28 | 29 | Based on equation (27) \f${\bf x}_0 = {^0}{\bf H}_w {\bf x}_w\f$ we first estimate the homography \f${^0}{\bf H}_w\f$. 30 | \snippet pose-from-homography-dlt-opencv.cpp Homography estimation 31 | 32 | Then using the constraint that \f$||{\bf c}^0_1|| = 1\f$ we normalize the homography. 33 | 34 | \snippet pose-from-homography-dlt-opencv.cpp Homography normalization 35 | 36 | Let us denote \f${\bf M} = {\Pi}^{-1} \; {^0}{\bf H}_w\f$ 37 | 38 | Noting that matrix M is also equal to \f${\bf M} = ({\bf c}^0_1, {\bf c}^0_2, {^0}{\bf T}_w)\f$ we are able to extract the corresponding vectors. 39 | 40 | \snippet pose-from-homography-dlt-opencv.cpp Extract c1, c2 41 | 42 | The third column of the rotation matrix is computed such as \f${\bf c}^0_3= {\bf c}^0_1 \times {\bf c}^0_2\f$ 43 | 44 | \snippet pose-from-homography-dlt-opencv.cpp Compute c3 45 | 46 | To finish we update the homogeneous transformation that corresponds to the estimated pose \f${^0}{\bf T}_w\f$. 47 | 48 | \snippet pose-from-homography-dlt-opencv.cpp Update pose 49 | 50 | Finally we define the main function in which we will initialize the input data before calling the previous function and computing the pose from the estimated homography. 51 | 52 | \snippet pose-from-homography-dlt-opencv.cpp Main function 53 | 54 | Then we create the data structures that will contain the 3D points coordinates \e wX in the world frame, their normalized coordinates \e xw in the world frame and their normalized coordinates \e xo in the image plane obtained by perspective projection. Note here that at least 4 coplanar points are requested to estimate the 8 parameters of the homography. 55 | 56 | \snippet pose-from-homography-dlt-opencv.cpp Create data structures 57 | 58 | For our simulation we then initialize the input data from a ground truth pose with the translation in \e ctw_truth and the rotation matrix in \e cRw_truth. 59 | For each point \e wX[i] we compute the perspective projection \e xo[i] = (xo, yo, 1). According to equation (27) we also set \f${\bf x}_w = ({^w}X, {^w}Y, 1)\f$ in \e xw vector. 60 | \snippet pose-from-homography-dlt-opencv.cpp Simulation 61 | 62 | From here we have initialized \f${\bf x_0} = (x_0,y_0,1)^T\f$ and \f${\bf x}_w = ({^w}X, {^w}Y, 1)^T\f$. We are now ready to call the function that does the pose estimation. 63 | 64 | \snippet pose-from-homography-dlt-opencv.cpp Call function 65 | 66 | \section dlt_planar_result_cv Resulting pose estimation 67 | 68 | If you run the previous code, it we produce the following result that shows that the estimated pose is equal to the ground truth one used to generate the input data: 69 | 70 | \code 71 | otw (ground truth): 72 | [-0.1; 73 | 0.1; 74 | 1.2] 75 | otw (computed with homography DLT): 76 | [-0.1; 77 | 0.09999999999999999; 78 | 1.2] 79 | oRw (ground truth): 80 | [0.7072945483755065, -0.7061704379962989, 0.03252282795827704; 81 | 0.7061704379962989, 0.7036809008245869, -0.07846338199958876; 82 | 0.03252282795827704, 0.07846338199958876, 0.9963863524490802] 83 | oRw (computed with homography DLT): 84 | [0.7072945483755065, -0.7061704379962993, 0.03252282795827707; 85 | 0.7061704379962989, 0.7036809008245873, -0.07846338199958841; 86 | 0.03252282795827677, 0.07846338199958854, 0.996386352449081] 87 | \endcode 88 | 89 | */ 90 | -------------------------------------------------------------------------------- /doc/tutorial-pose-dlt-planar-visp.doc: -------------------------------------------------------------------------------- 1 | /** 2 | 3 | \page tutorial-pose-dlt-planar-visp Pose from homography estimation 4 | \tableofcontents 5 | 6 | \section intro_pose_dlt_planar Introduction 7 | 8 | The homography can be decomposed to retrieve the pose. We consider here that all the points lie in the plane \f${^w}Z=0\f$. 9 | 10 | \section dlt_planar_code Source code 11 | 12 | The following source code that uses ViSP is also available in \ref pose-from-homography-dlt-visp.cpp file. It allows to compute the pose of the camera from at least 4 coplanar points. 13 | 14 | \include pose-from-homography-dlt-visp.cpp 15 | 16 | \section dlt_planar_explained Source code explained 17 | 18 | First of all we include ViSP headers that are requested to manipulate vectors and matrices. 19 | 20 | \snippet pose-from-homography-dlt-visp.cpp Include 21 | 22 | Then we introduce the function that does the homography estimation from coplanar points. This function is detailed in \ref tutorial-homography-visp. 23 | 24 | \snippet pose-from-homography-dlt-visp.cpp Homography DLT function 25 | 26 | Then we introduce the function that does the pose from homography estimation. 27 | \snippet pose-from-homography-dlt-visp.cpp Estimation function 28 | 29 | Based on equation (27) \f${\bf x}_0 = {^0}{\bf H}_w {\bf x}_w\f$ we first estimate the homography \f${^0}{\bf H}_w\f$. 30 | \snippet pose-from-homography-dlt-visp.cpp Homography estimation 31 | 32 | Then using the constraint that \f$||{\bf c}^0_1|| = 1\f$ we normalize the homography. 33 | 34 | \snippet pose-from-homography-dlt-visp.cpp Homography normalization 35 | 36 | Let us denote \f${\bf M} = {\Pi}^{-1} \; {^0}{\bf H}_w\f$ 37 | 38 | Noting that matrix M is also equal to \f${\bf M} = ({\bf c}^0_1, {\bf c}^0_2, {^0}{\bf T}_w)\f$ we are able to extract the corresponding vectors. 39 | 40 | \snippet pose-from-homography-dlt-visp.cpp Extract c1, c2 41 | 42 | The third column of the rotation matrix is computed such as \f${\bf c}^0_3= {\bf c}^0_1 \times {\bf c}^0_2\f$ 43 | 44 | \snippet pose-from-homography-dlt-visp.cpp Compute c3 45 | 46 | To finish we update the homogeneous transformation that corresponds to the estimated pose \f${^0}{\bf T}_w\f$. 47 | 48 | \snippet pose-from-homography-dlt-visp.cpp Update pose 49 | 50 | Finally we define the main function in which we will initialize the input data before calling the previous function and computing the pose from the estimated homography. 51 | 52 | \snippet pose-from-homography-dlt-visp.cpp Main function 53 | 54 | Then we create the data structures that will contain the 3D points coordinates \e wX in the world frame, their normalized coordinates \e xw in the world frame and their normalized coordinates \e xo in the image plane obtained by perspective projection. Note here that at least 4 coplanar points are requested to estimate the 8 parameters of the homography. 55 | 56 | \snippet pose-from-homography-dlt-visp.cpp Create data structures 57 | 58 | For our simulation we then initialize the input data from a ground truth pose \e oTw_truth. 59 | For each point \e wX[i] we compute the perspective projection \e xo[i] = (xo, yo, 1). According to equation (27) we also set \f${\bf x}_w = ({^w}X, {^w}Y, 1)\f$ in \e xw vector. 60 | \snippet pose-from-homography-dlt-visp.cpp Simulation 61 | 62 | From here we have initialized \f${\bf x_0} = (x_0,y_0,1)^T\f$ and \f${\bf x}_w = ({^w}X, {^w}Y, 1)^T\f$. We are now ready to call the function that does the pose estimation. 63 | 64 | \snippet pose-from-homography-dlt-visp.cpp Call function 65 | 66 | \section dlt_planar_result Resulting pose estimation 67 | 68 | If you run the previous code, it we produce the following result that shows that the estimated pose is equal to the ground truth one used to generate the input data: 69 | 70 | \code 71 | oTw (ground truth): 72 | 0.7072945484 -0.706170438 0.03252282796 -0.1 73 | 0.706170438 0.7036809008 -0.078463382 0.1 74 | 0.03252282796 0.078463382 0.9963863524 1.2 75 | 0 0 0 1 76 | oTw (computed with homography DLT): 77 | 0.7072945484 -0.706170438 0.03252282796 -0.1 78 | 0.706170438 0.7036809008 -0.078463382 0.1 79 | 0.03252282796 0.078463382 0.9963863524 1.2 80 | 0 0 0 1 81 | \endcode 82 | 83 | */ 84 | -------------------------------------------------------------------------------- /doc/tutorial-pose-dlt-visp.doc: -------------------------------------------------------------------------------- 1 | /** 2 | 3 | \page tutorial-pose-dlt-visp Pose from Direct Linear Transform method 4 | \tableofcontents 5 | 6 | \section intro_pose_dlt Introduction 7 | 8 | Although PnP is intrinsically a non-linear problem, a simple solution known as Direct Linear Transform (DLT) \cite HZ01 \cite Sut74 based on the resolution of a linear system can be considered. 9 | 10 | \section dlt_code Source code 11 | 12 | The following source code that uses ViSP is also available in \ref pose-dlt-visp.cpp file. It allows to compute the pose of the camera from points. 13 | 14 | \include pose-dlt-visp.cpp 15 | 16 | \section dlt_explained Source code explained 17 | 18 | First of all we include the header of the files that are requested to manipulate ViSP vectors and matrices. 19 | 20 | \snippet pose-dlt-visp.cpp Include 21 | 22 | Then we introduce the function that does the pose estimation. It takes as input parameters \f${^w}{\bf X} = (X,Y,Z,1)^T\f$ the 3D coordinates of the points in the world frame and \f${\bf x} = (x,y,1)^T\f$ their normalized coordinates in the image plane. It returns the estimated pose as an homogeneous matrix transformation. 23 | 24 | \snippet pose-dlt-visp.cpp Estimation function 25 | 26 | The implementation of the Direct Linear Transform algorithm is done next. First, for each point we update the values of matrix A using equation (5). Then we solve the system Ah=0 using a Singular Value Decomposition of A. Finaly, we determine the smallest eigen value that allows to identify the eigen vector that corresponds to the solution. 27 | 28 | \snippet pose-dlt-visp.cpp DLT 29 | 30 | From now the resulting eigen vector h that corresponds to the minimal 31 | eigen value of matrix A is used to update the homogenous transformation \e cTw: 32 | 33 | \snippet pose-dlt-visp.cpp Update homogeneous matrix 34 | 35 | Finally we define the main function in which we will initialize the input data before calling the previous function and computing the pose using DLT algorithm. 36 | 37 | \snippet pose-dlt-visp.cpp Main function 38 | 39 | First in the main we create the data structures that will contain the 3D points coordinates \e wX in the world frame, their coordinates in the image plane \e x obtained after prerspective projection. Note here that at least 4 non coplanar points are requested to estimate the pose. 40 | 41 | \snippet pose-dlt-visp.cpp Create data structures 42 | 43 | For our simulation we then initialize the input data from a ground truth pose \e cTw_truth. 44 | For each point we set in \e wX[i] the 3D coordinates in the world frame (wX, wY, wZ, 1) and compute in \e cX their 3D coordinates (cX, cY, cZ, 1) in the camera frame. Then in \e x[i] we update their coordinates (x, y) in the image plane, obtained by perspective projection. 45 | 46 | \snippet pose-dlt-visp.cpp Simulation 47 | 48 | From here we have initialized \f${^w}{\bf X} = (X,Y,Z,1)^T\f$ and \f${\bf x} = (x,y,1)^T\f$. We are now ready to call the function that does the pose estimation. 49 | 50 | \snippet pose-dlt-visp.cpp Call function 51 | 52 | \section dlt_result Resulting pose estimation 53 | 54 | If you run the previous code, it we produce the following result that shows that the estimated pose is equal to the ground truth one used to generate the input data: 55 | 56 | \code 57 | cTw (ground truth): 58 | 0.7072945484 -0.706170438 0.03252282796 0.1 59 | 0.706170438 0.7036809008 -0.078463382 0.1 60 | 0.03252282796 0.078463382 0.9963863524 1.2 61 | 0 0 0 1 62 | cTw (computed with DLT): 63 | 0.7072945484 -0.706170438 0.03252282796 0.1 64 | 0.706170438 0.7036809008 -0.078463382 0.1 65 | 0.03252282796 0.078463382 0.9963863524 1.2 66 | 0 0 0 1 67 | \endcode 68 | 69 | */ 70 | -------------------------------------------------------------------------------- /doc/tutorial-pose-gauss-newton-opencv.doc: -------------------------------------------------------------------------------- 1 | /** 2 | 3 | \page tutorial-pose-gauss-newton-opencv Pose from a non-linear minimization method 4 | \tableofcontents 5 | 6 | \section intro_pose_gauss_newton_cv Introduction 7 | 8 | The "gold-standard" solution to the PnP consists in estimating the six parameters of the transformation \e cTw by minimizing the forward projection error using a Gauss-Newton approach. A complete derivation of this problem is given in \cite CH06. 9 | 10 | \section gauss_newton_code_cv Source code 11 | 12 | The following source code that uses OpenCV is also available in \ref pose-gauss-newton-opencv.cpp file. It allows to compute the pose of the camera from points. 13 | 14 | \include pose-gauss-newton-opencv.cpp 15 | 16 | \section gauss_newton_explained_cv Source code explained 17 | 18 | First of all we include OpenCV headers that are requested to manipulate vectors and matrices and also to compute the exponential map. 19 | 20 | \snippet pose-gauss-newton-opencv.cpp Include 21 | 22 | Then we introduce the function that does the pose estimation. It takes as input parameters \f${^w}{\bf X} = (X,Y,Z,1)^T\f$ the 3D coordinates of the points in the world frame and \f${\bf x} = (x,y,1)^T\f$ their normalized coordinates in the image plane. It returns the estimated pose as an homogeneous matrix transformation. 23 | 24 | \snippet pose-gauss-newton-opencv.cpp Estimation function 25 | 26 | The implementation of the iterative Gauss-Newton minimization process is done next. First, we create the variables used by the algorithm. Since the algorithm needs an initialization, we set an initial value in \e cTw not to far from the solution. Such an initialization could be done using \ref tutorial-pose-dlt-opencv of \ref tutorial-pose-dementhon-opencv approaches. Finally, the estimation is iterated. 27 | 28 | \snippet pose-gauss-newton-opencv.cpp Gauss-Newton 29 | 30 | When the residual obtained between two successive iterations can be neglected, we can exit the while loop and return. 31 | 32 | Finally we define the main function in which we will initialize the input data before calling the previous function and computing the pose using non linear Gauss-Newton approach. 33 | 34 | \snippet pose-gauss-newton-opencv.cpp Main function 35 | 36 | First in the main we create the data structures that will contain the 3D points coordinates \e wX in the world frame, their coordinates in the camera frame \e cX and their coordinates in the image plane \e x obtained after prerspective projection. Note here that at least 4 non coplanar points are requested to estimate the pose. 37 | 38 | \snippet pose-gauss-newton-opencv.cpp Create data structures 39 | 40 | For our simulation we then initialize the input data from a ground truth pose with the translation in \e ctw_truth and the rotation matrix in \e cRw_truth. 41 | For each point we set in \e wX[i] the 3D coordinates in the world frame (wX, wY, wZ, 1) and compute in \e cX their 3D coordinates (cX, cY, cZ, 1) in the camera frame. Then in \e x[i] we update their coordinates (x, y) in the image plane, obtained by perspective projection. 42 | 43 | 44 | \snippet pose-gauss-newton-opencv.cpp Simulation 45 | 46 | From here we have initialized \f${^w}{\bf X} = (X,Y,Z,1)^T\f$ and \f${\bf x} = (x,y,1)^T\f$. Since the non-linear minimization method requires a initial value of the pose to estimate we initialize \f${^c}{\bf T}_w\f$ not so far from the solution, in \e ctw for the translation vector and in \e cRw for the rotation vector. 47 | 48 | \snippet pose-gauss-newton-opencv.cpp Set pose initial value 49 | 50 | \note In a real application this initialization has to be done using: 51 | - one of the algorithms described in \ref tutorial-pose-dementhon-opencv or \ref tutorial-pose-dlt-opencv when the 3D points of the model are non coplanar 52 | - \ref tutorial-pose-dlt-planar-opencv when the 3D points are coplanar. 53 | 54 | We are now ready to call the function that does the pose estimation. 55 | 56 | \snippet pose-gauss-newton-opencv.cpp Call function 57 | 58 | \section gauss_newton_result_cv Resulting pose estimation 59 | 60 | If you run the previous code, it we produce the following result that shows that the estimated pose is equal to the ground truth one used to generate the input data: 61 | 62 | \code 63 | ctw (ground truth): 64 | [-0.1; 65 | 0.1; 66 | 0.5] 67 | ctw (from non linear method): 68 | [-0.09999999999999963; 69 | 0.1000000000000001; 70 | 0.5000000000000006] 71 | cRw (ground truth): 72 | [0.7072945483755065, -0.7061704379962989, 0.03252282795827704; 73 | 0.7061704379962989, 0.7036809008245869, -0.07846338199958876; 74 | 0.03252282795827704, 0.07846338199958876, 0.9963863524490802] 75 | cRw (from non linear method): 76 | [0.7072945483755072, -0.7061704379962995, 0.03252282795827719; 77 | 0.7061704379962994, 0.7036809008245862, -0.07846338199958869; 78 | 0.03252282795827711, 0.07846338199958879, 0.9963863524490796] 79 | \endcode 80 | 81 | */ 82 | -------------------------------------------------------------------------------- /doc/tutorial-pose-gauss-newton-visp.doc: -------------------------------------------------------------------------------- 1 | /** 2 | 3 | \page tutorial-pose-gauss-newton-visp Pose from a non-linear minimization method 4 | \tableofcontents 5 | 6 | \section intro_pose_gauss_newton Introduction 7 | 8 | The "gold-standard" solution to the PnP consists in estimating the six parameters of the transformation \e cTw by minimizing the forward projection error using a Gauss-Newton approach. A complete derivation of this problem is given in \cite CH06. 9 | 10 | \section gauss_newton_code Source code 11 | 12 | The following source code that uses ViSP is also available in \ref pose-gauss-newton-visp.cpp file. It allows to compute the pose of the camera from points. 13 | 14 | \include pose-gauss-newton-visp.cpp 15 | 16 | \section gauss_newton_explained Source code explained 17 | 18 | First of all we include ViSP headers that are requested to manipulate vectors and matrices and also to compute the exponential map. 19 | 20 | \snippet pose-gauss-newton-visp.cpp Include 21 | 22 | Then we introduce the function that does the pose estimation. It takes as input parameters \f${^w}{\bf X} = (X,Y,Z,1)^T\f$ the 3D coordinates of the points in the world frame and \f${\bf x} = (x,y,1)^T\f$ their normalized coordinates in the image plane. It returns the estimated pose as an homogeneous matrix transformation. 23 | 24 | \snippet pose-gauss-newton-visp.cpp Estimation function 25 | 26 | The implementation of the iterative Gauss-Newton minimization process is done next. First, we create the variables used by the algorithm. Since the algorithm needs an initialization, we set an initial value in \e cTw not to far from the solution. Such an initialization could be done using \ref tutorial-pose-dlt-visp of \ref tutorial-pose-dementhon-visp approaches. Finally, the estimation is iterated. 27 | 28 | \snippet pose-gauss-newton-visp.cpp Gauss-Newton 29 | 30 | When the residual obtained between two successive iterations can be neglected, we can exit the while loop and return the estimated value of the pose. 31 | 32 | \snippet pose-gauss-newton-visp.cpp Return cTw 33 | 34 | 35 | Finally we define the main function in which we will initialize the input data before calling the previous function and computing the pose using non linear Gauss-Newton approach. 36 | 37 | \snippet pose-gauss-newton-visp.cpp Main function 38 | 39 | First in the main we create the data structures that will contain the 3D points coordinates \e wX in the world frame, their coordinates in the camera frame \e cX and their coordinates in the image plane \e x obtained after prerspective projection. Note here that at least 4 non coplanar points are requested to estimate the pose. 40 | 41 | \snippet pose-gauss-newton-visp.cpp Create data structures 42 | 43 | For our simulation we then initialize the input data from a ground truth pose \e cTw_truth. 44 | For each point we set in \e wX[i] the 3D coordinates in the world frame (wX, wY, wZ, 1) and compute in \e cX their 3D coordinates (cX, cY, cZ, 1) in the camera frame. Then in \e x[i] we update their coordinates (x, y) in the image plane, obtained by perspective projection. 45 | 46 | \snippet pose-gauss-newton-visp.cpp Simulation 47 | 48 | From here we have initialized \f${^w}{\bf X} = (X,Y,Z,1)^T\f$ and \f${\bf x} = (x,y,1)^T\f$. Since the non-linear minimization method requires a initial value of the pose to estimate we initialize \f${^c}{\bf T}_w\f$ not so far from the solution. 49 | 50 | \snippet pose-gauss-newton-visp.cpp Set pose initial value 51 | 52 | \note In a real application this initialization has to be done using: 53 | - one of the algorithms described in \ref tutorial-pose-dementhon-visp or \ref tutorial-pose-dlt-visp when the 3D points of the model are non coplanar 54 | - \ref tutorial-pose-dlt-planar-visp when the 3D points are coplanar. 55 | 56 | We are now ready to call the function that does the pose estimation. 57 | 58 | \snippet pose-gauss-newton-visp.cpp Call function 59 | 60 | \section gauss_newton_result Resulting pose estimation 61 | 62 | If you run the previous code, it we produce the following result that shows that the estimated pose is equal to the ground truth one used to generate the input data: 63 | 64 | \code 65 | cTw (ground truth): 66 | 0.7072945484 -0.706170438 0.03252282796 -0.1 67 | 0.706170438 0.7036809008 -0.078463382 0.1 68 | 0.03252282796 0.078463382 0.9963863524 0.5 69 | 0 0 0 1 70 | cTw (from non linear method): 71 | 0.7072945484 -0.706170438 0.03252282796 -0.1 72 | 0.706170438 0.7036809008 -0.078463382 0.1 73 | 0.03252282796 0.078463382 0.9963863524 0.5 74 | 0 0 0 1 75 | \endcode 76 | 77 | */ 78 | -------------------------------------------------------------------------------- /doc/tutorial-pose-mbt-visp.doc: -------------------------------------------------------------------------------- 1 | /** 2 | 3 | \page tutorial-pose-mbt-visp Pose from markerless model-based tracking 4 | \tableofcontents 5 | 6 | \section intro_mbt Introduction 7 | 8 | Various authors have proposed different formulations of the pose 9 | estimation problem which does not require the need of markers or 10 | keypoints matching process. 11 | 12 | In ViSP such an algorithm is proposed. It allows to track an object using its cad model. Considered objects should be modeled by lines, circles or cylinders. The model of the object could be defined in vrml format, or in cao format. The markerless model-based tracker considered here can handle moving-edges behind the contour of the model. 13 | 14 | The video below shows the result of a tea box model-based tracking. 15 | 16 | \htmlonly 17 | 18 | \endhtmlonly 19 | 20 | 21 | \section pose_mbt_code Source code 22 | 23 | The following source code also available in \ref pose-mbt-visp.cpp allows to compute the pose of the camera wrt. the object that is tracked. 24 | 25 | \include pose-mbt-visp.cpp 26 | 27 | \section pose_mbt_explained Source code explained 28 | 29 | Hereafter is the description of the most important lines in this example. 30 | 31 | \snippet pose-mbt-visp.cpp Include 32 | 33 | Here we include the header of the vpMbEdgeTracker class that allows to track an object from its cad model using moving-edges. The tracker will use image \c I and the intrinsic camera parameters \c cam as input. 34 | 35 | \snippet pose-mbt-visp.cpp Image 36 | 37 | As output, it will estimate \c cTw, the pose of the object in the camera frame. 38 | 39 | \snippet pose-mbt-visp.cpp cTw 40 | 41 | Once the input image \c teabox.pgm is loaded in \c I, a window is created and initialized with image \c I. Then we create an instance of the tracker. 42 | 43 | \snippet pose-mbt-visp.cpp Constructor 44 | 45 | There are then two different ways to initialize the tracker. 46 | 47 | - The first one, if libxml2 is available, is to read the settings from \c teabox.xml input file if the file exists. 48 | \snippet pose-mbt-visp.cpp Load xml 49 | The content of the xml file is the following: 50 | \code 51 | 52 | 53 | 54 | 55 | 5 56 | 180 57 | 58 | 59 | 8 60 | 61 | 62 | 10000 63 | 0.5 64 | 0.5 65 | 66 | 67 | 68 | 4 69 | 250 70 | 71 | 72 | 325.66776 73 | 243.69727 74 | 839.21470 75 | 839.44555 76 | 77 | 78 | \endcode 79 | 80 | - The second one consists in initializing the parameters directly in the source code: 81 | \snippet pose-mbt-visp.cpp Set parameters 82 | 83 | 84 | Now we are ready to load the cad model of the object. ViSP supports cad model in cao format or in vrml format. The cao format is a particular format only supported by ViSP. It doesn't require an additional 3rd party rather then vrml format that require Coin 3rd party. 85 | 86 | First we check if the file exists, then we load the cad model in cao format with: 87 | \snippet pose-mbt-visp.cpp Load cao 88 | The file \c teabox.cao describes first the vertices of the box, then the edges that corresponds to the faces. A more complete description of this file is provided in \ref tracking_mb_cao. The next figure gives the index of the vertices that are defined in \c teabox.cao. 89 | 90 | If the cad model in cao format doesn't exist, we check then if it exists in vrml format before loading: 91 | \snippet pose-mbt-visp.cpp Load wrl 92 | 93 | As for the cao format, \c teabox.wrl describes first the vertices of the box, then the edges that corresponds to the faces. A more complete description of this file is provided in \ref tracking_mb_wrl. 94 | 95 | \image html img-teabox-cao.jpg Index of the vertices used to model the tea box in cao format. 96 | 97 | Once the model of the object to track is loaded, with the next line the display in the image window of additional drawings in overlay such as the moving edges positions, is then enabled by: 98 | \snippet pose-mbt-visp.cpp Set display 99 | 100 | Now we have to initialize the tracker. With the next line we choose to use a user interaction. 101 | \snippet pose-mbt-visp.cpp Init 102 | 103 | The user has to click in the image on four vertices with their 3D coordinates defined in the "teabox.init" file. The following image shows where the user has to click. 104 | 105 | \image html img-teabox-click.jpg Image "teabox.ppm" used to help the user to initialize the tracker. 106 | 107 | Matched 2D and 3D coordinates are then used to compute an initial pose used to initialize the tracker. Note also that the third optional argument "true" is used here to enable the display of an image that may help the user for the initialization. The name of this image is the same as the "*.init" file except the extension that sould be ".ppm". In our case it will be "teabox.ppm". 108 | 109 | The content of \c teabox.init file that defines 3D coordinates of some points of the model used during user intialization is provided hereafter. Note that all the characters after character '#' are considered as comments. 110 | 111 | \includelineno teabox.init 112 | 113 | We give now the signification of each line of this file: 114 | - line 1: Number of 3D points that should be defined in this file. At least 4 points are required. Four is the minimal number of points requested to compute a pose. 115 | - line 2: Each point is defined by its 3D coordinates. Here we define the first point with coordinates (0,0,0). In the previous figure it corresponds to vertex 0 of the tea box. This point is also the origin of the frame in which all the points are defined. 116 | - line 3: 3D coordinates of vertex 3. 117 | - line 4: 3D coordinates of vertex 2. 118 | - line 5: 3D coordinates of vertex 5. 119 | 120 | Here the user has to click on vertex 0, 3, 2 and 5 in the window that displays image \c I. From the 3D coordinates defined in \c teabox.init and the corresponding 2D coordinates of the vertices obtained by user interaction a pose is computed that is than used to initialize the tracker. 121 | 122 | Next, in the infinite while loop, after displaying the next image, we track the object on a new image \c I. 123 | 124 | \snippet pose-mbt-visp.cpp Track 125 | 126 | The result of the tracking is a pose \c cTw that could be obtained by: 127 | \snippet pose-mbt-visp.cpp Get pose 128 | 129 | Next lines are used first to retrieve the camera parameters used by the tracker, then to display the visible part of the cad model using red lines with 2 as thickness, and finally to display the object frame at the estimated position \c cTw. Each axis of the frame are 0.025 meters long. Using vpColor::none indicates that x-axis is displayed in red, y-axis in green, while z-axis in blue. The thickness of the axis is 3. 130 | 131 | \snippet pose-mbt-visp.cpp Display 132 | 133 | \section model Object modeling 134 | 135 | \subsection tracking_mb_cao CAD model in cao format 136 | 137 | cao format is specific to ViSP. It allows to describe the CAD model of an object using a text file with extension \c .cao. 138 | The content of the file teabox.cao used in this example is given here: 139 | 140 | \includelineno teabox.cao 141 | 142 | This file describes the model of the tea box corresponding to the next image: 143 | 144 | \image html img-teabox-cao.jpg Index of the vertices used to model the tea box in cao format. 145 | 146 | We make the choice to describe the faces of the box from the 3D points that correspond to the vertices. We provide now a line by line description of the file. Notice that the characters after the '#' are considered as comments. 147 | - line 1: Header of the \c .cao file 148 | - line 3: The model is defined by 8 3D points. Here the 8 points correspond to the 8 vertices of the tea box presented in the previous figure. Thus, next 8 lines define the 3D points coordinates. 149 | - line 4: 3D point with coordinate (0,0,0) corresponding to vertex 0 of the tea box. This point is also the origin of the frame in which all the 3D points are defined. 150 | - line 5: 3D point with coordinate (0,0,-0.08) corresponding to vertex 1. 151 | - line 6 to 11: The other 3D points corresponding to vertices 2 to 7 respectively. 152 | - line 13: Number of 3D lines defined from two 3D points. It is possible to introduce 3D lines and then use these lines to define faces from these 3D lines. This is particularly useful to define faces from non-closed polygons. For instance, it can be used to specify the tracking of only 3 edges of a rectangle. Notice also that a 3D line that doesn't belong to a face is always visible and consequently always tracked. 153 | - line 15: Number of faces defined from 3D lines. In our teabox example we decide to define all the faces from 3D points, that is why this value is set to 0. 154 | - line 17: The number of faces defined by a set of 3D points. Here our teabox has 6 faces. Thus, next 6 lines describe each face from the 3D points defined previously line 4 to 11. Notice here that all the faces defined from 3D points corresponds to closed polygons. 155 | - line 18: First face defined by 4 3D points, respectively vertices 0,1,2,3. The orientation of the face is counter clockwise by going from vertex 0 to vertex 1, then 2 and 3. This fixes the orientation of the normal of the face going outside the object. 156 | - line 19: Second face also defined by 4 points, respectively vertices 1,6,5,2 to have a counter clockwise orientation. 157 | - line 20 to 23: The four other faces of the box. 158 | - line 25: Number of 3D cylinders describing the model. Since we model a simple box, the number of cylinders is 0. 159 | - line 27: Number of 3D circles describing the model. For the same reason, the number of circles is 0. 160 | 161 | \subsection tracking_mb_wrl CAD model in vrml format 162 | 163 | ViSP support vrml format only if Coin 3rd party is installed. This format allows to describe the CAD model of an object using a text file with extension \c .wrl. The content of the teabox.wrl file used in this example is given hereafter. This content is to make into relation with teabox.cao described in \ref tracking_mb_cao. 164 | 165 | \includelineno teabox.wrl 166 | 167 | This file describes the model of the tea box corresponding to the next image: 168 | 169 | \image html img-teabox-cao.jpg Index of the vertices used to model the tea box in vrml format. 170 | 171 | We provide now a line by line description of the file where the faces of the box are defined from the vertices: 172 | - line 1 to 10: Header of the \c .wrl file. 173 | - line 13 to 20: 3D coordinates of the 8 tea box vertices. 174 | - line 34 to 29: Each line describe a face. In this example, a face is defined by 4 vertices. For example, the first face join vertices 0,1,2,3. The orientation of the face is counter clockwise by going from vertex 0 to vertex 1, then 2 and 3. This fixes the orientation of the normal of the face going outside the object. 175 | 176 | 177 | */ 178 | -------------------------------------------------------------------------------- /doc/tutorial-template-matching-visp.doc: -------------------------------------------------------------------------------- 1 | /** 2 | 3 | \page tutorial-template-matching-visp Direct motion estimation through template matching 4 | \tableofcontents 5 | 6 | \section intro Introduction 7 | 8 | The appearance-based approaches, also known as template-based 9 | approaches, are different in the way that there is no low-level 10 | tracking or matching processes. It is also possible to consider that 11 | this 2D model is a reference image (or a template). In that case, 12 | the goal is to estimate the motion (or warp) between the current 13 | image and a reference template. 14 | 15 | In ViSP such an algorithm is proposed. It allows to track a template using image registration algorithms. 16 | Contrary to the common approaches based on visual features, this method allows to be much more robust to scene variations. 17 | 18 | The video below shows the result of the template tracking applied on a painting. 19 | 20 | \htmlonly 21 | 22 | \endhtmlonly 23 | 24 | 25 | \section tt_code Source code 26 | 27 | The following source code also available in \ref template-matching-visp.cpp allows to estimate the homography between the current image and the reference template defined by the user in the first image of the video. The reference template is here defined from a set of triangles. These triangles should rely on the same plane to estimate a consistant homography. 28 | 29 | \include template-matching-visp.cpp 30 | 31 | \section tt_explained Source code explained 32 | 33 | Hereafter is the description of the most important lines in this example. The template tracking is performed using vpTemplateTrackerSSDForwardAdditional class implemented in ViSP. 34 | Let us denote that "SSDForwardAdditional" refers to the similarity function used for the image registration. In ViSP, we have implemented two different similarity functions: the "Sum of Square Differences" (vpTemplateTrackerSSD classes \cite Baker04a) and the "Zero-mean Normalized Cross Correlation" (vpTemplateTrackerZNCC classes \cite Irani98a). Both methods can be used in different ways: Inverse Compositional, Forward Compositional, Forward Additional, or ESM. 35 | 36 | \snippet template-matching-visp.cpp Include 37 | 38 | Here we include the header of the vpTemplateTrackerSSDForwardAdditional class that allows to track the template. Actually, the tracker estimates the displacement of the template in the current image according to its initial pose. The computed displacement can be represented by multiple transformations, also called warps (vpTemplateTrackerWarp classes). In this example, we include the header vpTemplateTrackerWarpHomography class to define the possible transformation of the template as an homography. 39 | 40 | \snippet template-matching-visp.cpp Construction 41 | 42 | Once the tracker is created with the desired warp function, parameters can be tuned to be more consistent with the expected behavior. Depending on these parameters the perfomances of the tracker in terms of processing time and estimation could be affected. Since here we deal with 640 by 480 pixel wide images, the images are significantly subsampled in order to reduce the time of the image processing to be compatible with real-time. 43 | 44 | \code 45 | tracker.setSampling(3, 3); // Will consider only one pixel from three along rows and columns 46 | // to create the reference template 47 | tracker.setLambda(0.001); // Gain used in the optimization loop 48 | tracker.setIterationMax(50); // Maximum number of iterations for the optimization loop 49 | tracker.setPyramidal(2, 1); // First and last level of the pyramid. Full resolution image is at level 0. 50 | \endcode 51 | 52 | The last step of the initialization is to select the template that will be tracked during the sequence. 53 | 54 | \snippet template-matching-visp.cpp Init 55 | 56 | The vpTemplateTracker classes proposed in ViSP offer you the possibility to defined your template as multiple planar triangles. When calling the previous line, you will have to specify the triangles that define the template. 57 | 58 | \image html img-initClickTemplateTracker.png Initialization of the template without Delaunay triangulation. 59 | 60 | Let us denote that those triangles don't have to be spatially tied up. However, if you want to track a simple image as in this example, you should initialize the template as on the figure above. Left clicks on point number zero, one and two create the green triangle. Left clicks on point three and four and then right click on point number five create the red triangle and ends the initialization. 61 | If ViSP is build with OpenCV, we also provide an initialization with automatic triangulation using Delaunay. To use it, you just have to call vpTemplateTracker::initClick(I, true). Then by left clicking on points number zero, one, two, four and right clicking on point number five initializes the tracker as on the image above. 62 | 63 | Next, in the infinite while loop, after displaying the next image, we track the object on a new image I. 64 | 65 | \snippet template-matching-visp.cpp Track 66 | 67 | If you need to get the parameters of the current transformation of the template, it can be done by calling: 68 | 69 | \snippet template-matching-visp.cpp Homography 70 | 71 | For further information about the warping parameters, see the following \ref warp_tt section. 72 | 73 | Then according to the computed transformation obtained from the last call to track() function, next line is used to display the template using red lines. 74 | 75 | \snippet template-matching-visp.cpp Display 76 | 77 | \subsection warp_tt Warping classes 78 | 79 | In the example presented above, we focused on the vpTemplateTrackerWarpHomography warping class which is the most generic transformation available in ViSP for the template trackers. However, if you know that the template you want to track is constrained, other warps might be more suitable. 80 | 81 | \b vpTemplateTrackerWarpTranslation 82 | 83 | \f$w({\bf x},{\bf p}) = {\bf x} + {\bf t}\f$ with the following estimated parameters \f$ {\bf p} = (t_x, t_y)\f$ 84 | 85 | This class is the most simple transformation available for the template trackers. It only considers translation on two-axis (x-axis and y-axis). 86 | 87 | \b vpTemplateTrackerWarpSRT 88 | 89 | \f$w({\bf x},{\bf p}) = (1+s){\bf Rx} + {\bf t}\f$ with \f${\bf p} = (s, \theta, t_x, t_y)\f$ 90 | 91 | The SRT warp considers a scale factor, a rotation on z-axis and a 2D translation as in vpTemplateTrackerWarpTranslation. 92 | 93 | \b vpTemplateTrackerWarpAffine 94 | 95 | 96 | \f$ w({\bf x},{\bf p}) = {\bf Ax} + {\bf t}\f$ with \f${\bf A} = \left( \begin{array}{cc} 97 | 1+a_0 & a_2 \\ 98 | a_1 & 1+a_3 99 | \end{array} \right)\f$, \f${\bf t} = \left( \begin{array}{c} 100 | t_x \\ 101 | t_y 102 | \end{array} \right)\f$ and the estimated parameters \f${\bf p} = (a_0 ... a_3, t_x, t_y)\f$ 103 | 104 | The template transformation can also be defined as an affine transformation. This warping function preserves points, straight lines, and planes. 105 | 106 | \b vpTemplateTrackerWarpHomography 107 | 108 | \f$w({\bf x},{\bf p}) = {\bf Hx}\f$ with \f$ {\bf H}=\left( \begin{array}{ccc} 109 | 1 + p_0 & p_3 & p_6 \\ 110 | p_1 & 1+p_4 & p_7 \\ 111 | p_2 & p_5 & 1.0 112 | \end{array} \right) \f$ and the estimated parameters \f${\bf p} = (p_0 ... p_7)\f$ 113 | 114 | As remind, the vpTemplateTrackerWarpHomography estimates the eight parameters of the homography matrix \f$ {\bf H}\f$. 115 | 116 | \b vpTemplateTrackerWarpHomographySL3 117 | 118 | \f$w({\bf x},{\bf p}) = {\bf Hx}\f$ with \f${\bf p} = (p_0 ... p_7)\f$ 119 | 120 | The vpTemplateTrackerWarpHomographySL3 warp works exactly the same as the vpTemplateTrackerWarpHomography warp. The only difference is that here, the parameters of the homography are estimated in the SL3 reference frame. 121 | 122 | \subsection tune_tt How to tune the tracker 123 | 124 | When you want to obtain a perfect pose estimation, it is often time-consuming. However, by tuning the tracker, you can find a good compromise between speed and efficiency. Basically, what will make the difference is the size of the reference template. The more pixels it contains, the more time-consuming it will be. Fortunately, the solutions to avoid this problem are multiple. First of all lets come back on the vpTemplateTracker::setSampling() function. 125 | 126 | \code 127 | tracker.setSampling(4, 4); // Will consider only one pixel from four along rows and columns 128 | // to create the reference template. 129 | \endcode 130 | 131 | In the example above, we decided to consider only one pixel from 16 (4 by 4) to create the reference template. Obviously, by increasing those values it will consider much less pixels, which unfortunately decrease the efficiency, but the tracking phase will be much faster. 132 | 133 | The tracking phase relies on an iterative algorithm minimizing a cost function. What does it mean? It means this algorithm has, at some point, to stop! Once again, you have the possibility to reduce the number of iterations of the algorithm by taking the risk to fall in a local minimum. 134 | 135 | \code 136 | tracker.setIterationMax(20); // Maximum number of iterations for the optimization loop. 137 | \endcode 138 | 139 | If this is still not enough for you, let's remember that all of our trackers can be used in a pyramidal way. By reducing the number of levels considered by the algorithm, you will consider, once again, much less pixels and be faster. 140 | 141 | \code 142 | tracker.setPyramidal(3, 2); // First and last level of the pyramid 143 | \endcode 144 | Note here that when vpTemplateTracker::setPyramidal() function is not used, the pyramidal approach to speed up the algorithm is not used. 145 | 146 | Let us denote that if you're using vpTemplateTrackerSSDInverseCompositional or vpTemplateTrackerZNCCInverseCompositional, you also have another interesting option to speed up your tracking phase. 147 | 148 | \code 149 | tracker.setUseTemplateSelect(true); 150 | \endcode 151 | 152 | This function will force the tracker to only consider, in the reference template, the pixels that have an high gradient value. This is another solution to limit the number of considered pixels. 153 | 154 | As well as vpTemplateTrackerSSDInverseCompositional::setUseTemplateSelect() or vpTemplateTrackerZNCCInverseCompositional::setUseTemplateSelect(), another function, only available in vpTemplateTrackerSSDInverseCompositional and vpTemplateTrackerZNCCInverseCompositional is: 155 | 156 | \code 157 | tracker.setThresholdRMS(1e-6); 158 | \endcode 159 | 160 | By increasing this root mean square threshold value, the algorithm will reduce its number of iterations which should also speed up the tracking phase. This function should be used wisely with the vpTemplateTracker::setIterationMax() function. 161 | 162 | \subsection points_tt How to get the points of the template 163 | 164 | The previous code provided in tutorial-template-tracker-visp.cpp can be modified to get the coordinates of the corners of the triangles that define the zone to track. To this end, as shown in the next lines, before the while loop we first define a reference zone and the corresponding warped zone. Then in the loop, we update the warped zone using the parameters of the warping model that is estimated by the tracker. From the warped zone, we extract all the triangles, and then for each triangles, we get the corners coordinates. 165 | 166 | \code 167 | // Instantiate and get the reference zone 168 | vpTemplateTrackerZone zone_ref = tracker.getZoneRef(); 169 | // Instantiate a warped zone 170 | vpTemplateTrackerZone zone_warped; 171 | 172 | while(!g.end()){ 173 | g.acquire(I); 174 | vpDisplay::display(I); 175 | tracker.track(I); 176 | 177 | tracker.display(I, vpColor::red); 178 | 179 | // Get the estimated parameters 180 | vpColVector p = tracker.getp(); 181 | 182 | // Update the warped zone given the tracker estimated parameters 183 | warp.warpZone(zone_ref, p, zone_warped); 184 | 185 | // Parse all the triangles that describe the zone 186 | for (int i=0; i < zone_warped.getNbTriangle(); i++) { 187 | vpTemplateTrackerTriangle triangle; 188 | // Get a triangle 189 | zone_warped.getTriangle(i, triangle); 190 | std::vector corners; 191 | // Get the 3 triangle corners 192 | triangle.getCorners( corners ); 193 | 194 | // From here, each corner triangle is available in 195 | // corners[0], corners[1] and corners[2] 196 | 197 | // Display a green cross over each corner 198 | for(unsigned int j=0; j 4 | #include 5 | #include 6 | #if defined(HAVE_OPENCV_CALIB3D) 7 | #include 8 | #endif 9 | #if defined(HAVE_OPENCV_CALIB) 10 | #include 11 | #endif 12 | #if defined(HAVE_OPENCV_3D) 13 | #include 14 | #endif 15 | //! [Include] 16 | 17 | //! [Estimation function] 18 | cv::Mat homography_dlt(const std::vector< cv::Point2d >& x1, const std::vector< cv::Point2d >& x2) 19 | //! [Estimation function] 20 | { 21 | //! [DLT] 22 | int npoints = (int)x1.size(); 23 | cv::Mat A(2 * npoints, 9, CV_64F, cv::Scalar(0)); 24 | 25 | // We need here to compute the SVD on a (n*2)*9 matrix (where n is 26 | // the number of points). if n == 4, the matrix has more columns 27 | // than rows. The solution is to add an extra line with zeros 28 | if (npoints == 4) 29 | A.resize(2 * npoints + 1, cv::Scalar(0)); 30 | 31 | // Since the third line of matrix A is a linear combination of the first and second lines 32 | // (A is rank 2) we don't need to implement this third line 33 | for (int i = 0; i < npoints; i++) { // Update matrix A using eq. 23 34 | A.at(2 * i, 3) = -x1[i].x; // -xi_1 35 | A.at(2 * i, 4) = -x1[i].y; // -yi_1 36 | A.at(2 * i, 5) = -1; // -1 37 | A.at(2 * i, 6) = x2[i].y * x1[i].x; // yi_2 * xi_1 38 | A.at(2 * i, 7) = x2[i].y * x1[i].y; // yi_2 * yi_1 39 | A.at(2 * i, 8) = x2[i].y; // yi_2 40 | 41 | A.at(2 * i + 1, 0) = x1[i].x; // xi_1 42 | A.at(2 * i + 1, 1) = x1[i].y; // yi_1 43 | A.at(2 * i + 1, 2) = 1; // 1 44 | A.at(2 * i + 1, 6) = -x2[i].x * x1[i].x; // -xi_2 * xi_1 45 | A.at(2 * i + 1, 7) = -x2[i].x * x1[i].y; // -xi_2 * yi_1 46 | A.at(2 * i + 1, 8) = -x2[i].x; // -xi_2 47 | } 48 | 49 | // Add an extra line with zero. 50 | if (npoints == 4) { 51 | for (int i = 0; i < 9; i++) { 52 | A.at(2 * npoints, i) = 0; 53 | } 54 | } 55 | 56 | cv::Mat w, u, vt; 57 | cv::SVD::compute(A, w, u, vt); 58 | 59 | double smallestSv = w.at(0, 0); 60 | unsigned int indexSmallestSv = 0; 61 | for (int i = 1; i < w.rows; i++) { 62 | if ((w.at(i, 0) < smallestSv)) { 63 | smallestSv = w.at(i, 0); 64 | indexSmallestSv = i; 65 | } 66 | } 67 | 68 | cv::Mat h = vt.row(indexSmallestSv); 69 | 70 | if (h.at(0, 8) < 0) // tz < 0 71 | h *= -1; 72 | //! [DLT] 73 | 74 | //! [Update homography matrix] 75 | cv::Mat _2H1(3, 3, CV_64F); 76 | for (int i = 0; i < 3; i++) 77 | for (int j = 0; j < 3; j++) 78 | _2H1.at(i, j) = h.at(0, 3 * i + j); 79 | //! [Update homography matrix] 80 | 81 | return _2H1; 82 | } 83 | 84 | //! [Main function] 85 | int main() 86 | //! [Main function] 87 | { 88 | //! [Create data structures] 89 | std::vector< cv::Point2d > x1; // Points projected in the image plane linked to camera 1 90 | std::vector< cv::Point2d > x2; // Points projected in the image plane linked to camera 2 91 | 92 | std::vector< cv::Point3d > wX; // 3D points in the world plane 93 | //! [Create data structures] 94 | 95 | //! [Simulation] 96 | // Ground truth pose used to generate the data 97 | cv::Mat c1tw_truth = (cv::Mat_(3, 1) << -0.1, 0.1, 1.2); // Translation vector 98 | cv::Mat c1rw_truth = (cv::Mat_(3, 1) << CV_PI / 180 * (5), CV_PI / 180 * (0), CV_PI / 180 * (45)); // Rotation vector 99 | cv::Mat c1Rw_truth(3, 3, cv::DataType::type); // Rotation matrix 100 | cv::Rodrigues(c1rw_truth, c1Rw_truth); 101 | 102 | cv::Mat c2tc1 = (cv::Mat_(3, 1) << 0.01, 0.01, 0.2); // Translation vector 103 | cv::Mat c2rc1 = (cv::Mat_(3, 1) << CV_PI / 180 * (0), CV_PI / 180 * (3), CV_PI / 180 * (5)); // Rotation vector 104 | cv::Mat c2Rc1(3, 3, cv::DataType::type); // Rotation matrix 105 | cv::Rodrigues(c2rc1, c2Rc1); 106 | 107 | // Input data: 3D coordinates of at least 4 coplanar points 108 | double L = 0.2; 109 | wX.push_back(cv::Point3d(-L, -L, 0)); // wX_0 (-L, -L, 0)^T 110 | wX.push_back(cv::Point3d(2 * L, -L, 0)); // wX_1 ( L, -L, 0)^T 111 | wX.push_back(cv::Point3d(L, L, 0)); // wX_2 ( L, L, 0)^T 112 | wX.push_back(cv::Point3d(-L, L, 0)); // wX_3 (-L, L, 0)^T 113 | 114 | // Input data: 2D coordinates of the points on the image plane 115 | for (int i = 0; i < wX.size(); i++) { 116 | // Compute 3D points coordinates in the camera frame 1 117 | cv::Mat c1X = c1Rw_truth * cv::Mat(wX[i]) + c1tw_truth; // Update c1X, c1Y, c1Z 118 | // Compute 2D points coordinates in image plane from perspective projection 119 | x1.push_back(cv::Point2d(c1X.at(0, 0) / c1X.at(2, 0), // x1 = c1X/c1Z 120 | c1X.at(1, 0) / c1X.at(2, 0))); // y1 = c1Y/c1Z 121 | 122 | // Compute 3D points coordinates in the camera frame 2 123 | cv::Mat c2X = c2Rc1 * cv::Mat(c1X) + c2tc1; // Update c2X, c2Y, c2Z 124 | // Compute 2D points coordinates in image plane from perspective projection 125 | x2.push_back(cv::Point2d(c2X.at(0, 0) / c2X.at(2, 0), // x2 = c2X/c2Z 126 | c2X.at(1, 0) / c2X.at(2, 0))); // y2 = c2Y/c2Z 127 | } 128 | //! [Simulation] 129 | 130 | //! [Call function] 131 | cv::Mat _2H1 = homography_dlt(x1, x2); 132 | //! [Call function] 133 | 134 | std::cout << "2H1 (computed with DLT):\n" << _2H1 << std::endl; 135 | 136 | return 0; 137 | } 138 | -------------------------------------------------------------------------------- /opencv/homography-basis/pose-from-homography-dlt-opencv.cpp: -------------------------------------------------------------------------------- 1 | //! \example pose-from-homography-dlt-opencv.cpp 2 | //! [Include] 3 | #include 4 | #include 5 | #include 6 | #if defined(HAVE_OPENCV_CALIB3D) 7 | #include 8 | #endif 9 | #if defined(HAVE_OPENCV_CALIB) 10 | #include 11 | #endif 12 | #if defined(HAVE_OPENCV_3D) 13 | #include 14 | #endif 15 | //! [Include] 16 | 17 | //! [Homography DLT function] 18 | cv::Mat homography_dlt(const std::vector< cv::Point2d >& x1, const std::vector< cv::Point2d >& x2) 19 | //! [Homography DLT function] 20 | { 21 | //! [DLT] 22 | int npoints = (int)x1.size(); 23 | cv::Mat A(2 * npoints, 9, CV_64F, cv::Scalar(0)); 24 | 25 | // We need here to compute the SVD on a (n*2)*9 matrix (where n is 26 | // the number of points). if n == 4, the matrix has more columns 27 | // than rows. The solution is to add an extra line with zeros 28 | if (npoints == 4) 29 | A.resize(2 * npoints + 1, cv::Scalar(0)); 30 | 31 | // Since the third line of matrix A is a linear combination of the first and second lines 32 | // (A is rank 2) we don't need to implement this third line 33 | for (int i = 0; i < npoints; i++) { // Update matrix A using eq. 23 34 | A.at(2 * i, 3) = -x1[i].x; // -xi_1 35 | A.at(2 * i, 4) = -x1[i].y; // -yi_1 36 | A.at(2 * i, 5) = -1; // -1 37 | A.at(2 * i, 6) = x2[i].y * x1[i].x; // yi_2 * xi_1 38 | A.at(2 * i, 7) = x2[i].y * x1[i].y; // yi_2 * yi_1 39 | A.at(2 * i, 8) = x2[i].y; // yi_2 40 | 41 | A.at(2 * i + 1, 0) = x1[i].x; // xi_1 42 | A.at(2 * i + 1, 1) = x1[i].y; // yi_1 43 | A.at(2 * i + 1, 2) = 1; // 1 44 | A.at(2 * i + 1, 6) = -x2[i].x * x1[i].x; // -xi_2 * xi_1 45 | A.at(2 * i + 1, 7) = -x2[i].x * x1[i].y; // -xi_2 * yi_1 46 | A.at(2 * i + 1, 8) = -x2[i].x; // -xi_2 47 | } 48 | 49 | // Add an extra line with zero. 50 | if (npoints == 4) { 51 | for (int i = 0; i < 9; i++) { 52 | A.at(2 * npoints, i) = 0; 53 | } 54 | } 55 | 56 | cv::Mat w, u, vt; 57 | cv::SVD::compute(A, w, u, vt); 58 | 59 | double smallestSv = w.at(0, 0); 60 | unsigned int indexSmallestSv = 0; 61 | for (int i = 1; i < w.rows; i++) { 62 | if ((w.at(i, 0) < smallestSv)) { 63 | smallestSv = w.at(i, 0); 64 | indexSmallestSv = i; 65 | } 66 | } 67 | 68 | cv::Mat h = vt.row(indexSmallestSv); 69 | 70 | if (h.at(0, 8) < 0) // tz < 0 71 | h *= -1; 72 | //! [DLT] 73 | 74 | //! [Update homography matrix] 75 | cv::Mat _2H1(3, 3, CV_64F); 76 | for (int i = 0; i < 3; i++) { 77 | for (int j = 0; j < 3; j++) { 78 | _2H1.at(i, j) = h.at(0, 3 * i + j); 79 | } 80 | } 81 | //! [Update homography matrix] 82 | 83 | return _2H1; 84 | } 85 | 86 | //! [Estimation function] 87 | void pose_from_homography_dlt(const std::vector< cv::Point2d >& xw, 88 | const std::vector< cv::Point2d >& xo, 89 | cv::Mat& otw, cv::Mat& oRw) 90 | //! [Estimation function] 91 | { 92 | //! [Homography estimation] 93 | cv::Mat oHw = homography_dlt(xw, xo); 94 | //! [Homography estimation] 95 | 96 | //! [Homography normalization] 97 | // Normalization to ensure that ||c1|| = 1 98 | double norm = sqrt(oHw.at(0, 0) * oHw.at(0, 0) 99 | + oHw.at(1, 0) * oHw.at(1, 0) 100 | + oHw.at(2, 0) * oHw.at(2, 0)); 101 | oHw /= norm; 102 | //! [Homography normalization] 103 | 104 | //! [Extract c1, c2] 105 | cv::Mat c1 = oHw.col(0); 106 | cv::Mat c2 = oHw.col(1); 107 | //! [Extract c1, c2] 108 | //! [Compute c3] 109 | cv::Mat c3 = c1.cross(c2); 110 | //! [Compute c3] 111 | 112 | //! [Update pose] 113 | otw = oHw.col(2); 114 | 115 | for (int i = 0; i < 3; i++) { 116 | oRw.at(i, 0) = c1.at(i, 0); 117 | oRw.at(i, 1) = c2.at(i, 0); 118 | oRw.at(i, 2) = c3.at(i, 0); 119 | } 120 | //! [Update pose] 121 | } 122 | 123 | //! [Main function] 124 | int main() 125 | //! [Main function] 126 | { 127 | //! [Create data structures] 128 | int npoints = 4; 129 | 130 | std::vector< cv::Point3d > wX; // 3D points in the world plane 131 | 132 | std::vector< cv::Point2d > xw; // Normalized coordinates in the object frame 133 | std::vector< cv::Point2d > xo; // Normalized coordinates in the image plane 134 | //! [Create data structures] 135 | 136 | //! [Simulation] 137 | // Ground truth pose used to generate the data 138 | cv::Mat otw_truth = (cv::Mat_(3, 1) << -0.1, 0.1, 1.2); // Translation vector 139 | cv::Mat orw_truth = (cv::Mat_(3, 1) << CV_PI / 180 * (5), CV_PI / 180 * (0), CV_PI / 180 * (45)); // Rotation vector 140 | cv::Mat oRw_truth(3, 3, cv::DataType::type); // Rotation matrix 141 | cv::Rodrigues(orw_truth, oRw_truth); 142 | 143 | // Input data: 3D coordinates of at least 4 coplanar points 144 | double L = 0.2; 145 | wX.push_back(cv::Point3d(-L, -L, 0)); // wX_0 (-L, -L, 0)^T 146 | wX.push_back(cv::Point3d(2 * L, -L, 0)); // wX_1 ( L, -L, 0)^T 147 | wX.push_back(cv::Point3d(L, L, 0)); // wX_2 ( L, L, 0)^T 148 | wX.push_back(cv::Point3d(-L, L, 0)); // wX_3 (-L, L, 0)^T 149 | 150 | // Input data: 2D coordinates of the points on the image plane 151 | for (int i = 0; i < wX.size(); i++) { 152 | cv::Mat oX = oRw_truth * cv::Mat(wX[i]) + otw_truth; // Update oX, oY, oZ 153 | xo.push_back(cv::Point2d(oX.at(0, 0) / oX.at(2, 0), 154 | oX.at(1, 0) / oX.at(2, 0))); // xo = (oX/oZ, oY/oZ) 155 | 156 | xw.push_back(cv::Point2d(wX[i].x, wX[i].y)); // xw = (wX, wY) 157 | } 158 | //! [Simulation] 159 | 160 | //! [Call function] 161 | cv::Mat otw(3, 1, CV_64F); // Translation vector 162 | cv::Mat oRw(3, 3, CV_64F); // Rotation matrix 163 | 164 | pose_from_homography_dlt(xw, xo, otw, oRw); 165 | //! [Call function] 166 | 167 | std::cout << "otw (ground truth):\n" << otw_truth << std::endl; 168 | std::cout << "otw (computed with homography DLT):\n" << otw << std::endl; 169 | std::cout << "oRw (ground truth):\n" << oRw_truth << std::endl; 170 | std::cout << "oRw (computed with homography DLT):\n" << oRw << std::endl; 171 | 172 | return 0; 173 | } 174 | -------------------------------------------------------------------------------- /opencv/pose-basis/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10) 2 | project(pose-basis-opencv) 3 | 4 | find_package(OpenCV REQUIRED) 5 | 6 | set (CMAKE_CXX_STANDARD 11) 7 | 8 | include_directories(${OpenCV_INCLUDE_DIRS}) 9 | 10 | set(tutorial_cpp 11 | pose-dlt-opencv.cpp 12 | pose-dementhon-opencv.cpp 13 | pose-gauss-newton-opencv.cpp) 14 | 15 | foreach(cpp ${tutorial_cpp}) 16 | # Compute the name of the binary to create 17 | get_filename_component(binary ${cpp} NAME_WE) 18 | 19 | # From source compile the binary and add link rules 20 | add_executable(${binary} ${cpp}) 21 | target_link_libraries(${binary} ${OpenCV_LIBRARIES}) 22 | endforeach() 23 | 24 | -------------------------------------------------------------------------------- /opencv/pose-basis/pose-dementhon-opencv.cpp: -------------------------------------------------------------------------------- 1 | //! \example pose-dementhon-opencv.cpp 2 | 3 | //! [Include] 4 | #include 5 | #include 6 | #include 7 | #if defined(HAVE_OPENCV_CALIB3D) 8 | #include 9 | #endif 10 | #if defined(HAVE_OPENCV_CALIB) 11 | #include 12 | #endif 13 | #if defined(HAVE_OPENCV_3D) 14 | #include 15 | #endif 16 | //! [Include] 17 | 18 | //! [Estimation function] 19 | void pose_dementhon(const std::vector< cv::Point3d >& wX, 20 | const std::vector< cv::Point2d >& x, 21 | cv::Mat& ctw, cv::Mat& cRw) 22 | //! [Estimation function] 23 | { 24 | //! [POSIT] 25 | int npoints = (int)wX.size(); 26 | cv::Mat r1, r2, r3; 27 | cv::Mat A(npoints, 4, CV_64F); 28 | for (int i = 0; i < npoints; i++) { 29 | A.at(i, 0) = wX[i].x; 30 | A.at(i, 1) = wX[i].y; 31 | A.at(i, 2) = wX[i].z; 32 | A.at(i, 3) = 1; 33 | } 34 | cv::Mat Ap = A.inv(cv::DECOMP_SVD); 35 | 36 | cv::Mat eps(npoints, 1, CV_64F, cv::Scalar(0)); // Initialize epsilon_i = 0 37 | cv::Mat Bx(npoints, 1, CV_64F); 38 | cv::Mat By(npoints, 1, CV_64F); 39 | double tx, ty, tz; 40 | cv::Mat I, J; 41 | cv::Mat Istar(3, 1, CV_64F), Jstar(3, 1, CV_64F); 42 | 43 | // POSIT loop 44 | for (unsigned int iter = 0; iter < 20; iter++) { 45 | for (int i = 0; i < npoints; i++) { 46 | Bx.at(i, 0) = x[i].x * (eps.at(i, 0) + 1.); 47 | By.at(i, 0) = x[i].y * (eps.at(i, 0) + 1.); 48 | } 49 | 50 | I = Ap * Bx; // Notice that the pseudo inverse 51 | J = Ap * By; // of matrix A is a constant that has been precompiled. 52 | 53 | for (int i = 0; i < 3; i++) { 54 | Istar.at(i, 0) = I.at(i, 0); 55 | Jstar.at(i, 0) = J.at(i, 0); 56 | } 57 | 58 | // Estimation of the rotation matrix 59 | double normI = 0, normJ = 0; 60 | for (int i = 0; i < 3; i++) { 61 | normI += Istar.at(i, 0) * Istar.at(i, 0); 62 | normJ += Jstar.at(i, 0) * Jstar.at(i, 0); 63 | } 64 | normI = sqrt(normI); 65 | normJ = sqrt(normJ); 66 | r1 = Istar / normI; 67 | r2 = Jstar / normJ; 68 | r3 = r1.cross(r2); 69 | 70 | // Estimation of the translation 71 | tz = 1 / normI; 72 | tx = tz * I.at(3, 0); 73 | ty = tz * J.at(3, 0); 74 | 75 | // Update epsilon_i 76 | for (int i = 0; i < npoints; i++) { 77 | eps.at(i, 0) = (r3.at(0, 0) * wX[i].x 78 | + r3.at(1, 0) * wX[i].y 79 | + r3.at(2, 0) * wX[i].z) / tz; 80 | } 81 | } 82 | //! [POSIT] 83 | 84 | //! [Update homogeneous matrix] 85 | ctw.at(0, 0) = tx; // Translation tx 86 | ctw.at(1, 0) = ty; // Translation tx 87 | ctw.at(2, 0) = tz; // Translation tx 88 | for (int i = 0; i < 3; i++) { // Rotation matrix 89 | cRw.at(0, i) = r1.at(i, 0); 90 | cRw.at(1, i) = r2.at(i, 0); 91 | cRw.at(2, i) = r3.at(i, 0); 92 | } 93 | //! [Update homogeneous matrix] 94 | } 95 | 96 | //! [Main function] 97 | int main() 98 | //! [Main function] 99 | { 100 | //! [Create data structures] 101 | std::vector< cv::Point3d > wX; 102 | std::vector< cv::Point2d > x; 103 | //! [Create data structures] 104 | 105 | //! [Simulation] 106 | // Ground truth pose used to generate the data 107 | cv::Mat ctw_truth = (cv::Mat_(3, 1) << -0.1, 0.1, 0.5); // Translation vector 108 | cv::Mat crw_truth = (cv::Mat_(3, 1) << CV_PI / 180 * (5), CV_PI / 180 * (0), CV_PI / 180 * (45)); // Rotation vector 109 | cv::Mat cRw_truth(3, 3, cv::DataType::type); // Rotation matrix 110 | cv::Rodrigues(crw_truth, cRw_truth); 111 | 112 | // Input data: 3D coordinates of at least 4 non coplanar points 113 | double L = 0.2; 114 | wX.push_back(cv::Point3d(-L, -L, 0)); // wX_0 (-L, -L, 0 )^T 115 | wX.push_back(cv::Point3d(L, -L, 0.2)); // wX_1 ( L, -L, 0.2)^T 116 | wX.push_back(cv::Point3d(L, L, -0.1)); // wX_2 ( L, L,-0.1)^T 117 | wX.push_back(cv::Point3d(-L, L, 0)); // wX_3 (-L, L, 0 )^T 118 | 119 | // Input data: 2D coordinates of the points on the image plane 120 | for (int i = 0; i < wX.size(); i++) { 121 | cv::Mat cX = cRw_truth * cv::Mat(wX[i]) + ctw_truth; // Update cX, cY, cZ 122 | x.push_back(cv::Point2d(cX.at(0, 0) / cX.at(2, 0), 123 | cX.at(1, 0) / cX.at(2, 0))); // x = (cX/cZ, cY/cZ) 124 | } 125 | //! [Simulation] 126 | 127 | //! [Call function] 128 | cv::Mat ctw(3, 1, CV_64F); // Translation vector 129 | cv::Mat cRw(3, 3, CV_64F); // Rotation matrix 130 | 131 | pose_dementhon(wX, x, ctw, cRw); 132 | //! [Call function] 133 | 134 | std::cout << "ctw (ground truth):\n" << ctw_truth << std::endl; 135 | std::cout << "ctw (computed with Dementhon):\n" << ctw << std::endl; 136 | std::cout << "cRw (ground truth):\n" << cRw_truth << std::endl; 137 | std::cout << "cRw (computed with Dementhon):\n" << cRw << std::endl; 138 | 139 | return 0; 140 | } 141 | -------------------------------------------------------------------------------- /opencv/pose-basis/pose-dlt-opencv.cpp: -------------------------------------------------------------------------------- 1 | //! \example pose-dlt-opencv.cpp 2 | 3 | //! [Include] 4 | #include 5 | #include 6 | #include 7 | #if defined(HAVE_OPENCV_CALIB3D) 8 | #include 9 | #endif 10 | #if defined(HAVE_OPENCV_CALIB) 11 | #include 12 | #endif 13 | #if defined(HAVE_OPENCV_3D) 14 | #include 15 | #endif 16 | //! [Include] 17 | 18 | //! [Estimation function] 19 | void pose_dlt(const std::vector< cv::Point3d >& wX, const std::vector< cv::Point2d >& x, cv::Mat& ctw, cv::Mat& cRw) 20 | //! [Estimation function] 21 | { 22 | //! [DLT] 23 | int npoints = (int)wX.size(); 24 | cv::Mat A(2 * npoints, 12, CV_64F, cv::Scalar(0)); 25 | for (int i = 0; i < npoints; i++) { // Update matrix A using eq. 5 26 | A.at(2 * i, 0) = wX[i].x; 27 | A.at(2 * i, 1) = wX[i].y; 28 | A.at(2 * i, 2) = wX[i].z; 29 | A.at(2 * i, 3) = 1; 30 | 31 | A.at(2 * i + 1, 4) = wX[i].x; 32 | A.at(2 * i + 1, 5) = wX[i].y; 33 | A.at(2 * i + 1, 6) = wX[i].z; 34 | A.at(2 * i + 1, 7) = 1; 35 | 36 | A.at(2 * i, 8) = -x[i].x * wX[i].x; 37 | A.at(2 * i, 9) = -x[i].x * wX[i].y; 38 | A.at(2 * i, 10) = -x[i].x * wX[i].z; 39 | A.at(2 * i, 11) = -x[i].x; 40 | 41 | A.at(2 * i + 1, 8) = -x[i].y * wX[i].x; 42 | A.at(2 * i + 1, 9) = -x[i].y * wX[i].y; 43 | A.at(2 * i + 1, 10) = -x[i].y * wX[i].z; 44 | A.at(2 * i + 1, 11) = -x[i].y; 45 | } 46 | 47 | cv::Mat w, u, vt; 48 | cv::SVD::compute(A, w, u, vt); 49 | 50 | double smallestSv = w.at(0, 0); 51 | unsigned int indexSmallestSv = 0; 52 | for (int i = 1; i < w.rows; i++) { 53 | if ((w.at(i, 0) < smallestSv)) { 54 | smallestSv = w.at(i, 0); 55 | indexSmallestSv = i; 56 | } 57 | } 58 | cv::Mat h = vt.row(indexSmallestSv); 59 | 60 | if (h.at(0, 11) < 0) // tz < 0 61 | h *= -1; 62 | 63 | // Normalization to ensure that ||r3|| = 1 64 | double norm = sqrt(h.at(0, 8) * h.at(0, 8) 65 | + h.at(0, 9) * h.at(0, 9) 66 | + h.at(0, 10) * h.at(0, 10)); 67 | 68 | h /= norm; 69 | //! [DLT] 70 | 71 | //! [Update homogeneous matrix] 72 | for (int i = 0; i < 3; i++) { 73 | ctw.at(i, 0) = h.at(0, 4 * i + 3); // Translation 74 | for (int j = 0; j < 3; j++) { 75 | cRw.at(i, j) = h.at(0, 4 * i + j); // Rotation 76 | } 77 | } 78 | //! [Update homogeneous matrix] 79 | } 80 | 81 | 82 | //! [Main function] 83 | int main() 84 | //! [Main function] 85 | { 86 | //! [Create data structures] 87 | std::vector< cv::Point3d > wX; 88 | std::vector< cv::Point2d > x; 89 | //! [Create data structures] 90 | 91 | //! [Simulation] 92 | // Ground truth pose used to generate the data 93 | cv::Mat ctw_truth = (cv::Mat_(3, 1) << -0.1, 0.1, 1.2); // Translation vector 94 | cv::Mat crw_truth = (cv::Mat_(3, 1) << CV_PI / 180 * (5), CV_PI / 180 * (0), CV_PI / 180 * (45)); // Rotation vector 95 | cv::Mat cRw_truth(3, 3, cv::DataType::type); // Rotation matrix 96 | cv::Rodrigues(crw_truth, cRw_truth); 97 | 98 | // Input data: 3D coordinates of at least 6 non coplanar points 99 | double L = 0.2; 100 | wX.push_back(cv::Point3d(-L, -L, 0)); // wX_0 ( -L, -L, 0 )^T 101 | wX.push_back(cv::Point3d(2 * L, -L, 0.2)); // wX_1 (-2L, -L, 0.2)^T 102 | wX.push_back(cv::Point3d(L, L, 0.2)); // wX_2 ( L, L, 0.2)^T 103 | wX.push_back(cv::Point3d(-L, L, 0)); // wX_3 ( -L, L, 0 )^T 104 | wX.push_back(cv::Point3d(-2 * L, L, 0)); // wX_4 (-2L, L, 0 )^T 105 | wX.push_back(cv::Point3d(0, 0, 0.5)); // wX_5 ( 0, 0, 0.5)^T 106 | 107 | // Input data: 2D coordinates of the points on the image plane 108 | for (int i = 0; i < wX.size(); i++) { 109 | cv::Mat cX = cRw_truth * cv::Mat(wX[i]) + ctw_truth; // Update cX, cY, cZ 110 | x.push_back(cv::Point2d(cX.at(0, 0) / cX.at(2, 0), 111 | cX.at(1, 0) / cX.at(2, 0))); // x = (cX/cZ, cY/cZ) 112 | } 113 | //! [Simulation] 114 | 115 | //! [Call function] 116 | cv::Mat ctw(3, 1, CV_64F); // Translation vector 117 | cv::Mat cRw(3, 3, CV_64F); // Rotation matrix 118 | 119 | pose_dlt(wX, x, ctw, cRw); 120 | //! [Call function] 121 | 122 | std::cout << "ctw (ground truth):\n" << ctw_truth << std::endl; 123 | std::cout << "ctw (computed with DLT):\n" << ctw << std::endl; 124 | std::cout << "cRw (ground truth):\n" << cRw_truth << std::endl; 125 | std::cout << "cRw (computed with DLT):\n" << cRw << std::endl; 126 | 127 | return 0; 128 | } 129 | -------------------------------------------------------------------------------- /opencv/pose-basis/pose-gauss-newton-opencv.cpp: -------------------------------------------------------------------------------- 1 | //! \example pose-gauss-newton-opencv.cpp 2 | 3 | //! [Include] 4 | #include 5 | #include 6 | #include 7 | #if defined(HAVE_OPENCV_CALIB3D) 8 | #include 9 | #endif 10 | #if defined(HAVE_OPENCV_CALIB) 11 | #include 12 | #endif 13 | #if defined(HAVE_OPENCV_3D) 14 | #include 15 | #endif 16 | //! [Include] 17 | 18 | void exponential_map(const cv::Mat& v, cv::Mat& dt, cv::Mat& dR) { 19 | double vx = v.at(0, 0); 20 | double vy = v.at(1, 0); 21 | double vz = v.at(2, 0); 22 | double vtux = v.at(3, 0); 23 | double vtuy = v.at(4, 0); 24 | double vtuz = v.at(5, 0); 25 | cv::Mat tu = (cv::Mat_(3, 1) << vtux, vtuy, vtuz); // theta u 26 | cv::Rodrigues(tu, dR); 27 | 28 | double theta = sqrt(tu.dot(tu)); 29 | double sinc = (fabs(theta) < 1.0e-8) ? 1.0 : sin(theta) / theta; 30 | double mcosc = (fabs(theta) < 2.5e-4) ? 0.5 : (1. - cos(theta)) / theta / theta; 31 | double msinc = (fabs(theta) < 2.5e-4) ? (1. / 6.) : (1. - sin(theta) / theta) / theta / theta; 32 | 33 | dt.at(0, 0) = vx * (sinc + vtux * vtux * msinc) 34 | + vy * (vtux * vtuy * msinc - vtuz * mcosc) 35 | + vz * (vtux * vtuz * msinc + vtuy * mcosc); 36 | 37 | dt.at(1, 0) = vx * (vtux * vtuy * msinc + vtuz * mcosc) 38 | + vy * (sinc + vtuy * vtuy * msinc) 39 | + vz * (vtuy * vtuz * msinc - vtux * mcosc); 40 | 41 | dt.at(2, 0) = vx * (vtux * vtuz * msinc - vtuy * mcosc) 42 | + vy * (vtuy * vtuz * msinc + vtux * mcosc) 43 | + vz * (sinc + vtuz * vtuz * msinc); 44 | } 45 | 46 | //! [Estimation function] 47 | void pose_gauss_newton(const std::vector< cv::Point3d >& wX, 48 | const std::vector< cv::Point2d >& x, 49 | cv::Mat& ctw, cv::Mat& cRw) 50 | //! [Estimation function] 51 | { 52 | //! [Gauss-Newton] 53 | int npoints = (int)wX.size(); 54 | cv::Mat J(2 * npoints, 6, CV_64FC1); 55 | cv::Mat cX; 56 | double lambda = 0.25; 57 | cv::Mat err, sd(2 * npoints, 1, CV_64FC1), s(2 * npoints, 1, CV_64FC1); 58 | cv::Mat xq(npoints * 2, 1, CV_64FC1); 59 | // From input vector x = (x, y) we create a column vector xn = (x, y)^T to ease computation of e_q 60 | cv::Mat xn(npoints * 2, 1, CV_64FC1); 61 | //vpHomogeneousMatrix cTw_ = cTw; 62 | double residual = 0, residual_prev; 63 | cv::Mat Jp; 64 | 65 | // From input vector x = (x, y, 1)^T we create a new one xn = (x, y)^T to ease computation of e_q 66 | for (int i = 0; i < x.size(); i++) { 67 | xn.at(i * 2, 0) = x[i].x; // x 68 | xn.at(i * 2 + 1, 0) = x[i].y; // y 69 | } 70 | 71 | // Iterative Gauss-Newton minimization loop 72 | do { 73 | for (int i = 0; i < npoints; i++) { 74 | cX = cRw * cv::Mat(wX[i]) + ctw; // Update cX, cY, cZ 75 | 76 | double Xi = cX.at(0, 0); 77 | double Yi = cX.at(1, 0); 78 | double Zi = cX.at(2, 0); 79 | 80 | double xi = Xi / Zi; 81 | double yi = Yi / Zi; 82 | 83 | // Update x(q) 84 | xq.at(i * 2, 0) = xi; // x(q) = cX/cZ 85 | xq.at(i * 2 + 1, 0) = yi; // y(q) = cY/cZ 86 | 87 | // Update J using equation (11) 88 | J.at(i * 2, 0) = -1 / Zi; // -1/cZ 89 | J.at(i * 2, 1) = 0; // 0 90 | J.at(i * 2, 2) = xi / Zi; // x/cZ 91 | J.at(i * 2, 3) = xi * yi; // xy 92 | J.at(i * 2, 4) = -(1 + xi * xi); // -(1+x^2) 93 | J.at(i * 2, 5) = yi; // y 94 | 95 | J.at(i * 2 + 1, 0) = 0; // 0 96 | J.at(i * 2 + 1, 1) = -1 / Zi; // -1/cZ 97 | J.at(i * 2 + 1, 2) = yi / Zi; // y/cZ 98 | J.at(i * 2 + 1, 3) = 1 + yi * yi; // 1+y^2 99 | J.at(i * 2 + 1, 4) = -xi * yi; // -xy 100 | J.at(i * 2 + 1, 5) = -xi; // -x 101 | } 102 | 103 | cv::Mat e_q = xq - xn; // Equation (7) 104 | 105 | cv::Mat Jp = J.inv(cv::DECOMP_SVD); // Compute pseudo inverse of the Jacobian 106 | cv::Mat dq = -lambda * Jp * e_q; // Equation (10) 107 | 108 | cv::Mat dctw(3, 1, CV_64FC1), dcRw(3, 3, CV_64FC1); 109 | exponential_map(dq, dctw, dcRw); 110 | 111 | cRw = dcRw.t() * cRw; // Update the pose 112 | ctw = dcRw.t() * (ctw - dctw); 113 | 114 | residual_prev = residual; // Memorize previous residual 115 | residual = e_q.dot(e_q); // Compute the actual residual 116 | 117 | } while (fabs(residual - residual_prev) > 0); 118 | //! [Gauss-Newton] 119 | } 120 | 121 | //! [Main function] 122 | int main() 123 | //! [Main function] 124 | { 125 | //! [Create data structures] 126 | std::vector< cv::Point3d > wX; 127 | std::vector< cv::Point2d > x; 128 | //! [Create data structures] 129 | 130 | //! [Simulation] 131 | // Ground truth pose used to generate the data 132 | cv::Mat ctw_truth = (cv::Mat_(3, 1) << -0.1, 0.1, 0.5); // Translation vector 133 | cv::Mat crw_truth = (cv::Mat_(3, 1) << CV_PI / 180 * (5), CV_PI / 180 * (0), CV_PI / 180 * (45)); // Rotation vector 134 | cv::Mat cRw_truth(3, 3, CV_64FC1); // Rotation matrix 135 | cv::Rodrigues(crw_truth, cRw_truth); 136 | 137 | // Input data: 3D coordinates of at least 4 points 138 | double L = 0.2; 139 | wX.push_back(cv::Point3d(-L, -L, 0)); // wX_0 (-L, -L, 0)^T 140 | wX.push_back(cv::Point3d(2 * L, -L, 0)); // wX_1 ( L, -L, 0)^T 141 | wX.push_back(cv::Point3d(L, L, 0)); // wX_2 ( L, L, 0)^T 142 | wX.push_back(cv::Point3d(-L, L, 0)); // wX_3 (-L, L, 0)^T 143 | 144 | // Input data: 2D coordinates of the points on the image plane 145 | for (int i = 0; i < wX.size(); i++) { 146 | cv::Mat cX = cRw_truth * cv::Mat(wX[i]) + ctw_truth; // Update cX, cY, cZ 147 | x.push_back(cv::Point2d(cX.at(0, 0) / cX.at(2, 0), 148 | cX.at(1, 0) / cX.at(2, 0))); // x = (cX/cZ, cY/cZ) 149 | } 150 | //! [Simulation] 151 | 152 | //! [Set pose initial value] 153 | // Initialize the pose to estimate near the solution 154 | cv::Mat ctw = (cv::Mat_(3, 1) << -0.05, 0.05, 0.45); // Initial translation 155 | cv::Mat crw = (cv::Mat_(3, 1) << CV_PI / 180 * (1), CV_PI / 180 * (0), CV_PI / 180 * (35)); // Initial rotation vector 156 | cv::Mat cRw = cv::Mat::eye(3, 3, CV_64FC1); // Rotation matrix set to identity 157 | cv::Rodrigues(crw, cRw); 158 | //! [Set pose initial value] 159 | 160 | //! [Call function] 161 | pose_gauss_newton(wX, x, ctw, cRw); 162 | //! [Call function] 163 | 164 | std::cout << "ctw (ground truth):\n" << ctw_truth << std::endl; 165 | std::cout << "ctw (from non linear method):\n" << ctw << std::endl; 166 | std::cout << "cRw (ground truth):\n" << cRw_truth << std::endl; 167 | std::cout << "cRw (from non linear method):\n" << cRw << std::endl; 168 | 169 | return 0; 170 | } 171 | -------------------------------------------------------------------------------- /visp/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10) 2 | project(camera_localization_visp) 3 | 4 | #---------------------------------------------------------------------- 5 | # Try to find ViSP third party library 6 | #---------------------------------------------------------------------- 7 | find_package(VISP) 8 | 9 | #---------------------------------------------------------------------- 10 | # Propagate in sub directories 11 | #---------------------------------------------------------------------- 12 | add_subdirectory(homography-basis) 13 | add_subdirectory(pose-basis) 14 | add_subdirectory(pose-mbt) 15 | add_subdirectory(template-matching) 16 | 17 | -------------------------------------------------------------------------------- /visp/homography-basis/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | 2 | cmake_minimum_required(VERSION 3.10) 3 | project(homography-basis-visp) 4 | 5 | find_package(VISP REQUIRED) 6 | 7 | include_directories(${VISP_INCLUDE_DIRS}) 8 | 9 | set(tutorial_cpp 10 | homography-dlt-visp.cpp 11 | pose-from-homography-dlt-visp.cpp) 12 | 13 | foreach(cpp ${tutorial_cpp}) 14 | # Compute the name of the binary to create 15 | get_filename_component(binary ${cpp} NAME_WE) 16 | 17 | # From source compile the binary and add link rules 18 | add_executable(${binary} ${cpp}) 19 | target_link_libraries(${binary} ${VISP_LIBRARIES}) 20 | endforeach() 21 | 22 | -------------------------------------------------------------------------------- /visp/homography-basis/homography-dlt-visp.cpp: -------------------------------------------------------------------------------- 1 | //! \example homography-dlt-visp.cpp 2 | //! [Include] 3 | #include 4 | #include 5 | #include 6 | //! [Include] 7 | 8 | //! [Estimation function] 9 | vpMatrix homography_dlt(const std::vector< vpColVector > &x1, const std::vector< vpColVector > &x2) 10 | //! [Estimation function] 11 | { 12 | //! [DLT] 13 | int npoints = (int)x1.size(); 14 | vpMatrix A(2*npoints, 9, 0.); 15 | 16 | // We need here to compute the SVD on a (n*2)*9 matrix (where n is 17 | // the number of points). if n == 4, the matrix has more columns 18 | // than rows. This kind of matrix is not supported by GSL for 19 | // SVD. The solution is to add an extra line with zeros 20 | if (npoints == 4) 21 | A.resize(2*npoints+1, 9); 22 | 23 | // Since the third line of matrix A is a linear combination of the first and second lines 24 | // (A is rank 2) we don't need to implement this third line 25 | for(int i = 0; i < npoints; i++) { // Update matrix A using eq. 23 26 | A[2*i][3] = -x1[i][0]; // -xi_1 27 | A[2*i][4] = -x1[i][1]; // -yi_1 28 | A[2*i][5] = -x1[i][2]; // -1 29 | A[2*i][6] = x2[i][1] * x1[i][0]; // yi_2 * xi_1 30 | A[2*i][7] = x2[i][1] * x1[i][1]; // yi_2 * yi_1 31 | A[2*i][8] = x2[i][1] * x1[i][2]; // yi_2 32 | 33 | A[2*i+1][0] = x1[i][0]; // xi_1 34 | A[2*i+1][1] = x1[i][1]; // yi_1 35 | A[2*i+1][2] = x1[i][2]; // 1 36 | A[2*i+1][6] = -x2[i][0] * x1[i][0]; // -xi_2 * xi_1 37 | A[2*i+1][7] = -x2[i][0] * x1[i][1]; // -xi_2 * yi_1 38 | A[2*i+1][8] = -x2[i][0] * x1[i][2]; // -xi_2 39 | } 40 | 41 | // Add an extra line with zero. 42 | if (npoints == 4) { 43 | for (int i=0; i < 9; i ++) { 44 | A[2*npoints][i] = 0; 45 | } 46 | } 47 | 48 | vpColVector D; 49 | vpMatrix V; 50 | 51 | A.svd(D, V); 52 | 53 | double smallestSv = D[0]; 54 | unsigned int indexSmallestSv = 0 ; 55 | for (unsigned int i = 1; i < D.size(); i++) { 56 | if ((D[i] < smallestSv) ) { 57 | smallestSv = D[i]; 58 | indexSmallestSv = i ; 59 | } 60 | } 61 | 62 | #if VISP_VERSION_INT >= VP_VERSION_INT(2, 10, 0) 63 | vpColVector h = V.getCol(indexSmallestSv); 64 | #else 65 | vpColVector h = V.column(indexSmallestSv + 1); // Deprecated since ViSP 2.10.0 66 | #endif 67 | if (h[8] < 0) // tz < 0 68 | h *=-1; 69 | //! [DLT] 70 | 71 | //! [Update homography matrix] 72 | vpMatrix _2H1(3, 3); 73 | for (int i = 0 ; i < 3 ; i++) 74 | for (int j = 0 ; j < 3 ; j++) 75 | _2H1[i][j] = h[3*i+j]; 76 | //! [Update homography matrix] 77 | 78 | return _2H1; 79 | } 80 | 81 | //! [Main function] 82 | int main() 83 | //! [Main function] 84 | { 85 | //! [Create data structures] 86 | int npoints = 4; 87 | 88 | std::vector< vpColVector > x1(npoints); 89 | std::vector< vpColVector > x2(npoints); 90 | 91 | std::vector< vpColVector > wX(npoints); // 3D points in the world plane 92 | std::vector< vpColVector > c1X(npoints); // 3D points in the camera frame 1 93 | std::vector< vpColVector > c2X(npoints); // 3D points in the camera frame 2 94 | 95 | for (int i = 0; i < npoints; i++) { 96 | x1[i].resize(3); 97 | x2[i].resize(3); 98 | 99 | wX[i].resize(4); 100 | c1X[i].resize(4); 101 | c2X[i].resize(4); 102 | } 103 | //! [Create data structures] 104 | 105 | //! [Simulation] 106 | // Ground truth pose used to generate the data 107 | vpHomogeneousMatrix c1Tw_truth(-0.1, 0.1, 1.2, vpMath::rad(5), vpMath::rad(0), vpMath::rad(45)); 108 | vpHomogeneousMatrix c2Tc1(0.01, 0.01, 0.2, vpMath::rad(0), vpMath::rad(3), vpMath::rad(5)); 109 | 110 | // Input data: 3D coordinates of at least 4 coplanar points 111 | double L = 0.2; 112 | wX[0][0] = -L; wX[0][1] = -L; wX[0][2] = 0; wX[0][3] = 1; // wX_0 ( -L, -L, 0, 1)^T 113 | wX[1][0] = 2*L; wX[1][1] = -L; wX[1][2] = 0; wX[1][3] = 1; // wX_1 (-2L, -L, 0, 1)^T 114 | wX[2][0] = L; wX[2][1] = L; wX[2][2] = 0; wX[2][3] = 1; // wX_2 ( L, L, 0, 1)^T 115 | wX[3][0] = -L; wX[3][1] = L; wX[3][2] = 0; wX[3][3] = 1; // wX_3 ( -L, L, 0, 1)^T 116 | 117 | // Input data: 2D coordinates of the points on the image plane 118 | for(int i = 0; i < npoints; i++) { 119 | c1X[i] = c1Tw_truth * wX[i]; // Update c1X, c1Y, c1Z 120 | x1[i][0] = c1X[i][0] / c1X[i][2]; // x1 = c1X/c1Z 121 | x1[i][1] = c1X[i][1] / c1X[i][2]; // y1 = c1Y/c1Z 122 | x1[i][2] = 1; 123 | 124 | c2X[i] = c2Tc1 * c1Tw_truth * wX[i]; // Update cX, cY, cZ 125 | x2[i][0] = c2X[i][0] / c2X[i][2]; // x2 = c1X/c1Z 126 | x2[i][1] = c2X[i][1] / c2X[i][2]; // y2 = c1Y/c1Z 127 | x2[i][2] = 1; 128 | } 129 | //! [Simulation] 130 | 131 | //! [Call function] 132 | vpMatrix _2H1 = homography_dlt(x1, x2); 133 | //! [Call function] 134 | 135 | std::cout << "2H1 (computed with DLT):\n" << _2H1 << std::endl; 136 | 137 | return 0; 138 | } 139 | -------------------------------------------------------------------------------- /visp/homography-basis/pose-from-homography-dlt-visp.cpp: -------------------------------------------------------------------------------- 1 | //! \example pose-from-homography-dlt-visp.cpp 2 | //! [Include] 3 | #include 4 | #include 5 | #include 6 | //! [Include] 7 | 8 | //! [Homography DLT function] 9 | vpMatrix homography_dlt(const std::vector< vpColVector > &x1, const std::vector< vpColVector > &x2) 10 | //! [Homography DLT function] 11 | { 12 | int npoints = (int)x1.size(); 13 | vpMatrix A(2*npoints, 9, 0.); 14 | 15 | // We need here to compute the SVD on a (n*2)*9 matrix (where n is 16 | // the number of points). if n == 4, the matrix has more columns 17 | // than rows. This kind of matrix is not supported by GSL for 18 | // SVD. The solution is to add an extra line with zeros 19 | if (npoints == 4) 20 | A.resize(2*npoints+1, 9); 21 | 22 | // Since the third line of matrix A is a linear combination of the first and second lines 23 | // (A is rank 2) we don't need to implement this third line 24 | for(int i = 0; i < npoints; i++) { // Update matrix A using eq. 23 25 | A[2*i][3] = -x1[i][0]; // -xi_1 26 | A[2*i][4] = -x1[i][1]; // -yi_1 27 | A[2*i][5] = -x1[i][2]; // -1 28 | A[2*i][6] = x2[i][1] * x1[i][0]; // yi_2 * xi_1 29 | A[2*i][7] = x2[i][1] * x1[i][1]; // yi_2 * yi_1 30 | A[2*i][8] = x2[i][1] * x1[i][2]; // yi_2 31 | 32 | A[2*i+1][0] = x1[i][0]; // xi_1 33 | A[2*i+1][1] = x1[i][1]; // yi_1 34 | A[2*i+1][2] = x1[i][2]; // 1 35 | A[2*i+1][6] = -x2[i][0] * x1[i][0]; // -xi_2 * xi_1 36 | A[2*i+1][7] = -x2[i][0] * x1[i][1]; // -xi_2 * yi_1 37 | A[2*i+1][8] = -x2[i][0] * x1[i][2]; // -xi_2 38 | } 39 | 40 | // Add an extra line with zero. 41 | if (npoints == 4) { 42 | for (unsigned int i=0; i < 9; i ++) { 43 | A[2*npoints][i] = 0; 44 | } 45 | } 46 | 47 | vpColVector D; 48 | vpMatrix V; 49 | 50 | A.svd(D, V); 51 | 52 | double smallestSv = D[0]; 53 | unsigned int indexSmallestSv = 0 ; 54 | for (unsigned int i = 1; i < D.size(); i++) { 55 | if ((D[i] < smallestSv) ) { 56 | smallestSv = D[i]; 57 | indexSmallestSv = i ; 58 | } 59 | } 60 | #if VISP_VERSION_INT >= VP_VERSION_INT(2, 10, 0) 61 | vpColVector h = V.getCol(indexSmallestSv); 62 | #else 63 | vpColVector h = V.column(indexSmallestSv + 1); // Deprecated since ViSP 2.10.0 64 | #endif 65 | 66 | if (h[8] < 0) // tz < 0 67 | h *=-1; 68 | 69 | vpMatrix _2H1(3, 3); 70 | for (int i = 0 ; i < 3 ; i++) 71 | for (int j = 0 ; j < 3 ; j++) 72 | _2H1[i][j] = h[3*i+j] ; 73 | 74 | return _2H1; 75 | } 76 | 77 | //! [Estimation function] 78 | vpHomogeneousMatrix pose_from_homography_dlt(const std::vector< vpColVector > &xw, const std::vector< vpColVector > &xo) 79 | //! [Estimation function] 80 | { 81 | //! [Homography estimation] 82 | vpMatrix oHw = homography_dlt(xw, xo); 83 | //! [Homography estimation] 84 | 85 | //! [Homography normalization] 86 | // Normalization to ensure that ||c1|| = 1 87 | double norm = sqrt(vpMath::sqr(oHw[0][0]) + vpMath::sqr(oHw[1][0]) + vpMath::sqr(oHw[2][0])) ; 88 | oHw /= norm; 89 | //! [Homography normalization] 90 | 91 | //! [Extract c1, c2] 92 | vpColVector c1 = oHw.getCol(0); 93 | vpColVector c2 = oHw.getCol(1); 94 | //! [Extract c1, c2] 95 | //! [Compute c3] 96 | vpColVector c3 = vpColVector::crossProd(c1, c2); 97 | //! [Compute c3] 98 | 99 | //! [Update pose] 100 | vpHomogeneousMatrix oTw; 101 | for(int i=0; i < 3; i++) { 102 | oTw[i][0] = c1[i]; 103 | oTw[i][1] = c2[i]; 104 | oTw[i][2] = c3[i]; 105 | oTw[i][3] = oHw[i][2]; 106 | } 107 | //! [Update pose] 108 | 109 | return oTw; 110 | } 111 | 112 | //! [Main function] 113 | int main() 114 | //! [Main function] 115 | { 116 | //! [Create data structures] 117 | int npoints = 4; 118 | 119 | std::vector< vpColVector > wX(npoints); // 3D points in the world plane 120 | 121 | std::vector< vpColVector > xw(npoints); // Normalized coordinates in the object frame 122 | std::vector< vpColVector > xo(npoints); // Normalized coordinates in the image plane 123 | 124 | for (int i = 0; i < npoints; i++) { 125 | xw[i].resize(3); 126 | xo[i].resize(3); 127 | 128 | wX[i].resize(4); 129 | } 130 | //! [Create data structures] 131 | 132 | //! [Simulation] 133 | // Ground truth pose used to generate the data 134 | vpHomogeneousMatrix oTw_truth(-0.1, 0.1, 1.2, vpMath::rad(5), vpMath::rad(0), vpMath::rad(45)); 135 | 136 | // Input data: 3D coordinates of at least 4 coplanar points 137 | double L = 0.2; 138 | wX[0][0] = -L; wX[0][1] = -L; wX[0][2] = 0; wX[0][3] = 1; // wX_0 ( -L, -L, 0, 1)^T 139 | wX[1][0] = 2*L; wX[1][1] = -L; wX[1][2] = 0; wX[1][3] = 1; // wX_1 (-2L, -L, 0, 1)^T 140 | wX[2][0] = L; wX[2][1] = L; wX[2][2] = 0; wX[2][3] = 1; // wX_2 ( L, L, 0, 1)^T 141 | wX[3][0] = -L; wX[3][1] = L; wX[3][2] = 0; wX[3][3] = 1; // wX_3 ( -L, L, 0, 1)^T 142 | 143 | // Input data: 2D coordinates of the points on the image plane 144 | for(int i = 0; i < npoints; i++) { 145 | vpColVector oX = oTw_truth * wX[i]; // Update oX, oY, oZ 146 | xo[i][0] = oX[0] / oX[2]; // xo = oX/oZ 147 | xo[i][1] = oX[1] / oX[2]; // yo = oY/oZ 148 | xo[i][2] = 1; 149 | 150 | xw[i][0] = wX[i][0]; // xw = wX 151 | xw[i][1] = wX[i][1]; // xw = wY 152 | xw[i][2] = 1; 153 | } 154 | //! [Simulation] 155 | 156 | //! [Call function] 157 | vpHomogeneousMatrix oTw = pose_from_homography_dlt(xw, xo); 158 | //! [Call function] 159 | 160 | std::cout << "oTw (ground truth):\n" << oTw_truth << std::endl; 161 | std::cout << "oTw (computed with homography DLT):\n" << oTw << std::endl; 162 | 163 | return 0; 164 | } 165 | -------------------------------------------------------------------------------- /visp/pose-basis/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10) 2 | project(pose-basis-visp) 3 | 4 | find_package(VISP REQUIRED) 5 | 6 | include_directories(${VISP_INCLUDE_DIRS}) 7 | 8 | set(tutorial_cpp 9 | pose-dlt-visp.cpp 10 | pose-dementhon-visp.cpp 11 | pose-gauss-newton-visp.cpp) 12 | 13 | foreach(cpp ${tutorial_cpp}) 14 | # Compute the name of the binary to create 15 | get_filename_component(binary ${cpp} NAME_WE) 16 | 17 | # From source compile the binary and add link rules 18 | add_executable(${binary} ${cpp}) 19 | target_link_libraries(${binary} ${VISP_LIBRARIES}) 20 | endforeach() 21 | 22 | -------------------------------------------------------------------------------- /visp/pose-basis/pose-dementhon-visp.cpp: -------------------------------------------------------------------------------- 1 | //! \example pose-dementhon-visp.cpp 2 | 3 | //! [Include] 4 | #include 5 | #include 6 | #include 7 | //! [Include] 8 | 9 | //! [Estimation function] 10 | vpHomogeneousMatrix pose_dementhon(const std::vector< vpColVector > &wX, const std::vector< vpColVector > &x) 11 | //! [Estimation function] 12 | { 13 | //! [POSIT] 14 | int npoints = (int)wX.size(); 15 | vpColVector r1, r2, r3; 16 | vpMatrix A(npoints, 4); 17 | for(int i = 0; i < npoints; i++) { 18 | for (int j = 0; j < 4; j++) { 19 | A[i][j] = wX[i][j]; 20 | } 21 | } 22 | vpMatrix Ap = A.pseudoInverse(); 23 | 24 | vpColVector eps(npoints); 25 | eps = 0; // Initialize epsilon_i = 0 26 | 27 | vpColVector Bx(npoints); 28 | vpColVector By(npoints); 29 | double tx, ty, tz; 30 | vpMatrix I, J; 31 | vpColVector Istar(3), Jstar(3); 32 | 33 | // POSIT loop 34 | for(unsigned int iter = 0; iter < 20; iter ++) { 35 | for(int i = 0; i < npoints; i++) { 36 | Bx[i] = x[i][0] * (eps[i] + 1.); 37 | By[i] = x[i][1] * (eps[i] + 1.); 38 | } 39 | 40 | I = Ap * Bx; // Notice that the pseudo inverse 41 | J = Ap * By; // of matrix A is a constant that has been precompiled. 42 | 43 | for (int i = 0; i < 3; i++) { 44 | Istar[i] = I[i][0]; 45 | Jstar[i] = J[i][0]; 46 | } 47 | 48 | // Estimation of the rotation matrix 49 | double normI = sqrt( Istar.sumSquare() ); 50 | double normJ = sqrt( Jstar.sumSquare() ); 51 | r1 = Istar / normI; 52 | r2 = Jstar / normJ; 53 | r3 = vpColVector::crossProd(r1, r2); 54 | 55 | // Estimation of the translation 56 | tz = 1/normI; 57 | tx = tz * I[3][0]; 58 | ty = tz * J[3][0]; 59 | 60 | // Update epsilon_i 61 | for(int i = 0; i < npoints; i++) 62 | eps[i] = (r3[0] * wX[i][0] + r3[1] * wX[i][1] + r3[2] * wX[i][2]) / tz; 63 | } 64 | //! [POSIT] 65 | 66 | //! [Update homogeneous matrix] 67 | vpHomogeneousMatrix cTw; 68 | // Update translation vector 69 | cTw[0][3] = tx; 70 | cTw[1][3] = ty; 71 | cTw[2][3] = tz; 72 | cTw[3][3] = 1.; 73 | // update rotation matrix 74 | for(int i = 0; i < 3; i++) { 75 | cTw[0][i] = r1[i]; 76 | cTw[1][i] = r2[i]; 77 | cTw[2][i] = r3[i]; 78 | } 79 | //! [Update homogeneous matrix] 80 | 81 | return cTw; 82 | } 83 | 84 | //! [Main function] 85 | int main() 86 | //! [Main function] 87 | { 88 | //! [Create data structures] 89 | int npoints = 4; 90 | std::vector< vpColVector > wX(npoints); // 3D points 91 | std::vector< vpColVector > x(npoints); // Their 2D coordinates in the image plane 92 | 93 | for (int i = 0; i < npoints; i++) { 94 | wX[i].resize(4); 95 | x[i].resize(3); 96 | } 97 | //! [Create data structures] 98 | 99 | //! [Simulation] 100 | // Ground truth pose used to generate the data 101 | vpHomogeneousMatrix cTw_truth(-0.1, 0.1, 0.5, vpMath::rad(5), vpMath::rad(0), vpMath::rad(45)); 102 | 103 | // Input data: 3D coordinates of at least 4 non coplanar points 104 | double L = 0.2; 105 | wX[0][0] = -L; wX[0][1] = -L; wX[0][2] = 0; wX[0][3] = 1; // wX_0 ( -L, -L, 0, 1)^T 106 | wX[1][0] = L; wX[1][1] = -L; wX[1][2] = 0.2; wX[1][3] = 1; // wX_1 ( L, -L, 0.2, 1)^T 107 | wX[2][0] = L; wX[2][1] = L; wX[2][2] = -0.1; wX[2][3] = 1; // wX_2 ( L, L, -0.1, 1)^T 108 | wX[3][0] = -L; wX[3][1] = L; wX[3][2] = 0; wX[3][3] = 1; // wX_3 ( -L, L, 0, 1)^T 109 | 110 | // Input data: 2D coordinates of the points on the image plane 111 | for(int i = 0; i < npoints; i++) { 112 | vpColVector cX = cTw_truth * wX[i]; // Update cX, cY, cZ 113 | x[i][0] = cX[0] / cX[2]; // x = cX/cZ 114 | x[i][1] = cX[1] / cX[2]; // y = cY/cZ 115 | x[i][2] = 1.; 116 | } 117 | //! [Simulation] 118 | 119 | //! [Call function] 120 | vpHomogeneousMatrix cTw = pose_dementhon(wX, x); 121 | //! [Call function] 122 | 123 | std::cout << "cTw (ground truth):\n" << cTw_truth << std::endl; 124 | std::cout << "cTw (from Dementhon):\n" << cTw << std::endl; 125 | 126 | return 0; 127 | } 128 | -------------------------------------------------------------------------------- /visp/pose-basis/pose-dlt-visp.cpp: -------------------------------------------------------------------------------- 1 | //! \example pose-dlt-visp.cpp 2 | 3 | //! [Include] 4 | #include 5 | #include 6 | #include 7 | //! [Include] 8 | 9 | //! [Estimation function] 10 | vpHomogeneousMatrix pose_dlt(const std::vector< vpColVector > &wX, const std::vector< vpColVector > &x) 11 | //! [Estimation function] 12 | { 13 | //! [DLT] 14 | int npoints = (int)wX.size(); 15 | vpMatrix A(2*npoints, 12, 0.); 16 | for(int i = 0; i < npoints; i++) { // Update matrix A using eq. 5 17 | A[2*i][0] = wX[i][0]; 18 | A[2*i][1] = wX[i][1]; 19 | A[2*i][2] = wX[i][2]; 20 | A[2*i][3] = 1; 21 | 22 | A[2*i+1][4] = wX[i][0]; 23 | A[2*i+1][5] = wX[i][1]; 24 | A[2*i+1][6] = wX[i][2]; 25 | A[2*i+1][7] = 1; 26 | 27 | A[2*i][8] = -x[i][0] * wX[i][0]; 28 | A[2*i][9] = -x[i][0] * wX[i][1]; 29 | A[2*i][10] = -x[i][0] * wX[i][2]; 30 | A[2*i][11] = -x[i][0]; 31 | 32 | A[2*i+1][8] = -x[i][1] * wX[i][0]; 33 | A[2*i+1][9] = -x[i][1] * wX[i][1]; 34 | A[2*i+1][10] = -x[i][1] * wX[i][2]; 35 | A[2*i+1][11] = -x[i][1]; 36 | } 37 | 38 | vpColVector D; 39 | vpMatrix V; 40 | 41 | A.svd(D, V); 42 | 43 | double smallestSv = D[0]; 44 | unsigned int indexSmallestSv = 0 ; 45 | for (unsigned int i = 1; i < D.size(); i++) { 46 | if ((D[i] < smallestSv) ) { 47 | smallestSv = D[i]; 48 | indexSmallestSv = i; 49 | } 50 | } 51 | 52 | #if VISP_VERSION_INT >= VP_VERSION_INT(2, 10, 0) 53 | vpColVector h = V.getCol(indexSmallestSv); 54 | #else 55 | vpColVector h = V.column(indexSmallestSv + 1); // Deprecated since ViSP 2.10.0 56 | #endif 57 | 58 | if (h[11] < 0) // tz < 0 59 | h *=-1; 60 | 61 | // Normalization to ensure that ||r3|| = 1 62 | double norm = sqrt(vpMath::sqr(h[8]) + vpMath::sqr(h[9]) + vpMath::sqr(h[10])); 63 | 64 | h /= norm; 65 | //! [DLT] 66 | 67 | //! [Update homogeneous matrix] 68 | vpHomogeneousMatrix cTw; 69 | for (int i = 0 ; i < 3 ; i++) 70 | for (int j = 0 ; j < 4 ; j++) 71 | cTw[i][j] = h[4*i+j]; 72 | //! [Update homogeneous matrix] 73 | 74 | return cTw; 75 | } 76 | 77 | //! [Main function] 78 | int main() 79 | //! [Main function] 80 | { 81 | //! [Create data structures] 82 | int npoints = 6; 83 | 84 | std::vector< vpColVector > wX(npoints); // 3D points 85 | std::vector< vpColVector > x(npoints); // Their 2D coordinates in the image plane 86 | 87 | for (int i = 0; i < npoints; i++) { 88 | wX[i].resize(4); 89 | x[i].resize(3); 90 | } 91 | //! [Create data structures] 92 | 93 | //! [Simulation] 94 | // Ground truth pose used to generate the data 95 | vpHomogeneousMatrix cTw_truth(-0.1, 0.1, 1.2, vpMath::rad(5), vpMath::rad(0), vpMath::rad(45)); 96 | 97 | // Input data: 3D coordinates of at least 6 non coplanar points 98 | double L = 0.2; 99 | wX[0][0] = -L; wX[0][1] = -L; wX[0][2] = 0; wX[0][3] = 1; // wX_0 ( -L, -L, 0, 1)^T 100 | wX[1][0] = 2*L; wX[1][1] = -L; wX[1][2] = 0.2; wX[1][3] = 1; // wX_1 (-2L, -L, 0.2, 1)^T 101 | wX[2][0] = L; wX[2][1] = L; wX[2][2] = 0.2; wX[2][3] = 1; // wX_2 ( L, L, 0.2, 1)^T 102 | wX[3][0] = -L; wX[3][1] = L; wX[3][2] = 0; wX[3][3] = 1; // wX_3 ( -L, L, 0, 1)^T 103 | wX[4][0] = -2*L; wX[4][1] = L; wX[4][2] = 0; wX[4][3] = 1; // wX_4 (-2L, L, 0, 1)^T 104 | wX[5][0] = 0; wX[5][1] = 0; wX[5][2] = 0.5; wX[5][3] = 1; // wX_5 ( 0, 0, 0.5, 1)^T 105 | 106 | // Input data: 2D coordinates of the points on the image plane 107 | for(int i = 0; i < npoints; i++) { 108 | vpColVector cX = cTw_truth * wX[i]; // Update cX, cY, cZ 109 | x[i][0] = cX[0] / cX[2]; // x = cX/cZ 110 | x[i][1] = cX[1] / cX[2]; // y = cY/cZ 111 | x[i][2] = 1; 112 | } 113 | //! [Simulation] 114 | 115 | //! [Call function] 116 | vpHomogeneousMatrix cTw = pose_dlt(wX, x); 117 | //! [Call function] 118 | 119 | std::cout << "cTw (ground truth):\n" << cTw_truth << std::endl; 120 | std::cout << "cTw (computed with DLT):\n" << cTw << std::endl; 121 | 122 | return 0; 123 | } 124 | -------------------------------------------------------------------------------- /visp/pose-basis/pose-gauss-newton-visp.cpp: -------------------------------------------------------------------------------- 1 | //! \example pose-gauss-newton-visp.cpp 2 | //! [Include] 3 | #include 4 | #include 5 | #include 6 | #include 7 | //! [Include] 8 | 9 | //! [Estimation function] 10 | vpHomogeneousMatrix pose_gauss_newton( 11 | #if VISP_VERSION_INT >= VP_VERSION_INT(2, 10, 0) 12 | const 13 | #endif 14 | std::vector< vpColVector > &wX, const std::vector< vpColVector > &x, const vpHomogeneousMatrix &cTw) 15 | //! [Estimation function] 16 | { 17 | //! [Gauss-Newton] 18 | int npoints = (int)wX.size(); 19 | vpMatrix J(2*npoints, 6), Jp; 20 | vpColVector err, sd(2*npoints), s(2*npoints), xq(npoints*2), xn(npoints*2); 21 | vpHomogeneousMatrix cTw_ = cTw; 22 | double residual=0, residual_prev, lambda = 0.25; 23 | 24 | // From input vector x = (x, y, 1)^T we create a new one xn = (x, y)^T to ease computation of e_q 25 | for (int i = 0; i < x.size(); i ++) { 26 | xn[i*2] = x[i][0]; // x 27 | xn[i*2+1] = x[i][1]; // y 28 | } 29 | 30 | // Iterative Gauss-Newton minimization loop 31 | do { 32 | for (int i = 0; i < npoints; i++) { 33 | vpColVector cX = cTw_ * wX[i]; // Update cX, cY, cZ 34 | 35 | double Xi = cX[0]; 36 | double Yi = cX[1]; 37 | double Zi = cX[2]; 38 | 39 | double xi = Xi / Zi; 40 | double yi = Yi / Zi; 41 | 42 | // Update x(q) 43 | xq[i*2] = xi; // x(q) = cX/cZ 44 | xq[i*2+1] = yi; // y(q) = cY/cZ 45 | 46 | // Update J using equation (11) 47 | J[i*2][0] = -1 / Zi; // -1/cZ 48 | J[i*2][1] = 0; // 0 49 | J[i*2][2] = xi / Zi; // x/cZ 50 | J[i*2][3] = xi * yi; // xy 51 | J[i*2][4] = -(1 + xi * xi); // -(1+x^2) 52 | J[i*2][5] = yi; // y 53 | 54 | J[i*2+1][0] = 0; // 0 55 | J[i*2+1][1] = -1 / Zi; // -1/cZ 56 | J[i*2+1][2] = yi / Zi; // y/cZ 57 | J[i*2+1][3] = 1 + yi * yi; // 1+y^2 58 | J[i*2+1][4] = -xi * yi; // -xy 59 | J[i*2+1][5] = -xi; // -x 60 | } 61 | 62 | vpColVector e_q = xq - xn; // Equation (7) 63 | 64 | J.pseudoInverse(Jp); // Compute pseudo inverse of the Jacobian 65 | vpColVector dq = -lambda * Jp * e_q; // Equation (10) 66 | 67 | cTw_ = vpExponentialMap::direct(dq).inverse() * cTw_; // Update the pose 68 | 69 | residual_prev = residual; // Memorize previous residual 70 | residual = e_q.sumSquare(); // Compute the actual residual 71 | 72 | } while (fabs(residual - residual_prev) > 0); 73 | //! [Gauss-Newton] 74 | //! [Return cTw] 75 | return cTw_; 76 | //! [Return cTw] 77 | } 78 | 79 | //! [Main function] 80 | int main() 81 | //! [Main function] 82 | { 83 | //! [Create data structures] 84 | int npoints = 4; 85 | std::vector< vpColVector > wX(npoints); 86 | std::vector< vpColVector > x(npoints); 87 | 88 | for (int i = 0; i < npoints; i++) { 89 | wX[i].resize(4); 90 | x[i].resize(3); 91 | } 92 | //! [Create data structures] 93 | 94 | //! [Simulation] 95 | // Ground truth pose used to generate the data 96 | vpHomogeneousMatrix cTw_truth(-0.1, 0.1, 0.5, vpMath::rad(5), vpMath::rad(0), vpMath::rad(45)); 97 | 98 | // Input data: 3D coordinates of at least 4 points 99 | double L = 0.2; 100 | wX[0][0] = -L; wX[0][1] = -L; wX[0][2] = 0; wX[0][3] = 1; // wX_0 ( -L, -L, 0, 1)^T 101 | wX[1][0] = 2*L; wX[1][1] = -L; wX[1][2] = 0; wX[1][3] = 1; // wX_1 (-2L, -L, 0, 1)^T 102 | wX[2][0] = L; wX[2][1] = L; wX[2][2] = 0; wX[2][3] = 1; // wX_2 ( L, L, 0, 1)^T 103 | wX[3][0] = -L; wX[3][1] = L; wX[3][2] = 0; wX[3][3] = 1; // wX_3 ( -L, L, 0, 1)^T 104 | 105 | // Input data: 2D coordinates of the points on the image plane 106 | for(int i = 0; i < npoints; i++) { 107 | vpColVector cX = cTw_truth * wX[i]; // Update cX, cY, cZ 108 | x[i][0] = cX[0] / cX[2]; // x = cX/cZ 109 | x[i][1] = cX[1] / cX[2]; // y = cY/cZ 110 | x[i][2] = 1; 111 | } 112 | //! [Simulation] 113 | 114 | //! [Set pose initial value] 115 | // Initialize the pose to estimate near the solution 116 | vpHomogeneousMatrix cTw(-0.05, 0.05, 0.45, vpMath::rad(1), vpMath::rad(0), vpMath::rad(35)); 117 | //! [Set pose initial value] 118 | 119 | //! [Call function] 120 | cTw = pose_gauss_newton(wX, x, cTw); 121 | //! [Call function] 122 | 123 | std::cout << "cTw (ground truth):\n" << cTw_truth << std::endl; 124 | std::cout << "cTw (from non linear method):\n" << cTw << std::endl; 125 | 126 | return 0; 127 | } 128 | -------------------------------------------------------------------------------- /visp/pose-mbt/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10) 2 | project(pose-mbt-visp) 3 | 4 | find_package(VISP REQUIRED) 5 | 6 | # set the list of source files 7 | set(tutorial_cpp 8 | pose-mbt-visp.cpp) 9 | 10 | list(APPEND tutorial_data "${CMAKE_CURRENT_SOURCE_DIR}/teabox.mpg") 11 | list(APPEND tutorial_data "${CMAKE_CURRENT_SOURCE_DIR}/teabox.xml") 12 | list(APPEND tutorial_data "${CMAKE_CURRENT_SOURCE_DIR}/teabox.cao") 13 | list(APPEND tutorial_data "${CMAKE_CURRENT_SOURCE_DIR}/teabox.wrl") 14 | list(APPEND tutorial_data "${CMAKE_CURRENT_SOURCE_DIR}/teabox.init") 15 | list(APPEND tutorial_data "${CMAKE_CURRENT_SOURCE_DIR}/teabox.ppm") 16 | 17 | include_directories(${VISP_INCLUDE_DIRS}) 18 | 19 | foreach(cpp ${tutorial_cpp}) 20 | # Compute the name of the binary to create 21 | get_filename_component(binary ${cpp} NAME_WE) 22 | 23 | # From source compile the binary and add link rules 24 | add_executable(${binary} ${cpp}) 25 | target_link_libraries(${binary} ${VISP_LIBRARIES}) 26 | endforeach() 27 | 28 | # Copy the data files to the same location than the target 29 | foreach(data ${tutorial_data}) 30 | visp_copy_data(pose-mbt-visp.cpp ${data}) 31 | endforeach() 32 | -------------------------------------------------------------------------------- /visp/pose-mbt/pose-mbt-visp.cpp: -------------------------------------------------------------------------------- 1 | //! \example pose-mbt-visp.cpp 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | //! [Include] 8 | #include 9 | //! [Include] 10 | #include 11 | 12 | int main(int argc, char** argv) 13 | { 14 | #if defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100) || defined(VISP_HAVE_FFMPEG) 15 | try { 16 | std::string videoname = "teabox.mpg"; 17 | 18 | for (int i=0; i] [--help]\n" << std::endl; 23 | return 0; 24 | } 25 | } 26 | std::string parentname = vpIoTools::getParent(videoname); 27 | std::string objectname = vpIoTools::getNameWE(videoname); 28 | 29 | if(! parentname.empty()) 30 | objectname = parentname + "/" + objectname; 31 | 32 | std::cout << "Video name: " << videoname << std::endl; 33 | std::cout << "Tracker requested config files: " << objectname 34 | << ".[init," 35 | #ifdef VISP_HAVE_XML2 36 | << "xml," 37 | #endif 38 | << "cao or wrl]" << std::endl; 39 | std::cout << "Tracker optional config files: " << objectname << ".[ppm]" << std::endl; 40 | 41 | //! [Image] 42 | vpImage I; 43 | vpCameraParameters cam; 44 | //! [Image] 45 | //! [cTw] 46 | vpHomogeneousMatrix cTw; 47 | //! [cTw] 48 | 49 | vpVideoReader g; 50 | g.setFileName(videoname); 51 | g.open(I); 52 | 53 | #if defined(VISP_HAVE_X11) 54 | vpDisplayX display(I,100,100,"Model-based edge tracker");; 55 | #elif defined(VISP_HAVE_GDI) 56 | vpDisplayGDI display(I,100,100,"Model-based edge tracker");; 57 | #elif defined(VISP_HAVE_OPENCV) 58 | vpDisplayOpenCV display(I,100,100,"Model-based edge tracker");; 59 | #else 60 | std::cout << "No image viewer is available..." << std::endl; 61 | #endif 62 | 63 | //! [Constructor] 64 | vpMbEdgeTracker tracker; 65 | //! [Constructor] 66 | bool usexml = false; 67 | #ifdef VISP_HAVE_XML2 68 | //! [Load xml] 69 | if(vpIoTools::checkFilename(objectname + ".xml")) { 70 | tracker.loadConfigFile(objectname + ".xml"); 71 | usexml = true; 72 | } 73 | //! [Load xml] 74 | #endif 75 | if (! usexml) { 76 | //! [Set parameters] 77 | vpMe me; 78 | me.setMaskSize(5); 79 | me.setMaskNumber(180); 80 | me.setRange(8); 81 | me.setThreshold(10000); 82 | me.setMu1(0.5); 83 | me.setMu2(0.5); 84 | me.setSampleStep(4); 85 | me.setNbTotalSample(250); 86 | tracker.setMovingEdge(me); 87 | cam.initPersProjWithoutDistortion(839, 839, 325, 243); 88 | tracker.setCameraParameters(cam); 89 | //! [Set parameters] 90 | } 91 | //! [Load cao] 92 | if(vpIoTools::checkFilename(objectname + ".cao")) 93 | tracker.loadModel(objectname + ".cao"); 94 | //! [Load cao] 95 | //! [Load wrl] 96 | else if(vpIoTools::checkFilename(objectname + ".wrl")) 97 | tracker.loadModel(objectname + ".wrl"); 98 | //! [Load wrl] 99 | //! [Set display] 100 | tracker.setDisplayFeatures(true); 101 | //! [Set display] 102 | //! [Init] 103 | tracker.initClick(I, objectname + ".init", true); 104 | //! [Init] 105 | 106 | while(! g.end()){ 107 | g.acquire(I); 108 | vpDisplay::display(I); 109 | //! [Track] 110 | tracker.track(I); 111 | //! [Track] 112 | //! [Get pose] 113 | tracker.getPose(cTw); 114 | //! [Get pose] 115 | //! [Display] 116 | tracker.getCameraParameters(cam); 117 | tracker.display(I, cTw, cam, vpColor::red, 2); 118 | vpDisplay::displayFrame(I, cTw, cam, 0.025, vpColor::none, 3); 119 | //! [Display] 120 | vpDisplay::displayText(I, 10, 10, "A click to exit...", vpColor::red); 121 | vpDisplay::flush(I); 122 | if (vpDisplay::getClick(I, false)) 123 | break; 124 | } 125 | vpDisplay::getClick(I); 126 | } 127 | catch(vpException e) { 128 | std::cout << "Catch an exception: " << e << std::endl; 129 | } 130 | #else 131 | (void)argc; 132 | (void)argv; 133 | std::cout << "Install OpenCV or ffmpeg and rebuild ViSP to use this example." << std::endl; 134 | #endif 135 | } 136 | -------------------------------------------------------------------------------- /visp/pose-mbt/teabox.cao: -------------------------------------------------------------------------------- 1 | V1 2 | # 3D Points 3 | 8 # Number of points 4 | 0 0 0 # Point 0: X Y Z 5 | 0 0 -0.08 6 | 0.165 0 -0.08 7 | 0.165 0 0 8 | 0.165 0.068 0 9 | 0.165 0.068 -0.08 10 | 0 0.068 -0.08 11 | 0 0.068 0 # Point 7 12 | # 3D Lines 13 | 0 # Number of lines 14 | # Faces from 3D lines 15 | 0 # Number of faces 16 | # Faces from 3D points 17 | 6 # Number of faces 18 | 4 0 1 2 3 # Face 0: [number of points] [index of the 3D points]... 19 | 4 1 6 5 2 20 | 4 4 5 6 7 21 | 4 0 3 4 7 22 | 4 5 4 3 2 23 | 4 0 7 6 1 # Face 5 24 | # 3D cylinders 25 | 0 # Number of cylinders 26 | # 3D circles 27 | 0 # Number of circles 28 | -------------------------------------------------------------------------------- /visp/pose-mbt/teabox.init: -------------------------------------------------------------------------------- 1 | 4 # Number of points 2 | 0 0 0 # Point 0 3 | 0.165 0 0 # Point 3 4 | 0.165 0 -0.08 # Point 2 5 | 0.165 0.068 -0.08 # Point 5 6 | -------------------------------------------------------------------------------- /visp/pose-mbt/teabox.mpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lagadic/camera_localization/d4197a8320c974e51f47cb7c7adb4dc681c805d9/visp/pose-mbt/teabox.mpg -------------------------------------------------------------------------------- /visp/pose-mbt/teabox.ppm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lagadic/camera_localization/d4197a8320c974e51f47cb7c7adb4dc681c805d9/visp/pose-mbt/teabox.ppm -------------------------------------------------------------------------------- /visp/pose-mbt/teabox.wrl: -------------------------------------------------------------------------------- 1 | #VRML V2.0 utf8 2 | 3 | DEF fst_0 Group { 4 | children [ 5 | 6 | # Object "teabox" 7 | Shape { 8 | 9 | geometry DEF cube IndexedFaceSet { 10 | 11 | coord Coordinate { 12 | point [ 13 | 0 0 0 , 14 | 0 0 -0.08, 15 | 0.165 0 -0.08, 16 | 0.165 0 0 , 17 | 0.165 0.068 0 , 18 | 0.165 0.068 -0.08, 19 | 0 0.068 -0.08, 20 | 0 0.068 0 ] 21 | } 22 | 23 | coordIndex [ 24 | 0,1,2,3,-1, 25 | 1,6,5,2,-1, 26 | 4,5,6,7,-1, 27 | 0,3,4,7,-1, 28 | 5,4,3,2,-1, 29 | 0,7,6,1,-1]} 30 | } 31 | 32 | ] 33 | } 34 | -------------------------------------------------------------------------------- /visp/pose-mbt/teabox.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 5 6 | 180 7 | 8 | 9 | 8 10 | 11 | 12 | 10000 13 | 0.5 14 | 0.5 15 | 16 | 17 | 18 | 4 19 | 250 20 | 21 | 22 | 325.66776 23 | 243.69727 24 | 839.21470 25 | 839.44555 26 | 27 | 28 | 70 29 | 80 30 | 0.1 31 | 100 32 | 1 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /visp/template-matching/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | 2 | cmake_minimum_required(VERSION 3.10) 3 | project(template-matching-visp) 4 | 5 | find_package(VISP REQUIRED) 6 | 7 | # set the list of source files 8 | set(tutorial_cpp 9 | template-matching-visp.cpp) 10 | 11 | list(APPEND tutorial_data "${CMAKE_CURRENT_SOURCE_DIR}/bruegel.mpg") 12 | 13 | include_directories(${VISP_INCLUDE_DIRS}) 14 | 15 | foreach(cpp ${tutorial_cpp}) 16 | # Compute the name of the binary to create 17 | get_filename_component(binary ${cpp} NAME_WE) 18 | 19 | # From source compile the binary and add link rules 20 | add_executable(${binary} ${cpp}) 21 | target_link_libraries(${binary} ${VISP_LIBRARIES}) 22 | endforeach() 23 | 24 | # Copy the data files to the same location than the target 25 | foreach(data ${tutorial_data}) 26 | visp_copy_data(template-matching-visp.cpp ${data}) 27 | endforeach() 28 | -------------------------------------------------------------------------------- /visp/template-matching/bruegel.mpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lagadic/camera_localization/d4197a8320c974e51f47cb7c7adb4dc681c805d9/visp/template-matching/bruegel.mpg -------------------------------------------------------------------------------- /visp/template-matching/template-matching-visp.cpp: -------------------------------------------------------------------------------- 1 | //! \example template-matching-visp.cpp 2 | #include 3 | #include 4 | #include 5 | #include 6 | //! [Include] 7 | #include 8 | #include 9 | //! [Include] 10 | #include 11 | 12 | int main(int argc, char** argv) 13 | { 14 | #if defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100) || defined(VISP_HAVE_FFMPEG) 15 | std::string videoname = "bruegel.mpg"; 16 | 17 | for (int i=0; i] [--help]\n" << std::endl; 22 | return 0; 23 | } 24 | } 25 | 26 | std::cout << "Video name: " << videoname << std::endl; 27 | 28 | vpImage I; 29 | 30 | vpVideoReader g; 31 | g.setFileName(videoname); 32 | g.open(I); 33 | 34 | #if defined(VISP_HAVE_X11) 35 | vpDisplayX display; 36 | #elif defined(VISP_HAVE_GDI) 37 | vpDisplayGDI display; 38 | #elif defined(VISP_HAVE_OPENCV) 39 | vpDisplayOpenCV display; 40 | #else 41 | std::cout << "No image viewer is available..." << std::endl; 42 | #endif 43 | 44 | display.init(I, 100, 100, "Template tracker"); 45 | vpDisplay::display(I); 46 | vpDisplay::flush(I); 47 | 48 | //! [Construction] 49 | vpTemplateTrackerWarpHomography warp; 50 | vpTemplateTrackerSSDForwardAdditional tracker(&warp); 51 | //! [Construction] 52 | 53 | tracker.setSampling(3, 3); 54 | tracker.setLambda(0.001); 55 | tracker.setIterationMax(50); 56 | tracker.setPyramidal(2, 1); 57 | 58 | //! [Init] 59 | tracker.initClick(I); 60 | //! [Init] 61 | 62 | while(! g.end()){ 63 | g.acquire(I); 64 | vpDisplay::display(I); 65 | 66 | //! [Track] 67 | tracker.track(I); 68 | //! [Track] 69 | 70 | //! [Homography] 71 | vpColVector p = tracker.getp(); 72 | vpHomography H = warp.getHomography(p); 73 | std::cout << "Homography: \n" << H << std::endl; 74 | //! [Homography] 75 | 76 | //! [Display] 77 | tracker.display(I, vpColor::red); 78 | //! [Display] 79 | 80 | if (vpDisplay::getClick(I, false)) 81 | break; 82 | 83 | vpDisplay::flush(I); 84 | } 85 | vpDisplay::getClick(I); 86 | #endif 87 | } 88 | --------------------------------------------------------------------------------