├── .gitignore ├── Cargo.toml ├── LICENSE ├── README.md ├── benches └── mean_pyramid.rs ├── examples ├── README.md ├── candidates_coarse-to-fine.rs ├── candidates_dso.rs ├── dataset_tum-read-associations.rs ├── dataset_tum-read-trajectory.rs ├── optim_affine-2d.rs ├── optim_regression-1d.rs └── optim_rosenbrock.rs └── src ├── bin └── vors_track.rs ├── core ├── camera.rs ├── candidates │ ├── coarse_to_fine.rs │ ├── dso.rs │ └── mod.rs ├── gradient.rs ├── inverse_depth.rs ├── mod.rs ├── multires.rs └── track │ ├── inverse_compositional.rs │ ├── lm_optimizer.rs │ └── mod.rs ├── dataset ├── mod.rs └── tum_rgbd.rs ├── lib.rs ├── math ├── mod.rs ├── optimizer.rs ├── se3.rs └── so3.rs └── misc ├── colormap.rs ├── helper.rs ├── interop.rs ├── mod.rs ├── type_aliases.rs └── view.rs /.gitignore: -------------------------------------------------------------------------------- 1 | data/ 2 | out/ 3 | 4 | 5 | # Created by https://www.gitignore.io/api/vim,osx,rust,linux,windows 6 | # Edit at https://www.gitignore.io/?templates=vim,osx,rust,linux,windows 7 | 8 | ### Linux ### 9 | *~ 10 | 11 | # temporary files which can be created if a process still has a handle open of a deleted file 12 | .fuse_hidden* 13 | 14 | # KDE directory preferences 15 | .directory 16 | 17 | # Linux trash folder which might appear on any partition or disk 18 | .Trash-* 19 | 20 | # .nfs files are created when an open file is removed but is still being accessed 21 | .nfs* 22 | 23 | ### OSX ### 24 | # General 25 | .DS_Store 26 | .AppleDouble 27 | .LSOverride 28 | 29 | # Icon must end with two \r 30 | Icon 31 | 32 | # Thumbnails 33 | ._* 34 | 35 | # Files that might appear in the root of a volume 36 | .DocumentRevisions-V100 37 | .fseventsd 38 | .Spotlight-V100 39 | .TemporaryItems 40 | .Trashes 41 | .VolumeIcon.icns 42 | .com.apple.timemachine.donotpresent 43 | 44 | # Directories potentially created on remote AFP share 45 | .AppleDB 46 | .AppleDesktop 47 | Network Trash Folder 48 | Temporary Items 49 | .apdisk 50 | 51 | ### Rust ### 52 | # Generated by Cargo 53 | # will have compiled files and executables 54 | /target/ 55 | 56 | # Remove Cargo.lock from gitignore if creating an executable, leave it for libraries 57 | # More information here https://doc.rust-lang.org/cargo/guide/cargo-toml-vs-cargo-lock.html 58 | Cargo.lock 59 | 60 | # These are backup files generated by rustfmt 61 | **/*.rs.bk 62 | 63 | ### Vim ### 64 | # Swap 65 | [._]*.s[a-v][a-z] 66 | [._]*.sw[a-p] 67 | [._]s[a-rt-v][a-z] 68 | [._]ss[a-gi-z] 69 | [._]sw[a-p] 70 | 71 | # Session 72 | Session.vim 73 | 74 | # Temporary 75 | .netrwhist 76 | # Auto-generated tag files 77 | tags 78 | # Persistent undo 79 | [._]*.un~ 80 | 81 | ### Windows ### 82 | # Windows thumbnail cache files 83 | Thumbs.db 84 | ehthumbs.db 85 | ehthumbs_vista.db 86 | 87 | # Dump file 88 | *.stackdump 89 | 90 | # Folder config file 91 | [Dd]esktop.ini 92 | 93 | # Recycle Bin used on file shares 94 | $RECYCLE.BIN/ 95 | 96 | # Windows Installer files 97 | *.cab 98 | *.msi 99 | *.msix 100 | *.msm 101 | *.msp 102 | 103 | # Windows shortcuts 104 | *.lnk 105 | 106 | # End of https://www.gitignore.io/api/vim,osx,rust,linux,windows 107 | -------------------------------------------------------------------------------- /Cargo.toml: -------------------------------------------------------------------------------- 1 | [package] 2 | name = "visual-odometry-rs" 3 | version = "0.1.0" 4 | authors = ["Matthieu Pizenberg "] 5 | edition = "2018" 6 | 7 | description = "Visual odometry in Rust (vors)" 8 | homepage = "https://github.com/mpizenberg/visual-odometry-rs" 9 | repository = "https://github.com/mpizenberg/visual-odometry-rs" 10 | readme = "README.md" 11 | keywords = ["odometry", "slam", "vision", "rgbd"] # up to 5 keywords 12 | categories = ["science::robotics", "multimedia::video"] # up to 5 categories, cf crates.io/category_slugs 13 | license = "MPL-2.0" # SPDX license 14 | 15 | 16 | [dependencies] 17 | image = "0.19" # Encoding/decoding images in Rust. 18 | png = "0.12" 19 | byteorder = "1.2" # Reading numbers in [big/little]-endian. 20 | nalgebra = "0.17" # Linear algebra. 21 | rand = "0.6" # Random number generators. 22 | num-traits = "0.2" # Useful numeric traits. 23 | nom = "4.2" # Parsing files. 24 | itertools = "0.7" # More iterators. 25 | 26 | 27 | [dev-dependencies] 28 | quickcheck = "0.8" # Property based testing. 29 | quickcheck_macros = "0.8" 30 | approx = "0.3" # Approximate equalities. 31 | csv = "1" # CSV. 32 | criterion = "0.2" # Benchmarks. 33 | 34 | 35 | [[bench]] 36 | name = "mean_pyramid" 37 | harness = false 38 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Mozilla Public License Version 2.0 2 | ================================== 3 | 4 | 1. Definitions 5 | -------------- 6 | 7 | 1.1. "Contributor" 8 | means each individual or legal entity that creates, contributes to 9 | the creation of, or owns Covered Software. 10 | 11 | 1.2. "Contributor Version" 12 | means the combination of the Contributions of others (if any) used 13 | by a Contributor and that particular Contributor's Contribution. 14 | 15 | 1.3. "Contribution" 16 | means Covered Software of a particular Contributor. 17 | 18 | 1.4. "Covered Software" 19 | means Source Code Form to which the initial Contributor has attached 20 | the notice in Exhibit A, the Executable Form of such Source Code 21 | Form, and Modifications of such Source Code Form, in each case 22 | including portions thereof. 23 | 24 | 1.5. "Incompatible With Secondary Licenses" 25 | means 26 | 27 | (a) that the initial Contributor has attached the notice described 28 | in Exhibit B to the Covered Software; or 29 | 30 | (b) that the Covered Software was made available under the terms of 31 | version 1.1 or earlier of the License, but not also under the 32 | terms of a Secondary License. 33 | 34 | 1.6. "Executable Form" 35 | means any form of the work other than Source Code Form. 36 | 37 | 1.7. "Larger Work" 38 | means a work that combines Covered Software with other material, in 39 | a separate file or files, that is not Covered Software. 40 | 41 | 1.8. "License" 42 | means this document. 43 | 44 | 1.9. "Licensable" 45 | means having the right to grant, to the maximum extent possible, 46 | whether at the time of the initial grant or subsequently, any and 47 | all of the rights conveyed by this License. 48 | 49 | 1.10. "Modifications" 50 | means any of the following: 51 | 52 | (a) any file in Source Code Form that results from an addition to, 53 | deletion from, or modification of the contents of Covered 54 | Software; or 55 | 56 | (b) any new file in Source Code Form that contains any Covered 57 | Software. 58 | 59 | 1.11. "Patent Claims" of a Contributor 60 | means any patent claim(s), including without limitation, method, 61 | process, and apparatus claims, in any patent Licensable by such 62 | Contributor that would be infringed, but for the grant of the 63 | License, by the making, using, selling, offering for sale, having 64 | made, import, or transfer of either its Contributions or its 65 | Contributor Version. 66 | 67 | 1.12. "Secondary License" 68 | means either the GNU General Public License, Version 2.0, the GNU 69 | Lesser General Public License, Version 2.1, the GNU Affero General 70 | Public License, Version 3.0, or any later versions of those 71 | licenses. 72 | 73 | 1.13. "Source Code Form" 74 | means the form of the work preferred for making modifications. 75 | 76 | 1.14. "You" (or "Your") 77 | means an individual or a legal entity exercising rights under this 78 | License. For legal entities, "You" includes any entity that 79 | controls, is controlled by, or is under common control with You. For 80 | purposes of this definition, "control" means (a) the power, direct 81 | or indirect, to cause the direction or management of such entity, 82 | whether by contract or otherwise, or (b) ownership of more than 83 | fifty percent (50%) of the outstanding shares or beneficial 84 | ownership of such entity. 85 | 86 | 2. License Grants and Conditions 87 | -------------------------------- 88 | 89 | 2.1. Grants 90 | 91 | Each Contributor hereby grants You a world-wide, royalty-free, 92 | non-exclusive license: 93 | 94 | (a) under intellectual property rights (other than patent or trademark) 95 | Licensable by such Contributor to use, reproduce, make available, 96 | modify, display, perform, distribute, and otherwise exploit its 97 | Contributions, either on an unmodified basis, with Modifications, or 98 | as part of a Larger Work; and 99 | 100 | (b) under Patent Claims of such Contributor to make, use, sell, offer 101 | for sale, have made, import, and otherwise transfer either its 102 | Contributions or its Contributor Version. 103 | 104 | 2.2. Effective Date 105 | 106 | The licenses granted in Section 2.1 with respect to any Contribution 107 | become effective for each Contribution on the date the Contributor first 108 | distributes such Contribution. 109 | 110 | 2.3. Limitations on Grant Scope 111 | 112 | The licenses granted in this Section 2 are the only rights granted under 113 | this License. No additional rights or licenses will be implied from the 114 | distribution or licensing of Covered Software under this License. 115 | Notwithstanding Section 2.1(b) above, no patent license is granted by a 116 | Contributor: 117 | 118 | (a) for any code that a Contributor has removed from Covered Software; 119 | or 120 | 121 | (b) for infringements caused by: (i) Your and any other third party's 122 | modifications of Covered Software, or (ii) the combination of its 123 | Contributions with other software (except as part of its Contributor 124 | Version); or 125 | 126 | (c) under Patent Claims infringed by Covered Software in the absence of 127 | its Contributions. 128 | 129 | This License does not grant any rights in the trademarks, service marks, 130 | or logos of any Contributor (except as may be necessary to comply with 131 | the notice requirements in Section 3.4). 132 | 133 | 2.4. Subsequent Licenses 134 | 135 | No Contributor makes additional grants as a result of Your choice to 136 | distribute the Covered Software under a subsequent version of this 137 | License (see Section 10.2) or under the terms of a Secondary License (if 138 | permitted under the terms of Section 3.3). 139 | 140 | 2.5. Representation 141 | 142 | Each Contributor represents that the Contributor believes its 143 | Contributions are its original creation(s) or it has sufficient rights 144 | to grant the rights to its Contributions conveyed by this License. 145 | 146 | 2.6. Fair Use 147 | 148 | This License is not intended to limit any rights You have under 149 | applicable copyright doctrines of fair use, fair dealing, or other 150 | equivalents. 151 | 152 | 2.7. Conditions 153 | 154 | Sections 3.1, 3.2, 3.3, and 3.4 are conditions of the licenses granted 155 | in Section 2.1. 156 | 157 | 3. Responsibilities 158 | ------------------- 159 | 160 | 3.1. Distribution of Source Form 161 | 162 | All distribution of Covered Software in Source Code Form, including any 163 | Modifications that You create or to which You contribute, must be under 164 | the terms of this License. You must inform recipients that the Source 165 | Code Form of the Covered Software is governed by the terms of this 166 | License, and how they can obtain a copy of this License. You may not 167 | attempt to alter or restrict the recipients' rights in the Source Code 168 | Form. 169 | 170 | 3.2. Distribution of Executable Form 171 | 172 | If You distribute Covered Software in Executable Form then: 173 | 174 | (a) such Covered Software must also be made available in Source Code 175 | Form, as described in Section 3.1, and You must inform recipients of 176 | the Executable Form how they can obtain a copy of such Source Code 177 | Form by reasonable means in a timely manner, at a charge no more 178 | than the cost of distribution to the recipient; and 179 | 180 | (b) You may distribute such Executable Form under the terms of this 181 | License, or sublicense it under different terms, provided that the 182 | license for the Executable Form does not attempt to limit or alter 183 | the recipients' rights in the Source Code Form under this License. 184 | 185 | 3.3. Distribution of a Larger Work 186 | 187 | You may create and distribute a Larger Work under terms of Your choice, 188 | provided that You also comply with the requirements of this License for 189 | the Covered Software. If the Larger Work is a combination of Covered 190 | Software with a work governed by one or more Secondary Licenses, and the 191 | Covered Software is not Incompatible With Secondary Licenses, this 192 | License permits You to additionally distribute such Covered Software 193 | under the terms of such Secondary License(s), so that the recipient of 194 | the Larger Work may, at their option, further distribute the Covered 195 | Software under the terms of either this License or such Secondary 196 | License(s). 197 | 198 | 3.4. Notices 199 | 200 | You may not remove or alter the substance of any license notices 201 | (including copyright notices, patent notices, disclaimers of warranty, 202 | or limitations of liability) contained within the Source Code Form of 203 | the Covered Software, except that You may alter any license notices to 204 | the extent required to remedy known factual inaccuracies. 205 | 206 | 3.5. Application of Additional Terms 207 | 208 | You may choose to offer, and to charge a fee for, warranty, support, 209 | indemnity or liability obligations to one or more recipients of Covered 210 | Software. However, You may do so only on Your own behalf, and not on 211 | behalf of any Contributor. You must make it absolutely clear that any 212 | such warranty, support, indemnity, or liability obligation is offered by 213 | You alone, and You hereby agree to indemnify every Contributor for any 214 | liability incurred by such Contributor as a result of warranty, support, 215 | indemnity or liability terms You offer. You may include additional 216 | disclaimers of warranty and limitations of liability specific to any 217 | jurisdiction. 218 | 219 | 4. Inability to Comply Due to Statute or Regulation 220 | --------------------------------------------------- 221 | 222 | If it is impossible for You to comply with any of the terms of this 223 | License with respect to some or all of the Covered Software due to 224 | statute, judicial order, or regulation then You must: (a) comply with 225 | the terms of this License to the maximum extent possible; and (b) 226 | describe the limitations and the code they affect. Such description must 227 | be placed in a text file included with all distributions of the Covered 228 | Software under this License. Except to the extent prohibited by statute 229 | or regulation, such description must be sufficiently detailed for a 230 | recipient of ordinary skill to be able to understand it. 231 | 232 | 5. Termination 233 | -------------- 234 | 235 | 5.1. The rights granted under this License will terminate automatically 236 | if You fail to comply with any of its terms. However, if You become 237 | compliant, then the rights granted under this License from a particular 238 | Contributor are reinstated (a) provisionally, unless and until such 239 | Contributor explicitly and finally terminates Your grants, and (b) on an 240 | ongoing basis, if such Contributor fails to notify You of the 241 | non-compliance by some reasonable means prior to 60 days after You have 242 | come back into compliance. Moreover, Your grants from a particular 243 | Contributor are reinstated on an ongoing basis if such Contributor 244 | notifies You of the non-compliance by some reasonable means, this is the 245 | first time You have received notice of non-compliance with this License 246 | from such Contributor, and You become compliant prior to 30 days after 247 | Your receipt of the notice. 248 | 249 | 5.2. If You initiate litigation against any entity by asserting a patent 250 | infringement claim (excluding declaratory judgment actions, 251 | counter-claims, and cross-claims) alleging that a Contributor Version 252 | directly or indirectly infringes any patent, then the rights granted to 253 | You by any and all Contributors for the Covered Software under Section 254 | 2.1 of this License shall terminate. 255 | 256 | 5.3. In the event of termination under Sections 5.1 or 5.2 above, all 257 | end user license agreements (excluding distributors and resellers) which 258 | have been validly granted by You or Your distributors under this License 259 | prior to termination shall survive termination. 260 | 261 | ************************************************************************ 262 | * * 263 | * 6. Disclaimer of Warranty * 264 | * ------------------------- * 265 | * * 266 | * Covered Software is provided under this License on an "as is" * 267 | * basis, without warranty of any kind, either expressed, implied, or * 268 | * statutory, including, without limitation, warranties that the * 269 | * Covered Software is free of defects, merchantable, fit for a * 270 | * particular purpose or non-infringing. The entire risk as to the * 271 | * quality and performance of the Covered Software is with You. * 272 | * Should any Covered Software prove defective in any respect, You * 273 | * (not any Contributor) assume the cost of any necessary servicing, * 274 | * repair, or correction. This disclaimer of warranty constitutes an * 275 | * essential part of this License. No use of any Covered Software is * 276 | * authorized under this License except under this disclaimer. * 277 | * * 278 | ************************************************************************ 279 | 280 | ************************************************************************ 281 | * * 282 | * 7. Limitation of Liability * 283 | * -------------------------- * 284 | * * 285 | * Under no circumstances and under no legal theory, whether tort * 286 | * (including negligence), contract, or otherwise, shall any * 287 | * Contributor, or anyone who distributes Covered Software as * 288 | * permitted above, be liable to You for any direct, indirect, * 289 | * special, incidental, or consequential damages of any character * 290 | * including, without limitation, damages for lost profits, loss of * 291 | * goodwill, work stoppage, computer failure or malfunction, or any * 292 | * and all other commercial damages or losses, even if such party * 293 | * shall have been informed of the possibility of such damages. This * 294 | * limitation of liability shall not apply to liability for death or * 295 | * personal injury resulting from such party's negligence to the * 296 | * extent applicable law prohibits such limitation. Some * 297 | * jurisdictions do not allow the exclusion or limitation of * 298 | * incidental or consequential damages, so this exclusion and * 299 | * limitation may not apply to You. * 300 | * * 301 | ************************************************************************ 302 | 303 | 8. Litigation 304 | ------------- 305 | 306 | Any litigation relating to this License may be brought only in the 307 | courts of a jurisdiction where the defendant maintains its principal 308 | place of business and such litigation shall be governed by laws of that 309 | jurisdiction, without reference to its conflict-of-law provisions. 310 | Nothing in this Section shall prevent a party's ability to bring 311 | cross-claims or counter-claims. 312 | 313 | 9. Miscellaneous 314 | ---------------- 315 | 316 | This License represents the complete agreement concerning the subject 317 | matter hereof. If any provision of this License is held to be 318 | unenforceable, such provision shall be reformed only to the extent 319 | necessary to make it enforceable. Any law or regulation which provides 320 | that the language of a contract shall be construed against the drafter 321 | shall not be used to construe this License against a Contributor. 322 | 323 | 10. Versions of the License 324 | --------------------------- 325 | 326 | 10.1. New Versions 327 | 328 | Mozilla Foundation is the license steward. Except as provided in Section 329 | 10.3, no one other than the license steward has the right to modify or 330 | publish new versions of this License. Each version will be given a 331 | distinguishing version number. 332 | 333 | 10.2. Effect of New Versions 334 | 335 | You may distribute the Covered Software under the terms of the version 336 | of the License under which You originally received the Covered Software, 337 | or under the terms of any subsequent version published by the license 338 | steward. 339 | 340 | 10.3. Modified Versions 341 | 342 | If you create software not governed by this License, and you want to 343 | create a new license for such software, you may create and use a 344 | modified version of this License if you rename the license and remove 345 | any references to the name of the license steward (except to note that 346 | such modified license differs from this License). 347 | 348 | 10.4. Distributing Source Code Form that is Incompatible With Secondary 349 | Licenses 350 | 351 | If You choose to distribute Source Code Form that is Incompatible With 352 | Secondary Licenses under the terms of this version of the License, the 353 | notice described in Exhibit B of this License must be attached. 354 | 355 | Exhibit A - Source Code Form License Notice 356 | ------------------------------------------- 357 | 358 | This Source Code Form is subject to the terms of the Mozilla Public 359 | License, v. 2.0. If a copy of the MPL was not distributed with this 360 | file, You can obtain one at http://mozilla.org/MPL/2.0/. 361 | 362 | If it is not possible or desirable to put the notice in a particular 363 | file, then You may include the notice in a location (such as a LICENSE 364 | file in a relevant directory) where a recipient would be likely to look 365 | for such a notice. 366 | 367 | You may add additional accurate notices of copyright ownership. 368 | 369 | Exhibit B - "Incompatible With Secondary Licenses" Notice 370 | --------------------------------------------------------- 371 | 372 | This Source Code Form is "Incompatible With Secondary Licenses", as 373 | defined by the Mozilla Public License, v. 2.0. 374 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Visual Odometry in Rust (vors) 2 | 3 | This repository provides both a library ("crate" as we say in Rust) named visual-odometry-rs, 4 | (shortened vors) and a binary program named `vors_track`, 5 | for camera tracking ("visual odometry"). 6 | 7 | The program works on datasets following the [TUM RGB-D dataset format][tum-rgbd]. 8 | It is roughly a hundred lines of code (see `src/bin/vors_track.rs`), 9 | built upon the visual-odometry-rs crate also provided here. 10 | 11 | Once you have cloned this repository, 12 | you can run the binary program `vors_track` with cargo directly as follows: 13 | 14 | ```sh 15 | cargo run --release --bin vors_track -- fr1 /path/to/some/freiburg1/dataset/associations.txt 16 | ``` 17 | 18 | Have a look at [mpizenberg/rgbd-tracking-evaluation][rgbd-track-eval] 19 | for more info about the dataset requirements to run the binary program `vors_track`. 20 | 21 | The library is organized around four base namespaces: 22 | 23 | - `core::` Core modules for computing gradients, candidate points, camera tracking etc. 24 | - `dataset::` Helper modules for handling specific datasets. 25 | Currently only provides a module for TUM RGB-D compatible datasets. 26 | - `math::` Basic math modules for functionalities not already provided by [nalgebra][nalgebra], 27 | like Lie algebra for so3, se3, and an iterative optimizer trait. 28 | - `misc::` Helper modules for interoperability, visualization, and other things that did 29 | not fit elsewhere yet. 30 | 31 | [tum-rgbd]: https://vision.in.tum.de/data/datasets/rgbd-dataset 32 | [rgbd-track-eval]: https://github.com/mpizenberg/rgbd-tracking-evaluation 33 | [nalgebra]: https://www.nalgebra.org/ 34 | 35 | ## Library Usage Examples 36 | 37 | Self contained examples for usage of the API are available in the `examples/` directory. 38 | A readme is also present there for more detailed explanations on these examples. 39 | 40 | ## Functionalities and Vision 41 | 42 | Currently, vors provides a **visual odometry framework for working on direct RGB-D camera tracking**. 43 | Setting all this from the ground up took a lot of time and effort, 44 | but I think it is mature enough to be shared as is now. 45 | Beware, however, that the API is evolving a lot. 46 | My hope is that in the near future, we can improve the reach of this project 47 | by working both on research extensions, and platform availability. 48 | 49 | Example research extensions: 50 | 51 | - Using disparity search for depth initialization to be compatible with RGB (no depth) camera. 52 | - Adding a photometric term to the residual to account for automatic exposure variations. 53 | - Adding automatic photometric and/or geometric camera calibration. 54 | - Building a sliding window of keyframes optimization as in [DSO][dso] to reduce drift. 55 | - Intregrating loop closure and pose graph optimization for having a robust vSLAM system. 56 | - Fusion with IMU for improved tracking and reducing scale drift. 57 | - Modelization of rolling shutter (in most cameras) into the optimization problem. 58 | - Extension to stereo cameras. 59 | - Extension to omnidirectional cameras. 60 | 61 | Example platform extensions: 62 | 63 | - Making a C FFI to be able to run on systems with C drivers (kinect, realsense, ...). 64 | - Porting to the web with WebAssembly. 65 | - Porting to ARM for running in embedded systems and phones. 66 | 67 | ## Background Story 68 | 69 | Initially, this repository served as a personal experimental sandbox for computer vision in Rust. 70 | See for example my original questions on the rust [discourse][discourse] and [reddit channel][reddit]. 71 | Turns out I struggled a bit at first but then really liked the Rust way, compared to C++. 72 | 73 | As the name suggests, the focus is now on [visual odometry][vo], 74 | specifically on the recent research field of direct visual odometry. 75 | A reasonable introduction is available in those [lecture slides][vo-slides] 76 | by Waterloo Autonomous Vehicles lab. 77 | 78 | In particular, this project initially aimed at improving on the work of [DSO][dso] 79 | by J. Engel et. al. but with all the advantages of using the [Rust programming language][rust], 80 | including: 81 | 82 | - Performance without sacrificing code readability 83 | - No memory error, and much higher code safety and reliability 84 | - Friendly tooling ecosystem, no dependency issues, basically one-liner compilation and run 85 | - Best tooling for porting to the web with WebAssembly 86 | - Growing and mindful resources for porting to embedded systems 87 | - Wonderful community 88 | 89 | [discourse]: https://users.rust-lang.org/t/computer-vision-in-rust/16198 90 | [reddit]: https://www.reddit.com/r/rust/comments/84s5zo/computer_vision_in_rust/ 91 | [vo]: https://en.wikipedia.org/wiki/Visual_odometry 92 | [vo-slides]: http://wavelab.uwaterloo.ca/slam/2017-SLAM/Lecture14-Direct_visual_inertial_odometry_and_SLAM/slides.pdf 93 | [dso]: https://github.com/JakobEngel/dso 94 | [rust]: https://www.rust-lang.org/ 95 | 96 | ## License (MPL-2.0) 97 | 98 | This Source Code Form is subject to the terms of the Mozilla Public 99 | License, v. 2.0. If a copy of the MPL was not distributed with this 100 | file, You can obtain one at http://mozilla.org/MPL/2.0/. 101 | 102 | ## Contributions 103 | 104 | All forms of contribution are welcomed, **preferably first as github issues**. 105 | 106 | - Questions 107 | - Documentation 108 | - Tests 109 | - Benchmarks 110 | - Features 111 | 112 | In case of contribution to source code, 113 | it needs to use [rustfmt][rustfmt] and [clippy][clippy]. 114 | To run clippy: 115 | 116 | ``` 117 | touch src/lib.rs; cargo clippy --release --all-targets --all-features 118 | ``` 119 | 120 | [rustfmt]: https://github.com/rust-lang/rustfmt 121 | [clippy]: https://github.com/rust-lang/rust-clippy 122 | -------------------------------------------------------------------------------- /benches/mean_pyramid.rs: -------------------------------------------------------------------------------- 1 | // This Source Code Form is subject to the terms of the Mozilla Public 2 | // License, v. 2.0. If a copy of the MPL was not distributed with this 3 | // file, You can obtain one at http://mozilla.org/MPL/2.0/. 4 | 5 | use criterion::{criterion_group, criterion_main, Criterion}; 6 | use nalgebra::DMatrix; 7 | use visual_odometry_rs::core::multires; 8 | 9 | fn criterion_benchmark(c: &mut Criterion) { 10 | c.bench_function("mean_pyramid 5 480x640", |b| { 11 | let mat: DMatrix = DMatrix::repeat(480, 640, 1); 12 | b.iter(|| multires::mean_pyramid(5, mat.clone())) 13 | }); 14 | } 15 | 16 | criterion_group!(benches, criterion_benchmark); 17 | criterion_main!(benches); 18 | -------------------------------------------------------------------------------- /examples/README.md: -------------------------------------------------------------------------------- 1 | # Examples 2 | 3 | Examples showing usage of this library are located in this directory. 4 | They are regrouped by common prefix since we cannot have sub directories. 5 | 6 | - `dataset_...`: examples showing how to load data from supported datasets. 7 | - `candidates_...`: examples showing how to pick sparse candidate points for tracking. 8 | - `optim_...`: examples showing how the `OptimizerState` trait 9 | can be implemented on problems with different complexities. 10 | 11 | To run one example, move to the root of this repository 12 | (where the `Cargo.toml` file lives) and invoke the command: 13 | 14 | ```sh 15 | cargo run --release --example example_name [example_arguments] 16 | ``` 17 | 18 | ## Dataset 19 | 20 | ### tum-read-associations 21 | 22 | This example shows how to read the associations file 23 | of a dataset following the TUM RGB-D format. 24 | Such association file contains data similar to: 25 | 26 | ```txt 27 | # depth_timestamp depth_file_path rgb_timestamp rgb_file_path 28 | 1305031102.160407 depth/1305031102.160407.png 1305031102.175304 rgb/1305031102.175304.png 29 | 1305031102.226738 depth/1305031102.226738.png 1305031102.211214 rgb/1305031102.211214.png 30 | 1305031102.262886 depth/1305031102.262886.png 1305031102.275326 rgb/1305031102.275326.png 31 | ... 32 | ``` 33 | 34 | ### tum-read-trajectory 35 | 36 | This example shows how to read the trajectory file 37 | of a dataset following the TUM RGB-D format. 38 | Such trajectory file contains data similar to: 39 | 40 | ```txt 41 | # ground truth trajectory 42 | # timestamp tx ty tz qx qy qz qw 43 | 1305031098.6659 1.3563 0.6305 1.6380 0.6132 0.5962 -0.3311 -0.3986 44 | 1305031098.6758 1.3543 0.6306 1.6360 0.6129 0.5966 -0.3316 -0.3980 45 | ... 46 | ``` 47 | 48 | ## Candidates 49 | 50 | Candidate points are a subset of an image pixels 51 | suitable for tracking, to enable direct visual odometry. 52 | Traditionally, "indirect" approches try to track key points such as SIFT or ORB. 53 | In a direct approch, we do not track key points, but we still need 54 | a sparse set of points to track for beeing able to minimize our energy function. 55 | The selected points should usually satisfy two conditions: 56 | 57 | 1. Points shall be well distributed in the image. 58 | 2. The density of candidate points grows with the gradient magnitude. 59 | 60 | ### coarse-to-fine 61 | 62 | In this approach, candidates are selected by a coarse to fine mechanism. 63 | We build a pyramid of gradient magnitude images, 64 | where each level is half the resolution of the previous one. 65 | At the lowest resolution, we consider all points to be candidates. 66 | For each double resolution, 67 | for each 2x2 block corresponding to a candidate at the sub resolution, 68 | we select 1 or 2 of the pixels as candidates depending on a criteria. 69 | 70 | See for example the following image showing candidate points 71 | (in red) at each level of a 4-levels image pyramid. 72 | They contain respectively 609, 948, 1546 and 2636 candidate points. 73 | 74 | ![candidates coarse to fine][candidates-coarse-to-fine] 75 | 76 | You can run this candidates selection algorithm on an image of your choice as follows. 77 | This will create multiple png images named `candidates_0.png`, `candidates_1.png` ... 78 | in the directory where the image lives. 79 | 80 | ```sh 81 | cargo run --release --example candidates_coarse-to-fine /path/to/image 82 | ``` 83 | 84 | [candidates-coarse-to-fine]: https://mpizenberg.github.io/resources/vors/candidates-coarse-to-fine.png 85 | 86 | ### dso 87 | 88 | The candidates selection in [DSO][dso] is quite different, and requires far more parameters. 89 | At its heart, it consists of picking the highest gradient magnitude point per block. 90 | A block being a rectangular group of pixels. 91 | In practice there are many more complications. 92 | A pixel is only selected if its gradient magnitude is higher than a threshold, 93 | computed as the sum of a global constant, and a the median gradient magnitude of the current "region". 94 | Regions being like blocks but bigger. 95 | There are other details, like multiple levels, with lower thresholds, 96 | and possible random sub-selection to achieve an objective amount of points. 97 | 98 | You can run this candidates selection algorithm on an image of your choice as follows. 99 | This will create an image named `candidates.png` in the directory where the image lives. 100 | 101 | ```sh 102 | cargo run --release --example candidates_dso /path/to/image 103 | ``` 104 | 105 | [dso]: https://github.com/JakobEngel/dso 106 | 107 | ## Optimization 108 | 109 | In order to solve the non-linear problem of camera tracking by some energy minimization, 110 | there was a need to implement non-linear iterative solvers. 111 | In particular, I implemented a [Levenberg-Marquardt][levenberg] least square optimization. 112 | In the library code, the two modules inside `core::track` are an implementation of such algorithm. 113 | They minimize a reprojection error, in an [inverse compositional approach][baker], 114 | and a parameterization of the [rigid body motion][rigid-transformation] 115 | in the Lie algebra of twists [se3][lie]. 116 | 117 | [levenberg]: https://en.wikipedia.org/wiki/Levenberg%E2%80%93Marquardt_algorithm 118 | [baker]: http://www.ncorr.com/download/publications/bakerunify.pdf 119 | [rigid-transformation]: https://en.wikipedia.org/wiki/Rigid_transformation 120 | [lie]: http://ethaneade.com/lie.pdf 121 | 122 | After a lot of API trial and error, 123 | I ended with a code structure suitable for any iterative optimization algorithm. 124 | This "structure" has been formalized by the trait `OptimizerState` in the module `math::optimizer`. 125 | It basically goes as follows (see documentation for details). 126 | 127 | ```rust 128 | pub trait OptimizerState { 129 | fn init(obs: &Observations, model: Model) -> Self; 130 | fn step(&self) -> Result; 131 | fn eval(&self, obs: &Observations, new_model: Model) -> EvalState; 132 | fn stop_criterion(self, nb_iter: usize, eval_state: EvalState) -> (Self, Continue); 133 | fn iterative_solve(obs: &Observations, initial_model: Model) -> Result<(Self, usize), Error> { 134 | ... 135 | } 136 | } 137 | ``` 138 | 139 | It means that if you implement `init`, `step`, `eval` and `stop_criterion` for a struct 140 | of your custom type `MyOptimizer`, you will be able to call 141 | `MyOptimizer::iterative_solve(obs, model)` to get the solution. 142 | The implementation of `iterative_solve` is quite straightforward so don't hesitate to have a look at it. 143 | 144 | Details about the generic types and the four functions to implement are in the documentation. 145 | Simpler use cases than the camera tracking one are present in the following examples. 146 | 147 | ### regression-1d 148 | 149 | ![Regression of exponential data][optim_regression-1d] 150 | 151 | [optim_regression-1d]: https://mpizenberg.github.io/resources/vors/optim_regression-1d.svg 152 | 153 | In this example, we implement the `OptimizerState` trait 154 | to find the correct parameter `a` for modelling exponentially decreasing noisy data. 155 | It is implemented using a [Levenberg-Marquardt][levenberg] approach, 156 | but simpler approaches would have also worked. 157 | Details of the computation of the Jacobian and Hessian approximation are 158 | provided at the beginning of the example file. 159 | 160 | You can run the example as follows: 161 | 162 | ```sh 163 | cargo run --releas --example optim_regression-1d 164 | ``` 165 | 166 | ### rosenbrock 167 | 168 | ![Rosenbrock function][rosenbrock-png] 169 | 170 | _3D visualization of the Rosenbrock function, 171 | By Morn the Gorn - Own work, Public Domain_ 172 | 173 | The [Rosenbrock function][rosenbrock] is very often used to compare optimization algorithms. 174 | It is defined by: `f: (a, b, x, y) -> (a-x)^2 + b*(y-x^2)^2`. 175 | 176 | Given `a` and `b` fixed as constants, 177 | the global minimum of the rosenbrock function is obtained for `(x, y) = (a, a^2)` 178 | where its value is 0. 179 | Again, the implementation in the example is using the Levenberg-Marquardt algorithm. 180 | In the example code, we define `(a, b) = (1, 100)` and so the minimum is obtained for 181 | `(x, y) = (1, 1)`. 182 | You can run the example as follows: 183 | 184 | ```sh 185 | cargo run --release --example optim_rosenbrock 186 | ``` 187 | 188 | [rosenbrock-png]: https://mpizenberg.github.io/resources/vors/rosenbrock.png 189 | [rosenbrock]: https://en.wikipedia.org/wiki/Rosenbrock_function 190 | 191 | ### affine-2d 192 | 193 | ![Affine transformation][affine2d-jpg] 194 | 195 | [affine2d-jpg]: https://mpizenberg.github.io/resources/vors/affine2d.jpg 196 | 197 | _Original image on the left, automatically extracted "template" on the right._ 198 | 199 | In this example, we find the parameters of the affine transformation from 200 | the extracted "template" image on the right to the original image on the left. 201 | In the example we randomly generate the affine transformation, 202 | represented by the following matrix in homogeneous coordinates: 203 | 204 | ``` 205 | [ 1+p1 p3 p5 ] 206 | [ p2 1+p4 p6 ] 207 | [ 0 0 1 ] 208 | ``` 209 | 210 | We then optimize a direct image alignment problem of the form: 211 | 212 | ``` 213 | residual(x) = I(warp(x)) - T(x) 214 | ``` 215 | 216 | Where `T` is a template image, 217 | `I` an transformed image by a warp function (that should align with our original image), 218 | `x` a pixel coordinates in the template image, 219 | `warp` a 2D affine transformation of the form described above. 220 | 221 | Just like for the camera tracking, the resolution uses an inverse compositional approach 222 | as described in [Baker and Matthews, 2001][baker], 223 | with a Levenberg-Marquardt optimization algorithm. 224 | It also uses a multi-scale approach, where the solution at one level serves 225 | as an initialization for the next one. 226 | This exercise on a 2D affine transformation was very useful before implementing 227 | the actual 3D camera reprojection optimization. 228 | 229 | You can run the example with the image of your choosing as follows: 230 | 231 | ```sh 232 | cargo run --release --example optim_affine-2d /path/to/image 233 | ``` 234 | -------------------------------------------------------------------------------- /examples/candidates_coarse-to-fine.rs: -------------------------------------------------------------------------------- 1 | // This Source Code Form is subject to the terms of the Mozilla Public 2 | // License, v. 2.0. If a copy of the MPL was not distributed with this 3 | // file, You can obtain one at http://mozilla.org/MPL/2.0/. 4 | 5 | extern crate nalgebra as na; 6 | extern crate visual_odometry_rs as vors; 7 | 8 | use std::{env, error::Error, path::Path, path::PathBuf, process::exit}; 9 | use vors::core::{candidates::coarse_to_fine as candidates, gradient, multires}; 10 | use vors::misc::{interop, view}; 11 | 12 | type Img = na::DMatrix; 13 | type Mask = na::DMatrix; 14 | 15 | fn main() { 16 | let args: Vec = env::args().collect(); 17 | if let Err(error) = run(args) { 18 | eprintln!("{:?}", error); 19 | exit(1); 20 | } 21 | } 22 | 23 | const USAGE: &str = "Usage: cargo run --release --example candidates_coarse-to-fine image_file"; 24 | 25 | fn run(args: Vec) -> Result<(), Box> { 26 | // Check that the arguments are correct. 27 | let image_file_path = check_args(args)?; 28 | let img = read_image(&image_file_path)?; 29 | 30 | // Compute coarse to fine candidates. 31 | let nb_levels = 6; 32 | let diff_threshold = 7; 33 | let (candidates_coarse_to_fine, multires_img) = 34 | generate_candidates(img, nb_levels, diff_threshold); 35 | 36 | // Save each level in an image on the disk. 37 | let parent_dir = image_file_path.parent().unwrap(); 38 | for level in 0..nb_levels { 39 | let candidates_level = &candidates_coarse_to_fine[nb_levels - level - 1]; 40 | let img_level = &multires_img[level]; 41 | save_candidates(candidates_level, img_level, parent_dir, level)?; 42 | } 43 | 44 | // Display some stats. 45 | let nb_candidates = |mask: &Mask| mask.fold(0, |sum, x| if x { sum + 1 } else { sum }); 46 | let nb_candidates_levels: Vec<_> = candidates_coarse_to_fine 47 | .iter() 48 | .map(nb_candidates) 49 | .collect(); 50 | eprintln!("Candidate points per level: {:?}", nb_candidates_levels); 51 | 52 | Ok(()) 53 | } 54 | 55 | fn generate_candidates(img: Img, nb_levels: usize, diff_threshold: u16) -> (Vec, Vec) { 56 | // Compute the multi-resolution image pyramid. 57 | let multires_img = multires::mean_pyramid(nb_levels, img); 58 | 59 | // Compute the multi-resolution gradients pyramid (without first level). 60 | let mut multires_grad = multires::gradients_squared_norm(&multires_img); 61 | 62 | // Insert the gradient of the original resolution at the first position. 63 | // This one cannot be computed by the bloc gradients used in multires::gradients_squared_norm. 64 | multires_grad.insert(0, gradient::squared_norm_direct(&multires_img[0])); 65 | 66 | // Call the coarse to fine candidates selection algorithm. 67 | let candidates_coarse_to_fine = candidates::select(diff_threshold, &multires_grad); 68 | (candidates_coarse_to_fine, multires_img) 69 | } 70 | 71 | // HELPERS ##################################################################### 72 | 73 | /// Verify that command line arguments are correct. 74 | fn check_args(args: Vec) -> Result { 75 | match args.as_slice() { 76 | [_, image_file_path_str] => { 77 | let image_file_path = PathBuf::from(image_file_path_str); 78 | if image_file_path.is_file() { 79 | Ok(image_file_path) 80 | } else { 81 | eprintln!("{}", USAGE); 82 | Err(format!("This file does not exist: {}", image_file_path_str)) 83 | } 84 | } 85 | _ => { 86 | eprintln!("{}", USAGE); 87 | Err("Wrong number of arguments".to_string()) 88 | } 89 | } 90 | } 91 | 92 | fn read_image>(image_path: P) -> Result> { 93 | Ok(interop::matrix_from_image( 94 | image::open(image_path)?.to_luma(), 95 | )) 96 | } 97 | 98 | fn save_candidates>( 99 | candidates_map: &Mask, 100 | img: &Img, 101 | dir: P, 102 | level: usize, 103 | ) -> Result<(), std::io::Error> { 104 | let color_candidates = view::candidates_on_image(img, candidates_map); 105 | let file_name = format!("candidates_{}.png", level); 106 | color_candidates.save(dir.as_ref().join(file_name)) 107 | } 108 | -------------------------------------------------------------------------------- /examples/candidates_dso.rs: -------------------------------------------------------------------------------- 1 | // This Source Code Form is subject to the terms of the Mozilla Public 2 | // License, v. 2.0. If a copy of the MPL was not distributed with this 3 | // file, You can obtain one at http://mozilla.org/MPL/2.0/. 4 | 5 | extern crate nalgebra as na; 6 | extern crate visual_odometry_rs as vors; 7 | 8 | use std::{env, error::Error, path::Path, path::PathBuf, process::exit}; 9 | use vors::core::{candidates::dso as candidates, gradient}; 10 | use vors::misc::{interop, view}; 11 | 12 | type Img = na::DMatrix; 13 | 14 | fn main() { 15 | let args: Vec = env::args().collect(); 16 | if let Err(error) = run(args) { 17 | eprintln!("{:?}", error); 18 | exit(1); 19 | } 20 | } 21 | 22 | const USAGE: &str = "Usage: cargo run --release --example candidates_dso image_file"; 23 | 24 | fn run(args: Vec) -> Result<(), Box> { 25 | // Check that the arguments are correct. 26 | let image_file_path = check_args(args)?; 27 | let img = read_image(&image_file_path)?; 28 | 29 | // Compute candidates points. 30 | let candidate_points = generate_candidates(&img); 31 | save_candidates(&candidate_points, &img, image_file_path.parent().unwrap())?; 32 | 33 | // Display some stats. 34 | let nb_candidates = candidate_points.fold(0, |sum, x| if x { sum + 1 } else { sum }); 35 | println!("Number of candidate points: {}", nb_candidates); 36 | 37 | Ok(()) 38 | } 39 | 40 | fn generate_candidates(img: &Img) -> na::DMatrix { 41 | // Compute gradients norm of the image. 42 | let gradients = gradient::squared_norm_direct(img).map(|g2| f32::from(g2).sqrt() as u16); 43 | 44 | // Example of how to adapt default parameters config. 45 | let mut recursive_config = candidates::DEFAULT_RECURSIVE_CONFIG; 46 | let mut block_config = candidates::DEFAULT_BLOCK_CONFIG; 47 | recursive_config.nb_iterations_left = 2; 48 | block_config.nb_levels = 3; 49 | block_config.threshold_factor = 0.5; 50 | 51 | // Choose candidates based on gradients norms. 52 | candidates::select( 53 | &gradients, 54 | candidates::DEFAULT_REGION_CONFIG, 55 | block_config, 56 | recursive_config, 57 | 2000, 58 | ) 59 | } 60 | 61 | // HELPERS ##################################################################### 62 | 63 | /// Verify that command line arguments are correct. 64 | fn check_args(args: Vec) -> Result { 65 | match args.as_slice() { 66 | [_, image_file_path_str] => { 67 | let image_file_path = PathBuf::from(image_file_path_str); 68 | if image_file_path.is_file() { 69 | Ok(image_file_path) 70 | } else { 71 | eprintln!("{}", USAGE); 72 | Err(format!("This file does not exist: {}", image_file_path_str)) 73 | } 74 | } 75 | _ => { 76 | eprintln!("{}", USAGE); 77 | Err("Wrong number of arguments".to_string()) 78 | } 79 | } 80 | } 81 | 82 | fn read_image>(image_path: P) -> Result> { 83 | Ok(interop::matrix_from_image( 84 | image::open(image_path)?.to_luma(), 85 | )) 86 | } 87 | 88 | fn save_candidates>( 89 | candidates_map: &na::DMatrix, 90 | img: &Img, 91 | dir: P, 92 | ) -> Result<(), std::io::Error> { 93 | let color_candidates = view::candidates_on_image(img, candidates_map); 94 | color_candidates.save(dir.as_ref().join("candidates.png")) 95 | } 96 | -------------------------------------------------------------------------------- /examples/dataset_tum-read-associations.rs: -------------------------------------------------------------------------------- 1 | // This Source Code Form is subject to the terms of the Mozilla Public 2 | // License, v. 2.0. If a copy of the MPL was not distributed with this 3 | // file, You can obtain one at http://mozilla.org/MPL/2.0/. 4 | 5 | extern crate visual_odometry_rs as vors; 6 | 7 | use std::{env, error::Error, fs, io::BufReader, io::Read, path::PathBuf, process::exit}; 8 | use vors::dataset::tum_rgbd; 9 | 10 | fn main() { 11 | let args: Vec = env::args().collect(); 12 | if let Err(error) = run(args) { 13 | eprintln!("{:?}", error); 14 | exit(1); 15 | } 16 | } 17 | 18 | const USAGE: &str = 19 | "Usage: cargo run --release --example dataset_tum-read-associations associations_file"; 20 | 21 | fn run(args: Vec) -> Result<(), Box> { 22 | // Check that the arguments are correct. 23 | let associations_file_path = check_args(args)?; 24 | 25 | // Build a vector containing timestamps and full paths of images. 26 | let associations = parse_associations(associations_file_path)?; 27 | 28 | // Print to stdout first few associations. 29 | println!("First 3 associations:"); 30 | for assoc in associations.iter().take(3) { 31 | println!(); 32 | println!("Depth image timestamp: {}", assoc.depth_timestamp); 33 | println!( 34 | "Depth image absolute path: {}", 35 | assoc.depth_file_path.display() 36 | ); 37 | println!("Color image timestamp: {}", assoc.color_timestamp); 38 | println!( 39 | "Color image absolute path: {}", 40 | assoc.color_file_path.display() 41 | ); 42 | } 43 | 44 | Ok(()) 45 | } 46 | 47 | /// Verify that command line arguments are correct. 48 | fn check_args(args: Vec) -> Result { 49 | match args.as_slice() { 50 | [_, associations_file_path_str] => { 51 | let associations_file_path = PathBuf::from(associations_file_path_str); 52 | if associations_file_path.is_file() { 53 | Ok(associations_file_path) 54 | } else { 55 | eprintln!("{}", USAGE); 56 | Err(format!( 57 | "The association file does not exist or is not reachable: {}", 58 | associations_file_path_str 59 | )) 60 | } 61 | } 62 | _ => { 63 | eprintln!("{}", USAGE); 64 | Err("Wrong number of arguments".to_string()) 65 | } 66 | } 67 | } 68 | 69 | /// Open an association file and parse it into a vector of Association. 70 | fn parse_associations(file_path: PathBuf) -> Result, Box> { 71 | let file = fs::File::open(&file_path)?; 72 | let mut file_reader = BufReader::new(file); 73 | let mut content = String::new(); 74 | file_reader.read_to_string(&mut content)?; 75 | tum_rgbd::parse::associations(&content) 76 | .map(|v| v.iter().map(|a| abs_path(&file_path, a)).collect()) 77 | .map_err(|s| s.into()) 78 | } 79 | 80 | /// Transform relative images file paths into absolute ones. 81 | fn abs_path(file_path: &PathBuf, assoc: &tum_rgbd::Association) -> tum_rgbd::Association { 82 | let parent = file_path.parent().expect("How can this have no parent?"); 83 | tum_rgbd::Association { 84 | depth_timestamp: assoc.depth_timestamp, 85 | depth_file_path: parent.join(&assoc.depth_file_path), 86 | color_timestamp: assoc.color_timestamp, 87 | color_file_path: parent.join(&assoc.color_file_path), 88 | } 89 | } 90 | -------------------------------------------------------------------------------- /examples/dataset_tum-read-trajectory.rs: -------------------------------------------------------------------------------- 1 | // This Source Code Form is subject to the terms of the Mozilla Public 2 | // License, v. 2.0. If a copy of the MPL was not distributed with this 3 | // file, You can obtain one at http://mozilla.org/MPL/2.0/. 4 | 5 | extern crate visual_odometry_rs as vors; 6 | 7 | use std::{env, error::Error, fs, io::BufReader, io::Read, path::PathBuf, process::exit}; 8 | use vors::dataset::tum_rgbd; 9 | 10 | fn main() { 11 | let args: Vec = env::args().collect(); 12 | if let Err(error) = run(args) { 13 | eprintln!("{:?}", error); 14 | exit(1); 15 | } 16 | } 17 | 18 | const USAGE: &str = 19 | "Usage: cargo run --release --example dataset_tum-read-trajectory trajectory_file"; 20 | 21 | fn run(args: Vec) -> Result<(), Box> { 22 | // Check that the arguments are correct. 23 | let trajectory_file_path = check_args(args)?; 24 | 25 | // Build a vector of frames, containing timestamps and camera poses. 26 | let frames = parse_trajectory(trajectory_file_path)?; 27 | 28 | // Print to stdout first few frames. 29 | println!("First few frames:"); 30 | for frame in frames.iter().take(5) { 31 | println!("{}", frame.to_string()); 32 | } 33 | 34 | Ok(()) 35 | } 36 | 37 | /// Verify that command line arguments are correct. 38 | fn check_args(args: Vec) -> Result { 39 | match args.as_slice() { 40 | [_, trajectory_file_path_str] => { 41 | let trajectory_file_path = PathBuf::from(trajectory_file_path_str); 42 | if trajectory_file_path.is_file() { 43 | Ok(trajectory_file_path) 44 | } else { 45 | eprintln!("{}", USAGE); 46 | Err(format!( 47 | "The trajectory file does not exist or is not reachable: {}", 48 | trajectory_file_path_str 49 | )) 50 | } 51 | } 52 | _ => { 53 | eprintln!("{}", USAGE); 54 | Err("Wrong number of arguments".to_string()) 55 | } 56 | } 57 | } 58 | 59 | /// Open a trajectory file and parse it into a vector of Frames. 60 | fn parse_trajectory(file_path: PathBuf) -> Result, Box> { 61 | let file = fs::File::open(&file_path)?; 62 | let mut file_reader = BufReader::new(file); 63 | let mut content = String::new(); 64 | file_reader.read_to_string(&mut content)?; 65 | tum_rgbd::parse::trajectory(&content).map_err(|s| s.into()) 66 | } 67 | -------------------------------------------------------------------------------- /examples/optim_affine-2d.rs: -------------------------------------------------------------------------------- 1 | // This Source Code Form is subject to the terms of the Mozilla Public 2 | // License, v. 2.0. If a copy of the MPL was not distributed with this 3 | // file, You can obtain one at http://mozilla.org/MPL/2.0/. 4 | 5 | extern crate nalgebra as na; 6 | extern crate rand; 7 | extern crate visual_odometry_rs as vors; 8 | 9 | // use rand::{rngs::StdRng, Rng, SeedableRng}; 10 | use rand::Rng; 11 | use std::{env, error::Error, f32::consts, path::Path, path::PathBuf, process::exit}; 12 | use vors::math::optimizer::{self, Continue, State as _}; 13 | use vors::misc::type_aliases::{Mat3, Mat6, Vec3, Vec6}; 14 | use vors::{core::gradient, core::multires, misc::interop}; 15 | 16 | /// In this example, we attempt to find the affine 2D transformation 17 | /// between a template and another image. 18 | /// 19 | /// For the purpose of demonstration, we randomly generate ourselve 20 | /// by extraction a square template inside the chosen image. 21 | 22 | const USAGE: &str = "Usage: cargo run --release --example optim_affine-2d image_file"; 23 | 24 | type Mat2 = na::Matrix2; 25 | type Mat24 = na::Matrix2x4; 26 | type Mat23 = na::Matrix2x3; 27 | type Img = na::DMatrix; 28 | type Vec2 = na::Vector2; 29 | 30 | fn main() { 31 | let args: Vec = env::args().collect(); 32 | if let Err(err) = run(args) { 33 | eprintln!("{:?}", err); 34 | exit(1); 35 | }; 36 | } 37 | 38 | fn run(args: Vec) -> Result<(), Box> { 39 | // Load image. 40 | let image_path = check_args(args)?; 41 | let img = read_image(&image_path)?; 42 | 43 | // Extract template. 44 | let (template, affine2d) = random_template(&img); 45 | save_template(&template, image_path.parent().unwrap())?; 46 | 47 | // Precompute multi-resolution observations data. 48 | // We want roughly 200 points at the lowest resolution. 49 | let nb_levels = std::cmp::max( 50 | 1, 51 | (1.0 + (img.len() as f32 / 200.0).log(4.0)).round() as usize, 52 | ); 53 | let img_multires = multires::mean_pyramid(nb_levels, img); 54 | let template_multires = multires::mean_pyramid(nb_levels, template); 55 | let grad_multires: Vec<_> = template_multires.iter().map(gradient::centered).collect(); 56 | let jacobians_multires: Vec<_> = grad_multires.iter().map(affine_jacobians).collect(); 57 | let hessians_multires: Vec<_> = jacobians_multires.iter().map(hessians_vec).collect(); 58 | 59 | // Multi-resolution optimization. 60 | let mut model = Vec6::zeros(); 61 | for level in (0..nb_levels).rev() { 62 | println!("---------------- Level {}:", level); 63 | model[4] *= 2.0; 64 | model[5] *= 2.0; 65 | let obs = Obs { 66 | template: &template_multires[level], 67 | image: &img_multires[level], 68 | jacobians: &jacobians_multires[level], 69 | hessians: &hessians_multires[level], 70 | }; 71 | let (final_state, _nb_iter) = LMOptimizerState::iterative_solve(&obs, model)?; 72 | model = final_state.eval_data.model; 73 | } 74 | 75 | // Display results. 76 | println!("Ground truth: {}", affine2d); 77 | println!("Computed: {}", warp_mat(model)); 78 | Ok(()) 79 | } 80 | 81 | // OPTIMIZER ################################################################### 82 | 83 | struct Obs<'a> { 84 | template: &'a Img, 85 | image: &'a Img, 86 | jacobians: &'a Vec, 87 | hessians: &'a Vec, 88 | } 89 | 90 | struct LMOptimizerState { 91 | lm_coef: f32, 92 | eval_data: EvalData, 93 | } 94 | 95 | struct EvalData { 96 | model: Vec6, 97 | energy: f32, 98 | gradient: Vec6, 99 | hessian: Mat6, 100 | } 101 | 102 | type EvalState = Result; 103 | 104 | impl LMOptimizerState { 105 | /// Evaluate energy associated with a model. 106 | fn eval_energy(obs: &Obs, model: Vec6) -> (f32, Vec, Vec) { 107 | let nb_pixels = obs.template.len(); 108 | let (nb_rows, _) = obs.template.shape(); 109 | let mut x = 0_usize; // column "j" 110 | let mut y = 0_usize; // row "i" 111 | let mut inside_indices = Vec::with_capacity(nb_pixels); 112 | let mut residuals = Vec::with_capacity(nb_pixels); 113 | let mut energy = 0.0; 114 | for (idx, &tmp) in obs.template.iter().enumerate() { 115 | let (u, v) = warp(&model, x as f32, y as f32); 116 | if let Some(im) = interpolate(u, v, &obs.image) { 117 | // precompute residuals and energy 118 | let residual = im - f32::from(tmp); 119 | energy += residual * residual; 120 | residuals.push(residual); 121 | inside_indices.push(idx); // keep only inside points 122 | } 123 | // update x and y positions 124 | y += 1; 125 | if y >= nb_rows { 126 | x += 1; 127 | y = 0; 128 | } 129 | } 130 | energy /= inside_indices.len() as f32; 131 | (energy, residuals, inside_indices) 132 | } 133 | 134 | /// Compute evaluation data for the next iteration step. 135 | fn compute_eval_data(obs: &Obs, model: Vec6, pre: (f32, Vec, Vec)) -> EvalData { 136 | let (energy, residuals, inside_indices) = pre; 137 | let mut gradient = Vec6::zeros(); 138 | let mut hessian = Mat6::zeros(); 139 | for (i, &idx) in inside_indices.iter().enumerate() { 140 | let jac = obs.jacobians[idx]; 141 | let hes = obs.hessians[idx]; 142 | let res = residuals[i]; 143 | gradient += jac * res; 144 | hessian += hes; 145 | } 146 | EvalData { 147 | model, 148 | energy, 149 | gradient, 150 | hessian, 151 | } 152 | } 153 | } // LMOptimizerState 154 | 155 | impl<'a> optimizer::State, EvalState, Vec6, String> for LMOptimizerState { 156 | /// Initialize the optimizer state. 157 | /// Levenberg-Marquardt coefficient start at 0.1. 158 | fn init(obs: &Obs, model: Vec6) -> Self { 159 | Self { 160 | lm_coef: 0.1, 161 | eval_data: Self::compute_eval_data(obs, model, Self::eval_energy(obs, model)), 162 | } 163 | } 164 | 165 | /// Compute the Levenberg-Marquardt step. 166 | fn step(&self) -> Result { 167 | let mut hessian = self.eval_data.hessian; 168 | hessian.m11 *= 1.0 + self.lm_coef; 169 | hessian.m22 *= 1.0 + self.lm_coef; 170 | hessian.m33 *= 1.0 + self.lm_coef; 171 | hessian.m44 *= 1.0 + self.lm_coef; 172 | hessian.m55 *= 1.0 + self.lm_coef; 173 | hessian.m66 *= 1.0 + self.lm_coef; 174 | let cholesky = hessian.cholesky().ok_or("Error in cholesky.")?; 175 | let delta = cholesky.solve(&self.eval_data.gradient); 176 | let delta_warp = warp_mat(delta); 177 | let old_warp = warp_mat(self.eval_data.model); 178 | Ok(warp_params(old_warp * delta_warp.try_inverse().unwrap())) 179 | } 180 | 181 | /// Evaluate the new model. 182 | fn eval(&self, obs: &Obs, model: Vec6) -> EvalState { 183 | let pre = Self::eval_energy(obs, model); 184 | let energy = pre.0; 185 | let old_energy = self.eval_data.energy; 186 | if energy > old_energy { 187 | Err(energy) 188 | } else { 189 | Ok(Self::compute_eval_data(obs, model, pre)) 190 | } 191 | } 192 | 193 | /// Decide if iterations should continue. 194 | fn stop_criterion(self, nb_iter: usize, eval_state: EvalState) -> (Self, Continue) { 195 | let too_many_iterations = nb_iter >= 20; 196 | match (eval_state, too_many_iterations) { 197 | // Max number of iterations reached: 198 | (Err(_), true) => (self, Continue::Stop), 199 | (Ok(eval_data), true) => { 200 | println!("energy = {}", eval_data.energy); 201 | let mut kept_state = self; 202 | kept_state.eval_data = eval_data; 203 | (kept_state, Continue::Stop) 204 | } 205 | // Max number of iterations not reached yet: 206 | (Err(energy), false) => { 207 | let mut kept_state = self; 208 | kept_state.lm_coef *= 10.0; 209 | println!("\t back from {}, lm_coef = {}", energy, kept_state.lm_coef); 210 | (kept_state, Continue::Forward) 211 | } 212 | (Ok(eval_data), false) => { 213 | println!("energy = {}", eval_data.energy); 214 | let delta_energy = self.eval_data.energy - eval_data.energy; 215 | let mut kept_state = self; 216 | kept_state.lm_coef *= 0.1; 217 | kept_state.eval_data = eval_data; 218 | let continuation = if delta_energy > 0.01 { 219 | Continue::Forward 220 | } else { 221 | Continue::Stop 222 | }; 223 | (kept_state, continuation) 224 | } 225 | } 226 | } 227 | } 228 | 229 | // HELPERS ##################################################################### 230 | 231 | fn check_args(args: Vec) -> Result> { 232 | if args.len() != 2 { 233 | eprintln!("{}", USAGE); 234 | Err("Wrong number of arguments".into()) 235 | } else { 236 | let image_path = PathBuf::from(&args[1]); 237 | if image_path.is_file() { 238 | Ok(image_path) 239 | } else { 240 | Err(format!("File does not exist: {}", image_path.display()).into()) 241 | } 242 | } 243 | } 244 | 245 | fn read_image>(image_path: P) -> Result> { 246 | Ok(interop::matrix_from_image( 247 | image::open(image_path)?.to_luma(), 248 | )) 249 | } 250 | 251 | fn save_template>(template: &Img, dir: P) -> Result<(), std::io::Error> { 252 | let img = interop::image_from_matrix(template); 253 | img.save(dir.as_ref().join("template.png")) 254 | } 255 | 256 | fn random_template(img: &Img) -> (Img, Mat23) { 257 | // let seed = [0; 32]; 258 | // let mut rng: StdRng = SeedableRng::from_seed(seed); 259 | let mut rng = rand::thread_rng(); 260 | 261 | // Random scaling. 262 | let s_r = rng.gen_range(0.7, 0.8); 263 | let s_c = rng.gen_range(0.7, 0.8); 264 | 265 | // Deduce the size of the template image. 266 | let (img_rows, img_cols) = img.shape(); 267 | let img_rows = img_rows as f32; 268 | let img_cols = img_cols as f32; 269 | let tmp_rows = (s_r * img_rows).floor(); 270 | let tmp_cols = (s_c * img_cols).floor(); 271 | 272 | // Random small rotation angle. 273 | let max_angle = max_template_angle( 274 | img_rows - 2.0, // add small margin 275 | img_cols - 2.0, 276 | tmp_rows, 277 | tmp_cols, 278 | ); 279 | let a = rng.gen_range(-max_angle, max_angle); 280 | 281 | // Generate rotation and scaling matrix. 282 | let m: Mat2 = Mat2::new(s_c * a.cos(), -s_r * a.sin(), s_c * a.sin(), s_r * a.cos()); 283 | 284 | // Project points to find suitable random translation range, 285 | // such that the template stays fully inside the image. 286 | #[rustfmt::skip] 287 | let corners = Mat24::new( 288 | 0.0, img_cols-1.0, img_cols-1.0, 0.0, 289 | 0.0, 0.0, img_rows-1.0, img_rows-1.0, 290 | ); 291 | let t_corners = m * corners; 292 | let col_min = t_corners.row(0).min(); 293 | let col_max = t_corners.row(0).max(); 294 | let row_min = t_corners.row(1).min(); 295 | let row_max = t_corners.row(1).max(); 296 | 297 | // Generate suitable random translation. 298 | let t_cols_max = (-col_min + 1e-6).max(img_cols - 1.0 - col_max); 299 | let t_rows_max = (-row_min + 1e-6).max(img_rows - 1.0 - row_max); 300 | let t_cols = rng.gen_range(-col_min, t_cols_max); 301 | let t_rows = rng.gen_range(-row_min, t_rows_max); 302 | 303 | // Build the affine transformation matrix. 304 | let affine2d = Mat23::new(m.m11, m.m12, t_cols, m.m21, m.m22, t_rows); 305 | println!("affine2d: {}", affine2d); 306 | 307 | // Generate template image with this affine transformation. 308 | let template = project(&img, (img_rows as usize, img_cols as usize), &affine2d); 309 | (template, affine2d) 310 | } 311 | 312 | /// Find the max rotation of the inner (template) rectangle (rt, ct) such that it stays 313 | /// fully inside the outer (image) rectangle (ri, ci). 314 | /// We suppose rt < ri and ct < ci. 315 | /// If the circumscribed circle of the inner rectangle is fully inside the outer rectangle, 316 | /// all rotation angles are allowed, otherwise, we have to find the limit. 317 | fn max_template_angle(ri: f32, ci: f32, rt: f32, ct: f32) -> f32 { 318 | // By default, we limit at pi/8. 319 | let mut threshold = consts::FRAC_PI_8; 320 | 321 | // Inner diagonal. 322 | let inner_diag = (rt.powi(2) + ct.powi(2)).sqrt(); 323 | 324 | // Check rows dimension. 325 | if inner_diag > ri { 326 | threshold = threshold.min((ri / inner_diag).asin() - (rt / inner_diag).asin()); 327 | } 328 | 329 | // Check columns dimension. 330 | if inner_diag > ci { 331 | threshold = threshold.min((ci / inner_diag).asin() - (ct / inner_diag).asin()); 332 | } 333 | 334 | threshold 335 | } 336 | 337 | fn warp(model: &Vec6, x: f32, y: f32) -> (f32, f32) { 338 | ( 339 | (1.0 + model[0]) * x + model[2] * y + model[4], 340 | model[1] * x + (1.0 + model[3]) * y + model[5], 341 | ) 342 | } 343 | 344 | /// Affine warp parameterization: 345 | /// [ 1+p1 p3 p5 ] 346 | /// [ p2 1+p4 p6 ] 347 | /// [ 0 0 1 ] 348 | #[rustfmt::skip] 349 | fn warp_mat(params: Vec6) -> Mat3 { 350 | Mat3::new( 351 | params[0] + 1.0, params[2], params[4], 352 | params[1], params[3] + 1.0, params[5], 353 | 0.0, 0.0, 1.0, 354 | ) 355 | } 356 | 357 | fn warp_params(mat: Mat3) -> Vec6 { 358 | Vec6::new( 359 | mat.m11 - 1.0, 360 | mat.m21, 361 | mat.m12, 362 | mat.m22 - 1.0, 363 | mat.m13, 364 | mat.m23, 365 | ) 366 | } 367 | 368 | fn project(img: &Img, shape: (usize, usize), affine2d: &Mat23) -> Img { 369 | let (rows, cols) = shape; 370 | let project_ij = |i, j| affine2d * Vec3::new(j as f32, i as f32, 1.0); 371 | Img::from_fn(rows, cols, |i, j| interpolate_u8(&img, project_ij(i, j))) 372 | } 373 | 374 | /// Interpolate a pixel in the image. 375 | /// Bilinear interpolation, points are supposed to be fully inside img. 376 | fn interpolate_u8(img: &Img, pixel: Vec2) -> u8 { 377 | interpolate(pixel.x, pixel.y, img).unwrap() as u8 378 | } 379 | 380 | #[allow(clippy::cast_lossless)] 381 | #[allow(clippy::many_single_char_names)] 382 | fn interpolate(x: f32, y: f32, image: &Img) -> Option { 383 | let (height, width) = image.shape(); 384 | let u = x.floor(); 385 | let v = y.floor(); 386 | if u >= 0.0 && u < (width - 2) as f32 && v >= 0.0 && v < (height - 2) as f32 { 387 | let u_0 = u as usize; 388 | let v_0 = v as usize; 389 | let u_1 = u_0 + 1; 390 | let v_1 = v_0 + 1; 391 | let vu_00 = image[(v_0, u_0)] as f32; 392 | let vu_10 = image[(v_1, u_0)] as f32; 393 | let vu_01 = image[(v_0, u_1)] as f32; 394 | let vu_11 = image[(v_1, u_1)] as f32; 395 | let a = x - u; 396 | let b = y - v; 397 | Some( 398 | (1.0 - b) * (1.0 - a) * vu_00 399 | + b * (1.0 - a) * vu_10 400 | + (1.0 - b) * a * vu_01 401 | + b * a * vu_11, 402 | ) 403 | } else { 404 | None 405 | } 406 | } 407 | 408 | fn affine_jacobians(grad: &(na::DMatrix, na::DMatrix)) -> Vec { 409 | let (grad_x, grad_y) = grad; 410 | let (nb_rows, _) = grad_x.shape(); 411 | let mut x = 0_usize; 412 | let mut y = 0_usize; 413 | let mut jacobians = Vec::with_capacity(grad_x.len()); 414 | for (&gx, &gy) in grad_x.iter().zip(grad_y.iter()) { 415 | let gx_f = f32::from(gx); 416 | let gy_f = f32::from(gy); 417 | let x_f = x as f32; 418 | let y_f = y as f32; 419 | // CF Baker and Matthews. 420 | let jac = Vec6::new(x_f * gx_f, x_f * gy_f, y_f * gx_f, y_f * gy_f, gx_f, gy_f); 421 | jacobians.push(jac); 422 | y += 1; 423 | if y >= nb_rows { 424 | x += 1; 425 | y = 0; 426 | } 427 | } 428 | jacobians 429 | } 430 | 431 | #[allow(clippy::ptr_arg)] 432 | fn hessians_vec(jacobians: &Vec) -> Vec { 433 | jacobians.iter().map(|j| j * j.transpose()).collect() 434 | } 435 | -------------------------------------------------------------------------------- /examples/optim_regression-1d.rs: -------------------------------------------------------------------------------- 1 | // This Source Code Form is subject to the terms of the Mozilla Public 2 | // License, v. 2.0. If a copy of the MPL was not distributed with this 3 | // file, You can obtain one at http://mozilla.org/MPL/2.0/. 4 | 5 | extern crate nalgebra as na; 6 | extern crate rand; 7 | extern crate visual_odometry_rs as vors; 8 | 9 | use na::DVector; 10 | use rand::{distributions::Uniform, rngs::StdRng, SeedableRng}; 11 | use std::{f32, process::exit}; 12 | use vors::math::optimizer::{self, Continue, State as _}; 13 | 14 | /// In this example, we implement the `OptimizerState` trait to find the correct parameter `a` 15 | /// for modelling a noisy curve of the form: y = exp( -a * x ). 16 | /// Let f be the function: (a, x) -> exp( -a * x ). 17 | /// 18 | /// For this, we are using the Levenberg-Marquardt optimization schema. 19 | /// Note that any iterative optimization algorithm fitting the Optimizer trait 20 | /// could have been used instead. This just serves as an example. 21 | /// 22 | /// We thus try to minimize the expression: Sum_i ( |residual_i|^2 ) 23 | /// where each residual is: y_i - f(a, x_i) for a data point ( x_i, y_i ). 24 | /// 25 | /// The jacobian of the vector of residuals is obtained by deriving the expression with regard to a. 26 | /// Let f_1 be the derivative of f with regard a. 27 | /// f_1: (a, x) -> -x * f(a, x) 28 | /// For each residual, we thus obtain: 29 | /// jacobian_i = -x_i * f(a, x_i) 30 | /// 31 | /// The jacobian is 1-dimensional, to the hessian is a scalar: 32 | /// hessian = transpose(jacobian) * jacobian 33 | /// 34 | /// And the gradient is also a scalar: 35 | /// gradient = transpose(jacobian) * residuals 36 | /// 37 | /// So finally, if we note hessian_lm the slightly modified hessian (Levenberg-Marquardt coef), 38 | /// the iteration step is: 39 | /// step = - gradient / hessian 40 | 41 | fn main() { 42 | if let Err(err) = run() { 43 | eprintln!("{}", err); 44 | exit(1); 45 | }; 46 | } 47 | 48 | fn run() -> Result<(), String> { 49 | // Ground truth value for the parameter a. 50 | let a_ground_truth = 1.5; 51 | 52 | // Let's generate some noisy observations. 53 | let nb_data: usize = 100; 54 | let x_domain = linspace(-5.0, 3.0, nb_data); 55 | let seed = [0; 32]; 56 | let mut rng: StdRng = SeedableRng::from_seed(seed); 57 | let distribution = Uniform::from(-1.0..1.0); 58 | let noise = DVec::from_distribution(nb_data, &distribution, &mut rng); 59 | let y_data_noisy = f(a_ground_truth, &x_domain) + 0.1 * noise; 60 | let noisy_observations = Obs { 61 | x: x_domain, 62 | y: y_data_noisy, 63 | }; 64 | 65 | // Now we run the Levenberg-Marquardt optimizer 66 | // as defined below in the Optimizer trait implementation. 67 | let (final_state, nb_iteration) = LMOptimizerState::iterative_solve(&noisy_observations, 0.0)?; 68 | println!("After {} iterations:", nb_iteration); 69 | println!("Ground truth: a = {}", a_ground_truth); 70 | println!("Computed: a = {}", final_state.eval_data.model); 71 | Ok(()) 72 | } 73 | 74 | /// Simpler type alias for a vector of floats. 75 | type DVec = DVector; 76 | 77 | /// Helper function to generate regularly spaced values. 78 | fn linspace(start: f32, end: f32, nb: usize) -> DVec { 79 | DVec::from_fn(nb, |i, _| { 80 | (i as f32 * end + (nb - 1 - i) as f32 * start) / (nb as f32 - 1.0) 81 | }) 82 | } 83 | 84 | /// The function modelling our observations: y = exp(-a x) 85 | fn f(a: f32, x_domain: &DVec) -> DVec { 86 | x_domain.map(|x| (-a * x).exp()) 87 | } 88 | 89 | /// Type of the data observed: x and y coordinates of data points. 90 | struct Obs { 91 | x: DVec, 92 | y: DVec, 93 | } 94 | 95 | /// Levenberg-Marquardt (LM) state of the optimizer. 96 | struct LMOptimizerState { 97 | lm_coef: f32, 98 | eval_data: EvalData, 99 | } 100 | 101 | /// State useful to evaluate the model. 102 | struct EvalData { 103 | model: f32, 104 | energy: f32, 105 | gradient: f32, 106 | hessian: f32, 107 | } 108 | 109 | /// Same role as EvalData except we might not need to fully compute the EvalData. 110 | /// In such cases (evaluation stopped after noticing an increasing energy), 111 | /// the partial state only contains an error with the corresponding energy. 112 | type EvalState = Result; 113 | 114 | impl LMOptimizerState { 115 | /// Evaluate energy associated with a model. 116 | fn eval_energy(obs: &Obs, model: f32) -> (f32, DVec, DVec) { 117 | let f_model = f(model, &obs.x); 118 | let residuals = &f_model - &obs.y; 119 | let new_energy: f32 = residuals.iter().map(|r| r * r).sum(); 120 | (new_energy / residuals.len() as f32, residuals, f_model) 121 | } 122 | 123 | /// Compute evaluation data needed for the next iteration step. 124 | fn compute_eval_data(obs: &Obs, model: f32, pre: (f32, DVec, DVec)) -> EvalData { 125 | let (energy, residuals, f_model) = pre; 126 | let jacobian = -1.0 * f_model.component_mul(&obs.x); 127 | let gradient = jacobian.dot(&residuals); 128 | let hessian = jacobian.dot(&jacobian); 129 | EvalData { 130 | model, 131 | energy, 132 | gradient, 133 | hessian, 134 | } 135 | } 136 | } 137 | 138 | impl optimizer::State for LMOptimizerState { 139 | /// Initialize the optimizer state. 140 | /// Levenberg-Marquardt coefficient start at 0.1. 141 | fn init(obs: &Obs, model: f32) -> Self { 142 | Self { 143 | lm_coef: 0.1, 144 | eval_data: Self::compute_eval_data(obs, model, Self::eval_energy(obs, model)), 145 | } 146 | } 147 | 148 | /// Compute the Levenberg-Marquardt step. 149 | fn step(&self) -> Result { 150 | let hessian = (1.0 + self.lm_coef) * self.eval_data.hessian; 151 | Ok(self.eval_data.model - self.eval_data.gradient / hessian) 152 | } 153 | 154 | /// Evaluate the new model. 155 | fn eval(&self, obs: &Obs, model: f32) -> EvalState { 156 | let pre = Self::eval_energy(obs, model); 157 | let energy = pre.0; 158 | let old_energy = self.eval_data.energy; 159 | if energy > old_energy { 160 | Err(energy) 161 | } else { 162 | Ok(Self::compute_eval_data(obs, model, pre)) 163 | } 164 | } 165 | 166 | /// Decide if iterations should continue. 167 | fn stop_criterion(self, nb_iter: usize, eval_state: EvalState) -> (Self, Continue) { 168 | let too_many_iterations = nb_iter >= 20; 169 | match (eval_state, too_many_iterations) { 170 | // Max number of iterations reached: 171 | (Err(_), true) => (self, Continue::Stop), 172 | (Ok(eval_data), true) => { 173 | println!("a = {}, energy = {}", eval_data.model, eval_data.energy); 174 | let mut kept_state = self; 175 | kept_state.eval_data = eval_data; 176 | (kept_state, Continue::Stop) 177 | } 178 | // Can continue to iterate: 179 | (Err(model), false) => { 180 | let mut kept_state = self; 181 | kept_state.lm_coef *= 10.0; 182 | println!("\t back from {}, lm_coef = {}", model, kept_state.lm_coef); 183 | (kept_state, Continue::Forward) 184 | } 185 | (Ok(eval_data), false) => { 186 | println!("a = {}, energy = {}", eval_data.model, eval_data.energy); 187 | let delta_energy = self.eval_data.energy - eval_data.energy; 188 | let mut kept_state = self; 189 | kept_state.lm_coef *= 0.1; 190 | kept_state.eval_data = eval_data; 191 | let continuation = if delta_energy > 0.01 { 192 | Continue::Forward 193 | } else { 194 | Continue::Stop 195 | }; 196 | (kept_state, continuation) 197 | } 198 | } 199 | } 200 | } 201 | -------------------------------------------------------------------------------- /examples/optim_rosenbrock.rs: -------------------------------------------------------------------------------- 1 | // This Source Code Form is subject to the terms of the Mozilla Public 2 | // License, v. 2.0. If a copy of the MPL was not distributed with this 3 | // file, You can obtain one at http://mozilla.org/MPL/2.0/. 4 | 5 | extern crate nalgebra as na; 6 | extern crate visual_odometry_rs as vors; 7 | 8 | use std::process::exit; 9 | use vors::math::optimizer::{self, Continue, State as _}; 10 | 11 | /// In this example, we implement the OptimizerState trait to estimate a rosenbrock function. 12 | /// rosenbrock: (a, b, x, y) -> (a-x)^2 + b*(y-x^2)^2 13 | /// 14 | /// The two residuals are: r1 = (a-x)^2 and r2 = b*(y-x^2)^2 15 | /// So the jacobian matrix is: 16 | /// | -2.0 * (a - x) , 0.0 | 17 | /// | -4.0 * b * x * (y - x * x), 2.0 * b * (y - x * x) | 18 | fn main() { 19 | if let Err(err) = run() { 20 | eprintln!("{}", err); 21 | exit(1); 22 | }; 23 | } 24 | 25 | fn run() -> Result<(), String> { 26 | let initial_model = (-2.0, -2.0); 27 | let (final_state, nb_iteration) = LMOptimizerState::iterative_solve(&(), initial_model)?; 28 | println!("After {} iterations:", nb_iteration); 29 | println!("Computed: {:?}", final_state.eval_data.model); 30 | println!("Solution: (1.0, 1.0)"); 31 | Ok(()) 32 | } 33 | 34 | type Vec2 = na::Vector2; 35 | type Mat2 = na::Matrix2; 36 | 37 | const A: f32 = 1.0; 38 | const B: f32 = 100.0; 39 | 40 | /// Residuals 41 | fn rosenbrock_res(x: f32, y: f32) -> Vec2 { 42 | Vec2::new((A - x).powi(2), B * (y - x * x).powi(2)) 43 | } 44 | 45 | /// Jacobian 46 | #[rustfmt::skip] 47 | fn rosenbrock_jac(x: f32, y: f32) -> Mat2 { 48 | Mat2::new( 49 | -2.0 * (A - x) , 0.0 , // for 1st residual 50 | -4.0 * B * x * (y - x * x), 2.0 * B * (y - x * x), // for 2nd residual 51 | ) 52 | } 53 | 54 | /// Levenberg-Marquardt (LM) state of the optimizer. 55 | struct LMOptimizerState { 56 | lm_coef: f32, 57 | eval_data: EvalData, 58 | } 59 | 60 | /// State useful to evaluate the model. 61 | struct EvalData { 62 | model: (f32, f32), 63 | energy: f32, 64 | gradient: Vec2, 65 | hessian: Mat2, 66 | } 67 | 68 | /// Same role as EvalData except we might not need to fully compute the EvalData. 69 | /// In such cases (evaluation stopped after noticing an increasing energy), 70 | /// the partial state only contains an error with the corresponding energy. 71 | type EvalState = Result; 72 | 73 | impl LMOptimizerState { 74 | /// Compute evaluation data needed for the next iteration step. 75 | fn compute_eval_data(model: (f32, f32), residuals: Vec2) -> EvalData { 76 | let (x, y) = model; 77 | let energy = residuals.norm_squared(); 78 | let jacobian = rosenbrock_jac(x, y); 79 | let jacobian_t = jacobian.transpose(); 80 | let gradient = jacobian_t * residuals; 81 | let hessian = jacobian_t * jacobian; 82 | EvalData { 83 | model, 84 | energy, 85 | gradient, 86 | hessian, 87 | } 88 | } 89 | } 90 | 91 | impl optimizer::State<(), EvalState, (f32, f32), String> for LMOptimizerState { 92 | /// Initialize the optimizer state. 93 | /// Levenberg-Marquardt coefficient start at 0.1. 94 | fn init(_obs: &(), model: (f32, f32)) -> Self { 95 | Self { 96 | lm_coef: 0.1, 97 | eval_data: Self::compute_eval_data(model, rosenbrock_res(model.0, model.1)), 98 | } 99 | } 100 | 101 | /// Compute the Levenberg-Marquardt step. 102 | fn step(&self) -> Result<(f32, f32), String> { 103 | let mut hessian = self.eval_data.hessian; 104 | hessian.m11 *= 1.0 + self.lm_coef; 105 | hessian.m22 *= 1.0 + self.lm_coef; 106 | let cholesky = hessian.cholesky().ok_or("Issue in Cholesky")?; 107 | let delta = cholesky.solve(&self.eval_data.gradient); 108 | let (x, y) = self.eval_data.model; 109 | Ok((x - delta.x, y - delta.y)) 110 | } 111 | 112 | /// Evaluate the new model. 113 | fn eval(&self, _obs: &(), model: (f32, f32)) -> EvalState { 114 | let (x, y) = model; 115 | let residuals = rosenbrock_res(x, y); 116 | let energy = residuals.norm_squared(); 117 | let old_energy = self.eval_data.energy; 118 | if energy > old_energy { 119 | Err(energy) 120 | } else { 121 | Ok(Self::compute_eval_data(model, residuals)) 122 | } 123 | } 124 | 125 | /// Decide if iterations should continue. 126 | fn stop_criterion(self, nb_iter: usize, eval_state: EvalState) -> (Self, Continue) { 127 | let too_many_iterations = nb_iter >= 100; 128 | match (eval_state, too_many_iterations) { 129 | // Max number of iterations reached: 130 | (Err(_), true) => (self, Continue::Stop), 131 | (Ok(eval_data), true) => { 132 | println!( 133 | "model = {:?}, energy = {}", 134 | eval_data.model, eval_data.energy 135 | ); 136 | let mut kept_state = self; 137 | kept_state.eval_data = eval_data; 138 | (kept_state, Continue::Stop) 139 | } 140 | // Can continue to iterate: 141 | (Err(model), false) => { 142 | let mut kept_state = self; 143 | kept_state.lm_coef *= 10.0; 144 | println!("\t back from {:?}, lm_coef = {}", model, kept_state.lm_coef); 145 | (kept_state, Continue::Forward) 146 | } 147 | (Ok(eval_data), false) => { 148 | println!( 149 | "model = {:?}, energy = {}", 150 | eval_data.model, eval_data.energy 151 | ); 152 | let delta_energy = self.eval_data.energy - eval_data.energy; 153 | let mut kept_state = self; 154 | kept_state.lm_coef *= 0.1; 155 | kept_state.eval_data = eval_data; 156 | let continuation = if delta_energy > 1e-10 { 157 | Continue::Forward 158 | } else { 159 | Continue::Stop 160 | }; 161 | (kept_state, continuation) 162 | } 163 | } 164 | } 165 | } 166 | -------------------------------------------------------------------------------- /src/bin/vors_track.rs: -------------------------------------------------------------------------------- 1 | // This Source Code Form is subject to the terms of the Mozilla Public 2 | // License, v. 2.0. If a copy of the MPL was not distributed with this 3 | // file, You can obtain one at http://mozilla.org/MPL/2.0/. 4 | 5 | extern crate image; 6 | extern crate nalgebra as na; 7 | extern crate visual_odometry_rs as vors; 8 | 9 | use na::DMatrix; 10 | use std::{env, error::Error, fs, io::BufReader, io::Read, path::Path, path::PathBuf}; 11 | 12 | use vors::core::camera::Intrinsics; 13 | use vors::core::track::inverse_compositional as track; 14 | use vors::dataset::tum_rgbd; 15 | use vors::misc::{helper, interop}; 16 | 17 | fn main() { 18 | let args: Vec = env::args().collect(); 19 | if let Err(error) = my_run(&args) { 20 | eprintln!("{:?}", error); 21 | } 22 | } 23 | 24 | const USAGE: &str = "Usage: ./vors_track [fr1|fr2|fr3|icl] associations_file"; 25 | 26 | fn my_run(args: &[String]) -> Result<(), Box> { 27 | // Check that the arguments are correct. 28 | let valid_args = check_args(args)?; 29 | 30 | // Build a vector containing timestamps and full paths of images. 31 | let associations = parse_associations(valid_args.associations_file_path)?; 32 | 33 | // Setup tracking configuration. 34 | let config = track::Config { 35 | nb_levels: 6, 36 | candidates_diff_threshold: 7, 37 | depth_scale: tum_rgbd::DEPTH_SCALE, 38 | intrinsics: valid_args.intrinsics, 39 | idepth_variance: 0.0001, 40 | }; 41 | 42 | // Initialize tracker with first depth and color image. 43 | let (depth_map, img) = read_images(&associations[0])?; 44 | let depth_time = associations[0].depth_timestamp; 45 | let img_time = associations[0].color_timestamp; 46 | let mut tracker = config.init(depth_time, &depth_map, img_time, img); 47 | 48 | // Track every frame in the associations file. 49 | for assoc in associations.iter().skip(1) { 50 | // Load depth and color images. 51 | let (depth_map, img) = read_images(assoc)?; 52 | 53 | // Track the rgb-d image. 54 | tracker.track( 55 | assoc.depth_timestamp, 56 | &depth_map, 57 | assoc.color_timestamp, 58 | img, 59 | ); 60 | 61 | // Print to stdout the frame pose. 62 | let (timestamp, pose) = tracker.current_frame(); 63 | println!("{}", (tum_rgbd::Frame { timestamp, pose }).to_string()); 64 | } 65 | 66 | Ok(()) 67 | } 68 | 69 | struct Args { 70 | associations_file_path: PathBuf, 71 | intrinsics: Intrinsics, 72 | } 73 | 74 | /// Verify that command line arguments are correct. 75 | fn check_args(args: &[String]) -> Result { 76 | // eprintln!("{:?}", args); 77 | if let [_, camera_id, associations_file_path_str] = args { 78 | let intrinsics = create_camera(camera_id)?; 79 | let associations_file_path = PathBuf::from(associations_file_path_str); 80 | if associations_file_path.is_file() { 81 | Ok(Args { 82 | intrinsics, 83 | associations_file_path, 84 | }) 85 | } else { 86 | eprintln!("{}", USAGE); 87 | Err(format!( 88 | "The association file does not exist or is not reachable: {}", 89 | associations_file_path_str 90 | )) 91 | } 92 | } else { 93 | eprintln!("{}", USAGE); 94 | Err("Wrong number of arguments".to_string()) 95 | } 96 | } 97 | 98 | /// Create camera depending on `camera_id` command line argument. 99 | fn create_camera(camera_id: &str) -> Result { 100 | match camera_id { 101 | "fr1" => Ok(tum_rgbd::INTRINSICS_FR1), 102 | "fr2" => Ok(tum_rgbd::INTRINSICS_FR2), 103 | "fr3" => Ok(tum_rgbd::INTRINSICS_FR3), 104 | "icl" => Ok(tum_rgbd::INTRINSICS_ICL_NUIM), 105 | _ => { 106 | eprintln!("{}", USAGE); 107 | Err(format!("Unknown camera id: {}", camera_id)) 108 | } 109 | } 110 | } 111 | 112 | /// Open an association file and parse it into a vector of Association. 113 | fn parse_associations>( 114 | file_path: P, 115 | ) -> Result, Box> { 116 | let file = fs::File::open(&file_path)?; 117 | let mut file_reader = BufReader::new(file); 118 | let mut content = String::new(); 119 | file_reader.read_to_string(&mut content)?; 120 | tum_rgbd::parse::associations(&content) 121 | .map(|v| v.iter().map(|a| abs_path(&file_path, a)).collect()) 122 | .map_err(|s| s.into()) 123 | } 124 | 125 | /// Transform relative images file paths into absolute ones. 126 | fn abs_path>(file_path: P, assoc: &tum_rgbd::Association) -> tum_rgbd::Association { 127 | let parent = file_path 128 | .as_ref() 129 | .parent() 130 | .expect("How can this have no parent"); 131 | tum_rgbd::Association { 132 | depth_timestamp: assoc.depth_timestamp, 133 | depth_file_path: parent.join(&assoc.depth_file_path), 134 | color_timestamp: assoc.color_timestamp, 135 | color_file_path: parent.join(&assoc.color_file_path), 136 | } 137 | } 138 | 139 | /// Read a depth and color image given by an association. 140 | fn read_images(assoc: &tum_rgbd::Association) -> Result<(DMatrix, DMatrix), Box> { 141 | let (w, h, depth_map_vec_u16) = helper::read_png_16bits(&assoc.depth_file_path)?; 142 | let depth_map = DMatrix::from_row_slice(h, w, depth_map_vec_u16.as_slice()); 143 | let img = interop::matrix_from_image(image::open(&assoc.color_file_path)?.to_luma()); 144 | Ok((depth_map, img)) 145 | } 146 | -------------------------------------------------------------------------------- /src/core/camera.rs: -------------------------------------------------------------------------------- 1 | // This Source Code Form is subject to the terms of the Mozilla Public 2 | // License, v. 2.0. If a copy of the MPL was not distributed with this 3 | // file, You can obtain one at http://mozilla.org/MPL/2.0/. 4 | 5 | //! Helper types and functions to manipulate camera poses and projections. 6 | 7 | use nalgebra::Affine2; 8 | 9 | use crate::core::multires; 10 | use crate::misc::type_aliases::{Float, Iso3, Mat3, Point2, Point3, Vec3}; 11 | 12 | /// A camera has intrinsic and extrinsic parameters. 13 | /// Warning: extrinsics here is the pose of the camera, 14 | /// not directly the projection matrix parameters. 15 | /// 16 | /// As it stands, it is currently limited to the pinhole camera model. 17 | #[derive(PartialEq, Debug, Clone)] 18 | pub struct Camera { 19 | /// Intrinsic parameters of the camera. 20 | pub intrinsics: Intrinsics, 21 | /// Extrinsic parameters of the camera pose. 22 | pub extrinsics: Extrinsics, 23 | } 24 | 25 | impl Camera { 26 | /// Initialize a camera from intrinsic and extrinsic parameters. 27 | pub fn new(intrinsics: Intrinsics, extrinsics: Extrinsics) -> Self { 28 | Self { 29 | intrinsics, 30 | extrinsics, 31 | } 32 | } 33 | 34 | /// Project a 3D point into its corresponding pixel in the image generated by the camera. 35 | /// Result is still in homogeneous coordinates (thus `Vec3`). 36 | pub fn project(&self, point: Point3) -> Vec3 { 37 | self.intrinsics 38 | .project(extrinsics::project(&self.extrinsics, point)) 39 | } 40 | 41 | /// From a 2D pixel position and a depth info, 42 | /// back project this point into the 3D world. 43 | pub fn back_project(&self, point: Point2, depth: Float) -> Point3 { 44 | extrinsics::back_project(&self.extrinsics, self.intrinsics.back_project(point, depth)) 45 | } 46 | 47 | /// Generate a multi-resolution camera. 48 | /// Extrinsics are left intact, but intrinsics are scaled at each level. 49 | pub fn multi_res(self, n: usize) -> Vec { 50 | multires::limited_sequence(n, self, |cam| Some(cam.half_res())) 51 | } 52 | 53 | /// Generate a camera corresponding to an image with half resolution. 54 | /// Extrinsics are left intact, but intrinsics are scaled. 55 | pub fn half_res(&self) -> Self { 56 | Self::new(self.intrinsics.half_res(), self.extrinsics) 57 | } 58 | } 59 | 60 | // EXTRINSICS ############################################## 61 | 62 | /// Extrinsic parameters are represented by a rigid body motion (or direct isometry). 63 | pub type Extrinsics = Iso3; 64 | 65 | /// Module regrouping functions operating on extrinsics. 66 | pub mod extrinsics { 67 | use super::*; 68 | 69 | /// Project a 3D point from world coordinates to camera coordinates. 70 | pub fn project(pose: &Extrinsics, point: Point3) -> Point3 { 71 | pose.rotation.inverse() * (pose.translation.inverse() * point) 72 | } 73 | 74 | /// Back project a 3D point from camera coordinates to world coordinates. 75 | pub fn back_project(pose: &Extrinsics, point: Point3) -> Point3 { 76 | pose * point 77 | } 78 | } 79 | 80 | // INTRINSICS ############################################## 81 | 82 | /// Intrinsic parameters of a pinhole camera model. 83 | #[derive(PartialEq, Debug, Clone)] 84 | pub struct Intrinsics { 85 | /// Principal point (in the optical center axis) of the camera. 86 | pub principal_point: (Float, Float), 87 | /// Focal length in pixels along both axes. 88 | pub focal: (Float, Float), 89 | /// Skew of the camera, usually 0.0. 90 | pub skew: Float, 91 | } 92 | 93 | impl Intrinsics { 94 | /// Equivalent matrix representation of intrinsic parameters. 95 | #[rustfmt::skip] 96 | pub fn matrix(&self) -> Affine2 { 97 | Affine2::from_matrix_unchecked(Mat3::new( 98 | self.focal.0, self.skew, self.principal_point.0, 99 | 0.0, self.focal.1, self.principal_point.1, 100 | 0.0, 0.0, 1.0, 101 | )) 102 | } 103 | 104 | /// Generate a multi-resolution vector of intrinsic parameters. 105 | /// Each level corresponds to a camera with half resolution. 106 | pub fn multi_res(self, n: usize) -> Vec { 107 | multires::limited_sequence(n, self, |intrinsics| Some(intrinsics.half_res())) 108 | } 109 | 110 | /// Compute intrinsic parameters of a camera with half resolution. 111 | /// 112 | /// Since the (0,0) coordinates correspond the center of the first pixel, 113 | /// and not its top left corner, a shift of 0.5 is performed 114 | /// for the principal point before and after the resolution scaling. 115 | pub fn half_res(&self) -> Self { 116 | let (cx, cy) = self.principal_point; 117 | let (fx, fy) = self.focal; 118 | Self { 119 | principal_point: ((cx + 0.5) / 2.0 - 0.5, (cy + 0.5) / 2.0 - 0.5), 120 | focal: (0.5 * fx, 0.5 * fy), 121 | skew: self.skew, 122 | } 123 | } 124 | 125 | /// Project a 3D point in camera coordinates into a 2D homogeneous image point. 126 | pub fn project(&self, point: Point3) -> Vec3 { 127 | Vec3::new( 128 | self.focal.0 * point[0] + self.skew * point[1] + self.principal_point.0 * point[2], 129 | self.focal.1 * point[1] + self.principal_point.1 * point[2], 130 | point[2], 131 | ) 132 | } 133 | 134 | /// Back project a pixel with depth info into a 3D point in camera coordinates. 135 | pub fn back_project(&self, point: Point2, depth: Float) -> Point3 { 136 | let z = depth; 137 | let y = (point[1] - self.principal_point.1) * z / self.focal.1; 138 | let x = ((point[0] - self.principal_point.0) * z - self.skew * y) / self.focal.0; 139 | Point3::new(x, y, z) 140 | } 141 | } 142 | -------------------------------------------------------------------------------- /src/core/candidates/coarse_to_fine.rs: -------------------------------------------------------------------------------- 1 | // This Source Code Form is subject to the terms of the Mozilla Public 2 | // License, v. 2.0. If a copy of the MPL was not distributed with this 3 | // file, You can obtain one at http://mozilla.org/MPL/2.0/. 4 | 5 | //! Candidates points selection in a coarse to fine manner. 6 | 7 | use nalgebra::{DMatrix, Scalar}; 8 | 9 | /// Select a subset of points satisfying two conditions: 10 | /// * points shall be well-distributed in the image. 11 | /// * higher density where gradients are bigger. 12 | /// 13 | /// Each level is kept but important one 14 | /// is the one at the highest resolution (the last one). 15 | pub fn select(diff_threshold: T, gradients: &[DMatrix]) -> Vec> 16 | where 17 | T: Scalar + std::cmp::PartialOrd + std::ops::Add, 18 | { 19 | let (nrows, ncols) = gradients.last().unwrap().shape(); 20 | let mut init_candidates = Vec::new(); 21 | init_candidates.push(DMatrix::repeat(nrows, ncols, true)); 22 | let prune = |a, b, c, d| prune_with_thresh(diff_threshold, a, b, c, d); 23 | gradients 24 | .iter() 25 | .rev() // start with lower res 26 | .skip(1) // skip lower since all points are good 27 | .fold(init_candidates, |mut multires_masks, grad_mat| { 28 | let new_mask = select_2x2_bloc(multires_masks.last().unwrap(), grad_mat, prune); 29 | multires_masks.push(new_mask); 30 | multires_masks 31 | }) 32 | } 33 | 34 | /// Apply a predicate function on each 2x2 bloc. 35 | /// Only evaluate the function in selected blocs in the half resolution `pre_mask`. 36 | #[allow(clippy::many_single_char_names)] 37 | fn select_2x2_bloc(pre_mask: &DMatrix, mat: &DMatrix, f: F) -> DMatrix 38 | where 39 | T: Scalar, 40 | F: Fn(T, T, T, T) -> [bool; 4], 41 | { 42 | let (nrows, ncols) = mat.shape(); 43 | let (nrows_2, ncols_2) = pre_mask.shape(); 44 | assert_eq!((nrows_2, ncols_2), (nrows / 2, ncols / 2)); 45 | let mut mask = DMatrix::repeat(nrows, ncols, false); 46 | for j in 0..(ncols_2) { 47 | for i in 0..(nrows_2) { 48 | if pre_mask[(i, j)] { 49 | let a = mat[(2 * i, 2 * j)]; 50 | let b = mat[(2 * i + 1, 2 * j)]; 51 | let c = mat[(2 * i, 2 * j + 1)]; 52 | let d = mat[(2 * i + 1, 2 * j + 1)]; 53 | let ok = f(a, b, c, d); 54 | mask[(2 * i, 2 * j)] = ok[0]; 55 | mask[(2 * i + 1, 2 * j)] = ok[1]; 56 | mask[(2 * i, 2 * j + 1)] = ok[2]; 57 | mask[(2 * i + 1, 2 * j + 1)] = ok[3]; 58 | } 59 | } 60 | } 61 | mask 62 | } 63 | 64 | /// Discard the 2 or 3 lowest values. 65 | /// The second higher value is kept only if: 66 | /// second > third + thresh 67 | /// 68 | /// For example: with thresh = 5 69 | /// ( 0, 1, 8, 9 ) -> [ false, false, true, true ] 70 | /// ( 0, 9, 1, 8 ) -> [ false, true, false, true ] 71 | /// ( 1, 0, 9, 0 ) -> [ false, false, true, false ] 72 | #[allow(clippy::many_single_char_names)] 73 | fn prune_with_thresh(thresh: T, a: T, b: T, c: T, d: T) -> [bool; 4] 74 | where 75 | T: Scalar + std::cmp::PartialOrd + std::ops::Add, 76 | { 77 | // let thresh = 7.0 / 255.0; 78 | let mut temp = [(a, 0_usize), (b, 1_usize), (c, 2_usize), (d, 3_usize)]; 79 | temp.sort_unstable_by(|(x, _), (y, _)| x.partial_cmp(y).unwrap()); 80 | let (_, first) = temp[3]; 81 | let (x, second) = temp[2]; 82 | let (y, _) = temp[1]; 83 | let mut result = [false; 4]; 84 | result[first] = true; 85 | if x > y + thresh { 86 | result[second] = true; 87 | } 88 | result 89 | } 90 | -------------------------------------------------------------------------------- /src/core/candidates/dso.rs: -------------------------------------------------------------------------------- 1 | // This Source Code Form is subject to the terms of the Mozilla Public 2 | // License, v. 2.0. If a copy of the MPL was not distributed with this 3 | // file, You can obtain one at http://mozilla.org/MPL/2.0/. 4 | 5 | //! Candidates point selection according to 6 | //! "Direct Sparse Odometry", J.Engel, V.Koltun, D. Cremers, PAMI 2018. 7 | 8 | use nalgebra::{DMatrix, Scalar}; 9 | use num_traits::{self, cast::AsPrimitive, NumCast}; 10 | use rand::Rng; 11 | use std::ops::{Add, Div, Mul}; 12 | 13 | use crate::core::multires; 14 | use crate::misc::helper::div_rem; 15 | use crate::misc::type_aliases::Float; 16 | 17 | /// Trait for manipulating numbers types. 18 | pub trait Number: 19 | Scalar 20 | + Ord 21 | + NumCast 22 | + Add 23 | + Div 24 | + Mul 25 | + AsPrimitive 26 | + std::fmt::Display 27 | { 28 | } 29 | 30 | impl Number for u16 {} 31 | 32 | /// 0: not picked 33 | /// n: picked at level n 34 | pub type Picked = u8; 35 | 36 | /// Configuration of regions. 37 | pub struct RegionConfig { 38 | /// The region size. 39 | pub size: usize, 40 | /// Some coefficients used for the region threshold computation. 41 | pub threshold_coefs: (Float, T), 42 | } 43 | 44 | /// Configuration of blocks. 45 | #[derive(Copy, Clone)] 46 | pub struct BlockConfig { 47 | /// Base size of a block. 48 | pub base_size: usize, 49 | /// Number of levels for picking points in blocks. 50 | pub nb_levels: usize, 51 | /// Multiplier factor for block threshold computation. 52 | pub threshold_factor: Float, 53 | } 54 | 55 | /// Configuration of the recursive nature of candidates selection. 56 | /// If the number of points obtained after one iteration is not within 57 | /// given bounds, the algorithm adapts the base block size and re-iterates. 58 | pub struct RecursiveConfig { 59 | /// Max number of iterations left. 60 | pub nb_iterations_left: usize, 61 | /// Low percentage threshold of target number of points. 62 | pub low_thresh: Float, 63 | /// High percentage threshold of target number of points. 64 | pub high_thresh: Float, 65 | /// Threshold such that if we have random_thresh < points_ratio < high_thresh, 66 | /// we randomly sample points to have approximatel the desired target number of candidate 67 | /// points. 68 | pub random_thresh: Float, 69 | } 70 | 71 | /// Default region configuration according to DSO paper and code. 72 | pub const DEFAULT_REGION_CONFIG: RegionConfig = RegionConfig { 73 | size: 32, 74 | threshold_coefs: (1.0, 3), // (2.0, 3) in dso and (1.0, 3) in ldso 75 | }; 76 | 77 | /// Default block configuration according to DSO paper and code. 78 | pub const DEFAULT_BLOCK_CONFIG: BlockConfig = BlockConfig { 79 | base_size: 4, 80 | nb_levels: 3, 81 | threshold_factor: 0.5, 82 | }; 83 | 84 | /// Default recursive configuration according to DSO paper and code. 85 | pub const DEFAULT_RECURSIVE_CONFIG: RecursiveConfig = RecursiveConfig { 86 | nb_iterations_left: 1, 87 | low_thresh: 0.8, 88 | high_thresh: 4.0, 89 | random_thresh: 1.1, 90 | }; 91 | 92 | /// Select a subset of points satisfying two conditions: 93 | /// * points shall be well-distributed in the image. 94 | /// * higher density where gradients are bigger. 95 | #[allow(clippy::cast_possible_truncation)] 96 | #[allow(clippy::cast_sign_loss)] 97 | #[allow(clippy::cast_precision_loss)] 98 | pub fn select>( 99 | gradients: &DMatrix, 100 | region_config: RegionConfig, 101 | block_config: BlockConfig, 102 | recursive_config: RecursiveConfig, 103 | nb_target: usize, 104 | ) -> DMatrix { 105 | // Pick all block candidates 106 | let median_gradients = region_median_gradients(gradients, region_config.size); 107 | let regions_thresholds = region_thresholds(&median_gradients, region_config.threshold_coefs); 108 | let (vec_nb_candidates, picked) = pick_all_block_candidates( 109 | block_config, 110 | region_config.size, 111 | ®ions_thresholds, 112 | gradients, 113 | ); 114 | let nb_candidates: usize = vec_nb_candidates.iter().sum(); 115 | // eprintln!("Number of points picked by level: {:?}", vec_nb_candidates); 116 | // eprintln!("nb_candidates: {}", nb_candidates); 117 | let candidates_ratio = nb_candidates as Float / nb_target as Float; 118 | // The number of selected pixels behave approximately as 119 | // nb_candidates = K / (block_size + 1)^2 where K is scene dependant. 120 | // nb_target = K / (target_size + 1)^2 121 | // So sqrt( candidates_ratio ) = (target_size + 1) / (block_size + 1) 122 | // and in theory: 123 | // target_size = sqrt( ratio ) * (block_size + 1) - 1 124 | let target_size = candidates_ratio.sqrt() * (block_config.base_size as Float + 1.0) - 1.0; 125 | let target_size = std::cmp::max(1, target_size.round() as i32) as usize; 126 | // eprintln!("base_size: {}", block_config.base_size); 127 | // eprintln!("target_size: {}", target_size); 128 | if candidates_ratio < recursive_config.low_thresh 129 | || candidates_ratio > recursive_config.high_thresh 130 | { 131 | if target_size != block_config.base_size && recursive_config.nb_iterations_left > 0 { 132 | let mut b_config = block_config; 133 | b_config.base_size = target_size; 134 | let mut rec_config = recursive_config; 135 | rec_config.nb_iterations_left -= 1; 136 | select(gradients, region_config, b_config, rec_config, nb_target) 137 | } else { 138 | to_mask(&picked) 139 | } 140 | } else if candidates_ratio > recursive_config.random_thresh { 141 | // randomly select a correct % of points 142 | let mut rng = rand::thread_rng(); 143 | picked.map(|p| p > 0 && rng.gen::() <= (255.0 / candidates_ratio) as u8) 144 | } else { 145 | to_mask(&picked) 146 | } 147 | } 148 | 149 | /// Create a mask of picked points. 150 | fn to_mask(picked: &DMatrix) -> DMatrix { 151 | picked.map(|p| p > 0) 152 | } 153 | 154 | /// Pick candidates at all the block levels. 155 | #[allow(clippy::cast_possible_truncation)] 156 | fn pick_all_block_candidates>( 157 | block_config: BlockConfig, 158 | regions_size: usize, 159 | regions_thresholds: &DMatrix, 160 | gradients: &DMatrix, 161 | ) -> (Vec, DMatrix) { 162 | let (nb_rows, nb_cols) = gradients.shape(); 163 | let max_gradients_0 = init_max_gradients(gradients, block_config.base_size); 164 | let max_gradients_multires = 165 | multires::limited_sequence(block_config.nb_levels, max_gradients_0, |m| { 166 | multires::halve(m, max_of_four_gradients) 167 | }); 168 | let mut threshold_level_coef = 1.0; 169 | let mut nb_picked = Vec::new(); 170 | let (blocks_rows, blocks_cols) = max_gradients_multires[0].shape(); 171 | let mut mask = DMatrix::repeat(blocks_rows, blocks_cols, true); 172 | let mut candidates = DMatrix::repeat(nb_rows, nb_cols, 0); 173 | for (level, max_gradients_level) in max_gradients_multires.iter().enumerate() { 174 | // call pick_level_block_candidates() 175 | let (nb_picked_level, mask_next_level, new_candidates) = pick_level_block_candidates( 176 | threshold_level_coef, 177 | (level + 1) as u8, 178 | regions_size, 179 | regions_thresholds, 180 | max_gradients_level, 181 | &mask, 182 | candidates, 183 | ); 184 | nb_picked.push(nb_picked_level); 185 | mask = mask_next_level; 186 | candidates = new_candidates; 187 | threshold_level_coef *= block_config.threshold_factor; 188 | } 189 | (nb_picked, candidates) 190 | } 191 | 192 | /// Retrieve the pixel with max gradient for each block in the image. 193 | fn init_max_gradients>( 194 | gradients: &DMatrix, 195 | block_size: usize, 196 | ) -> DMatrix<(T, usize, usize)> { 197 | let (nb_rows, nb_cols) = gradients.shape(); 198 | let nb_rows_blocks = match div_rem(nb_rows, block_size) { 199 | (quot, 0) => quot, 200 | (quot, _) => quot + 1, 201 | }; 202 | let nb_cols_blocks = match div_rem(nb_cols, block_size) { 203 | (quot, 0) => quot, 204 | (quot, _) => quot + 1, 205 | }; 206 | DMatrix::from_fn(nb_rows_blocks, nb_cols_blocks, |bi, bj| { 207 | let start_i = bi * block_size; 208 | let start_j = bj * block_size; 209 | let end_i = std::cmp::min(start_i + block_size, nb_rows); 210 | let end_j = std::cmp::min(start_j + block_size, nb_cols); 211 | let mut tmp_max = (gradients[(start_i, start_j)], start_i, start_j); 212 | for j in start_j..end_j { 213 | for i in start_i..end_i { 214 | let g = gradients[(i, j)]; 215 | if g > tmp_max.0 { 216 | tmp_max = (g, i, j); 217 | } 218 | } 219 | } 220 | tmp_max 221 | }) 222 | } 223 | 224 | /// Retrieve the max and position of 4 gradients. 225 | fn max_of_four_gradients>( 226 | g1: (T, usize, usize), 227 | g2: (T, usize, usize), 228 | g3: (T, usize, usize), 229 | g4: (T, usize, usize), 230 | ) -> (T, usize, usize) { 231 | let g_max = |g_m1: (T, usize, usize), g_m2: (T, usize, usize)| { 232 | if g_m1.0 < g_m2.0 { 233 | g_m2 234 | } else { 235 | g_m1 236 | } 237 | }; 238 | g_max(g1, g_max(g2, g_max(g3, g4))) 239 | } 240 | 241 | /// For each block where the mask is "true", 242 | /// Select the pixel with the highest gradient magnitude. 243 | /// Returns the number of selected points and two matrices: 244 | /// * a mask of blocks to test for the next level, 245 | /// * the updated picked candidates. 246 | fn pick_level_block_candidates>( 247 | threshold_level_coef: Float, 248 | level: Picked, 249 | regions_size: usize, 250 | regions_thresholds: &DMatrix, 251 | max_gradients: &DMatrix<(T, usize, usize)>, 252 | mask: &DMatrix, 253 | candidates: DMatrix, 254 | ) -> (usize, DMatrix, DMatrix) { 255 | let (mask_height, mask_width) = mask.shape(); 256 | let mut mask_next_level = DMatrix::repeat(mask_height / 2, mask_width / 2, true); 257 | let mut candidates = candidates; 258 | let mut nb_picked = 0; 259 | // We use mask_width / 2 * 2 to avoid remainder pixels 260 | for j in 0..(mask_width / 2 * 2) { 261 | for i in 0..(mask_height / 2 * 2) { 262 | if mask[(i, j)] { 263 | let (g2, i_g, j_g) = max_gradients[(i, j)]; 264 | let threshold = regions_thresholds[(i_g / regions_size, j_g / regions_size)]; 265 | if g2.as_() >= threshold_level_coef * threshold.as_() { 266 | mask_next_level[(i / 2, j / 2)] = false; 267 | candidates[(i_g, j_g)] = level; 268 | nb_picked += 1; 269 | } 270 | } else { 271 | mask_next_level[(i / 2, j / 2)] = false; 272 | } 273 | } 274 | } 275 | (nb_picked, mask_next_level, candidates) 276 | } 277 | 278 | /// Smooth the medians and set thresholds given some coefficients (a,b): 279 | /// threshold = a * ( smooth( median ) + b ) ^ 2. 280 | #[allow(clippy::cast_precision_loss)] 281 | #[allow(clippy::cast_possible_wrap)] 282 | #[allow(clippy::cast_sign_loss)] 283 | #[allow(clippy::cast_possible_truncation)] 284 | fn region_thresholds>(median_gradients: &DMatrix, coefs: (Float, T)) -> DMatrix { 285 | let (nb_rows, nb_cols) = median_gradients.shape(); 286 | DMatrix::from_fn(nb_rows, nb_cols, |i, j| { 287 | let start_i = std::cmp::max(0, i as i32 - 1) as usize; 288 | let start_j = std::cmp::max(0, j as i32 - 1) as usize; 289 | let end_i = std::cmp::min(nb_rows, i + 2); 290 | let end_j = std::cmp::min(nb_cols, j + 2); 291 | let mut sum: T = num_traits::cast(0).unwrap(); 292 | let mut nb_elements = 0; 293 | for j in start_j..end_j { 294 | for i in start_i..end_i { 295 | sum = sum + median_gradients[(i, j)]; 296 | nb_elements += 1; 297 | } 298 | } 299 | let (a, b) = coefs; 300 | let thresh_tmp = sum.as_() / nb_elements as Float + b.as_(); 301 | num_traits::cast(a * thresh_tmp * thresh_tmp).expect("woops") 302 | }) 303 | } 304 | 305 | /// Compute median gradients magnitude of each region in the image. 306 | /// The regions on the right and bottom might be smaller. 307 | fn region_median_gradients>(gradients: &DMatrix, size: usize) -> DMatrix { 308 | let (nb_rows, nb_cols) = gradients.shape(); 309 | let nb_rows_regions = match div_rem(nb_rows, size) { 310 | (quot, 0) => quot, 311 | (quot, _) => quot + 1, 312 | }; 313 | let nb_cols_regions = match div_rem(nb_cols, size) { 314 | (quot, 0) => quot, 315 | (quot, _) => quot + 1, 316 | }; 317 | DMatrix::from_fn(nb_rows_regions, nb_cols_regions, |i, j| { 318 | let height = std::cmp::min(size, nb_rows - i * size); 319 | let width = std::cmp::min(size, nb_cols - j * size); 320 | let region_slice = gradients.slice((i * size, j * size), (height, width)); 321 | let mut region_cloned: Vec = region_slice.iter().cloned().collect(); 322 | region_cloned.sort_unstable(); 323 | region_cloned[region_cloned.len() / 2] 324 | }) 325 | } 326 | -------------------------------------------------------------------------------- /src/core/candidates/mod.rs: -------------------------------------------------------------------------------- 1 | // This Source Code Form is subject to the terms of the Mozilla Public 2 | // License, v. 2.0. If a copy of the MPL was not distributed with this 3 | // file, You can obtain one at http://mozilla.org/MPL/2.0/. 4 | 5 | //! Helper functions to choose candidate points for the tracking. 6 | 7 | pub mod coarse_to_fine; 8 | pub mod dso; 9 | -------------------------------------------------------------------------------- /src/core/gradient.rs: -------------------------------------------------------------------------------- 1 | // This Source Code Form is subject to the terms of the Mozilla Public 2 | // License, v. 2.0. If a copy of the MPL was not distributed with this 3 | // file, You can obtain one at http://mozilla.org/MPL/2.0/. 4 | 5 | //! Helper function to compute gradients 6 | 7 | use nalgebra as na; 8 | 9 | /// Compute a centered gradient. 10 | /// 11 | /// 1/2 * ( img(i+1,j) - img(i-1,j), img(i,j+1) - img(i,j-1) ) 12 | /// 13 | /// Gradients of pixels at the border of the image are set to 0. 14 | #[allow(clippy::similar_names)] 15 | pub fn centered(img: &na::DMatrix) -> (na::DMatrix, na::DMatrix) { 16 | // TODO: might be better to return DMatrix<(i16,i16)>? 17 | let (nb_rows, nb_cols) = img.shape(); 18 | let top = img.slice((0, 1), (nb_rows - 2, nb_cols - 2)); 19 | let bottom = img.slice((2, 1), (nb_rows - 2, nb_cols - 2)); 20 | let left = img.slice((1, 0), (nb_rows - 2, nb_cols - 2)); 21 | let right = img.slice((1, 2), (nb_rows - 2, nb_cols - 2)); 22 | let mut grad_x = na::DMatrix::zeros(nb_rows, nb_cols); 23 | let mut grad_y = na::DMatrix::zeros(nb_rows, nb_cols); 24 | let mut grad_x_inner = grad_x.slice_mut((1, 1), (nb_rows - 2, nb_cols - 2)); 25 | let mut grad_y_inner = grad_y.slice_mut((1, 1), (nb_rows - 2, nb_cols - 2)); 26 | for j in 0..nb_cols - 2 { 27 | for i in 0..nb_rows - 2 { 28 | grad_x_inner[(i, j)] = (i16::from(right[(i, j)]) - i16::from(left[(i, j)])) / 2; 29 | grad_y_inner[(i, j)] = (i16::from(bottom[(i, j)]) - i16::from(top[(i, j)])) / 2; 30 | } 31 | } 32 | (grad_x, grad_y) 33 | } 34 | 35 | /// Compute squared gradient norm from x and y gradient matrices. 36 | #[allow(clippy::cast_possible_truncation)] 37 | #[allow(clippy::cast_sign_loss)] 38 | pub fn squared_norm(grad_x: &na::DMatrix, grad_y: &na::DMatrix) -> na::DMatrix { 39 | grad_x.zip_map(grad_y, |gx, gy| { 40 | let gx = i32::from(gx); 41 | let gy = i32::from(gy); 42 | (gx * gx + gy * gy) as u16 43 | }) 44 | } 45 | 46 | /// Compute squared gradient norm directly from the image. 47 | #[allow(clippy::cast_possible_truncation)] 48 | #[allow(clippy::cast_sign_loss)] 49 | pub fn squared_norm_direct(im: &na::DMatrix) -> na::DMatrix { 50 | let (nb_rows, nb_cols) = im.shape(); 51 | let top = im.slice((0, 1), (nb_rows - 2, nb_cols - 2)); 52 | let bottom = im.slice((2, 1), (nb_rows - 2, nb_cols - 2)); 53 | let left = im.slice((1, 0), (nb_rows - 2, nb_cols - 2)); 54 | let right = im.slice((1, 2), (nb_rows - 2, nb_cols - 2)); 55 | let mut squared_norm_mat = na::DMatrix::zeros(nb_rows, nb_cols); 56 | let mut grad_inner = squared_norm_mat.slice_mut((1, 1), (nb_rows - 2, nb_cols - 2)); 57 | for j in 0..nb_cols - 2 { 58 | for i in 0..nb_rows - 2 { 59 | let gx = i32::from(right[(i, j)]) - i32::from(left[(i, j)]); 60 | let gy = i32::from(bottom[(i, j)]) - i32::from(top[(i, j)]); 61 | grad_inner[(i, j)] = ((gx * gx + gy * gy) / 4) as u16; 62 | } 63 | } 64 | squared_norm_mat 65 | } 66 | 67 | // BLOCS 2x2 ################################################################### 68 | 69 | /// Horizontal gradient in a 2x2 pixels block. 70 | /// 71 | /// The block is of the form: 72 | /// a c 73 | /// b d 74 | pub fn bloc_x(a: u8, b: u8, c: u8, d: u8) -> i16 { 75 | let a = i16::from(a); 76 | let b = i16::from(b); 77 | let c = i16::from(c); 78 | let d = i16::from(d); 79 | (c + d - a - b) / 2 80 | } 81 | 82 | /// Vertical gradient in a 2x2 pixels block. 83 | /// 84 | /// The block is of the form: 85 | /// a c 86 | /// b d 87 | pub fn bloc_y(a: u8, b: u8, c: u8, d: u8) -> i16 { 88 | let a = i16::from(a); 89 | let b = i16::from(b); 90 | let c = i16::from(c); 91 | let d = i16::from(d); 92 | (b - a + d - c) / 2 93 | } 94 | 95 | /// Gradient squared norm in a 2x2 pixels block. 96 | /// 97 | /// The block is of the form: 98 | /// a c 99 | /// b d 100 | #[allow(clippy::cast_possible_truncation)] 101 | #[allow(clippy::cast_sign_loss)] 102 | pub fn bloc_squared_norm(a: u8, b: u8, c: u8, d: u8) -> u16 { 103 | let a = i32::from(a); 104 | let b = i32::from(b); 105 | let c = i32::from(c); 106 | let d = i32::from(d); 107 | let dx = c + d - a - b; 108 | let dy = b - a + d - c; 109 | // I have checked that the max value is in u16. 110 | ((dx * dx + dy * dy) / 4) as u16 111 | } 112 | -------------------------------------------------------------------------------- /src/core/inverse_depth.rs: -------------------------------------------------------------------------------- 1 | // This Source Code Form is subject to the terms of the Mozilla Public 2 | // License, v. 2.0. If a copy of the MPL was not distributed with this 3 | // file, You can obtain one at http://mozilla.org/MPL/2.0/. 4 | 5 | //! Helper functions to manipulate inverse depth data from depth images. 6 | 7 | use crate::misc::type_aliases::Float; 8 | 9 | /// An inverse depth can be one of three values: unknown, discarded, or known with a given 10 | /// variance. 11 | #[derive(Copy, Clone, PartialEq, Debug)] 12 | pub enum InverseDepth { 13 | /// Not known by the capture device. 14 | Unknown, 15 | /// Value was considered too unreliable and discarded. 16 | Discarded, 17 | /// `WithVariance(inverse_depth, variance)`: known but with a given uncertainty. 18 | WithVariance(Float, Float), 19 | } 20 | 21 | /// Transform a depth value from a depth map into an inverse depth value with a given scaling. 22 | /// 23 | /// A value of 0 means that it is unknown. 24 | pub fn from_depth(scale: Float, depth: u16, variance: Float) -> InverseDepth { 25 | match depth { 26 | 0 => InverseDepth::Unknown, 27 | _ => InverseDepth::WithVariance(scale / Float::from(depth), variance), 28 | } 29 | } 30 | 31 | /// Transform inverse depth value back into a depth value with a given scaling. 32 | /// 33 | /// Unknown or discarded values are encoded with 0. 34 | #[allow(clippy::cast_precision_loss)] 35 | #[allow(clippy::cast_possible_truncation)] 36 | #[allow(clippy::cast_sign_loss)] 37 | pub fn to_depth(scale: Float, idepth: InverseDepth) -> u16 { 38 | match idepth { 39 | InverseDepth::WithVariance(x, _) => (scale / x).round() as u16, 40 | _ => 0, 41 | } 42 | } 43 | 44 | // Merging strategies ###################################### 45 | 46 | /// Fuse 4 inverse depth pixels of a bloc with a given merging strategy. 47 | /// 48 | /// It only keeps the known values and passes them as a `Vec` into the given strategy. 49 | pub fn fuse( 50 | a: InverseDepth, 51 | b: InverseDepth, 52 | c: InverseDepth, 53 | d: InverseDepth, 54 | strategy: F, 55 | ) -> InverseDepth 56 | where 57 | F: Fn(&[(Float, Float)]) -> InverseDepth, 58 | { 59 | strategy( 60 | [a, b, c, d] 61 | .iter() 62 | .filter_map(with_variance) 63 | .collect::>() 64 | .as_slice(), 65 | ) 66 | } 67 | 68 | fn with_variance(idepth: &InverseDepth) -> Option<(Float, Float)> { 69 | if let InverseDepth::WithVariance(idepth, var) = *idepth { 70 | Some((idepth, var)) 71 | } else { 72 | None 73 | } 74 | } 75 | 76 | /// Merge idepth pixels of a bloc into their mean idepth. 77 | /// 78 | /// Just like in DSO, inverse depth do not have statistical variance 79 | /// but some kind of "weight", proportional to how "trusty" they are. 80 | /// So here, the variance is to be considered as a weight instead. 81 | pub fn strategy_dso_mean(valid_values: &[(Float, Float)]) -> InverseDepth { 82 | match valid_values { 83 | [(d1, v1)] => InverseDepth::WithVariance(*d1, *v1), 84 | [(d1, v1), (d2, v2)] => { 85 | let sum = v1 + v2; 86 | InverseDepth::WithVariance((d1 * v1 + d2 * v2) / sum, sum) 87 | } 88 | [(d1, v1), (d2, v2), (d3, v3)] => { 89 | let sum = v1 + v2 + v3; 90 | InverseDepth::WithVariance((d1 * v1 + d2 * v2 + d3 * v3) / sum, sum) 91 | } 92 | [(d1, v1), (d2, v2), (d3, v3), (d4, v4)] => { 93 | let sum = v1 + v2 + v3 + v4; 94 | InverseDepth::WithVariance((d1 * v1 + d2 * v2 + d3 * v3 + d4 * v4) / sum, sum) 95 | } 96 | _ => InverseDepth::Unknown, 97 | } 98 | } 99 | 100 | /// Only merge inverse depths that are statistically similar. 101 | /// Others are discarded. 102 | /// 103 | /// Variance increases if only one inverse depth is known. 104 | /// Variance decreases if the four inverse depths are known. 105 | pub fn strategy_statistically_similar(valid_values: &[(Float, Float)]) -> InverseDepth { 106 | match valid_values { 107 | [(d1, v1)] => InverseDepth::WithVariance(*d1, 2.0 * v1), // v = 2/1 * mean 108 | [(d1, v1), (d2, v2)] => { 109 | let new_d = (d1 * v2 + d2 * v1) / (v1 + v2); 110 | let new_v = (v1 + v2) / 2.0; // v = 2/2 * mean 111 | if (d1 - new_d).powi(2) < new_v && (d2 - new_d).powi(2) < new_v { 112 | InverseDepth::WithVariance(new_d, new_v) 113 | } else { 114 | InverseDepth::Discarded 115 | } 116 | } 117 | [(d1, v1), (d2, v2), (d3, v3)] => { 118 | let v12 = v1 * v2; 119 | let v13 = v1 * v3; 120 | let v23 = v2 * v3; 121 | let new_d = (d1 * v23 + d2 * v13 + d3 * v12) / (v12 + v13 + v23); 122 | let new_v = 2.0 * (v1 + v2 + v3) / 9.0; // v = 2/3 * mean 123 | if (d1 - new_d).powi(2) < new_v 124 | && (d2 - new_d).powi(2) < new_v 125 | && (d3 - new_d).powi(2) < new_v 126 | { 127 | InverseDepth::WithVariance(new_d, new_v) 128 | } else { 129 | InverseDepth::Discarded 130 | } 131 | } 132 | [(d1, v1), (d2, v2), (d3, v3), (d4, v4)] => { 133 | let v123 = v1 * v2 * v3; 134 | let v234 = v2 * v3 * v4; 135 | let v341 = v3 * v4 * v1; 136 | let v412 = v4 * v1 * v2; 137 | let sum_v1234 = v123 + v234 + v341 + v412; 138 | let new_d = (d1 * v234 + d2 * v341 + d3 * v412 + d4 * v123) / sum_v1234; 139 | let new_v = (v1 + v2 + v3 + v4) / 8.0; // v = 2/4 * mean 140 | if (d1 - new_d).powi(2) < new_v 141 | && (d2 - new_d).powi(2) < new_v 142 | && (d3 - new_d).powi(2) < new_v 143 | && (d4 - new_d).powi(2) < new_v 144 | { 145 | InverseDepth::WithVariance(new_d, new_v) 146 | } else { 147 | InverseDepth::Discarded 148 | } 149 | } 150 | _ => InverseDepth::Unknown, 151 | } 152 | } 153 | -------------------------------------------------------------------------------- /src/core/mod.rs: -------------------------------------------------------------------------------- 1 | // This Source Code Form is subject to the terms of the Mozilla Public 2 | // License, v. 2.0. If a copy of the MPL was not distributed with this 3 | // file, You can obtain one at http://mozilla.org/MPL/2.0/. 4 | 5 | //! Core functionalities of Visual Odometry Rust. 6 | 7 | pub mod camera; 8 | pub mod candidates; 9 | pub mod gradient; 10 | pub mod inverse_depth; 11 | pub mod multires; 12 | pub mod track; 13 | -------------------------------------------------------------------------------- /src/core/multires.rs: -------------------------------------------------------------------------------- 1 | // This Source Code Form is subject to the terms of the Mozilla Public 2 | // License, v. 2.0. If a copy of the MPL was not distributed with this 3 | // file, You can obtain one at http://mozilla.org/MPL/2.0/. 4 | 5 | //! Helper functions for generation of multi-resolution data. 6 | 7 | use nalgebra::{DMatrix, Scalar}; 8 | 9 | use crate::core::gradient; 10 | 11 | /// Recursively generate a pyramid of matrices where each following level 12 | /// is half the previous resolution, computed with the mean of each 2x2 block. 13 | /// 14 | /// It consumes the original matrix to keep it as first level of the pyramid without copy. 15 | /// If you need it later, simply use something like `pyramid[0]`. 16 | /// 17 | /// PS: since we are using 2x2 blocs, 18 | /// border information is lost for odd resolutions. 19 | /// Some precision is also left to keep the pyramid data as `u8`. 20 | #[allow(clippy::cast_possible_truncation)] 21 | pub fn mean_pyramid(max_levels: usize, mat: DMatrix) -> Vec> { 22 | limited_sequence(max_levels, mat, |m| { 23 | halve(m, |a, b, c, d| { 24 | let a = u16::from(a); 25 | let b = u16::from(b); 26 | let c = u16::from(c); 27 | let d = u16::from(d); 28 | ((a + b + c + d) / 4) as u8 29 | }) 30 | }) 31 | } 32 | 33 | /// Recursively apply a function transforming an image 34 | /// until it's not possible anymore or the max length is reached. 35 | /// 36 | /// Using `max_length = 0` has the same effect than `max_length = 1` since 37 | /// the result vector contains always at least one matrix (the init matrix). 38 | pub fn limited_sequence Option, T>(max_length: usize, data: T, f: F) -> Vec { 39 | let mut length = 1; 40 | let f_limited = |x: &T| { 41 | if length < max_length { 42 | length += 1; 43 | f(x) 44 | } else { 45 | None 46 | } 47 | }; 48 | sequence(data, f_limited) 49 | } 50 | 51 | /// Recursively apply a function transforming data 52 | /// until it's not possible anymore. 53 | pub fn sequence Option, T>(data: T, mut f: F) -> Vec { 54 | let mut s = Vec::new(); 55 | s.push(data); 56 | while let Some(new_data) = f(s.last().unwrap()) { 57 | s.push(new_data); 58 | } 59 | s 60 | } 61 | 62 | /// Halve the resolution of a matrix by applying a function to each 2x2 block. 63 | /// 64 | /// If one size of the matrix is < 2 then this function returns None. 65 | /// If one size is odd, its last line/column is dropped. 66 | #[allow(clippy::many_single_char_names)] 67 | pub fn halve(mat: &DMatrix, f: F) -> Option> 68 | where 69 | F: Fn(T, T, T, T) -> U, 70 | T: Scalar, 71 | U: Scalar, 72 | { 73 | let (r, c) = mat.shape(); 74 | let half_r = r / 2; 75 | let half_c = c / 2; 76 | if half_r == 0 || half_c == 0 { 77 | None 78 | } else { 79 | let half_mat = DMatrix::::from_fn(half_r, half_c, |i, j| { 80 | let a = mat[(2 * i, 2 * j)]; 81 | let b = mat[(2 * i + 1, 2 * j)]; 82 | let c = mat[(2 * i, 2 * j + 1)]; 83 | let d = mat[(2 * i + 1, 2 * j + 1)]; 84 | f(a, b, c, d) 85 | }); 86 | Some(half_mat) 87 | } 88 | } 89 | 90 | // Gradients stuff ################################################### 91 | 92 | /// Compute centered gradients norm at each resolution from 93 | /// the image at the higher resolution. 94 | /// 95 | /// As a consequence there is one less level in the gradients pyramid. 96 | pub fn gradients_squared_norm(multires_mat: &[DMatrix]) -> Vec> { 97 | let nb_levels = multires_mat.len(); 98 | multires_mat 99 | .iter() 100 | .take(nb_levels - 1) 101 | .map(|mat| { 102 | halve(mat, gradient::bloc_squared_norm) 103 | .expect("There is an issue in gradients_squared_norm") 104 | }) 105 | .collect() 106 | } 107 | 108 | /// Compute centered gradients at each resolution from 109 | /// the image at the higher resolution. 110 | /// 111 | /// As a consequence there is one less level in the gradients pyramid. 112 | pub fn gradients_xy(multires_mat: &[DMatrix]) -> Vec<(DMatrix, DMatrix)> { 113 | // TODO: maybe it would be better to return Vec>, 114 | // to colocate the x and y gradient and do only one "halve" call? 115 | let nb_levels = multires_mat.len(); 116 | multires_mat 117 | .iter() 118 | .take(nb_levels - 1) 119 | .map(|mat| { 120 | ( 121 | halve(mat, gradient::bloc_x).expect("There is an issue in gradients_xy x."), 122 | halve(mat, gradient::bloc_y).expect("There is an issue in gradients_xy y."), 123 | ) 124 | }) 125 | .collect() 126 | } 127 | -------------------------------------------------------------------------------- /src/core/track/inverse_compositional.rs: -------------------------------------------------------------------------------- 1 | // This Source Code Form is subject to the terms of the Mozilla Public 2 | // License, v. 2.0. If a copy of the MPL was not distributed with this 3 | // file, You can obtain one at http://mozilla.org/MPL/2.0/. 4 | 5 | //! Types and functions to implement an inverse compositional tracking algorithm. 6 | //! 7 | //! Implementation of "Lucas-kanade 20 years on: A unifying framework" 8 | //! in the inverse compositional case. 9 | //! The warping function is parameterized by the Lie Algebra of twists se(3). 10 | 11 | use itertools::izip; 12 | use nalgebra::DMatrix; 13 | 14 | use crate::core::{ 15 | camera::Intrinsics, 16 | candidates::coarse_to_fine as candidates, 17 | gradient, 18 | inverse_depth::{self, InverseDepth}, 19 | multires, 20 | track::lm_optimizer::{self, LMOptimizerState}, 21 | }; 22 | use crate::math::optimizer::State as _; 23 | use crate::misc::helper; 24 | use crate::misc::type_aliases::{Float, Iso3, Mat6, Point2, Vec6}; 25 | 26 | /// Type alias to easily spot vectors that are indexed over multi-resolution levels. 27 | pub type Levels = Vec; 28 | 29 | /// Struct used for tracking the camera at each frame. 30 | /// Can only be constructed by initialization from a `Config`. 31 | pub struct Tracker { 32 | config: Config, 33 | state: State, 34 | } 35 | 36 | /// Configuration of the Tracker. 37 | pub struct Config { 38 | /// Number of levels in the multi-resolution pyramids of images. 39 | pub nb_levels: usize, 40 | /// Threshold for the candidates selection algorithm. 41 | pub candidates_diff_threshold: u16, 42 | /// Scale of the depth 16 bit images. 43 | /// This is 5000.0 for the TUM RGB-D dataset. 44 | pub depth_scale: Float, 45 | /// Camera intrinsic parameters. 46 | pub intrinsics: Intrinsics, 47 | /// Default variance of the inverse depth values coming from the depth map. 48 | pub idepth_variance: Float, 49 | } 50 | 51 | /// Internal state of the tracker. 52 | struct State { 53 | keyframe_multires_data: MultiresData, 54 | keyframe_depth_timestamp: f64, 55 | keyframe_img_timestamp: f64, 56 | keyframe_pose: Iso3, 57 | current_frame_depth_timestamp: f64, 58 | current_frame_img_timestamp: f64, 59 | current_frame_pose: Iso3, 60 | } 61 | 62 | /// Mostly multi-resolution data related to the frame. 63 | #[allow(clippy::type_complexity)] 64 | struct MultiresData { 65 | intrinsics_multires: Levels, 66 | img_multires: Levels>, 67 | usable_candidates_multires: Levels<(Vec<(usize, usize)>, Vec)>, 68 | jacobians_multires: Levels>, 69 | hessians_multires: Levels>, 70 | } 71 | 72 | impl Config { 73 | /// Initialize a tracker with the first RGB-D frame. 74 | pub fn init( 75 | self, 76 | keyframe_depth_timestamp: f64, 77 | depth_map: &DMatrix, 78 | keyframe_img_timestamp: f64, 79 | img: DMatrix, 80 | ) -> Tracker { 81 | // Precompute multi-resolution first frame data. 82 | let intrinsics_multires = self.intrinsics.clone().multi_res(self.nb_levels); 83 | let img_multires = multires::mean_pyramid(self.nb_levels, img); 84 | let keyframe_multires_data = 85 | precompute_multires_data(&self, depth_map, intrinsics_multires, img_multires); 86 | 87 | // Regroup everything under the returned Tracker. 88 | Tracker { 89 | state: State { 90 | keyframe_multires_data, 91 | keyframe_depth_timestamp, 92 | keyframe_img_timestamp, 93 | keyframe_pose: Iso3::identity(), 94 | current_frame_depth_timestamp: keyframe_depth_timestamp, 95 | current_frame_img_timestamp: keyframe_img_timestamp, 96 | current_frame_pose: Iso3::identity(), 97 | }, 98 | config: self, 99 | } 100 | } 101 | } // impl Config 102 | 103 | /// Precompute the multi-resolution data of a frame. 104 | #[allow(clippy::used_underscore_binding)] 105 | fn precompute_multires_data( 106 | config: &Config, 107 | depth_map: &DMatrix, 108 | intrinsics_multires: Levels, 109 | img_multires: Levels>, 110 | ) -> MultiresData { 111 | // Precompute multi-resolution of keyframe gradients. 112 | let mut gradients_multires = multires::gradients_xy(&img_multires); 113 | gradients_multires.insert(0, gradient::centered(&img_multires[0])); 114 | let gradients_squared_norm_multires: Vec<_> = gradients_multires 115 | .iter() 116 | .map(|(gx, gy)| gradient::squared_norm(gx, gy)) 117 | .collect(); 118 | 119 | // Precompute mask of candidate points for tracking. 120 | let candidates_points = candidates::select( 121 | config.candidates_diff_threshold, 122 | &gradients_squared_norm_multires, 123 | ) 124 | .pop() 125 | .unwrap(); 126 | 127 | // Only keep the "usable" points, i.e. those with a known depth information. 128 | let from_depth = |z| inverse_depth::from_depth(config.depth_scale, z, config.idepth_variance); 129 | let idepth_candidates = helper::zip_mask_map( 130 | depth_map, 131 | &candidates_points, 132 | InverseDepth::Unknown, 133 | from_depth, 134 | ); 135 | let fuse = |a, b, c, d| inverse_depth::fuse(a, b, c, d, inverse_depth::strategy_dso_mean); 136 | let idepth_multires = multires::limited_sequence(config.nb_levels, idepth_candidates, |m| { 137 | multires::halve(m, fuse) 138 | }); 139 | let usable_candidates_multires: Levels<_> = idepth_multires.iter().map(extract_z).collect(); 140 | 141 | // Precompute the Jacobians. 142 | let jacobians_multires: Levels> = izip!( 143 | &intrinsics_multires, 144 | &usable_candidates_multires, 145 | &gradients_multires, 146 | ) 147 | .map(|(intrinsics, (coord, _z), (gx, gy))| warp_jacobians(intrinsics, coord, _z, gx, gy)) 148 | .collect(); 149 | 150 | // Precompute the Hessians. 151 | let hessians_multires: Levels<_> = jacobians_multires.iter().map(hessians_vec).collect(); 152 | 153 | // Regroup everything under a MultiresData. 154 | MultiresData { 155 | intrinsics_multires, 156 | img_multires, 157 | usable_candidates_multires, 158 | jacobians_multires, 159 | hessians_multires, 160 | } 161 | } 162 | 163 | impl Tracker { 164 | /// Track a new frame. 165 | /// Internally mutates the tracker state. 166 | /// 167 | /// You can use `tracker.current_frame()` after tracking to retrieve the new frame pose. 168 | #[allow(clippy::used_underscore_binding)] 169 | #[allow(clippy::cast_precision_loss)] 170 | pub fn track( 171 | &mut self, 172 | depth_time: f64, 173 | depth_map: &DMatrix, 174 | img_time: f64, 175 | img: DMatrix, 176 | ) { 177 | let mut lm_model = self.state.current_frame_pose.inverse() * self.state.keyframe_pose; 178 | let img_multires = multires::mean_pyramid(self.config.nb_levels, img); 179 | let keyframe_data = &self.state.keyframe_multires_data; 180 | let mut optimization_went_well = true; 181 | for lvl in (0..self.config.nb_levels).rev() { 182 | let obs = lm_optimizer::Obs { 183 | intrinsics: &keyframe_data.intrinsics_multires[lvl], 184 | template: &keyframe_data.img_multires[lvl], 185 | image: &img_multires[lvl], 186 | coordinates: &keyframe_data.usable_candidates_multires[lvl].0, 187 | _z_candidates: &keyframe_data.usable_candidates_multires[lvl].1, 188 | jacobians: &keyframe_data.jacobians_multires[lvl], 189 | hessians: &keyframe_data.hessians_multires[lvl], 190 | }; 191 | match LMOptimizerState::iterative_solve(&obs, lm_model) { 192 | Ok((lm_state, _)) => { 193 | lm_model = lm_state.eval_data.model; 194 | } 195 | Err(err) => { 196 | eprintln!("{}", err); 197 | optimization_went_well = false; 198 | break; 199 | } 200 | } 201 | } 202 | 203 | // Update current frame info in tracker. 204 | self.state.current_frame_depth_timestamp = depth_time; 205 | self.state.current_frame_img_timestamp = img_time; 206 | if optimization_went_well { 207 | self.state.current_frame_pose = self.state.keyframe_pose * lm_model.inverse(); 208 | } 209 | 210 | // Check if we need to change the keyframe. 211 | let (coordinates, _z_candidates) = keyframe_data.usable_candidates_multires.last().unwrap(); 212 | let intrinsics = keyframe_data.intrinsics_multires.last().unwrap(); 213 | let optical_flow_sum: Float = _z_candidates 214 | .iter() 215 | .zip(coordinates.iter()) 216 | .map(|(&_z, &(x, y))| { 217 | let (u, v) = warp(&lm_model, x as Float, y as Float, _z, intrinsics); 218 | (x as Float - u).abs() + (y as Float - v).abs() 219 | }) 220 | .sum(); 221 | let optical_flow = optical_flow_sum / _z_candidates.len() as Float; 222 | eprintln!("Optical_flow: {}", optical_flow); 223 | 224 | let change_keyframe = optical_flow >= 1.0; 225 | 226 | // In case of keyframe change, update all keyframe info with current frame. 227 | if change_keyframe { 228 | let delta_time = depth_time - self.state.keyframe_depth_timestamp; 229 | eprintln!("Changing keyframe after: {} seconds", delta_time); 230 | self.state.keyframe_multires_data = precompute_multires_data( 231 | &self.config, 232 | depth_map, 233 | keyframe_data.intrinsics_multires.clone(), 234 | img_multires, 235 | ); 236 | self.state.keyframe_depth_timestamp = depth_time; 237 | self.state.keyframe_img_timestamp = img_time; 238 | self.state.keyframe_pose = self.state.current_frame_pose; 239 | } 240 | } // track 241 | 242 | /// Retrieve the current frame timestamp (of depth image) and pose. 243 | pub fn current_frame(&self) -> (f64, Iso3) { 244 | ( 245 | self.state.current_frame_depth_timestamp, 246 | self.state.current_frame_pose, 247 | ) 248 | } 249 | } // impl Tracker 250 | 251 | // Helper ###################################################################### 252 | 253 | // fn angle(uq: UnitQuaternion) -> Float { 254 | // let w = uq.into_inner().scalar(); 255 | // 2.0 * uq.into_inner().vector().norm().atan2(w) 256 | // } 257 | 258 | /// Extract known inverse depth values (and coordinates) into vectorized data. 259 | #[allow(clippy::used_underscore_binding)] 260 | fn extract_z(idepth_mat: &DMatrix) -> (Vec<(usize, usize)>, Vec) { 261 | let mut u = 0; 262 | let mut v = 0; 263 | // TODO: can allocating with a known max size improve performances? 264 | let mut coordinates = Vec::new(); 265 | let mut _z_vec = Vec::new(); 266 | let (nb_rows, _) = idepth_mat.shape(); 267 | for idepth in idepth_mat.iter() { 268 | if let InverseDepth::WithVariance(_z, _) = *idepth { 269 | coordinates.push((u, v)); 270 | _z_vec.push(_z); 271 | } 272 | v += 1; 273 | if v >= nb_rows { 274 | u += 1; 275 | v = 0; 276 | } 277 | } 278 | (coordinates, _z_vec) 279 | } 280 | 281 | /// Precompute jacobians for each candidate. 282 | #[allow(clippy::used_underscore_binding)] 283 | #[allow(clippy::cast_precision_loss)] 284 | fn warp_jacobians( 285 | intrinsics: &Intrinsics, 286 | coordinates: &[(usize, usize)], 287 | _z_candidates: &[Float], 288 | grad_x: &DMatrix, 289 | grad_y: &DMatrix, 290 | ) -> Vec { 291 | // Bind intrinsics to shorter names 292 | let (cu, cv) = intrinsics.principal_point; 293 | let (fu, fv) = intrinsics.focal; 294 | let s = intrinsics.skew; 295 | 296 | // Iterate on inverse depth candidates 297 | coordinates 298 | .iter() 299 | .zip(_z_candidates.iter()) 300 | .map(|(&(u, v), &_z)| { 301 | let gu = Float::from(grad_x[(v, u)]); 302 | let gv = Float::from(grad_y[(v, u)]); 303 | warp_jacobian_at(gu, gv, u as Float, v as Float, _z, cu, cv, fu, fv, s) 304 | }) 305 | .collect() 306 | } 307 | 308 | /// Jacobian of the warping function for the inverse compositional algorithm. 309 | #[allow(clippy::too_many_arguments)] 310 | #[allow(clippy::many_single_char_names)] 311 | #[allow(clippy::used_underscore_binding)] 312 | #[allow(clippy::similar_names)] 313 | fn warp_jacobian_at( 314 | gu: Float, 315 | gv: Float, 316 | u: Float, 317 | v: Float, 318 | _z: Float, 319 | cu: Float, 320 | cv: Float, 321 | fu: Float, 322 | fv: Float, 323 | s: Float, 324 | ) -> Vec6 { 325 | // Intermediate computations 326 | let a = u - cu; 327 | let b = v - cv; 328 | let c = a * fv - s * b; 329 | let _fv = 1.0 / fv; 330 | let _fuv = 1.0 / (fu * fv); 331 | 332 | // Jacobian of the warp 333 | Vec6::new( 334 | gu * _z * fu, // 335 | _z * (gu * s + gv * fv), // linear velocity terms 336 | -_z * (gu * a + gv * b), // ___ 337 | gu * (-a * b * _fv - s) + gv * (-b * b * _fv - fv), // 338 | gu * (a * c * _fuv + fu) + gv * (b * c * _fuv), // angular velocity terms 339 | gu * (-fu * fu * b + s * c) * _fuv + gv * (c / fu), // 340 | ) 341 | } 342 | 343 | /// Compute hessians components for each candidate point. 344 | #[allow(clippy::ptr_arg)] // TODO: Applying clippy lint here results in compilation error. 345 | fn hessians_vec(jacobians: &Vec) -> Vec { 346 | // TODO: might be better to inline this within the function computing the jacobians. 347 | jacobians.iter().map(|j| j * j.transpose()).collect() 348 | } 349 | 350 | /// Warp a point from an image to another by a given rigid body motion. 351 | #[allow(clippy::used_underscore_binding)] 352 | fn warp(model: &Iso3, x: Float, y: Float, _z: Float, intrinsics: &Intrinsics) -> (Float, Float) { 353 | // TODO: maybe move into the camera module? 354 | let x1 = intrinsics.back_project(Point2::new(x, y), 1.0 / _z); 355 | let x2 = model * x1; 356 | let uvz2 = intrinsics.project(x2); 357 | (uvz2.x / uvz2.z, uvz2.y / uvz2.z) 358 | } 359 | -------------------------------------------------------------------------------- /src/core/track/lm_optimizer.rs: -------------------------------------------------------------------------------- 1 | // This Source Code Form is subject to the terms of the Mozilla Public 2 | // License, v. 2.0. If a copy of the MPL was not distributed with this 3 | // file, You can obtain one at http://mozilla.org/MPL/2.0/. 4 | 5 | //! Levenberg-Marquardt implementation of the `optimizer::State` trait 6 | //! for the inverse compositional tracking algorithm. 7 | 8 | use nalgebra::{DMatrix, UnitQuaternion}; 9 | 10 | use crate::core::camera::Intrinsics; 11 | use crate::math::optimizer::{self, Continue}; 12 | use crate::math::se3; 13 | use crate::misc::type_aliases::{Float, Iso3, Mat6, Point2, Vec6}; 14 | 15 | /// State of the Levenberg-Marquardt optimizer. 16 | pub struct LMOptimizerState { 17 | /// Levenberg-Marquardt hessian diagonal coefficient. 18 | pub lm_coef: Float, 19 | /// Data resulting of a successful model evaluation. 20 | pub eval_data: EvalData, 21 | } 22 | 23 | /// Either a successfully constructed `EvalData` 24 | /// or an error containing the energy of a given model. 25 | /// 26 | /// The error is returned when the new computed energy 27 | /// is higher than the previous iteration energy. 28 | pub type EvalState = Result; 29 | 30 | /// Data resulting of a successful model evaluation. 31 | pub struct EvalData { 32 | /// The hessian matrix of the system. 33 | pub hessian: Mat6, 34 | /// The gradient of the system. 35 | pub gradient: Vec6, 36 | /// Energy associated with the current model. 37 | pub energy: Float, 38 | /// Estimated motion at the current state of iterations. 39 | pub model: Iso3, 40 | } 41 | 42 | /// Precomputed data available for the optimizer iterations: 43 | pub struct Obs<'a> { 44 | /// Intrinsic parameters of the camera. 45 | pub intrinsics: &'a Intrinsics, 46 | /// Reference ("keyframe") image. 47 | pub template: &'a DMatrix, 48 | /// Current image to track. 49 | pub image: &'a DMatrix, 50 | /// Coordinates of the points used for the tracking. 51 | pub coordinates: &'a Vec<(usize, usize)>, 52 | /// Inverse depth of the points used for the tracking. 53 | pub _z_candidates: &'a Vec, 54 | /// Jacobians precomputed for the points used for the tracking. 55 | pub jacobians: &'a Vec, 56 | /// Hessian matrices precomputed for the points used for the tracking. 57 | pub hessians: &'a Vec, 58 | } 59 | 60 | /// `(energy, inside_indices, residuals)`. 61 | type Precomputed = (Float, Vec, Vec); 62 | 63 | impl LMOptimizerState { 64 | /// Precompute the energy of a model. 65 | /// Also return the residuals vector and the indices of candidate points used. 66 | #[allow(clippy::cast_precision_loss)] 67 | #[allow(clippy::used_underscore_binding)] 68 | fn eval_energy(obs: &Obs, model: &Iso3) -> Precomputed { 69 | let mut inside_indices = Vec::new(); 70 | let mut residuals = Vec::new(); 71 | let mut energy_sum = 0.0; 72 | for (idx, &(x, y)) in obs.coordinates.iter().enumerate() { 73 | let _z = obs._z_candidates[idx]; 74 | // check if warp(x,y) is inside the image 75 | let (u, v) = warp(model, x as Float, y as Float, _z, obs.intrinsics); 76 | if let Some(im) = interpolate(u, v, obs.image) { 77 | // precompute residuals and energy 78 | let tmp = obs.template[(y, x)]; 79 | let r = im - Float::from(tmp); 80 | energy_sum += r * r; 81 | residuals.push(r); 82 | inside_indices.push(idx); // keep only inside points 83 | } 84 | } 85 | let energy = energy_sum / residuals.len() as Float; 86 | (energy, inside_indices, residuals) 87 | } 88 | 89 | /// Fully evaluate a model. 90 | fn compute_eval_data(obs: &Obs, model: Iso3, pre: Precomputed) -> EvalData { 91 | let (energy, inside_indices, residuals) = pre; 92 | let mut gradient = Vec6::zeros(); 93 | let mut hessian = Mat6::zeros(); 94 | for (i, idx) in inside_indices.into_iter().enumerate() { 95 | let jac = obs.jacobians[idx]; 96 | let hes = obs.hessians[idx]; 97 | let r = residuals[i]; 98 | gradient += jac * r; 99 | hessian += hes; 100 | } 101 | EvalData { 102 | hessian, 103 | gradient, 104 | energy, 105 | model, 106 | } 107 | } 108 | } 109 | 110 | /// `impl<'a> optimizer::State, EvalState, Iso3, String> for LMOptimizerState`. 111 | impl<'a> optimizer::State, EvalState, Iso3, String> for LMOptimizerState { 112 | /// Initialize the optimizer state. 113 | fn init(obs: &Obs, model: Iso3) -> Self { 114 | Self { 115 | lm_coef: 0.1, 116 | eval_data: Self::compute_eval_data(obs, model, Self::eval_energy(obs, &model)), 117 | } 118 | } 119 | 120 | /// Compute the step using Levenberg-Marquardt. 121 | /// Apply the step in an inverse compositional approach to compute the next motion estimation. 122 | /// May return an error at the Cholesky decomposition of the hessian. 123 | fn step(&self) -> Result { 124 | let mut hessian = self.eval_data.hessian; 125 | hessian.m11 *= 1.0 + self.lm_coef; 126 | hessian.m22 *= 1.0 + self.lm_coef; 127 | hessian.m33 *= 1.0 + self.lm_coef; 128 | hessian.m44 *= 1.0 + self.lm_coef; 129 | hessian.m55 *= 1.0 + self.lm_coef; 130 | hessian.m66 *= 1.0 + self.lm_coef; 131 | let cholesky = hessian 132 | .cholesky() 133 | .ok_or("Error at Cholesky decomposition of hessian")?; 134 | let delta_warp = se3::exp(cholesky.solve(&self.eval_data.gradient)); 135 | Ok(renormalize(self.eval_data.model * delta_warp.inverse())) 136 | } 137 | 138 | /// Compute residuals and energy of the new model. 139 | /// Then, evaluate the new hessian and gradient if the energy has decreased. 140 | fn eval(&self, obs: &Obs, model: Iso3) -> EvalState { 141 | let pre = Self::eval_energy(obs, &model); 142 | let energy = pre.0; 143 | let old_energy = self.eval_data.energy; 144 | if energy > old_energy { 145 | Err(energy) 146 | } else { 147 | Ok(Self::compute_eval_data(obs, model, pre)) 148 | } 149 | } 150 | 151 | /// Stop after too many iterations, 152 | /// or if the energy variation is too low. 153 | /// 154 | /// Also update the Levenberg-Marquardt coefficient 155 | /// depending on if the energy increased or decreased. 156 | fn stop_criterion(self, nb_iter: usize, eval_state: EvalState) -> (Self, Continue) { 157 | let too_many_iterations = nb_iter > 20; 158 | match (eval_state, too_many_iterations) { 159 | // Max number of iterations reached: 160 | (Err(_), true) => (self, Continue::Stop), 161 | (Ok(eval_data), true) => { 162 | // eprintln!("Energy: {}", eval_data.energy); 163 | let kept_state = Self { 164 | lm_coef: self.lm_coef, // does not matter actually 165 | eval_data, 166 | }; 167 | (kept_state, Continue::Stop) 168 | } 169 | // Can continue to iterate: 170 | (Err(_energy), false) => { 171 | // eprintln!("\t back from: {}", energy); 172 | let mut kept_state = self; 173 | kept_state.lm_coef *= 10.0; 174 | (kept_state, Continue::Forward) 175 | } 176 | (Ok(eval_data), false) => { 177 | let d_energy = self.eval_data.energy - eval_data.energy; 178 | // 1.0 is totally empiric here 179 | let continuation = if d_energy > 1.0 { 180 | Continue::Forward 181 | } else { 182 | Continue::Stop 183 | }; 184 | // eprintln!("Energy: {}", eval_data.energy); 185 | let kept_state = Self { 186 | lm_coef: 0.1 * self.lm_coef, 187 | eval_data, 188 | }; 189 | (kept_state, continuation) 190 | } 191 | } 192 | } // fn stop_criterion 193 | } // impl optimizer::State<...> for LMOptimizerState 194 | 195 | // Helper ###################################################################### 196 | 197 | /// First order Taylor approximation for renormalization of rotation part of motion. 198 | fn renormalize(motion: Iso3) -> Iso3 { 199 | let mut motion = motion; 200 | motion.rotation = renormalize_unit_quaternion(motion.rotation); 201 | motion 202 | } 203 | 204 | /// First order Taylor approximation for unit quaternion re-normalization. 205 | fn renormalize_unit_quaternion(uq: UnitQuaternion) -> UnitQuaternion { 206 | let q = uq.into_inner(); 207 | let sq_norm = q.norm_squared(); 208 | UnitQuaternion::new_unchecked(0.5 * (3.0 - sq_norm) * q) 209 | } 210 | 211 | /// Warp a point from an image to another by a given rigid body motion. 212 | #[allow(clippy::used_underscore_binding)] 213 | fn warp(model: &Iso3, x: Float, y: Float, _z: Float, intrinsics: &Intrinsics) -> (Float, Float) { 214 | // TODO: maybe move into the camera module? 215 | let x1 = intrinsics.back_project(Point2::new(x, y), 1.0 / _z); 216 | let x2 = model * x1; 217 | let uvz2 = intrinsics.project(x2); 218 | (uvz2.x / uvz2.z, uvz2.y / uvz2.z) 219 | } 220 | 221 | /// Simple linear interpolation of a pixel with floating point coordinates. 222 | /// Return `None` if the point is outside of the image boundaries. 223 | #[allow(clippy::many_single_char_names)] 224 | #[allow(clippy::cast_possible_truncation)] 225 | #[allow(clippy::cast_sign_loss)] 226 | #[allow(clippy::cast_precision_loss)] 227 | fn interpolate(x: Float, y: Float, image: &DMatrix) -> Option { 228 | let (height, width) = image.shape(); 229 | let u = x.floor(); 230 | let v = y.floor(); 231 | if u >= 0.0 && u < (width - 2) as Float && v >= 0.0 && v < (height - 2) as Float { 232 | let u_0 = u as usize; 233 | let v_0 = v as usize; 234 | let u_1 = u_0 + 1; 235 | let v_1 = v_0 + 1; 236 | let vu_00 = Float::from(image[(v_0, u_0)]); 237 | let vu_10 = Float::from(image[(v_1, u_0)]); 238 | let vu_01 = Float::from(image[(v_0, u_1)]); 239 | let vu_11 = Float::from(image[(v_1, u_1)]); 240 | let a = x - u; 241 | let b = y - v; 242 | Some( 243 | (1.0 - b) * (1.0 - a) * vu_00 244 | + b * (1.0 - a) * vu_10 245 | + (1.0 - b) * a * vu_01 246 | + b * a * vu_11, 247 | ) 248 | } else { 249 | None 250 | } 251 | } 252 | -------------------------------------------------------------------------------- /src/core/track/mod.rs: -------------------------------------------------------------------------------- 1 | // This Source Code Form is subject to the terms of the Mozilla Public 2 | // License, v. 2.0. If a copy of the MPL was not distributed with this 3 | // file, You can obtain one at http://mozilla.org/MPL/2.0/. 4 | 5 | //! Useful types and functions for tracking a camera. 6 | 7 | pub mod inverse_compositional; 8 | pub mod lm_optimizer; 9 | -------------------------------------------------------------------------------- /src/dataset/mod.rs: -------------------------------------------------------------------------------- 1 | // This Source Code Form is subject to the terms of the Mozilla Public 2 | // License, v. 2.0. If a copy of the MPL was not distributed with this 3 | // file, You can obtain one at http://mozilla.org/MPL/2.0/. 4 | 5 | //! Helpers modules for managing known RGB-D datasets. 6 | 7 | pub mod tum_rgbd; 8 | -------------------------------------------------------------------------------- /src/dataset/tum_rgbd.rs: -------------------------------------------------------------------------------- 1 | // This Source Code Form is subject to the terms of the Mozilla Public 2 | // License, v. 2.0. If a copy of the MPL was not distributed with this 3 | // file, You can obtain one at http://mozilla.org/MPL/2.0/. 4 | 5 | //! Helper functions to handle datasets compatible with TUM RGB-D. 6 | 7 | use nalgebra as na; 8 | use std::path::PathBuf; 9 | 10 | use crate::core::camera::Intrinsics; 11 | use crate::misc::type_aliases::{Float, Iso3}; 12 | 13 | /// U16 depth values are scaled for better precision. 14 | /// So 5000 in the 16 bits gray png corresponds to 1 meter. 15 | pub const DEPTH_SCALE: Float = 5000.0; 16 | 17 | /// Acceptable error is assimilated to 1cm at 1m. 18 | /// The difference between 1/1m and 1/1.01m is ~ 0.01 19 | /// so we will take a variance of 0.01^2 = 0.0001. 20 | pub const VARIANCE_ICL_NUIM: Float = 0.0001; 21 | 22 | /// Intrinsics parameters of the ICL-NUIM dataset. 23 | pub const INTRINSICS_ICL_NUIM: Intrinsics = Intrinsics { 24 | principal_point: (319.5, 239.5), 25 | focal: (481.20, -480.00), 26 | skew: 0.0, 27 | }; 28 | 29 | /// Intrinsics parameters of freiburg 1 (fr1) scenes in the TUM RGB-D dataset. 30 | #[allow(clippy::excessive_precision)] 31 | pub const INTRINSICS_FR1: Intrinsics = Intrinsics { 32 | principal_point: (318.643_040, 255.313_989), 33 | focal: (517.306_408, 516.469_215), 34 | skew: 0.0, 35 | }; 36 | 37 | /// Intrinsics parameters of freiburg 2 (fr2) scenes in the TUM RGB-D dataset. 38 | #[allow(clippy::excessive_precision)] 39 | pub const INTRINSICS_FR2: Intrinsics = Intrinsics { 40 | principal_point: (325.141_442, 249.701_764), 41 | focal: (520.908_620, 521.007_327), 42 | skew: 0.0, 43 | }; 44 | 45 | /// Intrinsics parameters of freiburg 3 (fr3) scenes in the TUM RGB-D dataset. 46 | #[allow(clippy::excessive_precision)] 47 | pub const INTRINSICS_FR3: Intrinsics = Intrinsics { 48 | principal_point: (320.106_653, 247.632_132), 49 | focal: (535.433_105, 539.212_524), 50 | skew: 0.0, 51 | }; 52 | 53 | /// Timestamp and 3D camera pose of a frame. 54 | #[derive(Debug)] 55 | pub struct Frame { 56 | /// Timestamp of the frame. 57 | pub timestamp: f64, 58 | /// Pose (rigid body motion / direct isometry) of the frame. 59 | pub pose: Iso3, 60 | } 61 | 62 | /// Association of two related depth and color timestamps and images file paths. 63 | #[derive(Debug)] 64 | pub struct Association { 65 | /// Timestamp of the depth image. 66 | pub depth_timestamp: f64, 67 | /// File path of the depth image. 68 | pub depth_file_path: PathBuf, 69 | /// Timestamp of the color image. 70 | pub color_timestamp: f64, 71 | /// File path of the color image. 72 | pub color_file_path: PathBuf, 73 | } 74 | 75 | /// Write Frame data in the TUM RGB-D format for trajectories. 76 | impl std::string::ToString for Frame { 77 | /// `timestamp tx ty tz qx qy qz qw` 78 | fn to_string(&self) -> String { 79 | let t = self.pose.translation.vector; 80 | let q = self.pose.rotation.into_inner().coords; 81 | format!( 82 | "{} {} {} {} {} {} {} {}", 83 | self.timestamp, t.x, t.y, t.z, q.x, q.y, q.z, q.w 84 | ) 85 | } 86 | } 87 | 88 | /// Parse useful files (trajectories, associations, ...) in a dataset using the TUM RGB-D format. 89 | pub mod parse { 90 | use super::*; 91 | use nom::{ 92 | alt, anychar, do_parse, double, float, is_not, many0, map, named, space, tag, 93 | types::CompleteStr, 94 | }; 95 | 96 | /// Parse an association file into a vector of `Association`. 97 | pub fn associations(file_content: &str) -> Result, String> { 98 | multi_line(association_line, file_content) 99 | } 100 | 101 | /// Parse a trajectory file into a vector of `Frame`. 102 | pub fn trajectory(file_content: &str) -> Result, String> { 103 | multi_line(trajectory_line, file_content) 104 | } 105 | 106 | fn multi_line(line_parser: F, file_content: &str) -> Result, String> 107 | where 108 | F: Fn(CompleteStr) -> nom::IResult>, 109 | { 110 | let mut vec_data = Vec::new(); 111 | for line in file_content.lines() { 112 | match line_parser(CompleteStr(line)) { 113 | Ok((_, Some(data))) => vec_data.push(data), 114 | Ok(_) => (), 115 | Err(_) => return Err("Parsing error".to_string()), 116 | } 117 | } 118 | Ok(vec_data) 119 | } 120 | 121 | // nom parsers ############################################################# 122 | 123 | // Associations -------------------- 124 | 125 | // Association line is either a comment or two timestamps and file paths. 126 | named!(association_line >, 127 | alt!( map!(comment, |_| None) | map!(association, Some) ) 128 | ); 129 | 130 | // Parse an association of depth and color timestamps and file paths. 131 | named!(association, 132 | do_parse!( 133 | depth_timestamp: double >> space >> 134 | depth_file_path: path >> space >> 135 | color_timestamp: double >> space >> 136 | color_file_path: path >> 137 | (Association { depth_timestamp, depth_file_path, color_timestamp, color_file_path }) 138 | ) 139 | ); 140 | 141 | named!(path, 142 | map!(is_not!(" \t\r\n"), |s| PathBuf::from(*s)) 143 | ); 144 | 145 | // Trajectory ---------------------- 146 | 147 | // Trajectory line is either a comment or a frame timestamp and pose. 148 | named!(trajectory_line >, 149 | alt!( map!(comment, |_| None) | map!(frame, Some) ) 150 | ); 151 | 152 | // Parse a comment. 153 | named!(comment, 154 | do_parse!( tag!("#") >> many0!(anychar) >> ()) 155 | ); 156 | 157 | // Parse a frame. 158 | named!(frame, 159 | do_parse!( 160 | t: double >> space >> 161 | p: pose >> 162 | (Frame { timestamp: t, pose: p }) 163 | ) 164 | ); 165 | 166 | // Parse extrinsics camera parameters. 167 | named!(pose, 168 | do_parse!( 169 | t: translation >> space >> 170 | r: rotation >> 171 | (Iso3::from_parts(t, r)) 172 | ) 173 | ); 174 | 175 | // Parse components of a translation. 176 | named!(translation >, 177 | do_parse!( 178 | x: float >> space >> 179 | y: float >> space >> 180 | z: float >> 181 | (na::Translation3::new(x, y, z)) 182 | ) 183 | ); 184 | 185 | // Parse components of a unit quaternion describing the rotation. 186 | named!(rotation >, 187 | do_parse!( 188 | qx: float >> space >> 189 | qy: float >> space >> 190 | qz: float >> space >> 191 | qw: float >> 192 | (na::UnitQuaternion::from_quaternion(na::Quaternion::new(qw, qx, qy, qz))) 193 | ) 194 | ); 195 | 196 | } // pub mod parse 197 | -------------------------------------------------------------------------------- /src/lib.rs: -------------------------------------------------------------------------------- 1 | // This Source Code Form is subject to the terms of the Mozilla Public 2 | // License, v. 2.0. If a copy of the MPL was not distributed with this 3 | // file, You can obtain one at http://mozilla.org/MPL/2.0/. 4 | 5 | //! # Visual Odometry in Rust (vors) 6 | //! 7 | //! `visual-odometry-rs` is a library providing implementation 8 | //! of visual odometry algorithms fully in Rust. 9 | 10 | #![warn(missing_docs)] 11 | 12 | pub mod core; 13 | pub mod dataset; 14 | pub mod math; 15 | pub mod misc; 16 | -------------------------------------------------------------------------------- /src/math/mod.rs: -------------------------------------------------------------------------------- 1 | // This Source Code Form is subject to the terms of the Mozilla Public 2 | // License, v. 2.0. If a copy of the MPL was not distributed with this 3 | // file, You can obtain one at http://mozilla.org/MPL/2.0/. 4 | 5 | //! Math-related modules. 6 | 7 | pub mod optimizer; 8 | pub mod se3; 9 | pub mod so3; 10 | -------------------------------------------------------------------------------- /src/math/optimizer.rs: -------------------------------------------------------------------------------- 1 | // This Source Code Form is subject to the terms of the Mozilla Public 2 | // License, v. 2.0. If a copy of the MPL was not distributed with this 3 | // file, You can obtain one at http://mozilla.org/MPL/2.0/. 4 | 5 | //! Guiding traits to implement iterative optimization algorithms. 6 | 7 | /// Enum used to indicate if iterations should continue or stop. 8 | /// Must be returned by the `stop_criterion` function. 9 | pub enum Continue { 10 | /// Stop iterations. 11 | Stop, 12 | /// Continue iterations. 13 | Forward, 14 | } 15 | 16 | /// An `State` 17 | /// is capable of iteratively minimizing an energy function, 18 | /// if provided few functions that are evaluated during iterations. 19 | /// 20 | /// It merely is a skeleton for any iterative optimizer, 21 | /// that I have found to be flexible enough for all my past needs. 22 | /// Here is a simple description of its generic types. 23 | /// 24 | /// * `Self`: State of the iterative optimizer. 25 | /// * `Observations`: Data used as reference during energy evaluations. 26 | /// * `EvalState`: Data computed while evaluating a model just computed. 27 | /// Will typically be result successfully containing all the data 28 | /// needed to update the optimizer state, 29 | /// or an error meaning that we stopped the evaluation because the energy increased. 30 | /// * `Model`: The model of what you are trying to optimize. 31 | /// * `Error`: Custom error type for potential failures in step computation. 32 | pub trait State 33 | where 34 | Self: std::marker::Sized, 35 | { 36 | /// Initialize the optimizer state. 37 | fn init(obs: &Observations, model: Model) -> Self; 38 | 39 | /// Compute an iteration step from the current optimizer state. 40 | /// May fail, in such cases, iterations are stopped. 41 | fn step(&self) -> Result; 42 | 43 | /// Evaluate the model. 44 | /// You might want to short-circuit evaluation of a full new state depending on your usage 45 | /// (e.g. if the energy increases). 46 | /// This is why it returns an `EvalState` and not `Self`. 47 | fn eval(&self, obs: &Observations, new_model: Model) -> EvalState; 48 | 49 | /// Function deciding if iterations should continue. 50 | /// Also return the state that will be used for next iteration. 51 | fn stop_criterion(self, nb_iter: usize, eval_state: EvalState) -> (Self, Continue); 52 | 53 | /// Iteratively solve your optimization problem, 54 | /// with the provided functions by the trait implementation. 55 | /// Return the final state and the number of iterations. 56 | /// May return an error if a step computation failed. 57 | fn iterative_solve(obs: &Observations, initial_model: Model) -> Result<(Self, usize), Error> { 58 | let mut state = Self::init(obs, initial_model); 59 | let mut nb_iter = 0; 60 | loop { 61 | nb_iter += 1; 62 | let new_model = state.step()?; 63 | let eval_state = state.eval(obs, new_model); 64 | let (kept_state, continuation) = state.stop_criterion(nb_iter, eval_state); 65 | state = kept_state; 66 | if let Continue::Stop = continuation { 67 | return Ok((state, nb_iter)); 68 | } 69 | } 70 | } 71 | } 72 | -------------------------------------------------------------------------------- /src/math/se3.rs: -------------------------------------------------------------------------------- 1 | // This Source Code Form is subject to the terms of the Mozilla Public 2 | // License, v. 2.0. If a copy of the MPL was not distributed with this 3 | // file, You can obtain one at http://mozilla.org/MPL/2.0/. 4 | 5 | //! Lie algebra/group functions for 3D rigid body motion. 6 | //! 7 | //! Interesting reads: 8 | //! - Sophus c++ library: 9 | //! - Ethan Eade course on Lie Groups for 2D and 3D transformations: 10 | //! - details: 11 | //! - summary: 12 | 13 | use nalgebra::{Quaternion, Translation3, UnitQuaternion}; 14 | use std::f32::consts::PI; 15 | 16 | use crate::math::so3; 17 | use crate::misc::type_aliases::{Float, Iso3, Mat3, Mat4, Vec3, Vec6}; 18 | 19 | const EPSILON_TAYLOR_SERIES: Float = 1e-2; 20 | const EPSILON_TAYLOR_SERIES_2: Float = EPSILON_TAYLOR_SERIES * EPSILON_TAYLOR_SERIES; 21 | const _1_6: Float = 1.0 / 6.0; 22 | const _1_8: Float = 0.125; 23 | const _1_12: Float = 1.0 / 12.0; 24 | const _1_15: Float = 1.0 / 15.0; 25 | const _1_24: Float = 1.0 / 24.0; 26 | const _1_48: Float = 1.0 / 48.0; 27 | const _1_120: Float = 1.0 / 120.0; 28 | 29 | /// Parameterization of a twist (element of se3). 30 | pub type Twist = Vec6; 31 | 32 | /// Retrieve the linear velocity part of the twist parameterization. 33 | pub fn linear_velocity(xi: Twist) -> Vec3 { 34 | Vec3::new(xi[0], xi[1], xi[2]) 35 | } 36 | 37 | /// Retrieve the angular velocity part of the twist parameterization. 38 | pub fn angular_velocity(xi: Twist) -> Vec3 { 39 | Vec3::new(xi[3], xi[4], xi[5]) 40 | } 41 | 42 | /// Hat operator. 43 | /// Goes from se3 parameters to se3 element (4x4 matrix). 44 | #[rustfmt::skip] 45 | pub fn hat(xi: Twist) -> Mat4 { 46 | let w1 = xi[3]; 47 | let w2 = xi[4]; 48 | let w3 = xi[5]; 49 | Mat4::new( 50 | 0.0, -w3, w2, xi[0], 51 | w3, 0.0, -w1, xi[1], 52 | -w2, w1, 0.0, xi[2], 53 | 0.0, 0.0, 0.0, 0.0, 54 | ) 55 | } 56 | 57 | /// Vee operator. Inverse of hat operator. 58 | /// Warning! does not check that the given top left 3x3 sub-matrix is skew-symmetric. 59 | pub fn vee(mat: Mat4) -> Twist { 60 | Vec6::new(mat.m14, mat.m24, mat.m34, mat.m32, mat.m13, mat.m21) 61 | } 62 | 63 | /// Compute the exponential map from Lie algebra se3 to Lie group SE3. 64 | /// Goes from se3 parameterization to SE3 element (rigid body motion). 65 | pub fn exp(xi: Twist) -> Iso3 { 66 | let xi_v = linear_velocity(xi); 67 | let xi_w = angular_velocity(xi); 68 | let theta_2 = xi_w.norm_squared(); 69 | let (omega, omega_2) = (so3::hat(xi_w), so3::hat_2(xi_w)); 70 | if theta_2 < EPSILON_TAYLOR_SERIES_2 { 71 | let real_factor = 1.0 - _1_8 * theta_2; // TAYLOR 72 | let imag_factor = 0.5 - _1_48 * theta_2; // TAYLOR 73 | let coef_omega = 0.5 - _1_24 * theta_2; // TAYLOR 74 | let coef_omega_2 = _1_6 - _1_120 * theta_2; // TAYLOR 75 | let v = Mat3::identity() + coef_omega * omega + coef_omega_2 * omega_2; 76 | let rotation = UnitQuaternion::from_quaternion(Quaternion::from_parts( 77 | real_factor, 78 | imag_factor * xi_w, 79 | )); 80 | Iso3::from_parts(Translation3::from(v * xi_v), rotation) 81 | } else { 82 | let theta = theta_2.sqrt(); 83 | let half_theta = 0.5 * theta; 84 | let real_factor = half_theta.cos(); 85 | let imag_factor = half_theta.sin() / theta; 86 | let coef_omega = (1.0 - theta.cos()) / theta_2; 87 | let coef_omega_2 = (theta - theta.sin()) / (theta * theta_2); 88 | let v = Mat3::identity() + coef_omega * omega + coef_omega_2 * omega_2; 89 | let rotation = UnitQuaternion::from_quaternion(Quaternion::from_parts( 90 | real_factor, 91 | imag_factor * xi_w, 92 | )); 93 | Iso3::from_parts(Translation3::from(v * xi_v), rotation) 94 | } 95 | } 96 | 97 | /// Compute the logarithm map from the Lie group SE3 to the Lie algebra se3. 98 | /// Inverse of the exponential map. 99 | pub fn log(iso: Iso3) -> Twist { 100 | let imag_vector = iso.rotation.vector(); 101 | let imag_norm_2 = imag_vector.norm_squared(); 102 | let real_factor = iso.rotation.scalar(); 103 | if imag_norm_2 < EPSILON_TAYLOR_SERIES_2 { 104 | let theta_by_imag_norm = 2.0 / real_factor; // TAYLOR 105 | let w = theta_by_imag_norm * imag_vector; 106 | let (omega, omega_2) = (so3::hat(w), so3::hat_2(w)); 107 | let x_2 = imag_norm_2 / (real_factor * real_factor); 108 | let coef_omega_2 = _1_12 * (1.0 + _1_15 * x_2); // TAYLOR 109 | let v_inv = Mat3::identity() - 0.5 * omega + coef_omega_2 * omega_2; 110 | let xi_v = v_inv * iso.translation.vector; 111 | Vec6::new(xi_v[0], xi_v[1], xi_v[2], w[0], w[1], w[2]) 112 | } else { 113 | let imag_norm = imag_norm_2.sqrt(); 114 | let theta = if real_factor.abs() < EPSILON_TAYLOR_SERIES { 115 | let alpha = real_factor.abs() / imag_norm; 116 | real_factor.signum() * (PI - 2.0 * alpha) // TAYLOR 117 | } else { 118 | // Is this correct? should I use atan2 instead? 119 | 2.0 * (imag_norm / real_factor).atan() 120 | }; 121 | let theta_2 = theta * theta; 122 | let w = (theta / imag_norm) * imag_vector; 123 | let (omega, omega_2) = (so3::hat(w), so3::hat_2(w)); 124 | let coef_omega_2 = (1.0 - 0.5 * theta * real_factor / imag_norm) / theta_2; 125 | let v_inv = Mat3::identity() - 0.5 * omega + coef_omega_2 * omega_2; 126 | let xi_v = v_inv * iso.translation.vector; 127 | Vec6::new(xi_v[0], xi_v[1], xi_v[2], w[0], w[1], w[2]) 128 | } 129 | } 130 | 131 | // TESTS ############################################################# 132 | 133 | #[cfg(test)] 134 | mod tests { 135 | 136 | use super::*; 137 | use approx; 138 | use quickcheck_macros; 139 | 140 | // The best precision I get for round trips with quickcheck random inputs 141 | // with exact trigonometric computations ("else" branches) is around 1e-4. 142 | const EPSILON_ROUNDTRIP_APPROX: Float = 1e-4; 143 | 144 | #[test] 145 | fn exp_log_round_trip() { 146 | let xi = Vec6::zeros(); 147 | assert_eq!(xi, log(exp(xi))); 148 | } 149 | 150 | // PROPERTY TESTS ################################################ 151 | 152 | #[quickcheck_macros::quickcheck] 153 | fn hat_vee_roundtrip(v1: Float, v2: Float, v3: Float, w1: Float, w2: Float, w3: Float) -> bool { 154 | let xi = Vec6::new(v1, v2, v3, w1, w2, w3); 155 | xi == vee(hat(xi)) 156 | } 157 | 158 | #[quickcheck_macros::quickcheck] 159 | fn log_exp_round_trip( 160 | t1: Float, 161 | t2: Float, 162 | t3: Float, 163 | a1: Float, 164 | a2: Float, 165 | a3: Float, 166 | ) -> bool { 167 | let rigid_motion = gen_rigid_motion(t1, t2, t3, a1, a2, a3); 168 | approx::relative_eq!( 169 | rigid_motion, 170 | exp(log(rigid_motion)), 171 | epsilon = EPSILON_ROUNDTRIP_APPROX 172 | ) 173 | } 174 | 175 | // GENERATORS #################################################### 176 | 177 | fn gen_rigid_motion(t1: Float, t2: Float, t3: Float, a1: Float, a2: Float, a3: Float) -> Iso3 { 178 | let translation = Translation3::from(Vec3::new(t1, t2, t3)); 179 | let rotation = UnitQuaternion::from_euler_angles(a1, a2, a3); 180 | Iso3::from_parts(translation, rotation) 181 | } 182 | } 183 | -------------------------------------------------------------------------------- /src/math/so3.rs: -------------------------------------------------------------------------------- 1 | // This Source Code Form is subject to the terms of the Mozilla Public 2 | // License, v. 2.0. If a copy of the MPL was not distributed with this 3 | // file, You can obtain one at http://mozilla.org/MPL/2.0/. 4 | 5 | //! Lie algebra/group functions for 3D rotations. 6 | //! 7 | //! Interesting reads: 8 | //! - Sophus c++ library: 9 | //! - Ethan Eade course on Lie Groups for 2D and 3D transformations: 10 | //! - details: 11 | //! - summary: 12 | 13 | use nalgebra::{Quaternion, UnitQuaternion}; 14 | use std::f32::consts::PI; 15 | 16 | use crate::misc::type_aliases::{Float, Mat3, Vec3}; 17 | 18 | /// Threshold for using Taylor series in computations. 19 | const EPSILON_TAYLOR_SERIES: Float = 1e-2; 20 | const EPSILON_TAYLOR_SERIES_2: Float = EPSILON_TAYLOR_SERIES * EPSILON_TAYLOR_SERIES; 21 | const _1_8: Float = 0.125; 22 | const _1_48: Float = 1.0 / 48.0; 23 | 24 | /// Hat operator. 25 | /// Goes from so3 parameterization to so3 element (skew-symmetric matrix). 26 | #[rustfmt::skip] 27 | pub fn hat(w: Vec3) -> Mat3 { 28 | Mat3::new( 29 | 0.0, -w.z, w.y, 30 | w.z, 0.0, -w.x, 31 | -w.y, w.x, 0.0, 32 | ) 33 | } 34 | 35 | /// Squared hat operator (`hat_2(w) == hat(w) * hat(w)`). 36 | /// Result is a symmetric matrix. 37 | #[rustfmt::skip] 38 | pub fn hat_2(w: Vec3) -> Mat3 { 39 | let w11 = w.x * w.x; 40 | let w12 = w.x * w.y; 41 | let w13 = w.x * w.z; 42 | let w22 = w.y * w.y; 43 | let w23 = w.y * w.z; 44 | let w33 = w.z * w.z; 45 | Mat3::new( 46 | -w22 - w33, w12, w13, 47 | w12, -w11 - w33, w23, 48 | w13, w23, -w11 - w22, 49 | ) 50 | } 51 | 52 | /// Vee operator. Inverse of hat operator. 53 | /// Warning! does not check that the given matrix is skew-symmetric. 54 | pub fn vee(mat: Mat3) -> Vec3 { 55 | Vec3::new(mat.m32, mat.m13, mat.m21) 56 | } 57 | 58 | /// Compute the exponential map from Lie algebra so3 to Lie group SO3. 59 | /// Goes from so3 parameterization to SO3 element (rotation). 60 | #[allow(clippy::useless_let_if_seq)] 61 | pub fn exp(w: Vec3) -> UnitQuaternion { 62 | let theta_2 = w.norm_squared(); 63 | let real_factor; 64 | let imag_factor; 65 | if theta_2 < EPSILON_TAYLOR_SERIES_2 { 66 | real_factor = 1.0 - _1_8 * theta_2; 67 | imag_factor = 0.5 - _1_48 * theta_2; 68 | } else { 69 | let theta = theta_2.sqrt(); 70 | let half_theta = 0.5 * theta; 71 | real_factor = half_theta.cos(); 72 | imag_factor = half_theta.sin() / theta; 73 | } 74 | // TODO: This is actually already a unit quaternion so we should not use 75 | // the from_quaternion function that performs a renormalization. 76 | UnitQuaternion::from_quaternion(Quaternion::from_parts(real_factor, imag_factor * w)) 77 | } 78 | 79 | /// Compute the logarithm map from the Lie group SO3 to the Lie algebra so3. 80 | /// Inverse of the exponential map. 81 | pub fn log(rotation: UnitQuaternion) -> Vec3 { 82 | let imag_vector = rotation.vector(); 83 | let imag_norm_2 = imag_vector.norm_squared(); 84 | let real_factor = rotation.scalar(); 85 | if imag_norm_2 < EPSILON_TAYLOR_SERIES_2 { 86 | let theta_by_imag_norm = 2.0 / real_factor; // TAYLOR 87 | theta_by_imag_norm * imag_vector 88 | } else if real_factor.abs() < EPSILON_TAYLOR_SERIES { 89 | let imag_norm = imag_norm_2.sqrt(); 90 | let alpha = real_factor.abs() / imag_norm; 91 | let theta = real_factor.signum() * (PI - 2.0 * alpha); // TAYLOR 92 | (theta / imag_norm) * imag_vector 93 | } else { 94 | let imag_norm = imag_norm_2.sqrt(); 95 | // Is atan correct? should I use atan2 instead? 96 | let theta = 2.0 * (imag_norm / real_factor).atan(); 97 | (theta / imag_norm) * imag_vector 98 | } 99 | } 100 | 101 | // TESTS ############################################################# 102 | 103 | #[cfg(test)] 104 | mod tests { 105 | 106 | use super::*; 107 | use approx; 108 | use quickcheck_macros; 109 | 110 | // The best precision I get for round trips with quickcheck random inputs 111 | // with exact trigonometric computations ("else" branches) is around 1e-6. 112 | const EPSILON_ROUNDTRIP_APPROX: Float = 1e-6; 113 | 114 | #[test] 115 | fn exp_log_round_trip() { 116 | let w = Vec3::zeros(); 117 | assert_eq!(w, log(exp(w))); 118 | } 119 | 120 | // PROPERTY TESTS ################################################ 121 | 122 | #[quickcheck_macros::quickcheck] 123 | fn hat_vee_roundtrip(x: Float, y: Float, z: Float) -> bool { 124 | let element = Vec3::new(x, y, z); 125 | element == vee(hat(element)) 126 | } 127 | 128 | #[quickcheck_macros::quickcheck] 129 | fn hat_2_ok(x: Float, y: Float, z: Float) -> bool { 130 | let element = Vec3::new(x, y, z); 131 | hat_2(element) == hat(element) * hat(element) 132 | } 133 | 134 | #[quickcheck_macros::quickcheck] 135 | fn log_exp_round_trip(roll: Float, pitch: Float, yaw: Float) -> bool { 136 | let rotation = gen_rotation(roll, pitch, yaw); 137 | approx::relative_eq!( 138 | rotation, 139 | exp(log(rotation)), 140 | epsilon = EPSILON_ROUNDTRIP_APPROX 141 | ) 142 | } 143 | 144 | // GENERATORS #################################################### 145 | 146 | fn gen_rotation(roll: Float, pitch: Float, yaw: Float) -> UnitQuaternion { 147 | UnitQuaternion::from_euler_angles(roll, pitch, yaw) 148 | } 149 | } 150 | -------------------------------------------------------------------------------- /src/misc/colormap.rs: -------------------------------------------------------------------------------- 1 | // This Source Code Form is subject to the terms of the Mozilla Public 2 | // License, v. 2.0. If a copy of the MPL was not distributed with this 3 | // file, You can obtain one at http://mozilla.org/MPL/2.0/. 4 | 5 | //! Colormap helpers for visualizations. 6 | 7 | /// Viridis colormap in an RGB u8 vector. 8 | pub fn viridis_u8() -> Vec<(u8, u8, u8)> { 9 | viridis() 10 | .into_iter() 11 | .map(|(r, g, b)| (to_u8(r), to_u8(g), to_u8(b))) 12 | .collect() 13 | } 14 | 15 | #[allow(clippy::cast_possible_truncation)] 16 | #[allow(clippy::cast_sign_loss)] 17 | fn to_u8(x: f32) -> u8 { 18 | (255.0 * x).round() as u8 19 | } 20 | 21 | /// Viridis colormap in an RGB f32 ([0-1]) vector. 22 | pub fn viridis() -> Vec<(f32, f32, f32)> { 23 | vec![ 24 | (0.267_004, 0.004_874, 0.329_415), 25 | (0.268_510, 0.009_604, 0.335_426), 26 | (0.269_943, 0.014_624, 0.341_378), 27 | (0.271_304, 0.019_941, 0.347_268), 28 | (0.272_593, 0.025_563, 0.353_093), 29 | (0.273_809, 0.031_497, 0.358_852), 30 | (0.274_952, 0.037_751, 0.364_543), 31 | (0.276_022, 0.044_167, 0.370_164), 32 | (0.277_018, 0.050_344, 0.375_714), 33 | (0.277_941, 0.056_324, 0.381_190), 34 | (0.278_790, 0.062_145, 0.386_592), 35 | (0.279_565, 0.067_835, 0.391_917), 36 | (0.280_266, 0.073_417, 0.397_163), 37 | (0.280_893, 0.078_907, 0.402_329), 38 | (0.281_445, 0.084_319, 0.407_414), 39 | (0.281_923, 0.089_666, 0.412_415), 40 | (0.282_327, 0.094_955, 0.417_330), 41 | (0.282_656, 0.100_195, 0.422_160), 42 | (0.282_910, 0.105_393, 0.426_902), 43 | (0.283_090, 0.110_553, 0.431_553), 44 | (0.283_197, 0.115_679, 0.436_114), 45 | (0.283_228, 0.120_777, 0.440_584), 46 | (0.283_186, 0.125_847, 0.444_960), 47 | (0.283_072, 0.130_894, 0.449_241), 48 | (0.282_883, 0.135_920, 0.453_427), 49 | (0.282_622, 0.140_925, 0.457_517), 50 | (0.282_290, 0.145_912, 0.461_509), 51 | (0.281_886, 0.150_881, 0.465_404), 52 | (0.281_412, 0.155_834, 0.469_201), 53 | (0.280_867, 0.160_771, 0.472_899), 54 | (0.280_254, 0.165_692, 0.476_497), 55 | (0.279_573, 0.170_598, 0.479_996), 56 | (0.278_826, 0.175_490, 0.483_396), 57 | (0.278_012, 0.180_366, 0.486_697), 58 | (0.277_134, 0.185_228, 0.489_898), 59 | (0.276_193, 0.190_074, 0.493_000), 60 | (0.275_191, 0.194_905, 0.496_004), 61 | (0.274_128, 0.199_720, 0.498_911), 62 | (0.273_005, 0.204_520, 0.501_720), 63 | (0.271_828, 0.209_303, 0.504_434), 64 | (0.270_594, 0.214_068, 0.507_052), 65 | (0.269_307, 0.218_817, 0.509_576), 66 | (0.267_968, 0.223_549, 0.512_008), 67 | (0.266_579, 0.228_262, 0.514_348), 68 | (0.265_144, 0.232_955, 0.516_599), 69 | (0.263_663, 0.237_630, 0.518_761), 70 | (0.262_138, 0.242_286, 0.520_837), 71 | (0.260_571, 0.246_921, 0.522_828), 72 | (0.258_964, 0.251_536, 0.524_736), 73 | (0.257_322, 0.256_130, 0.526_563), 74 | (0.255_645, 0.260_702, 0.528_311), 75 | (0.253_934, 0.265_253, 0.529_982), 76 | (0.252_194, 0.269_783, 0.531_579), 77 | (0.250_424, 0.274_290, 0.533_102), 78 | (0.248_628, 0.278_775, 0.534_555), 79 | (0.246_811, 0.283_236, 0.535_940), 80 | (0.244_972, 0.287_675, 0.537_260), 81 | (0.243_113, 0.292_091, 0.538_515), 82 | (0.241_237, 0.296_484, 0.539_709), 83 | (0.239_345, 0.300_854, 0.540_843), 84 | (0.237_441, 0.305_202, 0.541_921), 85 | (0.235_526, 0.309_526, 0.542_943), 86 | (0.233_602, 0.313_827, 0.543_914), 87 | (0.231_673, 0.318_105, 0.544_834), 88 | (0.229_739, 0.322_361, 0.545_706), 89 | (0.227_801, 0.326_594, 0.546_532), 90 | (0.225_863, 0.330_805, 0.547_313), 91 | (0.223_925, 0.334_994, 0.548_052), 92 | (0.221_989, 0.339_161, 0.548_752), 93 | (0.220_056, 0.343_306, 0.549_413), 94 | (0.218_129, 0.347_431, 0.550_037), 95 | (0.216_209, 0.351_535, 0.550_627), 96 | (0.214_297, 0.355_619, 0.551_184), 97 | (0.212_394, 0.359_682, 0.551_710), 98 | (0.210_503, 0.363_726, 0.552_206), 99 | (0.208_623, 0.367_751, 0.552_674), 100 | (0.206_756, 0.371_757, 0.553_116), 101 | (0.204_902, 0.375_745, 0.553_532), 102 | (0.203_063, 0.379_716, 0.553_925), 103 | (0.201_238, 0.383_669, 0.554_294), 104 | (0.199_429, 0.387_606, 0.554_642), 105 | (0.197_636, 0.391_527, 0.554_969), 106 | (0.195_859, 0.395_432, 0.555_276), 107 | (0.194_100, 0.399_323, 0.555_564), 108 | (0.192_357, 0.403_199, 0.555_835), 109 | (0.190_631, 0.407_061, 0.556_089), 110 | (0.188_922, 0.410_910, 0.556_326), 111 | (0.187_230, 0.414_746, 0.556_547), 112 | (0.185_555, 0.418_570, 0.556_752), 113 | (0.183_897, 0.422_382, 0.556_943), 114 | (0.182_255, 0.426_184, 0.557_120), 115 | (0.180_629, 0.429_974, 0.557_282), 116 | (0.179_018, 0.433_755, 0.557_430), 117 | (0.177_422, 0.437_527, 0.557_564), 118 | (0.175_841, 0.441_289, 0.557_685), 119 | (0.174_273, 0.445_044, 0.557_792), 120 | (0.172_718, 0.448_790, 0.557_885), 121 | (0.171_176, 0.452_529, 0.557_964), 122 | (0.169_645, 0.456_262, 0.558_030), 123 | (0.168_126, 0.459_988, 0.558_081), 124 | (0.166_617, 0.463_708, 0.558_119), 125 | (0.165_117, 0.467_422, 0.558_141), 126 | (0.163_625, 0.471_132, 0.558_148), 127 | (0.162_141, 0.474_838, 0.558_139), 128 | (0.160_664, 0.478_539, 0.558_114), 129 | (0.159_194, 0.482_237, 0.558_072), 130 | (0.157_729, 0.485_931, 0.558_013), 131 | (0.156_269, 0.489_623, 0.557_936), 132 | (0.154_814, 0.493_312, 0.557_839), 133 | (0.153_364, 0.497_000, 0.557_723), 134 | (0.151_918, 0.500_685, 0.557_587), 135 | (0.150_476, 0.504_369, 0.557_429), 136 | (0.149_039, 0.508_051, 0.557_250), 137 | (0.147_607, 0.511_732, 0.557_048), 138 | (0.146_180, 0.515_413, 0.556_822), 139 | (0.144_758, 0.519_093, 0.556_571), 140 | (0.143_343, 0.522_772, 0.556_294), 141 | (0.141_935, 0.526_452, 0.555_990), 142 | (0.140_535, 0.530_132, 0.555_658), 143 | (0.139_147, 0.533_812, 0.555_297), 144 | (0.137_770, 0.537_492, 0.554_906), 145 | (0.136_408, 0.541_172, 0.554_483), 146 | (0.135_065, 0.544_853, 0.554_029), 147 | (0.133_742, 0.548_534, 0.553_541), 148 | (0.132_444, 0.552_216, 0.553_018), 149 | (0.131_172, 0.555_898, 0.552_459), 150 | (0.129_932, 0.559_581, 0.551_863), 151 | (0.128_729, 0.563_265, 0.551_229), 152 | (0.127_567, 0.566_948, 0.550_555), 153 | (0.126_453, 0.570_633, 0.549_841), 154 | (0.125_393, 0.574_317, 0.549_085), 155 | (0.124_394, 0.578_002, 0.548_287), 156 | (0.123_462, 0.581_686, 0.547_444), 157 | (0.122_605, 0.585_371, 0.546_557), 158 | (0.121_831, 0.589_055, 0.545_622), 159 | (0.121_148, 0.592_738, 0.544_641), 160 | (0.120_565, 0.596_421, 0.543_610), 161 | (0.120_091, 0.600_103, 0.542_530), 162 | (0.119_737, 0.603_784, 0.541_399), 163 | (0.119_511, 0.607_463, 0.540_217), 164 | (0.119_423, 0.611_141, 0.538_981), 165 | (0.119_482, 0.614_817, 0.537_692), 166 | (0.119_698, 0.618_490, 0.536_347), 167 | (0.120_080, 0.622_160, 0.534_946), 168 | (0.120_638, 0.625_828, 0.533_488), 169 | (0.121_379, 0.629_492, 0.531_972), 170 | (0.122_312, 0.633_152, 0.530_398), 171 | (0.123_443, 0.636_808, 0.528_763), 172 | (0.124_779, 0.640_460, 0.527_067), 173 | (0.126_325, 0.644_107, 0.525_310), 174 | (0.128_087, 0.647_748, 0.523_490), 175 | (0.130_066, 0.651_384, 0.521_607), 176 | (0.132_267, 0.655_013, 0.519_660), 177 | (0.134_691, 0.658_636, 0.517_648), 178 | (0.137_339, 0.662_251, 0.515_571), 179 | (0.140_209, 0.665_859, 0.513_426), 180 | (0.143_302, 0.669_458, 0.511_215), 181 | (0.146_616, 0.673_049, 0.508_936), 182 | (0.150_147, 0.676_631, 0.506_588), 183 | (0.153_894, 0.680_203, 0.504_172), 184 | (0.157_851, 0.683_765, 0.501_685), 185 | (0.162_015, 0.687_316, 0.499_129), 186 | (0.166_383, 0.690_856, 0.496_501), 187 | (0.170_948, 0.694_384, 0.493_802), 188 | (0.175_706, 0.697_899, 0.491_032), 189 | (0.180_653, 0.701_402, 0.488_189), 190 | (0.185_782, 0.704_891, 0.485_273), 191 | (0.191_090, 0.708_366, 0.482_283), 192 | (0.196_570, 0.711_826, 0.479_221), 193 | (0.202_219, 0.715_271, 0.476_084), 194 | (0.208_030, 0.718_700, 0.472_873), 195 | (0.214_000, 0.722_113, 0.469_587), 196 | (0.220_123, 0.725_509, 0.466_226), 197 | (0.226_396, 0.728_887, 0.462_789), 198 | (0.232_814, 0.732_247, 0.459_276), 199 | (0.239_373, 0.735_588, 0.455_688), 200 | (0.246_069, 0.738_909, 0.452_024), 201 | (0.252_898, 0.742_211, 0.448_283), 202 | (0.259_856, 0.745_491, 0.444_466), 203 | (0.266_941, 0.748_750, 0.440_572), 204 | (0.274_149, 0.751_988, 0.436_600), 205 | (0.281_476, 0.755_202, 0.432_552), 206 | (0.288_921, 0.758_393, 0.428_426), 207 | (0.296_478, 0.761_561, 0.424_223), 208 | (0.304_147, 0.764_704, 0.419_943), 209 | (0.311_925, 0.767_822, 0.415_586), 210 | (0.319_808, 0.770_914, 0.411_152), 211 | (0.327_795, 0.773_979, 0.406_640), 212 | (0.335_885, 0.777_017, 0.402_049), 213 | (0.344_074, 0.780_028, 0.397_381), 214 | (0.352_359, 0.783_010, 0.392_635), 215 | (0.360_740, 0.785_964, 0.387_813), 216 | (0.369_214, 0.788_887, 0.382_914), 217 | (0.377_778, 0.791_781, 0.377_938), 218 | (0.386_432, 0.794_644, 0.372_886), 219 | (0.395_174, 0.797_475, 0.367_757), 220 | (0.404_001, 0.800_274, 0.362_552), 221 | (0.412_913, 0.803_040, 0.357_268), 222 | (0.421_908, 0.805_774, 0.351_910), 223 | (0.430_983, 0.808_473, 0.346_476), 224 | (0.440_136, 0.811_138, 0.340_967), 225 | (0.449_367, 0.813_768, 0.335_384), 226 | (0.458_673, 0.816_362, 0.329_727), 227 | (0.468_053, 0.818_921, 0.323_997), 228 | (0.477_504, 0.821_443, 0.318_195), 229 | (0.487_025, 0.823_928, 0.312_321), 230 | (0.496_615, 0.826_376, 0.306_376), 231 | (0.506_271, 0.828_786, 0.300_362), 232 | (0.515_991, 0.831_157, 0.294_278), 233 | (0.525_776, 0.833_490, 0.288_126), 234 | (0.535_621, 0.835_784, 0.281_908), 235 | (0.545_524, 0.838_039, 0.275_626), 236 | (0.555_483, 0.840_254, 0.269_281), 237 | (0.565_497, 0.842_429, 0.262_876), 238 | (0.575_562, 0.844_565, 0.256_414), 239 | (0.585_677, 0.846_661, 0.249_897), 240 | (0.595_839, 0.848_717, 0.243_328), 241 | (0.606_045, 0.850_733, 0.236_712), 242 | (0.616_292, 0.852_709, 0.230_051), 243 | (0.626_579, 0.854_645, 0.223_352), 244 | (0.636_901, 0.856_542, 0.216_620), 245 | (0.647_256, 0.858_399, 0.209_860), 246 | (0.657_641, 0.860_218, 0.203_082), 247 | (0.668_053, 0.861_999, 0.196_293), 248 | (0.678_488, 0.863_742, 0.189_503), 249 | (0.688_943, 0.865_447, 0.182_724), 250 | (0.699_414, 0.867_117, 0.175_970), 251 | (0.709_898, 0.868_750, 0.169_257), 252 | (0.720_391, 0.870_350, 0.162_602), 253 | (0.730_889, 0.871_915, 0.156_028), 254 | (0.741_388, 0.873_449, 0.149_561), 255 | (0.751_884, 0.874_951, 0.143_228), 256 | (0.762_373, 0.876_423, 0.137_064), 257 | (0.772_851, 0.877_868, 0.131_108), 258 | (0.783_315, 0.879_285, 0.125_405), 259 | (0.793_759, 0.880_677, 0.120_005), 260 | (0.804_181, 0.882_046, 0.114_965), 261 | (0.814_576, 0.883_393, 0.110_346), 262 | (0.824_940, 0.884_720, 0.106_217), 263 | (0.835_269, 0.886_029, 0.102_645), 264 | (0.845_560, 0.887_322, 0.099_702), 265 | (0.855_809, 0.888_601, 0.097_451), 266 | (0.866_013, 0.889_868, 0.095_952), 267 | (0.876_168, 0.891_124, 0.095_250), 268 | (0.886_271, 0.892_373, 0.095_374), 269 | (0.896_320, 0.893_616, 0.096_335), 270 | (0.906_311, 0.894_854, 0.098_124), 271 | (0.916_242, 0.896_091, 0.100_716), 272 | (0.926_105, 0.897_329, 0.104_070), 273 | (0.935_904, 0.898_570, 0.108_130), 274 | (0.945_636, 0.899_815, 0.112_837), 275 | (0.955_299, 0.901_065, 0.118_128), 276 | (0.964_893, 0.902_323, 0.123_940), 277 | (0.974_416, 0.903_589, 0.130_214), 278 | (0.983_868, 0.904_867, 0.136_896), 279 | (0.993_247, 0.906_156, 0.143_936), 280 | ] 281 | } 282 | -------------------------------------------------------------------------------- /src/misc/helper.rs: -------------------------------------------------------------------------------- 1 | // This Source Code Form is subject to the terms of the Mozilla Public 2 | // License, v. 2.0. If a copy of the MPL was not distributed with this 3 | // file, You can obtain one at http://mozilla.org/MPL/2.0/. 4 | 5 | //! Miscellaneous helper functions that didn't fit elsewhere. 6 | 7 | use byteorder::{BigEndian, ReadBytesExt}; 8 | use nalgebra::{DMatrix, Scalar}; 9 | use png::{self, HasParameters}; 10 | use std::{self, fs::File, io::Cursor, path::Path}; 11 | 12 | /// Read a 16 bit gray png image from a file. 13 | pub fn read_png_16bits>( 14 | file_path: P, 15 | ) -> Result<(usize, usize, Vec), png::DecodingError> { 16 | // Load 16 bits PNG depth image. 17 | let img_file = File::open(file_path)?; 18 | let mut decoder = png::Decoder::new(img_file); 19 | // Use the IDENTITY transformation because by default 20 | // it will use STRIP_16 which only keep 8 bits. 21 | // See also SWAP_ENDIAN that might be useful 22 | // (but seems not possible to use according to documentation). 23 | decoder.set(png::Transformations::IDENTITY); 24 | let (info, mut reader) = decoder.read_info()?; 25 | let mut buffer = vec![0; info.buffer_size()]; 26 | reader.next_frame(&mut buffer)?; 27 | 28 | // Transform buffer into 16 bits slice. 29 | // if cfg!(target_endian = "big") ... 30 | let mut buffer_u16 = vec![0; (info.width * info.height) as usize]; 31 | let mut buffer_cursor = Cursor::new(buffer); 32 | buffer_cursor.read_u16_into::(&mut buffer_u16)?; 33 | 34 | // Return u16 buffer. 35 | Ok((info.width as usize, info.height as usize, buffer_u16)) 36 | } 37 | 38 | /// Map a function onto a matrix, at positions given by a mask. 39 | /// A default value is used at the other positions. 40 | pub fn zip_mask_map(mat: &DMatrix, mask: &DMatrix, default: U, f: F) -> DMatrix 41 | where 42 | T: Scalar, 43 | U: Scalar, 44 | F: Fn(T) -> U, 45 | { 46 | mat.zip_map(mask, |x, is_true| if is_true { f(x) } else { default }) 47 | } 48 | 49 | /// Compute the quotient and remainder of x/y both at the same time. 50 | pub fn div_rem(x: T, y: T) -> (T, T) 51 | where 52 | T: std::ops::Div + std::ops::Rem + Copy, 53 | { 54 | (x / y, x % y) 55 | } 56 | -------------------------------------------------------------------------------- /src/misc/interop.rs: -------------------------------------------------------------------------------- 1 | // This Source Code Form is subject to the terms of the Mozilla Public 2 | // License, v. 2.0. If a copy of the MPL was not distributed with this 3 | // file, You can obtain one at http://mozilla.org/MPL/2.0/. 4 | 5 | //! Interoperability conversions between the image and matrix types. 6 | 7 | use image::{GrayImage, ImageBuffer, Luma, Rgb, RgbImage}; 8 | use nalgebra::DMatrix; 9 | 10 | /// Convert an `u8` matrix into a `GrayImage`. 11 | /// Inverse operation of `matrix_from_image`. 12 | /// 13 | /// Performs a transposition to accomodate for the 14 | /// column major matrix into the row major image. 15 | #[allow(clippy::cast_possible_truncation)] 16 | pub fn image_from_matrix(mat: &DMatrix) -> GrayImage { 17 | let (nb_rows, nb_cols) = mat.shape(); 18 | let mut img_buf = GrayImage::new(nb_cols as u32, nb_rows as u32); 19 | for (x, y, pixel) in img_buf.enumerate_pixels_mut() { 20 | *pixel = Luma([mat[(y as usize, x as usize)]]); 21 | } 22 | img_buf 23 | } 24 | 25 | /// Convert an `(u8,u8,8)` matrix into an `RgbImage`. 26 | /// 27 | /// Performs a transposition to accomodate for the 28 | /// column major matrix into the row major image. 29 | #[allow(clippy::cast_possible_truncation)] 30 | pub fn rgb_from_matrix(mat: &DMatrix<(u8, u8, u8)>) -> RgbImage { 31 | let (nb_rows, nb_cols) = mat.shape(); 32 | let mut img_buf = RgbImage::new(nb_cols as u32, nb_rows as u32); 33 | for (x, y, pixel) in img_buf.enumerate_pixels_mut() { 34 | let (r, g, b) = mat[(y as usize, x as usize)]; 35 | *pixel = Rgb([r, g, b]); 36 | } 37 | img_buf 38 | } 39 | 40 | /// Create a gray image with a borrowed reference to the matrix buffer. 41 | /// 42 | /// Very performant since no copy is performed, 43 | /// but produces a transposed image due to differences in row/column major. 44 | #[allow(clippy::cast_possible_truncation)] 45 | pub fn image_from_matrix_transposed(mat: &DMatrix) -> ImageBuffer, &[u8]> { 46 | let (nb_rows, nb_cols) = mat.shape(); 47 | ImageBuffer::from_raw(nb_rows as u32, nb_cols as u32, mat.as_slice()) 48 | .expect("Buffer not large enough") 49 | } 50 | 51 | /// Convert a `GrayImage` into an `u8` matrix. 52 | /// Inverse operation of `image_from_matrix`. 53 | pub fn matrix_from_image(img: GrayImage) -> DMatrix { 54 | let (width, height) = img.dimensions(); 55 | DMatrix::from_row_slice(height as usize, width as usize, &img.into_raw()) 56 | } 57 | -------------------------------------------------------------------------------- /src/misc/mod.rs: -------------------------------------------------------------------------------- 1 | // This Source Code Form is subject to the terms of the Mozilla Public 2 | // License, v. 2.0. If a copy of the MPL was not distributed with this 3 | // file, You can obtain one at http://mozilla.org/MPL/2.0/. 4 | 5 | //! Miscellaneous modules that didn't fit elsewhere. 6 | 7 | pub mod colormap; 8 | pub mod helper; 9 | pub mod interop; 10 | pub mod type_aliases; 11 | pub mod view; 12 | -------------------------------------------------------------------------------- /src/misc/type_aliases.rs: -------------------------------------------------------------------------------- 1 | // This Source Code Form is subject to the terms of the Mozilla Public 2 | // License, v. 2.0. If a copy of the MPL was not distributed with this 3 | // file, You can obtain one at http://mozilla.org/MPL/2.0/. 4 | 5 | //! Type aliases for common types used all over the code base. 6 | 7 | use nalgebra as na; 8 | 9 | /// At the moment, the library is focused on f32 computation. 10 | pub type Float = f32; 11 | 12 | /// A point with two Float coordinates. 13 | pub type Point2 = na::Point2; 14 | /// A point with three Float coordinates. 15 | pub type Point3 = na::Point3; 16 | 17 | /// A vector with three Float coordinates. 18 | pub type Vec3 = na::Vector3; 19 | /// A vector with six Float coordinates. 20 | pub type Vec6 = na::Vector6; 21 | 22 | /// A 3x3 matrix of Floats. 23 | pub type Mat3 = na::Matrix3; 24 | /// A 4x4 matrix of Floats. 25 | pub type Mat4 = na::Matrix4; 26 | /// A 6x6 matrix of Floats. 27 | pub type Mat6 = na::Matrix6; 28 | 29 | /// A direct 3D isometry, also known as rigid body motion. 30 | pub type Iso3 = na::Isometry3; 31 | -------------------------------------------------------------------------------- /src/misc/view.rs: -------------------------------------------------------------------------------- 1 | // This Source Code Form is subject to the terms of the Mozilla Public 2 | // License, v. 2.0. If a copy of the MPL was not distributed with this 3 | // file, You can obtain one at http://mozilla.org/MPL/2.0/. 4 | 5 | //! Helper functions to visualize images. 6 | 7 | use image::RgbImage; 8 | use nalgebra::DMatrix; 9 | 10 | use crate::core::inverse_depth::InverseDepth; 11 | use crate::misc::type_aliases::Float; 12 | use crate::misc::{colormap, interop}; 13 | 14 | /// Creates an RGB image containing the gray image 15 | /// and candidates points overimposed in red. 16 | pub fn candidates_on_image(img: &DMatrix, candidates: &DMatrix) -> RgbImage { 17 | let rgb_mat = img.zip_map(candidates, |i, a| fuse_img_with_color(i, (255, 0, 0), a)); 18 | interop::rgb_from_matrix(&rgb_mat) 19 | } 20 | 21 | fn fuse_img_with_color(intensity: u8, color: (u8, u8, u8), apply: bool) -> (u8, u8, u8) { 22 | if apply { 23 | color 24 | } else { 25 | (intensity, intensity, intensity) 26 | } 27 | } 28 | 29 | /// Create an RGB image of an inverse depth map. 30 | /// Uses `idepth_enum_colormap` for the color choices. 31 | pub fn idepth_image(idepth_map: &DMatrix) -> RgbImage { 32 | let viridis = &colormap::viridis_u8()[0..256]; 33 | let (d_min, d_max) = min_max(idepth_map).unwrap(); 34 | interop::rgb_from_matrix( 35 | &idepth_map.map(|idepth| idepth_enum_colormap(viridis, d_min, d_max, &idepth)), 36 | ) 37 | } 38 | 39 | /// Find the minimum and maximum values in an inverse depth matrix. 40 | fn min_max(idepth_map: &DMatrix) -> Option<(Float, Float)> { 41 | let mut min_temp: Option = None; 42 | let mut max_temp: Option = None; 43 | idepth_map.iter().for_each(|idepth| { 44 | if let InverseDepth::WithVariance(id, _) = *idepth { 45 | min_temp = min_temp.map(|x| x.min(id)).or_else(|| Some(id)); 46 | max_temp = max_temp.map(|x| x.max(id)).or_else(|| Some(id)); 47 | } 48 | }); 49 | if let (Some(min_value), Some(max_value)) = (min_temp, max_temp) { 50 | Some((min_value, max_value)) 51 | } else { 52 | None 53 | } 54 | } 55 | 56 | // INVERSE DEPTH HELPERS ############################################# 57 | 58 | /// Visualize the enum as an 8-bits intensity: 59 | /// - `Unknown`: black 60 | /// - `Discarded`: gray 61 | /// - `WithVariance`: white 62 | pub fn idepth_enum(idepth: &InverseDepth) -> u8 { 63 | match idepth { 64 | InverseDepth::Unknown => 0_u8, 65 | InverseDepth::Discarded => 50_u8, 66 | InverseDepth::WithVariance(_, _) => 255_u8, 67 | } 68 | } 69 | 70 | /// Visualize the enum with color depending on inverse depth: 71 | /// - `Unknown`: black 72 | /// - `Discarded`: red 73 | /// - `WithVariance`: viridis colormap 74 | #[allow(clippy::cast_sign_loss)] 75 | #[allow(clippy::cast_possible_truncation)] 76 | pub fn idepth_enum_colormap( 77 | colormap: &[(u8, u8, u8)], 78 | d_min: Float, 79 | d_max: Float, 80 | idepth: &InverseDepth, 81 | ) -> (u8, u8, u8) { 82 | match idepth { 83 | InverseDepth::Unknown => (0, 0, 0), 84 | InverseDepth::Discarded => (255, 0, 0), 85 | InverseDepth::WithVariance(d, _) => { 86 | let idx = (255.0 * (d - d_min) / (d_max - d_min)).round() as usize; 87 | colormap[idx] 88 | } 89 | } 90 | } 91 | --------------------------------------------------------------------------------