├── .gitignore ├── .gitmodules ├── CMakeLists.txt ├── LICENSE ├── README.md ├── cameraParams.mat ├── data ├── board.mp4 ├── demo.gif └── pixel-xl.mp4 ├── ggt.m ├── helpers ├── DataHash.m ├── nativeUpdateWeightsCpu.m ├── nativeUpdateWeightsGpu.m └── plotSystem.m ├── main.m ├── p2c.cu └── p2c.h /.gitignore: -------------------------------------------------------------------------------- 1 | .cache/* 2 | *.asv 3 | result.avi 4 | 5 | # Created by https://www.gitignore.io/api/c,c++,cuda,cmake,visualstudio 6 | 7 | ### C ### 8 | # Prerequisites 9 | *.d 10 | 11 | # Object files 12 | *.o 13 | *.ko 14 | *.obj 15 | *.elf 16 | 17 | # Linker output 18 | *.ilk 19 | *.map 20 | *.exp 21 | 22 | # Precompiled Headers 23 | *.gch 24 | *.pch 25 | 26 | # Libraries 27 | *.lib 28 | *.a 29 | *.la 30 | *.lo 31 | 32 | # Shared objects (inc. Windows DLLs) 33 | *.dll 34 | *.so 35 | *.so.* 36 | *.dylib 37 | 38 | # Executables 39 | *.exe 40 | *.out 41 | *.app 42 | *.i*86 43 | *.x86_64 44 | *.hex 45 | 46 | # Debug files 47 | *.dSYM/ 48 | *.su 49 | *.idb 50 | *.pdb 51 | 52 | # Kernel Module Compile Results 53 | *.mod* 54 | *.cmd 55 | .tmp_versions/ 56 | modules.order 57 | Module.symvers 58 | Mkfile.old 59 | dkms.conf 60 | 61 | ### C++ ### 62 | # Prerequisites 63 | 64 | # Compiled Object files 65 | *.slo 66 | 67 | # Precompiled Headers 68 | 69 | # Compiled Dynamic libraries 70 | 71 | # Fortran module files 72 | *.mod 73 | *.smod 74 | 75 | # Compiled Static libraries 76 | *.lai 77 | 78 | # Executables 79 | 80 | ### CMake ### 81 | CMakeCache.txt 82 | CMakeFiles 83 | CMakeScripts 84 | Testing 85 | Makefile 86 | cmake_install.cmake 87 | install_manifest.txt 88 | compile_commands.json 89 | CTestTestfile.cmake 90 | build 91 | 92 | ### CUDA ### 93 | *.i 94 | *.ii 95 | *.gpu 96 | *.ptx 97 | *.cubin 98 | *.fatbin 99 | 100 | ### VisualStudio ### 101 | ## Ignore Visual Studio temporary files, build results, and 102 | ## files generated by popular Visual Studio add-ons. 103 | ## 104 | ## Get latest from https://github.com/github/gitignore/blob/master/VisualStudio.gitignore 105 | 106 | # User-specific files 107 | *.suo 108 | *.user 109 | *.userosscache 110 | *.sln.docstates 111 | 112 | # User-specific files (MonoDevelop/Xamarin Studio) 113 | *.userprefs 114 | 115 | # Build results 116 | [Dd]ebug/ 117 | [Dd]ebugPublic/ 118 | [Rr]elease/ 119 | [Rr]eleases/ 120 | x64/ 121 | x86/ 122 | bld/ 123 | [Bb]in/ 124 | [Oo]bj/ 125 | [Ll]og/ 126 | 127 | # Visual Studio 2015 cache/options directory 128 | .vs/ 129 | # Uncomment if you have tasks that create the project's static files in wwwroot 130 | #wwwroot/ 131 | 132 | # MSTest test Results 133 | [Tt]est[Rr]esult*/ 134 | [Bb]uild[Ll]og.* 135 | 136 | # NUNIT 137 | *.VisualState.xml 138 | TestResult.xml 139 | 140 | # Build Results of an ATL Project 141 | [Dd]ebugPS/ 142 | [Rr]eleasePS/ 143 | dlldata.c 144 | 145 | # .NET Core 146 | project.lock.json 147 | project.fragment.lock.json 148 | artifacts/ 149 | **/Properties/launchSettings.json 150 | 151 | *_i.c 152 | *_p.c 153 | *_i.h 154 | *.meta 155 | *.pgc 156 | *.pgd 157 | *.rsp 158 | *.sbr 159 | *.tlb 160 | *.tli 161 | *.tlh 162 | *.tmp 163 | *.tmp_proj 164 | *.log 165 | *.vspscc 166 | *.vssscc 167 | .builds 168 | *.pidb 169 | *.svclog 170 | *.scc 171 | 172 | # Chutzpah Test files 173 | _Chutzpah* 174 | 175 | # Visual C++ cache files 176 | ipch/ 177 | *.aps 178 | *.ncb 179 | *.opendb 180 | *.opensdf 181 | *.sdf 182 | *.cachefile 183 | *.VC.db 184 | *.VC.VC.opendb 185 | 186 | # Visual Studio profiler 187 | *.psess 188 | *.vsp 189 | *.vspx 190 | *.sap 191 | 192 | # TFS 2012 Local Workspace 193 | $tf/ 194 | 195 | # Guidance Automation Toolkit 196 | *.gpState 197 | 198 | # ReSharper is a .NET coding add-in 199 | _ReSharper*/ 200 | *.[Rr]e[Ss]harper 201 | *.DotSettings.user 202 | 203 | # JustCode is a .NET coding add-in 204 | .JustCode 205 | 206 | # TeamCity is a build add-in 207 | _TeamCity* 208 | 209 | # DotCover is a Code Coverage Tool 210 | *.dotCover 211 | 212 | # Visual Studio code coverage results 213 | *.coverage 214 | *.coveragexml 215 | 216 | # NCrunch 217 | _NCrunch_* 218 | .*crunch*.local.xml 219 | nCrunchTemp_* 220 | 221 | # MightyMoose 222 | *.mm.* 223 | AutoTest.Net/ 224 | 225 | # Web workbench (sass) 226 | .sass-cache/ 227 | 228 | # Installshield output folder 229 | [Ee]xpress/ 230 | 231 | # DocProject is a documentation generator add-in 232 | DocProject/buildhelp/ 233 | DocProject/Help/*.HxT 234 | DocProject/Help/*.HxC 235 | DocProject/Help/*.hhc 236 | DocProject/Help/*.hhk 237 | DocProject/Help/*.hhp 238 | DocProject/Help/Html2 239 | DocProject/Help/html 240 | 241 | # Click-Once directory 242 | publish/ 243 | 244 | # Publish Web Output 245 | *.[Pp]ublish.xml 246 | *.azurePubxml 247 | # TODO: Uncomment the next line to ignore your web deploy settings. 248 | # By default, sensitive information, such as encrypted password 249 | # should be stored in the .pubxml.user file. 250 | #*.pubxml 251 | *.pubxml.user 252 | *.publishproj 253 | 254 | # Microsoft Azure Web App publish settings. Comment the next line if you want to 255 | # checkin your Azure Web App publish settings, but sensitive information contained 256 | # in these scripts will be unencrypted 257 | PublishScripts/ 258 | 259 | # NuGet Packages 260 | *.nupkg 261 | # The packages folder can be ignored because of Package Restore 262 | **/packages/* 263 | # except build/, which is used as an MSBuild target. 264 | !**/packages/build/ 265 | # Uncomment if necessary however generally it will be regenerated when needed 266 | #!**/packages/repositories.config 267 | # NuGet v3's project.json files produces more ignorable files 268 | *.nuget.props 269 | *.nuget.targets 270 | 271 | # Microsoft Azure Build Output 272 | csx/ 273 | *.build.csdef 274 | 275 | # Microsoft Azure Emulator 276 | ecf/ 277 | rcf/ 278 | 279 | # Windows Store app package directories and files 280 | AppPackages/ 281 | BundleArtifacts/ 282 | Package.StoreAssociation.xml 283 | _pkginfo.txt 284 | 285 | # Visual Studio cache files 286 | # files ending in .cache can be ignored 287 | *.[Cc]ache 288 | # but keep track of directories ending in .cache 289 | !*.[Cc]ache/ 290 | 291 | # Others 292 | ClientBin/ 293 | ~$* 294 | *~ 295 | *.dbmdl 296 | *.dbproj.schemaview 297 | *.jfm 298 | *.pfx 299 | *.publishsettings 300 | orleans.codegen.cs 301 | 302 | # Since there are multiple workflows, uncomment next line to ignore bower_components 303 | # (https://github.com/github/gitignore/pull/1529#issuecomment-104372622) 304 | #bower_components/ 305 | 306 | # RIA/Silverlight projects 307 | Generated_Code/ 308 | 309 | # Backup & report files from converting an old project file 310 | # to a newer Visual Studio version. Backup files are not needed, 311 | # because we have git ;-) 312 | _UpgradeReport_Files/ 313 | Backup*/ 314 | UpgradeLog*.XML 315 | UpgradeLog*.htm 316 | 317 | # SQL Server files 318 | *.mdf 319 | *.ldf 320 | *.ndf 321 | 322 | # Business Intelligence projects 323 | *.rdl.data 324 | *.bim.layout 325 | *.bim_*.settings 326 | 327 | # Microsoft Fakes 328 | FakesAssemblies/ 329 | 330 | # GhostDoc plugin setting file 331 | *.GhostDoc.xml 332 | 333 | # Node.js Tools for Visual Studio 334 | .ntvs_analysis.dat 335 | node_modules/ 336 | 337 | # Typescript v1 declaration files 338 | typings/ 339 | 340 | # Visual Studio 6 build log 341 | *.plg 342 | 343 | # Visual Studio 6 workspace options file 344 | *.opt 345 | 346 | # Visual Studio 6 auto-generated workspace file (contains which files were open etc.) 347 | *.vbw 348 | 349 | # Visual Studio LightSwitch build output 350 | **/*.HTMLClient/GeneratedArtifacts 351 | **/*.DesktopClient/GeneratedArtifacts 352 | **/*.DesktopClient/ModelManifest.xml 353 | **/*.Server/GeneratedArtifacts 354 | **/*.Server/ModelManifest.xml 355 | _Pvt_Extensions 356 | 357 | # Paket dependency manager 358 | .paket/paket.exe 359 | paket-files/ 360 | 361 | # FAKE - F# Make 362 | .fake/ 363 | 364 | # JetBrains Rider 365 | .idea/ 366 | *.sln.iml 367 | 368 | # CodeRush 369 | .cr/ 370 | 371 | # Python Tools for Visual Studio (PTVS) 372 | __pycache__/ 373 | *.pyc 374 | 375 | # Cake - Uncomment if you are using it 376 | # tools/** 377 | # !tools/packages.config 378 | 379 | # Telerik's JustMock configuration file 380 | *.jmconfig 381 | 382 | # BizTalk build output 383 | *.btp.cs 384 | *.btm.cs 385 | *.odx.cs 386 | *.xsd.cs 387 | 388 | ### VisualStudio Patch ### 389 | # By default, sensitive information, such as encrypted password 390 | # should be stored in the .pubxml.user file. 391 | 392 | # End of https://www.gitignore.io/api/c,c++,cuda,cmake,visualstudio 393 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "helpers/glm"] 2 | path = helpers/glm 3 | url = https://github.com/g-truc/glm 4 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | CMAKE_MINIMUM_REQUIRED( VERSION 3.2 FATAL_ERROR ) 2 | 3 | #=============================================================================== 4 | # project wide settings 5 | PROJECT( pf-localization ) 6 | 7 | SET( PFL_ROOT "${CMAKE_CURRENT_SOURCE_DIR}" CACHE PATH "root directory" ) 8 | SET( CMAKE_INSTALL_PREFIX "${PFL_ROOT}" ) 9 | set( CXX_STANDARD_REQUIRED ON ) 10 | set( CMAKE_CXX_STANDARD 11 ) 11 | 12 | #=============================================================================== 13 | # packages required to compile the project 14 | FIND_PACKAGE( CUDA REQUIRED ) 15 | 16 | #=============================================================================== 17 | # compiler specific configuration 18 | IF( MSVC ) 19 | ADD_DEFINITIONS( 20 | -DNOMINMAX 21 | -DVC_EXTRALEAN 22 | -DWINVER=0x0601 23 | -D_WIN32_WINNT=0x0601 24 | -DWIN32_LEAN_AND_MEAN 25 | -D_CRT_SECURE_NO_WARNINGS ) 26 | ENDIF() 27 | ADD_DEFINITIONS( -DBUILDING_DLL ) 28 | 29 | LIST( APPEND CUDA_NVCC_FLAGS "-Wno-deprecated-gpu-targets" ) 30 | INCLUDE_DIRECTORIES( "helpers/glm" ) 31 | 32 | #=============================================================================== 33 | # Build targets 34 | CUDA_ADD_LIBRARY( p2c "p2c.cu" "p2c.h" MODULE ) 35 | 36 | #=============================================================================== 37 | # Packaging and Installing 38 | INSTALL( TARGETS p2c DESTINATION bin ) 39 | INSTALL( FILES p2c.h DESTINATION bin ) 40 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | GNU LESSER GENERAL PUBLIC LICENSE 2 | Version 3, 29 June 2007 3 | 4 | Copyright (C) 2007 Free Software Foundation, Inc. 5 | Everyone is permitted to copy and distribute verbatim copies 6 | of this license document, but changing it is not allowed. 7 | 8 | 9 | This version of the GNU Lesser General Public License incorporates 10 | the terms and conditions of version 3 of the GNU General Public 11 | License, supplemented by the additional permissions listed below. 12 | 13 | 0. Additional Definitions. 14 | 15 | As used herein, "this License" refers to version 3 of the GNU Lesser 16 | General Public License, and the "GNU GPL" refers to version 3 of the GNU 17 | General Public License. 18 | 19 | "The Library" refers to a covered work governed by this License, 20 | other than an Application or a Combined Work as defined below. 21 | 22 | An "Application" is any work that makes use of an interface provided 23 | by the Library, but which is not otherwise based on the Library. 24 | Defining a subclass of a class defined by the Library is deemed a mode 25 | of using an interface provided by the Library. 26 | 27 | A "Combined Work" is a work produced by combining or linking an 28 | Application with the Library. The particular version of the Library 29 | with which the Combined Work was made is also called the "Linked 30 | Version". 31 | 32 | The "Minimal Corresponding Source" for a Combined Work means the 33 | Corresponding Source for the Combined Work, excluding any source code 34 | for portions of the Combined Work that, considered in isolation, are 35 | based on the Application, and not on the Linked Version. 36 | 37 | The "Corresponding Application Code" for a Combined Work means the 38 | object code and/or source code for the Application, including any data 39 | and utility programs needed for reproducing the Combined Work from the 40 | Application, but excluding the System Libraries of the Combined Work. 41 | 42 | 1. Exception to Section 3 of the GNU GPL. 43 | 44 | You may convey a covered work under sections 3 and 4 of this License 45 | without being bound by section 3 of the GNU GPL. 46 | 47 | 2. Conveying Modified Versions. 48 | 49 | If you modify a copy of the Library, and, in your modifications, a 50 | facility refers to a function or data to be supplied by an Application 51 | that uses the facility (other than as an argument passed when the 52 | facility is invoked), then you may convey a copy of the modified 53 | version: 54 | 55 | a) under this License, provided that you make a good faith effort to 56 | ensure that, in the event an Application does not supply the 57 | function or data, the facility still operates, and performs 58 | whatever part of its purpose remains meaningful, or 59 | 60 | b) under the GNU GPL, with none of the additional permissions of 61 | this License applicable to that copy. 62 | 63 | 3. Object Code Incorporating Material from Library Header Files. 64 | 65 | The object code form of an Application may incorporate material from 66 | a header file that is part of the Library. You may convey such object 67 | code under terms of your choice, provided that, if the incorporated 68 | material is not limited to numerical parameters, data structure 69 | layouts and accessors, or small macros, inline functions and templates 70 | (ten or fewer lines in length), you do both of the following: 71 | 72 | a) Give prominent notice with each copy of the object code that the 73 | Library is used in it and that the Library and its use are 74 | covered by this License. 75 | 76 | b) Accompany the object code with a copy of the GNU GPL and this license 77 | document. 78 | 79 | 4. Combined Works. 80 | 81 | You may convey a Combined Work under terms of your choice that, 82 | taken together, effectively do not restrict modification of the 83 | portions of the Library contained in the Combined Work and reverse 84 | engineering for debugging such modifications, if you also do each of 85 | the following: 86 | 87 | a) Give prominent notice with each copy of the Combined Work that 88 | the Library is used in it and that the Library and its use are 89 | covered by this License. 90 | 91 | b) Accompany the Combined Work with a copy of the GNU GPL and this license 92 | document. 93 | 94 | c) For a Combined Work that displays copyright notices during 95 | execution, include the copyright notice for the Library among 96 | these notices, as well as a reference directing the user to the 97 | copies of the GNU GPL and this license document. 98 | 99 | d) Do one of the following: 100 | 101 | 0) Convey the Minimal Corresponding Source under the terms of this 102 | License, and the Corresponding Application Code in a form 103 | suitable for, and under terms that permit, the user to 104 | recombine or relink the Application with a modified version of 105 | the Linked Version to produce a modified Combined Work, in the 106 | manner specified by section 6 of the GNU GPL for conveying 107 | Corresponding Source. 108 | 109 | 1) Use a suitable shared library mechanism for linking with the 110 | Library. A suitable mechanism is one that (a) uses at run time 111 | a copy of the Library already present on the user's computer 112 | system, and (b) will operate properly with a modified version 113 | of the Library that is interface-compatible with the Linked 114 | Version. 115 | 116 | e) Provide Installation Information, but only if you would otherwise 117 | be required to provide such information under section 6 of the 118 | GNU GPL, and only to the extent that such information is 119 | necessary to install and execute a modified version of the 120 | Combined Work produced by recombining or relinking the 121 | Application with a modified version of the Linked Version. (If 122 | you use option 4d0, the Installation Information must accompany 123 | the Minimal Corresponding Source and Corresponding Application 124 | Code. If you use option 4d1, you must provide the Installation 125 | Information in the manner specified by section 6 of the GNU GPL 126 | for conveying Corresponding Source.) 127 | 128 | 5. Combined Libraries. 129 | 130 | You may place library facilities that are a work based on the 131 | Library side by side in a single library together with other library 132 | facilities that are not Applications and are not covered by this 133 | License, and convey such a combined library under terms of your 134 | choice, if you do both of the following: 135 | 136 | a) Accompany the combined library with a copy of the same work based 137 | on the Library, uncombined with any other library facilities, 138 | conveyed under the terms of this License. 139 | 140 | b) Give prominent notice with the combined library that part of it 141 | is a work based on the Library, and explaining where to find the 142 | accompanying uncombined form of the same work. 143 | 144 | 6. Revised Versions of the GNU Lesser General Public License. 145 | 146 | The Free Software Foundation may publish revised and/or new versions 147 | of the GNU Lesser General Public License from time to time. Such new 148 | versions will be similar in spirit to the present version, but may 149 | differ in detail to address new problems or concerns. 150 | 151 | Each version is given a distinguishing version number. If the 152 | Library as you received it specifies that a certain numbered version 153 | of the GNU Lesser General Public License "or any later version" 154 | applies to it, you have the option of following the terms and 155 | conditions either of that published version or of any later version 156 | published by the Free Software Foundation. If the Library as you 157 | received it does not specify a version number of the GNU Lesser 158 | General Public License, you may choose any version of the GNU Lesser 159 | General Public License ever published by the Free Software Foundation. 160 | 161 | If the Library as you received it specifies that a proxy can decide 162 | whether future versions of the GNU Lesser General Public License shall 163 | apply, that proxy's public statement of acceptance of any version is 164 | permanent authorization for you to choose that version for the 165 | Library. 166 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # pf-localization 2 | Localization using a Particle Filter (and random walk model) 3 | ![Localization demo](data/demo.gif?raw=true) 4 | 5 | ## What is this? 6 | This is a Cuda-C++ library to localize a camera, pointed at a checkerboard pattern without solving the [PnP problem](https://en.wikipedia.org/wiki/Perspective-n-Point) and using a particle filter. Solving the PnP problem is not always feasible (specifically in case of real-time SLAM), therefore this project aims at localizing the camera rotation and translation with a [Random Walk model](https://en.wikipedia.org/wiki/Random_walk). 7 | 8 | ## Building 9 | In order to build this library, you need a system with CMake and Cuda SDK 7+ installed. 10 | 11 | ```bash 12 | cd 13 | mkdir build; 14 | cd build; 15 | cmake .. -G "Visual Studio 14 Win64" 16 | cmake --build . --config Release 17 | cmake --build . --target INSTALL 18 | ``` 19 | 20 | ## Running 21 | Matlab is required (but not necessary) to run this library with a sample dataset (in `data/board.mp4`). The Matlab script that uses this library is `main.m`. The first time you run the Matlab file, it'll take some time to build a cache of video's ground truth data using PnP solver of Matlab. 22 | 23 | Matlab code also ships with a Matlab implementation of the project. However, If you like to run this code with Cuda, you must have a Nvidia Cuda-capable graphics card. 24 | 25 | 26 | ## Reference 27 | This code is an adaptation of the theories provided in the following paper: 28 | 29 | ``` 30 | @article{doi: 10.1117/1.JEI.23.1.013029, 31 | author = { Seok-Han Lee}, 32 | title = {Real-time camera tracking using a particle filter combined with unscented Kalman filters}, 33 | journal = {Journal of Electronic Imaging}, 34 | volume = {23}, 35 | number = {}, 36 | pages = {23 - 23 - 19}, 37 | year = {2014}, 38 | doi = {10.1117/1.JEI.23.1.013029}, 39 | URL = {http://dx.doi.org/10.1117/1.JEI.23.1.013029}, 40 | eprint = {} 41 | } 42 | ``` 43 | -------------------------------------------------------------------------------- /cameraParams.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/3p3r/pf-localization/1228c6971bfb0f4d336f28e3165d4869a77ac0ac/cameraParams.mat -------------------------------------------------------------------------------- /data/board.mp4: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/3p3r/pf-localization/1228c6971bfb0f4d336f28e3165d4869a77ac0ac/data/board.mp4 -------------------------------------------------------------------------------- /data/demo.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/3p3r/pf-localization/1228c6971bfb0f4d336f28e3165d4869a77ac0ac/data/demo.gif -------------------------------------------------------------------------------- /data/pixel-xl.mp4: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/3p3r/pf-localization/1228c6971bfb0f4d336f28e3165d4869a77ac0ac/data/pixel-xl.mp4 -------------------------------------------------------------------------------- /ggt.m: -------------------------------------------------------------------------------- 1 | function out = ggt(videoFilePath) 2 | % GGT loads ground truth data from input video file. The input video file 3 | % must contain a valid Matlab-checkerboard (odd-by-even sized) and be 4 | % visible in ALL frames compeletely. 5 | % C = GGT('file.mp4') loads ground truth camera path from 'file.mp4'. 6 | addpath helpers; 7 | 8 | cacheDir = '.cache'; 9 | fileKey = DataHash(videoFilePath); 10 | cacheKey = [cacheDir '/' fileKey '.mat']; 11 | if ~exist(cacheDir, 'dir'); mkdir('.cache'); end; 12 | 13 | if exist(cacheKey, 'file') 14 | disp(['Cache exists for "' videoFilePath '" in: "' cacheKey '"']); 15 | load(cacheKey, 'out'); 16 | return; 17 | end 18 | 19 | disp(['Creating cache for "' videoFilePath '" in: "' cacheKey '"']); 20 | vr = VideoReader(videoFilePath); 21 | load('cameraParams.mat'); 22 | total = round(vr.Duration * vr.FrameRate); 23 | 24 | n=0; 25 | out = []; 26 | index = 1; 27 | while(hasFrame(vr)) 28 | msg = ['Creating cache entry: ' num2str(index) '/' num2str(total)]; 29 | fprintf(repmat('\b',1,n)); 30 | fprintf(msg); 31 | n=numel(msg); 32 | 33 | entry = {}; 34 | % frame's index 35 | entry.Index = index; 36 | % color frame 37 | frame = rgb2gray(readFrame(vr)); 38 | % grayscale frame 39 | frameUndistorted = undistortImage(frame, cameraParams); 40 | % checkerboard pattern 2d points and detected board size 41 | [entry.ImagePoints, entry.BoardSize] = detectCheckerboardPoints(frameUndistorted); 42 | % checkerboard 3d points 43 | entry.WorldPoints = zeros(prod(entry.BoardSize - 1),3); 44 | entry.WorldPoints(:,1:2) = generateCheckerboardPoints(entry.BoardSize, 30); % board's blocks are 30mm 45 | % camera's extrinsics which represent the coordinate system transformation from world coordinates to camera coordinates (true). 46 | [entry.ExtrinsicsRotationTrue, entry.ExtrinsicsTranslationTrue] = ... 47 | extrinsics(entry.ImagePoints, entry.WorldPoints(:,1:2), cameraParams); 48 | % camera's orientation and location, represent the 3-D camera pose in the world coordinates 49 | [entry.PoseRotationTrue, entry.PoseTranslationTrue] = ... 50 | extrinsicsToCameraPose(entry.ExtrinsicsRotationTrue, entry.ExtrinsicsTranslationTrue); 51 | % camera's extrinsics which represent the coordinate system transformation from world coordinates to camera coordinates (estimate). 52 | [pR,pt] = estimateWorldCameraPose(entry.ImagePoints, entry.WorldPoints, cameraParams); 53 | [entry.ExtrinsicsRotationEst, entry.ExtrinsicsTranslationEst] = cameraPoseToExtrinsics(pR, pt); 54 | % convert to quaternions, since we estimate quaternions in this implementation 55 | entry.ExtrinsicsRotationTrue = rotm2quat(entry.ExtrinsicsRotationTrue)'; 56 | entry.ExtrinsicsRotationEst = rotm2quat(entry.ExtrinsicsRotationEst)'; 57 | entry.PoseRotationTrue = rotm2quat(entry.PoseRotationTrue)'; 58 | % we use Nx1 sizes for everything by convention in this implementation 59 | entry.ExtrinsicsTranslationTrue = entry.ExtrinsicsTranslationTrue'; 60 | entry.ExtrinsicsTranslationEst = entry.ExtrinsicsTranslationEst'; 61 | entry.PoseTranslationTrue = entry.PoseTranslationTrue'; 62 | out = [out entry]; 63 | index = index + 1; 64 | end 65 | fprintf('\n'); 66 | save(cacheKey, 'out'); 67 | disp(['Cache created for "' videoFilePath '" in: "' cacheKey '"']); 68 | 69 | rmpath helpers; 70 | end -------------------------------------------------------------------------------- /helpers/DataHash.m: -------------------------------------------------------------------------------- 1 | function Hash = DataHash(Data, Opt) 2 | % DATAHASH - Checksum for Matlab array of any type 3 | % This function creates a hash value for an input of any type. The type and 4 | % dimensions of the input are considered as default, such that UINT8([0,0]) and 5 | % UINT16(0) have different hash values. Nested STRUCTs and CELLs are parsed 6 | % recursively. 7 | % 8 | % Hash = DataHash(Data, Opt) 9 | % INPUT: 10 | % Data: Array of these built-in types: 11 | % (U)INT8/16/32/64, SINGLE, DOUBLE, (real/complex, full/sparse) 12 | % CHAR, LOGICAL, CELL (nested), STRUCT (scalar or array, nested), 13 | % function_handle. 14 | % Opt: Struct to specify the hashing algorithm and the output format. 15 | % Opt and all its fields are optional. 16 | % Opt.Method: String, known methods for Java 1.6 (Matlab 2011b): 17 | % 'SHA-1', 'SHA-256', 'SHA-384', 'SHA-512', 'MD2', 'MD5'. 18 | % Call DataHash without inputs to get a list of available methods. 19 | % Default: 'MD5'. 20 | % Opt.Format: String specifying the output format: 21 | % 'hex', 'HEX': Lower/uppercase hexadecimal string. 22 | % 'double', 'uint8': Numerical vector. 23 | % 'base64': Base64 encoded string, only printable ASCII 24 | % characters, shorter than 'hex', no padding. 25 | % Default: 'hex'. 26 | % Opt.Input: Type of the input as string, not case-sensitive: 27 | % 'array': The contents, type and size of the input [Data] are 28 | % considered for the creation of the hash. Nested CELLs 29 | % and STRUCT arrays are parsed recursively. Empty arrays of 30 | % different type reply different hashs. 31 | % 'file': [Data] is treated as file name and the hash is calculated 32 | % for the files contents. 33 | % 'bin': [Data] is a numerical, LOGICAL or CHAR array. Only the 34 | % binary contents of the array is considered, such that 35 | % e.g. empty arrays of different type reply the same hash. 36 | % 'ascii': Same as 'bin', but only the 8-bit ASCII part of the 16-bit 37 | % Matlab CHARs is considered. 38 | % Default: 'array'. 39 | % 40 | % OUTPUT: 41 | % Hash: String, DOUBLE or UINT8 vector. The length depends on the hashing 42 | % method. 43 | % 44 | % EXAMPLES: 45 | % % Default: MD5, hex: 46 | % DataHash([]) % 5b302b7b2099a97ba2a276640a192485 47 | % % MD5, Base64: 48 | % Opt = struct('Format', 'base64', 'Method', 'MD5'); 49 | % DataHash(int32(1:10), Opt) % +tJN9yeF89h3jOFNN55XLg 50 | % % SHA-1, Base64: 51 | % S.a = uint8([]); 52 | % S.b = {{1:10}, struct('q', uint64(415))}; 53 | % Opt.Method = 'SHA-1'; 54 | % Opt.Format = 'HEX'; 55 | % DataHash(S, Opt) % 18672BE876463B25214CA9241B3C79CC926F3093 56 | % % SHA-1 of binary values: 57 | % Opt = struct('Method', 'SHA-1', 'Input', 'bin'); 58 | % DataHash(1:8, Opt) % 826cf9d3a5d74bbe415e97d4cecf03f445f69225 59 | % % SHA-256, consider ASCII part only (Matlab's CHAR has 16 bits!): 60 | % Opt.Method = 'SHA-256'; 61 | % Opt.Input = 'ascii'; 62 | % DataHash('abc', Opt) 63 | % % ba7816bf8f01cfea414140de5dae2223b00361a396177a9cb410ff61f20015ad 64 | % % Or equivalently: 65 | % Opt.Input = 'bin'; 66 | % DataHash(uint8('abc'), Opt) 67 | % 68 | % NOTES: 69 | % Function handles and user-defined objects cannot be converted uniquely: 70 | % - The subfunction ConvertFuncHandle uses the built-in function FUNCTIONS, 71 | % but the replied struct can depend on the Matlab version. 72 | % - It is tried to convert objects to UINT8 streams in the subfunction 73 | % ConvertObject. A conversion by STRUCT() might be more appropriate. 74 | % Adjust these subfunctions on demand. 75 | % 76 | % MATLAB CHARs have 16 bits! Use Opt.Input='ascii' for comparisons with e.g. 77 | % online hash generators. 78 | % 79 | % Matt Raum suggested this for e.g. user-defined objects: 80 | % DataHash(getByteStreamFromArray(Data) 81 | % This works very well, but unfortunately getByteStreamFromArray is 82 | % undocumented, such that it might vanish in the future or reply different 83 | % output. 84 | % 85 | % For arrays the calculated hash value might be changed in new versions. 86 | % Calling this function without inputs replies the version of the hash. 87 | % 88 | % The C-Mex function GetMD5 is 2 to 100 times faster, but obtains MD5 only: 89 | % http://www.mathworks.com/matlabcentral/fileexchange/25921 90 | % 91 | % Tested: Matlab 7.7, 7.8, 7.13, 8.6, WinXP/32, Win7/64 92 | % Author: Jan Simon, Heidelberg, (C) 2011-2016 matlab.2010(a)n(MINUS)simon.de 93 | % 94 | % See also: TYPECAST, CAST. 95 | % 96 | % Michael Kleder, "Compute Hash", no structs and cells: 97 | % http://www.mathworks.com/matlabcentral/fileexchange/8944 98 | % Tim, "Serialize/Deserialize", converts structs and cells to a byte stream: 99 | % http://www.mathworks.com/matlabcentral/fileexchange/29457 100 | 101 | % $JRev: R-H V:033 Sum:R+m7rAPNLvlw Date:18-Jun-2016 14:33:17 $ 102 | % $License: BSD (use/copy/change/redistribute on own risk, mention the author) $ 103 | % $File: Tools\GLFile\DataHash.m $ 104 | % History: 105 | % 001: 01-May-2011 21:52, First version. 106 | % 007: 10-Jun-2011 10:38, [Opt.Input], binary data, complex values considered. 107 | % 011: 26-May-2012 15:57, Fixed: Failed for binary input and empty data. 108 | % 014: 04-Nov-2012 11:37, Consider Mex-, MDL- and P-files also. 109 | % Thanks to David (author 243360), who found this bug. 110 | % Jan Achterhold (author 267816) suggested to consider Java objects. 111 | % 016: 01-Feb-2015 20:53, Java heap space exhausted for large files. 112 | % Now files are process in chunks to save memory. 113 | % 017: 15-Feb-2015 19:40, Collsions: Same hash for different data. 114 | % Examples: zeros(1,1) and zeros(1,1,0) 115 | % complex(0) and zeros(1,1,0,0) 116 | % Now the number of dimensions is included, to avoid this. 117 | % 022: 30-Mar-2015 00:04, Bugfix: Failed for strings and [] without TYPECASTX. 118 | % Ross found these 2 bugs, which occur when TYPECASTX is not installed. 119 | % If you need the base64 format padded with '=' characters, adjust 120 | % fBase64_enc as you like. 121 | % 026: 29-Jun-2015 00:13, Changed hash for STRUCTs. 122 | % Struct arrays are analysed field by field now, which is much faster. 123 | % 027: 13-Sep-2015 19:03, 'ascii' input as abbrev. for Input='bin' and UINT8(). 124 | % 028: 15-Oct-2015 23:11, Example values in help section updated to v022. 125 | % 029: 16-Oct-2015 22:32, Use default options for empty input. 126 | % 031: 28-Feb-2016 15:10, New hash value to get same reply as GetMD5. 127 | % New Matlab version (at least 2015b) use a fast method for TYPECAST, such 128 | % that calling James Tursa's TYPECASTX is not needed anymore. 129 | % Matlab 6.5 not supported anymore: MException for CATCH. 130 | % 033: 18-Jun-2016 14:28, BUGFIX: Failed on empty files. 131 | % Thanks to Christian (AuthorID 2918599). 132 | 133 | % OPEN BUGS: 134 | % Nath wrote: 135 | % function handle refering to struct containing the function will create 136 | % infinite loop. Is there any workaround ? 137 | % Example: 138 | % d= dynamicprops(); 139 | % addprop(d,'f'); 140 | % d.f= @(varargin) struct2cell(d); 141 | % DataHash(d.f) % infinite loop 142 | % This is caught with an error message concerning the recursion limit now. 143 | 144 | % Main function: =============================================================== 145 | % Default options: ------------------------------------------------------------- 146 | Method = 'MD5'; 147 | OutFormat = 'hex'; 148 | isFile = false; 149 | isBin = false; 150 | 151 | % Check number and type of inputs: --------------------------------------------- 152 | nArg = nargin; 153 | if nArg == 2 154 | if isa(Opt, 'struct') == 0 % Bad type of 2nd input: 155 | Error_L('BadInput2', '2nd input [Opt] must be a struct.'); 156 | end 157 | 158 | % Specify hash algorithm: 159 | if isfield(Opt, 'Method') && ~isempty(Opt.Method) % Short-circuiting 160 | Method = upper(Opt.Method); 161 | end 162 | 163 | % Specify output format: 164 | if isfield(Opt, 'Format') && ~isempty(Opt.Format) % Short-circuiting 165 | OutFormat = Opt.Format; 166 | end 167 | 168 | % Check if the Input type is specified - default: 'array': 169 | if isfield(Opt, 'Input') && ~isempty(Opt.Input) % Short-circuiting 170 | if strcmpi(Opt.Input, 'File') 171 | if ischar(Data) == 0 172 | Error_L('CannotOpen', '1st input FileName must be a string'); 173 | end 174 | isFile = true; 175 | 176 | elseif strncmpi(Opt.Input, 'bin', 3) % Accept 'binary' also 177 | if (isnumeric(Data) || ischar(Data) || islogical(Data)) == 0 || ... 178 | issparse(Data) 179 | Error_L('BadDataType', ... 180 | '1st input must be numeric, CHAR or LOGICAL for binary input.'); 181 | end 182 | isBin = true; 183 | 184 | elseif strncmpi(Opt.Input, 'asc', 3) % 8-bit ASCII characters 185 | if ~ischar(Data) 186 | Error_L('BadDataType', ... 187 | '1st input must be a CHAR for the input type ASCII.'); 188 | end 189 | isBin = true; 190 | Data = uint8(Data); 191 | end 192 | end 193 | 194 | elseif nArg == 0 % Reply version of this function: 195 | R = Version_L; 196 | 197 | if nargout == 0 198 | disp(R); 199 | else 200 | Hash = R; 201 | end 202 | 203 | return; 204 | 205 | elseif nArg ~= 1 % Bad number of arguments: 206 | Error_L('BadNInput', '1 or 2 inputs required.'); 207 | end 208 | 209 | % Create the engine: ----------------------------------------------------------- 210 | try 211 | Engine = java.security.MessageDigest.getInstance(Method); 212 | catch 213 | Error_L('BadInput2', 'Invalid algorithm: [%s].', Method); 214 | end 215 | 216 | % Create the hash value: ------------------------------------------------------- 217 | if isFile 218 | % Open the file: 219 | FID = fopen(Data, 'r'); 220 | if FID < 0 221 | % Check existence of file: 222 | Found = FileExist_L(Data); 223 | if Found 224 | Error_L('CantOpenFile', 'Cannot open file: %s.', Data); 225 | else 226 | Error_L('FileNotFound', 'File not found: %s.', Data); 227 | end 228 | end 229 | 230 | % Read file in chunks to save memory and Java heap space: 231 | Chunk = 1e6; % Fastest for 1e6 on Win7/64, HDD 232 | Count = Chunk; % Dummy value to satisfy WHILE condition 233 | while Count == Chunk 234 | [Data, Count] = fread(FID, Chunk, '*uint8'); 235 | if Count ~= 0 % Avoid error for empty file 236 | Engine.update(Data); 237 | end 238 | end 239 | fclose(FID); 240 | 241 | % Calculate the hash: 242 | Hash = typecast(Engine.digest, 'uint8'); 243 | 244 | elseif isBin % Contents of an elementary array, type tested already: 245 | if isempty(Data) % Nothing to do, Engine.update fails for empty input! 246 | Hash = typecast(Engine.digest, 'uint8'); 247 | else % Matlab's TYPECAST is less elegant: 248 | if isnumeric(Data) 249 | if isreal(Data) 250 | Engine.update(typecast(Data(:), 'uint8')); 251 | else 252 | Engine.update(typecast(real(Data(:)), 'uint8')); 253 | Engine.update(typecast(imag(Data(:)), 'uint8')); 254 | end 255 | elseif islogical(Data) % TYPECAST cannot handle LOGICAL 256 | Engine.update(typecast(uint8(Data(:)), 'uint8')); 257 | elseif ischar(Data) % TYPECAST cannot handle CHAR 258 | Engine.update(typecast(uint16(Data(:)), 'uint8')); 259 | % Bugfix: Line removed 260 | end 261 | Hash = typecast(Engine.digest, 'uint8'); 262 | end 263 | else % Array with type: 264 | Engine = CoreHash(Data, Engine); 265 | Hash = typecast(Engine.digest, 'uint8'); 266 | end 267 | 268 | % Convert hash specific output format: ----------------------------------------- 269 | switch OutFormat 270 | case 'hex' 271 | Hash = sprintf('%.2x', double(Hash)); 272 | case 'HEX' 273 | Hash = sprintf('%.2X', double(Hash)); 274 | case 'double' 275 | Hash = double(reshape(Hash, 1, [])); 276 | case 'uint8' 277 | Hash = reshape(Hash, 1, []); 278 | case 'base64' 279 | Hash = fBase64_enc(double(Hash)); 280 | otherwise 281 | Error_L('BadOutFormat', ... 282 | '[Opt.Format] must be: HEX, hex, uint8, double, base64.'); 283 | end 284 | 285 | % return; 286 | 287 | % ****************************************************************************** 288 | function Engine = CoreHash(Data, Engine) 289 | % This methods uses the slower TYPECAST of Matlab 290 | 291 | % Consider the type and dimensions of the array to distinguish arrays with the 292 | % same data, but different shape: [0 x 0] and [0 x 1], [1,2] and [1;2], 293 | % DOUBLE(0) and SINGLE([0,0]): 294 | % < v016: [class, size, data]. BUG! 0 and zeros(1,1,0) had the same hash! 295 | % >= v016: [class, ndims, size, data] 296 | Engine.update([uint8(class(Data)), ... 297 | typecast(uint64([ndims(Data), size(Data)]), 'uint8')]); 298 | 299 | if issparse(Data) % Sparse arrays to struct: 300 | [S.Index1, S.Index2, S.Value] = find(Data); 301 | Engine = CoreHash(S, Engine); 302 | elseif isstruct(Data) % Hash for all array elements and fields: 303 | F = sort(fieldnames(Data)); % Ignore order of fields 304 | for iField = 1:length(F) % Loop over fields 305 | aField = F{iField}; 306 | Engine.update(uint8(aField)); 307 | for iS = 1:numel(Data) % Loop over elements of struct array 308 | Engine = CoreHash(Data(iS).(aField), Engine); 309 | end 310 | end 311 | elseif iscell(Data) % Get hash for all cell elements: 312 | for iS = 1:numel(Data) 313 | Engine = CoreHash(Data{iS}, Engine); 314 | end 315 | elseif isempty(Data) % Nothing to do 316 | elseif isnumeric(Data) 317 | if isreal(Data) 318 | Engine.update(typecast(Data(:), 'uint8')); 319 | else 320 | Engine.update(typecast(real(Data(:)), 'uint8')); 321 | Engine.update(typecast(imag(Data(:)), 'uint8')); 322 | end 323 | elseif islogical(Data) % TYPECAST cannot handle LOGICAL 324 | Engine.update(typecast(uint8(Data(:)), 'uint8')); 325 | elseif ischar(Data) % TYPECAST cannot handle CHAR 326 | Engine.update(typecast(uint16(Data(:)), 'uint8')); 327 | elseif isa(Data, 'function_handle') 328 | Engine = CoreHash(ConvertFuncHandle(Data), Engine); 329 | elseif (isobject(Data) || isjava(Data)) && ismethod(Data, 'hashCode') 330 | Engine = CoreHash(char(Data.hashCode), Engine); 331 | else % Most likely a user-defined object: 332 | try 333 | BasicData = ConvertObject(Data); 334 | catch ME 335 | error(['JSimon:', mfilename, ':BadDataType'], ... 336 | '%s: Cannot create elementary array for type: %s\n %s', ... 337 | mfilename, class(Data), ME.message); 338 | end 339 | 340 | try 341 | Engine = CoreHash(BasicData, Engine); 342 | catch ME 343 | if strcmpi(ME.identifier, 'MATLAB:recursionLimit') 344 | ME = MException(['JSimon:', mfilename, ':RecursiveType'], ... 345 | '%s: Cannot create hash for recursive data type: %s', ... 346 | mfilename, class(Data)); 347 | end 348 | throw(ME); 349 | end 350 | end 351 | 352 | % return; 353 | 354 | % ****************************************************************************** 355 | function FuncKey = ConvertFuncHandle(FuncH) 356 | % The subfunction ConvertFuncHandle converts function_handles to a struct 357 | % using the Matlab function FUNCTIONS. The output of this function changes 358 | % with the Matlab version, such that DataHash(@sin) replies different hashes 359 | % under Matlab 6.5 and 2009a. 360 | % An alternative is using the function name and name of the file for 361 | % function_handles, but this is not unique for nested or anonymous functions. 362 | % If the MATLABROOT is removed from the file's path, at least the hash of 363 | % Matlab's toolbox functions is (usually!) not influenced by the version. 364 | % Finally I'm in doubt if there is a unique method to hash function handles. 365 | % Please adjust the subfunction ConvertFuncHandles to your needs. 366 | 367 | % The Matlab version influences the conversion by FUNCTIONS: 368 | % 1. The format of the struct replied FUNCTIONS is not fixed, 369 | % 2. The full paths of toolbox function e.g. for @mean differ. 370 | FuncKey = functions(FuncH); 371 | 372 | % Include modification file time and file size. Suggested by Aslak Grinsted: 373 | if ~isempty(FuncKey.file) 374 | d = dir(FuncKey.file); 375 | if ~isempty(d) 376 | FuncKey.filebytes = d.bytes; 377 | FuncKey.filedate = d.datenum; 378 | end 379 | end 380 | 381 | % ALTERNATIVE: Use name and path. The part of the toolbox functions 382 | % is replaced such that the hash for @mean does not depend on the Matlab 383 | % version. 384 | % Drawbacks: Anonymous functions, nested functions... 385 | % funcStruct = functions(FuncH); 386 | % funcfile = strrep(funcStruct.file, matlabroot, ''); 387 | % FuncKey = uint8([funcStruct.function, ' ', funcfile]); 388 | 389 | % Finally I'm afraid there is no unique method to get a hash for a function 390 | % handle. Please adjust this conversion to your needs. 391 | 392 | % return; 393 | 394 | % ****************************************************************************** 395 | function DataBin = ConvertObject(DataObj) 396 | % Convert a user-defined object to a binary stream. There cannot be a unique 397 | % solution, so this part is left for the user... 398 | 399 | try % Perhaps a direct conversion is implemented: 400 | DataBin = uint8(DataObj); 401 | 402 | % Matt Raum had this excellent idea - unfortunately this function is 403 | % undocumented and might not be supported in te future: 404 | % DataBin = getByteStreamFromArray(DataObj); 405 | 406 | catch % Or perhaps this is better: 407 | WarnS = warning('off', 'MATLAB:structOnObject'); 408 | DataBin = struct(DataObj); 409 | warning(WarnS); 410 | end 411 | 412 | % return; 413 | 414 | % ****************************************************************************** 415 | function Out = fBase64_enc(In) 416 | % Encode numeric vector of UINT8 values to base64 string. 417 | % The intention of this is to create a shorter hash than the HEX format. 418 | % Therefore a padding with '=' characters is omitted on purpose. 419 | 420 | Pool = [65:90, 97:122, 48:57, 43, 47]; % [0:9, a:z, A:Z, +, /] 421 | v8 = [128; 64; 32; 16; 8; 4; 2; 1]; 422 | v6 = [32, 16, 8, 4, 2, 1]; 423 | 424 | In = reshape(In, 1, []); 425 | X = rem(floor(In(ones(8, 1), :) ./ v8(:, ones(length(In), 1))), 2); 426 | Y = reshape([X(:); zeros(6 - rem(numel(X), 6), 1)], 6, []); 427 | Out = char(Pool(1 + v6 * Y)); 428 | 429 | % return; 430 | 431 | % ****************************************************************************** 432 | function Ex = FileExist_L(FileName) 433 | % A more reliable version of EXIST(FileName, 'file'): 434 | dirFile = dir(FileName); 435 | if length(dirFile) == 1 436 | Ex = ~(dirFile.isdir); 437 | else 438 | Ex = false; 439 | end 440 | 441 | % return; 442 | 443 | % ****************************************************************************** 444 | function R = Version_L() 445 | % The output differs between versions of this function. So give the user a 446 | % chance to recognize the version: 447 | % 1: 01-May-2011, Initial version 448 | % 2: 15-Feb-2015, The number of dimensions is considered in addition. 449 | % In version 1 these variables had the same hash: 450 | % zeros(1,1) and zeros(1,1,0), complex(0) and zeros(1,1,0,0) 451 | % 3: 29-Jun-2015, Struct arrays are processed field by field and not element 452 | % by element, because this is much faster. In consequence the hash value 453 | % differs, if the input contains a struct. 454 | % 4: 28-Feb-2016 15:20, same output as GetMD5 for MD5 sums. Therefore the 455 | % dimensions are casted to UINT64 at first. 456 | R.HashVersion = 4; 457 | R.Date = [2016, 2, 28]; 458 | 459 | R.HashMethod = {}; 460 | try 461 | Provider = java.security.Security.getProviders; 462 | for iProvider = 1:numel(Provider) 463 | S = char(Provider(iProvider).getServices); 464 | Index = strfind(S, 'MessageDigest.'); 465 | for iDigest = 1:length(Index) 466 | Digest = strtok(S(Index(iDigest):end)); 467 | Digest = strrep(Digest, 'MessageDigest.', ''); 468 | R.HashMethod = cat(2, R.HashMethod, {Digest}); 469 | end 470 | end 471 | catch ME 472 | fprintf(2, '%s\n', ME.message); 473 | R.HashMethod = 'error'; 474 | end 475 | 476 | % return; 477 | 478 | % ****************************************************************************** 479 | function Error_L(ID, varargin) 480 | 481 | error(['JSimon:', mfilename, ':', ID], ['*** %s: ', varargin{1}], ... 482 | mfilename, varargin{2:nargin - 1}); 483 | 484 | % return; 485 | -------------------------------------------------------------------------------- /helpers/nativeUpdateWeightsCpu.m: -------------------------------------------------------------------------------- 1 | % Updates particle weights on CPU 2 | function wp = nativeUpdateWeightsCpu(System) 3 | System.wp = libpointer('doublePtr',System.wp); 4 | calllib('p2c','updateWeights_cpu',System); 5 | wp = get(System.wp,'Value'); 6 | end 7 | -------------------------------------------------------------------------------- /helpers/nativeUpdateWeightsGpu.m: -------------------------------------------------------------------------------- 1 | % Updates particle weights on GPU 2 | function wp = nativeUpdateWeightsGpu(System) 3 | System.wp = libpointer('doublePtr',System.wp); 4 | calllib('p2c','updateWeights_gpu',System); 5 | wp = get(System.wp,'Value'); 6 | end 7 | -------------------------------------------------------------------------------- /helpers/plotSystem.m: -------------------------------------------------------------------------------- 1 | function plotSystem(vw,cameraParams,System,frame,entry,pfT,pfR,index) 2 | %{ 3 | 1 2 3 4 4 | 5 6 7 8 5 | 9 10 11 12 6 | 13 14 15 16 7 | 17 18 19 20 8 | 21 22 23 24 9 | 25 26 27 28 10 | %} 11 | 12 | m = 7; n = 4; 13 | figure(1); 14 | 15 | h = subplot(m,n,[23 24 27 28]); cla(h); 16 | [~,I] = sort(System.wp,'descend'); 17 | top = 100; semilogy(System.wp(sort(I(1:top)))); 18 | xlabel('Particles (n)'); 19 | title(['Top ' num2str(top) ' weights']); 20 | axis tight; box on; grid on; 21 | 22 | h = subplot(m,n,[1 2 5 6 9 10]); cla(h); 23 | showMatchedFeatures(frame,frame,System.imagePoints,worldToImage(cameraParams,quat2rotm(pfR'),pfT,System.worldPoints)); 24 | legend('Ground truth','Estimates'); 25 | title('Innovations'); 26 | axis tight; 27 | 28 | hold on; 29 | 30 | subplot(m,n,[13 14 17 18 21 22 25 26]); hold on; 31 | scatter3(entry.ExtrinsicsTranslationTrue(1), entry.ExtrinsicsTranslationTrue(2), entry.ExtrinsicsTranslationTrue(3), 'k.'); 32 | scatter3(entry.ExtrinsicsTranslationEst(1), entry.ExtrinsicsTranslationEst(2), entry.ExtrinsicsTranslationEst(3), 'r.'); 33 | scatter3(pfT(1), pfT(2), pfT(3), 'b.'); 34 | legend('True', 'RANSAC', 'Particle Filter'); 35 | title('3D camera path (world-units mm)'); 36 | xlabel('x'); ylabel('y'); zlabel('z'); 37 | axis tight; box on; grid on; view(3); 38 | hold off; 39 | 40 | subplot(m,n,3); hold on; 41 | scatter(index, entry.ExtrinsicsRotationTrue(1), 'k.'); 42 | scatter(index, entry.ExtrinsicsRotationEst(1), 'r.'); 43 | scatter(index, pfR(1), 'b.'); 44 | title('Rotation quaternion-w'); 45 | axis tight; box on; grid on; 46 | hold off; 47 | 48 | subplot(m,n,4); hold on; 49 | scatter(index, entry.ExtrinsicsRotationTrue(2), 'k.'); 50 | scatter(index, entry.ExtrinsicsRotationEst(2), 'r.'); 51 | scatter(index, pfR(2), 'b.'); 52 | title('Rotation quaternion-x'); 53 | axis tight; box on; grid on; 54 | hold off; 55 | 56 | subplot(m,n,7); hold on; 57 | scatter(index, entry.ExtrinsicsRotationTrue(3), 'k.'); 58 | scatter(index, entry.ExtrinsicsRotationEst(3), 'r.'); 59 | scatter(index, pfR(3), 'b.'); 60 | title('Rotation quaternion-y'); 61 | axis tight; box on; grid on; 62 | hold off; 63 | 64 | subplot(m,n,8); hold on; 65 | scatter(index, entry.ExtrinsicsRotationTrue(4), 'k.'); 66 | scatter(index, entry.ExtrinsicsRotationEst(4), 'r.'); 67 | scatter(index, pfR(4), 'b.'); 68 | title('Rotation quaternion-z'); 69 | axis tight; box on; grid on; 70 | hold off; 71 | 72 | subplot(m,n,[11 12]); hold on; 73 | scatter(index, entry.ExtrinsicsTranslationTrue(1), 'k.'); 74 | scatter(index, entry.ExtrinsicsTranslationEst(1), 'r.'); 75 | scatter(index, pfT(1), 'b.'); 76 | title('Translation x'); 77 | axis tight; box on; grid on; 78 | hold off; 79 | 80 | subplot(m,n,[15 16]); hold on; 81 | scatter(index, entry.ExtrinsicsTranslationTrue(2), 'k.'); 82 | scatter(index, entry.ExtrinsicsTranslationEst(2), 'r.'); 83 | scatter(index, pfT(2), 'b.'); 84 | title('Translation y'); 85 | axis tight; box on; grid on; 86 | hold off; 87 | 88 | subplot(m,n,[19 20]); hold on; 89 | scatter(index, entry.ExtrinsicsTranslationTrue(3), 'k.'); 90 | scatter(index, entry.ExtrinsicsTranslationEst(3), 'r.'); 91 | scatter(index, pfT(3), 'b.'); 92 | title('Translation z'); 93 | axis tight; box on; grid on; 94 | hold off; 95 | 96 | hold off; 97 | drawnow; 98 | 99 | writeVideo(vw, frame2im(getframe(gcf))); 100 | end 101 | -------------------------------------------------------------------------------- /main.m: -------------------------------------------------------------------------------- 1 | clc; 2 | clear all; 3 | close all; 4 | rng default; 5 | addpath bin; 6 | addpath helpers; 7 | if ~libisloaded('p2c'); loadlibrary p2c; end; 8 | 9 | N = 1000000; % particle count 10 | input = 'data/pixel-xl.mp4'; % video file 11 | policy = 'gpu'; % 'gpu', 'cpu', or 'matlab' 12 | positionSigma = [100 200 100]; % mm, in XYZ axes 13 | rotationSigma = [0.1 0.05 0.5 1]; % unitless 14 | groundTruth = ggt(input); % ground truth 2d and 3d data 15 | [positionNoise, rotationNoise] = createNoise(N, positionSigma, rotationSigma); 16 | frameCount = length(groundTruth); % this is how many total frames we have 17 | load('cameraParams.mat'); % previously calculated camera intrinsics (K-mat) 18 | 19 | % video IO 20 | vr = VideoReader(input); 21 | vw = VideoWriter('result.avi'); 22 | vw.FrameRate = vr.FrameRate; 23 | open(vw); 24 | 25 | % Initialization 26 | System.wp = zeros(N,1); % particle weights 27 | System.xp = zeros(N,1); % particles 28 | System.cameraParams = cameraParams.IntrinsicMatrix; 29 | System.imagePoints = []; % 2D feature locations in the image 30 | System.worldPoints = []; % corresponding 3D world points 31 | System.N = N; % static constant, number of particles 32 | System.F = 0; % updated every frame, number of features found in image 33 | 34 | index = 1; 35 | while index <= length(groundTruth) && hasFrame(vr) 36 | entry = groundTruth(index); 37 | frame = rgb2gray(readFrame(vr)); 38 | System.F = length(entry.WorldPoints); 39 | System.imagePoints = entry.ImagePoints; 40 | System.worldPoints = entry.WorldPoints; 41 | 42 | if index == 1 43 | % first frame is for initialization 44 | q = entry.PoseRotationTrue; 45 | t = entry.PoseTranslationTrue; 46 | [System.xp,System.wp] = createParticles(q,t,N); 47 | else 48 | % second frame onward is for tracking (localization) 49 | System.xp = updateParticles(System,positionNoise,rotationNoise); 50 | System.wp = updateWeights(System,cameraParams,policy); 51 | System.xp = resampleParticles(System); 52 | [q,t] = extractEstimates(System); 53 | plotSystem(vw,cameraParams,System,frame,entry,t,q,index); 54 | System.wp = resetWeights(System); 55 | end 56 | 57 | index = index + 1; 58 | end 59 | unloadlibrary p2c; 60 | close(vw); 61 | 62 | % _________________________________________________________________________ 63 | % Extratcs the mean of particles as system's estimate. 64 | function [q,t] = extractEstimates(System) 65 | q = sum(System.wp .* System.xp(:,4:7)); % estimated quaternion 66 | t = sum(System.wp .* System.xp(:,1:3)); % estimated translation 67 | [q,t] = cameraPoseToExtrinsics(quat2rotm(q),t); 68 | q = rotm2quat(q)'; 69 | end 70 | 71 | % Systematic resmapling of particles. 72 | function xp = resampleParticles(System) 73 | R = cumsum(System.wp); 74 | T = rand(1, System.N); 75 | [~, I] = histc(T, R); 76 | xp = System.xp(I + 1, :); 77 | end 78 | 79 | % Systematic resmapling of particles. 80 | function wp = resetWeights(System) 81 | wp = ones(System.N,1) / System.N; 82 | end 83 | 84 | % Updates particle weights based on their liklihood (measurement model). 85 | function wp = updateWeights(System,cameraParams,policy) 86 | if isequal(policy, 'gpu') 87 | % Execude on a Cuda capable device (GPU) 88 | wp = nativeUpdateWeightsGpu(System); 89 | elseif isequal(policy, 'cpu') 90 | % Execude with native code (C++11 runtime) 91 | wp = nativeUpdateWeightsCpu(System); 92 | else 93 | % Execude with matlab (matlab runtime) 94 | N = size(System.wp,1); 95 | observationPoints = System.imagePoints * 0; 96 | for i=1:N 97 | t = System.xp(i,1:3); 98 | R = quat2rotm(System.xp(i,4:7)); 99 | [Rx,tx] = cameraPoseToExtrinsics(R,t); 100 | camMatrix = cameraMatrix(cameraParams,Rx,tx); 101 | 102 | for j=1:System.F 103 | projection = [System.worldPoints(j,:) 1] * camMatrix; 104 | projection = projection ./ projection(3); 105 | observationPoints(j,:) = projection(1:2); 106 | end 107 | 108 | C = cov(observationPoints(:,:)); 109 | D = zeros(System.F,1); 110 | 111 | for j=1:System.F 112 | d = observationPoints(j,:)-System.imagePoints(j,:); 113 | D(j) = (d/C)*d'; 114 | end 115 | 116 | System.wp(i) = System.wp(i) * (1/(2*pi*sqrt(det(C))))*exp(-sum(D)/2); 117 | end 118 | wp = System.wp ./ sum(System.wp); 119 | end 120 | end 121 | 122 | % Adds noise to particles (system model) 123 | function xp = updateParticles(System,posNoise,rotNoise) 124 | xp(:,1:3) = System.xp(:,1:3) + posNoise; 125 | xp(:,4:7) = quatmultiply(System.xp(:,4:7), rotNoise); 126 | end 127 | 128 | % Creates the initial particle cloud 129 | function [xp,wp] = createParticles(q0,t0,N) 130 | xp = ones(N,7); 131 | xp(:,1) = t0(1); % x 132 | xp(:,2) = t0(2); % y 133 | xp(:,3) = t0(3); % z 134 | xp(:,4) = q0(1); % q-r 135 | xp(:,5) = q0(2); % q-i 136 | xp(:,6) = q0(3); % q-j 137 | xp(:,7) = q0(4); % q-k 138 | wp = ones(N,1) / N; 139 | end 140 | 141 | % genrate Random Noise. 142 | function [positionNoise, rotationNoise] = createNoise(N, positionSigma, rotationSigma) 143 | assert(isequal(size(positionSigma),[1,3]), 'position standard deviation msut be 3x1'); 144 | assert(isequal(size(rotationSigma),[1 4]), 'rotation standard deviation msut be 1x4'); 145 | v = rotationSigma(2:4) .* randn(N,3); 146 | r = rotationSigma(1) * randn(N,1); 147 | rotationNoise = zeros(N,4); 148 | for i=1:N 149 | rotationNoise(i,:) = [cos(r(i)) sin(r(i)) * v(i,:) / norm(v(i,:))]'; 150 | end 151 | positionNoise = repmat(positionSigma, N, 1) .* randn(N,3); 152 | end 153 | -------------------------------------------------------------------------------- /p2c.cu: -------------------------------------------------------------------------------- 1 | #include "p2c.h" 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #define GLM_FORCE_CUDA 10 | #include 11 | #include 12 | #include 13 | 14 | // https://en.wikipedia.org/wiki/Algorithms_for_calculating_variance 15 | class onlineCovariance { 16 | public: 17 | __host__ __device__ 18 | void update(double x, double y) { 19 | n += 1; 20 | auto dx = x - meanx; 21 | meanx += dx / n; 22 | meany += (y - meany) / n; 23 | C += dx * (y - meany); 24 | } 25 | 26 | __host__ __device__ 27 | double getPopulationCov() { 28 | return C / n; 29 | } 30 | 31 | __host__ __device__ 32 | // Bessel's correction for sample variance 33 | double getSampleCov() { 34 | return C / (n - 1); 35 | } 36 | 37 | private: 38 | double meanx = 0; 39 | double meany = 0; 40 | double C = 0; 41 | double n = 0; 42 | }; 43 | 44 | struct updateWeightsOp { 45 | updateWeightsOp(const System& input, double* observationPoints) : 46 | input(input), observationPoints(observationPoints) { /* no-op */ } 47 | 48 | System input; 49 | double *observationPoints; 50 | 51 | __host__ __device__ 52 | void operator()(std::size_t i) { 53 | using namespace glm; 54 | dvec3 t(input.xp[i], input.xp[input.N+i], input.xp[2*input.N+i]); 55 | dmat3x3 R = mat3_cast(dquat(input.xp[3 * input.N + i], input.xp[4 * input.N + i], input.xp[5 * input.N + i], input.xp[6 * input.N + i])); 56 | cameraPoseToExtrinsics(t, R); 57 | dmat4x3 camMatrix = cameraMatrix(R, t); 58 | 59 | onlineCovariance cov_xx; 60 | onlineCovariance cov_yy; 61 | onlineCovariance cov_xy; 62 | for (unsigned j = 0; j < input.F; ++j) { 63 | dvec3 projection{ 64 | camMatrix * dvec4(input.worldPoints[j], input.worldPoints[input.F + j], input.worldPoints[2 * input.F + j], 1) 65 | }; 66 | auto x = projection.x / projection.z; 67 | auto y = projection.y / projection.z; 68 | cov_xx.update(x, x); 69 | cov_yy.update(y, y); 70 | cov_xy.update(x, y); 71 | observationPoints[2 * i*input.F + 2 * j] = x; 72 | observationPoints[2 * i*input.F + 2 * j+1] = y; 73 | } 74 | 75 | dmat2x2 C(cov_xx.getSampleCov(), cov_xy.getSampleCov(), 76 | cov_xy.getSampleCov(), cov_yy.getSampleCov()); 77 | dmat2x2 invC = glm::inverse(C); 78 | 79 | double D = 0; 80 | for (unsigned j = 0; j < input.F; ++j) { 81 | glm::dvec2 d { 82 | observationPoints[2 * i*input.F + 2 * j] - input.imagePoints[j], 83 | observationPoints[2 * i*input.F + 2 * j + 1] - input.imagePoints[input.F + j] 84 | }; 85 | D += glm::dot(d, invC*d); 86 | } 87 | 88 | input.wp[i] = input.wp[i] * (1 / (2 * glm::pi()*sqrt(glm::determinant(C))))*exp(-D / 2); 89 | } 90 | 91 | __host__ __device__ 92 | // https://www.mathworks.com/help/vision/ref/cameraposetoextrinsics.html 93 | void cameraPoseToExtrinsics(glm::dvec3& t, glm::dmat3x3& R) { 94 | R = glm::transpose(R); 95 | t = -t * R; 96 | } 97 | 98 | __host__ __device__ 99 | // https://www.mathworks.com/help/vision/ref/cameramatrix.html 100 | glm::dmat4x3 cameraMatrix(const glm::dmat3x3& R, const glm::dvec3& t) { 101 | glm::dmat3x3 K(input.cameraParams[0], input.cameraParams[3], input.cameraParams[6], 102 | input.cameraParams[1], input.cameraParams[4], input.cameraParams[7], 103 | input.cameraParams[2], input.cameraParams[5], input.cameraParams[8]); 104 | 105 | glm::dmat4x3 Rt(R[0].x, R[1].x, R[2].x, 106 | R[0].y, R[1].y, R[2].y, 107 | R[0].z, R[1].z, R[2].z, 108 | t.x, t.y, t.z); 109 | 110 | return K * Rt; 111 | } 112 | }; 113 | 114 | extern "C" { 115 | 116 | DLL_PUBLIC void updateWeights_cpu(System input) { 117 | static std::vector observation_points; 118 | const auto observation_size = input.N * input.F * 2; 119 | 120 | if (observation_points.size() != observation_size) 121 | observation_points.resize(observation_size); 122 | 123 | const thrust::counting_iterator first(0); 124 | const auto last = first + input.N; 125 | thrust::for_each( 126 | thrust::host, first, last, 127 | updateWeightsOp(input, observation_points.data())); 128 | 129 | // normalize to 0...1, since weights are a PDF 130 | const auto sum = thrust::reduce(thrust::host, input.wp, input.wp + input.N); 131 | thrust::transform(thrust::host, input.wp, input.wp + input.N, input.wp, thrust::placeholders::_1 / sum); 132 | } 133 | 134 | DLL_PUBLIC void updateWeights_gpu(System input) { 135 | using vec = thrust::device_vector; 136 | 137 | static vec d_observation_points; 138 | const auto observation_size = input.N * input.F * 2; 139 | 140 | if (d_observation_points.size() != observation_size) 141 | d_observation_points.resize(observation_size); 142 | 143 | // transfer Matlab data >> to GPU (device) 144 | vec d_wp(input.wp, input.wp + input.N); 145 | vec d_xp(input.xp, input.xp + input.N*7); 146 | vec d_cameraParams(input.cameraParams, input.cameraParams + 3*3); 147 | vec d_imagePoints(input.imagePoints, input.imagePoints + input.F * 2); 148 | vec d_worldPoints(input.worldPoints, input.worldPoints + input.F * 2); 149 | 150 | // this will be available to every thread on GPU (data) 151 | const System d_system{ 152 | raw_pointer_cast(d_wp.data()), 153 | raw_pointer_cast(d_xp.data()), 154 | raw_pointer_cast(d_cameraParams.data()), 155 | raw_pointer_cast(d_imagePoints.data()), 156 | raw_pointer_cast(d_worldPoints.data()), 157 | input.N, 158 | input.F 159 | }; 160 | 161 | const thrust::counting_iterator first(0); 162 | const auto last = first + input.N; 163 | thrust::for_each( 164 | thrust::device, first, last, 165 | updateWeightsOp(d_system, raw_pointer_cast(d_observation_points.data()))); 166 | 167 | // normalize to 0...1, since weights are a PDF 168 | const auto sum = thrust::reduce(thrust::device, d_wp.cbegin(), d_wp.cend()); 169 | thrust::transform(thrust::device, d_wp.begin(), d_wp.end(), d_wp.begin(), thrust::placeholders::_1 / sum); 170 | 171 | // transfer GPU data >> to Matlab (host) 172 | ::cudaMemcpy(input.wp, thrust::raw_pointer_cast(d_wp.data()), 173 | input.N * sizeof(double), cudaMemcpyDeviceToHost); 174 | } 175 | 176 | } 177 | -------------------------------------------------------------------------------- /p2c.h: -------------------------------------------------------------------------------- 1 | // https://gcc.gnu.org/wiki/Visibility 2 | #if defined _WIN32 || defined __CYGWIN__ 3 | #ifdef BUILDING_DLL 4 | #ifdef __GNUC__ 5 | #define DLL_PUBLIC __attribute__ ((dllexport)) 6 | #else 7 | #define DLL_PUBLIC __declspec(dllexport) // Note: actually gcc seems to also supports this syntax. 8 | #endif 9 | #else 10 | #ifdef __GNUC__ 11 | #define DLL_PUBLIC __attribute__ ((dllimport)) 12 | #else 13 | #define DLL_PUBLIC __declspec(dllimport) // Note: actually gcc seems to also supports this syntax. 14 | #endif 15 | #endif 16 | #define DLL_LOCAL 17 | #else 18 | #if __GNUC__ >= 4 19 | #define DLL_PUBLIC __attribute__ ((visibility ("default"))) 20 | #define DLL_LOCAL __attribute__ ((visibility ("hidden"))) 21 | #else 22 | #define DLL_PUBLIC 23 | #define DLL_LOCAL 24 | #endif 25 | #endif 26 | 27 | /** 28 | * wp is Nx1 29 | * xp is Nx7 30 | * cameraParams is 3x3 31 | * imagePoints is Fx2 32 | * worldPoints is Fx3 33 | */ 34 | typedef struct System { 35 | double *wp; 36 | double *xp; 37 | double *cameraParams; 38 | double *imagePoints; 39 | double *worldPoints; 40 | unsigned int N; 41 | unsigned int F; 42 | } System; 43 | 44 | #ifdef __cplusplus 45 | extern "C" { 46 | #endif // __cplusplus 47 | 48 | DLL_PUBLIC void updateWeights_cpu(System input); 49 | DLL_PUBLIC void updateWeights_gpu(System input); 50 | 51 | #ifdef __cplusplus 52 | } // extern "C" 53 | #endif // __cplusplus 54 | --------------------------------------------------------------------------------