├── CMakeLists.txt ├── GPL_License.txt ├── README.md ├── demo ├── config_img_dir_example.ini ├── config_imgdir_kitti_srba.ini └── voc.yml.gz └── src ├── CBoWManager.h ├── CMakeLists.txt ├── CSRBAStereoSLAMEstimator.cpp ├── CSRBAStereoSLAMEstimator.h ├── CStereoSLAMKF.cpp ├── CStereoSLAMKF.h ├── srba-stereo-slam.h ├── srba-stereo-slam_common.h ├── srba-stereo-slam_main.cpp ├── srba-stereo-slam_utils.cpp └── srba-stereo-slam_utils.h /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | PROJECT(srba_stereo_slam) 2 | CMAKE_MINIMUM_REQUIRED(VERSION 2.4.5) 3 | if(COMMAND cmake_policy) 4 | cmake_policy(SET CMP0003 NEW) # Required by CMake 2.7+ 5 | endif(COMMAND cmake_policy) 6 | 7 | # Sources: 8 | add_subdirectory(src) 9 | 10 | -------------------------------------------------------------------------------- /GPL_License.txt: -------------------------------------------------------------------------------- 1 | GNU GENERAL PUBLIC LICENSE 2 | Version 3, 29 June 2007 3 | 4 | Copyright (C) 2007 Free Software Foundation, Inc. 5 | Everyone is permitted to copy and distribute verbatim copies 6 | of this license document, but changing it is not allowed. 7 | 8 | Preamble 9 | 10 | The GNU General Public License is a free, copyleft license for 11 | software and other kinds of works. 12 | 13 | The licenses for most software and other practical works are designed 14 | to take away your freedom to share and change the works. By contrast, 15 | the GNU General Public License is intended to guarantee your freedom to 16 | share and change all versions of a program--to make sure it remains free 17 | software for all its users. We, the Free Software Foundation, use the 18 | GNU General Public License for most of our software; it applies also to 19 | any other work released this way by its authors. You can apply it to 20 | your programs, too. 21 | 22 | When we speak of free software, we are referring to freedom, not 23 | price. Our General Public Licenses are designed to make sure that you 24 | have the freedom to distribute copies of free software (and charge for 25 | them if you wish), that you receive source code or can get it if you 26 | want it, that you can change the software or use pieces of it in new 27 | free programs, and that you know you can do these things. 28 | 29 | To protect your rights, we need to prevent others from denying you 30 | these rights or asking you to surrender the rights. Therefore, you have 31 | certain responsibilities if you distribute copies of the software, or if 32 | you modify it: responsibilities to respect the freedom of others. 33 | 34 | For example, if you distribute copies of such a program, whether 35 | gratis or for a fee, you must pass on to the recipients the same 36 | freedoms that you received. You must make sure that they, too, receive 37 | or can get the source code. And you must show them these terms so they 38 | know their rights. 39 | 40 | Developers that use the GNU GPL protect your rights with two steps: 41 | (1) assert copyright on the software, and (2) offer you this License 42 | giving you legal permission to copy, distribute and/or modify it. 43 | 44 | For the developers' and authors' protection, the GPL clearly explains 45 | that there is no warranty for this free software. For both users' and 46 | authors' sake, the GPL requires that modified versions be marked as 47 | changed, so that their problems will not be attributed erroneously to 48 | authors of previous versions. 49 | 50 | Some devices are designed to deny users access to install or run 51 | modified versions of the software inside them, although the manufacturer 52 | can do so. This is fundamentally incompatible with the aim of 53 | protecting users' freedom to change the software. The systematic 54 | pattern of such abuse occurs in the area of products for individuals to 55 | use, which is precisely where it is most unacceptable. Therefore, we 56 | have designed this version of the GPL to prohibit the practice for those 57 | products. If such problems arise substantially in other domains, we 58 | stand ready to extend this provision to those domains in future versions 59 | of the GPL, as needed to protect the freedom of users. 60 | 61 | Finally, every program is threatened constantly by software patents. 62 | States should not allow patents to restrict development and use of 63 | software on general-purpose computers, but in those that do, we wish to 64 | avoid the special danger that patents applied to a free program could 65 | make it effectively proprietary. To prevent this, the GPL assures that 66 | patents cannot be used to render the program non-free. 67 | 68 | The precise terms and conditions for copying, distribution and 69 | modification follow. 70 | 71 | TERMS AND CONDITIONS 72 | 73 | 0. Definitions. 74 | 75 | "This License" refers to version 3 of the GNU General Public License. 76 | 77 | "Copyright" also means copyright-like laws that apply to other kinds of 78 | works, such as semiconductor masks. 79 | 80 | "The Program" refers to any copyrightable work licensed under this 81 | License. Each licensee is addressed as "you". "Licensees" and 82 | "recipients" may be individuals or organizations. 83 | 84 | To "modify" a work means to copy from or adapt all or part of the work 85 | in a fashion requiring copyright permission, other than the making of an 86 | exact copy. The resulting work is called a "modified version" of the 87 | earlier work or a work "based on" the earlier work. 88 | 89 | A "covered work" means either the unmodified Program or a work based 90 | on the Program. 91 | 92 | To "propagate" a work means to do anything with it that, without 93 | permission, would make you directly or secondarily liable for 94 | infringement under applicable copyright law, except executing it on a 95 | computer or modifying a private copy. Propagation includes copying, 96 | distribution (with or without modification), making available to the 97 | public, and in some countries other activities as well. 98 | 99 | To "convey" a work means any kind of propagation that enables other 100 | parties to make or receive copies. Mere interaction with a user through 101 | a computer network, with no transfer of a copy, is not conveying. 102 | 103 | An interactive user interface displays "Appropriate Legal Notices" 104 | to the extent that it includes a convenient and prominently visible 105 | feature that (1) displays an appropriate copyright notice, and (2) 106 | tells the user that there is no warranty for the work (except to the 107 | extent that warranties are provided), that licensees may convey the 108 | work under this License, and how to view a copy of this License. If 109 | the interface presents a list of user commands or options, such as a 110 | menu, a prominent item in the list meets this criterion. 111 | 112 | 1. Source Code. 113 | 114 | The "source code" for a work means the preferred form of the work 115 | for making modifications to it. "Object code" means any non-source 116 | form of a work. 117 | 118 | A "Standard Interface" means an interface that either is an official 119 | standard defined by a recognized standards body, or, in the case of 120 | interfaces specified for a particular programming language, one that 121 | is widely used among developers working in that language. 122 | 123 | The "System Libraries" of an executable work include anything, other 124 | than the work as a whole, that (a) is included in the normal form of 125 | packaging a Major Component, but which is not part of that Major 126 | Component, and (b) serves only to enable use of the work with that 127 | Major Component, or to implement a Standard Interface for which an 128 | implementation is available to the public in source code form. A 129 | "Major Component", in this context, means a major essential component 130 | (kernel, window system, and so on) of the specific operating system 131 | (if any) on which the executable work runs, or a compiler used to 132 | produce the work, or an object code interpreter used to run it. 133 | 134 | The "Corresponding Source" for a work in object code form means all 135 | the source code needed to generate, install, and (for an executable 136 | work) run the object code and to modify the work, including scripts to 137 | control those activities. However, it does not include the work's 138 | System Libraries, or general-purpose tools or generally available free 139 | programs which are used unmodified in performing those activities but 140 | which are not part of the work. For example, Corresponding Source 141 | includes interface definition files associated with source files for 142 | the work, and the source code for shared libraries and dynamically 143 | linked subprograms that the work is specifically designed to require, 144 | such as by intimate data communication or control flow between those 145 | subprograms and other parts of the work. 146 | 147 | The Corresponding Source need not include anything that users 148 | can regenerate automatically from other parts of the Corresponding 149 | Source. 150 | 151 | The Corresponding Source for a work in source code form is that 152 | same work. 153 | 154 | 2. Basic Permissions. 155 | 156 | All rights granted under this License are granted for the term of 157 | copyright on the Program, and are irrevocable provided the stated 158 | conditions are met. This License explicitly affirms your unlimited 159 | permission to run the unmodified Program. The output from running a 160 | covered work is covered by this License only if the output, given its 161 | content, constitutes a covered work. This License acknowledges your 162 | rights of fair use or other equivalent, as provided by copyright law. 163 | 164 | You may make, run and propagate covered works that you do not 165 | convey, without conditions so long as your license otherwise remains 166 | in force. You may convey covered works to others for the sole purpose 167 | of having them make modifications exclusively for you, or provide you 168 | with facilities for running those works, provided that you comply with 169 | the terms of this License in conveying all material for which you do 170 | not control copyright. Those thus making or running the covered works 171 | for you must do so exclusively on your behalf, under your direction 172 | and control, on terms that prohibit them from making any copies of 173 | your copyrighted material outside their relationship with you. 174 | 175 | Conveying under any other circumstances is permitted solely under 176 | the conditions stated below. Sublicensing is not allowed; section 10 177 | makes it unnecessary. 178 | 179 | 3. Protecting Users' Legal Rights From Anti-Circumvention Law. 180 | 181 | No covered work shall be deemed part of an effective technological 182 | measure under any applicable law fulfilling obligations under article 183 | 11 of the WIPO copyright treaty adopted on 20 December 1996, or 184 | similar laws prohibiting or restricting circumvention of such 185 | measures. 186 | 187 | When you convey a covered work, you waive any legal power to forbid 188 | circumvention of technological measures to the extent such circumvention 189 | is effected by exercising rights under this License with respect to 190 | the covered work, and you disclaim any intention to limit operation or 191 | modification of the work as a means of enforcing, against the work's 192 | users, your or third parties' legal rights to forbid circumvention of 193 | technological measures. 194 | 195 | 4. Conveying Verbatim Copies. 196 | 197 | You may convey verbatim copies of the Program's source code as you 198 | receive it, in any medium, provided that you conspicuously and 199 | appropriately publish on each copy an appropriate copyright notice; 200 | keep intact all notices stating that this License and any 201 | non-permissive terms added in accord with section 7 apply to the code; 202 | keep intact all notices of the absence of any warranty; and give all 203 | recipients a copy of this License along with the Program. 204 | 205 | You may charge any price or no price for each copy that you convey, 206 | and you may offer support or warranty protection for a fee. 207 | 208 | 5. Conveying Modified Source Versions. 209 | 210 | You may convey a work based on the Program, or the modifications to 211 | produce it from the Program, in the form of source code under the 212 | terms of section 4, provided that you also meet all of these conditions: 213 | 214 | a) The work must carry prominent notices stating that you modified 215 | it, and giving a relevant date. 216 | 217 | b) The work must carry prominent notices stating that it is 218 | released under this License and any conditions added under section 219 | 7. This requirement modifies the requirement in section 4 to 220 | "keep intact all notices". 221 | 222 | c) You must license the entire work, as a whole, under this 223 | License to anyone who comes into possession of a copy. This 224 | License will therefore apply, along with any applicable section 7 225 | additional terms, to the whole of the work, and all its parts, 226 | regardless of how they are packaged. This License gives no 227 | permission to license the work in any other way, but it does not 228 | invalidate such permission if you have separately received it. 229 | 230 | d) If the work has interactive user interfaces, each must display 231 | Appropriate Legal Notices; however, if the Program has interactive 232 | interfaces that do not display Appropriate Legal Notices, your 233 | work need not make them do so. 234 | 235 | A compilation of a covered work with other separate and independent 236 | works, which are not by their nature extensions of the covered work, 237 | and which are not combined with it such as to form a larger program, 238 | in or on a volume of a storage or distribution medium, is called an 239 | "aggregate" if the compilation and its resulting copyright are not 240 | used to limit the access or legal rights of the compilation's users 241 | beyond what the individual works permit. Inclusion of a covered work 242 | in an aggregate does not cause this License to apply to the other 243 | parts of the aggregate. 244 | 245 | 6. Conveying Non-Source Forms. 246 | 247 | You may convey a covered work in object code form under the terms 248 | of sections 4 and 5, provided that you also convey the 249 | machine-readable Corresponding Source under the terms of this License, 250 | in one of these ways: 251 | 252 | a) Convey the object code in, or embodied in, a physical product 253 | (including a physical distribution medium), accompanied by the 254 | Corresponding Source fixed on a durable physical medium 255 | customarily used for software interchange. 256 | 257 | b) Convey the object code in, or embodied in, a physical product 258 | (including a physical distribution medium), accompanied by a 259 | written offer, valid for at least three years and valid for as 260 | long as you offer spare parts or customer support for that product 261 | model, to give anyone who possesses the object code either (1) a 262 | copy of the Corresponding Source for all the software in the 263 | product that is covered by this License, on a durable physical 264 | medium customarily used for software interchange, for a price no 265 | more than your reasonable cost of physically performing this 266 | conveying of source, or (2) access to copy the 267 | Corresponding Source from a network server at no charge. 268 | 269 | c) Convey individual copies of the object code with a copy of the 270 | written offer to provide the Corresponding Source. This 271 | alternative is allowed only occasionally and noncommercially, and 272 | only if you received the object code with such an offer, in accord 273 | with subsection 6b. 274 | 275 | d) Convey the object code by offering access from a designated 276 | place (gratis or for a charge), and offer equivalent access to the 277 | Corresponding Source in the same way through the same place at no 278 | further charge. You need not require recipients to copy the 279 | Corresponding Source along with the object code. If the place to 280 | copy the object code is a network server, the Corresponding Source 281 | may be on a different server (operated by you or a third party) 282 | that supports equivalent copying facilities, provided you maintain 283 | clear directions next to the object code saying where to find the 284 | Corresponding Source. Regardless of what server hosts the 285 | Corresponding Source, you remain obligated to ensure that it is 286 | available for as long as needed to satisfy these requirements. 287 | 288 | e) Convey the object code using peer-to-peer transmission, provided 289 | you inform other peers where the object code and Corresponding 290 | Source of the work are being offered to the general public at no 291 | charge under subsection 6d. 292 | 293 | A separable portion of the object code, whose source code is excluded 294 | from the Corresponding Source as a System Library, need not be 295 | included in conveying the object code work. 296 | 297 | A "User Product" is either (1) a "consumer product", which means any 298 | tangible personal property which is normally used for personal, family, 299 | or household purposes, or (2) anything designed or sold for incorporation 300 | into a dwelling. In determining whether a product is a consumer product, 301 | doubtful cases shall be resolved in favor of coverage. For a particular 302 | product received by a particular user, "normally used" refers to a 303 | typical or common use of that class of product, regardless of the status 304 | of the particular user or of the way in which the particular user 305 | actually uses, or expects or is expected to use, the product. A product 306 | is a consumer product regardless of whether the product has substantial 307 | commercial, industrial or non-consumer uses, unless such uses represent 308 | the only significant mode of use of the product. 309 | 310 | "Installation Information" for a User Product means any methods, 311 | procedures, authorization keys, or other information required to install 312 | and execute modified versions of a covered work in that User Product from 313 | a modified version of its Corresponding Source. The information must 314 | suffice to ensure that the continued functioning of the modified object 315 | code is in no case prevented or interfered with solely because 316 | modification has been made. 317 | 318 | If you convey an object code work under this section in, or with, or 319 | specifically for use in, a User Product, and the conveying occurs as 320 | part of a transaction in which the right of possession and use of the 321 | User Product is transferred to the recipient in perpetuity or for a 322 | fixed term (regardless of how the transaction is characterized), the 323 | Corresponding Source conveyed under this section must be accompanied 324 | by the Installation Information. But this requirement does not apply 325 | if neither you nor any third party retains the ability to install 326 | modified object code on the User Product (for example, the work has 327 | been installed in ROM). 328 | 329 | The requirement to provide Installation Information does not include a 330 | requirement to continue to provide support service, warranty, or updates 331 | for a work that has been modified or installed by the recipient, or for 332 | the User Product in which it has been modified or installed. Access to a 333 | network may be denied when the modification itself materially and 334 | adversely affects the operation of the network or violates the rules and 335 | protocols for communication across the network. 336 | 337 | Corresponding Source conveyed, and Installation Information provided, 338 | in accord with this section must be in a format that is publicly 339 | documented (and with an implementation available to the public in 340 | source code form), and must require no special password or key for 341 | unpacking, reading or copying. 342 | 343 | 7. Additional Terms. 344 | 345 | "Additional permissions" are terms that supplement the terms of this 346 | License by making exceptions from one or more of its conditions. 347 | Additional permissions that are applicable to the entire Program shall 348 | be treated as though they were included in this License, to the extent 349 | that they are valid under applicable law. If additional permissions 350 | apply only to part of the Program, that part may be used separately 351 | under those permissions, but the entire Program remains governed by 352 | this License without regard to the additional permissions. 353 | 354 | When you convey a copy of a covered work, you may at your option 355 | remove any additional permissions from that copy, or from any part of 356 | it. (Additional permissions may be written to require their own 357 | removal in certain cases when you modify the work.) You may place 358 | additional permissions on material, added by you to a covered work, 359 | for which you have or can give appropriate copyright permission. 360 | 361 | Notwithstanding any other provision of this License, for material you 362 | add to a covered work, you may (if authorized by the copyright holders of 363 | that material) supplement the terms of this License with terms: 364 | 365 | a) Disclaiming warranty or limiting liability differently from the 366 | terms of sections 15 and 16 of this License; or 367 | 368 | b) Requiring preservation of specified reasonable legal notices or 369 | author attributions in that material or in the Appropriate Legal 370 | Notices displayed by works containing it; or 371 | 372 | c) Prohibiting misrepresentation of the origin of that material, or 373 | requiring that modified versions of such material be marked in 374 | reasonable ways as different from the original version; or 375 | 376 | d) Limiting the use for publicity purposes of names of licensors or 377 | authors of the material; or 378 | 379 | e) Declining to grant rights under trademark law for use of some 380 | trade names, trademarks, or service marks; or 381 | 382 | f) Requiring indemnification of licensors and authors of that 383 | material by anyone who conveys the material (or modified versions of 384 | it) with contractual assumptions of liability to the recipient, for 385 | any liability that these contractual assumptions directly impose on 386 | those licensors and authors. 387 | 388 | All other non-permissive additional terms are considered "further 389 | restrictions" within the meaning of section 10. If the Program as you 390 | received it, or any part of it, contains a notice stating that it is 391 | governed by this License along with a term that is a further 392 | restriction, you may remove that term. If a license document contains 393 | a further restriction but permits relicensing or conveying under this 394 | License, you may add to a covered work material governed by the terms 395 | of that license document, provided that the further restriction does 396 | not survive such relicensing or conveying. 397 | 398 | If you add terms to a covered work in accord with this section, you 399 | must place, in the relevant source files, a statement of the 400 | additional terms that apply to those files, or a notice indicating 401 | where to find the applicable terms. 402 | 403 | Additional terms, permissive or non-permissive, may be stated in the 404 | form of a separately written license, or stated as exceptions; 405 | the above requirements apply either way. 406 | 407 | 8. Termination. 408 | 409 | You may not propagate or modify a covered work except as expressly 410 | provided under this License. Any attempt otherwise to propagate or 411 | modify it is void, and will automatically terminate your rights under 412 | this License (including any patent licenses granted under the third 413 | paragraph of section 11). 414 | 415 | However, if you cease all violation of this License, then your 416 | license from a particular copyright holder is reinstated (a) 417 | provisionally, unless and until the copyright holder explicitly and 418 | finally terminates your license, and (b) permanently, if the copyright 419 | holder fails to notify you of the violation by some reasonable means 420 | prior to 60 days after the cessation. 421 | 422 | Moreover, your license from a particular copyright holder is 423 | reinstated permanently if the copyright holder notifies you of the 424 | violation by some reasonable means, this is the first time you have 425 | received notice of violation of this License (for any work) from that 426 | copyright holder, and you cure the violation prior to 30 days after 427 | your receipt of the notice. 428 | 429 | Termination of your rights under this section does not terminate the 430 | licenses of parties who have received copies or rights from you under 431 | this License. If your rights have been terminated and not permanently 432 | reinstated, you do not qualify to receive new licenses for the same 433 | material under section 10. 434 | 435 | 9. Acceptance Not Required for Having Copies. 436 | 437 | You are not required to accept this License in order to receive or 438 | run a copy of the Program. Ancillary propagation of a covered work 439 | occurring solely as a consequence of using peer-to-peer transmission 440 | to receive a copy likewise does not require acceptance. However, 441 | nothing other than this License grants you permission to propagate or 442 | modify any covered work. These actions infringe copyright if you do 443 | not accept this License. Therefore, by modifying or propagating a 444 | covered work, you indicate your acceptance of this License to do so. 445 | 446 | 10. Automatic Licensing of Downstream Recipients. 447 | 448 | Each time you convey a covered work, the recipient automatically 449 | receives a license from the original licensors, to run, modify and 450 | propagate that work, subject to this License. You are not responsible 451 | for enforcing compliance by third parties with this License. 452 | 453 | An "entity transaction" is a transaction transferring control of an 454 | organization, or substantially all assets of one, or subdividing an 455 | organization, or merging organizations. If propagation of a covered 456 | work results from an entity transaction, each party to that 457 | transaction who receives a copy of the work also receives whatever 458 | licenses to the work the party's predecessor in interest had or could 459 | give under the previous paragraph, plus a right to possession of the 460 | Corresponding Source of the work from the predecessor in interest, if 461 | the predecessor has it or can get it with reasonable efforts. 462 | 463 | You may not impose any further restrictions on the exercise of the 464 | rights granted or affirmed under this License. For example, you may 465 | not impose a license fee, royalty, or other charge for exercise of 466 | rights granted under this License, and you may not initiate litigation 467 | (including a cross-claim or counterclaim in a lawsuit) alleging that 468 | any patent claim is infringed by making, using, selling, offering for 469 | sale, or importing the Program or any portion of it. 470 | 471 | 11. Patents. 472 | 473 | A "contributor" is a copyright holder who authorizes use under this 474 | License of the Program or a work on which the Program is based. The 475 | work thus licensed is called the contributor's "contributor version". 476 | 477 | A contributor's "essential patent claims" are all patent claims 478 | owned or controlled by the contributor, whether already acquired or 479 | hereafter acquired, that would be infringed by some manner, permitted 480 | by this License, of making, using, or selling its contributor version, 481 | but do not include claims that would be infringed only as a 482 | consequence of further modification of the contributor version. For 483 | purposes of this definition, "control" includes the right to grant 484 | patent sublicenses in a manner consistent with the requirements of 485 | this License. 486 | 487 | Each contributor grants you a non-exclusive, worldwide, royalty-free 488 | patent license under the contributor's essential patent claims, to 489 | make, use, sell, offer for sale, import and otherwise run, modify and 490 | propagate the contents of its contributor version. 491 | 492 | In the following three paragraphs, a "patent license" is any express 493 | agreement or commitment, however denominated, not to enforce a patent 494 | (such as an express permission to practice a patent or covenant not to 495 | sue for patent infringement). To "grant" such a patent license to a 496 | party means to make such an agreement or commitment not to enforce a 497 | patent against the party. 498 | 499 | If you convey a covered work, knowingly relying on a patent license, 500 | and the Corresponding Source of the work is not available for anyone 501 | to copy, free of charge and under the terms of this License, through a 502 | publicly available network server or other readily accessible means, 503 | then you must either (1) cause the Corresponding Source to be so 504 | available, or (2) arrange to deprive yourself of the benefit of the 505 | patent license for this particular work, or (3) arrange, in a manner 506 | consistent with the requirements of this License, to extend the patent 507 | license to downstream recipients. "Knowingly relying" means you have 508 | actual knowledge that, but for the patent license, your conveying the 509 | covered work in a country, or your recipient's use of the covered work 510 | in a country, would infringe one or more identifiable patents in that 511 | country that you have reason to believe are valid. 512 | 513 | If, pursuant to or in connection with a single transaction or 514 | arrangement, you convey, or propagate by procuring conveyance of, a 515 | covered work, and grant a patent license to some of the parties 516 | receiving the covered work authorizing them to use, propagate, modify 517 | or convey a specific copy of the covered work, then the patent license 518 | you grant is automatically extended to all recipients of the covered 519 | work and works based on it. 520 | 521 | A patent license is "discriminatory" if it does not include within 522 | the scope of its coverage, prohibits the exercise of, or is 523 | conditioned on the non-exercise of one or more of the rights that are 524 | specifically granted under this License. You may not convey a covered 525 | work if you are a party to an arrangement with a third party that is 526 | in the business of distributing software, under which you make payment 527 | to the third party based on the extent of your activity of conveying 528 | the work, and under which the third party grants, to any of the 529 | parties who would receive the covered work from you, a discriminatory 530 | patent license (a) in connection with copies of the covered work 531 | conveyed by you (or copies made from those copies), or (b) primarily 532 | for and in connection with specific products or compilations that 533 | contain the covered work, unless you entered into that arrangement, 534 | or that patent license was granted, prior to 28 March 2007. 535 | 536 | Nothing in this License shall be construed as excluding or limiting 537 | any implied license or other defenses to infringement that may 538 | otherwise be available to you under applicable patent law. 539 | 540 | 12. No Surrender of Others' Freedom. 541 | 542 | If conditions are imposed on you (whether by court order, agreement or 543 | otherwise) that contradict the conditions of this License, they do not 544 | excuse you from the conditions of this License. If you cannot convey a 545 | covered work so as to satisfy simultaneously your obligations under this 546 | License and any other pertinent obligations, then as a consequence you may 547 | not convey it at all. For example, if you agree to terms that obligate you 548 | to collect a royalty for further conveying from those to whom you convey 549 | the Program, the only way you could satisfy both those terms and this 550 | License would be to refrain entirely from conveying the Program. 551 | 552 | 13. Use with the GNU Affero General Public License. 553 | 554 | Notwithstanding any other provision of this License, you have 555 | permission to link or combine any covered work with a work licensed 556 | under version 3 of the GNU Affero General Public License into a single 557 | combined work, and to convey the resulting work. The terms of this 558 | License will continue to apply to the part which is the covered work, 559 | but the special requirements of the GNU Affero General Public License, 560 | section 13, concerning interaction through a network will apply to the 561 | combination as such. 562 | 563 | 14. Revised Versions of this License. 564 | 565 | The Free Software Foundation may publish revised and/or new versions of 566 | the GNU General Public License from time to time. Such new versions will 567 | be similar in spirit to the present version, but may differ in detail to 568 | address new problems or concerns. 569 | 570 | Each version is given a distinguishing version number. If the 571 | Program specifies that a certain numbered version of the GNU General 572 | Public License "or any later version" applies to it, you have the 573 | option of following the terms and conditions either of that numbered 574 | version or of any later version published by the Free Software 575 | Foundation. If the Program does not specify a version number of the 576 | GNU General Public License, you may choose any version ever published 577 | by the Free Software Foundation. 578 | 579 | If the Program specifies that a proxy can decide which future 580 | versions of the GNU General Public License can be used, that proxy's 581 | public statement of acceptance of a version permanently authorizes you 582 | to choose that version for the Program. 583 | 584 | Later license versions may give you additional or different 585 | permissions. However, no additional obligations are imposed on any 586 | author or copyright holder as a result of your choosing to follow a 587 | later version. 588 | 589 | 15. Disclaimer of Warranty. 590 | 591 | THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY 592 | APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT 593 | HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY 594 | OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, 595 | THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 596 | PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM 597 | IS WITH YOU. SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF 598 | ALL NECESSARY SERVICING, REPAIR OR CORRECTION. 599 | 600 | 16. Limitation of Liability. 601 | 602 | IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING 603 | WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS 604 | THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY 605 | GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE 606 | USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF 607 | DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD 608 | PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS), 609 | EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF 610 | SUCH DAMAGES. 611 | 612 | 17. Interpretation of Sections 15 and 16. 613 | 614 | If the disclaimer of warranty and limitation of liability provided 615 | above cannot be given local legal effect according to their terms, 616 | reviewing courts shall apply local law that most closely approximates 617 | an absolute waiver of all civil liability in connection with the 618 | Program, unless a warranty or assumption of liability accompanies a 619 | copy of the Program in return for a fee. 620 | 621 | END OF TERMS AND CONDITIONS 622 | 623 | How to Apply These Terms to Your New Programs 624 | 625 | If you develop a new program, and you want it to be of the greatest 626 | possible use to the public, the best way to achieve this is to make it 627 | free software which everyone can redistribute and change under these terms. 628 | 629 | To do so, attach the following notices to the program. It is safest 630 | to attach them to the start of each source file to most effectively 631 | state the exclusion of warranty; and each file should have at least 632 | the "copyright" line and a pointer to where the full notice is found. 633 | 634 | 635 | Copyright (C) 636 | 637 | This program is free software: you can redistribute it and/or modify 638 | it under the terms of the GNU General Public License as published by 639 | the Free Software Foundation, either version 3 of the License, or 640 | (at your option) any later version. 641 | 642 | This program is distributed in the hope that it will be useful, 643 | but WITHOUT ANY WARRANTY; without even the implied warranty of 644 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 645 | GNU General Public License for more details. 646 | 647 | You should have received a copy of the GNU General Public License 648 | along with this program. If not, see . 649 | 650 | Also add information on how to contact you by electronic and paper mail. 651 | 652 | If the program does terminal interaction, make it output a short 653 | notice like this when it starts in an interactive mode: 654 | 655 | Copyright (C) 656 | This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'. 657 | This is free software, and you are welcome to redistribute it 658 | under certain conditions; type `show c' for details. 659 | 660 | The hypothetical commands `show w' and `show c' should show the appropriate 661 | parts of the General Public License. Of course, your program's commands 662 | might be different; for a GUI interface, you would use an "about box". 663 | 664 | You should also get your employer (if you work as a programmer) or school, 665 | if any, to sign a "copyright disclaimer" for the program, if necessary. 666 | For more information on this, and how to apply and follow the GNU GPL, see 667 | . 668 | 669 | The GNU General Public License does not permit incorporating your program 670 | into proprietary programs. If your program is a subroutine library, you 671 | may consider it more useful to permit linking proprietary applications with 672 | the library. If this is what you want to do, use the GNU Lesser General 673 | Public License instead of this License. But first, please read 674 | . 675 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # SRBA-Stereo-SLAM 2 | **Note:** *Preliminary version* 3 | 4 | Library that performs stereo visual SLAM within a Sparser Relative Bundle Adjustment (SRBA) framework. 5 | More info about SRBA can be found [here](http://www.mrpt.org/srba) 6 | 7 | ## Building from sources 8 | 9 | ### Prerequisites 10 | 11 | * CMake (>=2.4.5) 12 | * OpenCV (>=2.4.8, >=3.0.0 not supported due to DBoW2 not supporting it) 13 | * [DBoW2](https://github.com/dorian3d/DBoW2) (1.0.2) (Check dependencies for this library) 14 | * [MRPT](https://github.com/MRPT/mrpt) (>=1.3.0) 15 | * [SRBA](https://github.com/MRPT/srba) (Header-only library, must be downloaded from GitHub for now) 16 | * [stereo-vo](https://github.com/famoreno/stereo-vo) 17 | 18 | ### Compiling 19 | 20 | This software can be compiled in Windows and GNU/Linux with `gcc` or `clang`. It should also work on OSX but it is untested. 21 | 22 | 1. Install all prerequisites above. Many of those can be installed in Ubuntu as follows: 23 | 24 | * Only for versions older than Ubuntu Wily (15.10): 25 | 26 | sudo add-apt-repository ppa:joseluisblancoc/mrpt 27 | sudo apt-get update 28 | 29 | * After that, for all Debian / Ubuntu versions: 30 | 31 | sudo apt-get install build-essential cmake libmrpt-dev libopencv-dev 32 | 33 | * Download and build [DBoW2] (http://webdiis.unizar.es/~dorian/index.php?p=32) library following instruction therein. This library needs [boost] (http://www.boost.org) library to run (tested with 1.55). 34 | 35 | * Clone [stereo-vo](https://github.com/famoreno/stereo-vo) and build following instructions therein. Optionally, run `make test` 36 | 37 | * Clone the header-only library [SRBA](https://github.com/MRPT/srba). Configure and generate the project with `cmake` so `SRBAConfig.cmake` is generated and, optionally, run `make test` to ensure everything works ok. 38 | 39 | 2. Create an empty build directory, invoke `cmake` and build as usual. 40 | 41 | mkdir build 42 | cd build 43 | cmake .. -DSRBA_DIR=[PATH_TO_SRBA_BUILD_DIR] 44 | 45 | 46 | 47 | -------------------------------------------------------------------------------- /demo/config_img_dir_example.ini: -------------------------------------------------------------------------------- 1 | ;----------------------------- 2 | ; Visual odometry params 3 | ;----------------------------- 4 | [RECTIFY] 5 | nOctaves = 1 // [int] (def:8) -- Number of octaves (1 for ORB) 6 | 7 | [DETECT] 8 | ; GENERAL 9 | ; detect_method = 0 // [int] (def:0) -- Method to detect keypoints: [0] ORB ; [1] FAST (one-scale) + ORB descriptors ; [2] FASTER ; [3] KLT 10 | min_distance = 5 // [int] (def:3) -- The allowed minimun distance (in pixels) between features 11 | 12 | ; FAST 13 | ; target_feats_per_pixel = 5 // [double] (def:0.001) -- Desired number of features per square pixel [ONLY FOR 'FASTER' METHOD] 14 | ; initial_FAST_threshold = 20 // [int] (def:20) -- Initial FAST threshold (between 10-30 is a good value) [THIS IS CONSTANT TO 20 FOR 'ORB' WITH OPENCV < 3.0] 15 | ; fast_min_th = 5 // [int] (def:5) -- Minimum limit for the FAST (within ORB) detector (dynamic) threshold [UNUSED WITH OPENCV<3.0] 16 | ; fast_max_th = 30 // [int] (def:30) -- Maximum limit for the FAST (within ORB) detector (dynamic) threshold [UNUSED WITH OPENCV<3.0] 17 | 18 | ; KLT 19 | ; KLT_win = 4 // [int] (def:4) -- Window for the KLT response evaluation 20 | ; minimum_KLT_response = 10 // [int] (def:10) -- Minimum KLT response to not to discard a feature as being in a textureless zone 21 | 22 | ; ORB 23 | ; orb_nfeats = 1000 // [int] (def:500) -- Number of ORB features to be detected 24 | ; orb_nlevels = 1 // [int] (def:8) -- The number of pyramid levels for the ORB detector 25 | ; minimum_ORB_response = 0 // [double] (def:0) -- Minimum ORB response [Harris response] to not to discard a feature as being in a textureless zone 26 | 27 | ; NON-MAX-SUP 28 | non_maximal_suppression = true // [bool] (def:true) -- Enable/disable the non-maximal suppression after the detection (5x5 windows is used) 29 | non_max_supp_method = 1 // [int] (def:0) -- Method to perform non maximal suppression: [0] Standard ; [1] Adaptive 30 | 31 | [MATCH] 32 | ; GENERAL 33 | ; match_method = 0 // [int] (def:2) -- Method to perform stereo matching: [0] DescBF ; [1] DescRbR ; [2] SAD 34 | max_y_diff = 2 // [int] (def:0) -- Maximum allowed distance in pixels from the same row in the images for corresponding feats 35 | enable_robust_1to1_match = false // [bool] (def:false) -- Only match if a pair of L/R features have the best match score to each other 36 | rectified_images = true // [bool] (def:false) -- Stereo images are rectified (corresponding pairs are in the same row) 37 | ; min_z = 60 // [double] (def:5) -- Min value for the Z coordinate of 3D feature to be considered (reject too close features) [UNUSED] 38 | ; max_z = 0.15 // [double] (def:0.3) -- Max value for the Z coordinate of 3D feature to be considered (reject too far features) [UNUSED] 39 | 40 | ; SAD LIMITS 41 | sad_max_distance = 2000 // [int] (def:200) -- The maximum SAD value to consider a pairing as a potential match 42 | ; sad_max_ratio = 0.5 // [double] (def:0.5) -- The maximum ratio between the two smallest SAD when searching for pairings 43 | 44 | ; ORB LIMITS 45 | ; orb_min_th = 30 // [int] (def:30) -- Minimum limit for the ORB matching threshold (dynamic ORB matching limits) 46 | ; orb_max_th = 80 // [int] (def:100) -- Maximum limit for the ORB matching threshold (dynamic ORB matching limits) 47 | orb_max_distance = 60 // [int] (def:40) -- Maximum allowed Hamming distance between a pair of features to be considered a match 48 | 49 | [IF-MATCH] 50 | ; GENERAL 51 | ; if_match_method = 0 // [int] (def:0) -- Inter-frame matching method: [0] DescBF ; [1] DescWin ; [2] SAD ; [3] OpticalFlow 52 | filter_fund_matrix = false // [bool] (def:) -- Whether or not use fundamental matrix to remove outliers between inter-frame matches 53 | 54 | ; WINDOW LIMITS 55 | window_height = 16 // [int] (def:) -- Window size for searching for inter-frame matches 56 | window_width = 16 // [int] (def:) -- Window size for searching for inter-frame matches 57 | 58 | ; SAD LIMITS 59 | sad_max_distance = 400 60 | sad_max_ratio = 0.5 // [double] (def:0.5) -- The maximum ratio between the two smallest SAD when searching for pairings 61 | 62 | ; ORB LIMITS -- the same values as for stereo-matching are employed 63 | 64 | [LEAST-SQUARES] 65 | ; std_noise_pixels = 1 // [double] (def:1) -- The standard deviation assumed for feature coordinates 66 | 67 | initial_max_iters = 30 // [int] (def:10) -- Maximum number of iterations for the initial stage 68 | max_iters = 30 // [int] (def:100) -- Final maximum number of iterations for the refinement stage 69 | max_incr_cost = 3 // [int] (def:3) -- Maximum allowed number of times the cost can grow 70 | residual_threshold = 15 // [double] (def:1) -- Residual threshold for detecting outliers 71 | min_mod_out_vector = 0.001 // [double] (def:0.001) -- Minimum modulus of the step output vector to continue iterating (ending condition) 72 | 73 | bad_tracking_th = 5 // [int] (def:) -- Minimum number of tracked features to yield a tracking error 74 | 75 | use_robust_kernel = true // [bool] (def:true) -- Set/Unset using robust kernel for optimization 76 | kernel_param = 2 // [double] (def:3) -- Robust kernel parameter (pseudo-Huber) 77 | 78 | da_stage2_method = 3 79 | 80 | use_previous_pose_as_initial = true // [bool] (def:true) -- Use the previous computed pose as the initial one for the next frame 81 | 82 | [GUI] 83 | show_gui = false // [bool] (def:true) Show GUI? 84 | draw_all_raw_feats = true // [bool] (def:false) Draw raw keypoints 85 | draw_lr_pairings = true // [bool] (def:false) Draw stereo matches 86 | draw_tracking = false // [bool] (def:true) Draw tracking information 87 | 88 | [GENERAL] 89 | vo_use_matches_ids = true // [bool] (def:false) Set/Unset tracking of the IDs of the matches through time 90 | vo_save_files = false // [bool] (def:false) Set/Unset storage of some information of the system in files as the process runs 91 | vo_debug = false // [bool] (def:false) Set/Unset showing application debugging info 92 | vo_pause_it = false // [bool] (def:false) Set/Unset pausing the application after each iteration 93 | vo_out_dir = out_vo_dir // [string] (def:'out') Sets the output directory for saving debug files 94 | ; vo_verbosity = 0 95 | 96 | camera_pose_on_robot= [0,0,0,0,0,0] // [vector] (def:all_zeros) Sets the pose of the camera on the robot 97 | 98 | ;----------------------------- 99 | ; Application options 100 | ;----------------------------- 101 | [APP_OPTIONS] 102 | out_dir = out_app // [string] (def:'') -- Application output folder 103 | debug = false // [bool] (def:false) -- Store and show some debugging information 104 | show3D = true // [bool] (def:false) -- Show information GUI 105 | enable_logger = false // [bool] (def:false) -- Enable time logger for certain operations (for debugging) 106 | verbose_level = 1 // [int] (def:0) -- Verbose level: [0] None ; [1] Important ; [2] More info 107 | 108 | pause_at_each_iteration = false // [bool] (def:false) -- Pause application after each iteration 109 | pause_after_show_op = true // [bool] (def:false) -- Pause application after showing parameters 110 | 111 | max_num_kfs = 500 // [int] (def:0 -unlimited-) -- Maximum number of KFs to be inserted in the system (app will finish when reached) 112 | 113 | ; capture_source = 1 // [int] (def:1) -- Image capture source: [0] Rawlog file (TO DO); [1] Image folder [NOTE: FILL 'IMG_SOURCE' SECTION] 114 | 115 | ; from_step = 0 // [int] (def:0) -- Number of the first frame to process 116 | ; to_step = 0 // [int] (def:0 -unlimited-) -- Number of the last frame to process 117 | 118 | ;----------------------------- 119 | ; Image source parameters 120 | ;----------------------------- 121 | [IMG_SOURCE] 122 | image_dir_url = dataset0 // [string] (def:'') -- Image folder path 123 | left_format = image_0\%06d.png // [string] (def:'') -- C-style format for the 'left image' filenames 124 | right_format = image_1\%06d.png // [string] (def:'') -- C-style format for the 'left image' filenames 125 | start_index = 0 // [int] (def:0) -- Starting frame index to be processed 126 | end_index = 1100 // [int] (def:0 -unlimited-) -- Last frame index to be processed [USE THIS INSTEAD OF 'from_step' and 'to_step' FOR IMAGE FOLDERS] 127 | 128 | ;----------------------------- 129 | ; Sparser Relative Bundle Adjustment 130 | ;----------------------------- 131 | [SRBA_GENERAL] 132 | voc_filename = voc.yml.gz // [string] (def:'') -- ORB data vocabulary filename 133 | pause_after_show_op = true // [bool] (def:false) -- Pause after showing application options 134 | 135 | srba_max_tree_depth = 4 // [int] (def:3) -- Maximum depth to keep spanning trees 136 | srba_max_optimize_depth = 5 // [int] (def:3) -- Maximum depth to optimize the graph 137 | srba_submap_size = 10 // [int] (def:15) -- Number of KFs within submaps 138 | srba_use_robust_kernel = true // [bool] (def:true) -- Use robust kernel for optimization 139 | srba_use_robust_kernel_stage1 = true //[bool] (def:true) -- Use robust kernel for optimization in SRBA stage 1 140 | srba_kernel_param = 1.5 // [double] (def:3.0) -- Pseudo-huber kernel param for least-squares optimization. 141 | 142 | [SRBA_DETECT] 143 | n_feats = 500 // [int] (def:500) -- Desired number of feats to be detected in the images (NOTE: will overwrite the visual odometer equivalent parameter) 144 | orb_adaptive_fast_th = true // [bool] (def:false) -- Set adaptive FAST threshold (within ORB method) according to the number of detected keypoints [NOT USED WITH OPENCV<3.0] 145 | detect_fast_th = 20 // [int] (def:5) -- Initial FAST Threshold for ORB keypoints (will be adaptad if 'orb_adaptive_fast_th' is true) 146 | adaptive_th_min_matches = 100 // [int] (def:100) -- Minimum number of matches to adapt FAST threshold for ORB [NOT USED WITH OPENCV<3.0] 147 | 148 | [SRBA_DATA_ASSOCIATION] 149 | da_stage2_method = 3 // [int] (def:2) -- Method for filtering outliers during second stage of DA: [0] None ; [1] Fundamental Matrix; [2] Change in pose only; [3] Both 150 | residual_th = 50 // [double] (def:) -- Filtering by change in pose (residual threshold) 151 | max_orb_distance_da = 60 // [double] (def:60) -- Maximum ORB distance for data association 152 | max_y_diff_epipolar = 2.0 // [double] (def:1.5) -- Filtering by fundamental matrix (epipolar threshold) 153 | ransac_fit_prob = 0.95 // [double] (def:0.95) -- Filtering by fundamental matrix (RANSAC fit threshold) 154 | 155 | [SRBA_KF_CREATION] 156 | max_rotation = 25 // [double] (def:15) -- Rotation limit for checking new KFs (in degrees) 157 | max_translation = 2.0 // [double] (def:0.3) -- Translation limit for checking new KFs (in meters) 158 | updated_matches_th = 60 // [int] (def:50) -- Minimum number of tracked matches to insert a new KF after DA 159 | up_matches_th_plus = 25 // [int] (def:25) -- This+'updated_matches_th' sets the minimum number of tracked matches to define new KF checking (geometrical) limits 160 | lc_distance = 10 // [int] (def:2) -- Minimum distance between KFs to consider a loop closure 161 | vo_id_tracking_th = 40 // [int] (def:40) -- Minimum number of tracked matches (from VO) to perform a new KF check 162 | use_initial_pose = true // [bool] (def:true) -- Use an initial estimation of the position of this KF taken from the odometry 163 | 164 | ;----------------------------- 165 | ; Camera parameters 166 | ;----------------------------- 167 | [CAMERA_LEFT] 168 | resolution = [1226 370] 169 | cx = 601.8873000000 170 | cy = 183.1104000000 171 | fx = 707.0912000000 172 | fy = 707.0912000000 173 | dist = [0.000000e+000 0.000000e+000 0.000000e+000 0.000000e+000 0.000000e+000] 174 | 175 | [CAMERA_RIGHT] 176 | resolution = [1226 370] 177 | cx = 601.8873000000 178 | cy = 183.1104000000 179 | fx = 707.0912000000 180 | fy = 707.0912000000 181 | dist = [0.000000e+000 0.000000e+000 0.000000e+000 0.000000e+000 0.000000e+000] 182 | 183 | [CAMERA_LEFT2RIGHT_POSE] 184 | pose_quaternion = [0.54000000 0.000000 0.000000 1.000000 0.000000 0.000000 0.000000] -------------------------------------------------------------------------------- /demo/config_imgdir_kitti_srba.ini: -------------------------------------------------------------------------------- 1 | ;----------------------------- 2 | ; Visual odometry params 3 | ;----------------------------- 4 | [RECTIFY] 5 | nOctaves = 1 // [int] (def:8) -- Number of octaves (1 for ORB) 6 | 7 | [DETECT] 8 | ; GENERAL 9 | min_distance = 5 // [int] (def:3) -- The allowed minimun distance (in pixels) between features 10 | 11 | ; NON-MAX-SUP 12 | non_maximal_suppression = true // [bool] (def:true) -- Enable/disable the non-maximal suppression after the detection (5x5 windows is used) 13 | non_max_supp_method = 1 // [int] (def:0) -- Method to perform non maximal suppression: [0] Standard ; [1] Adaptive 14 | 15 | [MATCH] 16 | ; GENERAL 17 | max_y_diff = 2 // [int] (def:0) -- Maximum allowed distance in pixels from the same row in the images for corresponding feats 18 | enable_robust_1to1_match = false // [bool] (def:false) -- Only match if a pair of L/R features have the best match score to each other 19 | rectified_images = true // [bool] (def:false) -- Stereo images are rectified (corresponding pairs are in the same row) 20 | 21 | ; SAD LIMITS 22 | sad_max_distance = 2000 // [int] (def:200) -- The maximum SAD value to consider a pairing as a potential match 23 | 24 | orb_max_distance = 60 // [int] (def:40) -- Maximum allowed Hamming distance between a pair of features to be considered a match 25 | 26 | [IF-MATCH] 27 | ; GENERAL 28 | filter_fund_matrix = false // [bool] (def:) -- Whether or not use fundamental matrix to remove outliers between inter-frame matches 29 | 30 | ; WINDOW LIMITS 31 | window_height = 16 // [int] (def:) -- Window size for searching for inter-frame matches 32 | window_width = 16 // [int] (def:) -- Window size for searching for inter-frame matches 33 | 34 | ; SAD LIMITS 35 | sad_max_distance = 400 36 | sad_max_ratio = 0.5 // [double] (def:0.5) -- The maximum ratio between the two smallest SAD when searching for pairings 37 | 38 | ; ORB LIMITS -- the same values as for stereo-matching are employed 39 | 40 | [LEAST_SQUARES] 41 | initial_max_iters = 30 // [int] (def:10) -- Maximum number of iterations for the initial stage 42 | max_iters = 30 // [int] (def:100) -- Final maximum number of iterations for the refinement stage 43 | max_incr_cost = 3 // [int] (def:3) -- Maximum allowed number of times the cost can grow 44 | residual_threshold = 15 // [double] (def:1) -- Residual threshold for detecting outliers 45 | min_mod_out_vector = 0.001 // [double] (def:0.001) -- Minimum modulus of the step output vector to continue iterating (ending condition) 46 | 47 | bad_tracking_th = 5 // [int] (def:) -- Minimum number of tracked features to yield a tracking error 48 | 49 | use_robust_kernel = true // [bool] (def:true) -- Set/Unset using robust kernel for optimization 50 | kernel_param = 2 // [double] (def:3) -- Robust kernel parameter (pseudo-Huber) 51 | 52 | da_stage2_method = 3 53 | 54 | use_previous_pose_as_initial = true // [bool] (def:true) -- Use the previous computed pose as the initial one for the next frame 55 | 56 | [GUI] 57 | show_gui = false // [bool] (def:true) Show GUI? 58 | draw_all_raw_feats = true // [bool] (def:false) Draw raw keypoints 59 | draw_lr_pairings = true // [bool] (def:false) Draw stereo matches 60 | draw_tracking = false // [bool] (def:true) Draw tracking information 61 | 62 | [GENERAL] 63 | vo_use_matches_ids = true // [bool] (def:false) Set/Unset tracking of the IDs of the matches through time 64 | vo_save_files = false // [bool] (def:false) Set/Unset storage of some information of the system in files as the process runs 65 | vo_debug = false // [bool] (def:false) Set/Unset showing application debugging info 66 | vo_pause_it = false // [bool] (def:false) Set/Unset pausing the application after each iteration 67 | vo_out_dir = out_vo_dir // [string] (def:'out') Sets the output directory for saving debug files 68 | 69 | camera_pose_on_robot= [0,0,0,0,0,0] // [vector] (def:all_zeros) Sets the pose of the camera on the robot 70 | 71 | ;----------------------------- 72 | ; Application options 73 | ;----------------------------- 74 | [APP_OPTIONS] 75 | out_dir = out_app3 // [string] (def:'') -- Application output folder 76 | debug = true // [bool] (def:false) -- Store and show some debugging information 77 | show3D = true // [bool] (def:false) -- Show information GUI 78 | enable_logger = false // [bool] (def:false) -- Enable time logger for certain operations (for debugging) 79 | verbose_level = 2 // [int] (def:0) -- Verbose level: [0] None ; [1] Important ; [2] More info 80 | 81 | pause_at_each_iteration = false // [bool] (def:false) -- Pause application after each iteration 82 | pause_after_show_op = false // [bool] (def:false) -- Pause application after showing parameters 83 | 84 | max_num_kfs = 500 // [int] (def:0 -unlimited-) -- Maximum number of KFs to be inserted in the system (app will finish when reached) 85 | 86 | ;----------------------------- 87 | ; Image source parameters 88 | ;----------------------------- 89 | [IMG_SOURCE] 90 | grabber_type = image_dir 91 | image_dir_url = dataset0 // [string] (def:'') -- Image folder path 92 | left_format = image_0\%06d.png // [string] (def:'') -- C-style format for the 'left image' filenames 93 | right_format = image_1\%06d.png // [string] (def:'') -- C-style format for the 'left image' filenames 94 | start_index = 0 // [int] (def:0) -- Starting frame index to be processed 95 | end_index = 1100 // [int] (def:0 -unlimited-) -- Last frame index to be processed [USE THIS INSTEAD OF 'from_step' and 'to_step' FOR IMAGE FOLDERS] 96 | 97 | ;----------------------------- 98 | ; Sparser Relative Bundle Adjustment 99 | ;----------------------------- 100 | [SRBA_GENERAL] 101 | voc_filename = voc.yml.gz // [string] (def:'') -- ORB data vocabulary filename 102 | pause_after_show_op = false // [bool] (def:false) -- Pause after showing application options 103 | 104 | srba_max_tree_depth = 4 // [int] (def:3) -- Maximum depth to keep spanning trees 105 | srba_max_optimize_depth = 5 // [int] (def:3) -- Maximum depth to optimize the graph 106 | srba_submap_size = 10 // [int] (def:15) -- Number of KFs within submaps 107 | srba_use_robust_kernel = true // [bool] (def:true) -- Use robust kernel for optimization 108 | srba_use_robust_kernel_stage1 = true //[bool] (def:true) -- Use robust kernel for optimization in SRBA stage 1 109 | srba_kernel_param = 1.5 // [double] (def:3.0) -- Pseudo-huber kernel param for least-squares optimization. 110 | 111 | [SRBA_DETECT] 112 | n_feats = 500 // [int] (def:500) -- Desired number of feats to be detected in the images (NOTE: will overwrite the visual odometer equivalent parameter) 113 | orb_adaptive_fast_th = true // [bool] (def:false) -- Set adaptive FAST threshold (within ORB method) according to the number of detected keypoints [NOT USED WITH OPENCV<3.0] 114 | detect_fast_th = 20 // [int] (def:5) -- Initial FAST Threshold for ORB keypoints (will be adaptad if 'orb_adaptive_fast_th' is true) 115 | adaptive_th_min_matches = 100 // [int] (def:100) -- Minimum number of matches to adapt FAST threshold for ORB [NOT USED WITH OPENCV<3.0] 116 | 117 | [SRBA_DATA_ASSOCIATION] 118 | da_stage2_method = 3 // [int] (def:2) -- Method for filtering outliers during second stage of DA: [0] None ; [1] Fundamental Matrix; [2] Change in pose only; [3] Both 119 | residual_th = 30 //50 // [double] (def:) -- Filtering by change in pose (residual threshold) 120 | max_orb_distance_da = 60 // [double] (def:60) -- Maximum ORB distance for data association 121 | max_y_diff_epipolar = 2.0 // [double] (def:1.5) -- Filtering by fundamental matrix (epipolar threshold) 122 | ransac_fit_prob = 0.95 // [double] (def:0.95) -- Filtering by fundamental matrix (RANSAC fit threshold) 123 | 124 | da_filter_by_direction = true 125 | 126 | [SRBA_KF_CREATION] 127 | max_rotation = 25 // [double] (def:15) -- Rotation limit for checking new KFs (in degrees) 128 | max_translation = 2.0 // [double] (def:0.3) -- Translation limit for checking new KFs (in meters) 129 | updated_matches_th = 60 // [int] (def:50) -- Minimum number of tracked matches to insert a new KF after DA 130 | up_matches_th_plus = 25 // [int] (def:25) -- This+'updated_matches_th' sets the minimum number of tracked matches to define new KF checking (geometrical) limits 131 | lc_distance = 10 // [int] (def:2) -- Minimum distance between KFs to consider a loop closure 132 | vo_id_tracking_th = 40 // [int] (def:40) -- Minimum number of tracked matches (from VO) to perform a new KF check 133 | use_initial_pose = true // [bool] (def:true) -- Use an initial estimation of the position of this KF taken from the odometry 134 | 135 | ;----------------------------- 136 | ; Camera parameters 137 | ;----------------------------- 138 | [CAMERA_LEFT] 139 | resolution = [1226 370] 140 | cx = 601.8873000000 141 | cy = 183.1104000000 142 | fx = 707.0912000000 143 | fy = 707.0912000000 144 | dist = [0.000000e+000 0.000000e+000 0.000000e+000 0.000000e+000 0.000000e+000] 145 | 146 | [CAMERA_RIGHT] 147 | resolution = [1226 370] 148 | cx = 601.8873000000 149 | cy = 183.1104000000 150 | fx = 707.0912000000 151 | fy = 707.0912000000 152 | dist = [0.000000e+000 0.000000e+000 0.000000e+000 0.000000e+000 0.000000e+000] 153 | 154 | [CAMERA_LEFT2RIGHT_POSE] 155 | pose_quaternion = [0.54000000 0.000000 0.000000 1.000000 0.000000 0.000000 0.000000] -------------------------------------------------------------------------------- /demo/voc.yml.gz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/famoreno/srba-stereo-slam/6e9d5df8ed3d927f2a2322847d0c8fe649902022/demo/voc.yml.gz -------------------------------------------------------------------------------- /src/CBoWManager.h: -------------------------------------------------------------------------------- 1 | /********************************************************************************* 2 | ** SRBA-Stereo-SLAM ** 3 | ********************************************************************************** 4 | ** ** 5 | ** Copyright(c) 2015-2017, Jose Luis Blanco, University of Almeria ** 6 | ** Copyright(c) 2015-2017, Francisco-Angel Moreno ** 7 | ** MAPIR group, University of Malaga ** 8 | ** All right reserved 9 | ** ** 10 | ** This program is free software: you can redistribute it and/or modify ** 11 | ** it under the terms of the GNU General Public License (version 3) as ** 12 | ** published by the Free Software Foundation. ** 13 | ** ** 14 | ** This program is distributed in the hope that it will be useful, but ** 15 | ** WITHOUT ANY WARRANTY; without even the implied warranty of ** 16 | ** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the ** 17 | ** GNU General Public License for more details. ** 18 | ** ** 19 | ** You should have received a copy of the GNU General Public License ** 20 | ** along with this program. If not, see . ** 21 | ** ** 22 | **********************************************************************************/ 23 | #pragma once 24 | 25 | // bag of words 26 | #include "DBoW2.h" // defines Surf64Vocabulary and Surf64Database 27 | #include "DUtils.h" 28 | #include "DUtilsCV.h" // defines macros CVXX 29 | #include "DVision.h" 30 | 31 | // stereo slam keyframe class 32 | #include 33 | #include "CStereoSLAMKF.h" 34 | 35 | // namespaces 36 | using namespace DBoW2; 37 | using namespace DUtils; 38 | using namespace DVision; 39 | using namespace mrpt; 40 | 41 | /********************************************* 42 | CLASS: Bag of words manager 43 | **********************************************/ 44 | class CBoWManager 45 | { 46 | // MEMBERS ------------------------------------- 47 | private: 48 | BriefVocabulary m_voc; // vocabulary 49 | BriefDatabase m_db; // database 50 | 51 | // METHODS ------------------------------------- 52 | public: 53 | // default constructor 54 | CBoWManager() {} 55 | 56 | /* loadVocabularyFromConfigFile : Loads the prebuilt vocabulary from a inifile 57 | [I] config -- Ini file from where to load 'vocabulary' filename 58 | */ 59 | inline void loadVocabularyFromConfigFile( const utils::CConfigFile & config, const string & section, const string & param ) 60 | { 61 | // set vocabulary and create db 62 | const string VOC_FILENAME = config.read_string(section,param,"",true); 63 | ASSERT_FILE_EXISTS_( VOC_FILENAME ) 64 | m_voc.load( VOC_FILENAME ); 65 | m_db.setVocabulary( m_voc, true, 5 ); 66 | } // end-loadVocabularyFromConfigFile 67 | 68 | /* insertIntoDB : Inserts a KF into the DB 69 | [I] kf -- KF that contains in 'm_descriptors_left' the ORB descriptors to insert into the DB 70 | */ 71 | inline void insertIntoDB( const CStereoSLAMKF & kf ) 72 | { 73 | vector out; 74 | m_change_structure_binary( kf.m_descriptors_left, out ); 75 | m_db.add( out ); 76 | } // end -- insertKFIntoDB 77 | 78 | /* queryDB : Queries the DB for the most similar KFs to the input one 79 | [I] kf -- KF that contains in 'm_descriptors_left' the ORB descriptors to insert into the DB 80 | [O] ret -- Structure with the results of the query 81 | [i] num_results -- Number of wanted results (def: 1) 82 | */ 83 | inline void queryDB( const CStereoSLAMKF & kf, QueryResults & ret, unsigned int num_results = 1 ) 84 | { 85 | vector out; 86 | m_change_structure_binary( kf.m_descriptors_left, out ); 87 | m_db.query( out, ret, num_results ); 88 | } // end -- queryKFInDB 89 | 90 | private: 91 | /* insertIntoDB : Adapts a ORB descriptor matrix to a vector of binary descriptors for using it with BoW 92 | [I] plain -- OpenCV Mat with the matrix of ORB descriptors to be converted 93 | [O] out -- Vector of bitset to 94 | */ 95 | void m_change_structure_binary( const Mat & plain, vector & out ) 96 | { 97 | out.resize( plain.rows ); 98 | for( unsigned int i = 0; i < static_cast(plain.rows); i ++ ) 99 | { 100 | // for each descriptor 101 | out[i].resize( plain.cols*8 ); // number of bits (256) 102 | for( unsigned int k = 0; k < static_cast(plain.cols); ++k ) 103 | { 104 | const uint8_t val = plain.at(i,k); 105 | for(unsigned int m = 0; m < 8; ++m) 106 | out[i][m+k*8] = (val >> m) & 1; 107 | } // end-for 108 | } // end-for 109 | } // end-changeStructureORB 110 | 111 | }; // end BoWManager 112 | -------------------------------------------------------------------------------- /src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | PROJECT(srba-stereo-slam) 2 | 3 | FIND_PACKAGE(SRBA REQUIRED) 4 | INCLUDE_DIRECTORIES(${SRBA_INCLUDE_DIRS}) 5 | 6 | FIND_PACKAGE(MRPT REQUIRED ${SRBA_REQUIRED_MRPT_MODULES} obs gui opengl vision hwdrivers graphslam) 7 | FIND_PACKAGE(OpenCV 2.4.5 REQUIRED) 8 | 9 | SET( VO_LIB_DIR 10 | "" CACHE PATH 11 | "Visual Odometry Library directory" ) 12 | SET( VO_INC_DIR 13 | "" CACHE PATH 14 | "Visual Odometry Include directory" ) 15 | 16 | SET( BOW2_DIR 17 | "" CACHE PATH 18 | "BoW2 path") 19 | 20 | SET( BOOST_INC_DIR 21 | "" CACHE PATH 22 | "Boost include dir" ) 23 | 24 | SET( BOW2_LIB_RELEASE_DIR ${BOW2_DIR}/lib ) 25 | SET( BOW2_LIB_DEBUG_DIR ${BOW2_DIR}/lib ) 26 | 27 | IF(UNIX) 28 | SET( VO_LIBS_RELEASE ${VO_LIB_DIR}/libstereo-odometry.so ) 29 | SET( VO_LIBS_DEBUG ${VO_LIB_DIR}/libstereo-odometry.so ) 30 | ELSE(UNIX) 31 | SET( VO_LIBS_RELEASE ${VO_LIB_DIR}/Release/stereo-odometry.lib ) 32 | SET( VO_LIBS_DEBUG ${VO_LIB_DIR}/Debug/stereo-odometry.lib ) 33 | ENDIF(UNIX) 34 | 35 | INCLUDE_DIRECTORIES( 36 | ${VO_INC_DIR} 37 | ${BOOST_INC_DIR} 38 | ${BOW2_DIR}/DBoW2 39 | ${BOW2_DIR}/DUtils 40 | ${BOW2_DIR}/DUtilsCV 41 | ${BOW2_DIR}/DVision ) 42 | 43 | # Declare the target (an executable) 44 | ADD_EXECUTABLE( srba-stereo-slam 45 | srba-stereo-slam_common.h 46 | CStereoSLAMKF.cpp CStereoSLAMKF.h 47 | CSRBAStereoSLAMEstimator.cpp CSRBAStereoSLAMEstimator.h 48 | CBoWManager.h 49 | srba-stereo-slam_utils.cpp srba-stereo-slam_utils.h 50 | srba-stereo-slam_main.cpp srba-stereo-slam.h ) 51 | 52 | # NOTE: It seems crucial the order of the linking libs!! First, our lib, then MRPT ones 53 | IF(UNIX) 54 | SET( LINK_LIBRARIES_RELEASE 55 | optimized ${VO_LIBS_RELEASE} 56 | optimized ${BOW2_LIB_RELEASE_DIR}/libDUtils.so 57 | optimized ${BOW2_LIB_RELEASE_DIR}/libDUtilsCV.so 58 | optimized ${BOW2_LIB_RELEASE_DIR}/libDVision.so 59 | optimized ${BOW2_LIB_RELEASE_DIR}/libDBoW2.so ) 60 | 61 | SET( LINK_LIBRARIES_DEBUG debug 62 | ${VO_LIBS_DEBUG} 63 | ${BOW2_LIB_DEBUG_DIR}/libDUtils.so 64 | ${BOW2_LIB_DEBUG_DIR}/libDUtilsCV.so 65 | ${BOW2_LIB_DEBUG_DIR}/libDVision.so 66 | ${BOW2_LIB_DEBUG_DIR}/libDBoW2.so ) 67 | ELSE(UNIX) 68 | SET( LINK_LIBRARIES_RELEASE 69 | optimized ${VO_LIBS_RELEASE} 70 | optimized ${BOW2_LIB_RELEASE_DIR}/Release/libDUtils.lib 71 | optimized ${BOW2_LIB_RELEASE_DIR}/Release/libDUtilsCV.lib 72 | optimized ${BOW2_LIB_RELEASE_DIR}/Release/libDVision.lib 73 | optimized ${BOW2_LIB_RELEASE_DIR}/Release/libDBoW2.lib ) 74 | 75 | SET( LINK_LIBRARIES_DEBUG debug 76 | ${VO_LIBS_DEBUG} 77 | ${BOW2_LIB_DEBUG_DIR}/Debug/libDUtils.lib 78 | ${BOW2_LIB_DEBUG_DIR}/Debug/libDUtilsCV.lib 79 | ${BOW2_LIB_DEBUG_DIR}/Debug/libDVision.lib 80 | ${BOW2_LIB_DEBUG_DIR}/Debug/libDBoW2.lib ) 81 | ENDIF(UNIX) 82 | 83 | TARGET_LINK_LIBRARIES( 84 | srba-stereo-slam 85 | ${LINK_LIBRARIES_RELEASE} 86 | ${LINK_LIBRARIES_DEBUG} 87 | ${MRPT_LIBS} 88 | ${OpenCV_LIBS} ) 89 | 90 | # Add user supplied extra options (optimization, etc...) 91 | IF(MSVC) 92 | # For MSVC to avoid the C1128 error about too large object files: 93 | SET(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} /bigobj") 94 | SET(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} /bigobj") 95 | ENDIF(MSVC) 96 | 97 | # Set optimized building: 98 | IF(CMAKE_COMPILER_IS_GNUCXX AND NOT CMAKE_BUILD_TYPE MATCHES "Debug") 99 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3 -mtune=native") 100 | ENDIF(CMAKE_COMPILER_IS_GNUCXX AND NOT CMAKE_BUILD_TYPE MATCHES "Debug") 101 | 102 | -------------------------------------------------------------------------------- /src/CSRBAStereoSLAMEstimator.h: -------------------------------------------------------------------------------- 1 | /********************************************************************************* 2 | ** SRBA-Stereo-SLAM ** 3 | ********************************************************************************** 4 | ** ** 5 | ** Copyright(c) 2015-2017, Jose Luis Blanco, University of Almeria ** 6 | ** Copyright(c) 2015-2017, Francisco-Angel Moreno ** 7 | ** MAPIR group, University of Malaga ** 8 | ** All right reserved 9 | ** ** 10 | ** This program is free software: you can redistribute it and/or modify ** 11 | ** it under the terms of the GNU General Public License (version 3) as ** 12 | ** published by the Free Software Foundation. ** 13 | ** ** 14 | ** This program is distributed in the hope that it will be useful, but ** 15 | ** WITHOUT ANY WARRANTY; without even the implied warranty of ** 16 | ** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the ** 17 | ** GNU General Public License for more details. ** 18 | ** ** 19 | ** You should have received a copy of the GNU General Public License ** 20 | ** along with this program. If not, see . ** 21 | ** ** 22 | **********************************************************************************/ 23 | #pragma once 24 | 25 | #include "srba-stereo-slam_common.h" // Common defines and headers 26 | #include "srba-stereo-slam_utils.h" 27 | #include "srba-stereo-slam.h" // mySRBA class definition 28 | #include "CStereoSLAMKF.h" 29 | #include "CBoWManager.h" // Bag of words manager 30 | 31 | extern TGeneralOptions general_options; 32 | 33 | class CSRBAStereoSLAMEstimator 34 | { 35 | public: 36 | ~CSRBAStereoSLAMEstimator() {} //!< Default destructor 37 | CSRBAStereoSLAMEstimator() : //!< Default constructor 38 | m_last_kf_ID(0), 39 | m_last_match_ID(0), 40 | m_last_num_tracked_feats(UNINITIALIZED_TRACKED_NUMBER) 41 | { 42 | opengl_params.span_tree_max_depth = 1000; 43 | opengl_params.draw_unknown_feats_ellipses = false; 44 | opengl_params.show_unknown_feats_ids = false; 45 | opengl_params.draw_kf_hierarchical = true; 46 | } 47 | 48 | TSRBAStereoSLAMOptions srba_options; 49 | mySRBA::TOpenGLRepresentationOptions opengl_params; 50 | 51 | mySRBA rba; // main member 52 | 53 | void initialize( const CConfigFile & config ); 54 | void performStereoSLAM(); 55 | 56 | private: 57 | // -- variables 58 | t_vector_kf m_keyframes; 59 | size_t m_last_kf_ID, 60 | m_last_match_ID, 61 | m_last_num_tracked_feats; 62 | 63 | double m_max_rotation, 64 | m_max_translation, 65 | m_max_rotation_limit, 66 | m_max_translation_limit; 67 | 68 | CTimeLogger m_time_logger, 69 | m_time_logger_define_kf; // Specific for inserting a new KF (and graph optimization) 70 | TStatsSRBAVector m_stats; 71 | 72 | CBoWManager m_bow_manager; 73 | 74 | rso::CStereoOdometryEstimator m_voEngine; 75 | rso::CStereoOdometryEstimator::TStereoOdometryRequest m_odom_request; 76 | rso::CStereoOdometryEstimator::TStereoOdometryResult m_odom_result; 77 | 78 | // poses 79 | CPose3DRotVec m_current_pose; // The estimated GLOBAL pose of the camera = last_kf_pose + vo_result + camera_pose_on_robot 80 | CPose3DRotVec m_last_kf_pose; // The GLOBAL pose of the last kf (set from RBA engine at each new KF insertion) 81 | CPose3DRotVec m_incr_pose_from_last_kf; // Accumulated incremental pose from the last KF (computed from vo_results at each time step, reset at new KF insertion) 82 | CPose3DRotVec m_incr_pose_from_last_check; // Accumulated incremental pose from the last check of new insertion KF (computed at each time step, reset at new KF check) 83 | 84 | // camera 85 | hwdrivers::CCameraSensor m_myCam; 86 | 87 | // gui 88 | gui::CDisplayWindow3DPtr m_win; 89 | 90 | // data association 91 | //struct TDAMatchInfo 92 | //{ 93 | // enum TDAMatchStatus{sTRACKED = 0, sNON_TRACKED, sREJ_SLOPE, sREJ_ORB, sREJ_FUND_MATRIX, sREJ_CHANGE_POSE, sREJ_CONSISTENCY}; 94 | // TDAMatchStatus status; 95 | // size_t other_idx; 96 | // double distance; 97 | // TDAMatchInfo () : status(sTRACKED) , other_idx(INVALID_IDX), distance(0.0) {} 98 | //}; 99 | 100 | struct TVectorDAMatchInfo 101 | { 102 | enum TDAMatchStatus{sTRACKED = 0, sNON_TRACKED, sREJ_SLOPE, sREJ_ORB, sREJ_FUND_MATRIX, sREJ_CHANGE_POSE, sREJ_CONSISTENCY}; 103 | vector m_status; 104 | vector m_matches; 105 | 106 | /** initialization constructor*/ 107 | TVectorDAMatchInfo( size_t s ) { 108 | m_status.resize(s,sTRACKED); 109 | m_matches.resize(s); 110 | } 111 | 112 | /** get size */ 113 | size_t size() { return m_status.size(); } 114 | }; 115 | 116 | // -- methods 117 | // state management 118 | // -- load from stream 119 | bool m_load_options_from_stream( std::ifstream & stream, TSRBAStereoSLAMOptions & options ); 120 | bool m_load_keypoints_from_stream( std::ifstream & stream, TKeyPointList & keypoints, Mat & descriptors ); 121 | bool m_load_matches_from_stream( std::ifstream & stream, TDMatchList & matches, vector & matches_ids ); 122 | bool m_load_state(); 123 | 124 | // -- save to stream 125 | bool m_dump_keypoints_to_stream( std::ofstream & stream, const TKeyPointList & keypoints, const Mat & descriptors ); 126 | bool m_dump_options_to_stream( std::ofstream & stream, const TSRBAStereoSLAMOptions & options ); 127 | bool m_dump_matches_to_stream( std::ofstream & stream, const TDMatchList & matches, const vector & matches_ids ); 128 | bool m_save_state(); 129 | 130 | // other 131 | LCResult m_check_loop_closure( 132 | const TKeyFrameID & new_kf_id, 133 | const QueryResults & ret, 134 | TLoopClosureInfo & lc_info ); 135 | 136 | bool m_get_similar_kfs( 137 | const TKeyFrameID & newKfId, 138 | const QueryResults & dbQueryResults, 139 | TLoopClosureInfo & out ); 140 | 141 | /** Computes matchings between a set of KF and this KF according to the query results */ 142 | void m_data_association( 143 | const CStereoSLAMKF & kf, // INPUT 144 | const TLoopClosureInfo & lc_info, // INPUT 145 | TVectorKfsDaInfo & out_da ); // OUTPUT 146 | 147 | /** Computes matching between a certain KF and this KF */ 148 | void m_internal_data_association( 149 | const CStereoSLAMKF & this_kf, // INPUT -- One KF to perform DA 150 | const CStereoSLAMKF & other_kf, // INPUT -- The other KF to perform DA with 151 | const Mat & curLDesc, // INPUT -- Left image descriptors from current KF 152 | const Mat & curRDesc, // INPUT -- Right image descriptors from current KF 153 | t_kf_da_info & out_da, // OUTPUT -- DA information from this KF wrt the other one 154 | const CPose3DRotVec & kf_ini_rel_pose = CPose3DRotVec() ); // oINPUT -- Initial estimation of the relative pose between the KFs 155 | 156 | /** Compute the change in pose between frames and check keypoint residuals to find outliers (to be deleted) */ 157 | void m_detect_outliers_with_change_in_pose ( 158 | t_vector_pair_idx_distance & other_matched, 159 | const CStereoSLAMKF & this_kf, 160 | const CStereoSLAMKF & other_kf, 161 | vector & outliers, // OUTPUT 162 | const CPose3DRotVec & kf_ini_rel_pose ); 163 | 164 | /** Compute the change in pose between frames and check keypoint residuals to find outliers */ 165 | void m_detect_outliers_with_change_in_pose ( 166 | TVectorDAMatchInfo & this_matches, 167 | //deque & this_matches, 168 | const CStereoSLAMKF & this_kf, 169 | const CStereoSLAMKF & other_kf, 170 | const CPose3DRotVec & kf_ini_rel_pose ); 171 | 172 | /** Compute the fundamental matrix between the left images to find outliers (to be deleted) */ 173 | void m_detect_outliers_with_F ( 174 | const t_vector_pair_idx_distance & other_matched, 175 | const CStereoSLAMKF & this_kf, 176 | const CStereoSLAMKF & other_kf, 177 | vector & outliers ); // OUTPUT 178 | 179 | /** Compute the fundamental matrix between the left images to find outliers */ 180 | void m_detect_outliers_with_F ( 181 | TVectorDAMatchInfo & this_matches, 182 | //deque & this_matches, 183 | const size_t & num_tracked, 184 | const CStereoSLAMKF & this_kf, 185 | const CStereoSLAMKF & other_kf ); 186 | 187 | /** Check matches directions to find outliers */ 188 | void m_detect_outliers_with_direction ( 189 | TVectorDAMatchInfo & this_matches, 190 | // deque & this_matches, 191 | const size_t & offset, 192 | const CStereoSLAMKF & this_kf, 193 | const CStereoSLAMKF & other_kf ); 194 | 195 | /** Check ORB distances to find outliers */ 196 | void m_detect_outliers_with_orb_distance ( 197 | TVectorDAMatchInfo & this_matches, 198 | //deque & this_matches, 199 | // const vector & matL, 200 | const CStereoSLAMKF & this_kf, 201 | const CStereoSLAMKF & other_kf ); 202 | 203 | 204 | }; // end--CSRBAStereoSLAMEstimator 205 | -------------------------------------------------------------------------------- /src/CStereoSLAMKF.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************************* 2 | ** SRBA-Stereo-SLAM ** 3 | ********************************************************************************** 4 | ** ** 5 | ** Copyright(c) 2015-2017, Jose Luis Blanco, University of Almeria ** 6 | ** Copyright(c) 2015-2017, Francisco-Angel Moreno ** 7 | ** MAPIR group, University of Malaga ** 8 | ** All right reserved 9 | ** ** 10 | ** This program is free software: you can redistribute it and/or modify ** 11 | ** it under the terms of the GNU General Public License (version 3) as ** 12 | ** published by the Free Software Foundation. ** 13 | ** ** 14 | ** This program is distributed in the hope that it will be useful, but ** 15 | ** WITHOUT ANY WARRANTY; without even the implied warranty of ** 16 | ** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the ** 17 | ** GNU General Public License for more details. ** 18 | ** ** 19 | ** You should have received a copy of the GNU General Public License ** 20 | ** along with this program. If not, see . ** 21 | ** ** 22 | **********************************************************************************/ 23 | #include "CStereoSLAMKF.h" 24 | 25 | extern TGeneralOptions general_options; 26 | 27 | // ---------------------------------------------------------- 28 | // dumpToConsole 29 | // ---------------------------------------------------------- 30 | void CStereoSLAMKF::dumpToConsole() 31 | { 32 | cout << "KEYFRAME [" << this->m_kf_ID << "]" << endl 33 | << "---------------------------------------------" << endl 34 | << " :: Camera pose = " << this->m_camera_pose << endl 35 | << " :: Matches [" << this->m_matches.size() << " out of " 36 | << this->m_keypoints_left.size() << "/" 37 | << this->m_keypoints_right.size() << "]: ID: left_kp_x,left_kp_y --> right_kp_x,right_kp_y" << endl 38 | << " -------------------------------------" 39 | << endl; 40 | 41 | for( size_t k = 0; k < m_matches.size(); ++k ) 42 | { 43 | const size_t id1 = m_matches[k].queryIdx; 44 | const size_t id2 = m_matches[k].trainIdx; 45 | 46 | cout << " " 47 | << m_matches_ID[k] << ": " 48 | << m_keypoints_left[id1].pt.x << "," 49 | << m_keypoints_left[id1].pt.y << " --> " 50 | << m_keypoints_right[id2].pt.x << "," 51 | << m_keypoints_right[id2].pt.y 52 | << endl; 53 | } // end-for 54 | cout << "++++++++++++++++++++++++++++++++++++++++++++++" << endl; 55 | } // end dumpToConsole 56 | 57 | // ---------------------------------------------------------- 58 | // saveInfoToFiles 59 | // ---------------------------------------------------------- 60 | void CStereoSLAMKF::saveInfoToFiles( const string & str_modif ) 61 | { 62 | // prepare output directory 63 | if( !mrpt::system::directoryExists( general_options.out_dir ) ) 64 | mrpt::system::createDirectory( general_options.out_dir ); 65 | 66 | // information 67 | string my_filename; 68 | if( str_modif.empty() ) 69 | my_filename = mrpt::format("%s\\info_kf%04d.txt", general_options.out_dir.c_str(), this->m_kf_ID); 70 | else 71 | my_filename = mrpt::format("%s\\%s_info_kf%04d.txt", general_options.out_dir.c_str(), str_modif.c_str(), this->m_kf_ID); 72 | 73 | FILE *f = mrpt::system::os::fopen( my_filename, "wt"); 74 | if( !f ) 75 | THROW_EXCEPTION( mrpt::format("Output file %s could not be opened", my_filename.c_str()) ); 76 | 77 | mrpt::system::os::fprintf(f, "%% [KF_ID] [MATCH_ID] [LEFT_PT{x y}] [RIGHT_PT{x y}] [MATCH_DISTANCE]\n"); 78 | size_t m_count = 0; 79 | vector::iterator it; 80 | const size_t n_matches_id_size = this->m_matches_ID.size(); 81 | for( it = this->m_matches.begin(); it != this->m_matches.end(); ++it, ++m_count ) 82 | { 83 | const cv::KeyPoint & lkp = this->m_keypoints_left[it->queryIdx]; 84 | const cv::KeyPoint & rkp = this->m_keypoints_right[it->trainIdx]; 85 | mrpt::system::os::fprintf( f,"%d %d %.2f %.2f %.2f %.2f %.2f\n", 86 | this->m_kf_ID, 87 | n_matches_id_size > 0 ? this->m_matches_ID[m_count] : 0, 88 | lkp.pt.x, lkp.pt.y, 89 | rkp.pt.x, rkp.pt.y, 90 | it->distance ); 91 | } // end-for 92 | mrpt::system::os::fclose(f); 93 | 94 | if( str_modif.empty() ) 95 | my_filename = mrpt::format("%s\\info_feats_kf%04d.txt", general_options.out_dir.c_str(), this->m_kf_ID); 96 | else 97 | my_filename = mrpt::format("%s\\%s_info_feats_kf%04d.txt", general_options.out_dir.c_str(), str_modif.c_str(), this->m_kf_ID); 98 | 99 | f = mrpt::system::os::fopen( my_filename, "wt"); 100 | if( !f ) 101 | THROW_EXCEPTION( mrpt::format("Output file %s could not be opened", my_filename.c_str()) ); 102 | 103 | mrpt::system::os::fprintf( f, "%d %d\n", this->m_keypoints_left.size(), this->m_keypoints_right.size() ); 104 | for( vector::iterator it = this->m_keypoints_left.begin(); it != this->m_keypoints_left.end(); ++it ) 105 | mrpt::system::os::fprintf( f, "%.2f %.2f\n", it->pt.x, it->pt.y ); 106 | for( vector::iterator it = this->m_keypoints_right.begin(); it != this->m_keypoints_right.end(); ++it ) 107 | mrpt::system::os::fprintf( f, "%.2f %.2f\n", it->pt.x, it->pt.y ); 108 | 109 | mrpt::system::os::fclose(f); 110 | } // end saveInfoToFiles 111 | -------------------------------------------------------------------------------- /src/CStereoSLAMKF.h: -------------------------------------------------------------------------------- 1 | /********************************************************************************* 2 | ** SRBA-Stereo-SLAM ** 3 | ********************************************************************************** 4 | ** ** 5 | ** Copyright(c) 2015-2017, Jose Luis Blanco, University of Almeria ** 6 | ** Copyright(c) 2015-2017, Francisco-Angel Moreno ** 7 | ** MAPIR group, University of Malaga ** 8 | ** All right reserved 9 | ** ** 10 | ** This program is free software: you can redistribute it and/or modify ** 11 | ** it under the terms of the GNU General Public License (version 3) as ** 12 | ** published by the Free Software Foundation. ** 13 | ** ** 14 | ** This program is distributed in the hope that it will be useful, but ** 15 | ** WITHOUT ANY WARRANTY; without even the implied warranty of ** 16 | ** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the ** 17 | ** GNU General Public License for more details. ** 18 | ** ** 19 | ** You should have received a copy of the GNU General Public License ** 20 | ** along with this program. If not, see . ** 21 | ** ** 22 | **********************************************************************************/ 23 | #pragma once 24 | #include "srba-stereo-slam_common.h" 25 | #include "srba-stereo-slam_utils.h" // stereo and general options 26 | 27 | // srba 28 | //#define SRBA_DETAILED_TIME_PROFILING 0 29 | //#include 30 | 31 | // graph-slam 32 | //#include // For global map recovery only 33 | //#include // To render the global map 34 | 35 | // others 36 | #include 37 | 38 | // defines 39 | using namespace mrpt; 40 | using namespace mrpt::gui; 41 | using namespace mrpt::opengl; 42 | using namespace mrpt::utils; 43 | using namespace mrpt::vision; 44 | using namespace mrpt::system; 45 | using namespace mrpt::obs; 46 | using namespace std; 47 | using namespace srba; 48 | using namespace cv; 49 | 50 | class CStereoSLAMKF; 51 | typedef vector t_vector_kf; 52 | 53 | /********************************************* 54 | CLASS: Keyframe for SRBA 55 | **********************************************/ 56 | class CStereoSLAMKF 57 | { 58 | public: 59 | /** Default empty constructor */ 60 | CStereoSLAMKF() : m_kf_ID(0) {} 61 | 62 | /** Sets the ID of this keyframe */ 63 | inline void setKFID( const size_t ID ) { 64 | m_kf_ID = ID; 65 | } 66 | 67 | /** Gets the last keypoints & descriptors lists and the left-right matches from the visual odometry estimator */ 68 | inline void getDataFromVOEngine( rso::CStereoOdometryEstimator & voEngine ) { 69 | voEngine.getValues( m_keypoints_left, m_keypoints_right, m_descriptors_left, m_descriptors_right, m_matches, m_matches_ID ); 70 | } 71 | 72 | /** Generates IDs for the inner matches, 73 | * -- useful when we insert the matches into this KF from another source (eg. odometry) and IDs were not generated 74 | */ 75 | inline void generateMatchesIDs( const size_t _starting_idx ) 76 | { 77 | size_t starting_idx = _starting_idx; 78 | for( size_t m = 0; m < m_matches.size(); ++m ) 79 | m_matches_ID.push_back( starting_idx++ ); 80 | } // end -- generateMatchesIDs 81 | 82 | /** Creates the keyframe and fill it with information 83 | * -- detect ORB(multi-scale) features in left and right image 84 | * -- match the features and create a new KF with the pairings 85 | */ 86 | void create( const CObservationStereoImagesPtr & stImgs, const TSRBAStereoSLAMOptions & options = TSRBAStereoSLAMOptions() /*default*/ ); 87 | 88 | /** Shows the content of the keyframe on the console */ 89 | void dumpToConsole(); 90 | 91 | /** Saves the information of the keyframe into a set of files 92 | * -- matched features in the image 93 | */ 94 | void saveInfoToFiles( const string & str_modif = string() ); 95 | 96 | // ------------------------------------------------------------------------------ 97 | // DATA MEMBERS 98 | // ------------------------------------------------------------------------------ 99 | TKeyPointList m_keypoints_left, m_keypoints_right; //!< vectors of keypoints (left and right) 100 | Mat m_descriptors_left, m_descriptors_right; //!< vectors of ORB descriptors 101 | TDMatchList m_matches; //!< vector of l-r matches 102 | vector m_matches_ID; //!< vector of ids of the matches 103 | CPose3DRotVec m_camera_pose; //!< estimated camera pose 104 | size_t m_kf_ID; //!< the id of this keyframe 105 | 106 | }; // end -- CStereoSLAMKF 107 | 108 | /** KpResponseSorter: helpful struct to order a vector of KeyPoints according to their response */ 109 | struct KpResponseSorter : public std::binary_function 110 | { 111 | const vector & m_data; 112 | KpResponseSorter( const vector & data ) : m_data( data ) { } 113 | bool operator() (size_t k1, size_t k2 ) const { 114 | return (m_data[k1].response > m_data[k2].response); 115 | } 116 | }; // end -- KpResponseSorter 117 | 118 | /** DATrackedSorter: helpful struct to order a vector of t_kf_da_info according to the number of tracked features */ 119 | struct DATrackedSorter : public std::binary_function 120 | { 121 | const TVectorKfsDaInfo & m_data; 122 | DATrackedSorter( const TVectorKfsDaInfo & data ) : m_data( data ) { } 123 | bool operator() (size_t k1, size_t k2 ) const { 124 | return ( m_data[k1].tracked_matches > m_data[k2].tracked_matches ); 125 | } 126 | }; // end -- DATrackedSorter 127 | 128 | -------------------------------------------------------------------------------- /src/srba-stereo-slam.h: -------------------------------------------------------------------------------- 1 | /********************************************************************************* 2 | ** SRBA-Stereo-SLAM ** 3 | ********************************************************************************** 4 | ** ** 5 | ** Copyright(c) 2015-2017, Jose Luis Blanco, University of Almeria ** 6 | ** Copyright(c) 2015-2017, Francisco-Angel Moreno ** 7 | ** MAPIR group, University of Malaga ** 8 | ** All right reserved 9 | ** ** 10 | ** This program is free software: you can redistribute it and/or modify ** 11 | ** it under the terms of the GNU General Public License (version 3) as ** 12 | ** published by the Free Software Foundation. ** 13 | ** ** 14 | ** This program is distributed in the hope that it will be useful, but ** 15 | ** WITHOUT ANY WARRANTY; without even the implied warranty of ** 16 | ** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the ** 17 | ** GNU General Public License for more details. ** 18 | ** ** 19 | ** You should have received a copy of the GNU General Public License ** 20 | ** along with this program. If not, see . ** 21 | ** ** 22 | **********************************************************************************/ 23 | #pragma once 24 | #include "srba-stereo-slam_common.h" 25 | using namespace srba; 26 | using namespace mrpt::opengl; 27 | using namespace mrpt::poses; 28 | 29 | // the options for the RBA Engine 30 | struct my_srba_options : public RBA_OPTIONS_DEFAULT 31 | { 32 | //typedef options::observation_noise_identity obs_noise_matrix_t; // The sensor noise matrix is the same for all observations and equal to \sigma * I(identity) 33 | typedef options::sensor_pose_on_robot_se3 sensor_pose_on_robot_t; 34 | typedef options::solver_LM_schur_dense_cholesky solver_t; // Solver algorithm 35 | //typedef ecps::local_areas_var_size edge_creation_policy_t; 36 | typedef ecps::local_areas_fixed_size edge_creation_policy_t; 37 | }; 38 | 39 | // define the RBA Problem state for this application 40 | typedef TRBA_Problem_state < 41 | kf2kf_poses::SE3, 42 | landmarks::Euclidean3D, 43 | observations::StereoCamera, 44 | my_srba_options 45 | > myRBAProblemState; 46 | 47 | // define the RBA Engine for this application 48 | typedef RbaEngine < 49 | kf2kf_poses::SE3, // 6D movement 50 | landmarks::Euclidean3D, // {X,Y,Z} 3D landmarks 51 | observations::StereoCamera, // Observations are stereo: o^i = {ul^i,vl^i,ur^i,vr^i} 52 | my_srba_options 53 | > 54 | myRBAEngine; 55 | 56 | /********************************************* 57 | CLASS: Customized SRBA engine 58 | **********************************************/ 59 | class mySRBA : public myRBAEngine 60 | { 61 | private : 62 | bool m_lc; //!< Indicates if a loop closure has been detected 63 | size_t m_lc_old_kf_id; //!< ID of the old KF for the loop closure 64 | deque m_localmap_center_ids; //!< Contains the IDs of the localmap centers for each KF 65 | map m_submap_kfs_from_localmap_center; //!< Contains the number of KF that a certain localmap contains 66 | 67 | deque< set > m_kf_localmap_center_ID; //!< Contains the IDs of the localmap centers for each KF 68 | map< TKeyFrameID, set > m_localmap_kf_IDs; //!< Contains the number of KF that a certain localmap contains 69 | 70 | pose_t m_initial_kf_pose; //!< Initial estimation of the pose of the added KF wrt the previous one 71 | 72 | public: 73 | /** Constructor */ 74 | mySRBA() : 75 | m_lc(false), 76 | m_lc_old_kf_id(0), 77 | m_localmap_center_ids(), 78 | m_submap_kfs_from_localmap_center(), 79 | m_kf_localmap_center_ID(), 80 | m_localmap_kf_IDs(), 81 | m_initial_kf_pose(pose_t()) 82 | {} 83 | 84 | /** Sets/unsets flag for loop closure */ 85 | void loopClosureDetected( bool _lc = true ) { 86 | m_lc = _lc; 87 | } 88 | 89 | /** Sets ID of the old KF for a loop closure */ 90 | void setLoopClosureOldID( size_t _id ) { 91 | m_lc_old_kf_id = _id; 92 | } 93 | 94 | /** Sets the initial pose of the KF to be added */ 95 | inline void setInitialKFPose( const pose_t & pose ) { 96 | m_initial_kf_pose = pose; 97 | } 98 | 99 | /** Indicates if the input KF ID correspond to a localmap center */ 100 | inline bool isKFLocalmapCenter( const TKeyFrameID & kf_id ) const { 101 | return m_localmap_kf_IDs.find(kf_id) != m_localmap_kf_IDs.end(); 102 | } // end -- isKFLocalmapCenter 103 | 104 | /** Gets the ID of the localmap center for the input KF ID */ 105 | inline TKeyFrameID getLocalmapCenterID( const TKeyFrameID & kf_id ) const { 106 | ASSERT_( kf_id < m_kf_localmap_center_ID.size() ); 107 | return *m_kf_localmap_center_ID[kf_id].rbegin(); 108 | } // end -- getLocalmapCenterID 109 | 110 | /** Prints the number of KFs in each localmap */ 111 | inline void dumpKfsInLocalmaps() { 112 | cout << "Kfs within localmaps:" << endl; 113 | for( map< TKeyFrameID, set >::iterator it = m_localmap_kf_IDs.begin(); it != m_localmap_kf_IDs.end(); ++it ) 114 | { 115 | cout << " Localmap " << it->first << "(" << it->second.size() << " kfs)" << endl; 116 | for( set::iterator it2 = it->second.begin(); it2 != it->second.end(); ++it2 ) 117 | cout << " --> " << *it2 << endl; 118 | } // end-for 119 | } // end -- dumpNumberOfKFs 120 | 121 | /** Prints the localmap centers for all the defined KFs */ 122 | inline void dumpLocalmapCenters() { 123 | cout << "Localmap centers:" << endl; 124 | for( size_t i = 0; i < m_kf_localmap_center_ID.size(); i++ ) 125 | { 126 | cout << " Kf " << i << endl; 127 | for( set::iterator it = m_kf_localmap_center_ID[i].begin(); it != m_kf_localmap_center_ID[i].end(); ++it ) 128 | cout << " --> " << *it << endl; 129 | } 130 | } // end -- dumpLocalmapCenters 131 | 132 | /** Gets the number of KFs for a certain localmap ID */ 133 | inline size_t getNumberOfKFsForLocalMap( const TKeyFrameID & kf_localmap_center_id ) 134 | { 135 | return m_localmap_kf_IDs.find(kf_localmap_center_id) != m_localmap_kf_IDs.end() ? 136 | m_localmap_kf_IDs[kf_localmap_center_id].size() : 137 | -1; 138 | } // end -- getNumberOfKFsForLocalMap 139 | 140 | private: 141 | /******************************* MAIN METHOD *******************************/ 142 | /** Implements the edge-creation policy, by default depending on "parameters.edge_creation_policy" if the user doesn't re-implement 143 | this virtual method. * See tutorials for examples of how to implement custom policies. */ 144 | virtual void edge_creation_policy( 145 | const TKeyFrameID new_kf_id, 146 | const traits_t::new_kf_observations_t & obs, 147 | std::vector & new_k2k_edge_ids ) 148 | { 149 | // :: this method should not be called for the first KF 150 | ASSERT_( new_kf_id >= 1 ) 151 | 152 | // :: set KF#0 to have itself as its base 153 | if( new_kf_id == 1 ) 154 | { 155 | set aux; aux.insert(0); 156 | m_kf_localmap_center_ID.push_back( aux ); 157 | m_localmap_kf_IDs[0] = set(); 158 | } 159 | 160 | // :: get sRBA state 161 | rba_problem_state_t & my_rba_state = this->get_rba_state(); 162 | 163 | const size_t MINIMUM_OBS_TO_LOOP_CLOSURE = parameters.ecp.min_obs_to_loop_closure; 164 | const size_t SUBMAP_SIZE = parameters.ecp.submap_size; // In # of KFs 165 | // although submaps have variable sizes from the time a loop is closed, by default they have SUBMAP_SIZE 166 | 167 | // :: get the current localmap base id 168 | TKeyFrameID currentLocalmapBaseId = 169 | isKFLocalmapCenter(new_kf_id-1) ? 170 | new_kf_id-1 : 171 | getLocalmapCenterID(new_kf_id-1); 172 | 173 | // :: two cases: 174 | const size_t NUM_KFS_LOCALMAP = getNumberOfKFsForLocalMap( currentLocalmapBaseId ); 175 | ASSERT_( NUM_KFS_LOCALMAP >= 0 ) 176 | if( NUM_KFS_LOCALMAP < SUBMAP_SIZE-1 ) 177 | { 178 | // :: not a localmap base --> just add the new kf to the current localmap 179 | // :: set the current localmap center as the center for the new kf 180 | set aux; aux.insert(currentLocalmapBaseId); 181 | m_kf_localmap_center_ID.push_back( aux ); 182 | 183 | // :: create the edge 184 | TNewEdgeInfo nei; 185 | nei.has_approx_init_val = false; // Filled in below 186 | nei.id = this->create_kf2kf_edge( new_kf_id, TPairKeyFrameID( currentLocalmapBaseId, new_kf_id ), obs ); 187 | 188 | if( NUM_KFS_LOCALMAP == 0 ) 189 | { 190 | // This is the first KF after a new center, so if we add an edge to it we must be very close: 191 | #ifdef SRBA_WORKAROUND_MSVC9_DEQUE_BUG 192 | my_rba_state.k2k_edges[nei.id]->inv_pose = m_initial_kf_pose; //pose_t(); 193 | #else 194 | my_rba_state.k2k_edges[nei.id].inv_pose = m_initial_kf_pose; // pose_t(); 195 | #endif 196 | } 197 | else 198 | { 199 | // Idea: the new KF should be close to the last one. 200 | #ifdef SRBA_WORKAROUND_MSVC9_DEQUE_BUG 201 | my_rba_state.k2k_edges[nei.id]->inv_pose = my_rba_state.k2k_edges[nei.id-1]->inv_pose; 202 | #else 203 | my_rba_state.k2k_edges[nei.id].inv_pose.inverseComposeFrom(my_rba_state.k2k_edges[nei.id-1].inv_pose,m_initial_kf_pose); 204 | #endif 205 | } 206 | new_k2k_edge_ids.push_back(nei); 207 | 208 | // :: insert this new kf id into the list of kfs for the current localmap 209 | m_localmap_kf_IDs[currentLocalmapBaseId].insert(new_kf_id); 210 | 211 | } // end-if-localmap-base 212 | else 213 | { 214 | // :: localmap base --> more than one edge can be added 215 | 216 | // :: go thru all observations and for those already-seen LMs, check the distance between their base KFs and (i_id): 217 | // :: make a list of base KFs of my new observations, ordered in descending order by # of shared observations: 218 | typedef std::multimap > my_base_sorted_lst_t; 219 | 220 | base_sorted_lst_t obs_for_each_base_sorted; 221 | srba::internal::make_ordered_list_base_kfs(obs, my_rba_state, obs_for_each_base_sorted); 222 | 223 | // :: make vote list for each central KF: 224 | map obs_for_each_area; 225 | for( base_sorted_lst_t::const_iterator it = obs_for_each_base_sorted.begin(); it != obs_for_each_base_sorted.end(); ++it ) 226 | { 227 | const size_t num_obs_this_base = it->first; 228 | const TKeyFrameID base_id = it->second; 229 | 230 | const TKeyFrameID thisLocalmapCenter = getLocalmapCenterID(base_id); 231 | obs_for_each_area[thisLocalmapCenter] += num_obs_this_base; 232 | } 233 | 234 | // :: sort by votes: 235 | my_base_sorted_lst_t obs_for_each_area_sorted; 236 | for( map::const_iterator it = obs_for_each_area.begin(); it != obs_for_each_area.end(); ++it ) 237 | { 238 | obs_for_each_area_sorted.insert( make_pair(it->second,it->first) ); 239 | } 240 | 241 | // :: go thru candidate areas: 242 | for( base_sorted_lst_t::const_iterator it = obs_for_each_area_sorted.begin(); it != obs_for_each_area_sorted.end(); ++it ) 243 | { 244 | const size_t num_obs_this_base = it->first; 245 | const TKeyFrameID central_kf_id = it->second; 246 | 247 | // Create edges to all these central KFs if they're too far: 248 | 249 | // Find the distance between "central_kf_id" <=> "new_kf_id" 250 | const TKeyFrameID from_id = new_kf_id; 251 | const TKeyFrameID to_id = central_kf_id; 252 | 253 | rba_problem_state_t::TSpanningTree::next_edge_maps_t::const_iterator it_from = my_rba_state.spanning_tree.sym.next_edge.find(from_id); 254 | 255 | topo_dist_t found_distance = numeric_limits::max(); 256 | 257 | if (it_from != my_rba_state.spanning_tree.sym.next_edge.end()) 258 | { 259 | const map & from_Ds = it_from->second; 260 | map::const_iterator it_to_dist = from_Ds.find(to_id); 261 | 262 | if (it_to_dist != from_Ds.end()) 263 | found_distance = it_to_dist->second.distance; 264 | } 265 | else 266 | { 267 | // The new KF doesn't still have any edge created to it, that's why we didn't found any spanning tree for it. 268 | // Since this means that the KF is aisolated from the rest of the world, leave the topological distance to infinity. 269 | } 270 | 271 | if( found_distance >= parameters.srba.max_optimize_depth ) 272 | { 273 | if( num_obs_this_base >= MINIMUM_OBS_TO_LOOP_CLOSURE ) 274 | { 275 | // The KF is TOO FAR: We will need to create an additional edge: 276 | TNewEdgeInfo nei; 277 | 278 | nei.id = this->create_kf2kf_edge(new_kf_id, TPairKeyFrameID( central_kf_id, new_kf_id ), obs); 279 | nei.has_approx_init_val = false; // Will need to estimate this one 280 | 281 | new_k2k_edge_ids.push_back(nei); 282 | 283 | // :: set the local map center as my localmap center 284 | if( new_kf_id < m_kf_localmap_center_ID.size() ) 285 | m_kf_localmap_center_ID[new_kf_id].insert(central_kf_id); 286 | else 287 | { 288 | set aux; aux.insert(central_kf_id); 289 | m_kf_localmap_center_ID.push_back( aux ); 290 | } 291 | 292 | // :: update localmaps kf ids 293 | m_localmap_kf_IDs[central_kf_id].insert(new_kf_id); 294 | my_rba_state.k2k_edges[nei.id].inv_pose = nei.id == 0 ? pose_t() : my_rba_state.k2k_edges[nei.id-1].inv_pose; 295 | 296 | } 297 | else 298 | { 299 | //if( this->m_verbose_level >= 1 ) cout << "[edge_creation_policy] Skipped extra edge " << central_kf_id <<"->"<=1, mrpt::format("Error for new KF#%u: no suitable linking KF found with a minimum of %u common observation: the node becomes isolated of the graph!", static_cast(new_kf_id),static_cast(MINIMUM_OBS_TO_LOOP_CLOSURE) )) 305 | m_localmap_kf_IDs[new_kf_id] = set(); 306 | } 307 | 308 | } // end-edge_creation_policy 309 | 310 | }; 311 | -------------------------------------------------------------------------------- /src/srba-stereo-slam_common.h: -------------------------------------------------------------------------------- 1 | /********************************************************************************* 2 | ** SRBA-Stereo-SLAM ** 3 | ********************************************************************************** 4 | ** ** 5 | ** Copyright(c) 2015-2017, Jose Luis Blanco, University of Almeria ** 6 | ** Copyright(c) 2015-2017, Francisco-Angel Moreno ** 7 | ** MAPIR group, University of Malaga ** 8 | ** All right reserved 9 | ** ** 10 | ** This program is free software: you can redistribute it and/or modify ** 11 | ** it under the terms of the GNU General Public License (version 3) as ** 12 | ** published by the Free Software Foundation. ** 13 | ** ** 14 | ** This program is distributed in the hope that it will be useful, but ** 15 | ** WITHOUT ANY WARRANTY; without even the implied warranty of ** 16 | ** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the ** 17 | ** GNU General Public License for more details. ** 18 | ** ** 19 | ** You should have received a copy of the GNU General Public License ** 20 | ** along with this program. If not, see . ** 21 | ** ** 22 | **********************************************************************************/ 23 | #pragma once 24 | 25 | // -- include 26 | // opencv 27 | #include 28 | #include 29 | #include 30 | #include 31 | 32 | // mrpt 33 | #include 34 | #include 35 | #include 36 | #include 37 | 38 | #include 39 | #include 40 | 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | 48 | // visual odometry 49 | #include 50 | 51 | // srba 52 | #include 53 | 54 | // -- define 55 | #define ENTER_LOGGER( _STR ) if( general_options.enableLogger ) tLog.enter( _STR ); 56 | #define LEAVE_LOGGER( _STR ) if( general_options.enableLogger ) tLog.leave( _STR ); 57 | #define MY_SQUARE(_X) (_X)*(_X) 58 | #define DUMP_ROTVEC_TO_STREAM( _STREAM, _ROTVEC ) \ 59 | _STREAM.write( (char*)&(_ROTVEC.m_coords[0]), sizeof(double) );\ 60 | _STREAM.write( (char*)&(_ROTVEC.m_coords[1]), sizeof(double) );\ 61 | _STREAM.write( (char*)&(_ROTVEC.m_coords[2]), sizeof(double) );\ 62 | _STREAM.write( (char*)&(_ROTVEC.m_rotvec[0]), sizeof(double) );\ 63 | _STREAM.write( (char*)&(_ROTVEC.m_rotvec[1]), sizeof(double) );\ 64 | _STREAM.write( (char*)&(_ROTVEC.m_rotvec[2]), sizeof(double) ); 65 | 66 | #define LOAD_ROTVEC_FROM_STREAM( _STREAM, _ROTVEC ) \ 67 | _STREAM.read( (char*)&(_ROTVEC.m_coords[0]), sizeof(double) );\ 68 | _STREAM.read( (char*)&(_ROTVEC.m_coords[1]), sizeof(double) );\ 69 | _STREAM.read( (char*)&(_ROTVEC.m_coords[2]), sizeof(double) );\ 70 | _STREAM.read( (char*)&(_ROTVEC.m_rotvec[0]), sizeof(double) );\ 71 | _STREAM.read( (char*)&(_ROTVEC.m_rotvec[1]), sizeof(double) );\ 72 | _STREAM.read( (char*)&(_ROTVEC.m_rotvec[2]), sizeof(double) ); 73 | 74 | #define DUMP_VECTORLIKE(_v) \ 75 | if( _v.size() > 0 ) { \ 76 | for(size_t k = 0; k < _v.size()-1; ++k) \ 77 | cout << #_v << "[" << k << "] = " << _v[k] << ", "; \ 78 | cout << #_v << "[" << (_v.size()-1) << "] = " << *(_v.rbegin()) << endl; } 79 | 80 | #define UNINITIALIZED_TRACKED_NUMBER -1 81 | #define GENERATE_NAME_WITH_KF(STR) mrpt::format("%s\\%s_kf%04d.txt", general_options.out_dir.c_str(), #STR, this->m_kf_ID) 82 | #define GENERATE_NAME_WITH_2KF(STR,OKF_ID) mrpt::format("%s\\%s_kf%04d_with_kf%04d.txt", general_options.out_dir.c_str(), #STR, this->m_kf_ID, OKF_ID) 83 | #define GENERATE_NAME_WITH_KF_OUT(STR,KF) mrpt::format("%s\\%s_kf%04d.txt", general_options.out_dir.c_str(), #STR, KF.m_kf_ID) 84 | #define GENERATE_NAME_WITH_2KF_OUT(_STR,_ID1,_ID2) mrpt::format("%s\\%s_kf%04d_with_kf%04d.txt", general_options.out_dir.c_str(), #_STR, _ID1, _ID2) 85 | #define DUMP_BOOL_VAR_TO_CONSOLE(_MSG,_VAR) cout << _MSG; _VAR ? cout << "Yes " : cout << "No "; cout << endl; 86 | #define VERBOSE_LEVEL(_LEV) if( general_options.verbose_level >= _LEV ) std::cout// System KFs 87 | #define INVALID_KF_ID -1 88 | #define INVALID_IDX -1 // to do: take these two lines to the header file 89 | #define OUTLIER_ID -2 90 | 91 | // -- typedef 92 | 93 | // -- namespace 94 | using namespace mrpt; 95 | using namespace mrpt::opengl; 96 | using namespace cv; 97 | using namespace srba; 98 | -------------------------------------------------------------------------------- /src/srba-stereo-slam_main.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************************* 2 | ** SRBA-Stereo-SLAM ** 3 | ********************************************************************************** 4 | ** ** 5 | ** Copyright(c) 2015-2017, Jose Luis Blanco, University of Almeria ** 6 | ** Copyright(c) 2015-2017, Francisco-Angel Moreno ** 7 | ** MAPIR group, University of Malaga ** 8 | ** All right reserved 9 | ** ** 10 | ** This program is free software: you can redistribute it and/or modify ** 11 | ** it under the terms of the GNU General Public License (version 3) as ** 12 | ** published by the Free Software Foundation. ** 13 | ** ** 14 | ** This program is distributed in the hope that it will be useful, but ** 15 | ** WITHOUT ANY WARRANTY; without even the implied warranty of ** 16 | ** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the ** 17 | ** GNU General Public License for more details. ** 18 | ** ** 19 | ** You should have received a copy of the GNU General Public License ** 20 | ** along with this program. If not, see . ** 21 | ** ** 22 | **********************************************************************************/ 23 | #include "CSRBAStereoSLAMEstimator.h" // <-- this includes all the rest of needed 24 | //#include 25 | 26 | TGeneralOptions general_options; // global variable 27 | 28 | // ------------------------------------------------------ 29 | // MAIN 30 | // ------------------------------------------------------ 31 | int main(int argc, char **argv) 32 | { 33 | try 34 | { 35 | if( argc < 2 ) 36 | { 37 | cout << "Use: srba-stereo-slam configFile" << endl; 38 | return -1; 39 | } 40 | 41 | std::string INI_FILENAME(argv[1]); 42 | ASSERT_FILE_EXISTS_( INI_FILENAME ) 43 | CConfigFile config( INI_FILENAME ); 44 | 45 | // get general parameters 46 | general_options.loadFromConfigFile( config ); // general app options 47 | 48 | // create srba estimator 49 | CSRBAStereoSLAMEstimator srba_stereo_estimator; 50 | 51 | // and initialize it 52 | srba_stereo_estimator.initialize( config ); 53 | 54 | if( general_options.from_step != 0 && general_options.to_step != 0 && 55 | general_options.to_step < general_options.from_step ) 56 | THROW_EXCEPTION( "Parameter 'toStep' is lower than 'fromStep'" ); 57 | 58 | general_options.dumpToConsole(); 59 | srba_stereo_estimator.srba_options.dumpToConsole(); 60 | srba_stereo_estimator.performStereoSLAM(); // MAIN ENTRY 61 | cout << "SRBA Stereo SLAM process done!" << endl; 62 | 63 | // srba_stereo_estimator.saveOutputToFile(); 64 | 65 | return 0; 66 | } catch (exception &e) 67 | { 68 | cout << "MRPT exception caught: " << e.what() << endl; 69 | return -1; 70 | } 71 | catch (...) 72 | { 73 | printf("Untyped exception!!"); 74 | return -1; 75 | } 76 | } 77 | -------------------------------------------------------------------------------- /src/srba-stereo-slam_utils.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************************* 2 | ** SRBA-Stereo-SLAM ** 3 | ********************************************************************************** 4 | ** ** 5 | ** Copyright(c) 2015-2017, Jose Luis Blanco, University of Almeria ** 6 | ** Copyright(c) 2015-2017, Francisco-Angel Moreno ** 7 | ** MAPIR group, University of Malaga ** 8 | ** All right reserved 9 | ** ** 10 | ** This program is free software: you can redistribute it and/or modify ** 11 | ** it under the terms of the GNU General Public License (version 3) as ** 12 | ** published by the Free Software Foundation. ** 13 | ** ** 14 | ** This program is distributed in the hope that it will be useful, but ** 15 | ** WITHOUT ANY WARRANTY; without even the implied warranty of ** 16 | ** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the ** 17 | ** GNU General Public License for more details. ** 18 | ** ** 19 | ** You should have received a copy of the GNU General Public License ** 20 | ** along with this program. If not, see . ** 21 | ** ** 22 | **********************************************************************************/ 23 | #include "srba-stereo-slam_utils.h" 24 | #include "srba-stereo-slam_common.h" 25 | #include "srba-stereo-slam.h" 26 | 27 | extern TGeneralOptions general_options; 28 | 29 | // These are static methods (only available when the header file is included) 30 | // --------------------------------------------------- 31 | // comparison (auxiliary methods) 32 | // --------------------------------------------------- 33 | bool compareKeypointLists( 34 | const TKeyPointList & list1, 35 | const Mat & desc1, 36 | const TKeyPointList & list2, 37 | const Mat & desc2 ) 38 | { 39 | if( list1.size() != list2.size() ) 40 | return false; 41 | 42 | if( desc1.size() != desc2.size() ) 43 | return false; 44 | 45 | // keyp 46 | TKeyPointList::const_iterator it1, it2; 47 | for( it1 = list1.begin(), it2 = list2.begin(); it1 != list1.end(); ++it1, ++it2 ) 48 | { 49 | if( it1->pt.x != it2->pt.x || it1->pt.y != it2->pt.y || it1->response != it2->response || it1->angle != it2->angle || 50 | it1->class_id != it2->class_id || it1->octave != it2->octave || it1->size != it2->size ) 51 | return false; 52 | } 53 | 54 | // desc 55 | MatConstIterator_ itd1, itd2; 56 | for( itd1 = desc1.begin(), itd2 = desc2.begin(); itd1 != desc1.end(); ++itd1, ++itd2 ) // stream << *it; 57 | { 58 | if( *itd1 != *itd2 ) 59 | return false; 60 | } 61 | 62 | return true; 63 | } 64 | 65 | bool compareMatchesLists( 66 | const TDMatchList & list1, 67 | const TDMatchList & list2 ) 68 | { 69 | if( list1.size() != list2.size() ) 70 | return false; 71 | 72 | TDMatchList::const_iterator it1, it2; 73 | for( it1 = list1.begin(), it2 = list2.begin(); it1 != list1.end(); ++it1, ++it2 ) 74 | { 75 | if( it1->queryIdx != it2->queryIdx || it1->trainIdx != it2->trainIdx || it1->distance != it2->distance || it1->imgIdx != it2->imgIdx ) 76 | return false; 77 | } 78 | 79 | return true; 80 | } 81 | 82 | bool compareOptions( 83 | const TSRBAStereoSLAMOptions & opt1, 84 | const TSRBAStereoSLAMOptions & opt2 ) 85 | { 86 | return opt1.n_levels == opt2.n_levels && opt1.n_feats == opt2.n_feats && opt1.min_ORB_distance == opt2.min_ORB_distance && 87 | opt1.matching_options == opt2.matching_options && 88 | opt1.max_y_diff_epipolar == opt2.max_y_diff_epipolar && 89 | opt1.max_orb_distance_da == opt2.max_orb_distance_da && 90 | opt1.ransac_fit_prob == opt2.ransac_fit_prob && 91 | opt1.max_translation == opt2.max_translation && opt1.max_rotation == opt2.max_rotation && 92 | opt1.residual_th == opt2.residual_th && opt1.non_maximal_suppression == opt2.non_maximal_suppression && 93 | opt1.updated_matches_th == opt2.updated_matches_th && opt1.up_matches_th_plus == opt2.up_matches_th_plus && 94 | opt1.detect_method == opt2.detect_method && opt1.detect_fast_th == opt2.detect_fast_th && 95 | opt1.non_max_supp_method == opt2.non_max_supp_method; 96 | } 97 | 98 | // --------------------------------------------------- 99 | // show kf information 100 | // --------------------------------------------------- 101 | void show_kf_numbers( 102 | COpenGLScenePtr & scene, 103 | const size_t & num_kf, 104 | const DBoW2::QueryResults & ret, 105 | const double & th ) 106 | { 107 | CRenderizablePtr obj; 108 | COpenGLViewportPtr vp = scene->getViewport("keyframes"); 109 | for( size_t k = 0; k < ret.size(); ++k ) 110 | { 111 | obj = vp->getByName( mrpt::format("ret%d_score",k) ); 112 | if( obj ) 113 | { 114 | CTextPtr score_txt = static_cast(obj); 115 | score_txt->setString( mrpt::format("%.3f",ret[k].Score) ); 116 | score_txt->setVisibility(); 117 | } 118 | 119 | obj = vp->getByName( mrpt::format("ret%d_id",k) ); 120 | if( obj ) 121 | { 122 | CTextPtr id_txt = static_cast(obj); 123 | id_txt->setString( mrpt::format("%d",ret[k].Id) ); 124 | id_txt->setVisibility(); 125 | } 126 | 127 | obj = vp->getByName( mrpt::format("ret%d_box",k) ); 128 | if( obj ) 129 | { 130 | CBoxPtr box = static_cast(obj); 131 | box->setVisibility(); 132 | box->setBoxCorners(mrpt::math::TPoint3D(0.5*k,0,0.0),mrpt::math::TPoint3D(0.5*k+0.25,ret[k].Score,0.0)); 133 | box->setColor(mrpt::utils::TColorf(1-3*ret[k].Score,3*ret[k].Score,0)); 134 | } 135 | } 136 | 137 | obj = vp->getByName( "th_line" ); 138 | if( obj ) 139 | { 140 | CSimpleLinePtr line = static_cast(obj); 141 | line->setLineCoords(-0.1,th,0,-0.15+0.5*ret.size(),th,0); 142 | } 143 | 144 | obj = vp->getByName( "th_value" ); 145 | if( obj ) 146 | { 147 | CTextPtr txt = static_cast(obj); 148 | txt->setString( mrpt::format("%.2f",th) ); 149 | txt->setPose( CPoint3D( -0.25+0.5*ret.size(), th+0.1, 0.0 ) ); 150 | } 151 | } // end-show_kf_numbers 152 | 153 | double updateTranslationThreshold( 154 | const double x, 155 | const double th ) 156 | { 157 | double newTh = 0.02 + (0.25/th)*x; 158 | newTh = newTh < 0.02 ? 0.02 : newTh; 159 | newTh = newTh > 0.3 ? 0.3 : newTh; 160 | return newTh; 161 | } // end -- updateTranslationThreshold 162 | 163 | double updateRotationThreshold( 164 | const double x, 165 | const double th ) 166 | { 167 | double newTh = 15 + 13/th*(x-th); 168 | newTh = newTh < 2 ? 2 : newTh; 169 | newTh = newTh > 15 ? 15 : newTh; 170 | return newTh; 171 | } // end -- updateTranslationThreshold 172 | 173 | /*------------------------------------------------------------ 174 | Checks the results of a DB query and search for potential 175 | loop closures, returning true if one is found. It also 176 | returns the IDs of the most similar keyframes. 177 | -------------------------------------------------------------*/ 178 | bool getSimilarKfs( 179 | const TKeyFrameID & newKfId, 180 | const DBoW2::QueryResults & dbQueryResults, 181 | mySRBA & rba, 182 | const TSRBAStereoSLAMOptions & stereoSlamOptions, 183 | TLoopClosureInfo & out ) 184 | { 185 | if( general_options.verbose_level >= 2 ) 186 | cout << "dbQueryResults: " << dbQueryResults << endl; 187 | 188 | const size_t qSize = dbQueryResults.size(); 189 | if( qSize == 0 ) 190 | THROW_EXCEPTION( "Parameter 'dbQueryResults' contains no results. This method should not be called here." ); 191 | 192 | if( qSize == 1 ) 193 | { 194 | out.similar_kfs.push_back( newKfId-1 ); 195 | return false; 196 | } 197 | 198 | if( dbQueryResults[0].Score < 0.04 /* TODO: absoluteDbQueryThreshold */ ) 199 | { 200 | SHOW_WARNING( "Best result in 'dbQueryResults' is below a threshold. Lost camera?" ); 201 | } 202 | 203 | // prepare output 204 | out.similar_kfs.clear(); 205 | out.similar_kfs.reserve( qSize+1 ); 206 | out.lc_id = INVALID_KF_ID; 207 | bool foundLoopClosure = false; 208 | 209 | // always insert last kf as a similar one 210 | out.similar_kfs.push_back( newKfId-1 ); 211 | 212 | // we've got enough good data, let's find the loop closure 213 | mySRBA::rba_problem_state_t & myRbaState = rba.get_rba_state(); 214 | 215 | // we've got a LC if in the list there is any far KF with a score large enough 216 | // if last inserted kf is a base, then use it, if not, use the previous one 217 | /** / 218 | const TKeyFrameID fromIdBase = 219 | rba.isKFLocalmapCenter( newKfId-1 ) ? 220 | newKfId-1 : 221 | rba.getLocalmapCenterID( newKfId-1 ); // get id of the last localmap center 222 | /**/ 223 | const size_t SUBMAP_SIZE = rba.parameters.ecp.submap_size; // In # of KFs 224 | const TKeyFrameID fromIdBase = SUBMAP_SIZE*((newKfId-1)/SUBMAP_SIZE); 225 | 226 | mySRBA::rba_problem_state_t::TSpanningTree::next_edge_maps_t::const_iterator itFrom = 227 | myRbaState.spanning_tree.sym.next_edge.find( fromIdBase ); // get spanning tree for the current localmap center 228 | 229 | // check the results 230 | const double loopClosureTh = 0.8*dbQueryResults[0].Score; 231 | for( size_t i = 0; i < dbQueryResults.size(); ++i ) 232 | { 233 | const TKeyFrameID toId = dbQueryResults[i].Id; 234 | 235 | if( toId == newKfId-1 ) // already inserted 236 | continue; 237 | 238 | // compute topologic distance 239 | topo_dist_t topoDistance = numeric_limits::max(); 240 | 241 | if( fromIdBase == toId ) 242 | topoDistance = 0; 243 | else 244 | { 245 | if( itFrom != myRbaState.spanning_tree.sym.next_edge.end() ) 246 | { 247 | map::const_iterator itToDist = itFrom->second.find( toId ); 248 | 249 | if( itToDist != itFrom->second.end() ) 250 | topoDistance = itToDist->second.distance; 251 | } 252 | else 253 | { 254 | // *** This shouldn't never happen *** 255 | THROW_EXCEPTION("[ERROR :: Check Loop Closure] 'it_from' is not into the spanning_tree!"); 256 | } 257 | } 258 | bool insertKf = false; 259 | if( topoDistance > stereoSlamOptions.lc_distance ) 260 | { 261 | // only set the lc with the first KF found 262 | if( dbQueryResults[i].Score > 0.05 && out.lc_id == INVALID_KF_ID) 263 | { 264 | out.lc_id = toId; 265 | foundLoopClosure = true; 266 | insertKf = true; 267 | VERBOSE_LEVEL(1) << " FOUND POTENTIAL LOOP CLOSURE " << endl; 268 | } 269 | } 270 | else 271 | { 272 | if( dbQueryResults[i].Score > loopClosureTh ) 273 | insertKf = true; 274 | } 275 | if( insertKf ) 276 | { 277 | // :: set this KF as similar 278 | out.similar_kfs.push_back( toId ); 279 | } 280 | 281 | VERBOSE_LEVEL(2) << " Distance from " << fromIdBase 282 | << " to " << toId << ":" << topoDistance << endl; 283 | } // end-for 284 | 285 | // ***** for all similar KFs, get a rough estimation of THIS pose wrt to them 286 | 287 | // prepare similar poses output 288 | out.similar_kfs_poses.resize( out.similar_kfs.size() ); 289 | 290 | // search along the spantree for the poses: 291 | mySRBA::frameid2pose_map_t spantree; 292 | rba.create_complete_spanning_tree(newKfId-1, spantree, rba.parameters.srba.max_tree_depth ); 293 | for( size_t k = 0; k < out.similar_kfs.size(); ++k ) 294 | { 295 | mySRBA::frameid2pose_map_t::const_iterator itP = spantree.find( out.similar_kfs[k] ); 296 | if( itP == spantree.end() ) 297 | out.similar_kfs_poses[k] = CPose3D(); 298 | else 299 | { 300 | out.similar_kfs_poses[k] = itP->second.pose; 301 | out.similar_kfs_poses[k].inverse(); 302 | } 303 | } 304 | 305 | // DEBUG ------------------------------------------ 306 | if( general_options.verbose_level >= 2 ) 307 | { 308 | DUMP_VECTORLIKE( out.similar_kfs ) 309 | } 310 | // ------------------------------------------------ 311 | 312 | return foundLoopClosure; 313 | } // end -- getSimilarKfs 314 | 315 | // ---------------------------------------------------------- 316 | // checks if there is a loop closure (according to the query database) and the RBA state 317 | // returns true if in 'ret' there is a topologically FAR keyframe strong enough (more than 80% of the best result) 318 | // ---------------------------------------------------------- 319 | LCResult checkLoopClosure( 320 | const TKeyFrameID & new_kf_id, 321 | const DBoW2::QueryResults & ret, 322 | mySRBA & rba, 323 | const TSRBAStereoSLAMOptions & stereo_slam_options, 324 | TLoopClosureInfo & lc_info ) 325 | { 326 | // preliminary checks 327 | if( ret.size() < 4 ) 328 | { 329 | lc_info.similar_kfs.resize(1); 330 | lc_info.similar_kfs[0] = new_kf_id-1; 331 | return lcr_NOT_ENOUGH_DATA; // at least 4 results, return just the last one 332 | } 333 | 334 | if( ret[0].Score < 0.04 ) 335 | { 336 | lc_info.similar_kfs.resize(1); 337 | lc_info.similar_kfs[0] = new_kf_id-1; 338 | return lcr_BAD_DATA; // none of them is over the minimal threshold -- lost camera? 339 | } 340 | 341 | /**/ 342 | if( ret[0].Score < 0.10 ) 343 | { 344 | lc_info.similar_kfs.resize(1); 345 | lc_info.similar_kfs[0] = new_kf_id-1; 346 | return lcr_NO_LC; // none of them is over the minimal threshold -- lost camera? 347 | } 348 | /**/ 349 | 350 | // prepare output 351 | lc_info.similar_kfs.clear(); 352 | lc_info.similar_kfs.reserve( ret.size()+1 ); 353 | 354 | // we've got enough good data, let's find the loop closure 355 | mySRBA::rba_problem_state_t & my_rba_state = rba.get_rba_state(); 356 | 357 | // we've got a LC if in the list there is any far KF with a score large enough 358 | // if last inserted kf is a base, then use it, if not, use the previous one 359 | const TKeyFrameID from_id_base = 360 | rba.isKFLocalmapCenter( new_kf_id-1 ) ? 361 | new_kf_id-1 : 362 | rba.getLocalmapCenterID( new_kf_id-1 ); // get id of the last localmap center 363 | 364 | mySRBA::rba_problem_state_t::TSpanningTree::next_edge_maps_t::const_iterator it_from = my_rba_state.spanning_tree.sym.next_edge.find(from_id_base); // get spanning tree for the current localmap center 365 | 366 | // ------------------------------------------------------- 367 | bool found_lc = false; 368 | const double threshold = 0.8*ret[0].Score; 369 | for( size_t k = 0; k < ret.size(); ++k ) 370 | { 371 | if( ret[k].Score < threshold ) break; // at least the highest one will pass this filter 372 | 373 | // get topographic distance between keyframes 374 | const TKeyFrameID to_id = ret[k].Id; 375 | 376 | topo_dist_t found_distance = numeric_limits::max(); 377 | 378 | if( it_from != my_rba_state.spanning_tree.sym.next_edge.end() ) 379 | { 380 | map::const_iterator it_to_dist = it_from->second.find(to_id); 381 | if (it_to_dist != it_from->second.end()) 382 | found_distance = it_to_dist->second.distance; 383 | } 384 | else 385 | { 386 | // *** This shouldn't never happen *** 387 | cout << " [ERROR :: Check Loop Closure] 'it_from' is not into the spanning_tree!" << endl; 388 | } 389 | 390 | // increment distance due to the edge from current KF to last localmap center 391 | if( found_distance < numeric_limits::max() ) 392 | found_distance++; 393 | 394 | lc_info.similar_kfs.push_back(to_id); 395 | if( found_distance > stereo_slam_options.lc_distance ) 396 | { 397 | found_lc = true; 398 | lc_info.lc_id = ret[k].Id; 399 | } 400 | 401 | // debug --------------------------------- 402 | cout << " [DEBUG :: Check Loop Closure] Topologic dist. from: " << new_kf_id << " to " << to_id << " is: " << found_distance << endl; 403 | // ------------------------------------------------------- 404 | 405 | } // end-for 406 | 407 | /**/ 408 | // if last KF is not inserted --> insert it 409 | if( lc_info.similar_kfs.end() == std::find(lc_info.similar_kfs.begin(), lc_info.similar_kfs.end(), new_kf_id-1) ) 410 | lc_info.similar_kfs.push_back( new_kf_id-1 ); 411 | /**/ 412 | return found_lc ? lcr_FOUND_LC : lcr_NO_LC; 413 | } // end-checkLoopClosure 414 | -------------------------------------------------------------------------------- /src/srba-stereo-slam_utils.h: -------------------------------------------------------------------------------- 1 | /********************************************************************************* 2 | ** SRBA-Stereo-SLAM ** 3 | ********************************************************************************** 4 | ** ** 5 | ** Copyright(c) 2015-2017, Jose Luis Blanco, University of Almeria ** 6 | ** Copyright(c) 2015-2017, Francisco-Angel Moreno ** 7 | ** MAPIR group, University of Malaga ** 8 | ** All right reserved 9 | ** ** 10 | ** This program is free software: you can redistribute it and/or modify ** 11 | ** it under the terms of the GNU General Public License (version 3) as ** 12 | ** published by the Free Software Foundation. ** 13 | ** ** 14 | ** This program is distributed in the hope that it will be useful, but ** 15 | ** WITHOUT ANY WARRANTY; without even the implied warranty of ** 16 | ** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the ** 17 | ** GNU General Public License for more details. ** 18 | ** ** 19 | ** You should have received a copy of the GNU General Public License ** 20 | ** along with this program. If not, see . ** 21 | ** ** 22 | **********************************************************************************/ 23 | #pragma once 24 | 25 | // bag of words 26 | #include "DBoW2.h" // defines QueryResults 27 | 28 | // opencv 29 | #include 30 | #include 31 | #include 32 | #include 33 | 34 | // #include "srba-stereo-slam.h" 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | 42 | #include "srba-stereo-slam_common.h" 43 | 44 | using namespace mrpt; 45 | using namespace mrpt::poses; 46 | 47 | typedef std::vector TKeyPointList; 48 | typedef std::vector TDMatchList; 49 | 50 | enum LCResult { lcr_NO_LC, lcr_FOUND_LC, lcr_BAD_DATA, lcr_NOT_ENOUGH_DATA }; //!< Enum for defining the result of loop closure search 51 | 52 | /********************************************* 53 | STRUCT: Data association information 54 | **********************************************/ 55 | typedef struct t_kf_da_info { 56 | size_t kf_idx; //!< Index of the 'other' KF 57 | size_t tracked_matches; //!< Number of tracked matches with the 'other' KF 58 | vector< pair< size_t, double > > tracking_info; //!< Pair of 'other_match_idx' -> 'mean_distance'. Vector size == number of current matches 59 | 60 | t_kf_da_info() : 61 | kf_idx(INVALID_KF_ID), 62 | tracked_matches(0), 63 | tracking_info( vector< pair< size_t, double > >() ) 64 | {} 65 | 66 | } t_kf_da_info; 67 | typedef vector TVectorKfsDaInfo; // Vector of DA information, one for each KEYFRAME compared with the current one 68 | 69 | /********************************************* 70 | STRUCT: Loop closure information 71 | **********************************************/ 72 | typedef struct TLoopClosureInfo { 73 | vector similar_kfs; //!< A vector containing the ids of the KFs similar to this one 74 | TKeyFrameID lc_id; //!< In case there is loop closure, here is the target KF's ID 75 | vector similar_kfs_poses; //!< A vector containing the poses of the similar KFs wrt this one 76 | 77 | TLoopClosureInfo() : 78 | similar_kfs( vector() ), 79 | lc_id(INVALID_KF_ID), 80 | similar_kfs_poses(vector()) 81 | {} 82 | } TLoopClosureInfo; 83 | 84 | /********************************************* 85 | STRUCT: Application options 86 | **********************************************/ 87 | typedef struct TGeneralOptions 88 | { 89 | enum captureSource { csRawlog, csImgDir }; 90 | captureSource cap_src; //!< [int] (def:1) -- Image source: [0] Rawlog file ; [1] Image directory 91 | 92 | int from_step, //!< [int] (def:0) -- Number of the first frame to process 93 | to_step, //!< [int] (def:0 -unlimited-) -- Number of the last frame to process 94 | save_at_iteration, //!< [int] (def:0) -- Iteration where to save the state (TO DO) 95 | max_num_kfs, //!< [int] (def:0 -unlimited-) -- Maximum number of KFs to be inserted in the system (app will finish when reached) 96 | start_index, //!< [int] (def:0) -- Input configuration (Image dir): starting image 97 | end_index, //!< [int] (def:0 -unlimited-) -- Input configuration (Image dir): ending image 98 | verbose_level; //!< [int] (def:0) -- Verbose level: [0] None ; [1] Important ; [2] More info 99 | 100 | bool debug, //!< [bool] (def:false) -- Store and show some debugging information 101 | show3D, //!< [bool] (def:false) -- Show information GUI 102 | enable_logger, //!< [bool] (def:false) -- Enable time logger for certain operations (for debugging). Time info will be shown at the program's end. 103 | load_state_from_file, //!< [bool] (def:false) -- Load application state from file (TO DO) 104 | save_state_to_file, //!< [bool] (def:false) -- Save application state to file (TO DO) 105 | pause_after_show_op, //!< [bool] (def:false) -- Pause application after showing parameters 106 | pause_at_each_iteration; //!< [bool] (def:false) -- Pause application after each iteration 107 | 108 | string out_dir, //!< [string] (def:'') -- Application output folder 109 | rawlog_file, //!< [string] (def:'') -- Rawlog file path 110 | state_file, //!< [string] (def:'') -- File where to save/load the application state (TO DO) 111 | image_dir_url, //!< [string] (def:'') -- Image folder path 112 | left_format, //!< [string] (def:'') -- Left image filename format 113 | right_format; //!< [string] (def:'') -- Right image filename format 114 | 115 | /** Default constructor */ 116 | TGeneralOptions() : 117 | cap_src(csImgDir), 118 | from_step(0), 119 | to_step(0), 120 | save_at_iteration(0), 121 | max_num_kfs(0), 122 | start_index(0), 123 | end_index(0), 124 | verbose_level(0), 125 | debug(false), 126 | show3D(false), 127 | enable_logger(false), 128 | load_state_from_file(false), 129 | save_state_to_file(false), 130 | pause_after_show_op(false), 131 | pause_at_each_iteration(false), 132 | out_dir(""), 133 | rawlog_file(""), 134 | state_file(""), 135 | image_dir_url(""), 136 | left_format(""), 137 | right_format("") 138 | {} 139 | 140 | /** Load data from config file */ 141 | void loadFromConfigFile( const mrpt::utils::CConfigFile & config ) 142 | { 143 | MRPT_LOAD_CONFIG_VAR(pause_after_show_op,bool,config,"APP_OPTIONS") 144 | 145 | MRPT_LOAD_CONFIG_VAR(out_dir,string,config,"APP_OPTIONS") 146 | MRPT_LOAD_CONFIG_VAR(debug,bool,config,"APP_OPTIONS") 147 | MRPT_LOAD_CONFIG_VAR(show3D,bool,config,"APP_OPTIONS") 148 | MRPT_LOAD_CONFIG_VAR(enable_logger,bool,config,"APP_OPTIONS") 149 | 150 | MRPT_LOAD_CONFIG_VAR(verbose_level,int,config,"APP_OPTIONS") 151 | MRPT_LOAD_CONFIG_VAR(pause_at_each_iteration,bool,config,"APP_OPTIONS") 152 | 153 | MRPT_LOAD_CONFIG_VAR(from_step,int,config,"APP_OPTIONS") 154 | MRPT_LOAD_CONFIG_VAR(to_step,int,config,"APP_OPTIONS") 155 | MRPT_LOAD_CONFIG_VAR(max_num_kfs,int,config,"APP_OPTIONS") 156 | 157 | MRPT_LOAD_CONFIG_VAR(save_state_to_file,bool,config,"APP_OPTIONS") 158 | MRPT_LOAD_CONFIG_VAR(save_at_iteration,int,config,"APP_OPTIONS") 159 | MRPT_LOAD_CONFIG_VAR(state_file,string,config,"APP_OPTIONS") 160 | if( save_state_to_file ) 161 | load_state_from_file = false; 162 | else 163 | { 164 | MRPT_LOAD_CONFIG_VAR(load_state_from_file,bool,config,"APP_OPTIONS") 165 | } 166 | 167 | int aux = config.read_int("APP_OPTIONS","capture_source",cap_src); 168 | switch(aux) 169 | { 170 | case 0 : cap_src = csRawlog; break; 171 | case 1 : default: cap_src = csImgDir; break; 172 | } 173 | 174 | MRPT_LOAD_CONFIG_VAR(rawlog_file,string,config,"IMG_SOURCE") 175 | 176 | MRPT_LOAD_CONFIG_VAR(image_dir_url,string,config,"IMG_SOURCE") 177 | MRPT_LOAD_CONFIG_VAR(left_format,string,config,"IMG_SOURCE") 178 | MRPT_LOAD_CONFIG_VAR(right_format,string,config,"IMG_SOURCE") 179 | MRPT_LOAD_CONFIG_VAR(start_index,int,config,"IMG_SOURCE") 180 | MRPT_LOAD_CONFIG_VAR(end_index,int,config,"IMG_SOURCE") 181 | 182 | } // end-loadFromConfigFile 183 | 184 | /** Show options */ 185 | void dumpToConsole( ) 186 | { 187 | cout << "---------------------------------------------------------" << endl; 188 | cout << " Application options" << endl; 189 | cout << "---------------------------------------------------------" << endl; 190 | if( cap_src == csRawlog ) 191 | cout << " :: Rawlog file: " << endl << " " << rawlog_file << endl; 192 | else if( cap_src == csImgDir ) 193 | { 194 | cout << " :: Image directory: " << endl << " " << image_dir_url << endl; 195 | cout << " :: Left image format: " << left_format << endl; 196 | cout << " :: Right image format: " << right_format << endl; 197 | cout << " :: Start index: " << start_index << endl; 198 | cout << " :: End index: " << end_index << endl; 199 | } 200 | 201 | cout << " :: Steps: From " << from_step << " to " << to_step << endl; 202 | cout << " :: Max number of keyframes "; max_num_kfs > 0 ? cout << max_num_kfs : cout << " unlimited"; cout << endl; 203 | DUMP_BOOL_VAR_TO_CONSOLE(" :: Debug?: ", debug) 204 | DUMP_BOOL_VAR_TO_CONSOLE(" :: Show3D?: ", show3D) 205 | DUMP_BOOL_VAR_TO_CONSOLE(" :: Enable time logger?: ", enable_logger) 206 | cout << " :: Output directory: '" << out_dir << "'" << endl; 207 | DUMP_BOOL_VAR_TO_CONSOLE(" :: Load state from file?: ", load_state_from_file) 208 | DUMP_BOOL_VAR_TO_CONSOLE(" :: Save state to file?: ", save_state_to_file) 209 | DUMP_BOOL_VAR_TO_CONSOLE(" :: Pause at each iteration?: ", pause_at_each_iteration) 210 | 211 | if( load_state_from_file || save_state_to_file) cout << " :: State file: " << state_file << endl; 212 | 213 | if( pause_after_show_op ) system::pause(); 214 | } 215 | 216 | } TGeneralOptions; 217 | 218 | /********************************************* 219 | STRUCT: SRBA Stereo SLAM options 220 | **********************************************/ 221 | typedef struct TSRBAStereoSLAMOptions 222 | { 223 | // -- declare enum types 224 | enum TDetectMethod { DM_ORB_ONLY = 0, DM_FAST_ORB }; 225 | enum TNonMaxSuppMethod { NMSM_STANDARD = 0, NMSM_ADAPTIVE }; 226 | enum TDAStage2Method { ST2M_NONE = 0, ST2M_FUNDMATRIX, ST2M_CHANGEPOSE, ST2M_BOTH }; 227 | 228 | utils::TStereoCamera stereo_camera; //!< The stereo camera parameters 229 | 230 | CPose3DRotVec camera_pose_on_robot_rvt, 231 | camera_pose_on_robot_rvt_inverse; 232 | 233 | TDetectMethod detect_method; //!< (def:ORB) -- Feature extraction method for SRBA system 234 | 235 | // detect 236 | size_t n_levels, //!< (def:1) -- Number of levels in the image pyramid -- fixed by now 237 | n_feats; //!< (def:500) -- Desired number of feats to be detected in the images 238 | 239 | int min_ORB_distance, //!< (def:0) For non-max-suppression 240 | detect_fast_th, //!< (def:5) -- Initial FAST Threshold for ORB keypoints (will be adaptad if 'orb_adaptive_fast_th' is true) 241 | adaptive_th_min_matches; //!< (def:100) -- Minimum number of stereo matches to force adaptation of FAST and/or ORB thresholds. 242 | 243 | bool orb_adaptive_fast_th; //!< (def:false) -- Set/Unset adaptive FAST threshold (within ORB method) to get the desired number of feats 244 | 245 | TNonMaxSuppMethod non_max_supp_method; //!< (def:standard) Method to perform nom maximal suppression 246 | 247 | vision::TMatchingOptions matching_options; //!< Matching options 248 | 249 | // inter-frame match 250 | double ransac_fit_prob, //!< (def:0.95) -- Filtering by fundamental matrix (RANSAC fit threshold) 251 | max_y_diff_epipolar, //!< (def:1.5) -- Filtering by fundamental matrix (epipolar threshold) 252 | max_orb_distance_da; //!< (def:60) -- Maximum ORB distance for data association 253 | 254 | // least-squares 255 | TDAStage2Method da_stage2_method; //!< (def:2) -- Method for filtering outliers during second stage of DA (after ORB matching): [0] None ; [1] Fundamental Matrix; [2] Change in pose only ; [3] Both 256 | double query_score_th; //!< (def:0.04) -- Minimum allowed query value for the most similar KF (will raise an error if it falls below this) 257 | 258 | bool use_initial_pose; //!< (def:true) -- Use an initial estimation of the position of this KF taken from the odometry 259 | int vo_id_tracking_th; //!< (def:40) -- Threshold for the number of tracked features from last KF 260 | 261 | // da-filters 262 | bool da_filter_by_direction, //!< (def:false) -- Filter DA matches by their direction 263 | da_filter_by_orb_distance, //!< (def:true) -- Filter DA matches by their ORB distance 264 | da_filter_by_fund_matrix, //!< (def:true) -- Filter DA matches by computing left-left fundamental matrix and checking distance to epipolar lines 265 | da_filter_by_pose_change; //!< (def:true) -- Filter DA matches by computing pose change and checking reprojection errors 266 | 267 | // general 268 | double residual_th, 269 | max_rotation, //!< (def:15) -- Rotation limit for checking new KFs (in degrees) 270 | max_translation, //!< (def:0.3) -- Translation limit for checking new KFs (in meters) 271 | srba_kernel_param; //!< (def:3.0) -- Pseudo-huber kernel param for least-squares optimization. 272 | 273 | bool non_maximal_suppression, //!< (def:false) -- Perform non-maximal-suppression 274 | pause_after_show_op, //!< (def:false) -- Pause after showing parameters 275 | srba_use_robust_kernel, //!< (def:true) -- Use robust kernel for optimization 276 | srba_use_robust_kernel_stage1; //!< (def:true) -- Use robust kernel for optimization in SRBA stage 1 277 | 278 | size_t updated_matches_th, //!< (def:50) -- Minimum number of tracked matches to insert a new KF after DA 279 | up_matches_th_plus, //!< (def:25) -- This+'updated_matches_th' sets the minimum number of tracked matches to define new KF checking (geometrical) limits 280 | lc_distance, //!< (def:2) -- Minimum distance between KFs to consider a loop closure 281 | srba_submap_size, //!< (def:15) -- Number of KFs within submaps 282 | srba_max_tree_depth, //!< (def:3) -- Maximum depth to keep spanning trees 283 | srba_max_optimize_depth; //!< (def:3) -- Maximum depth to optimize the graph 284 | 285 | /** Default constructor */ 286 | TSRBAStereoSLAMOptions() : 287 | detect_method( DM_ORB_ONLY ), 288 | n_levels(1), 289 | n_feats(500), 290 | min_ORB_distance(0), 291 | detect_fast_th(5), 292 | adaptive_th_min_matches(100), 293 | orb_adaptive_fast_th(false), 294 | non_max_supp_method( NMSM_STANDARD ), 295 | ransac_fit_prob(0.95), 296 | max_y_diff_epipolar(1.5), 297 | max_orb_distance_da(60), 298 | da_stage2_method( ST2M_CHANGEPOSE ), 299 | query_score_th(0.04), 300 | use_initial_pose(true), 301 | vo_id_tracking_th(40), 302 | da_filter_by_direction(false), 303 | da_filter_by_orb_distance(true), 304 | da_filter_by_fund_matrix(true), 305 | da_filter_by_pose_change(true), 306 | residual_th(50), 307 | max_rotation(15.), 308 | max_translation(0.30), 309 | srba_kernel_param(3.0), 310 | non_maximal_suppression(false), 311 | pause_after_show_op (false), 312 | srba_use_robust_kernel(true), 313 | srba_use_robust_kernel_stage1(true), 314 | updated_matches_th(50), 315 | up_matches_th_plus(25), 316 | lc_distance(2), 317 | srba_submap_size(15), 318 | srba_max_tree_depth(3), 319 | srba_max_optimize_depth(3) 320 | {} 321 | 322 | /** Copy operator */ 323 | void operator=( const TSRBAStereoSLAMOptions & o ) 324 | { 325 | camera_pose_on_robot_rvt = o.camera_pose_on_robot_rvt; 326 | camera_pose_on_robot_rvt_inverse = o.camera_pose_on_robot_rvt_inverse; 327 | detect_method = o.detect_method; 328 | n_levels = o.n_levels; 329 | n_feats = o.n_feats; 330 | min_ORB_distance = o.min_ORB_distance; 331 | detect_fast_th = o.detect_fast_th; 332 | adaptive_th_min_matches = o.adaptive_th_min_matches; 333 | orb_adaptive_fast_th = o.orb_adaptive_fast_th; 334 | non_max_supp_method = o.non_max_supp_method; 335 | ransac_fit_prob = o.ransac_fit_prob; 336 | max_y_diff_epipolar = o.max_y_diff_epipolar; 337 | max_orb_distance_da = o.max_orb_distance_da; 338 | da_stage2_method = o.da_stage2_method; 339 | query_score_th = o.query_score_th; 340 | use_initial_pose = o.use_initial_pose; 341 | vo_id_tracking_th = o.vo_id_tracking_th; 342 | da_filter_by_direction = o.da_filter_by_direction; 343 | da_filter_by_orb_distance = o.da_filter_by_orb_distance; 344 | da_filter_by_fund_matrix = o.da_filter_by_fund_matrix; 345 | da_filter_by_pose_change = o.da_filter_by_pose_change; 346 | residual_th = o.residual_th; 347 | max_rotation = o.max_rotation; 348 | max_translation = o.max_translation; 349 | srba_kernel_param = o.srba_kernel_param; 350 | non_maximal_suppression = o.non_maximal_suppression; 351 | pause_after_show_op = o.pause_after_show_op; 352 | srba_use_robust_kernel = o.srba_use_robust_kernel; 353 | srba_use_robust_kernel_stage1 = o.srba_use_robust_kernel_stage1; 354 | updated_matches_th = o.updated_matches_th; 355 | up_matches_th_plus = o.up_matches_th_plus; 356 | lc_distance = o.lc_distance; 357 | srba_submap_size = o.srba_submap_size; 358 | srba_max_tree_depth = o.srba_max_tree_depth; 359 | srba_max_optimize_depth = o.srba_max_optimize_depth; 360 | } 361 | 362 | /** Load options from an .ini file */ 363 | void loadFromConfigFile( const mrpt::utils::CConfigFile & config ) 364 | { 365 | // stereo camera 366 | stereo_camera.loadFromConfigFile("CAMERA",config); // will be used for both the SRBA and the Visual Odometry engines 367 | 368 | // general parameters 369 | MRPT_LOAD_CONFIG_VAR(pause_after_show_op,bool,config,"SRBA_GENERAL") 370 | MRPT_LOAD_CONFIG_VAR(srba_max_tree_depth,int,config,"SRBA_GENERAL") 371 | MRPT_LOAD_CONFIG_VAR(srba_max_optimize_depth,int,config,"SRBA_GENERAL") 372 | MRPT_LOAD_CONFIG_VAR(srba_submap_size,int,config,"SRBA_GENERAL") 373 | MRPT_LOAD_CONFIG_VAR(srba_use_robust_kernel,bool,config,"SRBA_GENERAL") 374 | MRPT_LOAD_CONFIG_VAR(srba_use_robust_kernel_stage1,bool,config,"SRBA_GENERAL") 375 | MRPT_LOAD_CONFIG_VAR(srba_kernel_param,double,config,"SRBA_GENERAL") 376 | 377 | // keypoints detection 378 | MRPT_LOAD_CONFIG_VAR(n_feats,int,config,"SRBA_DETECT") 379 | MRPT_LOAD_CONFIG_VAR(orb_adaptive_fast_th,bool,config,"SRBA_DETECT") 380 | MRPT_LOAD_CONFIG_VAR(detect_fast_th,int,config,"SRBA_DETECT") 381 | MRPT_LOAD_CONFIG_VAR(adaptive_th_min_matches,int,config,"SRBA_DETECT") 382 | 383 | // data association 384 | int aux = config.read_int("SRBA_DATA_ASSOCIATION","da_stage2_method",da_stage2_method,false); 385 | switch( aux ) 386 | { 387 | case 0 : default : da_stage2_method = ST2M_NONE; break; 388 | case 1 : da_stage2_method = ST2M_FUNDMATRIX; break; 389 | case 2 : da_stage2_method = ST2M_CHANGEPOSE; break; 390 | case 3 : da_stage2_method = ST2M_BOTH; break; 391 | } 392 | MRPT_LOAD_CONFIG_VAR(residual_th,double,config,"SRBA_DATA_ASSOCIATION") 393 | MRPT_LOAD_CONFIG_VAR(max_y_diff_epipolar,double,config,"SRBA_DATA_ASSOCIATION") 394 | MRPT_LOAD_CONFIG_VAR(ransac_fit_prob,double,config,"SRBA_DATA_ASSOCIATION") 395 | MRPT_LOAD_CONFIG_VAR(max_orb_distance_da,double,config,"SRBA_DATA_ASSOCIATION") 396 | MRPT_LOAD_CONFIG_VAR(query_score_th,double,config,"SRBA_DATA_ASSOCIATION") 397 | 398 | MRPT_LOAD_CONFIG_VAR(da_filter_by_direction,bool,config,"SRBA_DATA_ASSOCIATION") 399 | MRPT_LOAD_CONFIG_VAR(da_filter_by_orb_distance,bool,config,"SRBA_DATA_ASSOCIATION") 400 | MRPT_LOAD_CONFIG_VAR(da_filter_by_fund_matrix,bool,config,"SRBA_DATA_ASSOCIATION") 401 | MRPT_LOAD_CONFIG_VAR(da_filter_by_pose_change,bool,config,"SRBA_DATA_ASSOCIATION") 402 | 403 | // new kf creation 404 | MRPT_LOAD_CONFIG_VAR(max_rotation,double,config,"SRBA_KF_CREATION") 405 | MRPT_LOAD_CONFIG_VAR(max_translation,double,config,"SRBA_KF_CREATION") 406 | MRPT_LOAD_CONFIG_VAR(updated_matches_th,int,config,"SRBA_KF_CREATION") 407 | MRPT_LOAD_CONFIG_VAR(up_matches_th_plus,int,config,"SRBA_KF_CREATION") 408 | 409 | MRPT_LOAD_CONFIG_VAR(lc_distance,int,config,"SRBA_KF_CREATION") 410 | MRPT_LOAD_CONFIG_VAR(vo_id_tracking_th,int,config,"SRBA_KF_CREATION") 411 | MRPT_LOAD_CONFIG_VAR(use_initial_pose,bool,config,"SRBA_KF_CREATION") 412 | 413 | // detect_method = config.read_int ("SRBA","srba_detect_method",detect_method,false) == 0 ? DM_ORB_ONLY : DM_FAST_ORB; 414 | // MRPT_LOAD_CONFIG_VAR(n_levels,int,config,"SRBA_DETECT") // <- by now, will be 1 for only ORB 415 | // MRPT_LOAD_CONFIG_VAR(non_maximal_suppression,bool,config,"DETECT") // <- for visual odometry 416 | 417 | // matching_options.loadFromConfigFile( config, "MATCH" ); // <- for stereo matching, LIKELY TO BE DELETED 418 | // MRPT_LOAD_CONFIG_VAR(min_ORB_distance,int,config,"DETECT") // UNUSED 419 | // non_max_supp_method = config.read_int ("DETECT","non_max_supp_method",non_max_supp_method,false) == 0 ? NMSM_STANDARD : NMSM_ADAPTIVE; 420 | 421 | } // end loadFromConfigFile 422 | 423 | /** Show options on the console */ 424 | void dumpToConsole( ) 425 | { 426 | cout << "---------------------------------------------------------" << endl; 427 | cout << " Stereo SLAM system with the following options" << endl; 428 | cout << "---------------------------------------------------------" << endl; 429 | 430 | // General options 431 | cout << " [General] " << endl; 432 | cout << " Max tree depth: " << srba_max_tree_depth << endl; 433 | cout << " Max optimization depth: " << srba_max_optimize_depth << endl; 434 | cout << " Submap size: " << srba_submap_size << endl; 435 | DUMP_BOOL_VAR_TO_CONSOLE(" Use robust kernel in optimization (stage 1): ", srba_use_robust_kernel_stage1) 436 | DUMP_BOOL_VAR_TO_CONSOLE(" Use robust kernel in optimization: ", srba_use_robust_kernel) 437 | if( srba_use_robust_kernel_stage1 || srba_use_robust_kernel ) 438 | cout << " Robust kernel parameter: " << srba_kernel_param << endl; 439 | 440 | // Detection options 441 | cout << " [Detection] " << endl; 442 | cout << " Detection method: "; detect_method == 0 ? cout << "ORB" : cout << "FAST+ORB"; cout << endl; 443 | cout << " Number of keypoints to detect: " << n_feats << endl; 444 | DUMP_BOOL_VAR_TO_CONSOLE(" Use adaptive FAST threshold in ORB: ", orb_adaptive_fast_th) 445 | cout << " Initial FAST Threshold for ORB keypoints: " << detect_fast_th << endl; 446 | cout << " Minimum number of matches to force adaptation of FAST/ORB thresholds: " << adaptive_th_min_matches << endl; 447 | 448 | // matching_options.dumpToConsole(); 449 | 450 | // Data association options 451 | cout << " [Data Association] " << endl; 452 | DUMP_BOOL_VAR_TO_CONSOLE(" Filter by match direction?: ", da_filter_by_direction) 453 | DUMP_BOOL_VAR_TO_CONSOLE(" Filter by ORB distance?: ", da_filter_by_orb_distance) 454 | DUMP_BOOL_VAR_TO_CONSOLE(" Filter by fundamental matrix?: ", da_filter_by_fund_matrix) 455 | DUMP_BOOL_VAR_TO_CONSOLE(" Filter by pose change?: ", da_filter_by_pose_change) 456 | 457 | cout << " Stage 2 filtering method: "; 458 | switch( da_stage2_method ) 459 | { 460 | case ST2M_NONE : cout << "None"; break; 461 | case ST2M_FUNDMATRIX : cout << "Fundamental matrix"; break; 462 | case ST2M_CHANGEPOSE : cout << "Change in pose"; break; 463 | case ST2M_BOTH : cout << "Fundamental matrix + Change in pose"; break; 464 | } 465 | cout << endl; 466 | 467 | cout << " Residual threshold: " << residual_th << endl; 468 | cout << " Max feat dist to ep-line in inter-frame matching: " << max_y_diff_epipolar << " px." << endl; 469 | cout << " Max distance between ORB descriptors for data association: " << max_orb_distance_da << endl; 470 | cout << " Probability of RANSAC Fundamental Matrix fit: " << ransac_fit_prob << endl; 471 | cout << " DB query result minimum value to keep running: " << query_score_th << endl; 472 | 473 | // KF creation options 474 | cout << " [Key-frame creation] " << endl; 475 | cout << " Initial threshold for testing new KF: " << max_translation << " m. and " << max_rotation << " deg." << endl; 476 | cout << " Update map when # of inter-frame (IF) matches is below: " << updated_matches_th << endl; 477 | cout << " Adapt movement th. when # of IF matches is below: " << up_matches_th_plus+updated_matches_th << endl; 478 | cout << " KF distance to consider a Loop Closure (LC): " << lc_distance << endl; 479 | cout << " Threshold for the number of tracked features from last KF: " << vo_id_tracking_th << endl; 480 | DUMP_BOOL_VAR_TO_CONSOLE(" Use initial pose?: ", use_initial_pose) 481 | 482 | if( pause_after_show_op ) system::pause(); 483 | } // end dumpToConsole 484 | 485 | private: 486 | 487 | } TSRBAStereoSLAMOptions; 488 | 489 | /********************************************* 490 | STRUCT: System statistics at each KF insertion 491 | **********************************************/ 492 | typedef struct TStatsSRBA 493 | { 494 | double time; 495 | size_t numberKFs, numberFeatsNew, numberFeatsCommon; 496 | 497 | TStatsSRBA( const double _time, 498 | const size_t _numberFeatsNew = 0, 499 | const size_t _numberFeatsCommon = 0, 500 | const size_t _numberKFs = 0 ) : 501 | time(_time), 502 | numberFeatsNew(_numberFeatsNew), 503 | numberFeatsCommon(_numberFeatsCommon), 504 | numberKFs(_numberKFs) 505 | {} 506 | } TStatsSRBA; 507 | typedef vector TStatsSRBAVector; 508 | 509 | typedef vector< pair > t_vector_pair_idx_distance; 510 | 511 | // These are static methods (only available when the header file is included) 512 | // -- comparison methods 513 | bool compareKeypointLists( const TKeyPointList & list1, const Mat & desc1, const TKeyPointList & list2, const Mat & desc2 ); 514 | bool compareMatchesLists( const TDMatchList & list1, const TDMatchList & list2 ); 515 | bool compareOptions( const TSRBAStereoSLAMOptions & opt1, const TSRBAStereoSLAMOptions & opt2 ); 516 | 517 | // -- threshold management 518 | double updateTranslationThreshold( const double x, const double th ); 519 | double updateRotationThreshold( const double x, const double th ); 520 | 521 | // -- others 522 | void show_kf_numbers( 523 | COpenGLScenePtr & scene, 524 | const size_t & num_kf, 525 | const DBoW2::QueryResults & ret, 526 | const double & th = 0.0 ); 527 | 528 | CPose3DRotVec getRelativePose( 529 | const TKeyFrameID & fromId, 530 | const TKeyFrameID & toId, 531 | const CPose3DRotVec & voIncrPose ); // <-- ?? TODO: Remove this if unnecessary 532 | 533 | // -- inline methods 534 | inline void computeDispersion( 535 | const TKeyPointList & list, 536 | const TDMatchList & matches, 537 | double & std_x, 538 | double & std_y ) 539 | { 540 | double mx = 0, my = 0; 541 | for( TDMatchList::const_iterator it = matches.begin(); it != matches.end(); ++it ) 542 | { 543 | mx += list[it->queryIdx].pt.x; 544 | my += list[it->queryIdx].pt.y; 545 | } 546 | mx /= matches.size(); 547 | my /= matches.size(); 548 | 549 | for( TDMatchList::const_iterator it = matches.begin(); it != matches.end(); ++it ) 550 | { 551 | std_x += mrpt::utils::square(list[it->queryIdx].pt.x-mx); 552 | std_y += mrpt::utils::square(list[it->queryIdx].pt.y-my); 553 | } 554 | std_x = sqrt(std_x); 555 | std_y = sqrt(std_y); 556 | } // end-computeDispersion 557 | 558 | inline mrpt::math::TPoint3D projectMatchTo3D( 559 | const double & ul, 560 | const double & vl, 561 | const double & ur, 562 | const mrpt::utils::TStereoCamera & stereoCamera ) 563 | { 564 | // camera 565 | const double & cul = stereoCamera.leftCamera.cx(); 566 | const double & cvl = stereoCamera.leftCamera.cy(); 567 | const double & fl = stereoCamera.leftCamera.fx(); 568 | const double & cur = stereoCamera.rightCamera.cx(); 569 | const double & fr = stereoCamera.rightCamera.fx(); 570 | const double & baseline = stereoCamera.rightCameraPose[0]; 571 | 572 | const double b_d = baseline/(fl*(cur-ur)+fr*(ul-cul)); 573 | return mrpt::math::TPoint3D(b_d*fr*(ul-cul),b_d*fr*(vl-cvl),b_d*fl*fr); 574 | } // end-projectMatchTo3D 575 | --------------------------------------------------------------------------------