├── .gitignore ├── BundleAdjustment ├── BAInterface.cpp ├── BAInterface.h ├── CMakeLists.txt ├── GlobalBundleAdjustor.cpp ├── GlobalBundleAdjustor.h ├── GlobalBundleAdjustorDebug.cpp ├── GlobalBundleAdjustorIO.cpp └── GlobalBundleAdjustorUpdate.cpp ├── CMakeLists.txt ├── Data ├── 00000.yaml ├── 00001.yaml ├── 00002.yaml ├── 00003.yaml ├── 00004.yaml ├── 00005.yaml ├── 00006.yaml ├── 00007.yaml ├── 00008.yaml ├── 00009.yaml ├── 00010.yaml ├── 00011.yaml ├── 00012.yaml ├── 00013.yaml ├── 00014.yaml ├── 00015.yaml ├── 00016.yaml ├── 00017.yaml ├── 00018.yaml ├── 00019.yaml ├── 00020.yaml ├── 00021.yaml ├── 00022.yaml ├── 00023.yaml ├── 00024.yaml ├── 00025.yaml ├── 00026.yaml ├── 00027.yaml ├── 00028.yaml ├── 00029.yaml ├── 00030.yaml ├── 00031.yaml ├── 00032.yaml ├── 00033.yaml ├── 00034.yaml ├── 00035.yaml ├── 00036.yaml ├── 00037.yaml ├── 00038.yaml ├── 00039.yaml ├── 00040.yaml ├── 00041.yaml ├── 00042.yaml ├── 00043.yaml ├── 00044.yaml ├── 00045.yaml ├── 00046.yaml ├── 00047.yaml ├── 00048.yaml ├── 00049.yaml ├── 00050.yaml ├── 00051.yaml ├── 00052.yaml ├── 00053.yaml ├── 00054.yaml ├── 00055.yaml ├── 00056.yaml ├── 00057.yaml ├── 00058.yaml ├── 00059.yaml ├── 00060.yaml ├── 00061.yaml ├── 00062.yaml ├── 00063.yaml ├── 00064.yaml ├── 00065.yaml ├── 00066.yaml ├── 00067.yaml ├── 00068.yaml ├── 00069.yaml ├── 00070.yaml ├── 00071.yaml ├── 00072.yaml ├── 00073.yaml ├── 00074.yaml ├── 00075.yaml ├── 00076.yaml ├── 00077.yaml ├── 00078.yaml ├── 00079.yaml ├── 00080.yaml ├── 00081.yaml ├── 00082.yaml ├── 00083.yaml ├── 00084.yaml ├── 00085.yaml ├── 00086.yaml ├── 00087.yaml ├── 00088.yaml ├── 00089.yaml ├── 00090.yaml ├── 00091.yaml └── 00092.yaml ├── Example └── ExampleYAML.cpp ├── Geometry ├── CMakeLists.txt ├── Camera.cpp ├── Camera.h ├── Depth.h ├── Intrinsic.h ├── M-Estimator.h ├── Point.h ├── Rigid.h └── Rotation.h ├── LICENSE ├── LinearAlgebra ├── LinearSystem.h ├── Matrix2x2.h ├── Matrix2x3.h ├── Matrix2x4.h ├── Matrix2x6.h ├── Matrix2x7.h ├── Matrix2x8.h ├── Matrix3x3.h ├── Matrix3x4.h ├── Matrix3x6.h ├── Matrix3x7.h ├── Matrix3x8.h ├── Matrix4x4.h ├── Matrix6x6.h ├── Matrix6x7.h ├── Matrix8x8.h ├── MatrixMxN.h ├── MatrixNxN.h ├── Vector2.h ├── Vector3.h ├── Vector4.h ├── Vector6.h ├── Vector8.h └── VectorN.h ├── Map ├── CMakeLists.txt ├── Feature.h ├── Frame.h ├── GlobalMap.cpp └── GlobalMap.h ├── PlatformIndependence ├── def_missing.h ├── gen_sse_impl.py ├── sse.h ├── sse.in ├── sse_impl.h └── windows_thread_wrapper.h ├── README.md ├── Utility ├── AlignedMatrix.h ├── AlignedVector.h ├── CMakeLists.txt ├── Candidate.h ├── Configurator.h ├── MultiThread.h ├── SSE.h ├── Timer.h ├── Utility.cpp └── Utility.h └── stdafx.h /.gitignore: -------------------------------------------------------------------------------- 1 | 2 | # Created by https://www.gitignore.io/api/c++,cmake,visualstudio,visualstudiocode,emacs 3 | 4 | ### C++ ### 5 | # Prerequisites 6 | *.d 7 | 8 | # Compiled Object files 9 | *.slo 10 | *.lo 11 | *.o 12 | *.obj 13 | 14 | # Precompiled Headers 15 | *.gch 16 | *.pch 17 | 18 | # Compiled Dynamic libraries 19 | *.so 20 | *.dylib 21 | *.dll 22 | 23 | # Fortran module files 24 | *.mod 25 | *.smod 26 | 27 | # Compiled Static libraries 28 | *.lai 29 | *.la 30 | *.a 31 | *.lib 32 | 33 | # Executables 34 | *.exe 35 | *.out 36 | *.app 37 | 38 | ### CMake ### 39 | CMakeCache.txt 40 | CMakeFiles 41 | CMakeScripts 42 | Testing 43 | Makefile 44 | cmake_install.cmake 45 | install_manifest.txt 46 | compile_commands.json 47 | CTestTestfile.cmake 48 | 49 | ### Emacs ### 50 | # -*- mode: gitignore; -*- 51 | *~ 52 | \#*\# 53 | /.emacs.desktop 54 | /.emacs.desktop.lock 55 | *.elc 56 | auto-save-list 57 | tramp 58 | .\#* 59 | 60 | # Org-mode 61 | .org-id-locations 62 | *_archive 63 | 64 | # flymake-mode 65 | *_flymake.* 66 | 67 | # eshell files 68 | /eshell/history 69 | /eshell/lastdir 70 | 71 | # elpa packages 72 | /elpa/ 73 | 74 | # reftex files 75 | *.rel 76 | 77 | # AUCTeX auto folder 78 | /auto/ 79 | 80 | # cask packages 81 | .cask/ 82 | dist/ 83 | 84 | # Flycheck 85 | flycheck_*.el 86 | 87 | # server auth directory 88 | /server/ 89 | 90 | # projectiles files 91 | .projectile 92 | 93 | # directory configuration 94 | .dir-locals.el 95 | 96 | ### VisualStudioCode ### 97 | .vscode/* 98 | !.vscode/settings.json 99 | !.vscode/tasks.json 100 | !.vscode/launch.json 101 | !.vscode/extensions.json 102 | 103 | ### VisualStudio ### 104 | ## Ignore Visual Studio temporary files, build results, and 105 | ## files generated by popular Visual Studio add-ons. 106 | ## 107 | ## Get latest from https://github.com/github/gitignore/blob/master/VisualStudio.gitignore 108 | 109 | # User-specific files 110 | *.suo 111 | *.user 112 | *.userosscache 113 | *.sln.docstates 114 | 115 | # User-specific files (MonoDevelop/Xamarin Studio) 116 | *.userprefs 117 | 118 | # Build results 119 | [Dd]ebug/ 120 | [Dd]ebugPublic/ 121 | [Rr]elease/ 122 | [Rr]eleases/ 123 | x64/ 124 | x86/ 125 | bld/ 126 | [Bb]in/ 127 | [Oo]bj/ 128 | [Ll]og/ 129 | 130 | # Visual Studio 2015 cache/options directory 131 | .vs/ 132 | # Uncomment if you have tasks that create the project's static files in wwwroot 133 | #wwwroot/ 134 | 135 | # MSTest test Results 136 | [Tt]est[Rr]esult*/ 137 | [Bb]uild[Ll]og.* 138 | 139 | # NUNIT 140 | *.VisualState.xml 141 | TestResult.xml 142 | 143 | # Build Results of an ATL Project 144 | [Dd]ebugPS/ 145 | [Rr]eleasePS/ 146 | dlldata.c 147 | 148 | # .NET Core 149 | project.lock.json 150 | project.fragment.lock.json 151 | artifacts/ 152 | **/Properties/launchSettings.json 153 | 154 | *_i.c 155 | *_p.c 156 | *_i.h 157 | *.ilk 158 | *.meta 159 | *.pdb 160 | *.pgc 161 | *.pgd 162 | *.rsp 163 | *.sbr 164 | *.tlb 165 | *.tli 166 | *.tlh 167 | *.tmp 168 | *.tmp_proj 169 | *.log 170 | *.vspscc 171 | *.vssscc 172 | .builds 173 | *.pidb 174 | *.svclog 175 | *.scc 176 | 177 | # Chutzpah Test files 178 | _Chutzpah* 179 | 180 | # Visual C++ cache files 181 | ipch/ 182 | *.aps 183 | *.ncb 184 | *.opendb 185 | *.opensdf 186 | *.sdf 187 | *.cachefile 188 | *.VC.db 189 | *.VC.VC.opendb 190 | 191 | # Visual Studio profiler 192 | *.psess 193 | *.vsp 194 | *.vspx 195 | *.sap 196 | 197 | # TFS 2012 Local Workspace 198 | $tf/ 199 | 200 | # Guidance Automation Toolkit 201 | *.gpState 202 | 203 | # ReSharper is a .NET coding add-in 204 | _ReSharper*/ 205 | *.[Rr]e[Ss]harper 206 | *.DotSettings.user 207 | 208 | # JustCode is a .NET coding add-in 209 | .JustCode 210 | 211 | # TeamCity is a build add-in 212 | _TeamCity* 213 | 214 | # DotCover is a Code Coverage Tool 215 | *.dotCover 216 | 217 | # Visual Studio code coverage results 218 | *.coverage 219 | *.coveragexml 220 | 221 | # NCrunch 222 | _NCrunch_* 223 | .*crunch*.local.xml 224 | nCrunchTemp_* 225 | 226 | # MightyMoose 227 | *.mm.* 228 | AutoTest.Net/ 229 | 230 | # Web workbench (sass) 231 | .sass-cache/ 232 | 233 | # Installshield output folder 234 | [Ee]xpress/ 235 | 236 | # DocProject is a documentation generator add-in 237 | DocProject/buildhelp/ 238 | DocProject/Help/*.HxT 239 | DocProject/Help/*.HxC 240 | DocProject/Help/*.hhc 241 | DocProject/Help/*.hhk 242 | DocProject/Help/*.hhp 243 | DocProject/Help/Html2 244 | DocProject/Help/html 245 | 246 | # Click-Once directory 247 | publish/ 248 | 249 | # Publish Web Output 250 | *.[Pp]ublish.xml 251 | *.azurePubxml 252 | # TODO: Comment the next line if you want to checkin your web deploy settings 253 | # but database connection strings (with potential passwords) will be unencrypted 254 | *.pubxml 255 | *.publishproj 256 | 257 | # Microsoft Azure Web App publish settings. Comment the next line if you want to 258 | # checkin your Azure Web App publish settings, but sensitive information contained 259 | # in these scripts will be unencrypted 260 | PublishScripts/ 261 | 262 | # NuGet Packages 263 | *.nupkg 264 | # The packages folder can be ignored because of Package Restore 265 | **/packages/* 266 | # except build/, which is used as an MSBuild target. 267 | !**/packages/build/ 268 | # Uncomment if necessary however generally it will be regenerated when needed 269 | #!**/packages/repositories.config 270 | # NuGet v3's project.json files produces more ignoreable files 271 | *.nuget.props 272 | *.nuget.targets 273 | 274 | # Microsoft Azure Build Output 275 | csx/ 276 | *.build.csdef 277 | 278 | # Microsoft Azure Emulator 279 | ecf/ 280 | rcf/ 281 | 282 | # Windows Store app package directories and files 283 | AppPackages/ 284 | BundleArtifacts/ 285 | Package.StoreAssociation.xml 286 | _pkginfo.txt 287 | 288 | # Visual Studio cache files 289 | # files ending in .cache can be ignored 290 | *.[Cc]ache 291 | # but keep track of directories ending in .cache 292 | !*.[Cc]ache/ 293 | 294 | # Others 295 | ClientBin/ 296 | ~$* 297 | *.dbmdl 298 | *.dbproj.schemaview 299 | *.jfm 300 | *.pfx 301 | *.publishsettings 302 | node_modules/ 303 | orleans.codegen.cs 304 | 305 | # Since there are multiple workflows, uncomment next line to ignore bower_components 306 | # (https://github.com/github/gitignore/pull/1529#issuecomment-104372622) 307 | #bower_components/ 308 | 309 | # RIA/Silverlight projects 310 | Generated_Code/ 311 | 312 | # Backup & report files from converting an old project file 313 | # to a newer Visual Studio version. Backup files are not needed, 314 | # because we have git ;-) 315 | _UpgradeReport_Files/ 316 | Backup*/ 317 | UpgradeLog*.XML 318 | UpgradeLog*.htm 319 | 320 | # SQL Server files 321 | *.mdf 322 | *.ldf 323 | 324 | # Business Intelligence projects 325 | *.rdl.data 326 | *.bim.layout 327 | *.bim_*.settings 328 | 329 | # Microsoft Fakes 330 | FakesAssemblies/ 331 | 332 | # GhostDoc plugin setting file 333 | *.GhostDoc.xml 334 | 335 | # Node.js Tools for Visual Studio 336 | .ntvs_analysis.dat 337 | 338 | # Visual Studio 6 build log 339 | *.plg 340 | 341 | # Visual Studio 6 workspace options file 342 | *.opt 343 | 344 | # Visual Studio 6 auto-generated workspace file (contains which files were open etc.) 345 | *.vbw 346 | 347 | # Visual Studio LightSwitch build output 348 | **/*.HTMLClient/GeneratedArtifacts 349 | **/*.DesktopClient/GeneratedArtifacts 350 | **/*.DesktopClient/ModelManifest.xml 351 | **/*.Server/GeneratedArtifacts 352 | **/*.Server/ModelManifest.xml 353 | _Pvt_Extensions 354 | 355 | # Paket dependency manager 356 | .paket/paket.exe 357 | paket-files/ 358 | 359 | # FAKE - F# Make 360 | .fake/ 361 | 362 | # JetBrains Rider 363 | .idea/ 364 | *.sln.iml 365 | 366 | # CodeRush 367 | .cr/ 368 | 369 | # Python Tools for Visual Studio (PTVS) 370 | __pycache__/ 371 | *.pyc 372 | 373 | # Cake - Uncomment if you are using it 374 | # tools/** 375 | # !tools/packages.config 376 | 377 | # End of https://www.gitignore.io/api/c++,cmake,visualstudio,visualstudiocode,emacs 378 | 379 | .DS_Store 380 | cmake-build-* 381 | *build/ 382 | 383 | .vscode/ 384 | 385 | -------------------------------------------------------------------------------- /BundleAdjustment/BAInterface.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of EIBA. 3 | * 4 | * Copyright (C) 2017 Zhejiang University 5 | * For more information see 6 | * 7 | * Licensed under the Apache License, Version 2.0 (the "License"); 8 | * You may not use this file except in compliance with the License. 9 | * You may obtain a copy of the License at 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | * Unless required by applicable law or agreed to in writing, software 12 | * distributed under the License is distributed on an "AS IS" BASIS, 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | * 17 | */ 18 | 19 | #pragma once 20 | 21 | #include 22 | #include 23 | #include 24 | 25 | namespace EIBA { 26 | /// @class Point2 27 | class Point2 28 | { 29 | public: 30 | float m_x, m_y; 31 | }; 32 | 33 | /// @class Point3 34 | class Point3 : public Point2 35 | { 36 | public: 37 | float m_z; 38 | }; 39 | 40 | /// @class Camera 41 | class Camera 42 | { 43 | public: 44 | /// rotation 45 | float m_rotation[3][3]; 46 | /// position 47 | Point3 m_position; 48 | }; 49 | 50 | /// @class Feature 51 | class Feature 52 | { 53 | public: 54 | /// 2D measurement 55 | Point2 m_pt; 56 | /// inverse depth, set 0 if invalid 57 | float m_inv_depth; 58 | }; 59 | 60 | /// @class Measurement 61 | class Measurement : public Feature 62 | { 63 | public: 64 | /// matched keyframe index 65 | int m_matched_kf_idx; 66 | /// matched source feature local index (in keyframe) 67 | int m_matched_ftr_idx; 68 | /// covariance 69 | float m_cov2[2][2]; 70 | }; 71 | 72 | /// @class KeyFrame 73 | class KeyFrame 74 | { 75 | public: 76 | /// camera pose 77 | Camera m_camera; 78 | /// depth in keyframe 79 | std::vector m_inv_depths; 80 | /// mark camera updated 81 | bool m_camera_updated; 82 | /// mark depth updated 83 | std::vector m_depths_updated; 84 | }; 85 | 86 | /// @struct BAParam 87 | struct BAParam { 88 | /// weight of first frame pose prior 89 | float weight_fix = 1.0e2f; 90 | /// weight of feature measurements 91 | float weight_feature = 1.0e-5f; 92 | /// weight of frame constraints 93 | float weight_prior = 1.0e-3f; 94 | /// rotation variance of first frame pose prior 95 | float variance_fix_rotation = 3.046174198662e-6f; // (0.1*pi/180)^2 96 | /// position variance of first frame pose prior 97 | float variance_fix_position = 0.01f; // 0.1^2 98 | /// variance of inverse depth measurement 99 | float variance_depth_map = 0.01f; 100 | /// threshold for update rotation 101 | float update_rotation = 3.046174198662e-6f; 102 | /// threshold for update position 103 | float update_position = 1.0e-4f; 104 | /// threshold for update depth 105 | float update_depth = 1.0e-4f; 106 | /// set verbose level 107 | int verbose = 0; 108 | /// width and height, set this to print reprojection error in pixel 109 | int width = 640, height = 480; 110 | /// camera intrinsic, set this to print reprojection error in pixel 111 | float fx_fy_cx_cy[4] = {535.4, 539.2, 320.1, 247.6}; 112 | }; 113 | 114 | /// @class BAInterface 115 | class BAInterface { 116 | public: 117 | /// @brief constructor of BAInterface 118 | explicit BAInterface(); 119 | 120 | /// @brief destructor of BAInterface 121 | ~BAInterface(); 122 | 123 | /// @brief set parameters 124 | /// @param param BA parameters 125 | void SetParam(const BAParam ¶m); 126 | 127 | /// @brief push keyframe feature observations 128 | /// @param initial_camera initial guess of current frame camera pose (rotation + position) 129 | /// @param initial_depths initial guess of last frame source feature's inverse depth 130 | /// @param source_features current frame source features(2D + inverse depth measurement) 131 | /// @oaram measured_features current frame measured feature matches (2D + inverse depth measurement + matched source feature index + covariance) 132 | void PushKeyframeFeatures(const Camera &initial_camera, 133 | const std::vector &initial_depths, 134 | const std::vector &source_features, 135 | const std::vector &measured_features); 136 | 137 | /// @brief push between frame constrains 138 | /// @param kf_idx1 index of C1 139 | /// @param kf_idx2 index of C2 140 | /// @param C12 C2 * C1_{-1} * C12 141 | /// @param covariance 142 | void PushBetweenFrameConstraint(int kf_idx1, int kf_idx2, 143 | const Camera &C12, const float cov6[6][6]); 144 | 145 | /// @brief optimize, get updated keyframes and depths 146 | /// @oaram KFs store optimized Keyframes in optKFs 147 | void Optimize(std::vector *optKFs = nullptr); 148 | 149 | #ifdef ENABLE_YAML_IO 150 | /// @brief push frame info from YAML files 151 | /// @param filename filename of YAML 152 | /// @note this would call PushKeyframeFeatures(*) and PushBetweenFrameConstraint(*) 153 | void PushFrameInfoFromYAML(std::string filename); 154 | #endif 155 | }; 156 | 157 | } // namespace EIBA 158 | -------------------------------------------------------------------------------- /BundleAdjustment/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(BundleAdjustment 2 | GlobalBundleAdjustor.cpp 3 | GlobalBundleAdjustorIO.cpp 4 | GlobalBundleAdjustorDebug.cpp 5 | GlobalBundleAdjustorUpdate.cpp 6 | BAInterface.cpp 7 | BAInterface.h) 8 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.6) 2 | project(EIBA) 3 | 4 | if (NOT CMAKE_BUILD_TYPE) 5 | set(CMAKE_BUILD_TYPE Release) 6 | endif () 7 | 8 | message(STATUS "BUILD TYPE:" ${CMAKE_BUILD_TYPE}) 9 | 10 | set(CMAKE_CXX_FLAGS_DEBUG "$ENV{CXXFLAGS} -O0 -Wall -g -ggdb") 11 | set(CMAKE_CXX_FLAGS_RELEASE "$ENV{CXXFLAGS} -O3 -w") 12 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14 -msse2 -msse3 -msse4") 13 | include_directories(PlatformIndependence) 14 | 15 | add_definitions(-DCFG_DEPTH_MAP -DCFG_GROUND_TRUTH -DCFG_TUNE_PARAMETERS) 16 | 17 | option(YAML_IO "YAML_IO" ON) 18 | 19 | # enable YAML IO 20 | if (YAML_IO) 21 | message("Enable Yaml IO") 22 | add_definitions(-DENABLE_YAML_IO) 23 | find_package(yaml-cpp REQUIRED) 24 | include_directories(${YAML_CPP_INCLUDE_DIR}) 25 | endif() 26 | 27 | 28 | include_directories( 29 | . 30 | BundleAdjustment 31 | Geometry 32 | ImageTracking 33 | LinearAlgebra 34 | Map 35 | Math 36 | Utility) 37 | 38 | add_subdirectory(BundleAdjustment) 39 | add_subdirectory(Geometry) 40 | add_subdirectory(Map) 41 | add_subdirectory(Utility) 42 | 43 | set(EIBA_LINK_LIBS 44 | ${EIBA_LINK_LIBS} 45 | BundleAdjustment 46 | Geometry 47 | Map 48 | Utility 49 | pthread 50 | ) 51 | 52 | if(YAML_IO) 53 | set(EIBA_LINK_LIBS ${EIBA_LINK_LIBS} ${YAML_CPP_LIBRARIES}) 54 | add_executable(ExampleYAML Example/ExampleYAML.cpp) 55 | target_link_libraries(ExampleYAML ${EIBA_LINK_LIBS}) 56 | endif() 57 | -------------------------------------------------------------------------------- /Data/00000.yaml: -------------------------------------------------------------------------------- 1 | features: 2 | initial_guess: 3 | current_camera_pose_guess: 4 | - [1, 0, 0, -0] 5 | - [0, 1, 0, -0] 6 | - [0, 0, 1, -0] 7 | source_features: 8 | - [0.1679118, -0.06787835, 0.4345936] 9 | - [0.01288755, -0.05860535, 0.4199916] 10 | - [0.5694807, -0.342359, 0] 11 | - [-0.09731042, -0.06602374, 0.4140787] 12 | - [0.09693685, -0.05675075, 0.4257131] 13 | - [-0.1514755, -0.02151336, 0.4492363] 14 | - [0.2332835, -0.1921365, 0] 15 | - [0.06518489, -0.4425074, 0] 16 | - [0.4331341, -0.07715134, 0] 17 | - [-0.0394098, -0.05118695, 0.4257131] 18 | - [0.3117295, -0.1105341, 0.381971] 19 | - [0.1436309, -0.1828635, 0.3702332] 20 | - [0.300523, -0.2143917, 0.3877472] 21 | - [-0.3102353, -0.08271514, 0.4725898] 22 | - [0.3677624, -0.1624629, 0.3965107] 23 | - [0.4798281, -0.435089, 0] 24 | - [0.1436309, -0.1235163, 0.3731343] 25 | - [0.268771, -0.4369436, 0] 26 | - [0.418192, -0.2774481, 0] 27 | - [-0.4503175, -0.1068249, 0] 28 | - [-0.05061637, -0.3738872, 0] 29 | - [-0.4820695, -0.2496291, 0] 30 | - [0.2052671, 0.01557862, 0] 31 | - [0.2781098, 0.1305638, 0.4784689] 32 | - [-0.454053, -0.1772997, 0.3965107] 33 | - [0.3939111, 0.008160226, 0.4666356] 34 | - [0.4480762, 0.01743322, 0.4666356] 35 | - [-0.2429959, -0.06602374, 0.4170142] 36 | - [-0.3027643, -0.02151336, 0.4755112] 37 | - [0.575084, -0.40727, 0] 38 | - [0.2444901, -0.1383531, 0.367242] 39 | - [-0.2766156, -0.3924332, 0.4024145] 40 | - [-0.2243183, 0.2010386, 0] 41 | - [0.5414643, -0.2922849, 0] 42 | - [0.276242, 0.1843472, 0] 43 | - [-0.101046, -0.01594957, 0] 44 | - [0.3378782, 0.02670622, 0.5252101] 45 | - [-0.2336571, -0.4499258, 0] 46 | - [0.3248039, -0.4480712, 0] 47 | - [-0.1327979, -0.4480712, 0] 48 | - [-0.2411281, -0.3405044, 0.4403346] 49 | - [0.2818453, 0.03412462, 0.5252101] 50 | - [0.03530069, 0.008160226, 0.4697041] 51 | - [-0.04127756, -0.3238131, 0] 52 | - [-0.3494583, -0.2997033, 0.4520796] 53 | - [-0.297161, 0.2474035, 0] 54 | - [0.3341427, -0.02522256, 0] 55 | - [0.05584608, -0.2255193, 0] 56 | - [0.08012699, -0.1272255, 0] 57 | - [-0.5026149, 0.04896141, 0] 58 | - [-0.01326112, -0.2514837, 0.4374453] 59 | - [0.418192, -0.1327893, 0] 60 | - [-0.2635413, -0.1958457, 0.4315926] 61 | - [0.4872992, -0.2885757, 0] 62 | - [-0.003922312, -0.1216617, 0.367242] 63 | - [-0.4708629, -0.3034125, 0] 64 | - [-0.4633918, 0.3957715, 0.5017561] 65 | - [-0.1701532, -0.08456974, 0] 66 | - [0.5246544, -0.2385015, 0] 67 | - [-0.2075084, -0.2644659, 0] 68 | - [0.3453493, 0.09718099, 0.4578755] 69 | - [-0.1888308, -0.1958457, 0.4315926] 70 | - [-0.5138214, -0.4480712, 0] 71 | - [0.3229361, 0.2585311, 0.4199916] 72 | - [-0.2822189, 0.2993323, 0] 73 | - [0.2239447, 0.1287092, 0] 74 | - [0.3640269, -0.2329377, 0.4081633] 75 | - [0.2575644, 0.3401335, 0.4608295] 76 | - [-0.3345162, 0.3401335, 0.4784689] 77 | - [-0.1757564, 0.3438427, 0.4608295] 78 | - [0.4779603, 0.2270029, 0.3965107] 79 | - [0.4219275, 0.1231454, 0.3497727] 80 | - [0.5769517, 0.004451027, 0] 81 | - [0.315465, -0.1624629, 0.3877472] 82 | - [-0.3886814, 0.3290059, 0] 83 | - [-0.4316399, 0.04710681, 0.5017561] 84 | - [-0.218715, 0.2548219, 0.4170142] 85 | - [-0.005790074, 0.2307121, 0.3995206] 86 | - [-0.4708629, -0.3905786, 0.4755112] 87 | - [0.190325, 0.2140208, 0] 88 | - [0.4835637, -0.08456974, 0.3965107] 89 | - [0.1044079, 0.2381305, 0] 90 | - [0.4424729, -0.2181009, 0.3760812] 91 | - [0.3640269, -0.1086795, 0.390625] 92 | - [0.1865894, -0.4406528, 0] 93 | - [-0.4036234, -0.2793027, 0.4492363] 94 | - [-0.2822189, -0.2811573, 0] 95 | - [-0.5847964, 0.03412462, 0] 96 | - [0.2407545, 0.2436943, 0] 97 | - [-0.3793425, 0.0749258, 0] 98 | - [-0.1701532, 0.1231454, 0] 99 | - [-0.3307807, -0.3646142, 0.4578755] 100 | - [-0.009525599, 0.0730712, 0] 101 | - [0.1660441, 0.09532639, 0] 102 | - [0.2146059, -0.06045995, 0] 103 | - [0.3995143, -0.08827894, 0.3935458] 104 | - [-0.4092267, -0.08827894, 0.4081633] 105 | - [0.2818453, -0.1550445, 0] 106 | - [0.4144565, -0.04005936, 0.4403346] 107 | - [-0.06929399, -0.006676569, 0.4697041] 108 | - [0.1604408, -0.05860535, 0.438866] 109 | - [0.4069854, -0.08456974, 0] 110 | - [0.01849084, -0.05118695, 0.4301075] 111 | - [0.5713485, -0.3367952, 0] 112 | - [-0.1496078, -0.02522256, 0.445474] 113 | - [0.3285394, -0.1179525, 0.3849115] 114 | - [0.2276802, -0.1847181, 0.3789314] 115 | - [0.0745237, -0.4406528, 0] 116 | - [0.2725065, -0.4406528, 0] 117 | - [-0.3251774, -0.08086054, 0] 118 | - [-0.04874861, -0.3813056, 0] 119 | - [0.4779603, -0.4369436, 0] 120 | - [-0.4372432, -0.1179525, 0.381971] 121 | - [0.2986552, -0.2143917, 0.3870268] 122 | - [0.3995143, -0.1847181, 0.3995206] 123 | - [-0.2542025, -0.06973295, 0.4199916] 124 | - [0.3883078, 0.004451027, 0] 125 | - [-0.06742623, -0.01038577, 0.4652028] 126 | - [0.4144565, -0.2774481, 0] 127 | - [-0.4372432, -0.2589021, 0.4417742] 128 | - [0.1529697, -0.1402077, 0] 129 | - [0.276242, 0.1305638, 0] 130 | - [0.2015315, 0.01557862, 0] 131 | - [-0.1944341, 0.171365, 0] 132 | - [0.4742248, 0.2270029, 0] 133 | - [0.02596189, -0.1327893, 0] 134 | - [-0.4596563, 0.4013353, 0] 135 | - [0.2837131, 0.03412462, 0.5252101] 136 | - [0.5339932, -0.2403561, 0] 137 | - [-0.2355248, -0.3813056, 0] 138 | - [0.575084, -0.4146884, 0] 139 | - [-0.4970116, 0.03783381, 0] 140 | - [-0.2429959, -0.2922849, 0] 141 | - [-0.2317893, 0.2566765, 0.4184801] 142 | - [0.3360104, 0.2752225, 0] 143 | - [-0.03380651, -0.2848665, 0] 144 | - [0.4219275, 0.1231454, 0.3512469] 145 | - [0.2575644, 0.3382789, 0] 146 | - [0.04837503, -0.2292285, 0] 147 | - [-0.1570788, 0.3605341, 0.4740235] 148 | - [-0.3513261, -0.006676569, 0.4718316] 149 | - [-0.1944341, -0.1958457, 0.4315926] 150 | - [-0.5007471, -0.3405044, 0] 151 | - [-0.336384, 0.3456973, 0] 152 | - [-0.3438551, -0.3034125, 0.4506534] 153 | - [-0.2728801, -0.1958457, 0.4330879] 154 | - [0.01475531, 0.02670622, 0] 155 | - [-0.003922312, 0.2307121, 0.3995206] 156 | - [0.2500934, 0.2121661, 0] 157 | - [0.1044079, 0.2566765, 0.4148001] 158 | - [0.4107209, -0.08086054, 0] 159 | - [0.09693685, -0.05860535, 0] 160 | - [0.2015315, -0.03635016, 0] 161 | - [0.3210683, -0.1179525, 0.3850597] 162 | - [0.02222636, -0.05860535, 0] 163 | - [0.2388868, -0.1402077, 0] 164 | - [0.5526708, -0.3108309, 0] 165 | - [0.3135973, -0.1995549, 0.3922492] 166 | - [-0.336384, -0.02151336, 0.481232] 167 | - [-0.4409787, -0.1105341, 0] 168 | - [-0.05995518, -0.006676569, 0.4652028] 169 | - [-0.1496078, -0.02893176, 0] 170 | - [0.3883078, 0.0007418284, 0] 171 | - [0.3883078, -0.1698813, 0.3981843] 172 | - [0.2837131, 0.1416914, 0] 173 | - [-0.4335077, -0.2514837, 0] 174 | - [0.007284262, -0.1402077, 0] 175 | - [0.3808367, -0.2440653, 0.4072656] 176 | - [-0.2317893, 0.2381305, 0] 177 | - [0.0894658, -0.1476261, 0] 178 | - [0.3434815, 0.2603857, 0] 179 | - [-0.2616735, -0.07344215, 0.4199916] 180 | - [0.2538289, 0.3345697, 0] 181 | - [-0.06742623, -0.3553413, 0] 182 | - [0.3434815, 0.0675074, 0] 183 | - [0.4704893, 0.2232938, 0] 184 | - [0.4929025, -0.4146884, 0] 185 | - [-0.1645499, 0.3568249, 0.4755112] 186 | - [-0.478334, -0.3850148, 0.4755112] 187 | - [0.425663, 0.1194362, 0] 188 | - [-0.007657837, 0.2158754, 0] 189 | - [-0.4036234, 0.3271513, 0] 190 | - [-0.2691446, -0.2811573, 0] 191 | - [0.268771, 0.05267061, 0.5104645] 192 | - [-0.5156892, 0.03783381, 0] 193 | - [-0.485805, 0.3864985, 0] 194 | - [-0.05995518, -0.2737389, 0] 195 | - [0.2538289, 0.2529673, 0] 196 | - [-0.1944341, 0.1565282, 0] 197 | - [0.5526708, -0.2218101, 0] 198 | - [0.5526708, -0.006676569, 0] 199 | - [-0.2243183, -0.3627596, 0] 200 | - [0.3135973, -0.03635016, 0] 201 | - [0.4779603, -0.04376855, 0.436186] 202 | - [-0.2019051, -0.2811573, 0] 203 | - [-0.1944341, -0.1921365, 0.4336514] 204 | - [0.1044079, 0.2529673, 0] 205 | - [0.02222636, 0.01557862, 0] 206 | - [-0.3289129, 0.3271513, 0] 207 | - [0.1791184, 0.09718099, 0] 208 | - [0.4032499, -0.05860535, 0] 209 | - [0.05958161, -0.05860535, 0] 210 | - [0.11935, -0.04376855, 0] 211 | - [0.2837131, -0.1476261, 0] 212 | - [0.3434815, -0.1327893, 0.3912669] 213 | - [0.4331341, 0.01557862, 0] 214 | - [0.3434815, -0.2218101, 0.4018646] 215 | - [0.1791184, -0.01409497, 0] 216 | - [-0.03007098, -0.01409497, 0] 217 | - [0.11935, -0.1327893, 0] 218 | - [-0.01512889, -0.1031157, 0] 219 | - [-0.1496078, -0.05860535, 0] 220 | - [-0.3886814, -0.04376855, 0] 221 | - [0.2090026, -0.1031157, 0] 222 | - [0.3584236, 0.0749258, 0] 223 | - [-0.3139709, -0.05860535, 0] 224 | - [0.418192, -0.1476261, 0] 225 | - [0.3135973, 0.134273, 0] 226 | - [-0.4335077, -0.1476261, 0] 227 | - [-0.2542025, -0.01409497, 0] 228 | - [-0.4036234, 0.01557862, 0] 229 | - [-0.4484497, 0.3271513, 0] 230 | - [0.2388868, 0.03041542, 0] 231 | - [0.418192, -0.2366469, 0] 232 | - [-0.5231602, 0.03041542, 0] 233 | - [0.2837131, 0.1936202, 0] 234 | - [-0.478334, -0.3850148, 0] 235 | - [0.02969741, 0.2084569, 0] 236 | - [-0.3886814, 0.3419881, 0] 237 | - [-0.4484497, -0.2366469, 0] 238 | - [-0.2542025, 0.2826409, 0] 239 | - [-0.05995518, -0.3850148, 0] 240 | - [0.5078446, -0.01409497, 0] 241 | - [0.1940605, 0.1491098, 0] 242 | - [0.2388868, 0.3123145, 0] 243 | - [-0.2093762, -0.3108309, 0] 244 | - [0.3285394, 0.2678041, 0] 245 | - [0.1044079, 0.1936202, 0] 246 | - [0.4929025, -0.3405044, 0] 247 | - [0.2090026, -0.1921365, 0] 248 | - [-0.2392604, 0.2084569, 0] 249 | - [-0.3139709, 0.3419881, 0] 250 | - [-0.1645499, 0.3419881, 0] 251 | - [0.1641763, 0.2232938, 0] 252 | - [-0.4036234, -0.3850148, 0.4732608] 253 | - [-0.2990288, -0.3108309, 0] 254 | - [0.4480762, 0.2084569, 0] 255 | - [-0.03007098, -0.2514837, 0] 256 | - [-0.5231602, -0.3256676, 0] 257 | - [-0.3438551, 0.04525221, 0] -------------------------------------------------------------------------------- /Example/ExampleYAML.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of EIBA. 3 | * 4 | * Copyright (C) 2017 Zhejiang University 5 | * For more information see 6 | * 7 | * Licensed under the Apache License, Version 2.0 (the "License"); 8 | * You may not use this file except in compliance with the License. 9 | * You may obtain a copy of the License at 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | * Unless required by applicable law or agreed to in writing, software 12 | * distributed under the License is distributed on an "AS IS" BASIS, 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | * 17 | */ 18 | 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include "BundleAdjustment/BAInterface.h" 24 | 25 | int main(int argc, char *argv[]) { 26 | EIBA::BAInterface interface; 27 | EIBA::BAParam param; 28 | param.verbose = 1; 29 | interface.SetParam(param); 30 | 31 | for (int i = 0; i < 93; ++i) { 32 | std::stringstream ss; 33 | ss << argv[1] << "/" << std::setfill('0') << std::setw(5) << i << ".yaml"; 34 | interface.PushFrameInfoFromYAML(ss.str()); 35 | std::vector KFs; 36 | interface.Optimize(&KFs); 37 | } 38 | 39 | return 0; 40 | } 41 | -------------------------------------------------------------------------------- /Geometry/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(Geometry 2 | Camera.cpp 3 | ) 4 | target_link_libraries(Geometry ${CVD_LIBRARY}) 5 | -------------------------------------------------------------------------------- /Geometry/Camera.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of EIBA. 3 | * 4 | * Copyright (C) 2017 Zhejiang University 5 | * For more information see 6 | * 7 | * Licensed under the Apache License, Version 2.0 (the "License"); 8 | * You may not use this file except in compliance with the License. 9 | * You may obtain a copy of the License at 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | * Unless required by applicable law or agreed to in writing, software 12 | * distributed under the License is distributed on an "AS IS" BASIS, 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | * 17 | */ 18 | 19 | #include "Camera.h" 20 | 21 | #ifdef CFG_TUNE_PARAMETERS 22 | extern float CAMERA_EPSILON_BIAS_GYROSCOPE = 3.046174198662e-4f; 23 | extern void LOAD_PARAMETERS_CAMERA(const Configurator &cfgor) 24 | { 25 | CAMERA_EPSILON_BIAS_GYROSCOPE = cfgor.GetArgument( 26 | "param_camera_epsilone_bias_gyroscope", 27 | sqrt(CAMERA_EPSILON_BIAS_GYROSCOPE) * UT_FACTOR_RAD_TO_DEG); 28 | CAMERA_EPSILON_BIAS_GYROSCOPE = CAMERA_EPSILON_BIAS_GYROSCOPE * 29 | CAMERA_EPSILON_BIAS_GYROSCOPE * 30 | UT_FACTOR_DEG_TO_RAD * UT_FACTOR_DEG_TO_RAD; 31 | } 32 | #endif 33 | -------------------------------------------------------------------------------- /Geometry/Point.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of EIBA. 3 | * 4 | * Copyright (C) 2017 Zhejiang University 5 | * For more information see 6 | * 7 | * Licensed under the Apache License, Version 2.0 (the "License"); 8 | * You may not use this file except in compliance with the License. 9 | * You may obtain a copy of the License at 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | * Unless required by applicable law or agreed to in writing, software 12 | * distributed under the License is distributed on an "AS IS" BASIS, 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | * 17 | */ 18 | 19 | #include "PlatformIndependence/sse.h" 20 | 21 | #ifndef _POINT_H_ 22 | #define _POINT_H_ 23 | 24 | #include "stdafx.h" 25 | 26 | #include "Matrix2x2.h" 27 | #include "Matrix2x3.h" 28 | #include "Vector3.h" 29 | 30 | class Point2D : public LA::Vector2f 31 | { 32 | public: 33 | inline Point2D() : LA::Vector2f() {} 34 | inline Point2D(const float x, const float y) : LA::Vector2f(x, y) {} 35 | inline Point2D(const LA::Vector2f &x) { memcpy(this, x, 8); } 36 | }; 37 | 38 | class Point2DCovariance : public LA::SymmetricMatrix2x2f 39 | { 40 | public: 41 | inline Point2DCovariance() {} 42 | inline Point2DCovariance(const LA::SymmetricMatrix2x2f &S) { *this = S; } 43 | inline Point2DCovariance(const float sxx, const float sxy, const float syy) 44 | : LA::SymmetricMatrix2x2f(sxx, sxy, syy) 45 | { 46 | } 47 | 48 | inline const float &sxx() const { return m00(); } 49 | inline float &sxx() { return m00(); } 50 | inline const float &sxy() const { return m01(); } 51 | inline float &sxy() { return m01(); } 52 | inline const float &syy() const { return m11(); } 53 | inline float &syy() { return m11(); } 54 | inline void operator=(const LA::SymmetricMatrix2x2f &M) 55 | { 56 | memcpy(m_data, &M, sizeof(Point2DCovariance)); 57 | } 58 | inline void operator+=(const LA::Vector2f &x) 59 | { 60 | sxx() = x.x() * x.x() + sxx(); 61 | sxy() = x.x() * x.y() + sxy(); 62 | syy() = x.y() * x.y() + syy(); 63 | } 64 | inline void operator+=(const Point2DCovariance &S) 65 | { 66 | sxx() = S.sxx() + sxx(); 67 | sxy() = S.sxy() + sxy(); 68 | syy() = S.syy() + syy(); 69 | } 70 | 71 | inline float Area() const { return UT_PI * sqrt(Determinant()); } 72 | enum EigenDecomposeResult { 73 | EIGEN_DECOMPOSE_SUCCESS_X_SUCCESS_Y, 74 | EIGEN_DECOMPOSE_SUCCESS_X_FAIL_Y, 75 | EIGEN_DECOMPOSE_FAIL_X_SUCCESS_Y, 76 | EIGEN_DECOMPOSE_FAIL_X_FAIL_Y 77 | }; 78 | inline EigenDecomposeResult 79 | EigenDecompose(LA::AlignedMatrix2x2f &U, LA::Vector2f &l, 80 | const float eps = FLT_EPSILON) const 81 | { 82 | l.x() = (sxx() - syy()) * 0.5f; 83 | const float r = sqrt(l.x() * l.x() + sxy() * sxy()); 84 | l.y() = l.x() + syy(); 85 | l.x() = l.y() + r; 86 | l.y() = l.y() - r; 87 | U.m00() = -sxy(); 88 | U.m10() = sxx() - l.x(); 89 | U.m01() = -sxy(); 90 | U.m11() = sxx() - l.y(); 91 | const _pi__m128 lv2 = 92 | _pi_mm_mul_ps(U.m_00_01_10_11(), U.m_00_01_10_11()); 93 | const float lv0 = sqrt(lv2.m128_f32[0] + lv2.m128_f32[2]); 94 | const bool sccX = lv0 > eps; 95 | if (sccX) { 96 | const float sv0 = 1.0f / lv0; 97 | U.m00() *= sv0; 98 | U.m10() *= sv0; 99 | } else { 100 | l.x() = 0.0f; 101 | U.m00() = U.m10() = 0.0f; 102 | } 103 | const float lv1 = sqrt(lv2.m128_f32[1] + lv2.m128_f32[3]); 104 | const bool sccY = lv1 > eps; 105 | if (sccY) { 106 | const float sv1 = 1 / lv1; 107 | U.m01() *= sv1; 108 | U.m11() *= sv1; 109 | } else { 110 | l.y() = 0.0f; 111 | U.m01() = U.m11() = 0.0f; 112 | } 113 | if (sccX && sccY) 114 | return EIGEN_DECOMPOSE_SUCCESS_X_SUCCESS_Y; 115 | else if (sccX && !sccY) 116 | return EIGEN_DECOMPOSE_SUCCESS_X_FAIL_Y; 117 | else if (!sccX && sccY) 118 | return EIGEN_DECOMPOSE_FAIL_X_SUCCESS_Y; 119 | else 120 | return EIGEN_DECOMPOSE_FAIL_X_FAIL_Y; 121 | } 122 | inline void EigenCompose(const LA::AlignedMatrix2x2f &U, 123 | const LA::Vector2f &l) 124 | { 125 | const _pi__m128 t = _pi_mm_mul_ps(U.m_00_01_10_11(), U.m_00_01_10_11()); 126 | sxx() = l.x() * t.m128_f32[0] + l.y() * t.m128_f32[1]; 127 | sxy() = l.x() * U.m00() * U.m10() + l.y() * U.m01() * U.m11(); 128 | syy() = l.x() * t.m128_f32[2] + l.y() * t.m128_f32[3]; 129 | } 130 | }; 131 | 132 | class Point3D : public LA::AlignedVector3f 133 | { 134 | public: 135 | inline Point3D() { w() = 1.0f; } 136 | inline Point3D(const float x, const float y, const float z) 137 | { 138 | Set(x, y, z); 139 | } 140 | inline Point3D(const float *X) 141 | { 142 | memcpy(this, X, 12); 143 | w() = 1.0f; 144 | } 145 | inline Point3D(const LA::Vector2f &x, const float z) { Set(x, z); } 146 | inline Point3D(const AlignedVector3f &X) 147 | { 148 | xyzw() = X.xyzr(); 149 | w() = 1.0f; 150 | } 151 | inline Point3D(const _pi__m128 &X) 152 | { 153 | xyzw() = X; 154 | w() = 1.0f; 155 | } 156 | 157 | inline const _pi__m128 &xyzw() const { return xyzr(); } 158 | inline _pi__m128 &xyzw() { return xyzr(); } 159 | inline const float &w() const { return r(); } 160 | inline float &w() { return r(); } 161 | inline void operator=(const Point3D &X) { xyzw() = X.xyzw(); } 162 | inline void operator=(const AlignedVector3f &X) 163 | { 164 | xyzw() = X.xyzr(); 165 | w() = 1.0f; 166 | } 167 | inline void operator+=(const AlignedVector3f &X) 168 | { 169 | xyzw() = _pi_mm_add_ps(X.xyzr(), xyzw()); 170 | w() = 1.0f; 171 | } 172 | inline Point3D operator+(const AlignedVector3f &X) const 173 | { 174 | return Point3D(_pi_mm_add_ps(xyzw(), X.xyzr())); 175 | } 176 | inline Point3D operator-(const AlignedVector3f &X) const 177 | { 178 | return Point3D(_pi_mm_sub_ps(xyzw(), X.xyzr())); 179 | } 180 | inline Point3D operator*(const float s) const 181 | { 182 | return Point3D(_pi_mm_mul_ps(xyzw(), _pi_mm_set1_ps(s))); 183 | } 184 | inline Point3D operator*(const _pi__m128 &s) const 185 | { 186 | return Point3D(_pi_mm_mul_ps(xyzw(), s)); 187 | } 188 | inline Point3D operator/(const float d) const 189 | { 190 | return Point3D(_pi_mm_mul_ps(xyzw(), _pi_mm_set1_ps(1.0f / d))); 191 | } 192 | 193 | inline void Set(const float *X) 194 | { 195 | memcpy(this, X, 12); 196 | w() = 1.0f; 197 | } 198 | inline void Set(const double *X) 199 | { 200 | v012r() = _pi_mm_setr_ps(float(X[0]), float(X[1]), float(X[2]), 1.0f); 201 | } 202 | inline void Set(const float x, const float y, const float z) 203 | { 204 | xyzw() = _pi_mm_setr_ps(x, y, z, 1.0f); 205 | } 206 | inline void Set(const LA::Vector2f &x, const float z) 207 | { 208 | xyzw() = _pi_mm_setr_ps(x.x() * z, x.y() * z, z, 1.0f); 209 | } 210 | 211 | inline void MakeZero() 212 | { 213 | memset(this, 0, 12); 214 | w() = 1.0f; 215 | } 216 | 217 | inline void Normalize() 218 | { 219 | xyzw() = 220 | _pi_mm_mul_ps(_pi_mm_set1_ps(1.0f / sqrt(SquaredLength())), xyzw()); 221 | w() = 1.0f; 222 | } 223 | inline Point3D Cross(const AlignedVector3f &v) const 224 | { 225 | return Point3D(LA::AlignedVector3f::Cross(v)); 226 | } 227 | 228 | inline void Load(FILE *fp) 229 | { 230 | LA::AlignedVector3f::Load(fp); 231 | w() = 1.0f; 232 | } 233 | 234 | inline void Random(const float pMax) 235 | { 236 | LA::AlignedVector3f::Random(pMax); 237 | w() = 1.0f; 238 | } 239 | static inline Point3D GetRandom(const float pMax) 240 | { 241 | Point3D p; 242 | p.Random(pMax); 243 | return p; 244 | } 245 | }; 246 | 247 | #ifdef CFG_DEBUG_EIGEN 248 | class EigenPoint2D : public EigenVector2f 249 | { 250 | public: 251 | inline EigenPoint2D() : EigenVector2f() {} 252 | inline EigenPoint2D(const EigenVector2f &e_x) : EigenVector2f(e_x) {} 253 | inline EigenPoint2D(const float x, const float y) : EigenVector2f(x, y) {} 254 | }; 255 | class EigenPoint3D : public EigenVector3f 256 | { 257 | public: 258 | inline EigenPoint3D() : EigenVector3f() {} 259 | inline EigenPoint3D(const EigenVector3f &e_X) : EigenVector3f(e_X) {} 260 | inline EigenPoint3D(const EigenPoint2D &e_x) 261 | : EigenVector3f(e_x.x(), e_x.y(), 1.0f) 262 | { 263 | } 264 | inline EigenPoint3D(const EigenPoint2D &e_x, const float z) 265 | : EigenVector3f(e_x.x() * z, e_x.y() * z, z) 266 | { 267 | } 268 | inline EigenPoint3D(const float x, const float y, const float z) 269 | : EigenVector3f(x, y, z) 270 | { 271 | } 272 | inline EigenMatrix2x3f GetJacobianProjection() const 273 | { 274 | EigenMatrix2x3f e_J; 275 | const Eigen::Vector3f &e_X = *this; 276 | const float zI = 1.0f / e_X.z(), z2I = zI * zI; 277 | e_J(0, 0) = zI; 278 | e_J(0, 1) = 0.0f; 279 | e_J(0, 2) = -e_X.x() * z2I; 280 | e_J(1, 0) = 0.0f; 281 | e_J(1, 1) = zI; 282 | e_J(1, 2) = -e_X.y() * z2I; 283 | return e_J; 284 | } 285 | }; 286 | #endif 287 | #endif 288 | -------------------------------------------------------------------------------- /LinearAlgebra/LinearSystem.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of EIBA. 3 | * 4 | * Copyright (C) 2017 Zhejiang University 5 | * For more information see 6 | * 7 | * Licensed under the Apache License, Version 2.0 (the "License"); 8 | * You may not use this file except in compliance with the License. 9 | * You may obtain a copy of the License at 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | * Unless required by applicable law or agreed to in writing, software 12 | * distributed under the License is distributed on an "AS IS" BASIS, 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | * 17 | */ 18 | 19 | #ifndef _LINEAR_SYSTEM_H_ 20 | #define _LINEAR_SYSTEM_H_ 21 | 22 | #include 23 | 24 | #include "Utility.h" 25 | 26 | namespace LA 27 | { 28 | namespace LS 29 | { 30 | template inline bool DecomposeLDL(TYPE **A) 31 | { 32 | TYPE *t = A[N - 1]; 33 | const TYPE eps = UT::Epsilon(); 34 | for (int k = 0; k < N; ++k) { 35 | TYPE *ak = A[k]; 36 | const TYPE dk = ak[k]; 37 | if (UT::NotANumber(dk) || fabs(dk) <= eps) return false; 38 | memcpy(t, ak + k + 1, sizeof(TYPE) * (N - k - 1)); 39 | const TYPE dkI = 1 / dk; 40 | ak[k] = dkI; 41 | for (int j = k + 1; j < N; ++j) ak[j] *= dkI; 42 | for (int i = k + 1; i < N; ++i) { 43 | TYPE *ai = A[i]; 44 | const TYPE aki = t[i - k - 1]; 45 | for (int j = i; j < N; ++j) ai[j] -= aki * ak[j]; 46 | } 47 | } 48 | return true; 49 | } 50 | 51 | template inline bool SolveLDL(TYPE **A, TYPE *b) 52 | { 53 | if (!DecomposeLDL(A)) return false; 54 | for (int i = 0; i < N; ++i) { 55 | const TYPE *ai = A[i]; 56 | const TYPE bi = b[i]; 57 | for (int j = i + 1; j < N; ++j) b[j] -= ai[j] * bi; 58 | } 59 | for (int i = N - 1; i >= 0; --i) { 60 | const TYPE *ai = A[i]; 61 | TYPE bi = ai[i] * b[i]; 62 | for (int j = i + 1; j < N; ++j) bi -= ai[j] * b[j]; 63 | b[i] = bi; 64 | } 65 | return true; 66 | } 67 | 68 | template 69 | inline bool SolveLDL(TYPE **A, const bool decomposed = false) 70 | { 71 | if (!decomposed && !DecomposeLDL(A)) return false; 72 | for (int i = 0; i < N; ++i) { 73 | const TYPE *ai = A[i]; 74 | const TYPE bi = ai[N]; 75 | for (int j = i + 1; j < N; ++j) A[j][N] -= ai[j] * bi; 76 | } 77 | for (int i = N - 1; i >= 0; --i) { 78 | const TYPE *ai = A[i]; 79 | TYPE bi = ai[i] * ai[N]; 80 | for (int j = i + 1; j < N; ++j) bi -= ai[j] * A[j][N]; 81 | A[i][N] = bi; 82 | } 83 | return true; 84 | } 85 | 86 | template 87 | inline bool InverseLDL(TYPE **A, const bool decomposed = false) 88 | { 89 | if (!decomposed && !DecomposeLDL(A)) return false; 90 | for (int k = 0; k < N; ++k) { 91 | const TYPE *ak = A[k]; 92 | for (int j = k + 1; j < N; ++j) A[j][k] = -ak[j]; 93 | for (int i = k + 1; i < N; ++i) { 94 | const TYPE *ai = A[i]; 95 | const TYPE bi = ai[k]; 96 | for (int j = i + 1; j < N; ++j) A[j][k] -= ai[j] * bi; 97 | } 98 | for (int i = N - 1; i > k; --i) { 99 | const TYPE *ai = A[i]; 100 | TYPE bi = ai[i] * ai[k]; 101 | for (int j = i + 1; j < N; ++j) bi -= ai[j] * A[j][k]; 102 | A[i][k] = bi; 103 | } 104 | TYPE bk = ak[k]; 105 | for (int j = k + 1; j < N; ++j) bk -= ak[j] * A[j][k]; 106 | A[k][k] = bk; 107 | } 108 | for (int i = 0; i < N; ++i) 109 | for (int j = i + 1; j < N; ++j) A[i][j] = A[j][i]; 110 | return true; 111 | } 112 | } 113 | } 114 | 115 | #ifdef CFG_DEBUG_EIGEN 116 | #include 117 | template 118 | inline void EigenMarginalize(Eigen::Matrix &e_A, 119 | Eigen::Matrix &e_b) 120 | { 121 | const Eigen::Matrix e_M = 122 | e_A.block(Nm, 0) * e_A.block(0, 0).inverse(); 123 | e_A.block(Nm, 0) -= e_M * e_A.block(0, 0); 124 | e_b.block(Nm, 0) -= e_M * e_b.block(0, 0); 125 | } 126 | #endif 127 | 128 | #endif 129 | -------------------------------------------------------------------------------- /LinearAlgebra/Matrix2x3.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of EIBA. 3 | * 4 | * Copyright (C) 2017 Zhejiang University 5 | * For more information see 6 | * 7 | * Licensed under the Apache License, Version 2.0 (the "License"); 8 | * You may not use this file except in compliance with the License. 9 | * You may obtain a copy of the License at 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | * Unless required by applicable law or agreed to in writing, software 12 | * distributed under the License is distributed on an "AS IS" BASIS, 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | * 17 | */ 18 | 19 | #include "PlatformIndependence/sse.h" 20 | 21 | #ifndef _MATRIX_2x3_H_ 22 | #define _MATRIX_2x3_H_ 23 | 24 | #include "Vector3.h" 25 | 26 | namespace LA 27 | { 28 | class AlignedMatrix2x3f 29 | { 30 | public: 31 | inline const _pi__m128 &m_00_01_02_r0() const { return m_data[0]; } 32 | inline _pi__m128 &m_00_01_02_r0() { return m_data[0]; } 33 | inline const _pi__m128 &m_10_11_12_r1() const { return m_data[1]; } 34 | inline _pi__m128 &m_10_11_12_r1() { return m_data[1]; } 35 | inline const float &m00() const { return m_data[0].m128_f32[0]; } 36 | inline float &m00() { return m_data[0].m128_f32[0]; } 37 | inline const float &m01() const { return m_data[0].m128_f32[1]; } 38 | inline float &m01() { return m_data[0].m128_f32[1]; } 39 | inline const float &m02() const { return m_data[0].m128_f32[2]; } 40 | inline float &m02() { return m_data[0].m128_f32[2]; } 41 | inline const float &r0() const { return m_data[0].m128_f32[3]; } 42 | inline float &r0() { return m_data[0].m128_f32[3]; } 43 | inline const float &m10() const { return m_data[1].m128_f32[0]; } 44 | inline float &m10() { return m_data[1].m128_f32[0]; } 45 | inline const float &m11() const { return m_data[1].m128_f32[1]; } 46 | inline float &m11() { return m_data[1].m128_f32[1]; } 47 | inline const float &m12() const { return m_data[1].m128_f32[2]; } 48 | inline float &m12() { return m_data[1].m128_f32[2]; } 49 | inline const float &r1() const { return m_data[1].m128_f32[3]; } 50 | inline float &r1() { return m_data[1].m128_f32[3]; } 51 | inline operator const float *() const { return (const float *)this; } 52 | inline operator float *() { return (float *)this; } 53 | inline void operator+=(const AlignedMatrix2x3f &M) 54 | { 55 | m_00_01_02_r0() = _pi_mm_add_ps(M.m_00_01_02_r0(), m_00_01_02_r0()); 56 | m_10_11_12_r1() = _pi_mm_add_ps(M.m_10_11_12_r1(), m_10_11_12_r1()); 57 | } 58 | inline void operator-=(const AlignedMatrix2x3f &M) 59 | { 60 | m_00_01_02_r0() = _pi_mm_sub_ps(m_00_01_02_r0(), M.m_00_01_02_r0()); 61 | m_10_11_12_r1() = _pi_mm_sub_ps(m_10_11_12_r1(), M.m_10_11_12_r1()); 62 | } 63 | inline void operator*=(const float s) 64 | { 65 | const _pi__m128 _s = _pi_mm_set1_ps(s); 66 | Scale(_s); 67 | } 68 | inline void operator*=(const _pi__m128 &s) { Scale(s); } 69 | inline void Set(const float m00, const float m01, const float m02, 70 | const float m10, const float m11, const float m12) 71 | { 72 | m_00_01_02_r0() = _pi_mm_setr_ps(m00, m01, m02, 0); 73 | m_10_11_12_r1() = _pi_mm_setr_ps(m10, m11, m12, 0); 74 | } 75 | inline void Set(const float *M) 76 | { 77 | memcpy(&m00(), M, 12); 78 | memcpy(&m10(), M + 3, 12); 79 | } 80 | inline void Get(float *M) const 81 | { 82 | memcpy(M, &m00(), 12); 83 | memcpy(M + 3, &m10(), 12); 84 | } 85 | 86 | inline void MakeZero() { memset(this, 0, sizeof(AlignedMatrix2x3f)); } 87 | inline void MakeZero2x2() 88 | { 89 | m00() = 0.0f; 90 | m01() = 0.0f; 91 | m10() = 0.0f; 92 | m11() = 0.0f; 93 | } 94 | 95 | inline void Scale(const _pi__m128 &s) 96 | { 97 | m_00_01_02_r0() = _pi_mm_mul_ps(s, m_00_01_02_r0()); 98 | m_10_11_12_r1() = _pi_mm_mul_ps(s, m_10_11_12_r1()); 99 | } 100 | 101 | inline void AddATBTo(const Vector2f &B, AlignedVector3f &ATB) const 102 | { 103 | ATB.v012r() = _pi_mm_add_ps( 104 | _pi_mm_add_ps( 105 | _pi_mm_mul_ps(m_00_01_02_r0(), _pi_mm_set1_ps(B.v0())), 106 | _pi_mm_mul_ps(m_10_11_12_r1(), _pi_mm_set1_ps(B.v1()))), 107 | ATB.v012r()); 108 | } 109 | 110 | inline void Print(const bool e = false) const 111 | { 112 | if (e) { 113 | UT::Print("%e %e %e\n", m00(), m01(), m02()); 114 | UT::Print("%e %e %e\n", m10(), m11(), m12()); 115 | } else { 116 | UT::Print("%f %f %f\n", m00(), m01(), m02()); 117 | UT::Print("%f %f %f\n", m10(), m11(), m12()); 118 | } 119 | } 120 | inline void AssertZero() const 121 | { 122 | UT::AssertEqual( 123 | SSE::Sum012(_pi_mm_mul_ps(m_00_01_02_r0(), m_00_01_02_r0())), 0.0f); 124 | UT::AssertEqual( 125 | SSE::Sum012(_pi_mm_mul_ps(m_10_11_12_r1(), m_10_11_12_r1())), 0.0f); 126 | } 127 | inline bool AssertEqual(const AlignedMatrix2x3f &M, 128 | const int verbose = 1) const 129 | { 130 | if (UT::VectorAssertEqual(&m00(), &M.m00(), 3, 0) && 131 | UT::VectorAssertEqual(&m10(), &M.m10(), 3, 0)) 132 | return true; 133 | if (verbose) { 134 | UT::PrintSeparator(); 135 | Print(verbose > 1); 136 | UT::PrintSeparator(); 137 | M.Print(verbose > 1); 138 | } 139 | return false; 140 | } 141 | 142 | inline void SetInfinite() 143 | { 144 | m_00_01_02_r0() = m_10_11_12_r1() = _pi_mm_set1_ps(FLT_MAX); 145 | } 146 | inline void AssertInfinite() const 147 | { 148 | UT_ASSERT(m00() == FLT_MAX); 149 | UT_ASSERT(m01() == FLT_MAX); 150 | UT_ASSERT(m02() == FLT_MAX); 151 | UT_ASSERT(r0() == FLT_MAX); 152 | UT_ASSERT(m10() == FLT_MAX); 153 | UT_ASSERT(m11() == FLT_MAX); 154 | UT_ASSERT(m12() == FLT_MAX); 155 | UT_ASSERT(r1() == FLT_MAX); 156 | } 157 | inline void AssertFinite() const 158 | { 159 | UT_ASSERT(m00() != FLT_MAX); 160 | UT_ASSERT(m01() != FLT_MAX); 161 | UT_ASSERT(m02() != FLT_MAX); 162 | UT_ASSERT(r0() != FLT_MAX); 163 | UT_ASSERT(m10() != FLT_MAX); 164 | UT_ASSERT(m11() != FLT_MAX); 165 | UT_ASSERT(m12() != FLT_MAX); 166 | UT_ASSERT(r1() != FLT_MAX); 167 | } 168 | 169 | static inline void AB(const SymmetricMatrix2x2f &A, 170 | const AlignedMatrix2x3f &B1, 171 | const AlignedMatrix2x3f &B2, AlignedMatrix2x3f &AB1, 172 | AlignedMatrix2x3f &AB2) 173 | { 174 | const _pi__m128 a00 = _pi_mm_set1_ps(A.m00()), 175 | a01 = _pi_mm_set1_ps(A.m01()), 176 | a11 = _pi_mm_set1_ps(A.m11()); 177 | AB1.m_00_01_02_r0() = 178 | _pi_mm_add_ps(_pi_mm_mul_ps(a00, B1.m_00_01_02_r0()), 179 | _pi_mm_mul_ps(a01, B1.m_10_11_12_r1())); 180 | AB1.m_10_11_12_r1() = 181 | _pi_mm_add_ps(_pi_mm_mul_ps(a01, B1.m_00_01_02_r0()), 182 | _pi_mm_mul_ps(a11, B1.m_10_11_12_r1())); 183 | AB2.m_00_01_02_r0() = 184 | _pi_mm_add_ps(_pi_mm_mul_ps(a00, B2.m_00_01_02_r0()), 185 | _pi_mm_mul_ps(a01, B2.m_10_11_12_r1())); 186 | AB2.m_10_11_12_r1() = 187 | _pi_mm_add_ps(_pi_mm_mul_ps(a01, B2.m_00_01_02_r0()), 188 | _pi_mm_mul_ps(a11, B2.m_10_11_12_r1())); 189 | } 190 | 191 | protected: 192 | _pi__m128 m_data[2]; 193 | }; 194 | } 195 | 196 | #ifdef CFG_DEBUG_EIGEN 197 | class EigenMatrix2x3f : public Eigen::Matrix 198 | { 199 | public: 200 | inline EigenMatrix2x3f() = default; 201 | inline EigenMatrix2x3f(const Eigen::Matrix &e_M) 202 | : Eigen::Matrix(e_M) 203 | { 204 | } 205 | inline EigenMatrix2x3f(const LA::AlignedMatrix2x3f &M) 206 | : Eigen::Matrix() 207 | { 208 | Eigen::Matrix &e_M = *this; 209 | e_M(0, 0) = M.m00(); 210 | e_M(0, 1) = M.m01(); 211 | e_M(0, 2) = M.m02(); 212 | e_M(1, 0) = M.m10(); 213 | e_M(1, 1) = M.m11(); 214 | e_M(1, 2) = M.m12(); 215 | } 216 | inline void operator=(const Eigen::Matrix &e_M) 217 | { 218 | *((Eigen::Matrix *)this) = e_M; 219 | } 220 | inline LA::AlignedMatrix2x3f GetAlignedMatrix2x3f() const 221 | { 222 | LA::AlignedMatrix2x3f M; 223 | const Eigen::Matrix &e_M = *this; 224 | M.m00() = e_M(0, 0); 225 | M.m01() = e_M(0, 1); 226 | M.m02() = e_M(0, 2); 227 | M.m10() = e_M(1, 0); 228 | M.m11() = e_M(1, 1); 229 | M.m12() = e_M(1, 2); 230 | return M; 231 | } 232 | inline void Print(const bool e = false) const 233 | { 234 | GetAlignedMatrix2x3f().Print(e); 235 | } 236 | inline bool AssertEqual(const LA::AlignedMatrix2x3f &M, 237 | const int verbose = 1) const 238 | { 239 | return GetAlignedMatrix2x3f().AssertEqual(M, verbose); 240 | } 241 | inline bool AssertEqual(const EigenMatrix2x3f &e_M, 242 | const int verbose = 1) const 243 | { 244 | return AssertEqual(e_M.GetAlignedMatrix2x3f(), verbose); 245 | } 246 | }; 247 | #endif 248 | #endif 249 | -------------------------------------------------------------------------------- /LinearAlgebra/Matrix2x4.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of EIBA. 3 | * 4 | * Copyright (C) 2017 Zhejiang University 5 | * For more information see 6 | * 7 | * Licensed under the Apache License, Version 2.0 (the "License"); 8 | * You may not use this file except in compliance with the License. 9 | * You may obtain a copy of the License at 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | * Unless required by applicable law or agreed to in writing, software 12 | * distributed under the License is distributed on an "AS IS" BASIS, 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | * 17 | */ 18 | 19 | #include "PlatformIndependence/sse.h" 20 | 21 | #ifndef _MATRIX_2x4_H_ 22 | #define _MATRIX_2x4_H_ 23 | 24 | #include "Utility.h" 25 | 26 | namespace LA 27 | { 28 | class AlignedMatrix2x4f 29 | { 30 | public: 31 | inline const _pi__m128 &m_00_01_02_03() const { return m_data[0]; } 32 | inline _pi__m128 &m_00_01_02_03() { return m_data[0]; } 33 | inline const _pi__m128 &m_10_11_12_13() const { return m_data[1]; } 34 | inline _pi__m128 &m_10_11_12_13() { return m_data[1]; } 35 | inline const float &m00() const { return m_data[0].m128_f32[0]; } 36 | inline float &m00() { return m_data[0].m128_f32[0]; } 37 | inline const float &m01() const { return m_data[0].m128_f32[1]; } 38 | inline float &m01() { return m_data[0].m128_f32[1]; } 39 | inline const float &m02() const { return m_data[0].m128_f32[2]; } 40 | inline float &m02() { return m_data[0].m128_f32[2]; } 41 | inline const float &m03() const { return m_data[0].m128_f32[3]; } 42 | inline float &m03() { return m_data[0].m128_f32[3]; } 43 | inline const float &m10() const { return m_data[1].m128_f32[0]; } 44 | inline float &m10() { return m_data[1].m128_f32[0]; } 45 | inline const float &m11() const { return m_data[1].m128_f32[1]; } 46 | inline float &m11() { return m_data[1].m128_f32[1]; } 47 | inline const float &m12() const { return m_data[1].m128_f32[2]; } 48 | inline float &m12() { return m_data[1].m128_f32[2]; } 49 | inline const float &m13() const { return m_data[1].m128_f32[3]; } 50 | inline float &m13() { return m_data[1].m128_f32[3]; } 51 | inline operator const float *() const { return (const float *)this; } 52 | inline operator float *() { return (float *)this; } 53 | inline void operator+=(const AlignedMatrix2x4f &M) 54 | { 55 | m_data[0] = _pi_mm_add_ps(M.m_data[0], m_data[0]); 56 | m_data[1] = _pi_mm_add_ps(M.m_data[1], m_data[1]); 57 | } 58 | inline void operator-=(const AlignedMatrix2x4f &M) 59 | { 60 | m_data[0] = _pi_mm_sub_ps(m_data[0], M.m_data[0]); 61 | m_data[1] = _pi_mm_sub_ps(m_data[1], M.m_data[1]); 62 | } 63 | inline void operator*=(const _pi__m128 &s) 64 | { 65 | m_data[0] = _pi_mm_mul_ps(s, m_data[0]); 66 | m_data[1] = _pi_mm_mul_ps(s, m_data[1]); 67 | } 68 | 69 | inline void Set(const float m00, const float m01, const float m02, 70 | const float m03, const float m10, const float m11, 71 | const float m12, const float m13) 72 | { 73 | m_data[0] = _pi_mm_setr_ps(m00, m01, m02, m03); 74 | m_data[1] = _pi_mm_setr_ps(m10, m11, m12, m13); 75 | } 76 | inline void Set(const float *M) 77 | { 78 | memcpy(&m00(), M, sizeof(AlignedMatrix2x4f)); 79 | } 80 | inline void Get(float *M) const 81 | { 82 | memcpy(M, &m00(), sizeof(AlignedMatrix2x4f)); 83 | } 84 | 85 | inline void MakeZero() { memset(this, 0, sizeof(AlignedMatrix2x4f)); } 86 | inline void Print(const bool e = false) const 87 | { 88 | if (e) { 89 | UT::Print("%e %e %e %e\n", m00(), m01(), m02(), m03()); 90 | UT::Print("%e %e %e %e\n", m10(), m11(), m12(), m13()); 91 | } else { 92 | UT::Print("%f %f %f %f\n", m00(), m01(), m02(), m03()); 93 | UT::Print("%f %f %f %f\n", m10(), m11(), m12(), m13()); 94 | } 95 | } 96 | inline void AssertZero() const 97 | { 98 | UT::AssertEqual( 99 | SSE::Sum012(_pi_mm_mul_ps(m_00_01_02_03(), m_00_01_02_03())), 0.0f); 100 | UT::AssertEqual( 101 | SSE::Sum012(_pi_mm_mul_ps(m_10_11_12_13(), m_10_11_12_13())), 0.0f); 102 | } 103 | inline bool AssertEqual(const AlignedMatrix2x4f &M, 104 | const int verbose = 1) const 105 | { 106 | if (UT::VectorAssertEqual(&m00(), &M.m00(), 8, 0)) return true; 107 | if (verbose) { 108 | UT::PrintSeparator(); 109 | Print(verbose > 1); 110 | UT::PrintSeparator(); 111 | M.Print(verbose > 1); 112 | } 113 | return false; 114 | } 115 | 116 | protected: 117 | _pi__m128 m_data[2]; 118 | }; 119 | } 120 | 121 | #ifdef CFG_DEBUG_EIGEN 122 | #include 123 | class EigenMatrix2x4f : public Eigen::Matrix 124 | { 125 | public: 126 | inline EigenMatrix2x4f() = default; 127 | inline EigenMatrix2x4f(const Eigen::Matrix &e_M) 128 | : Eigen::Matrix(e_M) 129 | { 130 | } 131 | inline EigenMatrix2x4f(const LA::AlignedMatrix2x4f &M) 132 | : Eigen::Matrix() 133 | { 134 | Eigen::Matrix &e_M = *this; 135 | e_M(0, 0) = M.m00(); 136 | e_M(0, 1) = M.m01(); 137 | e_M(0, 2) = M.m02(); 138 | e_M(0, 3) = M.m03(); 139 | e_M(1, 0) = M.m10(); 140 | e_M(1, 1) = M.m11(); 141 | e_M(0, 2) = M.m02(); 142 | e_M(0, 3) = M.m03(); 143 | } 144 | inline void operator=(const Eigen::Matrix &e_M) 145 | { 146 | *((Eigen::Matrix *)this) = e_M; 147 | } 148 | inline LA::AlignedMatrix2x4f GetAlignedMatrix2x4f() const 149 | { 150 | LA::AlignedMatrix2x4f M; 151 | const Eigen::Matrix &e_M = *this; 152 | M.m00() = e_M(0, 0); 153 | M.m01() = e_M(0, 1); 154 | M.m02() = e_M(0, 2); 155 | M.m03() = e_M(0, 3); 156 | M.m10() = e_M(1, 0); 157 | M.m11() = e_M(1, 1); 158 | M.m12() = e_M(1, 2); 159 | M.m13() = e_M(1, 3); 160 | return M; 161 | } 162 | inline void Print(const bool e = false) const 163 | { 164 | GetAlignedMatrix2x4f().Print(e); 165 | } 166 | inline bool AssertEqual(const LA::AlignedMatrix2x4f &M, 167 | const int verbose = 1) const 168 | { 169 | return GetAlignedMatrix2x4f().AssertEqual(M, verbose); 170 | } 171 | inline bool AssertEqual(const EigenMatrix2x4f &e_M, 172 | const int verbose = 1) const 173 | { 174 | return AssertEqual(e_M.GetAlignedMatrix2x4f(), verbose); 175 | } 176 | }; 177 | #endif 178 | #endif 179 | -------------------------------------------------------------------------------- /LinearAlgebra/Matrix2x6.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of EIBA. 3 | * 4 | * Copyright (C) 2017 Zhejiang University 5 | * For more information see 6 | * 7 | * Licensed under the Apache License, Version 2.0 (the "License"); 8 | * You may not use this file except in compliance with the License. 9 | * You may obtain a copy of the License at 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | * Unless required by applicable law or agreed to in writing, software 12 | * distributed under the License is distributed on an "AS IS" BASIS, 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | * 17 | */ 18 | 19 | #include "PlatformIndependence/sse.h" 20 | 21 | #ifndef _MATRIX_2x6_H_ 22 | #define _MATRIX_2x6_H_ 23 | 24 | #include "Matrix2x3.h" 25 | #include "Vector6.h" 26 | 27 | namespace LA 28 | { 29 | class AlignedMatrix2x6f 30 | { 31 | public: 32 | inline const _pi__m128 &m_00_01_02_03() const { return m_data[0]; } 33 | inline _pi__m128 &m_00_01_02_03() { return m_data[0]; } 34 | inline const _pi__m128 &m_04_05_x_x() const { return m_data[1]; } 35 | inline _pi__m128 &m_04_05_x_x() { return m_data[1]; } 36 | inline const _pi__m128 &m_10_11_12_13() const { return m_data[2]; } 37 | inline _pi__m128 &m_10_11_12_13() { return m_data[2]; } 38 | inline const _pi__m128 &m_14_15_x_x() const { return m_data[3]; } 39 | inline _pi__m128 &m_14_15_x_x() { return m_data[3]; } 40 | inline const float &m00() const { return m_data[0].m128_f32[0]; } 41 | inline float &m00() { return m_data[0].m128_f32[0]; } 42 | inline const float &m01() const { return m_data[0].m128_f32[1]; } 43 | inline float &m01() { return m_data[0].m128_f32[1]; } 44 | inline const float &m02() const { return m_data[0].m128_f32[2]; } 45 | inline float &m02() { return m_data[0].m128_f32[2]; } 46 | inline const float &m03() const { return m_data[0].m128_f32[3]; } 47 | inline float &m03() { return m_data[0].m128_f32[3]; } 48 | inline const float &m04() const { return m_data[1].m128_f32[0]; } 49 | inline float &m04() { return m_data[1].m128_f32[0]; } 50 | inline const float &m05() const { return m_data[1].m128_f32[1]; } 51 | inline float &m05() { return m_data[1].m128_f32[1]; } 52 | inline const float &m10() const { return m_data[2].m128_f32[0]; } 53 | inline float &m10() { return m_data[2].m128_f32[0]; } 54 | inline const float &m11() const { return m_data[2].m128_f32[1]; } 55 | inline float &m11() { return m_data[2].m128_f32[1]; } 56 | inline const float &m12() const { return m_data[2].m128_f32[2]; } 57 | inline float &m12() { return m_data[2].m128_f32[2]; } 58 | inline const float &m13() const { return m_data[2].m128_f32[3]; } 59 | inline float &m13() { return m_data[2].m128_f32[3]; } 60 | inline const float &m14() const { return m_data[3].m128_f32[0]; } 61 | inline float &m14() { return m_data[3].m128_f32[0]; } 62 | inline const float &m15() const { return m_data[3].m128_f32[1]; } 63 | inline float &m15() { return m_data[3].m128_f32[1]; } 64 | inline operator const float *() const { return (const float *)this; } 65 | inline operator float *() { return (float *)this; } 66 | inline void Set(const AlignedMatrix2x3f &M1, AlignedMatrix2x3f &M2) 67 | { 68 | memcpy(&m00(), &M1.m00(), 12); 69 | memcpy(&m03(), &M2.m00(), 12); 70 | memcpy(&m10(), &M1.m10(), 12); 71 | memcpy(&m13(), &M2.m10(), 12); 72 | } 73 | inline void MakeZero() { memset(this, 0, sizeof(AlignedMatrix2x6f)); } 74 | inline void Print(const bool e = false) const 75 | { 76 | if (e) { 77 | UT::Print("%e %e %e %e %e %e\n", m00(), m01(), m02(), m03(), m04(), 78 | m05()); 79 | UT::Print("%e %e %e %e %e %e\n", m10(), m11(), m12(), m13(), m14(), 80 | m15()); 81 | } else { 82 | UT::Print("%f %f %f %f %f %f\n", m00(), m01(), m02(), m03(), m04(), 83 | m05()); 84 | UT::Print("%f %f %f %f %f %f\n", m10(), m11(), m12(), m13(), m14(), 85 | m15()); 86 | } 87 | } 88 | inline bool AssertEqual(const AlignedMatrix2x6f &M, 89 | const int verbose = 1) const 90 | { 91 | if (UT::VectorAssertEqual(&m00(), &M.m00(), 6, 0) && 92 | UT::VectorAssertEqual(&m10(), &M.m10(), 6, 0)) 93 | return true; 94 | if (verbose) { 95 | UT::PrintSeparator(); 96 | Print(verbose > 1); 97 | UT::PrintSeparator(); 98 | M.Print(verbose > 1); 99 | } 100 | return false; 101 | } 102 | 103 | static inline void AB(const SymmetricMatrix2x2f &A, 104 | const AlignedMatrix2x6f &B, AlignedMatrix2x6f &AB) 105 | { 106 | const _pi__m128 a01 = _pi_mm_set1_ps(A.m01()); 107 | AB.m_00_01_02_03() = _pi_mm_add_ps( 108 | _pi_mm_mul_ps(_pi_mm_set1_ps(A.m00()), B.m_00_01_02_03()), 109 | _pi_mm_mul_ps(a01, B.m_10_11_12_13())); 110 | AB.m04() = A.m00() * B.m04() + A.m01() * B.m14(); 111 | AB.m05() = A.m00() * B.m05() + A.m01() * B.m15(); 112 | AB.m_10_11_12_13() = _pi_mm_add_ps( 113 | _pi_mm_mul_ps(a01, B.m_00_01_02_03()), 114 | _pi_mm_mul_ps(_pi_mm_set1_ps(A.m11()), B.m_10_11_12_13())); 115 | AB.m14() = A.m01() * B.m04() + A.m11() * B.m14(); 116 | AB.m15() = A.m01() * B.m05() + A.m11() * B.m15(); 117 | } 118 | 119 | static inline void AddATBTo(const AlignedMatrix2x6f &A, Vector2f &B, 120 | AlignedVector6f &ATB) 121 | { 122 | ATB.v0123() = _pi_mm_add_ps( 123 | _pi_mm_add_ps( 124 | _pi_mm_mul_ps(A.m_00_01_02_03(), _pi_mm_set1_ps(B.v0())), 125 | _pi_mm_mul_ps(A.m_10_11_12_13(), _pi_mm_set1_ps(B.v1()))), 126 | ATB.v0123()); 127 | ATB.v4() = A.m04() * B.v0() + A.m14() + B.v1() + ATB.v4(); 128 | ATB.v5() = A.m05() * B.v0() + A.m15() * B.v1() + ATB.v5(); 129 | } 130 | 131 | protected: 132 | _pi__m128 m_data[4]; 133 | }; 134 | } 135 | 136 | #ifdef CFG_DEBUG_EIGEN 137 | class EigenMatrix2x6f : public Eigen::Matrix 138 | { 139 | public: 140 | inline EigenMatrix2x6f() = default; 141 | inline EigenMatrix2x6f(const Eigen::Matrix &e_M) 142 | : Eigen::Matrix(e_M) 143 | { 144 | } 145 | inline EigenMatrix2x6f(const LA::AlignedMatrix2x6f &M) 146 | : Eigen::Matrix() 147 | { 148 | const float *_M[2] = {&M.m00(), &M.m10()}; 149 | Eigen::Matrix &e_M = *this; 150 | for (int i = 0; i < 2; ++i) 151 | for (int j = 0; j < 6; ++j) e_M(i, j) = _M[i][j]; 152 | } 153 | inline EigenMatrix2x6f(const EigenMatrix2x3f &M0, const EigenMatrix2x3f &M1) 154 | { 155 | block<2, 3>(0, 0) = M0; 156 | block<2, 3>(0, 3) = M1; 157 | } 158 | inline void operator=(const Eigen::Matrix &e_M) 159 | { 160 | *((Eigen::Matrix *)this) = e_M; 161 | } 162 | inline LA::AlignedMatrix2x6f GetAlignedMatrix2x6f() const 163 | { 164 | LA::AlignedMatrix2x6f M; 165 | float *_M[2] = {&M.m00(), &M.m10()}; 166 | const Eigen::Matrix &e_M = *this; 167 | for (int i = 0; i < 2; ++i) 168 | for (int j = 0; j < 6; ++j) _M[i][j] = e_M(i, j); 169 | return M; 170 | } 171 | inline void Print(const bool e = false) const 172 | { 173 | GetAlignedMatrix2x6f().Print(e); 174 | } 175 | inline bool AssertEqual(const LA::AlignedMatrix2x6f &M, 176 | const int verbose = 1) const 177 | { 178 | return GetAlignedMatrix2x6f().AssertEqual(M, verbose); 179 | } 180 | inline bool AssertEqual(const EigenMatrix2x6f &e_M, 181 | const int verbose = 1) const 182 | { 183 | return AssertEqual(e_M.GetAlignedMatrix2x6f(), verbose); 184 | } 185 | }; 186 | #endif 187 | #endif 188 | -------------------------------------------------------------------------------- /LinearAlgebra/Matrix2x7.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of EIBA. 3 | * 4 | * Copyright (C) 2017 Zhejiang University 5 | * For more information see 6 | * 7 | * Licensed under the Apache License, Version 2.0 (the "License"); 8 | * You may not use this file except in compliance with the License. 9 | * You may obtain a copy of the License at 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | * Unless required by applicable law or agreed to in writing, software 12 | * distributed under the License is distributed on an "AS IS" BASIS, 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | * 17 | */ 18 | 19 | #include "PlatformIndependence/sse.h" 20 | 21 | #ifndef _MATRIX_2x7_H_ 22 | #define _MATRIX_2x7_H_ 23 | 24 | #include "Matrix2x6.h" 25 | 26 | namespace LA 27 | { 28 | class AlignedMatrix2x7f 29 | { 30 | public: 31 | inline const _pi__m128 &m_00_01_02_03() const { return m_data[0]; } 32 | inline _pi__m128 &m_00_01_02_03() { return m_data[0]; } 33 | inline const _pi__m128 &m_04_05_06_x() const { return m_data[1]; } 34 | inline _pi__m128 &m_04_05_06_x() { return m_data[1]; } 35 | inline const _pi__m128 &m_10_11_12_13() const { return m_data[2]; } 36 | inline _pi__m128 &m_10_11_12_13() { return m_data[2]; } 37 | inline const _pi__m128 &m_14_15_16_x() const { return m_data[3]; } 38 | inline _pi__m128 &m_14_15_16_x() { return m_data[3]; } 39 | inline const float &m00() const { return m_data[0].m128_f32[0]; } 40 | inline float &m00() { return m_data[0].m128_f32[0]; } 41 | inline const float &m01() const { return m_data[0].m128_f32[1]; } 42 | inline float &m01() { return m_data[0].m128_f32[1]; } 43 | inline const float &m02() const { return m_data[0].m128_f32[2]; } 44 | inline float &m02() { return m_data[0].m128_f32[2]; } 45 | inline const float &m03() const { return m_data[0].m128_f32[3]; } 46 | inline float &m03() { return m_data[0].m128_f32[3]; } 47 | inline const float &m04() const { return m_data[1].m128_f32[0]; } 48 | inline float &m04() { return m_data[1].m128_f32[0]; } 49 | inline const float &m05() const { return m_data[1].m128_f32[1]; } 50 | inline float &m05() { return m_data[1].m128_f32[1]; } 51 | inline const float &m06() const { return m_data[1].m128_f32[2]; } 52 | inline float &m06() { return m_data[1].m128_f32[2]; } 53 | inline const float &m10() const { return m_data[2].m128_f32[0]; } 54 | inline float &m10() { return m_data[2].m128_f32[0]; } 55 | inline const float &m11() const { return m_data[2].m128_f32[1]; } 56 | inline float &m11() { return m_data[2].m128_f32[1]; } 57 | inline const float &m12() const { return m_data[2].m128_f32[2]; } 58 | inline float &m12() { return m_data[2].m128_f32[2]; } 59 | inline const float &m13() const { return m_data[2].m128_f32[3]; } 60 | inline float &m13() { return m_data[2].m128_f32[3]; } 61 | inline const float &m14() const { return m_data[3].m128_f32[0]; } 62 | inline float &m14() { return m_data[3].m128_f32[0]; } 63 | inline const float &m15() const { return m_data[3].m128_f32[1]; } 64 | inline float &m15() { return m_data[3].m128_f32[1]; } 65 | inline const float &m16() const { return m_data[3].m128_f32[2]; } 66 | inline float &m16() { return m_data[3].m128_f32[2]; } 67 | inline operator const float *() const { return (const float *)this; } 68 | inline operator float *() { return (float *)this; } 69 | inline void Set(const AlignedMatrix2x6f &M0, const Vector2f &M1) 70 | { 71 | memcpy(&m00(), &M0.m00(), 24); 72 | m06() = M1.v0(); 73 | memcpy(&m10(), &M0.m10(), 24); 74 | m16() = M1.v1(); 75 | } 76 | inline void MakeZero() { memset(this, 0, sizeof(AlignedMatrix2x7f)); } 77 | inline void Print(const bool e = false) const 78 | { 79 | if (e) { 80 | UT::Print("%e %e %e %e %e %e %e\n", m00(), m01(), m02(), m03(), 81 | m04(), m05(), m06()); 82 | UT::Print("%e %e %e %e %e %e %e\n", m10(), m11(), m12(), m13(), 83 | m14(), m15(), m16()); 84 | } else { 85 | UT::Print("%f %f %f %f %f %f %f\n", m00(), m01(), m02(), m03(), 86 | m04(), m05(), m06()); 87 | UT::Print("%f %f %f %f %f %f %f\n", m10(), m11(), m12(), m13(), 88 | m14(), m15(), m16()); 89 | } 90 | } 91 | inline bool AssertEqual(const AlignedMatrix2x7f &M, 92 | const int verbose = 1) const 93 | { 94 | if (UT::VectorAssertEqual(&m00(), &M.m00(), 7, 0) && 95 | UT::VectorAssertEqual(&m10(), &M.m10(), 7, 0)) 96 | return true; 97 | if (verbose) { 98 | UT::PrintSeparator(); 99 | Print(verbose > 1); 100 | UT::PrintSeparator(); 101 | M.Print(verbose > 1); 102 | } 103 | return false; 104 | } 105 | 106 | protected: 107 | _pi__m128 m_data[4]; 108 | }; 109 | } 110 | 111 | #ifdef CFG_DEBUG_EIGEN 112 | class EigenMatrix2x7f : public Eigen::Matrix 113 | { 114 | public: 115 | inline EigenMatrix2x7f() = default; 116 | inline EigenMatrix2x7f(const Eigen::Matrix &e_M) 117 | : Eigen::Matrix(e_M) 118 | { 119 | } 120 | inline EigenMatrix2x7f(const LA::AlignedMatrix2x7f &M) 121 | : Eigen::Matrix() 122 | { 123 | const float *_M[2] = {&M.m00(), &M.m10()}; 124 | Eigen::Matrix &e_M = *this; 125 | for (int i = 0; i < 2; ++i) 126 | for (int j = 0; j < 7; ++j) e_M(i, j) = _M[i][j]; 127 | } 128 | inline EigenMatrix2x7f(const EigenMatrix2x6f &M0, const EigenVector2f &M1) 129 | { 130 | block<2, 6>(0, 0) = M0; 131 | block<2, 1>(0, 6) = M1; 132 | } 133 | inline void operator=(const Eigen::Matrix &e_M) 134 | { 135 | *((Eigen::Matrix *)this) = e_M; 136 | } 137 | inline LA::AlignedMatrix2x7f GetAlignedMatrix2x7f() const 138 | { 139 | LA::AlignedMatrix2x7f M; 140 | float *_M[2] = {&M.m00(), &M.m10()}; 141 | const Eigen::Matrix &e_M = *this; 142 | for (int i = 0; i < 2; ++i) 143 | for (int j = 0; j < 7; ++j) _M[i][j] = e_M(i, j); 144 | return M; 145 | } 146 | inline void Print(const bool e = false) const 147 | { 148 | GetAlignedMatrix2x7f().Print(e); 149 | } 150 | inline bool AssertEqual(const LA::AlignedMatrix2x7f &M, 151 | const int verbose = 1) const 152 | { 153 | return GetAlignedMatrix2x7f().AssertEqual(M, verbose); 154 | } 155 | inline bool AssertEqual(const EigenMatrix2x7f &e_M, 156 | const int verbose = 1) const 157 | { 158 | return AssertEqual(e_M.GetAlignedMatrix2x7f(), verbose); 159 | } 160 | }; 161 | #endif 162 | #endif 163 | -------------------------------------------------------------------------------- /LinearAlgebra/Matrix2x8.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of EIBA. 3 | * 4 | * Copyright (C) 2017 Zhejiang University 5 | * For more information see 6 | * 7 | * Licensed under the Apache License, Version 2.0 (the "License"); 8 | * You may not use this file except in compliance with the License. 9 | * You may obtain a copy of the License at 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | * Unless required by applicable law or agreed to in writing, software 12 | * distributed under the License is distributed on an "AS IS" BASIS, 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | * 17 | */ 18 | 19 | #include "PlatformIndependence/sse.h" 20 | 21 | #ifndef _MATRIX_2x8_H_ 22 | #define _MATRIX_2x8_H_ 23 | 24 | #include "Vector8.h" 25 | 26 | namespace LA 27 | { 28 | class AlignedMatrix2x8f 29 | { 30 | public: 31 | inline const _pi__m128 &m_00_01_02_03() const { return m_data[0]; } 32 | inline _pi__m128 &m_00_01_02_03() { return m_data[0]; } 33 | inline const _pi__m128 &m_04_05_06_07() const { return m_data[1]; } 34 | inline _pi__m128 &m_04_05_06_07() { return m_data[1]; } 35 | inline const _pi__m128 &m_10_11_12_13() const { return m_data[2]; } 36 | inline _pi__m128 &m_10_11_12_13() { return m_data[2]; } 37 | inline const _pi__m128 &m_14_15_16_17() const { return m_data[3]; } 38 | inline _pi__m128 &m_14_15_16_17() { return m_data[3]; } 39 | inline const float &m00() const { return m_data[0].m128_f32[0]; } 40 | inline float &m00() { return m_data[0].m128_f32[0]; } 41 | inline const float &m01() const { return m_data[0].m128_f32[1]; } 42 | inline float &m01() { return m_data[0].m128_f32[1]; } 43 | inline const float &m02() const { return m_data[0].m128_f32[2]; } 44 | inline float &m02() { return m_data[0].m128_f32[2]; } 45 | inline const float &m03() const { return m_data[0].m128_f32[3]; } 46 | inline float &m03() { return m_data[0].m128_f32[3]; } 47 | inline const float &m04() const { return m_data[1].m128_f32[0]; } 48 | inline float &m04() { return m_data[1].m128_f32[0]; } 49 | inline const float &m05() const { return m_data[1].m128_f32[1]; } 50 | inline float &m05() { return m_data[1].m128_f32[1]; } 51 | inline const float &m06() const { return m_data[1].m128_f32[2]; } 52 | inline float &m06() { return m_data[1].m128_f32[2]; } 53 | inline const float &m07() const { return m_data[1].m128_f32[3]; } 54 | inline float &m07() { return m_data[1].m128_f32[3]; } 55 | inline const float &m10() const { return m_data[2].m128_f32[0]; } 56 | inline float &m10() { return m_data[2].m128_f32[0]; } 57 | inline const float &m11() const { return m_data[2].m128_f32[1]; } 58 | inline float &m11() { return m_data[2].m128_f32[1]; } 59 | inline const float &m12() const { return m_data[2].m128_f32[2]; } 60 | inline float &m12() { return m_data[2].m128_f32[2]; } 61 | inline const float &m13() const { return m_data[2].m128_f32[3]; } 62 | inline float &m13() { return m_data[2].m128_f32[3]; } 63 | inline const float &m14() const { return m_data[3].m128_f32[0]; } 64 | inline float &m14() { return m_data[3].m128_f32[0]; } 65 | inline const float &m15() const { return m_data[3].m128_f32[1]; } 66 | inline float &m15() { return m_data[3].m128_f32[1]; } 67 | inline const float &m16() const { return m_data[3].m128_f32[2]; } 68 | inline float &m16() { return m_data[3].m128_f32[2]; } 69 | inline const float &m17() const { return m_data[3].m128_f32[3]; } 70 | inline float &m17() { return m_data[3].m128_f32[3]; } 71 | inline operator const float *() const { return (const float *)this; } 72 | inline operator float *() { return (float *)this; } 73 | inline void operator+=(const AlignedMatrix2x8f &M) 74 | { 75 | m_00_01_02_03() = _pi_mm_add_ps(M.m_00_01_02_03(), m_00_01_02_03()); 76 | m_04_05_06_07() = _pi_mm_add_ps(M.m_04_05_06_07(), m_04_05_06_07()); 77 | m_10_11_12_13() = _pi_mm_add_ps(M.m_10_11_12_13(), m_10_11_12_13()); 78 | m_14_15_16_17() = _pi_mm_add_ps(M.m_14_15_16_17(), m_14_15_16_17()); 79 | } 80 | inline void operator-=(const AlignedMatrix2x8f &M) 81 | { 82 | m_00_01_02_03() = _pi_mm_sub_ps(m_00_01_02_03(), M.m_00_01_02_03()); 83 | m_04_05_06_07() = _pi_mm_sub_ps(m_04_05_06_07(), M.m_04_05_06_07()); 84 | m_10_11_12_13() = _pi_mm_sub_ps(m_10_11_12_13(), M.m_10_11_12_13()); 85 | m_14_15_16_17() = _pi_mm_sub_ps(m_14_15_16_17(), M.m_14_15_16_17()); 86 | } 87 | inline void operator*=(const _pi__m128 &s) 88 | { 89 | m_00_01_02_03() = _pi_mm_mul_ps(s, m_00_01_02_03()); 90 | m_04_05_06_07() = _pi_mm_mul_ps(s, m_04_05_06_07()); 91 | m_10_11_12_13() = _pi_mm_mul_ps(s, m_10_11_12_13()); 92 | m_14_15_16_17() = _pi_mm_mul_ps(s, m_14_15_16_17()); 93 | } 94 | 95 | inline void MakeZero() { memset(this, 0, sizeof(AlignedMatrix2x8f)); } 96 | inline void Print(const bool e = false) const 97 | { 98 | if (e) { 99 | UT::Print("%e %e %e %e %e %e %e %e\n", m00(), m01(), m02(), m03(), 100 | m04(), m05(), m06(), m07()); 101 | UT::Print("%e %e %e %e %e %e %e %e\n", m10(), m11(), m12(), m13(), 102 | m14(), m15(), m16(), m17()); 103 | } else { 104 | UT::Print("%f %f %f %f %f %f %f %f\n", m00(), m01(), m02(), m03(), 105 | m04(), m05(), m06(), m07()); 106 | UT::Print("%f %f %f %f %f %f %f %f\n", m10(), m11(), m12(), m13(), 107 | m14(), m15(), m16(), m17()); 108 | } 109 | } 110 | inline void AssertZero() const 111 | { 112 | UT::AssertEqual( 113 | SSE::Sum012(_pi_mm_mul_ps(m_00_01_02_03(), m_00_01_02_03())), 0.0f); 114 | UT::AssertEqual( 115 | SSE::Sum012(_pi_mm_mul_ps(m_04_05_06_07(), m_04_05_06_07())), 0.0f); 116 | UT::AssertEqual( 117 | SSE::Sum012(_pi_mm_mul_ps(m_10_11_12_13(), m_10_11_12_13())), 0.0f); 118 | UT::AssertEqual( 119 | SSE::Sum012(_pi_mm_mul_ps(m_14_15_16_17(), m_14_15_16_17())), 0.0f); 120 | } 121 | inline bool AssertEqual(const AlignedMatrix2x8f &M, 122 | const int verbose = 1) const 123 | { 124 | if (UT::VectorAssertEqual(&m00(), &M.m00(), 16, 0)) return true; 125 | if (verbose) { 126 | UT::PrintSeparator(); 127 | Print(verbose > 1); 128 | UT::PrintSeparator(); 129 | M.Print(verbose > 1); 130 | } 131 | return false; 132 | } 133 | 134 | protected: 135 | _pi__m128 m_data[4]; 136 | }; 137 | } 138 | 139 | #ifdef CFG_DEBUG_EIGEN 140 | class EigenMatrix2x8f : public Eigen::Matrix 141 | { 142 | public: 143 | inline EigenMatrix2x8f() = default; 144 | inline EigenMatrix2x8f(const Eigen::Matrix &e_M) 145 | : Eigen::Matrix(e_M) 146 | { 147 | } 148 | inline EigenMatrix2x8f(const LA::AlignedMatrix2x8f &M) 149 | : Eigen::Matrix() 150 | { 151 | const float *_M[2] = {&M.m00(), &M.m10()}; 152 | Eigen::Matrix &e_M = *this; 153 | for (int i = 0; i < 2; ++i) 154 | for (int j = 0; j < 8; ++j) e_M(i, j) = _M[i][j]; 155 | } 156 | inline void operator=(const Eigen::Matrix &e_M) 157 | { 158 | *((Eigen::Matrix *)this) = e_M; 159 | } 160 | inline LA::AlignedMatrix2x8f GetAlignedMatrix2x8f() const 161 | { 162 | LA::AlignedMatrix2x8f M; 163 | float *_M[2] = {&M.m00(), &M.m10()}; 164 | const Eigen::Matrix &e_M = *this; 165 | for (int i = 0; i < 2; ++i) 166 | for (int j = 0; j < 8; ++j) _M[i][j] = e_M(i, j); 167 | return M; 168 | } 169 | inline void Print(const bool e = false) const 170 | { 171 | GetAlignedMatrix2x8f().Print(e); 172 | } 173 | inline bool AssertEqual(const LA::AlignedMatrix2x8f &M, 174 | const int verbose = 1) const 175 | { 176 | return GetAlignedMatrix2x8f().AssertEqual(M, verbose); 177 | } 178 | inline bool AssertEqual(const EigenMatrix2x8f &e_M, 179 | const int verbose = 1) const 180 | { 181 | return AssertEqual(e_M.GetAlignedMatrix2x8f(), verbose); 182 | } 183 | }; 184 | #endif 185 | #endif 186 | -------------------------------------------------------------------------------- /LinearAlgebra/Matrix3x4.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of EIBA. 3 | * 4 | * Copyright (C) 2017 Zhejiang University 5 | * For more information see 6 | * 7 | * Licensed under the Apache License, Version 2.0 (the "License"); 8 | * You may not use this file except in compliance with the License. 9 | * You may obtain a copy of the License at 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | * Unless required by applicable law or agreed to in writing, software 12 | * distributed under the License is distributed on an "AS IS" BASIS, 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | * 17 | */ 18 | 19 | #include "PlatformIndependence/sse.h" 20 | 21 | #ifndef _MATRIX_3x4_H_ 22 | #define _MATRIX_3x4_H_ 23 | 24 | #include "Matrix3x3.h" 25 | 26 | namespace LA 27 | { 28 | class AlignedMatrix3x4f 29 | { 30 | public: 31 | inline const _pi__m128 &m_00_01_02_03() const { return m_data[0]; } 32 | inline _pi__m128 &m_00_01_02_03() { return m_data[0]; } 33 | inline const _pi__m128 &m_10_11_12_13() const { return m_data[1]; } 34 | inline _pi__m128 &m_10_11_12_13() { return m_data[1]; } 35 | inline const _pi__m128 &m_20_21_22_23() const { return m_data[2]; } 36 | inline _pi__m128 &m_20_21_22_23() { return m_data[2]; } 37 | inline const float &m00() const { return m_data[0].m128_f32[0]; } 38 | inline float &m00() { return m_data[0].m128_f32[0]; } 39 | inline const float &m01() const { return m_data[0].m128_f32[1]; } 40 | inline float &m01() { return m_data[0].m128_f32[1]; } 41 | inline const float &m02() const { return m_data[0].m128_f32[2]; } 42 | inline float &m02() { return m_data[0].m128_f32[2]; } 43 | inline const float &m03() const { return m_data[0].m128_f32[3]; } 44 | inline float &m03() { return m_data[0].m128_f32[3]; } 45 | inline const float &m10() const { return m_data[1].m128_f32[0]; } 46 | inline float &m10() { return m_data[1].m128_f32[0]; } 47 | inline const float &m11() const { return m_data[1].m128_f32[1]; } 48 | inline float &m11() { return m_data[1].m128_f32[1]; } 49 | inline const float &m12() const { return m_data[1].m128_f32[2]; } 50 | inline float &m12() { return m_data[1].m128_f32[2]; } 51 | inline const float &m13() const { return m_data[1].m128_f32[3]; } 52 | inline float &m13() { return m_data[1].m128_f32[3]; } 53 | inline const float &m20() const { return m_data[2].m128_f32[0]; } 54 | inline float &m20() { return m_data[2].m128_f32[0]; } 55 | inline const float &m21() const { return m_data[2].m128_f32[1]; } 56 | inline float &m21() { return m_data[2].m128_f32[1]; } 57 | inline const float &m22() const { return m_data[2].m128_f32[2]; } 58 | inline float &m22() { return m_data[2].m128_f32[2]; } 59 | inline const float &m23() const { return m_data[2].m128_f32[3]; } 60 | inline float &m23() { return m_data[2].m128_f32[3]; } 61 | inline operator const float *() const { return (const float *)this; } 62 | inline operator float *() { return (float *)this; } 63 | inline void operator+=(const AlignedMatrix3x4f &M) 64 | { 65 | m_data[0] = _pi_mm_add_ps(M.m_data[0], m_data[0]); 66 | m_data[1] = _pi_mm_add_ps(M.m_data[1], m_data[1]); 67 | m_data[2] = _pi_mm_add_ps(M.m_data[2], m_data[2]); 68 | } 69 | inline void operator-=(const AlignedMatrix3x4f &M) 70 | { 71 | m_data[0] = _pi_mm_sub_ps(m_data[0], M.m_data[0]); 72 | m_data[1] = _pi_mm_sub_ps(m_data[1], M.m_data[1]); 73 | m_data[2] = _pi_mm_sub_ps(m_data[2], M.m_data[2]); 74 | } 75 | inline void operator*=(const _pi__m128 &s) 76 | { 77 | m_data[0] = _pi_mm_mul_ps(s, m_data[0]); 78 | m_data[1] = _pi_mm_mul_ps(s, m_data[1]); 79 | m_data[2] = _pi_mm_mul_ps(s, m_data[2]); 80 | } 81 | 82 | inline void Set(const float *M) 83 | { 84 | memcpy(&m00(), M, sizeof(AlignedMatrix3x4f)); 85 | } 86 | inline void Set(const AlignedMatrix3x3f &M0, const AlignedVector3f &M1) 87 | { 88 | memcpy(&m00(), &M0.m00(), 12); 89 | m03() = M1.v0(); 90 | memcpy(&m10(), &M0.m10(), 12); 91 | m13() = M1.v1(); 92 | memcpy(&m20(), &M0.m20(), 12); 93 | m23() = M1.v2(); 94 | } 95 | inline void Get(float *M) const 96 | { 97 | memcpy(M, &m00(), sizeof(AlignedMatrix3x4f)); 98 | } 99 | 100 | inline void MakeZero() { memset(this, 0, sizeof(AlignedMatrix3x4f)); } 101 | inline void Print(const bool e = false) const 102 | { 103 | if (e) { 104 | UT::Print("%e %e %e %e\n", m00(), m01(), m02(), m03()); 105 | UT::Print("%e %e %e %e\n", m10(), m11(), m12(), m13()); 106 | UT::Print("%e %e %e %e\n", m20(), m21(), m22(), m23()); 107 | } else { 108 | UT::Print("%f %f %f %f\n", m00(), m01(), m02(), m03()); 109 | UT::Print("%f %f %f %f\n", m10(), m11(), m12(), m13()); 110 | UT::Print("%f %f %f %f\n", m20(), m21(), m23(), m23()); 111 | } 112 | } 113 | inline void AssertZero() const 114 | { 115 | UT::AssertEqual( 116 | SSE::Sum012(_pi_mm_mul_ps(m_00_01_02_03(), m_00_01_02_03())), 0.0f); 117 | UT::AssertEqual( 118 | SSE::Sum012(_pi_mm_mul_ps(m_10_11_12_13(), m_10_11_12_13())), 0.0f); 119 | UT::AssertEqual( 120 | SSE::Sum012(_pi_mm_mul_ps(m_20_21_22_23(), m_20_21_22_23())), 0.0f); 121 | } 122 | inline bool AssertEqual(const AlignedMatrix3x4f &M, 123 | const int verbose = 1) const 124 | { 125 | if (UT::VectorAssertEqual(&m00(), &M.m00(), 12, 0)) return true; 126 | if (verbose) { 127 | UT::PrintSeparator(); 128 | Print(verbose > 1); 129 | UT::PrintSeparator(); 130 | M.Print(verbose > 1); 131 | } 132 | return false; 133 | } 134 | 135 | static inline void ATBToSymmetric(const AlignedMatrix3x3f &A, 136 | const AlignedMatrix3x4f &B, 137 | AlignedMatrix3x4f &ATB) 138 | { 139 | ATB.m_00_01_02_03() = _pi_mm_add_ps( 140 | _pi_mm_add_ps( 141 | _pi_mm_mul_ps(_pi_mm_set1_ps(A.m00()), B.m_00_01_02_03()), 142 | _pi_mm_mul_ps(_pi_mm_set1_ps(A.m10()), B.m_10_11_12_13())), 143 | _pi_mm_mul_ps(_pi_mm_set1_ps(A.m20()), B.m_20_21_22_23())); 144 | ATB.m_10_11_12_13() = _pi_mm_add_ps( 145 | _pi_mm_add_ps( 146 | _pi_mm_mul_ps(_pi_mm_set1_ps(A.m01()), B.m_00_01_02_03()), 147 | _pi_mm_mul_ps(_pi_mm_set1_ps(A.m11()), B.m_10_11_12_13())), 148 | _pi_mm_mul_ps(_pi_mm_set1_ps(A.m21()), B.m_20_21_22_23())); 149 | ATB.m22() = A.m02() * B.m02() + A.m12() * B.m12() + A.m22() * B.m22(); 150 | ATB.m23() = A.m02() * B.m03() + A.m12() * B.m13() + A.m22() * B.m23(); 151 | } 152 | 153 | protected: 154 | _pi__m128 m_data[3]; 155 | }; 156 | } 157 | 158 | #ifdef CFG_DEBUG_EIGEN 159 | class EigenMatrix3x4f : public Eigen::Matrix 160 | { 161 | public: 162 | inline EigenMatrix3x4f() = default; 163 | inline EigenMatrix3x4f(const Eigen::Matrix &e_M) 164 | : Eigen::Matrix(e_M) 165 | { 166 | } 167 | inline EigenMatrix3x4f(const LA::AlignedMatrix3x4f &M) 168 | : Eigen::Matrix() 169 | { 170 | Eigen::Matrix &e_M = *this; 171 | e_M(0, 0) = M.m00(); 172 | e_M(0, 1) = M.m01(); 173 | e_M(0, 2) = M.m02(); 174 | e_M(0, 3) = M.m03(); 175 | e_M(1, 0) = M.m10(); 176 | e_M(1, 1) = M.m11(); 177 | e_M(0, 2) = M.m02(); 178 | e_M(0, 3) = M.m03(); 179 | e_M(2, 0) = M.m20(); 180 | e_M(2, 1) = M.m21(); 181 | e_M(2, 2) = M.m22(); 182 | e_M(2, 3) = M.m23(); 183 | } 184 | inline EigenMatrix3x4f(const EigenMatrix3x3f &e_M0, 185 | const EigenVector3f &e_M1) 186 | { 187 | block<3, 3>(0, 0) = e_M0; 188 | block<3, 1>(0, 3) = e_M1; 189 | } 190 | inline void operator=(const Eigen::Matrix &e_M) 191 | { 192 | *((Eigen::Matrix *)this) = e_M; 193 | } 194 | inline LA::AlignedMatrix3x4f GetAlignedMatrix3x4f() const 195 | { 196 | LA::AlignedMatrix3x4f M; 197 | const Eigen::Matrix &e_M = *this; 198 | M.m00() = e_M(0, 0); 199 | M.m01() = e_M(0, 1); 200 | M.m02() = e_M(0, 2); 201 | M.m03() = e_M(0, 3); 202 | M.m10() = e_M(1, 0); 203 | M.m11() = e_M(1, 1); 204 | M.m12() = e_M(1, 2); 205 | M.m13() = e_M(1, 3); 206 | M.m20() = e_M(2, 0); 207 | M.m21() = e_M(2, 1); 208 | M.m22() = e_M(2, 2); 209 | M.m23() = e_M(2, 3); 210 | return M; 211 | } 212 | inline void Print(const bool e = false) const 213 | { 214 | GetAlignedMatrix3x4f().Print(e); 215 | } 216 | inline bool AssertEqual(const LA::AlignedMatrix3x4f &M, 217 | const int verbose = 1) const 218 | { 219 | return GetAlignedMatrix3x4f().AssertEqual(M, verbose); 220 | } 221 | inline bool AssertEqual(const EigenMatrix3x4f &e_M, 222 | const int verbose = 1) const 223 | { 224 | return AssertEqual(e_M.GetAlignedMatrix3x4f(), verbose); 225 | } 226 | }; 227 | #endif 228 | #endif 229 | -------------------------------------------------------------------------------- /LinearAlgebra/Matrix3x6.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of EIBA. 3 | * 4 | * Copyright (C) 2017 Zhejiang University 5 | * For more information see 6 | * 7 | * Licensed under the Apache License, Version 2.0 (the "License"); 8 | * You may not use this file except in compliance with the License. 9 | * You may obtain a copy of the License at 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | * Unless required by applicable law or agreed to in writing, software 12 | * distributed under the License is distributed on an "AS IS" BASIS, 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | * 17 | */ 18 | 19 | #include "PlatformIndependence/sse.h" 20 | 21 | #ifndef _MATRIX_3x6_H_ 22 | #define _MATRIX_3x6_H_ 23 | 24 | #include "Matrix2x3.h" 25 | #include "Matrix3x3.h" 26 | 27 | namespace LA 28 | { 29 | class AlignedMatrix3x6f 30 | { 31 | public: 32 | inline const _pi__m128 &m_00_01_02_03() const { return m_data[0]; } 33 | inline _pi__m128 &m_00_01_02_03() { return m_data[0]; } 34 | inline const _pi__m128 &m_04_05_x_x() const { return m_data[1]; } 35 | inline _pi__m128 &m_04_05_x_x() { return m_data[1]; } 36 | inline const _pi__m128 &m_10_11_12_13() const { return m_data[2]; } 37 | inline _pi__m128 &m_10_11_12_13() { return m_data[2]; } 38 | inline const _pi__m128 &m_14_15_x_x() const { return m_data[3]; } 39 | inline _pi__m128 &m_14_15_x_x() { return m_data[3]; } 40 | inline const _pi__m128 &m_20_21_22_23() const { return m_data[4]; } 41 | inline _pi__m128 &m_20_21_22_23() { return m_data[4]; } 42 | inline const _pi__m128 &m_24_25_x_x() const { return m_data[5]; } 43 | inline _pi__m128 &m_24_25_x_x() { return m_data[5]; } 44 | inline const float &m00() const { return m_data[0].m128_f32[0]; } 45 | inline float &m00() { return m_data[0].m128_f32[0]; } 46 | inline const float &m01() const { return m_data[0].m128_f32[1]; } 47 | inline float &m01() { return m_data[0].m128_f32[1]; } 48 | inline const float &m02() const { return m_data[0].m128_f32[2]; } 49 | inline float &m02() { return m_data[0].m128_f32[2]; } 50 | inline const float &m03() const { return m_data[0].m128_f32[3]; } 51 | inline float &m03() { return m_data[0].m128_f32[3]; } 52 | inline const float &m04() const { return m_data[1].m128_f32[0]; } 53 | inline float &m04() { return m_data[1].m128_f32[0]; } 54 | inline const float &m05() const { return m_data[1].m128_f32[1]; } 55 | inline float &m05() { return m_data[1].m128_f32[1]; } 56 | inline const float &m10() const { return m_data[2].m128_f32[0]; } 57 | inline float &m10() { return m_data[2].m128_f32[0]; } 58 | inline const float &m11() const { return m_data[2].m128_f32[1]; } 59 | inline float &m11() { return m_data[2].m128_f32[1]; } 60 | inline const float &m12() const { return m_data[2].m128_f32[2]; } 61 | inline float &m12() { return m_data[2].m128_f32[2]; } 62 | inline const float &m13() const { return m_data[2].m128_f32[3]; } 63 | inline float &m13() { return m_data[2].m128_f32[3]; } 64 | inline const float &m14() const { return m_data[3].m128_f32[0]; } 65 | inline float &m14() { return m_data[3].m128_f32[0]; } 66 | inline const float &m15() const { return m_data[3].m128_f32[1]; } 67 | inline float &m15() { return m_data[3].m128_f32[1]; } 68 | inline const float &m20() const { return m_data[4].m128_f32[0]; } 69 | inline float &m20() { return m_data[4].m128_f32[0]; } 70 | inline const float &m21() const { return m_data[4].m128_f32[1]; } 71 | inline float &m21() { return m_data[4].m128_f32[1]; } 72 | inline const float &m22() const { return m_data[4].m128_f32[2]; } 73 | inline float &m22() { return m_data[4].m128_f32[2]; } 74 | inline const float &m23() const { return m_data[4].m128_f32[3]; } 75 | inline float &m23() { return m_data[4].m128_f32[3]; } 76 | inline const float &m24() const { return m_data[5].m128_f32[0]; } 77 | inline float &m24() { return m_data[5].m128_f32[0]; } 78 | inline const float &m25() const { return m_data[5].m128_f32[1]; } 79 | inline float &m25() { return m_data[5].m128_f32[1]; } 80 | inline operator const float *() const { return (const float *)this; } 81 | inline operator float *() { return (float *)this; } 82 | inline void MakeZero() { memset(this, 0, sizeof(AlignedMatrix3x6f)); } 83 | inline void Print(const bool e = false) const 84 | { 85 | if (e) { 86 | UT::Print("%e %e %e %e %e %e\n", m00(), m01(), m02(), m03(), m04(), 87 | m05()); 88 | UT::Print("%e %e %e %e %e %e\n", m10(), m11(), m12(), m13(), m14(), 89 | m15()); 90 | UT::Print("%e %e %e %e %e %e\n", m20(), m21(), m22(), m23(), m24(), 91 | m25()); 92 | } else { 93 | UT::Print("%f %f %f %f %f %f\n", m00(), m01(), m02(), m03(), m04(), 94 | m05()); 95 | UT::Print("%f %f %f %f %f %f\n", m10(), m11(), m12(), m13(), m14(), 96 | m15()); 97 | UT::Print("%f %f %f %f %f %f\n", m20(), m21(), m22(), m23(), m24(), 98 | m25()); 99 | } 100 | } 101 | inline bool AssertEqual(const AlignedMatrix3x6f &M, 102 | const int verbose = 1) const 103 | { 104 | if (UT::VectorAssertEqual(&m00(), &M.m00(), 6, 0) && 105 | UT::VectorAssertEqual(&m10(), &M.m10(), 6, 0) && 106 | UT::VectorAssertEqual(&m20(), &M.m20(), 6, 0)) 107 | return true; 108 | if (verbose) { 109 | UT::PrintSeparator(); 110 | Print(verbose > 1); 111 | UT::PrintSeparator(); 112 | M.Print(verbose > 1); 113 | } 114 | return false; 115 | } 116 | 117 | static inline void AB(const SymmetricMatrix3x3f &A, 118 | const AlignedMatrix3x6f &B, AlignedMatrix3x6f &AB) 119 | { 120 | const _pi__m128 a01 = _pi_mm_set1_ps(A.m01()), 121 | a02 = _pi_mm_set1_ps(A.m02()), 122 | a12 = _pi_mm_set1_ps(A.m12()); 123 | AB.m_00_01_02_03() = _pi_mm_add_ps( 124 | _pi_mm_mul_ps(_pi_mm_set1_ps(A.m00()), B.m_00_01_02_03()), 125 | _pi_mm_add_ps(_pi_mm_mul_ps(a01, B.m_10_11_12_13()), 126 | _pi_mm_mul_ps(a02, B.m_20_21_22_23()))); 127 | AB.m04() = A.m00() * B.m04() + A.m01() * B.m14() + A.m02() * B.m24(); 128 | AB.m05() = A.m00() * B.m05() + A.m01() * B.m15() + A.m02() * B.m25(); 129 | AB.m_10_11_12_13() = 130 | _pi_mm_add_ps(_pi_mm_mul_ps(a01, B.m_00_01_02_03()), 131 | _pi_mm_add_ps(_pi_mm_mul_ps(_pi_mm_set1_ps(A.m11()), 132 | B.m_10_11_12_13()), 133 | _pi_mm_mul_ps(a12, B.m_20_21_22_23()))); 134 | AB.m14() = A.m01() * B.m04() + A.m11() * B.m14() + A.m12() * B.m24(); 135 | AB.m15() = A.m01() * B.m05() + A.m11() * B.m15() + A.m12() * B.m25(); 136 | AB.m_20_21_22_23() = 137 | _pi_mm_add_ps(_pi_mm_mul_ps(a02, B.m_00_01_02_03()), 138 | _pi_mm_add_ps(_pi_mm_mul_ps(a12, B.m_10_11_12_13()), 139 | _pi_mm_mul_ps(_pi_mm_set1_ps(A.m22()), 140 | B.m_20_21_22_23()))); 141 | AB.m24() = A.m02() * B.m04() + A.m12() * B.m14() + A.m22() * B.m24(); 142 | AB.m25() = A.m02() * B.m05() + A.m12() * B.m15() + A.m22() * B.m25(); 143 | } 144 | static inline void AB(const SymmetricMatrix2x2f &A0, const float A1, 145 | const AlignedMatrix3x6f &B, AlignedMatrix3x6f &AB) 146 | { 147 | const _pi__m128 a01 = _pi_mm_set1_ps(A0.m01()); 148 | AB.m_00_01_02_03() = _pi_mm_add_ps( 149 | _pi_mm_mul_ps(_pi_mm_set1_ps(A0.m00()), B.m_00_01_02_03()), 150 | _pi_mm_mul_ps(a01, B.m_10_11_12_13())); 151 | AB.m04() = A0.m00() * B.m04() + A0.m01() * B.m14(); 152 | AB.m05() = A0.m00() * B.m05() + A0.m01() * B.m15(); 153 | AB.m_10_11_12_13() = _pi_mm_add_ps( 154 | _pi_mm_mul_ps(a01, B.m_00_01_02_03()), 155 | _pi_mm_mul_ps(_pi_mm_set1_ps(A0.m11()), B.m_10_11_12_13())); 156 | AB.m14() = A0.m01() * B.m04() + A0.m11() * B.m14(); 157 | AB.m15() = A0.m01() * B.m05() + A0.m11() * B.m15(); 158 | AB.m_20_21_22_23() = 159 | _pi_mm_mul_ps(_pi_mm_set1_ps(A1), B.m_20_21_22_23()); 160 | AB.m24() = A1 * B.m24(); 161 | AB.m25() = A1 * B.m25(); 162 | } 163 | 164 | protected: 165 | _pi__m128 m_data[6]; 166 | }; 167 | } 168 | 169 | #ifdef CFG_DEBUG_EIGEN 170 | #include 171 | class EigenMatrix3x6f : public Eigen::Matrix 172 | { 173 | public: 174 | inline EigenMatrix3x6f() = default; 175 | inline EigenMatrix3x6f(const Eigen::Matrix &e_M) 176 | : Eigen::Matrix(e_M) 177 | { 178 | } 179 | inline EigenMatrix3x6f(const LA::AlignedMatrix3x6f &M) 180 | : Eigen::Matrix() 181 | { 182 | const float *_M[3] = {&M.m00(), &M.m10(), &M.m20()}; 183 | Eigen::Matrix &e_M = *this; 184 | for (int i = 0; i < 3; ++i) 185 | for (int j = 0; j < 6; ++j) e_M(i, j) = _M[i][j]; 186 | } 187 | inline EigenMatrix3x6f(const EigenMatrix3x3f &e_M0, 188 | const EigenMatrix3x3f &e_M1) 189 | { 190 | block<3, 3>(0, 0) = e_M0; 191 | block<3, 3>(0, 3) = e_M1; 192 | } 193 | inline EigenMatrix3x6f(const EigenMatrix2x3f &e_M00, 194 | const EigenMatrix2x3f &e_M01, 195 | const Eigen::Matrix &e_M10, 196 | const Eigen::Matrix &e_M11) 197 | { 198 | block<2, 3>(0, 0) = e_M00; 199 | block<2, 3>(0, 3) = e_M01; 200 | block<1, 3>(2, 0) = e_M10; 201 | block<1, 3>(2, 3) = e_M11; 202 | } 203 | inline void operator=(const Eigen::Matrix &e_M) 204 | { 205 | *((Eigen::Matrix *)this) = e_M; 206 | } 207 | inline LA::AlignedMatrix3x6f GetAlignedMatrix3x6f() const 208 | { 209 | LA::AlignedMatrix3x6f M; 210 | float *_M[3] = {&M.m00(), &M.m10(), &M.m20()}; 211 | const Eigen::Matrix &e_M = *this; 212 | for (int i = 0; i < 3; ++i) 213 | for (int j = 0; j < 6; ++j) _M[i][j] = e_M(i, j); 214 | return M; 215 | } 216 | inline void Print(const bool e = false) const 217 | { 218 | GetAlignedMatrix3x6f().Print(e); 219 | } 220 | inline bool AssertEqual(const LA::AlignedMatrix3x6f &M, 221 | const int verbose = 1) const 222 | { 223 | return GetAlignedMatrix3x6f().AssertEqual(M, verbose); 224 | } 225 | inline bool AssertEqual(const EigenMatrix3x6f &e_M, 226 | const int verbose = 1) const 227 | { 228 | return AssertEqual(e_M.GetAlignedMatrix3x6f(), verbose); 229 | } 230 | }; 231 | #endif 232 | #endif 233 | -------------------------------------------------------------------------------- /LinearAlgebra/Matrix3x7.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of EIBA. 3 | * 4 | * Copyright (C) 2017 Zhejiang University 5 | * For more information see 6 | * 7 | * Licensed under the Apache License, Version 2.0 (the "License"); 8 | * You may not use this file except in compliance with the License. 9 | * You may obtain a copy of the License at 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | * Unless required by applicable law or agreed to in writing, software 12 | * distributed under the License is distributed on an "AS IS" BASIS, 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | * 17 | */ 18 | 19 | #include "PlatformIndependence/sse.h" 20 | 21 | #ifndef _MATRIX_3x7_H_ 22 | #define _MATRIX_3x7_H_ 23 | 24 | #include "Matrix3x6.h" 25 | 26 | namespace LA 27 | { 28 | class AlignedMatrix3x7f 29 | { 30 | public: 31 | inline const _pi__m128 &m_00_01_02_03() const { return m_data[0]; } 32 | inline _pi__m128 &m_00_01_02_03() { return m_data[0]; } 33 | inline const _pi__m128 &m_04_05_06_x() const { return m_data[1]; } 34 | inline _pi__m128 &m_04_05_06_x() { return m_data[1]; } 35 | inline const _pi__m128 &m_10_11_12_13() const { return m_data[2]; } 36 | inline _pi__m128 &m_10_11_12_13() { return m_data[2]; } 37 | inline const _pi__m128 &m_14_15_16_x() const { return m_data[3]; } 38 | inline _pi__m128 &m_14_15_16_x() { return m_data[3]; } 39 | inline const _pi__m128 &m_20_21_22_23() const { return m_data[4]; } 40 | inline _pi__m128 &m_20_21_22_23() { return m_data[4]; } 41 | inline const _pi__m128 &m_24_25_26_x() const { return m_data[5]; } 42 | inline _pi__m128 &m_24_25_26_x() { return m_data[5]; } 43 | inline const float &m00() const { return m_data[0].m128_f32[0]; } 44 | inline float &m00() { return m_data[0].m128_f32[0]; } 45 | inline const float &m01() const { return m_data[0].m128_f32[1]; } 46 | inline float &m01() { return m_data[0].m128_f32[1]; } 47 | inline const float &m02() const { return m_data[0].m128_f32[2]; } 48 | inline float &m02() { return m_data[0].m128_f32[2]; } 49 | inline const float &m03() const { return m_data[0].m128_f32[3]; } 50 | inline float &m03() { return m_data[0].m128_f32[3]; } 51 | inline const float &m04() const { return m_data[1].m128_f32[0]; } 52 | inline float &m04() { return m_data[1].m128_f32[0]; } 53 | inline const float &m05() const { return m_data[1].m128_f32[1]; } 54 | inline float &m05() { return m_data[1].m128_f32[1]; } 55 | inline const float &m06() const { return m_data[1].m128_f32[2]; } 56 | inline float &m06() { return m_data[1].m128_f32[2]; } 57 | inline const float &m10() const { return m_data[2].m128_f32[0]; } 58 | inline float &m10() { return m_data[2].m128_f32[0]; } 59 | inline const float &m11() const { return m_data[2].m128_f32[1]; } 60 | inline float &m11() { return m_data[2].m128_f32[1]; } 61 | inline const float &m12() const { return m_data[2].m128_f32[2]; } 62 | inline float &m12() { return m_data[2].m128_f32[2]; } 63 | inline const float &m13() const { return m_data[2].m128_f32[3]; } 64 | inline float &m13() { return m_data[2].m128_f32[3]; } 65 | inline const float &m14() const { return m_data[3].m128_f32[0]; } 66 | inline float &m14() { return m_data[3].m128_f32[0]; } 67 | inline const float &m15() const { return m_data[3].m128_f32[1]; } 68 | inline float &m15() { return m_data[3].m128_f32[1]; } 69 | inline const float &m16() const { return m_data[3].m128_f32[2]; } 70 | inline float &m16() { return m_data[3].m128_f32[2]; } 71 | inline const float &m20() const { return m_data[4].m128_f32[0]; } 72 | inline float &m20() { return m_data[4].m128_f32[0]; } 73 | inline const float &m21() const { return m_data[4].m128_f32[1]; } 74 | inline float &m21() { return m_data[4].m128_f32[1]; } 75 | inline const float &m22() const { return m_data[4].m128_f32[2]; } 76 | inline float &m22() { return m_data[4].m128_f32[2]; } 77 | inline const float &m23() const { return m_data[4].m128_f32[3]; } 78 | inline float &m23() { return m_data[4].m128_f32[3]; } 79 | inline const float &m24() const { return m_data[5].m128_f32[0]; } 80 | inline float &m24() { return m_data[5].m128_f32[0]; } 81 | inline const float &m25() const { return m_data[5].m128_f32[1]; } 82 | inline float &m25() { return m_data[5].m128_f32[1]; } 83 | inline const float &m26() const { return m_data[5].m128_f32[2]; } 84 | inline float &m26() { return m_data[5].m128_f32[2]; } 85 | inline operator const float *() const { return (const float *)this; } 86 | inline operator float *() { return (float *)this; } 87 | inline void Set(const AlignedMatrix3x3f &M0, const AlignedMatrix3x3f &M1, 88 | const AlignedVector3f &M2) 89 | { 90 | memcpy(&m00(), &M0.m00(), 12); 91 | memcpy(&m03(), &M1.m00(), 12); 92 | m06() = M2.v0(); 93 | memcpy(&m10(), &M0.m10(), 12); 94 | memcpy(&m13(), &M1.m10(), 12); 95 | m16() = M2.v1(); 96 | memcpy(&m20(), &M0.m20(), 12); 97 | memcpy(&m23(), &M1.m20(), 12); 98 | m26() = M2.v2(); 99 | } 100 | inline void Set(const AlignedMatrix3x6f &M0, const Vector2f &M10, 101 | const float M11) 102 | { 103 | memcpy(&m00(), &M0.m00(), 24); 104 | m06() = M10.v0(); 105 | memcpy(&m10(), &M0.m10(), 24); 106 | m16() = M10.v1(); 107 | memcpy(&m20(), &M0.m20(), 24); 108 | m26() = M11; 109 | } 110 | 111 | inline void MakeZero() { memset(this, 0, sizeof(AlignedMatrix3x7f)); } 112 | inline void Print(const bool e = false) const 113 | { 114 | if (e) { 115 | UT::Print("%e %e %e %e %e %e %e\n", m00(), m01(), m02(), m03(), 116 | m04(), m05(), m06()); 117 | UT::Print("%e %e %e %e %e %e %e\n", m10(), m11(), m12(), m13(), 118 | m14(), m15(), m16()); 119 | UT::Print("%e %e %e %e %e %e %e\n", m20(), m21(), m22(), m23(), 120 | m24(), m25(), m26()); 121 | } else { 122 | UT::Print("%f %f %f %f %f %f %f\n", m00(), m01(), m02(), m03(), 123 | m04(), m05(), m06()); 124 | UT::Print("%f %f %f %f %f %f %f\n", m10(), m11(), m12(), m13(), 125 | m14(), m15(), m16()); 126 | UT::Print("%f %f %f %f %f %f %f\n", m20(), m21(), m22(), m23(), 127 | m24(), m25(), m26()); 128 | } 129 | } 130 | inline bool AssertEqual(const AlignedMatrix3x7f &M, 131 | const int verbose = 1) const 132 | { 133 | if (UT::VectorAssertEqual(&m00(), &M.m00(), 7, 0) && 134 | UT::VectorAssertEqual(&m10(), &M.m10(), 7, 0) && 135 | UT::VectorAssertEqual(&m20(), &M.m20(), 7, 0)) 136 | return true; 137 | if (verbose) { 138 | UT::PrintSeparator(); 139 | Print(verbose > 1); 140 | UT::PrintSeparator(); 141 | M.Print(verbose > 1); 142 | } 143 | return false; 144 | } 145 | 146 | protected: 147 | _pi__m128 m_data[6]; 148 | }; 149 | } 150 | 151 | #ifdef CFG_DEBUG_EIGEN 152 | #include 153 | class EigenMatrix3x7f : public Eigen::Matrix 154 | { 155 | public: 156 | inline EigenMatrix3x7f() = default; 157 | inline EigenMatrix3x7f(const Eigen::Matrix &e_M) 158 | : Eigen::Matrix(e_M) 159 | { 160 | } 161 | inline EigenMatrix3x7f(const LA::AlignedMatrix3x7f &M) 162 | : Eigen::Matrix() 163 | { 164 | const float *_M[3] = {&M.m00(), &M.m10(), &M.m20()}; 165 | Eigen::Matrix &e_M = *this; 166 | for (int i = 0; i < 3; ++i) 167 | for (int j = 0; j < 7; ++j) e_M(i, j) = _M[i][j]; 168 | } 169 | inline EigenMatrix3x7f(const EigenMatrix3x6f &e_M0, 170 | const EigenVector3f &e_M1) 171 | { 172 | block<3, 6>(0, 0) = e_M0; 173 | block<3, 1>(0, 6) = e_M1; 174 | } 175 | inline void operator=(const Eigen::Matrix &e_M) 176 | { 177 | *((Eigen::Matrix *)this) = e_M; 178 | } 179 | inline LA::AlignedMatrix3x7f GetAlignedMatrix3x7f() const 180 | { 181 | LA::AlignedMatrix3x7f M; 182 | float *_M[3] = {&M.m00(), &M.m10(), &M.m20()}; 183 | const Eigen::Matrix &e_M = *this; 184 | for (int i = 0; i < 3; ++i) 185 | for (int j = 0; j < 7; ++j) _M[i][j] = e_M(i, j); 186 | return M; 187 | } 188 | inline void Print(const bool e = false) const 189 | { 190 | GetAlignedMatrix3x7f().Print(e); 191 | } 192 | inline bool AssertEqual(const LA::AlignedMatrix3x7f &M, 193 | const int verbose = 1) const 194 | { 195 | return GetAlignedMatrix3x7f().AssertEqual(M, verbose); 196 | } 197 | inline bool AssertEqual(const EigenMatrix3x7f &e_M, 198 | const int verbose = 1) const 199 | { 200 | return AssertEqual(e_M.GetAlignedMatrix3x7f(), verbose); 201 | } 202 | }; 203 | #endif 204 | #endif 205 | -------------------------------------------------------------------------------- /LinearAlgebra/Matrix3x8.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of EIBA. 3 | * 4 | * Copyright (C) 2017 Zhejiang University 5 | * For more information see 6 | * 7 | * Licensed under the Apache License, Version 2.0 (the "License"); 8 | * You may not use this file except in compliance with the License. 9 | * You may obtain a copy of the License at 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | * Unless required by applicable law or agreed to in writing, software 12 | * distributed under the License is distributed on an "AS IS" BASIS, 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | * 17 | */ 18 | 19 | #include "PlatformIndependence/sse.h" 20 | 21 | #ifndef _MATRIX_3x8_H_ 22 | #define _MATRIX_3x8_H_ 23 | 24 | #include "Matrix3x7.h" 25 | 26 | namespace LA 27 | { 28 | class AlignedMatrix3x8f 29 | { 30 | public: 31 | inline const _pi__m128 &m_00_01_02_03() const { return m_data[0]; } 32 | inline _pi__m128 &m_00_01_02_03() { return m_data[0]; } 33 | inline const _pi__m128 &m_04_05_06_07() const { return m_data[1]; } 34 | inline _pi__m128 &m_04_05_06_07() { return m_data[1]; } 35 | inline const _pi__m128 &m_10_11_12_13() const { return m_data[2]; } 36 | inline _pi__m128 &m_10_11_12_13() { return m_data[2]; } 37 | inline const _pi__m128 &m_14_15_16_17() const { return m_data[3]; } 38 | inline _pi__m128 &m_14_15_16_17() { return m_data[3]; } 39 | inline const _pi__m128 &m_20_21_22_23() const { return m_data[4]; } 40 | inline _pi__m128 &m_20_21_22_23() { return m_data[4]; } 41 | inline const _pi__m128 &m_24_25_26_27() const { return m_data[5]; } 42 | inline _pi__m128 &m_24_25_26_27() { return m_data[5]; } 43 | inline const float &m00() const { return m_data[0].m128_f32[0]; } 44 | inline float &m00() { return m_data[0].m128_f32[0]; } 45 | inline const float &m01() const { return m_data[0].m128_f32[1]; } 46 | inline float &m01() { return m_data[0].m128_f32[1]; } 47 | inline const float &m02() const { return m_data[0].m128_f32[2]; } 48 | inline float &m02() { return m_data[0].m128_f32[2]; } 49 | inline const float &m03() const { return m_data[0].m128_f32[3]; } 50 | inline float &m03() { return m_data[0].m128_f32[3]; } 51 | inline const float &m04() const { return m_data[1].m128_f32[0]; } 52 | inline float &m04() { return m_data[1].m128_f32[0]; } 53 | inline const float &m05() const { return m_data[1].m128_f32[1]; } 54 | inline float &m05() { return m_data[1].m128_f32[1]; } 55 | inline const float &m06() const { return m_data[1].m128_f32[2]; } 56 | inline float &m06() { return m_data[1].m128_f32[2]; } 57 | inline const float &m07() const { return m_data[1].m128_f32[3]; } 58 | inline float &m07() { return m_data[1].m128_f32[3]; } 59 | inline const float &m10() const { return m_data[2].m128_f32[0]; } 60 | inline float &m10() { return m_data[2].m128_f32[0]; } 61 | inline const float &m11() const { return m_data[2].m128_f32[1]; } 62 | inline float &m11() { return m_data[2].m128_f32[1]; } 63 | inline const float &m12() const { return m_data[2].m128_f32[2]; } 64 | inline float &m12() { return m_data[2].m128_f32[2]; } 65 | inline const float &m13() const { return m_data[2].m128_f32[3]; } 66 | inline float &m13() { return m_data[2].m128_f32[3]; } 67 | inline const float &m14() const { return m_data[3].m128_f32[0]; } 68 | inline float &m14() { return m_data[3].m128_f32[0]; } 69 | inline const float &m15() const { return m_data[3].m128_f32[1]; } 70 | inline float &m15() { return m_data[3].m128_f32[1]; } 71 | inline const float &m16() const { return m_data[3].m128_f32[2]; } 72 | inline float &m16() { return m_data[3].m128_f32[2]; } 73 | inline const float &m17() const { return m_data[3].m128_f32[3]; } 74 | inline float &m17() { return m_data[3].m128_f32[3]; } 75 | inline const float &m20() const { return m_data[4].m128_f32[0]; } 76 | inline float &m20() { return m_data[4].m128_f32[0]; } 77 | inline const float &m21() const { return m_data[4].m128_f32[1]; } 78 | inline float &m21() { return m_data[4].m128_f32[1]; } 79 | inline const float &m22() const { return m_data[4].m128_f32[2]; } 80 | inline float &m22() { return m_data[4].m128_f32[2]; } 81 | inline const float &m23() const { return m_data[4].m128_f32[3]; } 82 | inline float &m23() { return m_data[4].m128_f32[3]; } 83 | inline const float &m24() const { return m_data[5].m128_f32[0]; } 84 | inline float &m24() { return m_data[5].m128_f32[0]; } 85 | inline const float &m25() const { return m_data[5].m128_f32[1]; } 86 | inline float &m25() { return m_data[5].m128_f32[1]; } 87 | inline const float &m26() const { return m_data[5].m128_f32[2]; } 88 | inline float &m26() { return m_data[5].m128_f32[2]; } 89 | inline const float &m27() const { return m_data[5].m128_f32[3]; } 90 | inline float &m27() { return m_data[5].m128_f32[3]; } 91 | inline operator const float *() const { return (const float *)this; } 92 | inline operator float *() { return (float *)this; } 93 | #if 0 94 | inline void Set(const AlignedMatrix3x3f &M0, const AlignedMatrix3x3f &M1, const AlignedVector3f &M2) 95 | { 96 | memcpy(&m00(), &M0.m00(), 12); memcpy(&m03(), &M1.m00(), 12); m06() = M2.v0(); 97 | memcpy(&m10(), &M0.m10(), 12); memcpy(&m13(), &M1.m10(), 12); m16() = M2.v1(); 98 | memcpy(&m20(), &M0.m20(), 12); memcpy(&m23(), &M1.m20(), 12); m26() = M2.v2(); 99 | } 100 | inline void Set(const AlignedMatrix3x6f &M0, const Vector2f &M10, const float M11) 101 | { 102 | memcpy(&m00(), &M0.m00(), 24); m06() = M10.v0(); 103 | memcpy(&m10(), &M0.m10(), 24); m16() = M10.v1(); 104 | memcpy(&m20(), &M0.m20(), 24); m26() = M11; 105 | } 106 | #endif 107 | 108 | inline void MakeZero() { memset(this, 0, sizeof(AlignedMatrix3x8f)); } 109 | inline void Print(const bool e = false) const 110 | { 111 | if (e) { 112 | UT::Print("%e %e %e %e %e %e %e %e\n", m00(), m01(), m02(), m03(), 113 | m04(), m05(), m06(), m07()); 114 | UT::Print("%e %e %e %e %e %e %e %e\n", m10(), m11(), m12(), m13(), 115 | m14(), m15(), m16(), m17()); 116 | UT::Print("%e %e %e %e %e %e %e %e\n", m20(), m21(), m22(), m23(), 117 | m24(), m25(), m26(), m27()); 118 | } else { 119 | UT::Print("%f %f %f %f %f %f %f %f\n", m00(), m01(), m02(), m03(), 120 | m04(), m05(), m06(), m07()); 121 | UT::Print("%f %f %f %f %f %f %f %f\n", m10(), m11(), m12(), m13(), 122 | m14(), m15(), m16(), m17()); 123 | UT::Print("%f %f %f %f %f %f %f %f\n", m20(), m21(), m22(), m23(), 124 | m24(), m25(), m26(), m27()); 125 | } 126 | } 127 | inline bool AssertEqual(const AlignedMatrix3x8f &M, 128 | const int verbose = 1) const 129 | { 130 | if (UT::VectorAssertEqual(&m00(), &M.m00(), 24, 0)) return true; 131 | if (verbose) { 132 | UT::PrintSeparator(); 133 | Print(verbose > 1); 134 | UT::PrintSeparator(); 135 | M.Print(verbose > 1); 136 | } 137 | return false; 138 | } 139 | 140 | protected: 141 | _pi__m128 m_data[6]; 142 | }; 143 | } 144 | 145 | #ifdef CFG_DEBUG_EIGEN 146 | #include 147 | class EigenMatrix3x8f : public Eigen::Matrix 148 | { 149 | public: 150 | inline EigenMatrix3x8f() = default; 151 | inline EigenMatrix3x8f(const Eigen::Matrix &e_M) 152 | : Eigen::Matrix(e_M) 153 | { 154 | } 155 | inline EigenMatrix3x8f(const LA::AlignedMatrix3x8f &M) 156 | : Eigen::Matrix() 157 | { 158 | const float *_M[3] = {&M.m00(), &M.m10(), &M.m20()}; 159 | Eigen::Matrix &e_M = *this; 160 | for (int i = 0; i < 3; ++i) 161 | for (int j = 0; j < 8; ++j) e_M(i, j) = _M[i][j]; 162 | } 163 | inline EigenMatrix3x8f(const EigenMatrix3x7f &e_M0, 164 | const EigenVector3f &e_M1) 165 | { 166 | block<3, 7>(0, 0) = e_M0; 167 | block<3, 1>(0, 7) = e_M1; 168 | } 169 | inline void operator=(const Eigen::Matrix &e_M) 170 | { 171 | *((Eigen::Matrix *)this) = e_M; 172 | } 173 | inline LA::AlignedMatrix3x8f GetAlignedMatrix3x8f() const 174 | { 175 | LA::AlignedMatrix3x8f M; 176 | float *_M[3] = {&M.m00(), &M.m10(), &M.m20()}; 177 | const Eigen::Matrix &e_M = *this; 178 | for (int i = 0; i < 3; ++i) 179 | for (int j = 0; j < 8; ++j) _M[i][j] = e_M(i, j); 180 | return M; 181 | } 182 | inline void Print(const bool e = false) const 183 | { 184 | GetAlignedMatrix3x8f().Print(e); 185 | } 186 | inline bool AssertEqual(const LA::AlignedMatrix3x8f &M, 187 | const int verbose = 1) const 188 | { 189 | return GetAlignedMatrix3x8f().AssertEqual(M, verbose); 190 | } 191 | inline bool AssertEqual(const EigenMatrix3x8f &e_M, 192 | const int verbose = 1) const 193 | { 194 | return AssertEqual(e_M.GetAlignedMatrix3x8f(), verbose); 195 | } 196 | }; 197 | #endif 198 | #endif 199 | -------------------------------------------------------------------------------- /LinearAlgebra/MatrixNxN.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of EIBA. 3 | * 4 | * Copyright (C) 2017 Zhejiang University 5 | * For more information see 6 | * 7 | * Licensed under the Apache License, Version 2.0 (the "License"); 8 | * You may not use this file except in compliance with the License. 9 | * You may obtain a copy of the License at 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | * Unless required by applicable law or agreed to in writing, software 12 | * distributed under the License is distributed on an "AS IS" BASIS, 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | * 17 | */ 18 | 19 | #ifndef _MATRIX_NxN_H_ 20 | #define _MATRIX_NxN_H_ 21 | 22 | #include "LinearSystem.h" 23 | #include "Matrix4x4.h" 24 | #include "Matrix6x6.h" 25 | #include "Matrix8x8.h" 26 | #include "VectorN.h" 27 | 28 | namespace LA 29 | { 30 | template class MatrixNxNf 31 | { 32 | public: 33 | inline const float *operator[](const int i) const { return m_data[i]; } 34 | inline float *operator[](const int i) { return m_data[i]; } 35 | inline MatrixNxNf operator-(const MatrixNxNf &B) const 36 | { 37 | MatrixNxNf AmB; 38 | for (int i = 0; i < N; ++i) 39 | for (int j = 0; j < N; ++j) AmB[i][j] = m_data[i][j] - B[i][j]; 40 | return AmB; 41 | } 42 | 43 | template 44 | inline void SetBlockDiagonalUpper(const int i, const MATRIX &M); 45 | template <> 46 | inline void 47 | SetBlockDiagonalUpper(const int i, 48 | const SymmetricMatrix3x3f &M) 49 | { 50 | int k = i; 51 | memcpy(m_data[k] + k, &M.m00(), 12); 52 | ++k; 53 | memcpy(m_data[k] + k, &M.m11(), 8); 54 | ++k; 55 | m_data[k][k] = M.m22(); 56 | } 57 | template <> 58 | inline void 59 | SetBlockDiagonalUpper(const int i, 60 | const SymmetricMatrix8x8f &M) 61 | { 62 | int k = i; 63 | memcpy(m_data[k] + k, &M.m00(), 32); 64 | ++k; 65 | memcpy(m_data[k] + k, &M.m11(), 28); 66 | ++k; 67 | memcpy(m_data[k] + k, &M.m22(), 24); 68 | ++k; 69 | memcpy(m_data[k] + k, &M.m33(), 20); 70 | ++k; 71 | memcpy(m_data[k] + k, &M.m44(), 16); 72 | ++k; 73 | memcpy(m_data[k] + k, &M.m55(), 12); 74 | ++k; 75 | memcpy(m_data[k] + k, &M.m66(), 8); 76 | ++k; 77 | m_data[k][k] = M.m77(); 78 | } 79 | 80 | template 81 | inline void SetBlock(const int i, const int j, const MATRIX &M); 82 | template <> 83 | inline void SetBlock(const int i, const int j, 84 | const Matrix3x3f &M) 85 | { 86 | int k = i; 87 | memcpy(m_data[k++] + j, &M.m00(), 12); 88 | memcpy(m_data[k++] + j, &M.m10(), 12); 89 | memcpy(m_data[k] + j, &M.m20(), 12); 90 | } 91 | template <> 92 | inline void SetBlock(const int i, const int j, const Vector3f &v) 93 | { 94 | int k = i; 95 | m_data[k++][j] = v.v0(); 96 | m_data[k++][j] = v.v1(); 97 | m_data[k][j] = v.v2(); 98 | } 99 | template <> 100 | inline void SetBlock(const int i, const int j, 101 | const AlignedVector8f &v) 102 | { 103 | int k = i; 104 | m_data[k++][j] = v.v0(); 105 | m_data[k++][j] = v.v1(); 106 | m_data[k++][j] = v.v2(); 107 | m_data[k++][j] = v.v3(); 108 | m_data[k++][j] = v.v4(); 109 | m_data[k++][j] = v.v5(); 110 | m_data[k++][j] = v.v6(); 111 | m_data[k][j] = v.v7(); 112 | } 113 | 114 | template 115 | inline void GetBlock(const int i, const int j, MATRIX &M) const; 116 | template <> 117 | inline void GetBlock(const int i, const int j, 118 | AlignedMatrix3x3f &M) const 119 | { 120 | memcpy(&M.m00(), m_data[i] + j, 12); 121 | memcpy(&M.m10(), m_data[i + 1] + j, 12); 122 | memcpy(&M.m20(), m_data[i + 2] + j, 12); 123 | } 124 | template <> 125 | inline void GetBlock(const int i, const int j, 126 | AlignedMatrix4x4f &M) const 127 | { 128 | memcpy(&M.m00(), m_data[i] + j, 16); 129 | memcpy(&M.m10(), m_data[i + 1] + j, 16); 130 | memcpy(&M.m20(), m_data[i + 2] + j, 16); 131 | memcpy(&M.m30(), m_data[i + 3] + j, 16); 132 | } 133 | template<> inline void GetBlock(AlignedMatrix6x6f>(const int i, const int j, AlignedMatrix6x6f &M) const 134 | { 135 | memcpy(&M.m00(), m_data[i] + j, 24); 136 | memcpy(&M.m10(), m_data[i + 1] + j, 24); 137 | memcpy(&M.m20(), m_data[i + 2] + j, 24); 138 | memcpy(&M.m30(), m_data[i + 3] + j, 24); 139 | memcpy(&M.m40(), m_data[i + 4] + j, 24); 140 | memcpy(&M.m50(), m_data[i + 5] + j, 24); 141 | } 142 | 143 | inline void MakeZero() { 144 | memset(this, 0, sizeof(MatrixNxNf)); } 145 | 146 | inline void SetLowerFromUpper() 147 | { 148 | for (int i = 0; i < N; ++i) 149 | for (int j = i; j < N; ++j) m_data[j][i] = m_data[i][j]; 150 | } 151 | 152 | inline bool InverseLDL() { 153 | return InverseLDL(*this); } 154 | static inline bool InverseLDL(MatrixNxNf &A) 155 | { 156 | float *_A[N]; 157 | for (int i = 0; i < N; ++i) _A[i] = A[i]; 158 | return LS::InverseLDL(_A); 159 | } 160 | 161 | static inline bool SolveLDL(MatrixNxNf &A, VectorNf &b) 162 | { 163 | float *_A[N]; 164 | for (int i = 0; i < N; ++i) _A[i] = A[i]; 165 | return LS::SolveLDL(_A, b); 166 | } 167 | template static inline bool SolveLDL(MatrixNxNf &A, VectorNf &b) 168 | { 169 | #ifdef CFG_DEBUG 170 | UT_ASSERT(_N < N); 171 | #endif 172 | float *_A[_N]; 173 | for (int i = 0; i < _N; ++i) _A[i] = A[i]; 174 | if (!LS::SolveLDL(_A, b)) return false; 175 | for (int i = _N; i < N; ++i) b[i] = 0.0f; 176 | return true; 177 | } 178 | 179 | inline void Print(const bool e = false) const 180 | { 181 | for (int i = 0; i < N; ++i) { 182 | for (int j = 0; j < N; ++j) { 183 | if (e) 184 | UT::Print("%e ", m_data[i][j]); 185 | else 186 | UT::Print("%.4f ", m_data[i][j]); 187 | } 188 | UT::Print("\n"); 189 | } 190 | } 191 | 192 | inline bool AssertEqual(const MatrixNxNf &M, const int verbose = 1, const float eps = 0.0f) const 193 | { 194 | if (UT::VectorAssertEqual(&m_data[0][0], &M.m_data[0][0], N * N, 0, 195 | eps)) 196 | return true; 197 | if (verbose) { 198 | UT::PrintSeparator(); 199 | Print(verbose > 1); 200 | UT::PrintSeparator(); 201 | M.Print(verbose > 1); 202 | const MatrixNxNf E = *this - M; 203 | UT::PrintSeparator(); 204 | E.Print(verbose > 1); 205 | } 206 | return false; 207 | } 208 | 209 | protected: 210 | 211 | float m_data[N][N]; 212 | }; 213 | } 214 | 215 | #ifdef CFG_DEBUG_EIGEN 216 | template class EigenMatrixNxNf : public Eigen::Matrix 217 | { 218 | public: 219 | inline EigenMatrixNxNf() = default; 220 | inline EigenMatrixNxNf(const Eigen::Matrix &e_M) 221 | : Eigen::Matrix(e_M) 222 | { 223 | } 224 | inline EigenMatrixNxNf(const LA::MatrixNxNf &M) 225 | : Eigen::Matrix() 226 | { 227 | Eigen::Matrix &e_M = *this; 228 | for (int i = 0; i < N; ++i) 229 | for (int j = 0; j < N; ++j) e_M(i, j) = M[i][j]; 230 | } 231 | inline void operator=(const Eigen::Matrix &e_M) 232 | { 233 | *((Eigen::Matrix *)this) = e_M; 234 | } 235 | inline LA::MatrixNxNf GetMatrixNxNf() const 236 | { 237 | LA::MatrixNxNf M; 238 | const Eigen::Matrix &e_M = *this; 239 | for (int i = 0; i < N; ++i) 240 | for (int j = 0; j < N; ++j) M[i][j] = e_M(i, j); 241 | return M; 242 | } 243 | template 244 | static inline EigenVectorNf Solve(const EigenMatrixNxNf &A, 245 | const EigenVectorNf &b) 246 | { 247 | EigenVectorNf x; 248 | x.block<_N, 1>(0, 0) = 249 | A.block<_N, _N>(0, 0).inverse() * b.block<_N, 1>(0, 0); 250 | for (int i = _N; i < N; ++i) x(i, 0) = 0.0f; 251 | return x; 252 | } 253 | inline void Print(const bool e = false) const { GetMatrixNxNf().Print(e); } 254 | inline bool AssertEqual(const LA::MatrixNxNf &M, const int verbose = 1, 255 | const float eps = 0.0f) const 256 | { 257 | return GetMatrixNxNf().AssertEqual(M, verbose, eps); 258 | } 259 | inline bool AssertEqual(const EigenMatrixNxNf &e_M, const int verbose = 1, 260 | const float eps = 0.0f) const 261 | { 262 | return AssertEqual(e_M.GetMatrixNxNf(), verbose, eps); 263 | } 264 | }; 265 | #endif 266 | #endif 267 | -------------------------------------------------------------------------------- /LinearAlgebra/Vector2.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of EIBA. 3 | * 4 | * Copyright (C) 2017 Zhejiang University 5 | * For more information see 6 | * 7 | * Licensed under the Apache License, Version 2.0 (the "License"); 8 | * You may not use this file except in compliance with the License. 9 | * You may obtain a copy of the License at 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | * Unless required by applicable law or agreed to in writing, software 12 | * distributed under the License is distributed on an "AS IS" BASIS, 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | * 17 | */ 18 | 19 | #ifndef _VECTOR_2_H_ 20 | #define _VECTOR_2_H_ 21 | 22 | #include "Utility.h" 23 | 24 | namespace LA 25 | { 26 | template class Vector2 27 | { 28 | public: 29 | inline Vector2() = default; 30 | inline Vector2(const TYPE v0, const TYPE v1) { Set(v0, v1); } 31 | inline const TYPE &v0() const { return m_data[0]; } 32 | inline TYPE &v0() { return m_data[0]; } 33 | inline const TYPE &v1() const { return m_data[1]; } 34 | inline TYPE &v1() { return m_data[1]; } 35 | inline const TYPE &x() const { return m_data[0]; } 36 | inline TYPE &x() { return m_data[0]; } 37 | inline const TYPE &y() const { return m_data[1]; } 38 | inline TYPE &y() { return m_data[1]; } 39 | inline operator const TYPE *() const { return (const TYPE *)this; } 40 | inline operator TYPE *() { return (TYPE *)this; } 41 | inline bool operator==(const Vector2 &v) const 42 | { 43 | return v0() == v.v0() && v1() == v.v1(); 44 | } 45 | inline void operator+=(const Vector2 &v) 46 | { 47 | v0() = v.v0() + v0(); 48 | v1() = v.v1() + v1(); 49 | } 50 | inline void operator-=(const Vector2 &v) 51 | { 52 | v0() = -v.v0() + v0(); 53 | v1() = -v.v1() + v1(); 54 | } 55 | inline void operator*=(const TYPE s) 56 | { 57 | v0() *= s; 58 | v1() *= s; 59 | } 60 | inline void operator*=(const Vector2 &s) 61 | { 62 | v0() *= s.v0(); 63 | v1() *= s.v1(); 64 | } 65 | inline Vector2 operator+(const Vector2 &v) const 66 | { 67 | return Vector2(v0() + v.v0(), v1() + v.v1()); 68 | } 69 | inline Vector2 operator-(const Vector2 &v) const 70 | { 71 | return Vector2(v0() - v.v0(), v1() - v.v1()); 72 | } 73 | inline Vector2 operator*(const TYPE s) const 74 | { 75 | return Vector2(v0() * s, v1() * s); 76 | } 77 | 78 | inline void Set(const TYPE v0, const TYPE v1) 79 | { 80 | this->v0() = v0; 81 | this->v1() = v1; 82 | } 83 | 84 | inline void MakeZero() { memset(this, 0, sizeof(Vector2)); } 85 | inline bool Valid() const { return v0() != UT::Invalid(); } 86 | inline bool Invalid() const { return v0() == UT::Invalid(); } 87 | inline void Invalidate() { v0() = UT::Invalid(); } 88 | inline TYPE SquaredLength() const { return v0() * v0() + v1() * v1(); } 89 | inline TYPE Dot(const Vector2 &v) const 90 | { 91 | return v0() * v.v0() + v1() * v.v1(); 92 | } 93 | inline void Print(const bool e = false) const 94 | { 95 | UT::PrintValue(v0(), e); 96 | UT::Print(" "); 97 | UT::PrintValue(v1(), e); 98 | UT::Print("\n"); 99 | } 100 | inline void Print(const std::string str, const bool e) const 101 | { 102 | UT::Print("%s", str.c_str()); 103 | Print(e); 104 | } 105 | inline bool AssertEqual(const Vector2 &v, const int verbose = 1, 106 | const TYPE eps = 0.0f) const 107 | { 108 | if (UT::VectorAssertEqual(&v0(), &v.v0(), 2, 0, eps)) return true; 109 | if (verbose) { 110 | UT::PrintSeparator(); 111 | Print(verbose > 1); 112 | v.Print(verbose > 1); 113 | } 114 | return false; 115 | } 116 | 117 | protected: 118 | TYPE m_data[2]; 119 | }; 120 | 121 | typedef Vector2 Vector2s; 122 | typedef Vector2 Vector2us; 123 | typedef Vector2 Vector2i; 124 | typedef Vector2 Vector2f; 125 | typedef Vector2 Vector2d; 126 | } 127 | 128 | #ifdef CFG_DEBUG_EIGEN 129 | #include 130 | class EigenVector2f : public Eigen::Vector2f 131 | { 132 | public: 133 | inline EigenVector2f() = default; 134 | inline EigenVector2f(const Eigen::Vector2f &e_v) : Eigen::Vector2f(e_v) {} 135 | inline EigenVector2f(const LA::Vector2f &v) 136 | : Eigen::Vector2f(v.v0(), v.v1()) 137 | { 138 | } 139 | inline EigenVector2f(const float v0, const float v1) 140 | : Eigen::Vector2f(v0, v1) 141 | { 142 | } 143 | inline EigenVector2f(const float *v) : Eigen::Vector2f(v[0], v[1]) {} 144 | inline void operator=(const Eigen::Vector2f &e_v) 145 | { 146 | *((Eigen::Vector2f *)this) = e_v; 147 | } 148 | inline LA::Vector2f GetVector2f() const 149 | { 150 | LA::Vector2f v; 151 | const Eigen::Vector2f &e_v = *this; 152 | v.v0() = e_v(0); 153 | v.v1() = e_v(1); 154 | return v; 155 | } 156 | inline float SquaredLength() const { return GetVector2f().SquaredLength(); } 157 | inline void Print(const bool e = false) const { GetVector2f().Print(e); } 158 | inline bool AssertEqual(const LA::Vector2f &v, const int verbose = 1, 159 | const float eps = 0.0f) const 160 | { 161 | return GetVector2f().AssertEqual(v, verbose, eps); 162 | } 163 | inline bool AssertEqual(const EigenVector2f &e_v, const int verbose = 1, 164 | const float eps = 0.0f) const 165 | { 166 | return AssertEqual(e_v.GetVector2f(), verbose, eps); 167 | } 168 | }; 169 | #endif 170 | #endif 171 | -------------------------------------------------------------------------------- /LinearAlgebra/Vector4.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of EIBA. 3 | * 4 | * Copyright (C) 2017 Zhejiang University 5 | * For more information see 6 | * 7 | * Licensed under the Apache License, Version 2.0 (the "License"); 8 | * You may not use this file except in compliance with the License. 9 | * You may obtain a copy of the License at 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | * Unless required by applicable law or agreed to in writing, software 12 | * distributed under the License is distributed on an "AS IS" BASIS, 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | * 17 | */ 18 | 19 | #include "PlatformIndependence/sse.h" 20 | 21 | #ifndef _VECTOR_4_H_ 22 | #define _VECTOR_4_H_ 23 | 24 | #include "Utility.h" 25 | 26 | namespace LA 27 | { 28 | class AlignedVector4f 29 | { 30 | public: 31 | inline const _pi__m128 &v0123() const { return m_data; } 32 | inline _pi__m128 &v0123() { return m_data; } 33 | inline const float &v0() const { return m_data.m128_f32[0]; } 34 | inline float &v0() { return m_data.m128_f32[0]; } 35 | inline const float &v1() const { return m_data.m128_f32[1]; } 36 | inline float &v1() { return m_data.m128_f32[1]; } 37 | inline const float &v2() const { return m_data.m128_f32[2]; } 38 | inline float &v2() { return m_data.m128_f32[2]; } 39 | inline const float &v3() const { return m_data.m128_f32[3]; } 40 | inline float &v3() { return m_data.m128_f32[3]; } 41 | inline const _pi__m128 &xyzw() const { return v0123(); } 42 | inline _pi__m128 &xyzw() { return v0123(); } 43 | inline const float &x() const { return v0(); } 44 | inline float &x() { return v0(); } 45 | inline const float &y() const { return v1(); } 46 | inline float &y() { return v1(); } 47 | inline const float &z() const { return v2(); } 48 | inline float &z() { return v2(); } 49 | inline const float &w() const { return v3(); } 50 | inline float &w() { return v3(); } 51 | inline operator const float *() const { return (const float *)this; } 52 | inline operator float *() { return (float *)this; } 53 | inline bool operator==(const AlignedVector4f &v) const 54 | { 55 | return v0() == v.v0() && v1() == v.v1() && v2() == v.v2() && 56 | v3() == v.v3(); 57 | } 58 | inline void operator*=(const float s) { Scale(s); } 59 | inline AlignedVector4f operator-(const AlignedVector4f &b) const 60 | { 61 | AlignedVector4f amb; 62 | amb.v0123() = _pi_mm_sub_ps(v0123(), b.v0123()); 63 | return amb; 64 | } 65 | 66 | inline void Set(const float *v) { memcpy(this, v, 16); } 67 | inline void Set(const double *v) 68 | { 69 | v0123() = 70 | _pi_mm_setr_ps(float(v[0]), float(v[1]), float(v[2]), float(v[3])); 71 | } 72 | inline void Get(float *v) const { memcpy(v, this, 16); } 73 | inline void MakeZero() { memset(this, 0, sizeof(AlignedVector4f)); } 74 | inline void MakeMinus() 75 | { 76 | v0123() = _pi_mm_sub_ps(_pi_mm_setzero_ps(), v0123()); 77 | } 78 | inline AlignedVector4f GetMinus() const 79 | { 80 | AlignedVector4f v; 81 | v.v0123() = _pi_mm_sub_ps(_pi_mm_setzero_ps(), v0123()); 82 | return v; 83 | } 84 | 85 | inline bool Valid() const { return v0() != FLT_MAX; } 86 | inline bool Invalid() const { return v0() == FLT_MAX; } 87 | inline void Invalidate() { v0() = FLT_MAX; } 88 | inline void Scale(const float s) 89 | { 90 | const _pi__m128 _s = _pi_mm_set1_ps(s); 91 | Scale(_s); 92 | } 93 | inline void Scale(const _pi__m128 &s) 94 | { 95 | v0123() = _pi_mm_mul_ps(s, v0123()); 96 | } 97 | inline float SquaredLength() const 98 | { 99 | return SSE::Sum(_pi_mm_mul_ps(v0123(), v0123())); 100 | } 101 | inline float Dot(const AlignedVector4f &v) const 102 | { 103 | return SSE::Sum(_pi_mm_mul_ps(v0123(), v.v0123())); 104 | } 105 | 106 | inline void Print(const bool e = false) const 107 | { 108 | if (e) 109 | UT::Print("%e %e %e %e\n", v0(), v1(), v2(), v3()); 110 | else 111 | UT::Print("%f %f %f %f\n", v0(), v1(), v2(), v3()); 112 | } 113 | inline void Save(FILE *fp, const bool e = false) const 114 | { 115 | if (e) 116 | fprintf(fp, "%e %e %e %e\n", v0(), v1(), v2(), v3()); 117 | else 118 | fprintf(fp, "%f %f %f %f\n", v0(), v1(), v2(), v3()); 119 | } 120 | inline void Load(FILE *fp) 121 | { 122 | #ifdef CFG_DEBUG 123 | const int N = fscanf(fp, "%f %f %f %f", &v0(), &v1(), &v2(), &v3()); 124 | UT_ASSERT(N == 4); 125 | #else 126 | fscanf(fp, "%f %f %f %f", &v0(), &v1(), &v2(), &v3()); 127 | #endif 128 | } 129 | 130 | inline void Random(const float mMax) { UT::Random(&v0(), 4, -mMax, mMax); } 131 | static inline AlignedVector4f GetRandom(const float mMax) 132 | { 133 | AlignedVector4f v; 134 | v.Random(mMax); 135 | return v; 136 | } 137 | 138 | inline void AssertZero() const { UT::AssertEqual(SquaredLength(), 0.0f); } 139 | inline bool AssertEqual(const AlignedVector4f &v, const int verbose = 1, 140 | const float eps = 0.0f) const 141 | { 142 | if (UT::VectorAssertEqual(&v0(), &v.v0(), 4, 0, eps)) return true; 143 | if (verbose) { 144 | UT::PrintSeparator(); 145 | Print(verbose > 1); 146 | v.Print(verbose > 1); 147 | } 148 | return false; 149 | } 150 | 151 | inline void SetInfinite() { v0123() = _pi_mm_set1_ps(FLT_MAX); } 152 | inline void AssertInfinite() const 153 | { 154 | UT_ASSERT(v0() == FLT_MAX); 155 | UT_ASSERT(v1() == FLT_MAX); 156 | UT_ASSERT(v2() == FLT_MAX); 157 | UT_ASSERT(v3() == FLT_MAX); 158 | } 159 | inline void AssertFinite() const 160 | { 161 | UT_ASSERT(v0() != FLT_MAX); 162 | UT_ASSERT(v1() != FLT_MAX); 163 | UT_ASSERT(v2() != FLT_MAX); 164 | UT_ASSERT(v3() != FLT_MAX); 165 | } 166 | 167 | protected: 168 | _pi__m128 m_data; 169 | }; 170 | } 171 | 172 | #ifdef CFG_DEBUG_EIGEN 173 | #include 174 | class EigenVector4f : public Eigen::Vector4f 175 | { 176 | public: 177 | inline EigenVector4f() = default; 178 | inline EigenVector4f(const Eigen::Vector4f &e_v) : Eigen::Vector4f(e_v) {} 179 | inline EigenVector4f(const LA::AlignedVector4f &v) 180 | : Eigen::Vector4f(v.v0(), v.v1(), v.v2(), v.v3()) 181 | { 182 | } 183 | inline EigenVector4f(const float v0, const float v1, const float v2, 184 | const float v3) 185 | : Eigen::Vector4f(v0, v1, v2, v3) 186 | { 187 | } 188 | inline void operator=(const Eigen::Vector4f &e_v) 189 | { 190 | *((Eigen::Vector4f *)this) = e_v; 191 | } 192 | inline LA::AlignedVector4f GetAlignedVector4f() const 193 | { 194 | LA::AlignedVector4f v; 195 | const Eigen::Vector4f &e_v = *this; 196 | v.v0() = e_v(0); 197 | v.v1() = e_v(1); 198 | v.v2() = e_v(2); 199 | v.v3() = e_v(3); 200 | return v; 201 | } 202 | inline float SquaredLength() const 203 | { 204 | return GetAlignedVector4f().SquaredLength(); 205 | } 206 | inline void Print(const bool e = false) const 207 | { 208 | GetAlignedVector4f().Print(e); 209 | } 210 | static inline EigenVector4f GetRandom(const float mMax) 211 | { 212 | return EigenVector4f(LA::AlignedVector4f::GetRandom(mMax)); 213 | } 214 | inline bool AssertEqual(const LA::AlignedVector4f &v, const int verbose = 1, 215 | const float eps = 0.0f) const 216 | { 217 | return GetAlignedVector4f().AssertEqual(v, verbose, eps); 218 | } 219 | inline bool AssertEqual(const EigenVector4f &e_v, const int verbose = 1, 220 | const float eps = 0.0f) const 221 | { 222 | return AssertEqual(e_v.GetAlignedVector4f(), verbose, eps); 223 | } 224 | }; 225 | #endif 226 | #endif 227 | -------------------------------------------------------------------------------- /LinearAlgebra/Vector8.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of EIBA. 3 | * 4 | * Copyright (C) 2017 Zhejiang University 5 | * For more information see 6 | * 7 | * Licensed under the Apache License, Version 2.0 (the "License"); 8 | * You may not use this file except in compliance with the License. 9 | * You may obtain a copy of the License at 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | * Unless required by applicable law or agreed to in writing, software 12 | * distributed under the License is distributed on an "AS IS" BASIS, 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | * 17 | */ 18 | 19 | #include "PlatformIndependence/sse.h" 20 | 21 | #ifndef _VECTOR_8_H_ 22 | #define _VECTOR_8_H_ 23 | 24 | #include "Utility.h" 25 | 26 | namespace LA 27 | { 28 | class AlignedVector8f 29 | { 30 | public: 31 | inline AlignedVector8f() = default; 32 | inline AlignedVector8f(const float *v) { Set(v); } 33 | inline const _pi__m128 &v0123() const { return m_data[0]; } 34 | inline _pi__m128 &v0123() { return m_data[0]; } 35 | inline const _pi__m128 &v4567() const { return m_data[1]; } 36 | inline _pi__m128 &v4567() { return m_data[1]; } 37 | inline const float &v0() const { return m_data[0].m128_f32[0]; } 38 | inline float &v0() { return m_data[0].m128_f32[0]; } 39 | inline const float &v1() const { return m_data[0].m128_f32[1]; } 40 | inline float &v1() { return m_data[0].m128_f32[1]; } 41 | inline const float &v2() const { return m_data[0].m128_f32[2]; } 42 | inline float &v2() { return m_data[0].m128_f32[2]; } 43 | inline const float &v3() const { return m_data[0].m128_f32[3]; } 44 | inline float &v3() { return m_data[0].m128_f32[3]; } 45 | inline const float &v4() const { return m_data[1].m128_f32[0]; } 46 | inline float &v4() { return m_data[1].m128_f32[0]; } 47 | inline const float &v5() const { return m_data[1].m128_f32[1]; } 48 | inline float &v5() { return m_data[1].m128_f32[1]; } 49 | inline const float &v6() const { return m_data[1].m128_f32[2]; } 50 | inline float &v6() { return m_data[1].m128_f32[2]; } 51 | inline const float &v7() const { return m_data[1].m128_f32[3]; } 52 | inline float &v7() { return m_data[1].m128_f32[3]; } 53 | inline operator const float *() const { return (const float *)this; } 54 | inline operator float *() { return (float *)this; } 55 | inline void operator+=(const AlignedVector8f &v) 56 | { 57 | v0123() = _pi_mm_add_ps(v.v0123(), v0123()); 58 | v4567() = _pi_mm_add_ps(v.v4567(), v4567()); 59 | } 60 | inline void operator-=(const AlignedVector8f &v) 61 | { 62 | v0123() = _pi_mm_sub_ps(v0123(), v.v0123()); 63 | v4567() = _pi_mm_sub_ps(v4567(), v.v4567()); 64 | } 65 | inline void operator*=(const float s) { Scale(s); } 66 | inline void operator*=(const _pi__m128 &s) { Scale(s); } 67 | inline AlignedVector8f operator*(const _pi__m128 &s) const 68 | { 69 | AlignedVector8f v; 70 | GetScaled(s, v); 71 | return v; 72 | } 73 | 74 | inline void Set(const float *v) 75 | { 76 | memcpy(this, v, sizeof(AlignedVector8f)); 77 | } 78 | inline void Get(float *v) const 79 | { 80 | memcpy(v, this, sizeof(AlignedVector8f)); 81 | } 82 | 83 | inline void MakeZero() { memset(this, 0, sizeof(AlignedVector8f)); } 84 | inline void MakeMinus() 85 | { 86 | v0123() = _pi_mm_sub_ps(_pi_mm_setzero_ps(), v0123()); 87 | v4567() = _pi_mm_sub_ps(_pi_mm_setzero_ps(), v4567()); 88 | } 89 | 90 | inline void Scale(const float s) 91 | { 92 | const _pi__m128 _s = _pi_mm_set1_ps(s); 93 | Scale(_s); 94 | } 95 | inline void Scale(const _pi__m128 &s) 96 | { 97 | v0123() = _pi_mm_mul_ps(s, v0123()); 98 | v4567() = _pi_mm_mul_ps(s, v4567()); 99 | } 100 | inline void GetScaled(const _pi__m128 &s, AlignedVector8f &v) const 101 | { 102 | v.v0123() = _pi_mm_mul_ps(s, v0123()); 103 | v.v4567() = _pi_mm_mul_ps(s, v4567()); 104 | } 105 | inline float SquaredLength() const 106 | { 107 | return SSE::Sum(_pi_mm_add_ps(_pi_mm_mul_ps(v0123(), v0123()), 108 | _pi_mm_mul_ps(v4567(), v4567()))); 109 | } 110 | inline float Dot(const AlignedVector8f &v) const 111 | { 112 | return SSE::Sum(_pi_mm_add_ps(_pi_mm_mul_ps(v0123(), v.v0123()), 113 | _pi_mm_mul_ps(v4567(), v.v4567()))); 114 | } 115 | 116 | inline void Print(const bool e = false) const 117 | { 118 | if (e) 119 | UT::Print("%e %e %e %e %e %e %e %e\n", v0(), v1(), v2(), v3(), v4(), 120 | v5(), v6(), v7()); 121 | else 122 | UT::Print("%f %f %f %f %f %f %f %f\n", v0(), v1(), v2(), v3(), v4(), 123 | v5(), v6(), v7()); 124 | } 125 | 126 | static inline void apb(const AlignedVector8f &a, const AlignedVector8f &b, 127 | AlignedVector8f &apb) 128 | { 129 | apb.v0123() = _pi_mm_add_ps(a.v0123(), b.v0123()); 130 | apb.v4567() = _pi_mm_add_ps(a.v4567(), b.v4567()); 131 | } 132 | 133 | inline void Random(const float mMax) { UT::Random(&v0(), 8, -mMax, mMax); } 134 | static inline AlignedVector8f GetRandom(const float mMax) 135 | { 136 | AlignedVector8f v; 137 | v.Random(mMax); 138 | return v; 139 | } 140 | 141 | inline void AssertZero() const { UT::AssertEqual(SquaredLength(), 0.0f); } 142 | inline bool AssertEqual(const LA::AlignedVector8f &v, 143 | const int verbose = 1) const 144 | { 145 | if (UT::VectorAssertEqual(&v0(), &v.v0(), 8, 0)) return true; 146 | if (verbose) { 147 | UT::PrintSeparator(); 148 | Print(verbose > 1); 149 | v.Print(verbose > 1); 150 | } 151 | return false; 152 | } 153 | 154 | protected: 155 | _pi__m128 m_data[2]; 156 | }; 157 | } 158 | 159 | #ifdef CFG_DEBUG_EIGEN 160 | #include 161 | class EigenVector8f : public Eigen::Matrix 162 | { 163 | public: 164 | inline EigenVector8f() = default; 165 | inline EigenVector8f(const Eigen::Matrix &e_v) 166 | : Eigen::Matrix(e_v) 167 | { 168 | } 169 | inline EigenVector8f(const LA::AlignedVector8f &v) 170 | : Eigen::Matrix() 171 | { 172 | const float *_v = v; 173 | Eigen::Matrix &e_v = *this; 174 | for (int i = 0; i < 8; ++i) e_v(i, 0) = _v[i]; 175 | } 176 | inline void operator=(const Eigen::Matrix &e_v) 177 | { 178 | *((Eigen::Matrix *)this) = e_v; 179 | } 180 | inline LA::AlignedVector8f GetAlignedVector8f() const 181 | { 182 | LA::AlignedVector8f v; 183 | float *_v = v; 184 | const Eigen::Matrix &e_v = *this; 185 | for (int i = 0; i < 8; ++i) _v[i] = e_v(i, 0); 186 | return v; 187 | } 188 | inline float SquaredLength() const 189 | { 190 | return GetAlignedVector8f().SquaredLength(); 191 | } 192 | inline void Print(const bool e = false) const 193 | { 194 | GetAlignedVector8f().Print(e); 195 | } 196 | static inline EigenVector8f GetRandom(const float mMax) 197 | { 198 | return EigenVector8f(LA::AlignedVector8f::GetRandom(mMax)); 199 | } 200 | inline bool AssertEqual(const LA::AlignedVector8f &v, 201 | const int verbose = 1) const 202 | { 203 | return GetAlignedVector8f().AssertEqual(v, verbose); 204 | } 205 | inline bool AssertEqual(const EigenVector8f &e_v, 206 | const int verbose = 1) const 207 | { 208 | return AssertEqual(e_v.GetAlignedVector8f(), verbose); 209 | } 210 | }; 211 | #endif 212 | #endif 213 | -------------------------------------------------------------------------------- /Map/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(Map 2 | GlobalMap.cpp 3 | ) 4 | target_link_libraries(Map 5 | ${Boost_LIBRARIES} 6 | ) 7 | -------------------------------------------------------------------------------- /Map/GlobalMap.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of EIBA. 3 | * 4 | * Copyright (C) 2017 Zhejiang University 5 | * For more information see 6 | * 7 | * Licensed under the Apache License, Version 2.0 (the "License"); 8 | * You may not use this file except in compliance with the License. 9 | * You may obtain a copy of the License at 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | * Unless required by applicable law or agreed to in writing, software 12 | * distributed under the License is distributed on an "AS IS" BASIS, 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | * 17 | */ 18 | 19 | #include "GlobalMap.h" 20 | 21 | void GlobalMap::IT_Reset(const int nKFsMax) 22 | { 23 | MT_WRITE_LOCK_BEGIN(m_MT); 24 | m_Cs.Reserve(nKFsMax); 25 | m_Cs.Resize(0); 26 | m_ds.resize(0); 27 | m_sIT = m_sLD = m_sLBA = m_sOST = true; 28 | MT_WRITE_LOCK_END(m_MT); 29 | } 30 | 31 | void GlobalMap::IT_Push(const FRM::Tag &T, const Rigid3D &C, 32 | const std::vector &ds) 33 | { 34 | MT_WRITE_LOCK_BEGIN(m_MT); 35 | m_Ts.push_back(T); 36 | m_Cs.Push(C); 37 | m_ds.insert(m_ds.end(), ds.begin(), ds.end()); 38 | /*m_sLD = m_sLBA = */ m_sOST = false; 39 | MT_WRITE_LOCK_END(m_MT); 40 | } 41 | 42 | bool GlobalMap::IT_Synchronize(AlignedVector &Cs, 43 | std::vector &ds) 44 | { 45 | MT_READ_LOCK_BEGIN(m_MT); 46 | if (m_sIT) return true; 47 | #ifdef CFG_DEBUG 48 | UT_ASSERT(m_Cs.Size() == Cs.Size() && m_ds.size() == ds.size()); 49 | #endif 50 | Cs.Set(m_Cs); 51 | ds = m_ds; 52 | MT_READ_LOCK_END(m_MT); 53 | MT_WRITE_LOCK_BEGIN(m_MT); 54 | m_sIT = true; 55 | MT_WRITE_LOCK_END(m_MT); 56 | return false; 57 | } 58 | 59 | bool GlobalMap::LD_Synchronize(AlignedVector &Cs, 60 | std::vector &ds) 61 | { 62 | MT_READ_LOCK_BEGIN(m_MT); 63 | if (m_sLD) return true; 64 | #ifdef CFG_DEBUG 65 | UT_ASSERT(m_Cs.Size() == Cs.Size() && m_ds.size() == ds.size()); 66 | #endif 67 | Cs.Set(m_Cs); 68 | ds = m_ds; 69 | MT_READ_LOCK_END(m_MT); 70 | MT_WRITE_LOCK_BEGIN(m_MT); 71 | m_sLD = true; 72 | MT_WRITE_LOCK_END(m_MT); 73 | return false; 74 | } 75 | 76 | bool GlobalMap::LBA_Synchronize(AlignedVector &Cs) 77 | { 78 | MT_READ_LOCK_BEGIN(m_MT); 79 | if (m_sLBA) return true; 80 | #ifdef CFG_DEBUG 81 | UT_ASSERT(m_Cs.Size() == Cs.Size()); 82 | #endif 83 | Cs.Set(m_Cs); 84 | MT_READ_LOCK_END(m_MT); 85 | MT_WRITE_LOCK_BEGIN(m_MT); 86 | m_sLBA = true; 87 | MT_WRITE_LOCK_END(m_MT); 88 | return false; 89 | } 90 | 91 | bool GlobalMap::OST_Synchronize(std::vector &Ts, 92 | AlignedVector &Cs) 93 | { 94 | MT_READ_LOCK_BEGIN(m_MT); 95 | if (m_sOST) return true; 96 | Ts = m_Ts; 97 | Cs.Set(m_Cs); 98 | MT_READ_LOCK_END(m_MT); 99 | MT_WRITE_LOCK_BEGIN(m_MT); 100 | m_sOST = true; 101 | MT_WRITE_LOCK_END(m_MT); 102 | return false; 103 | } 104 | 105 | void GlobalMap::GBA_Update(const AlignedVector &Cs, 106 | const std::vector &ds) 107 | { 108 | // MT_WRITE_LOCK_BEGIN(m_MT); 109 | // m_Cs.Set(Cs); 110 | // m_ds = ds; 111 | #ifdef CFG_DEBUG 112 | UT_ASSERT(m_Cs.Size() >= Cs.Size() && m_ds.size() >= ds.size()); 113 | #endif 114 | m_Cs.Resize(Cs.Size()); 115 | m_ds.resize(ds.size()); 116 | memcpy(m_Cs.Data(), Cs.Data(), sizeof(Rigid3D) * Cs.Size()); 117 | memcpy(m_ds.data(), ds.data(), sizeof(DepthInverseGaussian) * ds.size()); 118 | m_sIT = m_sLD = m_sLBA = m_sOST = false; 119 | // MT_WRITE_LOCK_END(m_MT); 120 | } 121 | 122 | void GlobalMap::SaveB(FILE *fp) 123 | { 124 | MT_READ_LOCK_BEGIN(m_MT); 125 | UT::SaveB(m_K, fp); 126 | UT::SaveB(m_pIMU, fp); 127 | FRM::VectorSaveB(m_Ts, fp); 128 | m_Cs.SaveB(fp); 129 | UT::VectorSaveB(m_ds, fp); 130 | UT::SaveB(m_sIT, fp); 131 | UT::SaveB(m_sLD, fp); 132 | UT::SaveB(m_sLBA, fp); 133 | UT::SaveB(m_sOST, fp); 134 | MT_READ_LOCK_END(m_MT); 135 | } 136 | 137 | void GlobalMap::LoadB(FILE *fp) 138 | { 139 | MT_WRITE_LOCK_BEGIN(m_MT); 140 | UT::LoadB(m_K, fp); 141 | UT::LoadB(m_pIMU, fp); 142 | FRM::VectorLoadB(m_Ts, fp); 143 | m_Cs.LoadB(fp); 144 | UT::VectorLoadB(m_ds, fp); 145 | UT::LoadB(m_sIT, fp); 146 | UT::LoadB(m_sLD, fp); 147 | UT::LoadB(m_sLBA, fp); 148 | UT::LoadB(m_sOST, fp); 149 | MT_WRITE_LOCK_END(m_MT); 150 | } 151 | 152 | void GlobalMap::AssertConsistency() 153 | { 154 | MT_READ_LOCK_BEGIN(m_MT); 155 | const int nKFs = int(m_Ts.size()); 156 | for (int iKF = 0; iKF < nKFs; ++iKF) { 157 | if (iKF > 0) UT_ASSERT(m_Ts[iKF - 1] < m_Ts[iKF]); 158 | m_Cs[iKF].AssertOrthogonal(); 159 | } 160 | MT_READ_LOCK_END(m_MT); 161 | } 162 | -------------------------------------------------------------------------------- /Map/GlobalMap.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of EIBA. 3 | * 4 | * Copyright (C) 2017 Zhejiang University 5 | * For more information see 6 | * 7 | * Licensed under the Apache License, Version 2.0 (the "License"); 8 | * You may not use this file except in compliance with the License. 9 | * You may obtain a copy of the License at 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | * Unless required by applicable law or agreed to in writing, software 12 | * distributed under the License is distributed on an "AS IS" BASIS, 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | * 17 | */ 18 | 19 | #include "PlatformIndependence/sse.h" 20 | 21 | #ifndef _GLOBAL_MAP_H_ 22 | #define _GLOBAL_MAP_H_ 23 | 24 | #include "stdafx.h" 25 | 26 | #include "Depth.h" 27 | #include "Frame.h" 28 | #include "MultiThread.h" 29 | #include "Rigid.h" 30 | 31 | class GlobalMap 32 | { 33 | public: 34 | class KeyFrame : public FRM::Frame 35 | { 36 | public: 37 | inline KeyFrame() : FRM::Frame() {} 38 | inline KeyFrame(const KeyFrame &KF) { *this = KF; } 39 | inline void operator=(const KeyFrame &KF) 40 | { 41 | *((FRM::Frame *)this) = KF; 42 | m_id = KF.m_id; 43 | memcpy(m_ixIP, KF.m_ixIP, sizeof(m_ixIP)); 44 | m_xs = KF.m_xs; 45 | m_iKFsPrior = KF.m_iKFsPrior; 46 | m_Zps.Set(KF.m_Zps); 47 | } 48 | inline bool operator==(const KeyFrame &KF) const 49 | { 50 | return *((FRM::Frame *)this) == KF && m_id == KF.m_id && 51 | UT::VectorEqual(m_ixIP, KF.m_ixIP, FRM_IP_LEVELS_PLUS_ONE) && 52 | UT::VectorEqual(m_xs, KF.m_xs) && 53 | UT::VectorEqual(m_iKFsPrior, KF.m_iKFsPrior); 54 | } 55 | inline bool operator==(const int iFrm) const 56 | { 57 | return m_T.m_iFrm == iFrm; 58 | } 59 | inline bool operator<(const int iFrm) const 60 | { 61 | return m_T.m_iFrm < iFrm; 62 | } 63 | inline void Initialize(const FRM::Frame &F, const int id) 64 | { 65 | FRM::Frame::Initialize(F); 66 | m_id = id; 67 | memset(m_ixIP, 0, sizeof(m_ixIP)); 68 | m_xs.resize(0); 69 | m_iKFsPrior.resize(0); 70 | m_Zps.Resize(0); 71 | } 72 | #if 0 73 | inline int SearchFeatureLevel(const int ix) const 74 | { 75 | #ifdef CFG_DEBUG 76 | UT_ASSERT(ix < int(m_xs.size())); 77 | #endif 78 | for(int iLvl = 0; iLvl < FRM_IP_LEVELS; ++iLvl) 79 | { 80 | if(m_ixIP[iLvl + 1] > ix) 81 | return iLvl; 82 | } 83 | return -1; 84 | } 85 | #endif 86 | inline int SearchKeyFrameMatch(const int iKF) const 87 | { 88 | const std::vector::const_iterator ik = 89 | std::lower_bound(m_iKFsMatch.begin(), m_iKFsMatch.end(), iKF); 90 | return ik == m_iKFsMatch.end() || *ik != iKF 91 | ? -1 92 | : int(ik - m_iKFsMatch.begin()); 93 | } 94 | inline int SearchKeyFramePrior(const int iKF) const 95 | { 96 | const std::vector::const_iterator ip = 97 | std::lower_bound(m_iKFsPrior.begin(), m_iKFsPrior.end(), iKF); 98 | return ip == m_iKFsPrior.end() || *ip != iKF 99 | ? -1 100 | : int(ip - m_iKFsPrior.begin()); 101 | } 102 | inline bool 103 | PushKeyFramePrior(const int iKF, const Camera::Pose::Prior::Rigid &Zp, 104 | AlignedVector &ZpsTmp) 105 | { 106 | const std::vector::const_iterator ip = 107 | std::lower_bound(m_iKFsPrior.begin(), m_iKFsPrior.end(), iKF); 108 | if (ip != m_iKFsPrior.end() && *ip == iKF) return false; 109 | if (ip == m_iKFsPrior.end()) { 110 | m_iKFsPrior.push_back(iKF); 111 | m_Zps.Push(Zp); 112 | } else { 113 | m_Zps.Insert(int(ip - m_iKFsPrior.begin()), Zp, ZpsTmp); 114 | m_iKFsPrior.insert(ip, iKF); 115 | } 116 | return true; 117 | } 118 | inline void SaveB(FILE *fp) const 119 | { 120 | FRM::Frame::SaveB(fp); 121 | UT::SaveB(m_id, fp); 122 | UT::SaveB(m_ixIP, FRM_IP_LEVELS_PLUS_ONE, fp); 123 | UT::VectorSaveB(m_xs, fp); 124 | UT::VectorSaveB(m_iKFsPrior, fp); 125 | m_Zps.SaveB(fp); 126 | } 127 | inline void LoadB(FILE *fp) 128 | { 129 | FRM::Frame::LoadB(fp); 130 | UT::LoadB(m_id, fp); 131 | UT::LoadB(m_ixIP, FRM_IP_LEVELS_PLUS_ONE, fp); 132 | UT::VectorLoadB(m_xs, fp); 133 | printf("source m_xs(2D+inv_d) %d \n", m_xs.size()); 134 | UT::VectorLoadB(m_iKFsPrior, fp); 135 | m_Zps.LoadB(fp); 136 | } 137 | inline void AssertConsistency() const 138 | { 139 | Frame::AssertConsistency(); 140 | UT_ASSERT(m_ixIP[0] == 0); 141 | for (int iLvl = 0; iLvl < FRM_IP_LEVELS; ++iLvl) 142 | UT_ASSERT(m_ixIP[iLvl] <= m_ixIP[iLvl + 1]); 143 | UT_ASSERT(m_ixIP[FRM_IP_LEVELS] == int(m_xs.size())); 144 | const int Np = int(m_iKFsPrior.size()); 145 | UT_ASSERT(m_Zps.Size() == Np); 146 | for (int ip = 0; ip < Np; ++ip) { 147 | if (ip > 0) UT_ASSERT(m_iKFsPrior[ip - 1] < m_iKFsPrior[ip]); 148 | m_Zps[ip].AssertConsistency(); 149 | } 150 | } 151 | 152 | public: 153 | // m_id : start index (global) of current keyframe's features 154 | // m_ixIP : start index (local) of current keyframe's features in each level, start from 0 155 | int m_id, m_ixIP[FRM_IP_LEVELS_PLUS_ONE]; 156 | // source features 157 | std::vector m_xs; 158 | // it seems that this is always empty 159 | std::vector m_iKFsPrior; 160 | // it seems that this is always empty 161 | AlignedVector m_Zps; 162 | }; 163 | 164 | class KeyFramePrior 165 | { 166 | public: 167 | inline KeyFramePrior() {} 168 | inline KeyFramePrior(const int iKF1, const int iKF2, 169 | const Camera::Pose::Prior::Rigid &Zp) 170 | : m_iKF1(iKF1), m_iKF2(iKF2), m_Zp(Zp) 171 | { 172 | } 173 | 174 | public: 175 | int m_iKF1, m_iKF2; 176 | Camera::Pose::Prior::Rigid m_Zp; 177 | }; 178 | 179 | public: 180 | void IT_Reset(const int nKFsMax); 181 | void IT_Push(const FRM::Tag &T, const Rigid3D &C, 182 | const std::vector &ds); 183 | bool IT_Synchronize(AlignedVector &Cs, 184 | std::vector &ds); 185 | bool LD_Synchronize(AlignedVector &Cs, 186 | std::vector &ds); 187 | bool LBA_Synchronize(AlignedVector &Cs); 188 | bool OST_Synchronize(std::vector &Ts, AlignedVector &Cs); 189 | void GBA_Update(const AlignedVector &Cs, 190 | const std::vector &ds); 191 | 192 | void SaveB(FILE *fp); 193 | void LoadB(FILE *fp); 194 | void AssertConsistency(); 195 | 196 | public: 197 | 198 | Intrinsic m_K; 199 | Point3D m_pIMU; 200 | 201 | protected: 202 | std::vector m_Ts; 203 | AlignedVector m_Cs; 204 | std::vector m_ds; 205 | bool m_sIT, m_sLD, m_sLBA, m_sOST; 206 | // boost::shared_mutex m_MT; 207 | std::shared_timed_mutex m_MT; 208 | }; 209 | 210 | #endif 211 | -------------------------------------------------------------------------------- /PlatformIndependence/def_missing.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of EIBA. 3 | * 4 | * Copyright (C) 2017 Zhejiang University 5 | * For more information see 6 | * 7 | * Licensed under the Apache License, Version 2.0 (the "License"); 8 | * You may not use this file except in compliance with the License. 9 | * You may obtain a copy of the License at 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | * Unless required by applicable law or agreed to in writing, software 12 | * distributed under the License is distributed on an "AS IS" BASIS, 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | * 17 | */ 18 | 19 | #ifndef PLATFORM_INDEPENDENCE_DEF_MISSING_H_ 20 | #define PLATFORM_INDEPENDENCE_DEF_MISSING_H_ 21 | 22 | #ifndef WIN32 23 | #include 24 | #include // UCHAR_MAX... 25 | #ifndef __APPLE__ 26 | #include 27 | #endif 28 | #include 29 | #include 30 | 31 | #ifndef _access 32 | #define _access access 33 | #endif 34 | 35 | #ifndef _aligned_malloc 36 | # ifdef __APPLE__ 37 | # define _aligned_malloc(size, align) malloc(size) 38 | # else 39 | # define _aligned_malloc(size, align) memalign(align, size) 40 | # endif 41 | #endif 42 | 43 | #ifndef _aligned_free 44 | #define _aligned_free free 45 | #endif 46 | 47 | #ifndef _finite 48 | #define _finite std::isfinite 49 | #endif 50 | 51 | #ifndef _isnan 52 | // #define _isnan isnan // not works 53 | inline int _isnan(double x) { return std::isnan(x); } 54 | #endif 55 | 56 | #ifndef _isnanf 57 | #define _isnanf std::isnan 58 | #endif 59 | 60 | #ifndef Sleep 61 | #define Sleep sleep 62 | #endif 63 | 64 | // #ifndef FLT_EPSILON 65 | // #define FLT_EPSILON 1e-6f 66 | // #endif 67 | 68 | // #ifndef DBL_EPSILON 69 | // #define DBL_EPSILON 1e-6 70 | // #endif 71 | 72 | // // copy from stdafx.h 73 | 74 | // typedef unsigned char ubyte; 75 | // typedef unsigned short ushort; 76 | // typedef unsigned int uint; 77 | // typedef unsigned long long ullong; 78 | 79 | // #ifdef max 80 | // #undef max 81 | // #endif 82 | // #ifdef min 83 | // #undef min 84 | // #endif 85 | 86 | // #ifndef MAX_LINE_LENGTH 87 | // #define MAX_LINE_LENGTH 512 88 | // #endif 89 | 90 | // #ifndef PI 91 | // #define PI 3.141592654f 92 | // #endif 93 | 94 | // #ifndef PIx2 95 | // #define PIx2 6.283185308f 96 | // #endif 97 | 98 | // #ifndef FACTOR_RAD_TO_DEG 99 | // #define FACTOR_RAD_TO_DEG 57.295779505601046646705075978956f 100 | // #endif 101 | 102 | // #ifndef FACTOR_DEG_TO_RAD 103 | // #define FACTOR_DEG_TO_RAD 0.01745329252222222222222222222222f 104 | // #endif 105 | 106 | // #ifndef SWAP 107 | // #define SWAP(a, b, t) \ 108 | // (t) = (a); \ 109 | // (a) = (b); \ 110 | // (b) = (t) 111 | // #endif 112 | #endif // #ifndef WIN32 113 | 114 | #endif 115 | -------------------------------------------------------------------------------- /PlatformIndependence/gen_sse_impl.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | 3 | import clang.cindex 4 | import re 5 | import subprocess 6 | 7 | intrinsic_types = {'__m64', '__m128', '__m128i', '__m128d'} 8 | pointer_arg_matcher = re.compile(r'.*(__m64|__m128|__m128i|__m128d).*\*.*') 9 | non_pointer_arg_matcher = re.compile(r'.*(__m64|__m128|__m128i|__m128d).*') 10 | header = "//Do not edit this file, it's automatically generated." 11 | 12 | 13 | class FuncDeclVisitor: 14 | """For a intrinsic function declaration, generate the implementatio""" 15 | 16 | def __init__(self): 17 | self.alreay_visited = set() 18 | self.macros = [] 19 | # self.funcnames = [] 20 | # self.funcimpls = [] 21 | # self.taildefines = [] 22 | 23 | def __call__(self, node): 24 | # ret, name, arg_types, arg_names = self.parse(node) 25 | old_ret, old_name, old_arg_types, old_arg_names = self.parse(node) 26 | if old_name in self.alreay_visited: 27 | return 28 | self.alreay_visited.add(old_name) 29 | new_ret = old_ret.replace('__m', '_pi__m') 30 | new_name = '_pi' + old_name 31 | new_arg_types, new_paras = [], [] 32 | for t, n in zip(old_arg_types, old_arg_names): 33 | # new_arg_types.append(t.replace('__m', 'my__m')) 34 | if pointer_arg_matcher.match(t): 35 | new_paras.append('&' + self.quote(n) + '->native_obj') 36 | elif non_pointer_arg_matcher.match(t): 37 | new_paras.append(self.quote(n) + '.native_obj') 38 | else: 39 | new_paras.append(n) 40 | macro_head = '#define ' + new_name + self.quote( 41 | ','.join(old_arg_names)) 42 | # impl = '#define ' + new_name + '(' + \ 43 | # ','.join(t + ' ' + n for t, n in zip(new_arg_types, old_arg_names)) + \ 44 | # ')' 45 | macro_impl = old_name + self.quote(','.join(new_paras)) 46 | if old_ret in intrinsic_types: 47 | macro_impl = self.native_to_my_expr(macro_impl) 48 | # exprs = [] 49 | # if old_ret in intrinsic_types: 50 | # exprs.append(new_ret + ' tmp') 51 | # exprs.append('tmp.native_obj=' + old_name + self.quote(','.join(new_paras))) 52 | # exprs.append('tmp') 53 | # else: 54 | # exprs.append(old_name + self.quote(','.join(new_paras))) 55 | # impl = define_head + ' ' + self.quote(','.join(exprs)) 56 | # if old_ret in intrinsic_types: 57 | # impl = new_ret + ' tmp;' + \ 58 | # 'tmp.native_obj=' + impl + 'return tmp;' 59 | # elif 'void' != old_ret: 60 | # impl = 'return ' + impl 61 | 62 | # cur_func = decl + '{' + impl + '}' 63 | # impl = define_head + define_impl 64 | # self.funcimpls.append(impl) 65 | # self.funcnames.append((old_name, new_name)) 66 | # self.taildefines.append('#ifdef ' + old_name + '\n' + 67 | # '#undef ' + old_name + '\n' + '#endif\n' 68 | # '#define ' + old_name + self.quote(','.join(old_arg_names)) + 69 | # ' ' + new_name + self.quote(','.join(old_arg_names))) 70 | win32_macro = ' '.join(['#define', new_name, old_name]) 71 | linux_macro = macro_head + macro_impl 72 | self.macros.append('\n'.join( 73 | ['#ifdef WIN32', win32_macro, '#else', linux_macro, '#endif'])) 74 | 75 | @staticmethod 76 | def parse(node): 77 | ret = node.result_type.spelling 78 | arg_types = [] 79 | arg_names = [] 80 | for a in node.get_arguments(): 81 | arg_types.append(a.type.spelling) 82 | arg_names.append(a.spelling) 83 | return ret, node.spelling, arg_types, arg_names 84 | 85 | @staticmethod 86 | def quote(name): 87 | return '(' + name + ')' 88 | 89 | @staticmethod 90 | def native_to_my_expr(expr): 91 | return 'from_native_obj' + FuncDeclVisitor.quote(expr) 92 | 93 | def result(self): 94 | return self.result 95 | 96 | 97 | def visit_func_decls(node, visitor): 98 | """visit AST, for each function declaration node, call visitor""" 99 | if node.kind == clang.cindex.CursorKind.FUNCTION_DECL: 100 | visitor(node) 101 | for c in node.get_children(): 102 | visit_func_decls(c, visitor) 103 | 104 | 105 | if __name__ == '__main__': 106 | src, out = "sse.in", "sse_impl.h" 107 | code = 'class __m64{}; class __m128{}; class __m128i{}; class __m128d{};' 108 | with open(src, 'r') as f: 109 | code = code + ';'.join(f) 110 | code = code + ';' 111 | # for line in f: 112 | # code = code + line + ';' 113 | 114 | index = clang.cindex.Index.create() 115 | unit = index.parse('tmp.cc', None, [('tmp.cc', code)]) 116 | visitor = FuncDeclVisitor() 117 | visit_func_decls(unit.cursor, visitor) 118 | 119 | with open(out, 'w') as outfile: 120 | outfile.write(header + '\n') 121 | outfile.write('\n'.join(visitor.macros)) 122 | outfile.write('\n') 123 | # outfile.write('\n'.join(visitor.funcimpls)) 124 | # outfile.write('\n') 125 | # outfile.write('\n'.join(visitor.taildefines)) 126 | # for old_name, new_name in visitor.funcnames: 127 | # outfile.write('#ifdef ' + old_name + '\n') 128 | # outfile.write('#undef ' + old_name + '\n') 129 | # outfile.write('#endif\n') 130 | # outfile.write('#define ' + old_name + ' ' + new_name + '\n') 131 | subprocess.check_call(['clang-format', '-i', out]) 132 | -------------------------------------------------------------------------------- /PlatformIndependence/sse.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of EIBA. 3 | * 4 | * Copyright (C) 2017 Zhejiang University 5 | * For more information see 6 | * 7 | * Licensed under the Apache License, Version 2.0 (the "License"); 8 | * You may not use this file except in compliance with the License. 9 | * You may obtain a copy of the License at 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | * Unless required by applicable law or agreed to in writing, software 12 | * distributed under the License is distributed on an "AS IS" BASIS, 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | * 17 | */ 18 | 19 | #ifndef PLATFORM_INDEPENDENCE_SSE_H_ 20 | #define PLATFORM_INDEPENDENCE_SSE_H_ 21 | // clang-format off 22 | // Wanna preserve the order 23 | #include // MMX 24 | #include //SSE 25 | #include //SSE2 26 | 27 | // FIXME: following works, but not know why 28 | #ifndef WIN32 29 | #include //SSE3 30 | #include //SSSE3 31 | #include //SSE4.1 32 | #include //SSE4.2 33 | #include //SSE4A 34 | #endif 35 | 36 | // clang-format on 37 | 38 | #ifdef WIN32 39 | #define _pi__m64 __m64 40 | #define _pi__m128 __m128 41 | #define _pi__m128i __m128i 42 | #define _pi__m128d __m128d 43 | #else 44 | // namespace pi 45 | // { 46 | // union __m64 { 47 | // uint64 m64_u64; 48 | // float m64_f32[2]; 49 | // int8 m64_i8[8]; 50 | // int16 m64_i16[4]; 51 | // int32 m64_i32[2]; 52 | // int64 m64_i64; 53 | // uint8 m64_u8[8]; 54 | // uint16 m64_u16[4]; 55 | // uint32 m64_u32[2]; 56 | // ::__m64 native_obj; 57 | // } __attribute__((aligned(8), packed)); 58 | // } 59 | // /* It seems make pe::__m64 implicitly convert to ::__m64 a bad idea, reasons: 60 | // * (1) the compiler do the implicit conversion at most once, e.g following 61 | // not 62 | // * work: 63 | // * class Foo 64 | // * { 65 | // * public: 66 | // * Foo(::__m64); 67 | // * }; 68 | // * void g(Foo) {} 69 | // * pe::__m64 x; 70 | // * g(x); // error, need 2 implicit conversion to work 71 | // * (2) it may fails on compound types, e.g: 72 | // * void f(::__m64* ); 73 | // * pe::__m64 x; 74 | // * f(&x); 75 | // * 76 | // * What' more, should make __m64 takes trial constructor(i.e do not provide 77 | // * default one), otherwise it can not used in anonymous unions, e.g: 78 | // * struct Foo{ 79 | // * union{ 80 | // * int foo; 81 | // * __m64 bar; 82 | // * }; // gcc complains if __m64 has constructor 83 | // * As a consequence, we can not supply any constructors at all. 84 | // * 85 | // * What we need to do is define pe::__m64 to be able to mimic ::_m64 86 | // * completely, i.e define all necessary intrinsic functions on pe::__m64 87 | // * 88 | // * The same reason holds for other types. 89 | // */ 90 | 91 | /* Problems with above approach : 92 | * (1) conflict with global definition, we can solve it by using #define, e.g 93 | * void _pi_mm_blah(...) { _mm_blah(...) } 94 | * #define _mm_blah _pi_mm_blah 95 | * (2) some intrinsic functions are implemented as macro, use clang to parse is 96 | * not easy 97 | */ 98 | 99 | /* 100 | * It seems using macro gives an easy and portable solution! 101 | */ 102 | namespace 103 | { 104 | static_assert(sizeof(char) == 1, "sizeof(char) != 1"); 105 | static_assert(sizeof(short) == 2, "sizeof(short) != 2"); 106 | static_assert(sizeof(int) == 4, "sizeof(int) != 4"); 107 | static_assert(sizeof(long) == 8, "sizeof(long) != 8"); 108 | 109 | static_assert(sizeof(float) == 4, "sizeof(float) != 4"); 110 | static_assert(sizeof(double) == 8, "sizeof(double) != 8"); 111 | 112 | typedef signed char int8; 113 | typedef unsigned char uint8; 114 | typedef short int16; 115 | typedef unsigned short uint16; 116 | typedef int int32; 117 | typedef unsigned int uint32; 118 | typedef long int64; 119 | typedef unsigned long uint64; 120 | } 121 | 122 | union _pi__m64 { 123 | uint64 m64_u64; 124 | float m64_f32[2]; 125 | int8 m64_i8[8]; 126 | int16 m64_i16[4]; 127 | int32 m64_i32[2]; 128 | int64 m64_i64; 129 | uint8 m64_u8[8]; 130 | uint16 m64_u16[4]; 131 | uint32 m64_u32[2]; 132 | 133 | __m64 native_obj; 134 | } __attribute__((aligned(8), packed)); 135 | 136 | union _pi__m128 { 137 | float m128_f32[4]; 138 | uint64 m128_u64[2]; 139 | int8 m128_i8[16]; 140 | int16 m128_i16[8]; 141 | int32 m128_i32[4]; 142 | int64 m128_i64[2]; 143 | uint8 m128_u8[16]; 144 | uint16 m128_u16[8]; 145 | uint32 m128_u32[4]; 146 | 147 | __m128 native_obj; 148 | } __attribute__((aligned(16), packed)); 149 | 150 | union _pi__m128i { 151 | int8 m128i_i8[16]; 152 | int16 m128i_i16[8]; 153 | int32 m128i_i32[4]; 154 | int64 m128i_i64[2]; 155 | uint8 m128i_u8[16]; 156 | uint16 m128i_u16[8]; 157 | uint32 m128i_u32[4]; 158 | uint64 m128i_u64[2]; 159 | 160 | __m128i native_obj; 161 | } __attribute__((aligned(16), packed)); 162 | 163 | union _pi__m128d { 164 | double m128d_f64[2]; 165 | __m128d native_obj; 166 | } __attribute__((aligned(16), packed)); 167 | 168 | inline _pi__m64 from_native_obj(const __m64 &x) 169 | { 170 | _pi__m64 y; 171 | y.native_obj = x; 172 | return y; 173 | } 174 | 175 | inline _pi__m128 from_native_obj(const __m128 &x) 176 | { 177 | _pi__m128 y; 178 | y.native_obj = x; 179 | return y; 180 | } 181 | 182 | inline _pi__m128i from_native_obj(const __m128i &x) 183 | { 184 | _pi__m128i y; 185 | y.native_obj = x; 186 | return y; 187 | } 188 | 189 | inline _pi__m128d from_native_obj(const __m128d &x) 190 | { 191 | _pi__m128d y; 192 | y.native_obj = x; 193 | return y; 194 | } 195 | #endif // WIN32 196 | #include "sse_impl.h" 197 | #endif 198 | -------------------------------------------------------------------------------- /PlatformIndependence/windows_thread_wrapper.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of EIBA. 3 | * 4 | * Copyright (C) 2017 Zhejiang University 5 | * For more information see 6 | * 7 | * Licensed under the Apache License, Version 2.0 (the "License"); 8 | * You may not use this file except in compliance with the License. 9 | * You may obtain a copy of the License at 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | * Unless required by applicable law or agreed to in writing, software 12 | * distributed under the License is distributed on an "AS IS" BASIS, 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | * 17 | */ 18 | 19 | #ifndef _WINDOWS_THREAD_WRAPPER_H_ 20 | #define _WINDOWS_THREAD_WRAPPER_H_ 21 | 22 | #ifndef WIN32 23 | #include 24 | 25 | #define WINAPI 26 | 27 | typedef void *DWORD; 28 | typedef unsigned long ULONG; 29 | typedef DWORD *LPDWORD; 30 | typedef void *LPVOID; 31 | typedef pthread_t HANDLE; 32 | typedef void *LPSECURITY_ATTRIBUTES; 33 | typedef DWORD(WINAPI *PTHREAD_START_ROUTINE)(LPVOID lpThreadParameter); 34 | typedef PTHREAD_START_ROUTINE LPTHREAD_START_ROUTINE; 35 | typedef bool BOOL; 36 | #define TRUE 1 37 | #define FALSE 0 38 | 39 | #define INFINITE 0xFFFFFFFF 40 | 41 | inline HANDLE CreateThread(LPSECURITY_ATTRIBUTES lpThreadAttributes, 42 | size_t dwStackSize, 43 | LPTHREAD_START_ROUTINE lpStartAddress, 44 | LPVOID lpParameter, ULONG dwCreationFlags, 45 | LPDWORD lpThreadId) 46 | { 47 | pthread_t pid; 48 | pthread_create(&pid, (pthread_attr_t *)lpThreadAttributes, 49 | (void *(*)(void *))lpStartAddress, lpParameter); 50 | return pid; 51 | } 52 | 53 | inline BOOL CloseHandle(HANDLE hObject) 54 | { 55 | return (pthread_detach(hObject) == 0); 56 | } 57 | #endif // WIN32 58 | #endif 59 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Efficient Incremental BA 2 | 3 | This source code provides the efficient incremental bundle adjustment implementation, which is part of our RKD-SLAM. 4 | 5 | ## 1. Introduction 6 | 7 | We present **RKD-SLAM**, a robust keyframe-based dense SLAM approach for an RGB-D camera that can robustly handle fast motion and dense loop closure, run without time limitation in a moderate size scene. For reducing accumulation error, we also introduce a very **efficient incremental bundle adjustment (EIBA)** algorithm, that can provide nearly the same solution as global BA, but with significantly less computation time, which is proportional to how many variables are actually changed. 8 | 9 | ## 2. Related Publications 10 | 11 | Haomin Liu, Chen Li, Guojun Chen, Guofeng Zhang, Michael Kaess and Hujun Bao. Robust Keyframe-based Dense SLAM with an RGB-D Camera [J]. arXiv preprint arXiv:1711.05166, 2017. [**[arXiv report](https://arxiv.org/abs/1711.05166)**]. 12 | 13 | 14 | ## 3. License 15 | 16 | EIBA is released under a [Apache License 2.0](https://www.apache.org/licenses/LICENSE-2.0). Please contact [Guofeng Zhang](mailto:zhangguofeng@cad.zju.edu.cn) if you have any questions. 17 | 18 | If you use this source code for your academic publication, please cite our paper: 19 | 20 | @article{LiuLCZKB2017, 21 | title={Robust Keyframe-based Dense SLAM with an RGB-D Camera}, 22 | author={Haomin Liu and Chen Li and Guojun Chen and Guofeng Zhang and Michael Kaess and Hujun Bao}, 23 | journal={arXiv preprint arXiv:1711.05166}, 24 | year={2017} 25 | } 26 | 27 | ## 4. Installation 28 | 29 | #### Dependencies 30 | - [yaml-cpp](https://github.com/jbeder/yaml-cpp) 31 | 32 | ```bash 33 | git clone https://github.com/jbeder/yaml-cpp.git 34 | cd yaml-cpp 35 | mkdir build && cd build 36 | cmake .. 37 | make -j4 38 | sudo make install 39 | ``` 40 | 41 | #### Environment 42 | 43 | The project has been tested in ubuntu 16.04. 44 | 45 | You can build and run example: 46 | 47 | ```bash 48 | cd /path/to/this/project 49 | mkdir build && cd build 50 | cmake .. 51 | make -j4 52 | ./ExamlpeYAML ../Data/ 53 | ``` 54 | 55 | ## 5. Usage 56 | 57 | We use inverse depth parameterization for 3D points, which means that all the feature points are parameterized in the form of inverse depth and the corresponding camera location from which they were first observed. The first observations are also called source features. In our program, we only optimize camera pose and source feature inverse depth. 58 | 59 | We provide an interface class `BAInterface`, which can be used to push feature measurements `PushKeyframeFeatures(*)`and frame constraints`PushBetweenFrameConstraint()`, then you can call `Optimize(*)` to run optimization and get optimized camera pose and inverse depths. 60 | 61 | Besides, we also provide a function `PushFrameInfoFromYAML(*)`, which can be used to load our example YAML format data files and push data to BA system. 62 | 63 | #### `PushKeyframeFeatures(*)` 64 | 65 | - Call this to push feature observations 66 | - Initial guess of current frame camera pose (rotation and position) 67 | - Initial guess of last frame source feature's inverse depth 68 | - Current frame source features (2D feature measurements ) 69 | - Measured features (matches) and covariance 70 | - The correspondence for measured features to the matched source features is indicated by matched keyframe index (globally) and matched source feature index (locally in its keyframe). 71 | 72 | #### `PushBetweenFrameConstraint(*)` 73 | 74 | - Call this to push frame constrains 75 | - two indices of frame constraint 76 | 77 | - relative pose of frame constraint![](http://latex.codecogs.com/gif.latex?T_{12})and covariance 78 | 79 | - The function minimize the energy of ![](http://latex.codecogs.com/gif.latex?C_1C_2^{-1}T_{12}) 80 | 81 | #### `Optimize(optKFs)` 82 | 83 | - Call this to run EIBA optimization 84 | - Optimized result would been stored in `optKFs` 85 | - Optimized keyframes and depths 86 | - if keyframes or depths have been updated by EIBA, the corresponding boolean mark would be set true. 87 | 88 | #### `SetParams(*)` 89 | 90 | - Call this to set optimization parameters 91 | - Please refer to the code comments for more informations. 92 | 93 | ### Notes 94 | 95 | - We use **rotation** and **postion** model for input and output camera pose, that means ![](http://latex.codecogs.com/gif.latex?\\mathbf{X}_c=R(\\mathbf{X}_w-\\mathbf{p})) , where![](http://latex.codecogs.com/gif.latex?\\mathbf{X}_c)) is the point in world frame, ![](http://latex.codecogs.com/gif.latex?\\mathbf{X}_w)) is the point in camera frame. 96 | - All the 2D features should have been undistorted and normalized to z=1 plane. 97 | - If measured depth is invalid, set the corrresponding inverse depth source observation`source_features[i].m_inv_depth` to 0. 98 | 99 | ### Example 100 | 101 | We provide an example `ExampleYAML.cpp` to call EIBA by providing YAML format frame data, which is generated by the tracking part of RKD-SLAM. This example data is recorded from TUM RGBD fr3/long_office_household, which contains 93 keyframes. 102 | 103 | Here is a simplified example of YAML data format: 104 | 105 | ```yaml 106 | # keyframe feature measurement 107 | features: 108 | # initial guess 109 | initial_guess: 110 | # current frame camera pose (rotation and position) 111 | current_camera_pose_guess: 112 | - [1, 0, 0, 0] 113 | - [0, 1, 0, 0] 114 | - [0, 0, 1, 0] 115 | # last frame inverse depth guess 116 | last_inv_depth_guess: 117 | - [0.5] 118 | # source features (first observation of features) 119 | # point2D (normalized in z=1 plane) and inverse depth 120 | source_features: 121 | - [0.2, -0.08, 0.3] 122 | # measured feature matches 123 | # matched source feature index is indicated by its keyframe index, and source feature index 124 | # kf_idx: matched keyframe index (globally) 125 | # ftr_idx: matched source feature index (locally) 126 | # pt: point2D (normalized in z=1 plane) and inverse depth 127 | # cov2: covariance 128 | measured_features: 129 | - kf_idx: 0 130 | ftr_idx: 15 131 | pt: [0.33, -0.42, 0.2] 132 | cov2: 133 | - [280000, 600] 134 | - [600, 280000] 135 | # between frame constraints 136 | # see README.md for more information 137 | # keyframe_index_1/2: two indices of frame constraint 138 | # relative_pose_Rp: T_{12} 139 | # cov6: covariance 140 | frame_constraints: 141 | - keyframe_index_1: 0 142 | keyframe_index_2: 1 143 | relative_pose_Rp: 144 | - [1, 0, 0, 0] 145 | - [0, 1, 0, 0] 146 | - [0, 0, 1, 0] 147 | cov6: 148 | - [3000, 0, 0, 0, 0, 0] 149 | - [0, 3000, 0, 0, 0, 0] 150 | - [0, 0, 3000, 0, 0, 0] 151 | - [0, 0, 0, 100, 0, 0] 152 | - [0, 0, 0, 0, 100, 0] 153 | - [0, 0, 0, 0, 0, 100] 154 | ``` 155 | -------------------------------------------------------------------------------- /Utility/AlignedMatrix.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of EIBA. 3 | * 4 | * Copyright (C) 2017 Zhejiang University 5 | * For more information see 6 | * 7 | * Licensed under the Apache License, Version 2.0 (the "License"); 8 | * You may not use this file except in compliance with the License. 9 | * You may obtain a copy of the License at 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | * Unless required by applicable law or agreed to in writing, software 12 | * distributed under the License is distributed on an "AS IS" BASIS, 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | * 17 | */ 18 | 19 | #ifndef _ALIGNED_MATRIX_H_ 20 | #define _ALIGNED_MATRIX_H_ 21 | 22 | #include "SSE.h" 23 | 24 | template class AlignedMatrixN 25 | { 26 | public: 27 | inline AlignedMatrixN() 28 | { 29 | m_own = true; 30 | m_data[0] = NULL; 31 | } 32 | inline ~AlignedMatrixN() 33 | { 34 | if (m_data[0] && m_own) SSE::Free(m_data[0]); 35 | } 36 | 37 | inline void Create() 38 | { 39 | if (m_data[0] && m_own) return; 40 | m_own = true; 41 | m_data[0] = SSE::Malloc(N); 42 | for (int r = 1; r < N; ++r) m_data[r] = m_data[r - 1] + N; 43 | } 44 | 45 | inline void Destroy() 46 | { 47 | if (m_data[0] && m_own) SSE::Free(m_data[0]); 48 | m_own = true; 49 | // m_data[0] = NULL; 50 | memset(m_data, 0, sizeof(m_data)); 51 | } 52 | 53 | template inline void Set(TYPE *V) { OWN ? Set_T(V) : Set_F(V); } 54 | inline void Set_T(TYPE *data) 55 | { 56 | Create(); 57 | memcpy(m_data[0], data, sizeof(TYPE) * N * N); 58 | } 59 | inline void Set_F(TYPE *data) 60 | { 61 | m_own = false; 62 | m_data[0] = data; 63 | for (int r = 1; r < N; ++r) m_data[r] = m_data[r - 1] + N; 64 | } 65 | 66 | inline const TYPE *operator[](const int y) const { return m_data[y]; } 67 | inline TYPE *operator[](const int y) { return m_data[y]; } 68 | protected: 69 | bool m_own; 70 | TYPE *m_data[N]; 71 | }; 72 | 73 | template class AlignedMatrixX 74 | { 75 | public: 76 | inline AlignedMatrixX(TYPE *data, const int Nr, const int Nc, 77 | const bool own = true) 78 | { 79 | m_own = own; 80 | m_Nr = Nr; 81 | m_Nc = Nc; 82 | m_data.resize(m_Nr); 83 | if (m_own) { 84 | const int N = m_Nr * m_Nc; 85 | m_data[0] = SSE::Malloc(N); 86 | memcpy(m_data[0], data, sizeof(TYPE) * N); 87 | } else 88 | m_data[0] = data; 89 | for (int r = 1; r < m_Nr; ++r) m_data[r] = m_data[r - 1] + m_Nc; 90 | } 91 | 92 | inline ~AlignedMatrixX() 93 | { 94 | if (!m_data.empty() && m_own) SSE::Free(m_data[0]); 95 | } 96 | 97 | inline const TYPE *operator[](const int y) const { return m_data[y]; } 98 | inline TYPE *operator[](const int y) { return m_data[y]; } 99 | protected: 100 | bool m_own; 101 | int m_Nr, m_Nc; 102 | std::vector m_data; 103 | }; 104 | 105 | #endif 106 | -------------------------------------------------------------------------------- /Utility/AlignedVector.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of EIBA. 3 | * 4 | * Copyright (C) 2017 Zhejiang University 5 | * For more information see 6 | * 7 | * Licensed under the Apache License, Version 2.0 (the "License"); 8 | * You may not use this file except in compliance with the License. 9 | * You may obtain a copy of the License at 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | * Unless required by applicable law or agreed to in writing, software 12 | * distributed under the License is distributed on an "AS IS" BASIS, 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | * 17 | */ 18 | 19 | #ifndef _ALIGNED_VECTOR_H_ 20 | #define _ALIGNED_VECTOR_H_ 21 | 22 | #include "Utility.h" 23 | 24 | template 25 | class AlignedVector 26 | { 27 | public: 28 | inline AlignedVector() 29 | { 30 | m_own = true; 31 | m_data = NULL; 32 | m_size = m_capacity = 0; 33 | } 34 | inline AlignedVector(const int N) 35 | { 36 | m_own = true; 37 | m_data = SSE::Malloc(N); 38 | m_size = m_capacity = N; 39 | } 40 | // inline AlignedVector(const AlignedVector &V) 41 | //{ 42 | // m_own = true; 43 | // m_data = NULL; 44 | // m_size = m_capacity = 0; 45 | // Set(V); 46 | //} 47 | inline AlignedVector(const AlignedVector &V) 48 | { 49 | m_own = true; 50 | m_data = NULL; 51 | m_size = m_capacity = 0; 52 | Set((TYPE *)V.Data(), V.Size()); 53 | } 54 | inline AlignedVector(TYPE *V, const int N, const bool own = true) 55 | { 56 | m_own = true; 57 | m_data = NULL; 58 | m_size = m_capacity = 0; 59 | if (own) 60 | Set(V, N); 61 | else 62 | Set(V, N); 63 | } 64 | inline ~AlignedVector() 65 | { 66 | if (m_data && m_own) SSE::Free(m_data); 67 | } 68 | // inline void operator = (const AlignedVector &V) 69 | // { Set(V); } 70 | inline void operator=(AlignedVector &V) 71 | { 72 | Set(V.Data(), V.Size()); 73 | } 74 | inline void Resize(const int N, const bool retain = false) 75 | { 76 | if (N <= m_capacity) 77 | m_size = N; 78 | else { 79 | // if(m_data && m_own) 80 | // SSE::Free(m_data); 81 | // else 82 | // m_own = true; 83 | // m_data = SSE::Malloc(N); 84 | 85 | TYPE *dataBkp = m_data; 86 | m_data = SSE::Malloc(N); 87 | if (dataBkp) { 88 | if (retain) memcpy(m_data, dataBkp, sizeof(TYPE) * m_size); 89 | if (m_own) SSE::Free(dataBkp); 90 | } 91 | m_own = true; 92 | 93 | m_size = m_capacity = N; 94 | } 95 | } 96 | inline void Reserve(const int N) 97 | { 98 | Clear(); 99 | m_data = SSE::Malloc(N); 100 | m_capacity = N; 101 | } 102 | inline void Clear() 103 | { 104 | if (m_data && m_own) SSE::Free(m_data); 105 | m_own = true; 106 | m_data = NULL; 107 | m_size = m_capacity = 0; 108 | } 109 | inline bool Empty() const { return m_size == 0; } 110 | inline void Push(const TYPE &v) 111 | { 112 | if (m_size == m_capacity) { 113 | const int growth = 114 | GROWTH == 0 ? std::max(std::min(m_capacity, GROWTH_MAX), 1) 115 | : GROWTH; 116 | m_capacity += growth; 117 | TYPE *dataBkp = m_data; 118 | m_data = SSE::Malloc(m_capacity); 119 | if (dataBkp) { 120 | memcpy(m_data, dataBkp, sizeof(TYPE) * m_size); 121 | if (m_own) SSE::Free(dataBkp); 122 | } 123 | m_data[m_size++] = v; 124 | m_own = true; 125 | } else 126 | m_data[m_size++] = v; 127 | } 128 | inline void Push(const TYPE *V, const int N) 129 | { 130 | if (m_size + N > m_capacity) { 131 | const int growth = N + GROWTH - (GROWTH % N); 132 | m_capacity += growth; 133 | TYPE *dataBkp = m_data; 134 | m_data = SSE::Malloc(m_capacity); 135 | if (dataBkp) { 136 | memcpy(m_data, dataBkp, sizeof(TYPE) * m_size); 137 | if (m_own) SSE::Free(dataBkp); 138 | } 139 | m_own = true; 140 | } 141 | memcpy(m_data + m_size, V, sizeof(TYPE) * N); 142 | m_size += N; 143 | } 144 | inline void Insert(const int i, const int N, 145 | AlignedVector &VTmp) 146 | { 147 | VTmp.Set(Data() + i, Size() - i); 148 | Resize(i + N, true); 149 | Push(VTmp.Data(), VTmp.Size()); 150 | } 151 | inline void Insert(const int i, const TYPE &v, 152 | AlignedVector &VTmp) 153 | { 154 | VTmp.Set(Data() + i, Size() - i); 155 | Resize(i + 1, true); 156 | m_data[i] = v; 157 | Push(VTmp.Data(), VTmp.Size()); 158 | } 159 | inline void Erase(const int i) 160 | { 161 | for (int j = i; j < m_size; ++j) m_data[j] = m_data[j + 1]; 162 | Resize(m_size - 1); 163 | } 164 | inline void MakeZero() { memset(Data(), 0, sizeof(TYPE) * Size()); } 165 | inline void MakeZero(const int i1, const int i2) 166 | { 167 | memset(Data() + i1, 0, sizeof(TYPE) * (i2 - i1)); 168 | } 169 | template inline void Set(TYPE *V, const int N) 170 | { 171 | OWN ? Set_True(V, N) : Set_False(V, N); 172 | } 173 | inline void Set_True(TYPE *V, const int N) 174 | { 175 | Resize(N); 176 | memcpy(Data(), V, sizeof(TYPE) * N); 177 | } 178 | inline void Set_False(TYPE *V, const int N) 179 | { 180 | if (m_data && m_own) SSE::Free(m_data); 181 | m_data = V; 182 | m_size = m_capacity = N; 183 | m_own = false; 184 | } 185 | inline void Set(const AlignedVector &V) 186 | { 187 | const int N = V.Size(); 188 | Resize(N); 189 | memcpy(m_data, V.Data(), sizeof(TYPE) * N); 190 | } 191 | inline void Set(const AlignedVector &V, 192 | const std::vector &iVs) 193 | { 194 | const int N = int(iVs.size()); 195 | Resize(N); 196 | for (int i = 0; i < N; ++i) m_data[i] = V[iVs[i]]; 197 | } 198 | inline void Get(TYPE *V) const { memcpy(V, m_data, sizeof(TYPE) * m_size); } 199 | inline void Swap(AlignedVector &V) 200 | { 201 | UT_SWAP(m_own, V.m_own); 202 | UT_SWAP(m_data, V.m_data); 203 | UT_SWAP(m_size, V.m_size); 204 | UT_SWAP(m_capacity, V.m_capacity); 205 | } 206 | inline void Concatenate(const AlignedVector &V1, 207 | const AlignedVector &V2) 208 | { 209 | Resize(V1.Size() + V2.Size()); 210 | memcpy(m_data, V1.Data(), sizeof(TYPE) * V1.Size()); 211 | memcpy(m_data + V1.Size(), V2.Data(), sizeof(TYPE) * V2.Size()); 212 | } 213 | 214 | inline bool 215 | operator==(const AlignedVector &V) const 216 | { 217 | return Size() == V.Size() && UT::VectorEqual(Data(), V.Data(), Size()); 218 | } 219 | inline const TYPE &operator[](const int i) const 220 | { 221 | #ifdef CFG_DEBUG 222 | UT_ASSERT(i >= 0 && i < m_size); 223 | #endif 224 | return m_data[i]; 225 | } 226 | inline TYPE &operator[](const int i) 227 | { 228 | #ifdef CFG_DEBUG 229 | UT_ASSERT(i >= 0 && i < m_size); 230 | #endif 231 | return m_data[i]; 232 | } 233 | inline const TYPE *Data() const { return m_data; } 234 | inline TYPE *Data() { return m_data; } 235 | inline const TYPE &Back() const 236 | { 237 | #ifdef CFG_DEBUG 238 | UT_ASSERT(m_size != 0); 239 | #endif 240 | return m_data[m_size - 1]; 241 | } 242 | inline TYPE &Back() 243 | { 244 | #ifdef CFG_DEBUG 245 | UT_ASSERT(m_size != 0); 246 | #endif 247 | return m_data[m_size - 1]; 248 | } 249 | 250 | inline const TYPE *End() const { return m_data + m_size; } 251 | inline TYPE *End() { return m_data + m_size; } 252 | inline int Size() const { return m_size; } 253 | inline void SaveB(FILE *fp) const 254 | { 255 | UT::SaveB(m_size, fp); 256 | UT::SaveB(m_data, m_size, fp); 257 | } 258 | inline void LoadB(FILE *fp) 259 | { 260 | const int N = UT::LoadB(fp); 261 | Resize(N); 262 | UT::LoadB(m_data, N, fp); 263 | } 264 | inline bool SaveB(const std::string fileName) const 265 | { 266 | FILE *fp = fopen(fileName.c_str(), "wb"); 267 | if (!fp) return false; 268 | SaveB(fp); 269 | fclose(fp); 270 | UT::PrintSaved(fileName); 271 | return true; 272 | } 273 | inline bool LoadB(const std::string fileName) 274 | { 275 | FILE *fp = fopen(fileName.c_str(), "rb"); 276 | if (!fp) return false; 277 | LoadB(fp); 278 | fclose(fp); 279 | UT::PrintLoaded(fileName); 280 | return true; 281 | } 282 | 283 | inline float MemoryMB() const 284 | { 285 | return m_own ? 0 : UT::MemoryMB(m_capacity); 286 | } 287 | 288 | protected: 289 | bool m_own; 290 | TYPE *m_data; 291 | int m_size, m_capacity; 292 | }; 293 | 294 | #endif 295 | -------------------------------------------------------------------------------- /Utility/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(Utility Utility.cpp) 2 | target_link_libraries(Utility 3 | ${Boost_LIBRARIES} 4 | ) 5 | -------------------------------------------------------------------------------- /Utility/Candidate.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of EIBA. 3 | * 4 | * Copyright (C) 2017 Zhejiang University 5 | * For more information see 6 | * 7 | * Licensed under the Apache License, Version 2.0 (the "License"); 8 | * You may not use this file except in compliance with the License. 9 | * You may obtain a copy of the License at 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | * Unless required by applicable law or agreed to in writing, software 12 | * distributed under the License is distributed on an "AS IS" BASIS, 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | * 17 | */ 18 | 19 | #ifndef _CANDIDATE_H_ 20 | #define _CANDIDATE_H_ 21 | 22 | #include "stdafx.h" 23 | 24 | #include 25 | 26 | template class Candidate 27 | { 28 | public: 29 | inline Candidate() {} 30 | inline Candidate(const int idx, const TYPE score) { Set(idx, score); } 31 | inline void Set(const int idx, const TYPE score) 32 | { 33 | m_idx = idx; 34 | m_score = score; 35 | } 36 | inline void Get(int &idx, TYPE &score) const 37 | { 38 | idx = m_idx; 39 | score = m_score; 40 | } 41 | inline bool operator<(const Candidate &c) const 42 | { 43 | return m_score < c.m_score; 44 | } 45 | inline bool operator>(const Candidate &c) const 46 | { 47 | return m_score > c.m_score; 48 | } 49 | static inline void MakeZero(const int N, 50 | std::vector> &candidates) 51 | { 52 | candidates.resize(N); 53 | for (int i = 0; i < N; ++i) candidates[i].Set(i, 0); 54 | } 55 | static inline void RemoveZero(std::vector> &candidates) 56 | { 57 | int i, j; 58 | const int N = int(candidates.size()); 59 | for (i = j = 0; i < N; ++i) { 60 | if (candidates[i].m_score != 0) candidates[j++] = candidates[i]; 61 | } 62 | candidates.resize(j); 63 | } 64 | static inline void SortAscending(std::vector> &candidates) 65 | { 66 | std::sort(candidates.begin(), candidates.end()); 67 | } 68 | static inline void SortDescending(std::vector> &candidates) 69 | { 70 | std::sort(candidates.begin(), candidates.end(), 71 | std::greater>()); 72 | } 73 | static inline void 74 | MarkCandidates(const std::vector> &candidates, 75 | std::vector &marks, const int N, const ubyte mark) 76 | { 77 | #ifdef CFG_DEBUG 78 | UT_ASSERT(N < int(candidates.size())); 79 | #endif 80 | for (int i = 0; i < N; ++i) marks[candidates[i].m_idx] |= mark; 81 | } 82 | // static inline void MarkCandidates(const std::vector > 83 | // &candidates, std::vector &marks, const int N1, const ubyte mark1, 84 | // const ubyte mark2) 85 | //{ 86 | // const int N = int(candidates.size()), _N1 = std::min(N1, N); 87 | // MarkCandidates(candidates, marks, _N1, mark1); 88 | // for(int i = _N1; i < N; ++i) 89 | // marks[candidates[i].m_idx] = mark2; 90 | //} 91 | public: 92 | int m_idx; 93 | TYPE m_score; 94 | }; 95 | 96 | #endif 97 | -------------------------------------------------------------------------------- /Utility/Configurator.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of EIBA. 3 | * 4 | * Copyright (C) 2017 Zhejiang University 5 | * For more information see 6 | * 7 | * Licensed under the Apache License, Version 2.0 (the "License"); 8 | * You may not use this file except in compliance with the License. 9 | * You may obtain a copy of the License at 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | * Unless required by applicable law or agreed to in writing, software 12 | * distributed under the License is distributed on an "AS IS" BASIS, 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | * 17 | */ 18 | 19 | #ifndef _CONFIGURATOR_H_ 20 | #define _CONFIGURATOR_H_ 21 | 22 | #include 23 | 24 | #include "Utility.h" 25 | 26 | class Configurator 27 | { 28 | public: 29 | inline Configurator() {} 30 | inline Configurator(const char *fileName) { Load(fileName); } 31 | inline std::string GetArgument(const std::string directive, 32 | const std::string default_val = "") const 33 | { 34 | const DirectiveArgumentMap::const_iterator it = 35 | m_directiveArgumentMap.find(directive); 36 | if (it == m_directiveArgumentMap.end()) 37 | return default_val; 38 | else 39 | return it->second; 40 | } 41 | inline int GetArgument(const std::string directive, const int default_val) const 42 | { 43 | const DirectiveArgumentMap::const_iterator it = 44 | m_directiveArgumentMap.find(directive); 45 | if (it == m_directiveArgumentMap.end()) 46 | return default_val; 47 | else 48 | return atoi(it->second.c_str()); 49 | } 50 | inline float GetArgument(const std::string directive, 51 | const float default_val) const 52 | { 53 | const DirectiveArgumentMap::const_iterator it = 54 | m_directiveArgumentMap.find(directive); 55 | if (it == m_directiveArgumentMap.end()) 56 | return default_val; 57 | else 58 | return float(atof(it->second.c_str())); 59 | } 60 | inline double GetArgument(const std::string directive, 61 | const double default_val) const 62 | { 63 | const DirectiveArgumentMap::const_iterator it = 64 | m_directiveArgumentMap.find(directive); 65 | if (it == m_directiveArgumentMap.end()) 66 | return default_val; 67 | else 68 | return double(atof(it->second.c_str())); 69 | } 70 | 71 | inline void SetArgument(const std::string directive, 72 | const std::string argument) 73 | { 74 | DirectiveArgumentMap::iterator it = 75 | m_directiveArgumentMap.find(directive); 76 | if (it == m_directiveArgumentMap.end()) 77 | m_directiveArgumentMap.insert( 78 | DirectiveArgumentMap::value_type(directive, argument)); 79 | else 80 | it->second = argument; 81 | } 82 | inline void SetArgument(const std::string directive, const int argument) 83 | { 84 | char buf[UT_STRING_MAX_LENGTH]; 85 | sprintf(buf, "%d", argument); 86 | DirectiveArgumentMap::iterator it = 87 | m_directiveArgumentMap.find(directive); 88 | if (it == m_directiveArgumentMap.end()) 89 | m_directiveArgumentMap.insert( 90 | DirectiveArgumentMap::value_type(directive, buf)); 91 | else 92 | it->second = buf; 93 | } 94 | inline void SetArgument(const std::string directive, const float argument) 95 | { 96 | char buf[UT_STRING_MAX_LENGTH]; 97 | sprintf(buf, "%f", argument); 98 | DirectiveArgumentMap::iterator it = 99 | m_directiveArgumentMap.find(directive); 100 | if (it == m_directiveArgumentMap.end()) 101 | m_directiveArgumentMap.insert( 102 | DirectiveArgumentMap::value_type(directive, buf)); 103 | else 104 | it->second = buf; 105 | } 106 | inline void SetArgument(const std::string directive, const double argument) 107 | { 108 | char buf[UT_STRING_MAX_LENGTH]; 109 | sprintf(buf, "%f", argument); 110 | DirectiveArgumentMap::iterator it = 111 | m_directiveArgumentMap.find(directive); 112 | if (it == m_directiveArgumentMap.end()) 113 | m_directiveArgumentMap.insert( 114 | DirectiveArgumentMap::value_type(directive, buf)); 115 | else 116 | it->second = buf; 117 | } 118 | 119 | inline bool Load(const char *fileName) 120 | { 121 | m_directiveArgumentMap.clear(); 122 | FILE *fp = fopen(fileName, "r"); 123 | if (!fp) return false; 124 | char buf[UT_STRING_MAX_LENGTH]; 125 | while (fgets(buf, UT_STRING_MAX_LENGTH, fp)) { 126 | int len = int(strlen(buf)); 127 | int i, j; 128 | for (i = j = 0; i < len; i++) { 129 | if (buf[i] != 10 && buf[i] != ' ') buf[j++] = buf[i]; 130 | } 131 | len = j; 132 | 133 | buf[len] = 0; 134 | if (len < 2 || buf[0] == '/' && buf[1] == '/') continue; 135 | const std::string directive = strtok(buf, "="); 136 | const char *argument = strtok(NULL, "="); 137 | if (!argument) continue; 138 | m_directiveArgumentMap.insert( 139 | DirectiveArgumentMap::value_type(directive, argument)); 140 | } 141 | fclose(fp); 142 | UT::PrintLoaded(fileName); 143 | return true; 144 | } 145 | 146 | void Print() const 147 | { 148 | UT::Print("[Configurator]\n"); 149 | for (DirectiveArgumentMap::const_iterator it = 150 | m_directiveArgumentMap.begin(); 151 | it != m_directiveArgumentMap.end(); it++) 152 | UT::Print(" %s = %s\n", it->first.c_str(), it->second.c_str()); 153 | } 154 | 155 | private: 156 | typedef std::map DirectiveArgumentMap; 157 | DirectiveArgumentMap m_directiveArgumentMap; 158 | }; 159 | 160 | #endif 161 | -------------------------------------------------------------------------------- /Utility/MultiThread.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of EIBA. 3 | * 4 | * Copyright (C) 2017 Zhejiang University 5 | * For more information see 6 | * 7 | * Licensed under the Apache License, Version 2.0 (the "License"); 8 | * You may not use this file except in compliance with the License. 9 | * You may obtain a copy of the License at 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | * Unless required by applicable law or agreed to in writing, software 12 | * distributed under the License is distributed on an "AS IS" BASIS, 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | * 17 | */ 18 | 19 | #include "PlatformIndependence/sse.h" 20 | #include "PlatformIndependence/windows_thread_wrapper.h" 21 | 22 | #ifndef _MULTI_THREAD_H_ 23 | #define _MULTI_THREAD_H_ 24 | 25 | #include "stdafx.h" 26 | 27 | //#include 28 | //#include 29 | //#include 30 | #include 31 | #include 32 | 33 | #include "Utility.h" 34 | 35 | //#define MT_SCOPE_LOCK_BEGIN(m) \ 36 | // { \ 37 | // boost::mutex::scoped_lock sl(m); 38 | //#define MT_SCOPE_LOCK_END(m) } 39 | //#define MT_READ_LOCK_BEGIN(m) \ 40 | // { \ 41 | // boost::shared_lock rl(m); 42 | //#define MT_READ_LOCK_END(m) } 43 | //#define MT_WRITE_LOCK_BEGIN(m) \ 44 | // { \ 45 | // boost::upgrade_lock wl1(m); \ 46 | // boost::upgrade_to_unique_lock wl2(wl1); 47 | //#define MT_WRITE_LOCK_END(m) } 48 | 49 | #define MT_SCOPE_LOCK_BEGIN(m) { std::lock_guard lock{m} 50 | #define MT_SCOPE_LOCK_END(m) } 51 | #define MT_READ_LOCK_BEGIN(m) { std::shared_lock rl(m); 52 | #define MT_READ_LOCK_END(m) } 53 | #define MT_WRITE_LOCK_BEGIN(m) { std::unique_lock wl(m); 54 | #define MT_WRITE_LOCK_END(m) } 55 | 56 | namespace MT 57 | { 58 | class Thread 59 | { 60 | public: 61 | virtual void Initialize(const bool serial = false, 62 | const std::string name = "") 63 | { 64 | m_serial = serial; 65 | m_name = name; 66 | } 67 | 68 | virtual void Reset() 69 | { 70 | MT_WRITE_LOCK_BEGIN(m_runMT); 71 | m_run = 0; 72 | MT_WRITE_LOCK_END(m_runMT); 73 | } 74 | 75 | virtual void Start() 76 | { 77 | Reset(); 78 | #ifdef CFG_SERIAL 79 | if (!m_serial) 80 | #endif 81 | CloseHandle(CreateThread(NULL, 0, Run, this, 0, NULL)); 82 | } 83 | 84 | virtual void Stop() 85 | { 86 | #ifdef CFG_SERIAL 87 | if (m_serial) return; 88 | #endif 89 | // UT::Print("[%s] Stop1\n", m_name.c_str()); 90 | MT_WRITE_LOCK_BEGIN(m_runMT); 91 | if (m_run == -2) return; 92 | m_run = -1; 93 | m_runCDT.notify_all(); 94 | MT_WRITE_LOCK_END(m_runMT); 95 | 96 | MT_READ_LOCK_BEGIN(m_runMT); 97 | while (m_run != -2) m_runCDT.wait(rl); 98 | MT_READ_LOCK_END(m_runMT); 99 | // UT::Print("[%s] Stop2\n", m_name.c_str()); 100 | } 101 | 102 | virtual bool WakeUp() 103 | { 104 | #ifdef CFG_SERIAL 105 | if (m_serial) { 106 | Run(); 107 | return true; 108 | } 109 | #endif 110 | MT_WRITE_LOCK_BEGIN(m_runMT); 111 | if (m_run != 0) return false; 112 | m_run = 1; 113 | m_runCDT.notify_one(); 114 | MT_WRITE_LOCK_END(m_runMT); 115 | return true; 116 | } 117 | 118 | virtual void Run() = 0; 119 | static inline DWORD WINAPI Run(LPVOID pParam) 120 | { 121 | Thread *pThread = (Thread *)pParam; 122 | // UT::Print("[%s] Run1\n", pThread->m_name.c_str()); 123 | while (1) { 124 | MT_READ_LOCK_BEGIN(pThread->m_runMT); 125 | while (pThread->m_run == 0) pThread->m_runCDT.wait(rl); 126 | if (pThread->m_run == -1) break; 127 | MT_READ_LOCK_END(pThread->m_runMT); 128 | 129 | pThread->Run(); 130 | 131 | MT_WRITE_LOCK_BEGIN(pThread->m_runMT); 132 | pThread->m_run = 0; 133 | #ifdef CFG_VIEW 134 | pThread->m_runCDT.notify_one(); 135 | #endif 136 | MT_WRITE_LOCK_END(pThread->m_runMT); 137 | } 138 | // UT::Print("[%s] Run2\n", pThread->m_name.c_str()); 139 | MT_WRITE_LOCK_BEGIN(pThread->m_runMT); 140 | pThread->m_run = -2; 141 | pThread->m_runCDT.notify_all(); 142 | MT_WRITE_LOCK_END(pThread->m_runMT); 143 | return 0; 144 | } 145 | 146 | virtual bool Running() 147 | { 148 | MT_READ_LOCK_BEGIN(m_runMT); 149 | return m_run == 1; 150 | MT_READ_LOCK_END(m_runMT); 151 | } 152 | 153 | #ifdef CFG_VIEW 154 | virtual void Synchronize() 155 | { 156 | MT_READ_LOCK_BEGIN(m_runMT); 157 | while (m_run == 1) m_runCDT.wait(rl); 158 | MT_READ_LOCK_END(m_runMT); 159 | } 160 | #endif 161 | 162 | protected: 163 | int m_run; 164 | // boost::shared_mutex m_runMT; 165 | std::shared_timed_mutex m_runMT; 166 | // boost::condition m_runCDT; 167 | std::condition_variable_any m_runCDT; 168 | 169 | bool m_serial; 170 | std::string m_name; 171 | }; 172 | } 173 | 174 | #endif 175 | -------------------------------------------------------------------------------- /Utility/Timer.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of EIBA. 3 | * 4 | * Copyright (C) 2017 Zhejiang University 5 | * For more information see 6 | * 7 | * Licensed under the Apache License, Version 2.0 (the "License"); 8 | * You may not use this file except in compliance with the License. 9 | * You may obtain a copy of the License at 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | * Unless required by applicable law or agreed to in writing, software 12 | * distributed under the License is distributed on an "AS IS" BASIS, 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | * 17 | */ 18 | 19 | #ifndef _TIMER_H_ 20 | #define _TIMER_H_ 21 | 22 | #ifndef WIN32 23 | #include 24 | #endif 25 | 26 | #include "Utility.h" 27 | 28 | class Timer 29 | { 30 | public: 31 | inline void Reset(const int N = 1) 32 | { 33 | m_first = true; 34 | m_idx = 0; 35 | m_start = m_avg = 0.0; 36 | m_times.assign(N, 0.0); 37 | } 38 | inline void Start() { m_start = GetTime(); } 39 | inline void Stop() 40 | { 41 | m_times[m_idx] = GetTime() - m_start + m_times[m_idx]; 42 | } 43 | inline void Finish() 44 | { 45 | ++m_idx; 46 | m_avg = 0.0; 47 | if (m_first) { 48 | for (int i = 0; i < m_idx; ++i) m_avg = m_times[i] + m_avg; 49 | m_avg /= m_idx; 50 | } else { 51 | const int N = int(m_times.size()); 52 | for (int i = 0; i < N; ++i) m_avg = m_times[i] + m_avg; 53 | m_avg /= N; 54 | } 55 | if (m_idx == m_times.size()) { 56 | m_first = false; 57 | m_idx = 0; 58 | } 59 | m_times[m_idx] = 0.0; 60 | } 61 | inline double GetAverageSeconds() const { return m_avg; } 62 | inline int GetAverageMilliseconds() const 63 | { 64 | return int(m_avg * 1000.0 + 0.5); 65 | } 66 | inline double GetFPS() const 67 | { 68 | return m_avg == 0.0 ? 0.0 : 1 / GetAverageSeconds(); 69 | } 70 | 71 | inline void SaveB(FILE *fp) const 72 | { 73 | UT::SaveB(m_first, fp); 74 | UT::SaveB(m_idx, fp); 75 | UT::SaveB(m_start, fp); 76 | UT::SaveB(m_avg, fp); 77 | UT::VectorSaveB(m_times, fp); 78 | } 79 | inline void LoadB(FILE *fp) 80 | { 81 | UT::LoadB(m_first, fp); 82 | UT::LoadB(m_idx, fp); 83 | UT::LoadB(m_start, fp); 84 | UT::LoadB(m_avg, fp); 85 | UT::VectorLoadB(m_times, fp); 86 | } 87 | 88 | inline static double GetTime() 89 | { 90 | #ifndef WIN32 91 | struct timeval time; 92 | gettimeofday(&time, NULL); 93 | return time.tv_sec + (time.tv_usec / 1000000.0); 94 | 95 | #else 96 | SYSTEMTIME systime; 97 | GetSystemTime(&systime); 98 | return 0.001 * double(systime.wMilliseconds) + 99 | 1.0 * double(systime.wSecond) + 60.0 * double(systime.wMinute) + 100 | 3600.0 * double(systime.wHour); 101 | #endif 102 | } 103 | 104 | private: 105 | bool m_first; 106 | int m_idx; 107 | double m_start, m_avg; 108 | std::vector m_times; 109 | }; 110 | 111 | #endif 112 | -------------------------------------------------------------------------------- /stdafx.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of EIBA. 3 | * 4 | * Copyright (C) 2017 Zhejiang University 5 | * For more information see 6 | * 7 | * Licensed under the Apache License, Version 2.0 (the "License"); 8 | * You may not use this file except in compliance with the License. 9 | * You may obtain a copy of the License at 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | * Unless required by applicable law or agreed to in writing, software 12 | * distributed under the License is distributed on an "AS IS" BASIS, 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | * 17 | */ 18 | 19 | // stdafx.h : include file for standard system include files, 20 | // or project specific include files that are used frequently, but 21 | // are changed infrequently 22 | // 23 | 24 | #pragma once 25 | 26 | #ifdef WIN32 27 | #include "targetver.h" 28 | #include 29 | #endif 30 | 31 | #include 32 | 33 | #define CFG_SERIAL 34 | #define CFG_VERBOSE 35 | 36 | #ifdef WIN32 37 | //#define CFG_VIEW 38 | //#define CFG_TEST 39 | #define CFG_DEPTH_MAP 40 | #define CFG_GROUND_TRUTH 41 | #define CFG_TUNE_PARAMETERS 42 | //#define CFG_DEBUG 43 | #ifdef CFG_DEBUG 44 | #define CFG_DEBUG_EIGEN 45 | #endif 46 | #endif 47 | 48 | #define CFG_VIEW 49 | #define CFG_DEPTH_MAP 50 | 51 | 52 | // TODO: reference additional headers your program requires here 53 | #include 54 | #include 55 | #include 56 | #include 57 | #include 58 | #include 59 | #include 60 | #include 61 | #include 62 | #ifdef WIN32 63 | #include 64 | #include 65 | #endif 66 | #include 67 | 68 | typedef unsigned char ubyte; 69 | typedef unsigned short ushort; 70 | typedef unsigned int uint; 71 | typedef unsigned long long ullong; 72 | 73 | #ifdef max 74 | #undef max 75 | #endif 76 | #ifdef min 77 | #undef min 78 | #endif 79 | #ifdef small 80 | #undef small 81 | #endif 82 | #ifdef _C2 83 | #undef _C2 84 | #endif 85 | #ifdef _T 86 | #undef _T 87 | #endif 88 | #ifdef ERROR 89 | #undef ERROR 90 | #endif 91 | --------------------------------------------------------------------------------