├── LICENSE ├── README.md ├── components.py ├── covisibility.py ├── dataset.py ├── feature.py ├── imgs ├── point_cloud.png ├── pose_graph.png └── sptam_overview.png ├── loopclosing.py ├── mapping.py ├── motion.py ├── optimization.py ├── params.py ├── sptam.py └── viewer.py /LICENSE: -------------------------------------------------------------------------------- 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 | # stereo_ptam 2 | 3 | This python project is a complete implementation of Stereo PTAM, based on C++ project [lrse/sptam](https://github.com/lrse/sptam) and paper "[S-PTAM: Stereo Parallel Tracking and Mapping](http://webdiis.unizar.es/~jcivera/papers/pire_etal_ras17.pdf) Taihu Pire et al. RAS17", with some modifications. 4 | 5 | > S-PTAM is a Stereo SLAM system able to compute the camera trajectory in real-time. It heavily exploits the parallel nature of the SLAM problem, separating the time-constrained pose estimation from less pressing matters such as map building and refinement tasks. On the other hand, the stereo setting allows to reconstruct a metric 3D map for each frame of stereo images, improving the accuracy of the mapping process with respect to monocular SLAM and avoiding the well-known bootstrapping problem. Also, the real scale of the environment is an essential feature for robots which have to interact with their surrounding workspace. 6 | 7 | S-PTAM system overview (from [S-PTAM paper](http://webdiis.unizar.es/~jcivera/papers/pire_etal_ras17.pdf) page 11): 8 | ![](imgs/sptam_overview.png) 9 | 10 | As stated in the [S-PTAM paper](http://webdiis.unizar.es/~jcivera/papers/pire_etal_ras17.pdf) (page 39), S-PTAM's results on KITTI dataset is comparable to stereo version of [ORB-SLAM2](https://github.com/raulmur/ORB_SLAM2), and better than stereo LSD-SLAM. It's very inspiring, I'm trying to reproduce the results. 11 | 12 | 13 | ## Features 14 | (of this implementation) 15 | * Multithreads Tracking, Mapping, and Loop Closing; 16 | * Covisibility Graph (representing the relation between keyframes, mappoints and measurements); 17 | * Local Bundle Adjustment and Pose Graph Optimization; 18 | * Motion Model (used for pose prediction, then for reliable feature matching); 19 | * Point Clouds and Graph visualization; 20 | * Data loader for datasets [KITTI Odometry](http://www.cvlibs.net/datasets/kitti/eval_odometry.php) and [EuRoC MAV](http://projects.asl.ethz.ch/datasets/doku.php?id=kmavvisualinertialdatasets); 21 | * Reasonable speed: ~50ms per frame on EuRoC, and ~70ms per frame on KITTI. 22 | 23 | ## Requirements 24 | * Python 3.6+ 25 | * numpy 26 | * cv2 27 | * [g2o](https://github.com/uoip/g2opy) (python binding of C++ library [g2o](https://github.com/RainerKuemmerle/g2o)) for optimization 28 | * [pangolin](https://github.com/uoip/pangolin) (python binding of C++ library [Pangolin](http://github.com/stevenlovegrove/Pangolin)) for visualization 29 | 30 | ## Usage 31 | `python sptam.py --dataset kitti --path path/to/your/KITTI_odometry_dataset/sequences/00` 32 | or 33 | `python sptam.py --dataset euroc --path path/to/your/EuRoC_MAV_dataset/MH_01_easy` 34 | 35 | ## Results 36 | Visual results (screenshots from my experiment) on KITTI odometry sequence 00: 37 | * graph: 38 | As shown below, all loops have been closed (loop points are marked in black). 39 | ![](imgs/pose_graph.png) 40 | * point cloud: 41 | ![](imgs/point_cloud.png) 42 | 43 | 44 | ### TODO: 45 | Exhaustive evaluation on datasets. (There seems to be a python package [MichaelGrupp/evo](https://github.com/MichaelGrupp/evo) for odometry/SLAM algorithm evaluation) 46 | 47 | ## License 48 | This python reimplementation is largely based on [sptam](https://github.com/lrse/sptam), so it's licensed under GPLv3 License. 49 | 50 | ## Contact 51 | If you have problems related to the base S-PTAM algorithm, you can contact original authors [lrse](https://github.com/lrse) (robotica@dc.uba.ar), or refer to the related papers: 52 | [1] Taihú Pire,Thomas Fischer, Gastón Castro, Pablo De Cristóforis, Javier Civera and Julio Jacobo Berlles. 53 | **S-PTAM: Stereo Parallel Tracking and Mapping** 54 | Robotics and Autonomous Systems, 2017. 55 | 56 | [2] Taihú Pire, Thomas Fischer, Javier Civera, Pablo De Cristóforis and Julio Jacobo Berlles. 57 | **Stereo Parallel Tracking and Mapping for Robot Localization** 58 | Proc. of The International Conference on Intelligent Robots and Systems (IROS), Hamburg, Germany, 2015. 59 | 60 | 61 | If you have interest in the python implementation here, just email me (Hang Qi, qihang@outlook.com); 62 | -------------------------------------------------------------------------------- /components.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import cv2 3 | import g2o 4 | 5 | from threading import Lock, Thread 6 | from queue import Queue 7 | 8 | from enum import Enum 9 | from collections import defaultdict 10 | 11 | from covisibility import GraphKeyFrame 12 | from covisibility import GraphMapPoint 13 | from covisibility import GraphMeasurement 14 | 15 | 16 | 17 | 18 | class Camera(object): 19 | def __init__(self, fx, fy, cx, cy, width, height, 20 | frustum_near, frustum_far, baseline): 21 | self.fx = fx 22 | self.fy = fy 23 | self.cx = cx 24 | self.cy = cy 25 | self.baseline = baseline 26 | 27 | self.intrinsic = np.array([ 28 | [fx, 0, cx], 29 | [0, fy, cy], 30 | [0, 0, 1]]) 31 | 32 | self.frustum_near = frustum_near 33 | self.frustum_far = frustum_far 34 | 35 | self.width = width 36 | self.height = height 37 | 38 | def compute_right_camera_pose(self, pose): 39 | pos = pose * np.array([self.baseline, 0, 0]) 40 | return g2o.Isometry3d(pose.orientation(), pos) 41 | 42 | 43 | 44 | class Frame(object): 45 | def __init__(self, idx, pose, feature, cam, timestamp=None, 46 | pose_covariance=np.identity(6)): 47 | self.idx = idx 48 | self.pose = pose # g2o.Isometry3d 49 | self.feature = feature 50 | self.cam = cam 51 | self.timestamp = timestamp 52 | self.image = feature.image 53 | 54 | self.orientation = pose.orientation() 55 | self.position = pose.position() 56 | self.pose_covariance = pose_covariance 57 | 58 | self.transform_matrix = pose.inverse().matrix()[:3] # shape: (3, 4) 59 | self.projection_matrix = ( 60 | self.cam.intrinsic.dot(self.transform_matrix)) # from world frame to image 61 | 62 | # batch version 63 | def can_view(self, points, ground=False, margin=20): # Frustum Culling 64 | points = np.transpose(points) 65 | (u, v), depth = self.project(self.transform(points)) 66 | 67 | if ground: 68 | return np.logical_and.reduce([ 69 | depth >= self.cam.frustum_near, 70 | depth <= self.cam.frustum_far, 71 | u >= - margin, 72 | u <= self.cam.width + margin]) 73 | else: 74 | return np.logical_and.reduce([ 75 | depth >= self.cam.frustum_near, 76 | depth <= self.cam.frustum_far, 77 | u >= - margin, 78 | u <= self.cam.width + margin, 79 | v >= - margin, 80 | v <= self.cam.height + margin]) 81 | 82 | 83 | def update_pose(self, pose): 84 | if isinstance(pose, g2o.SE3Quat): 85 | self.pose = g2o.Isometry3d(pose.orientation(), pose.position()) 86 | else: 87 | self.pose = pose 88 | self.orientation = self.pose.orientation() 89 | self.position = self.pose.position() 90 | 91 | self.transform_matrix = self.pose.inverse().matrix()[:3] 92 | self.projection_matrix = ( 93 | self.cam.intrinsic.dot(self.transform_matrix)) 94 | 95 | def transform(self, points): # from world coordinates 96 | ''' 97 | Transform points from world coordinates frame to camera frame. 98 | Args: 99 | points: a point or an array of points, of shape (3,) or (3, N). 100 | ''' 101 | R = self.transform_matrix[:3, :3] 102 | if points.ndim == 1: 103 | t = self.transform_matrix[:3, 3] 104 | else: 105 | t = self.transform_matrix[:3, 3:] 106 | return R.dot(points) + t 107 | 108 | def project(self, points): 109 | ''' 110 | Project points from camera frame to image's pixel coordinates. 111 | Args: 112 | points: a point or an array of points, of shape (3,) or (3, N). 113 | Returns: 114 | Projected pixel coordinates, and respective depth. 115 | ''' 116 | projection = self.cam.intrinsic.dot(points / points[-1:]) 117 | return projection[:2], points[-1] 118 | 119 | def find_matches(self, points, descriptors): 120 | ''' 121 | Match to points from world frame. 122 | Args: 123 | points: a list/array of points. shape: (N, 3) 124 | descriptors: a list of feature descriptors. length: N 125 | Returns: 126 | List of successfully matched (queryIdx, trainIdx) pairs. 127 | ''' 128 | points = np.transpose(points) 129 | proj, _ = self.project(self.transform(points)) 130 | proj = proj.transpose() 131 | return self.feature.find_matches(proj, descriptors) 132 | 133 | def get_keypoint(self, i): 134 | return self.feature.get_keypoint(i) 135 | def get_descriptor(self, i): 136 | return self.feature.get_descriptor(i) 137 | def get_color(self, pt): 138 | return self.feature.get_color(pt) 139 | def set_matched(self, i): 140 | self.feature.set_matched(i) 141 | def get_unmatched_keypoints(self): 142 | return self.feature.get_unmatched_keypoints() 143 | 144 | 145 | 146 | class StereoFrame(Frame): 147 | def __init__(self, idx, pose, feature, right_feature, cam, 148 | right_cam=None, timestamp=None, pose_covariance=np.identity(6)): 149 | 150 | super().__init__(idx, pose, feature, cam, timestamp, pose_covariance) 151 | self.left = Frame(idx, pose, feature, cam, timestamp, pose_covariance) 152 | self.right = Frame(idx, 153 | cam.compute_right_camera_pose(pose), 154 | right_feature, right_cam or cam, 155 | timestamp, pose_covariance) 156 | 157 | def find_matches(self, source, points, descriptors): 158 | 159 | q2 = Queue() 160 | def find_right(points, descriptors, q): 161 | m = dict(self.right.find_matches(points, descriptors)) 162 | q.put(m) 163 | t2 = Thread(target=find_right, args=(points, descriptors, q2)) 164 | t2.start() 165 | matches_left = dict(self.left.find_matches(points, descriptors)) 166 | t2.join() 167 | matches_right = q2.get() 168 | 169 | measurements = [] 170 | for i, j in matches_left.items(): 171 | if i in matches_right: 172 | j2 = matches_right[i] 173 | 174 | y1 = self.left.get_keypoint(j).pt[1] 175 | y2 = self.right.get_keypoint(j2).pt[1] 176 | if abs(y1 - y2) > 2.5: # epipolar constraint 177 | continue # TODO: choose one 178 | 179 | meas = Measurement( 180 | Measurement.Type.STEREO, 181 | source, 182 | [self.left.get_keypoint(j), 183 | self.right.get_keypoint(j2)], 184 | [self.left.get_descriptor(j), 185 | self.right.get_descriptor(j2)]) 186 | measurements.append((i, meas)) 187 | self.left.set_matched(j) 188 | self.right.set_matched(j2) 189 | else: 190 | meas = Measurement( 191 | Measurement.Type.LEFT, 192 | source, 193 | [self.left.get_keypoint(j)], 194 | [self.left.get_descriptor(j)]) 195 | measurements.append((i, meas)) 196 | self.left.set_matched(j) 197 | 198 | for i, j in matches_right.items(): 199 | if i not in matches_left: 200 | meas = Measurement( 201 | Measurement.Type.RIGHT, 202 | source, 203 | [self.right.get_keypoint(j)], 204 | [self.right.get_descriptor(j)]) 205 | measurements.append((i, meas)) 206 | self.right.set_matched(j) 207 | 208 | return measurements 209 | 210 | def match_mappoints(self, mappoints, source): 211 | points = [] 212 | descriptors = [] 213 | for mappoint in mappoints: 214 | points.append(mappoint.position) 215 | descriptors.append(mappoint.descriptor) 216 | matched_measurements = self.find_matches(source, points, descriptors) 217 | 218 | measurements = [] 219 | for i, meas in matched_measurements: 220 | meas.mappoint = mappoints[i] 221 | measurements.append(meas) 222 | return measurements 223 | 224 | def triangulate(self): 225 | kps_left, desps_left, idx_left = self.left.get_unmatched_keypoints() 226 | kps_right, desps_right, idx_right = self.right.get_unmatched_keypoints() 227 | 228 | mappoints, matches = self.triangulate_points( 229 | kps_left, desps_left, kps_right, desps_right) 230 | 231 | measurements = [] 232 | for mappoint, (i, j) in zip(mappoints, matches): 233 | meas = Measurement( 234 | Measurement.Type.STEREO, 235 | Measurement.Source.TRIANGULATION, 236 | [kps_left[i], kps_right[j]], 237 | [desps_left[i], desps_right[j]]) 238 | meas.mappoint = mappoint 239 | meas.view = self.transform(mappoint.position) 240 | measurements.append(meas) 241 | 242 | self.left.set_matched(idx_left[i]) 243 | self.right.set_matched(idx_right[j]) 244 | 245 | return mappoints, measurements 246 | 247 | def triangulate_points(self, kps_left, desps_left, kps_right, desps_right): 248 | matches = self.feature.row_match( 249 | kps_left, desps_left, kps_right, desps_right) 250 | assert len(matches) > 0 251 | 252 | px_left = np.array([kps_left[m.queryIdx].pt for m in matches]) 253 | px_right = np.array([kps_right[m.trainIdx].pt for m in matches]) 254 | 255 | points = cv2.triangulatePoints( 256 | self.left.projection_matrix, 257 | self.right.projection_matrix, 258 | px_left.transpose(), 259 | px_right.transpose() 260 | ).transpose() # shape: (N, 4) 261 | 262 | points = points[:, :3] / points[:, 3:] 263 | 264 | can_view = np.logical_and( 265 | self.left.can_view(points), 266 | self.right.can_view(points)) 267 | 268 | mappoints = [] 269 | matchs = [] 270 | for i, point in enumerate(points): 271 | if not can_view[i]: 272 | continue 273 | normal = point - self.position 274 | normal = normal / np.linalg.norm(normal) 275 | 276 | color = self.left.get_color(px_left[i]) 277 | 278 | mappoint = MapPoint( 279 | point, normal, desps_left[matches[i].queryIdx], color) 280 | mappoints.append(mappoint) 281 | matchs.append((matches[i].queryIdx, matches[i].trainIdx)) 282 | 283 | return mappoints, matchs 284 | 285 | def update_pose(self, pose): 286 | super().update_pose(pose) 287 | self.right.update_pose(pose) 288 | self.left.update_pose( 289 | self.cam.compute_right_camera_pose(pose)) 290 | 291 | # batch version 292 | def can_view(self, mappoints): 293 | points = [] 294 | point_normals = [] 295 | for i, p in enumerate(mappoints): 296 | points.append(p.position) 297 | point_normals.append(p.normal) 298 | points = np.asarray(points) 299 | point_normals = np.asarray(point_normals) 300 | 301 | normals = points - self.position 302 | normals /= np.linalg.norm(normals, axis=-1, keepdims=True) 303 | cos = np.clip(np.sum(point_normals * normals, axis=1), -1, 1) 304 | parallel = np.arccos(cos) < (np.pi / 4) 305 | 306 | can_view = np.logical_or( 307 | self.left.can_view(points), 308 | self.right.can_view(points)) 309 | 310 | return np.logical_and(parallel, can_view) 311 | 312 | def to_keyframe(self): 313 | return KeyFrame( 314 | self.idx, self.pose, 315 | self.left.feature, self.right.feature, 316 | self.cam, self.right.cam, 317 | self.pose_covariance) 318 | 319 | 320 | 321 | class KeyFrame(GraphKeyFrame, StereoFrame): 322 | _id = 0 323 | _id_lock = Lock() 324 | 325 | def __init__(self, *args, **kwargs): 326 | GraphKeyFrame.__init__(self) 327 | StereoFrame.__init__(self, *args, **kwargs) 328 | 329 | with KeyFrame._id_lock: 330 | self.id = KeyFrame._id 331 | KeyFrame._id += 1 332 | 333 | self.reference_keyframe = None 334 | self.reference_constraint = None 335 | self.preceding_keyframe = None 336 | self.preceding_constraint = None 337 | self.loop_keyframe = None 338 | self.loop_constraint = None 339 | self.fixed = False 340 | 341 | def update_reference(self, reference=None): 342 | if reference is not None: 343 | self.reference_keyframe = reference 344 | self.reference_constraint = ( 345 | self.reference_keyframe.pose.inverse() * self.pose) 346 | 347 | def update_preceding(self, preceding=None): 348 | if preceding is not None: 349 | self.preceding_keyframe = preceding 350 | self.preceding_constraint = ( 351 | self.preceding_keyframe.pose.inverse() * self.pose) 352 | 353 | def set_loop(self, keyframe, constraint): 354 | self.loop_keyframe = keyframe 355 | self.loop_constraint = constraint 356 | 357 | def is_fixed(self): 358 | return self.fixed 359 | 360 | def set_fixed(self, fixed=True): 361 | self.fixed = fixed 362 | 363 | 364 | 365 | class MapPoint(GraphMapPoint): 366 | _id = 0 367 | _id_lock = Lock() 368 | 369 | def __init__(self, position, normal, descriptor, 370 | color=np.zeros(3), 371 | covariance=np.identity(3) * 1e-4): 372 | super().__init__() 373 | 374 | with MapPoint._id_lock: 375 | self.id = MapPoint._id 376 | MapPoint._id += 1 377 | 378 | self.position = position 379 | self.normal = normal 380 | self.descriptor = descriptor 381 | self.covariance = covariance 382 | self.color = color 383 | # self.owner = None 384 | 385 | self.count = defaultdict(int) 386 | 387 | def update_position(self, position): 388 | self.position = position 389 | def update_normal(self, normal): 390 | self.normal = normal 391 | def update_descriptor(self, descriptor): 392 | self.descriptor = descriptor 393 | def set_color(self, color): 394 | self.color = color 395 | 396 | def is_bad(self): 397 | with self._lock: 398 | status = ( 399 | self.count['meas'] == 0 400 | or (self.count['outlier'] > 20 401 | and self.count['outlier'] > self.count['inlier']) 402 | or (self.count['proj'] > 20 403 | and self.count['proj'] > self.count['meas'] * 10)) 404 | return status 405 | 406 | def increase_outlier_count(self): 407 | with self._lock: 408 | self.count['outlier'] += 1 409 | def increase_inlier_count(self): 410 | with self._lock: 411 | self.count['inlier'] += 1 412 | def increase_projection_count(self): 413 | with self._lock: 414 | self.count['proj'] += 1 415 | def increase_measurement_count(self): 416 | with self._lock: 417 | self.count['meas'] += 1 418 | 419 | 420 | 421 | class Measurement(GraphMeasurement): 422 | 423 | Source = Enum('Measurement.Source', ['TRIANGULATION', 'TRACKING', 'REFIND']) 424 | Type = Enum('Measurement.Type', ['STEREO', 'LEFT', 'RIGHT']) 425 | 426 | def __init__(self, type, source, keypoints, descriptors): 427 | super().__init__() 428 | 429 | self.type = type 430 | self.source = source 431 | self.keypoints = keypoints 432 | self.descriptors = descriptors 433 | self.view = None # mappoint's position in current coordinates frame 434 | 435 | self.xy = np.array(self.keypoints[0].pt) 436 | if self.is_stereo(): 437 | self.xyx = np.array([ 438 | *keypoints[0].pt, keypoints[1].pt[0]]) 439 | 440 | self.triangulation = (source == self.Source.TRIANGULATION) 441 | 442 | def get_descriptor(self, i=0): 443 | return self.descriptors[i] 444 | def get_keypoint(self, i=0): 445 | return self.keypoints[i] 446 | 447 | def get_descriptors(self): 448 | return self.descriptors 449 | def get_keypoints(self): 450 | return self.keypoints 451 | 452 | def is_stereo(self): 453 | return self.type == Measurement.Type.STEREO 454 | def is_left(self): 455 | return self.type == Measurement.Type.LEFT 456 | def is_right(self): 457 | return self.type == Measurement.Type.RIGHT 458 | 459 | def from_triangulation(self): 460 | return self.triangulation 461 | def from_tracking(self): 462 | return self.source == Measurement.Source.TRACKING 463 | def from_refind(self): 464 | return self.source == Measurement.Source.REFIND -------------------------------------------------------------------------------- /covisibility.py: -------------------------------------------------------------------------------- 1 | from threading import Lock 2 | 3 | from collections import defaultdict, Counter 4 | from itertools import chain 5 | 6 | 7 | 8 | class GraphKeyFrame(object): 9 | def __init__(self): 10 | self.id = None 11 | self.meas = dict() 12 | self.covisible = defaultdict(int) 13 | self._lock = Lock() 14 | 15 | def __hash__(self): 16 | return self.id 17 | 18 | def __eq__(self, rhs): 19 | return (isinstance(rhs, GraphKeyFrame) and 20 | self.id == rhs.id) 21 | def __lt__(self, rhs): 22 | return self.id < rhs.id # predate 23 | def __le__(self, rhs): 24 | return self.id <= rhs.id 25 | 26 | def measurements(self): 27 | with self._lock: 28 | return self.meas.keys() 29 | 30 | def mappoints(self): 31 | with self._lock: 32 | return self.meas.values() 33 | 34 | def add_measurement(self, m): 35 | with self._lock: 36 | self.meas[m] = m.mappoint 37 | 38 | def remove_measurement(self, m): 39 | with self._lock: 40 | try: 41 | del self.meas[m] 42 | except KeyError: 43 | pass 44 | 45 | def covisibility_keyframes(self): 46 | with self._lock: 47 | return self.covisible.copy() # shallow copy 48 | 49 | def add_covisibility_keyframe(self, kf): 50 | with self._lock: 51 | self.covisible[kf] += 1 52 | 53 | 54 | 55 | class GraphMapPoint(object): 56 | def __init__(self): 57 | self.id = None 58 | self.meas = dict() 59 | self._lock = Lock() 60 | 61 | def __hash__(self): 62 | return self.id 63 | 64 | def __eq__(self, rhs): 65 | return (isinstance(rhs, GraphMapPoint) and 66 | self.id == rhs.id) 67 | def __lt__(self, rhs): 68 | return self.id < rhs.id 69 | def __le__(self, rhs): 70 | return self.id <= rhs.id 71 | 72 | def measurements(self): 73 | with self._lock: 74 | return self.meas.keys() 75 | 76 | def keyframes(self): 77 | with self._lock: 78 | return self.meas.values() 79 | 80 | def add_measurement(self, m): 81 | with self._lock: 82 | self.meas[m] = m.keyframe 83 | 84 | def remove_measurement(self, m): 85 | with self._lock: 86 | try: 87 | del self.meas[m] 88 | except KeyError: 89 | pass 90 | 91 | 92 | 93 | class GraphMeasurement(object): 94 | def __init__(self): 95 | self.keyframe = None 96 | self.mappoint = None 97 | 98 | @property 99 | def id(self): 100 | return (self.keyframe.id, self.mappoint.id) 101 | 102 | def __hash__(self): 103 | return hash(self.id) 104 | 105 | def __eq__(self, rhs): 106 | return (isinstance(rhs, GraphMeasurement) and 107 | self.id == rhs.id) 108 | 109 | 110 | 111 | 112 | class CovisibilityGraph(object): 113 | def __init__(self, ): 114 | self._lock = Lock() 115 | 116 | self.kfs = [] 117 | self.pts = set() 118 | 119 | self.kfs_set = set() 120 | self.meas_lookup = dict() 121 | 122 | def keyframes(self): 123 | with self._lock: 124 | return self.kfs.copy() 125 | 126 | def mappoints(self): 127 | with self._lock: 128 | return self.pts.copy() 129 | 130 | def add_keyframe(self, kf): 131 | with self._lock: 132 | self.kfs.append(kf) 133 | self.kfs_set.add(kf) 134 | 135 | def add_mappoint(self, pt): 136 | with self._lock: 137 | self.pts.add(pt) 138 | 139 | def remove_mappoint(self, pt): 140 | with self._lock: 141 | try: 142 | for m in pt.measurements(): 143 | m.keyframe.remove_measurement(m) 144 | del self.meas_lookup[m.id] 145 | self.pts.remove(pt) 146 | except: 147 | pass 148 | 149 | def add_measurement(self, kf, pt, meas): 150 | with self._lock: 151 | if kf not in self.kfs_set or pt not in self.pts: 152 | return 153 | 154 | for m in pt.measurements(): 155 | if m.keyframe == kf: 156 | continue 157 | kf.add_covisibility_keyframe(m.keyframe) 158 | m.keyframe.add_covisibility_keyframe(kf) 159 | 160 | meas.keyframe = kf 161 | meas.mappoint = pt 162 | kf.add_measurement(meas) 163 | pt.add_measurement(meas) 164 | 165 | self.meas_lookup[meas.id] = meas 166 | 167 | def remove_measurement(self, m): 168 | m.keyframe.remove_measurement(m) 169 | m.mappoint.remove_measurement(m) 170 | with self._lock: 171 | try: 172 | del self.meas_lookup[m.id] 173 | except: 174 | pass 175 | 176 | def has_measurement(self, *args): 177 | with self._lock: 178 | if len(args) == 1: # measurement 179 | return args[0].id in self.meas_lookup 180 | elif len(args) == 2: # keyframe, mappoint 181 | id = (args[0].id, args[1].id) 182 | return id in self.meas_lookup 183 | else: 184 | raise TypeError 185 | 186 | def get_reference_frame(self, seedpoints): 187 | assert len(seedpoints) > 0 188 | visible = [pt.keyframes() for pt in seedpoints] 189 | visible = Counter(chain(*visible)) 190 | return visible.most_common(1)[0][0] 191 | 192 | def get_local_map(self, seedpoints, window_size=15): 193 | reference = self.get_reference_frame(seedpoints) 194 | covisible = chain( 195 | reference.covisibility_keyframes().items(), [(reference, float('inf'))]) 196 | covisible = sorted(covisible, key=lambda _:_[1], reverse=True) 197 | 198 | local_map = [seedpoints] 199 | local_keyframes = [] 200 | for kf, n in covisible[:window_size]: 201 | if n < 1: 202 | continue 203 | local_map.append(kf.mappoints()) 204 | local_keyframes.append(kf) 205 | local_map = list(set(chain(*local_map))) 206 | 207 | return local_map, local_keyframes 208 | 209 | def get_local_map_v2(self, seedframes, window_size=12, loop_window_size=8): 210 | covisible = [] 211 | for kf in set(seedframes): 212 | covisible.append(Counter(kf.covisibility_keyframes())) 213 | covisible = sum(covisible, Counter()) 214 | for kf in set(seedframes): 215 | covisible[kf] = float('inf') 216 | local = sorted( 217 | covisible.items(), key=lambda _:_[1], reverse=True) 218 | 219 | id = max([_.id for _ in covisible]) 220 | loop_frames = [_ for _ in local if _[0].id < id-50] 221 | 222 | local = local[:window_size] 223 | loop_local = [] 224 | if len(loop_frames) > 0: 225 | loop_covisible = sorted( 226 | loop_frames[0][0].covisibility_keyframes().items(), 227 | key=lambda _:_[1], reverse=True) 228 | 229 | for kf, n in loop_covisible: 230 | if kf not in set([_[0] for _ in local]): 231 | loop_local.append((kf, n)) 232 | if len(loop_local) >= loop_window_size: 233 | break 234 | 235 | local = chain(local, loop_local) 236 | 237 | local_map = [] 238 | local_keyframes = [] 239 | for kf, n in local: 240 | if n < 1: 241 | continue 242 | local_map.append(kf.mappoints()) 243 | local_keyframes.append(kf) 244 | local_map = list(set(chain(*local_map))) 245 | return local_map, local_keyframes -------------------------------------------------------------------------------- /dataset.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import cv2 3 | import os 4 | import time 5 | 6 | from collections import defaultdict, namedtuple 7 | 8 | from threading import Thread, Lock 9 | from multiprocessing import Process, Queue 10 | 11 | 12 | 13 | class ImageReader(object): 14 | def __init__(self, ids, timestamps, cam=None): 15 | self.ids = ids 16 | self.timestamps = timestamps 17 | self.cam = cam 18 | self.cache = dict() 19 | self.idx = 0 20 | 21 | self.ahead = 10 # 10 images ahead of current index 22 | self.waiting = 1.5 # waiting time 23 | 24 | self.preload_thread = Thread(target=self.preload) 25 | self.thread_started = False 26 | 27 | def read(self, path): 28 | img = cv2.imread(path, -1) 29 | if self.cam is None: 30 | return img 31 | else: 32 | return self.cam.rectify(img) 33 | 34 | def preload(self): 35 | idx = self.idx 36 | t = float('inf') 37 | while True: 38 | if time.time() - t > self.waiting: 39 | return 40 | if self.idx == idx: 41 | time.sleep(1e-2) 42 | continue 43 | 44 | for i in range(self.idx, self.idx + self.ahead): 45 | if i not in self.cache and i < len(self.ids): 46 | self.cache[i] = self.read(self.ids[i]) 47 | if self.idx + self.ahead > len(self.ids): 48 | return 49 | idx = self.idx 50 | t = time.time() 51 | 52 | def __len__(self): 53 | return len(self.ids) 54 | 55 | def __getitem__(self, idx): 56 | self.idx = idx 57 | # if not self.thread_started: 58 | # self.thread_started = True 59 | # self.preload_thread.start() 60 | 61 | if idx in self.cache: 62 | img = self.cache[idx] 63 | del self.cache[idx] 64 | else: 65 | img = self.read(self.ids[idx]) 66 | return img 67 | 68 | def __iter__(self): 69 | for i, timestamp in enumerate(self.timestamps): 70 | yield timestamp, self[i] 71 | 72 | @property 73 | def dtype(self): 74 | return self[0].dtype 75 | @property 76 | def shape(self): 77 | return self[0].shape 78 | 79 | 80 | 81 | 82 | class KITTIOdometry(object): # without lidar 83 | ''' 84 | path example: 'path/to/your/KITTI odometry dataset/sequences/00' 85 | ''' 86 | def __init__(self, path): 87 | Cam = namedtuple('cam', 'fx fy cx cy width height baseline') 88 | cam00_02 = Cam(718.856, 718.856, 607.1928, 185.2157, 1241, 376, 0.5371657) 89 | cam03 = Cam(721.5377, 721.5377, 609.5593, 172.854, 1241, 376, 0.53715) 90 | cam04_12 = Cam(707.0912, 707.0912, 601.8873, 183.1104, 1241, 376, 0.53715) 91 | 92 | path = os.path.expanduser(path) 93 | timestamps = np.loadtxt(os.path.join(path, 'times.txt')) 94 | self.left = ImageReader(self.listdir(os.path.join(path, 'image_2')), 95 | timestamps) 96 | self.right = ImageReader(self.listdir(os.path.join(path, 'image_3')), 97 | timestamps) 98 | 99 | assert len(self.left) == len(self.right) 100 | self.timestamps = self.left.timestamps 101 | 102 | sequence = int(path.strip(os.path.sep).split(os.path.sep)[-1]) 103 | if sequence < 3: 104 | self.cam = cam00_02 105 | elif sequence == 3: 106 | self.cam = cam03 107 | elif sequence < 13: 108 | self.cam = cam04_12 109 | 110 | def sort(self, xs): 111 | return sorted(xs, key=lambda x:float(x[:-4])) 112 | 113 | def listdir(self, dir): 114 | files = [_ for _ in os.listdir(dir) if _.endswith('.png')] 115 | return [os.path.join(dir, _) for _ in self.sort(files)] 116 | 117 | def __len__(self): 118 | return len(self.left) 119 | 120 | 121 | 122 | 123 | 124 | 125 | class Camera(object): 126 | def __init__(self, 127 | width, height, 128 | intrinsic_matrix, 129 | undistort_rectify=False, 130 | extrinsic_matrix=None, 131 | distortion_coeffs=None, 132 | rectification_matrix=None, 133 | projection_matrix=None): 134 | 135 | self.width = width 136 | self.height = height 137 | self.intrinsic_matrix = intrinsic_matrix 138 | self.extrinsic_matrix = extrinsic_matrix 139 | self.distortion_coeffs = distortion_coeffs 140 | self.rectification_matrix = rectification_matrix 141 | self.projection_matrix = projection_matrix 142 | self.undistort_rectify = undistort_rectify 143 | self.fx = intrinsic_matrix[0, 0] 144 | self.fy = intrinsic_matrix[1, 1] 145 | self.cx = intrinsic_matrix[0, 2] 146 | self.cy = intrinsic_matrix[1, 2] 147 | 148 | if undistort_rectify: 149 | self.remap = cv2.initUndistortRectifyMap( 150 | cameraMatrix=self.intrinsic_matrix, 151 | distCoeffs=self.distortion_coeffs, 152 | R=self.rectification_matrix, 153 | newCameraMatrix=self.projection_matrix, 154 | size=(width, height), 155 | m1type=cv2.CV_8U) 156 | else: 157 | self.remap = None 158 | 159 | def rectify(self, img): 160 | if self.remap is None: 161 | return img 162 | else: 163 | return cv2.remap(img, *self.remap, cv2.INTER_LINEAR) 164 | 165 | class StereoCamera(object): 166 | def __init__(self, left_cam, right_cam): 167 | self.left_cam = left_cam 168 | self.right_cam = right_cam 169 | 170 | self.width = left_cam.width 171 | self.height = left_cam.height 172 | self.intrinsic_matrix = left_cam.intrinsic_matrix 173 | self.extrinsic_matrix = left_cam.extrinsic_matrix 174 | self.fx = left_cam.fx 175 | self.fy = left_cam.fy 176 | self.cx = left_cam.cx 177 | self.cy = left_cam.cy 178 | self.baseline = abs(right_cam.projection_matrix[0, 3] / 179 | right_cam.projection_matrix[0, 0]) 180 | self.focal_baseline = self.fx * self.baseline 181 | 182 | 183 | class EuRoCDataset(object): # Stereo + IMU 184 | ''' 185 | path example: 'path/to/your/EuRoC Mav dataset/MH_01_easy' 186 | ''' 187 | def __init__(self, path, rectify=True): 188 | self.left_cam = Camera( 189 | width=752, height=480, 190 | intrinsic_matrix = np.array([ 191 | [458.654, 0.000000, 367.215], 192 | [0.000000, 457.296, 248.375], 193 | [0.000000, 0.000000, 1.000000]]), 194 | undistort_rectify=rectify, 195 | distortion_coeffs = np.array( 196 | [-0.28340811, 0.07395907, 0.00019359, 1.76187114e-05, 0.000000]), 197 | rectification_matrix = np.array([ 198 | [0.999966347530033, -0.001422739138722922, 0.008079580483432283], 199 | [0.001365741834644127, 0.9999741760894847, 0.007055629199258132], 200 | [-0.008089410156878961, -0.007044357138835809, 0.9999424675829176]]), 201 | projection_matrix = np.array([ 202 | [435.2046959714599, 0, 367.4517211914062, 0], 203 | [0, 435.2046959714599, 252.2008514404297, 0], 204 | [0., 0, 1, 0]]), 205 | extrinsic_matrix = np.array([ 206 | [0.0148655429818, -0.999880929698, 0.00414029679422, -0.0216401454975], 207 | [0.999557249008, 0.0149672133247, 0.025715529948, -0.064676986768], 208 | [-0.0257744366974, 0.00375618835797, 0.999660727178, 0.00981073058949], 209 | [0.0, 0.0, 0.0, 1.0]]) 210 | ) 211 | self.right_cam = Camera( 212 | width=752, height=480, 213 | intrinsic_matrix = np.array([ 214 | [457.587, 0.000000, 379.999], 215 | [0.000000, 456.134, 255.238], 216 | [0.000000, 0.000000, 1.000000]]), 217 | undistort_rectify=rectify, 218 | distortion_coeffs = np.array( 219 | [-0.28368365, 0.07451284, -0.00010473, -3.555907e-05, 0.0]), 220 | rectification_matrix = np.array([ 221 | [0.9999633526194376, -0.003625811871560086, 0.007755443660172947], 222 | [0.003680398547259526, 0.9999684752771629, -0.007035845251224894], 223 | [-0.007729688520722713, 0.007064130529506649, 0.999945173484644]]), 224 | projection_matrix = np.array([ 225 | [435.2046959714599, 0, 367.4517211914062, -47.90639384423901], 226 | [0, 435.2046959714599, 252.2008514404297, 0], 227 | [0, 0, 1, 0]]), 228 | extrinsic_matrix = np.array([ 229 | [0.0125552670891, -0.999755099723, 0.0182237714554, -0.0198435579556], 230 | [0.999598781151, 0.0130119051815, 0.0251588363115, 0.0453689425024], 231 | [-0.0253898008918, 0.0179005838253, 0.999517347078, 0.00786212447038], 232 | [0.0, 0.0, 0.0, 1.0]]) 233 | ) 234 | 235 | path = os.path.expanduser(path) 236 | self.left = ImageReader( 237 | *self.list_imgs(os.path.join(path, 'mav0', 'cam0', 'data')), 238 | self.left_cam) 239 | self.right = ImageReader( 240 | *self.list_imgs(os.path.join(path, 'mav0', 'cam1', 'data')), 241 | self.right_cam) 242 | assert len(self.left) == len(self.right) 243 | self.timestamps = self.left.timestamps 244 | 245 | self.cam = StereoCamera(self.left_cam, self.right_cam) 246 | 247 | def list_imgs(self, dir): 248 | xs = [_ for _ in os.listdir(dir) if _.endswith('.png')] 249 | xs = sorted(xs, key=lambda x:float(x[:-4])) 250 | timestamps = [float(_[:-4]) * 1e-9 for _ in xs] 251 | return [os.path.join(dir, _) for _ in xs], timestamps 252 | 253 | def __len__(self): 254 | return len(self.left) -------------------------------------------------------------------------------- /feature.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import cv2 3 | 4 | from collections import defaultdict 5 | from numbers import Number 6 | 7 | from threading import Thread, Lock 8 | from queue import Queue 9 | 10 | 11 | 12 | class ImageFeature(object): 13 | def __init__(self, image, params): 14 | # TODO: pyramid representation 15 | self.image = image 16 | self.height, self.width = image.shape[:2] 17 | 18 | self.keypoints = [] # list of cv2.KeyPoint 19 | self.descriptors = [] # numpy.ndarray 20 | 21 | self.detector = params.feature_detector 22 | self.extractor = params.descriptor_extractor 23 | self.matcher = params.descriptor_matcher 24 | 25 | self.cell_size = params.matching_cell_size 26 | self.distance = params.matching_distance 27 | self.neighborhood = ( 28 | params.matching_cell_size * params.matching_neighborhood) 29 | 30 | self._lock = Lock() 31 | 32 | def extract(self): 33 | self.keypoints = self.detector.detect(self.image) 34 | self.keypoints, self.descriptors = self.extractor.compute( 35 | self.image, self.keypoints) 36 | 37 | self.unmatched = np.ones(len(self.keypoints), dtype=bool) 38 | 39 | def draw_keypoints(self, name='keypoints', delay=1): 40 | if self.image.ndim == 2: 41 | image = np.repeat(self.image[..., np.newaxis], 3, axis=2) 42 | else: 43 | image = self.image 44 | img = cv2.drawKeypoints(image, self.keypoints, None, flags=0) 45 | cv2.imshow(name, img);cv2.waitKey(delay) 46 | 47 | def find_matches(self, predictions, descriptors): 48 | matches = dict() 49 | distances = defaultdict(lambda: float('inf')) 50 | for m, query_idx, train_idx in self.matched_by(descriptors): 51 | if m.distance > min(distances[train_idx], self.distance): 52 | continue 53 | 54 | pt1 = predictions[query_idx] 55 | pt2 = self.keypoints[train_idx].pt 56 | dx = pt1[0] - pt2[0] 57 | dy = pt1[1] - pt2[1] 58 | if np.sqrt(dx*dx + dy*dy) > self.neighborhood: 59 | continue 60 | 61 | matches[train_idx] = query_idx 62 | distances[train_idx] = m.distance 63 | matches = [(i, j) for j, i in matches.items()] 64 | return matches 65 | 66 | def matched_by(self, descriptors): 67 | with self._lock: 68 | unmatched_descriptors = self.descriptors[self.unmatched] 69 | if len(unmatched_descriptors) == 0: 70 | return [] 71 | lookup = dict(zip( 72 | range(len(unmatched_descriptors)), 73 | np.where(self.unmatched)[0])) 74 | 75 | # TODO: reduce matched points by using predicted position 76 | matches = self.matcher.match( 77 | np.array(descriptors), unmatched_descriptors) 78 | return [(m, m.queryIdx, m.trainIdx) for m in matches] 79 | 80 | def row_match(self, *args, **kwargs): 81 | return row_match(self.matcher, *args, **kwargs) 82 | 83 | def circular_stereo_match(self, *args, **kwargs): 84 | return circular_stereo_match(self.matcher, *args, **kwargs) 85 | 86 | def get_keypoint(self, i): 87 | return self.keypoints[i] 88 | def get_descriptor(self, i): 89 | return self.descriptors[i] 90 | 91 | def get_color(self, pt): 92 | x = int(np.clip(pt[0], 0, self.width-1)) 93 | y = int(np.clip(pt[1], 0, self.height-1)) 94 | color = self.image[y, x] 95 | if isinstance(color, Number): 96 | color = np.array([color, color, color]) 97 | return color[::-1] / 255. 98 | 99 | def set_matched(self, i): 100 | with self._lock: 101 | self.unmatched[i] = False 102 | 103 | def get_unmatched_keypoints(self): 104 | keypoints = [] 105 | descriptors = [] 106 | indices = [] 107 | 108 | with self._lock: 109 | for i in np.where(self.unmatched)[0]: 110 | keypoints.append(self.keypoints[i]) 111 | descriptors.append(self.descriptors[i]) 112 | indices.append(i) 113 | 114 | return keypoints, descriptors, indices 115 | 116 | 117 | 118 | # TODO: only match points in neighboring rows 119 | def row_match(matcher, kps1, desps1, kps2, desps2, 120 | matching_distance=40, 121 | max_row_distance=2.5, 122 | max_disparity=100): 123 | 124 | matches = matcher.match(np.array(desps1), np.array(desps2)) 125 | good = [] 126 | for m in matches: 127 | pt1 = kps1[m.queryIdx].pt 128 | pt2 = kps2[m.trainIdx].pt 129 | if (m.distance < matching_distance and 130 | abs(pt1[1] - pt2[1]) < max_row_distance and 131 | abs(pt1[0] - pt2[0]) < max_disparity): # epipolar constraint 132 | good.append(m) 133 | return good 134 | 135 | 136 | 137 | def circular_stereo_match( 138 | matcher, 139 | desps1, desps2, matches12, 140 | desps3, desps4, matches34, 141 | matching_distance=30, 142 | min_matches=10, ratio=0.8): 143 | 144 | dict_m13 = dict() 145 | dict_m24 = dict() 146 | dict_m34 = dict((m.queryIdx, m) for m in matches34) 147 | 148 | ms13 = matcher.knnMatch(np.array(desps1), np.array(desps3), k=2) 149 | for (m, n) in ms13: 150 | if m.distance < min(matching_distance, n.distance * ratio): 151 | dict_m13[m.queryIdx] = m 152 | 153 | # to avoid unnecessary computation 154 | if len(dict_m13) < min_matches: 155 | return [] 156 | 157 | ms24 = matcher.knnMatch(np.array(desps2), np.array(desps4), k=2) 158 | for (m, n) in ms24: 159 | if m.distance < min(matching_distance, n.distance * ratio): 160 | dict_m24[m.queryIdx] = m 161 | 162 | matches = [] 163 | for m in matches12: 164 | shared13 = dict_m13.get(m.queryIdx, None) 165 | shared24 = dict_m24.get(m.trainIdx, None) 166 | 167 | if shared13 is not None and shared24 is not None: 168 | shared34 = dict_m34.get(shared13.trainIdx, None) 169 | if (shared34 is not None and 170 | shared34.trainIdx == shared24.trainIdx): 171 | matches.append((shared13, shared24)) 172 | return matches -------------------------------------------------------------------------------- /imgs/point_cloud.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/uoip/stereo_ptam/294fb977a02cca6a793ed8dca193301b3aae80ec/imgs/point_cloud.png -------------------------------------------------------------------------------- /imgs/pose_graph.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/uoip/stereo_ptam/294fb977a02cca6a793ed8dca193301b3aae80ec/imgs/pose_graph.png -------------------------------------------------------------------------------- /imgs/sptam_overview.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/uoip/stereo_ptam/294fb977a02cca6a793ed8dca193301b3aae80ec/imgs/sptam_overview.png -------------------------------------------------------------------------------- /loopclosing.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import cv2 3 | 4 | import g2o 5 | from g2o.contrib import SmoothEstimatePropagator 6 | 7 | import time 8 | from threading import Thread, Lock 9 | from queue import Queue 10 | 11 | from collections import defaultdict, namedtuple 12 | 13 | from optimization import PoseGraphOptimization 14 | from components import Measurement 15 | 16 | 17 | 18 | # a very simple implementation 19 | class LoopDetection(object): 20 | def __init__(self, params): 21 | self.params = params 22 | self.nns = NearestNeighbors() 23 | 24 | def add_keyframe(self, keyframe): 25 | embedding = keyframe.feature.descriptors.mean(axis=0) 26 | self.nns.add_item(embedding, keyframe) 27 | 28 | def detect(self, keyframe): 29 | embedding = keyframe.feature.descriptors.mean(axis=0) 30 | kfs, ds = self.nns.search(embedding, k=20) 31 | 32 | if len(kfs) > 0 and kfs[0] == keyframe: 33 | kfs, ds = kfs[1:], ds[1:] 34 | if len(kfs) == 0: 35 | return None 36 | 37 | min_d = np.min(ds) 38 | for kf, d in zip(kfs, ds): 39 | if abs(kf.id - keyframe.id) < self.params.lc_min_inbetween_frames: 40 | continue 41 | if (np.linalg.norm(kf.position - keyframe.position) > 42 | self.params.lc_max_inbetween_distance): 43 | break 44 | if d > self.params.lc_embedding_distance or d > min_d * 1.5: 45 | break 46 | return kf 47 | return None 48 | 49 | 50 | 51 | class LoopClosing(object): 52 | def __init__(self, system, params): 53 | self.system = system 54 | self.params = params 55 | 56 | self.loop_detector = LoopDetection(params) 57 | self.optimizer = PoseGraphOptimization() 58 | 59 | self.loops = [] 60 | self.stopped = False 61 | 62 | self._queue = Queue() 63 | self.maintenance_thread = Thread(target=self.maintenance) 64 | self.maintenance_thread.start() 65 | 66 | def stop(self): 67 | self.stopped = True 68 | self._queue.put(None) 69 | self.maintenance_thread.join() 70 | print('loop closing stopped') 71 | 72 | def add_keyframe(self, keyframe): 73 | self._queue.put(keyframe) 74 | self.loop_detector.add_keyframe(keyframe) 75 | 76 | def add_keyframes(self, keyframes): 77 | for kf in keyframes: 78 | self.add_keyframe(kf) 79 | 80 | def maintenance(self): 81 | last_query_keyframe = None 82 | while not self.stopped: 83 | keyframe = self._queue.get() 84 | if keyframe is None or self.stopped: 85 | return 86 | 87 | # check if this keyframe share many mappoints with a loop keyframe 88 | covisible = sorted( 89 | keyframe.covisibility_keyframes().items(), 90 | key=lambda _:_[1], reverse=True) 91 | if any([(keyframe.id - _[0].id) > 5 for _ in covisible[:2]]): 92 | continue 93 | 94 | if (last_query_keyframe is not None and 95 | abs(last_query_keyframe.id - keyframe.id) < 3): 96 | continue 97 | 98 | detected = self.loop_detector.detect(keyframe) 99 | if detected is None: 100 | continue 101 | 102 | query_keyframe = keyframe 103 | match_keyframe = detected 104 | 105 | result = match_and_estimate( 106 | query_keyframe, match_keyframe, self.params) 107 | 108 | if result is None: 109 | continue 110 | if (result.n_inliers < max(self.params.lc_inliers_threshold, 111 | result.n_matches * self.params.lc_inliers_ratio)): 112 | continue 113 | 114 | dist = result.correction.position() 115 | if self.params.ground: 116 | dist = dist[:2] 117 | if np.abs(dist).max() > self.params.lc_distance_threshold: 118 | continue 119 | 120 | self.loops.append( 121 | (match_keyframe, query_keyframe, result.constraint)) 122 | query_keyframe.set_loop(match_keyframe, result.constraint) 123 | 124 | # We have to ensure that the mapping thread is on a safe part of code, 125 | # before the selection of KFs to optimize 126 | safe_window = self.system.mapping.lock_window() 127 | safe_window.add(self.system.reference) 128 | for kf in self.system.reference.covisibility_keyframes(): 129 | safe_window.add(kf) 130 | 131 | 132 | # The safe window established between the Local Mapping must be 133 | # inside the considered KFs. 134 | considered_keyframes = self.system.graph.keyframes() 135 | 136 | self.optimizer.set_data(considered_keyframes, self.loops) 137 | 138 | before_lc = [ 139 | g2o.Isometry3d(kf.orientation, kf.position) for kf in safe_window] 140 | 141 | # Propagate initial estimate through 10% of total keyframes 142 | # (or at least 20 keyframes) 143 | d = max(20, len(considered_keyframes) * 0.1) 144 | propagator = SmoothEstimatePropagator(self.optimizer, d) 145 | propagator.propagate(self.optimizer.vertex(match_keyframe.id)) 146 | 147 | # self.optimizer.set_verbose(True) 148 | self.optimizer.optimize(20) 149 | 150 | # Exclude KFs that may being use by the local BA. 151 | self.optimizer.update_poses_and_points( 152 | considered_keyframes, exclude=safe_window) 153 | 154 | self.system.stop_adding_keyframes() 155 | 156 | # Wait until mapper flushes everything to the map 157 | self.system.mapping.wait_until_empty_queue() 158 | while self.system.mapping.is_processing(): 159 | time.sleep(1e-4) 160 | 161 | # Calculating optimization introduced by local mapping while loop was been closed 162 | for i, kf in enumerate(safe_window): 163 | after_lc = g2o.Isometry3d(kf.orientation, kf.position) 164 | corr = before_lc[i].inverse() * after_lc 165 | 166 | vertex = self.optimizer.vertex(kf.id) 167 | vertex.set_estimate(vertex.estimate() * corr) 168 | 169 | self.system.pause() 170 | 171 | for keyframe in considered_keyframes[::-1]: 172 | if keyframe in safe_window: 173 | reference = keyframe 174 | break 175 | uncorrected = g2o.Isometry3d( 176 | reference.orientation, 177 | reference.position) 178 | corrected = self.optimizer.vertex(reference.id).estimate() 179 | T = uncorrected.inverse() * corrected # close to result.correction 180 | 181 | # We need to wait for the end of the current frame tracking and ensure that we 182 | # won't interfere with the tracker. 183 | while self.system.is_tracking(): 184 | time.sleep(1e-4) 185 | self.system.set_loop_correction(T) 186 | 187 | # Updating keyframes and map points on the lba zone 188 | self.optimizer.update_poses_and_points(safe_window) 189 | 190 | # keyframes after loop closing 191 | keyframes = self.system.graph.keyframes() 192 | if len(keyframes) > len(considered_keyframes): 193 | self.optimizer.update_poses_and_points( 194 | keyframes[len(considered_keyframes) - len(keyframes):], 195 | correction=T) 196 | 197 | for m13, _ in result.stereo_matches: 198 | query_meas = result.query_stereo_measurements[m13.queryIdx] 199 | match_meas = result.match_stereo_measurements[m13.trainIdx] 200 | 201 | new_query_meas = Measurement( 202 | Measurement.Type.STEREO, 203 | Measurement.Source.REFIND, 204 | query_meas.get_keypoints(), 205 | query_meas.get_descriptors()) 206 | self.system.graph.add_measurement( 207 | query_keyframe, match_meas.mappoint, new_query_meas) 208 | 209 | new_match_meas = Measurement( 210 | Measurement.Type.STEREO, 211 | Measurement.Source.REFIND, 212 | match_meas.get_keypoints(), 213 | match_meas.get_descriptors()) 214 | self.system.graph.add_measurement( 215 | match_keyframe, query_meas.mappoint, new_match_meas) 216 | 217 | self.system.mapping.free_window() 218 | self.system.resume_adding_keyframes() 219 | self.system.unpause() 220 | 221 | while not self._queue.empty(): 222 | keyframe = self._queue.get() 223 | if keyframe is None: 224 | return 225 | last_query_keyframe = query_keyframe 226 | 227 | 228 | 229 | def match_and_estimate(query_keyframe, match_keyframe, params): 230 | query = defaultdict(list) 231 | for m in query_keyframe.measurements(): 232 | if m.from_triangulation(): 233 | query['measurements'].append(m) 234 | query['kps1'].append(m.get_keypoint(0)) 235 | query['kps2'].append(m.get_keypoint(1)) 236 | query['desps1'].append(m.get_descriptor(0)) 237 | query['desps2'].append(m.get_descriptor(1)) 238 | n = len(query['matches']) 239 | query['matches'].append(cv2.DMatch(n, n, 0)) 240 | 241 | match = defaultdict(list) 242 | for m in match_keyframe.measurements(): 243 | if m.from_triangulation(): 244 | match['measurements'].append(m) 245 | match['kps1'].append(m.get_keypoint(0)) 246 | match['kps2'].append(m.get_keypoint(1)) 247 | match['desps1'].append(m.get_descriptor(0)) 248 | match['desps2'].append(m.get_descriptor(1)) 249 | n = len(match['matches']) 250 | match['matches'].append(cv2.DMatch(n, n, 0)) 251 | 252 | stereo_matches = query_keyframe.feature.circular_stereo_match( 253 | query['desps1'], query['desps2'], query['matches'], 254 | match['desps1'], match['desps2'], match['matches'], 255 | params.matching_distance, 256 | params.lc_inliers_threshold) 257 | 258 | n_matches = len(stereo_matches) 259 | if n_matches < params.lc_inliers_threshold: 260 | return None 261 | 262 | for m13, _ in stereo_matches: 263 | i, j = m13.queryIdx, m13.trainIdx 264 | query['px'].append(query['kps1'][i].pt) 265 | query['pt'].append(query['measurements'][i].view) 266 | match['px'].append(match['kps1'][j].pt) 267 | match['pt'].append(match['measurements'][j].view) 268 | 269 | # query_keyframe's pose in match_keyframe's coordinates frame 270 | T13, inliers13 = solve_pnp_ransac( 271 | query['pt'], match['px'], match_keyframe.cam.intrinsic) 272 | 273 | T31, inliers31 = solve_pnp_ransac( 274 | match['pt'], query['px'], query_keyframe.cam.intrinsic) 275 | 276 | if T13 is None or T13 is None: 277 | return None 278 | 279 | delta = T31 * T13 280 | if (g2o.AngleAxis(delta.rotation()).angle() > 0.1 or 281 | np.linalg.norm(delta.translation()) > 0.5): # 5.7° or 0.5m 282 | return None 283 | 284 | n_inliers = len(set(inliers13) & set(inliers31)) 285 | query_pose = g2o.Isometry3d( 286 | query_keyframe.orientation, query_keyframe.position) 287 | match_pose = g2o.Isometry3d( 288 | match_keyframe.orientation, match_keyframe.position) 289 | 290 | # TODO: combine T13 and T31 291 | constraint = T13 292 | estimated_pose = match_pose * constraint 293 | correction = query_pose.inverse() * estimated_pose 294 | 295 | return namedtuple('MatchEstimateResult', 296 | ['estimated_pose', 'constraint', 'correction', 'query_stereo_measurements', 297 | 'match_stereo_measurements', 'stereo_matches', 'n_matches', 'n_inliers'])( 298 | estimated_pose, constraint, correction, query['measurements'], 299 | match['measurements'], stereo_matches, n_matches, n_inliers) 300 | 301 | 302 | def solve_pnp_ransac(pts3d, pts, intrinsic_matrix): 303 | val, rvec, tvec, inliers = cv2.solvePnPRansac( 304 | np.array(pts3d), np.array(pts), 305 | intrinsic_matrix, None, None, None, 306 | False, 50, 2.0, 0.99, None) 307 | if inliers is None or len(inliers) < 5: 308 | return None, None 309 | 310 | T = g2o.Isometry3d(cv2.Rodrigues(rvec)[0], tvec) 311 | return T, inliers.ravel() 312 | 313 | 314 | 315 | class NearestNeighbors(object): 316 | def __init__(self, dim=None): 317 | self.n = 0 318 | self.dim = dim 319 | self.items = dict() 320 | self.data = [] 321 | if dim is not None: 322 | self.data = np.zeros((1000, dim), dtype='float32') 323 | 324 | def add_item(self, vector, item): 325 | assert vector.ndim == 1 326 | if self.n >= len(self.data): 327 | if self.dim is None: 328 | self.dim = len(vector) 329 | self.data = np.zeros((1000, self.dim), dtype='float32') 330 | else: 331 | self.data.resize( 332 | (2 * len(self.data), self.dim) , refcheck=False) 333 | self.items[self.n] = item 334 | self.data[self.n] = vector 335 | self.n += 1 336 | 337 | def search(self, query, k): # searching from 100000 items consume 30ms 338 | if len(self.data) == 0: 339 | return [], [] 340 | 341 | ds = np.linalg.norm(query[np.newaxis, :] - self.data[:self.n], axis=1) 342 | ns = np.argsort(ds)[:k] 343 | return [self.items[n] for n in ns], ds[ns] -------------------------------------------------------------------------------- /mapping.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | from queue import Queue 4 | from threading import Thread, Lock, Condition 5 | import time 6 | 7 | from itertools import chain 8 | from collections import defaultdict 9 | 10 | from optimization import LocalBA 11 | from components import Measurement 12 | 13 | 14 | 15 | class Mapping(object): 16 | def __init__(self, graph, params): 17 | self.graph = graph 18 | self.params = params 19 | self.local_keyframes = [] 20 | 21 | self.optimizer = LocalBA() 22 | 23 | def add_keyframe(self, keyframe, measurements): 24 | self.graph.add_keyframe(keyframe) 25 | self.create_points(keyframe) 26 | 27 | for m in measurements: 28 | self.graph.add_measurement(keyframe, m.mappoint, m) 29 | 30 | self.local_keyframes.clear() 31 | self.local_keyframes.append(keyframe) 32 | 33 | self.fill(self.local_keyframes, keyframe) 34 | self.refind(self.local_keyframes, self.get_owned_points(keyframe)) 35 | 36 | self.bundle_adjust(self.local_keyframes) 37 | self.points_culling(self.local_keyframes) 38 | 39 | def fill(self, keyframes, keyframe): 40 | covisible = sorted( 41 | keyframe.covisibility_keyframes().items(), 42 | key=lambda _:_[1], reverse=True) 43 | 44 | for kf, n in covisible: 45 | if n > 0 and kf not in keyframes and self.is_safe(kf): 46 | keyframes.append(kf) 47 | if len(keyframes) >= self.params.local_window_size: 48 | return 49 | 50 | def create_points(self, keyframe): 51 | mappoints, measurements = keyframe.triangulate() 52 | self.add_measurements(keyframe, mappoints, measurements) 53 | 54 | def add_measurements(self, keyframe, mappoints, measurements): 55 | for mappoint, measurement in zip(mappoints, measurements): 56 | self.graph.add_mappoint(mappoint) 57 | self.graph.add_measurement(keyframe, mappoint, measurement) 58 | mappoint.increase_measurement_count() 59 | 60 | def bundle_adjust(self, keyframes): 61 | adjust_keyframes = set() 62 | for kf in keyframes: 63 | if not kf.is_fixed(): 64 | adjust_keyframes.add(kf) 65 | 66 | fixed_keyframes = set() 67 | for kf in adjust_keyframes: 68 | for ck, n in kf.covisibility_keyframes().items(): 69 | if (n > 0 and ck not in adjust_keyframes 70 | and self.is_safe(ck) and ck < kf): 71 | fixed_keyframes.add(ck) 72 | 73 | self.optimizer.set_data(adjust_keyframes, fixed_keyframes) 74 | completed = self.optimizer.optimize(self.params.ba_max_iterations) 75 | 76 | self.optimizer.update_poses() 77 | self.optimizer.update_points() 78 | 79 | if completed: 80 | self.remove_measurements(self.optimizer.get_bad_measurements()) 81 | return completed 82 | 83 | def is_safe(self, keyframe): 84 | return True 85 | 86 | def get_owned_points(self, keyframe): 87 | owned = [] 88 | for m in keyframe.measurements(): 89 | if m.from_triangulation(): 90 | owned.append(m.mappoint) 91 | return owned 92 | 93 | def filter_unmatched_points(self, keyframe, mappoints): 94 | filtered = [] 95 | for i in np.where(keyframe.can_view(mappoints))[0]: 96 | pt = mappoints[i] 97 | if (not pt.is_bad() and 98 | not self.graph.has_measurement(keyframe, pt)): 99 | filtered.append(pt) 100 | return filtered 101 | 102 | def refind(self, keyframes, new_mappoints): # time consuming 103 | if len(new_mappoints) == 0: 104 | return 105 | for keyframe in keyframes: 106 | filtered = self.filter_unmatched_points(keyframe, new_mappoints) 107 | if len(filtered) == 0: 108 | continue 109 | for mappoint in filtered: 110 | mappoint.increase_projection_count() 111 | 112 | measuremets = keyframe.match_mappoints(filtered, Measurement.Source.REFIND) 113 | 114 | for m in measuremets: 115 | self.graph.add_measurement(keyframe, m.mappoint, m) 116 | m.mappoint.increase_measurement_count() 117 | 118 | def remove_measurements(self, measurements): 119 | for m in measurements: 120 | m.mappoint.increase_outlier_count() 121 | self.graph.remove_measurement(m) 122 | 123 | def points_culling(self, keyframes): # Remove bad mappoints 124 | mappoints = set(chain(*[kf.mappoints() for kf in keyframes])) 125 | for pt in mappoints: 126 | if pt.is_bad(): 127 | self.graph.remove_mappoint(pt) 128 | 129 | 130 | 131 | 132 | class MappingThread(Mapping): 133 | def __init__(self, graph, params): 134 | super().__init__(graph, params) 135 | 136 | self._requests_cv = Condition() 137 | self._requests = [False, False] # requests: [LOCKWINDOW_REQUEST, PROCESS_REQUEST] 138 | 139 | self._lock = Lock() 140 | self.locked_window = set() 141 | self.status = defaultdict(bool) 142 | 143 | self._queue = Queue() 144 | self.maintenance_thread = Thread(target=self.maintenance) 145 | self.maintenance_thread.start() 146 | 147 | def add_keyframe(self, keyframe, measurements): 148 | self.graph.add_keyframe(keyframe) 149 | 150 | self.create_points(keyframe) 151 | for m in measurements: 152 | self.graph.add_measurement(keyframe, m.mappoint, m) 153 | 154 | self._queue.put(keyframe) 155 | with self._requests_cv: 156 | self._requests_cv.notify() 157 | 158 | def maintenance(self): 159 | stopped = False 160 | while not stopped: 161 | while not self._queue.empty(): 162 | keyframe = self._queue.get() 163 | if keyframe is None: 164 | stopped = True 165 | self._requests[1] = True 166 | break 167 | else: 168 | self.local_keyframes.append(keyframe) 169 | if len(self.local_keyframes) >= 5: 170 | self._requests[1] = True 171 | break 172 | 173 | with self._requests_cv: 174 | if self._requests.count(True) == 0: 175 | self._requests_cv.wait() 176 | 177 | while not self._queue.empty(): 178 | keyframe = self._queue.get() 179 | if keyframe is None: 180 | stopped = True 181 | self._requests[1] = True 182 | break 183 | else: 184 | self.local_keyframes.append(keyframe) 185 | if len(self.local_keyframes) >= 5: 186 | self._requests[1] = True 187 | 188 | requests = self._requests[:] 189 | self._requests[0] = False 190 | self._requests[1] = False 191 | 192 | self.status['processing'] = True 193 | 194 | if requests[1] and len(self.local_keyframes) > 0: 195 | self.fill(self.local_keyframes, self.local_keyframes[-1]) 196 | 197 | if requests[0]: 198 | with self._lock: 199 | for kf in self.local_keyframes: 200 | self.locked_window.add(kf) 201 | for ck, n in kf.covisibility_keyframes().items(): 202 | if n > 0: 203 | self.locked_window.add(ck) 204 | self.status['window_locked'] = True 205 | 206 | if requests[1] and len(self.local_keyframes) > 0: 207 | completed = self.bundle_adjust(self.local_keyframes) 208 | if completed: 209 | self.points_culling(self.local_keyframes) 210 | self.local_keyframes.clear() 211 | 212 | self.status['processing'] = False 213 | 214 | def stop(self): 215 | with self._requests_cv: 216 | self._requests_cv.notify() 217 | 218 | while not self._queue.empty(): 219 | time.sleep(1e-4) 220 | self._queue.put(None) # sentinel value 221 | self.maintenance_thread.join() 222 | print('mapping stopped') 223 | 224 | def is_safe(self, keyframe): 225 | with self._lock: 226 | return not self.is_window_locked() or keyframe in self.locked_window 227 | 228 | def is_processing(self): 229 | return self.status['processing'] 230 | 231 | def lock_window(self): 232 | with self._lock: 233 | self.status['window_locked'] = False 234 | self.locked_window.clear() 235 | 236 | with self._requests_cv: 237 | self._requests[0] = True 238 | self._requests_cv.notify() 239 | 240 | while not self.is_window_locked(): 241 | time.sleep(1e-4) 242 | return self.locked_window 243 | 244 | def free_window(self): 245 | with self._lock: 246 | self.status['window_locked'] = False 247 | self.locked_window.clear() 248 | 249 | def is_window_locked(self): 250 | return self.status['window_locked'] 251 | 252 | def wait_until_empty_queue(self): 253 | while not self._queue.empty(): 254 | time.sleep(1e-4) 255 | 256 | def interrupt_ba(self): 257 | self.optimizer.abort() -------------------------------------------------------------------------------- /motion.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import g2o 3 | 4 | 5 | 6 | class MotionModel(object): 7 | def __init__(self, 8 | timestamp=None, 9 | initial_position=np.zeros(3), 10 | initial_orientation=g2o.Quaternion(), 11 | initial_covariance=None): 12 | 13 | self.timestamp = timestamp 14 | self.position = initial_position 15 | self.orientation = initial_orientation 16 | self.covariance = initial_covariance # pose covariance 17 | 18 | self.v_linear = np.zeros(3) # linear velocity 19 | self.v_angular_angle = 0 20 | self.v_angular_axis = np.array([1, 0, 0]) 21 | 22 | self.initialized = False 23 | # damping factor 24 | self.damp = 0.95 25 | 26 | def current_pose(self): 27 | ''' 28 | Get the current camera pose. 29 | ''' 30 | return (g2o.Isometry3d(self.orientation, self.position), 31 | self.covariance) 32 | 33 | def predict_pose(self, timestamp): 34 | ''' 35 | Predict the next camera pose. 36 | ''' 37 | if not self.initialized: 38 | return (g2o.Isometry3d(self.orientation, self.position), 39 | self.covariance) 40 | 41 | dt = timestamp - self.timestamp 42 | 43 | delta_angle = g2o.AngleAxis( 44 | self.v_angular_angle * dt * self.damp, 45 | self.v_angular_axis) 46 | delta_orientation = g2o.Quaternion(delta_angle) 47 | 48 | position = self.position + self.v_linear * dt * self.damp 49 | orientation = self.orientation * delta_orientation 50 | 51 | return (g2o.Isometry3d(orientation, position), self.covariance) 52 | 53 | def update_pose(self, timestamp, 54 | new_position, new_orientation, new_covariance=None): 55 | ''' 56 | Update the motion model when given a new camera pose. 57 | ''' 58 | if self.initialized: 59 | dt = timestamp - self.timestamp 60 | assert dt != 0 61 | 62 | v_linear = (new_position - self.position) / dt 63 | self.v_linear = v_linear 64 | 65 | delta_q = self.orientation.inverse() * new_orientation 66 | delta_q.normalize() 67 | 68 | delta_angle = g2o.AngleAxis(delta_q) 69 | angle = delta_angle.angle() 70 | axis = delta_angle.axis() 71 | 72 | if angle > np.pi: 73 | axis = axis * -1 74 | angle = 2 * np.pi - angle 75 | 76 | self.v_angular_axis = axis 77 | self.v_angular_angle = angle / dt 78 | 79 | self.timestamp = timestamp 80 | self.position = new_position 81 | self.orientation = new_orientation 82 | self.covariance = new_covariance 83 | self.initialized = True 84 | 85 | def apply_correction(self, correction): # corr: g2o.Isometry3d or matrix44 86 | ''' 87 | Reset the model given a new camera pose. 88 | Note: This method will be called when it happens an abrupt change in the pose (LoopClosing) 89 | ''' 90 | if not isinstance(correction, g2o.Isometry3d): 91 | correction = g2o.Isometry3d(correction) 92 | 93 | current = g2o.Isometry3d(self.orientation, self.position) 94 | current = current * correction 95 | 96 | self.position = current.position() 97 | self.orientation = current.orientation() 98 | 99 | self.v_linear = ( 100 | correction.inverse().orientation() * self.v_linear) 101 | self.v_angular_axis = ( 102 | correction.inverse().orientation() * self.v_angular_axis) -------------------------------------------------------------------------------- /optimization.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import g2o 3 | 4 | 5 | 6 | class BundleAdjustment(g2o.SparseOptimizer): 7 | def __init__(self, ): 8 | super().__init__() 9 | 10 | # Higher confident (better than CHOLMOD, according to 11 | # paper "3-D Mapping With an RGB-D Camera") 12 | solver = g2o.BlockSolverSE3(g2o.LinearSolverCSparseSE3()) 13 | solver = g2o.OptimizationAlgorithmLevenberg(solver) 14 | super().set_algorithm(solver) 15 | 16 | # Convergence Criterion 17 | terminate = g2o.SparseOptimizerTerminateAction() 18 | terminate.set_gain_threshold(1e-6) 19 | super().add_post_iteration_action(terminate) 20 | 21 | # Robust cost Function (Huber function) delta 22 | self.delta = np.sqrt(5.991) 23 | self.aborted = False 24 | 25 | def optimize(self, max_iterations=10): 26 | super().initialize_optimization() 27 | super().optimize(max_iterations) 28 | try: 29 | return not self.aborted 30 | finally: 31 | self.aborted = False 32 | 33 | def add_pose(self, pose_id, pose, cam, fixed=False): 34 | sbacam = g2o.SBACam( 35 | pose.orientation(), pose.position()) 36 | sbacam.set_cam( 37 | cam.fx, cam.fy, cam.cx, cam.cy, cam.baseline) 38 | 39 | v_se3 = g2o.VertexCam() 40 | v_se3.set_id(pose_id * 2) 41 | v_se3.set_estimate(sbacam) 42 | v_se3.set_fixed(fixed) 43 | super().add_vertex(v_se3) 44 | 45 | def add_point(self, point_id, point, fixed=False, marginalized=True): 46 | v_p = g2o.VertexSBAPointXYZ() 47 | v_p.set_id(point_id * 2 + 1) 48 | v_p.set_marginalized(marginalized) 49 | v_p.set_estimate(point) 50 | v_p.set_fixed(fixed) 51 | super().add_vertex(v_p) 52 | 53 | def add_edge(self, id, point_id, pose_id, meas): 54 | if meas.is_stereo(): 55 | edge = self.stereo_edge(meas.xyx) 56 | elif meas.is_left(): 57 | edge = self.mono_edge(meas.xy) 58 | elif meas.is_right(): 59 | edge = self.mono_edge_right(meas.xy) 60 | 61 | edge.set_id(id) 62 | edge.set_vertex(0, self.vertex(point_id * 2 + 1)) 63 | edge.set_vertex(1, self.vertex(pose_id * 2)) 64 | kernel = g2o.RobustKernelHuber(self.delta) 65 | edge.set_robust_kernel(kernel) 66 | super().add_edge(edge) 67 | 68 | def stereo_edge(self, projection, information=np.identity(3)): 69 | e = g2o.EdgeProjectP2SC() 70 | e.set_measurement(projection) 71 | e.set_information(information) 72 | return e 73 | 74 | def mono_edge(self, projection, 75 | information=np.identity(2) * 0.5): 76 | e = g2o.EdgeProjectP2MC() 77 | e.set_measurement(projection) 78 | e.set_information(information) 79 | return e 80 | 81 | def mono_edge_right(self, projection, 82 | information=np.identity(2) * 0.5): 83 | e = g2o.EdgeProjectP2MCRight() 84 | e.set_measurement(projection) 85 | e.set_information(information) 86 | return e 87 | 88 | def get_pose(self, id): 89 | return self.vertex(id * 2).estimate() 90 | 91 | def get_point(self, id): 92 | return self.vertex(id * 2 + 1).estimate() 93 | 94 | def abort(self): 95 | self.aborted = True 96 | 97 | 98 | 99 | class LocalBA(object): 100 | def __init__(self, ): 101 | self.optimizer = BundleAdjustment() 102 | self.measurements = [] 103 | self.keyframes = [] 104 | self.mappoints = set() 105 | 106 | # threshold for confidence interval of 95% 107 | self.huber_threshold = 5.991 108 | 109 | def set_data(self, adjust_keyframes, fixed_keyframes): 110 | self.clear() 111 | for kf in adjust_keyframes: 112 | self.optimizer.add_pose(kf.id, kf.pose, kf.cam, fixed=False) 113 | self.keyframes.append(kf) 114 | 115 | for m in kf.measurements(): 116 | pt = m.mappoint 117 | if pt not in self.mappoints: 118 | self.optimizer.add_point(pt.id, pt.position) 119 | self.mappoints.add(pt) 120 | 121 | edge_id = len(self.measurements) 122 | self.optimizer.add_edge(edge_id, pt.id, kf.id, m) 123 | self.measurements.append(m) 124 | 125 | for kf in fixed_keyframes: 126 | self.optimizer.add_pose(kf.id, kf.pose, kf.cam, fixed=True) 127 | for m in kf.measurements(): 128 | if m.mappoint in self.mappoints: 129 | edge_id = len(self.measurements) 130 | self.optimizer.add_edge(edge_id, m.mappoint.id, kf.id, m) 131 | self.measurements.append(m) 132 | 133 | def update_points(self): 134 | for mappoint in self.mappoints: 135 | mappoint.update_position(self.optimizer.get_point(mappoint.id)) 136 | 137 | def update_poses(self): 138 | for keyframe in self.keyframes: 139 | keyframe.update_pose(self.optimizer.get_pose(keyframe.id)) 140 | keyframe.update_reference() 141 | keyframe.update_preceding() 142 | 143 | def get_bad_measurements(self): 144 | bad_measurements = [] 145 | for edge in self.optimizer.active_edges(): 146 | if edge.chi2() > self.huber_threshold: 147 | bad_measurements.append(self.measurements[edge.id()]) 148 | return bad_measurements 149 | 150 | def clear(self): 151 | self.optimizer.clear() 152 | self.keyframes.clear() 153 | self.mappoints.clear() 154 | self.measurements.clear() 155 | 156 | def abort(self): 157 | self.optimizer.abort() 158 | 159 | def optimize(self, max_iterations): 160 | return self.optimizer.optimize(max_iterations) 161 | 162 | 163 | 164 | 165 | class PoseGraphOptimization(g2o.SparseOptimizer): 166 | def __init__(self): 167 | super().__init__() 168 | solver = g2o.BlockSolverSE3(g2o.LinearSolverCholmodSE3()) 169 | solver = g2o.OptimizationAlgorithmLevenberg(solver) 170 | super().set_algorithm(solver) 171 | 172 | def optimize(self, max_iterations=20): 173 | super().initialize_optimization() 174 | super().optimize(max_iterations) 175 | 176 | def add_vertex(self, id, pose, fixed=False): 177 | v_se3 = g2o.VertexSE3() 178 | v_se3.set_id(id) 179 | v_se3.set_estimate(pose) 180 | v_se3.set_fixed(fixed) 181 | super().add_vertex(v_se3) 182 | 183 | def add_edge(self, vertices, 184 | measurement=None, 185 | information=np.identity(6), 186 | robust_kernel=None): 187 | 188 | edge = g2o.EdgeSE3() 189 | for i, v in enumerate(vertices): 190 | if isinstance(v, int): 191 | v = self.vertex(v) 192 | edge.set_vertex(i, v) 193 | 194 | if measurement is None: 195 | measurement = ( 196 | edge.vertex(0).estimate().inverse() * 197 | edge.vertex(1).estimate()) 198 | edge.set_measurement(measurement) 199 | edge.set_information(information) 200 | if robust_kernel is not None: 201 | edge.set_robust_kernel(robust_kernel) 202 | super().add_edge(edge) 203 | 204 | 205 | def set_data(self, keyframes, loops): 206 | super().clear() 207 | anchor=None 208 | for kf, *_ in loops: 209 | if anchor is None or kf < anchor: 210 | anchor = kf 211 | 212 | for i, kf in enumerate(keyframes): 213 | pose = g2o.Isometry3d( 214 | kf.orientation, 215 | kf.position) 216 | 217 | fixed = i == 0 218 | if anchor is not None: 219 | fixed = kf <= anchor 220 | self.add_vertex(kf.id, pose, fixed=fixed) 221 | 222 | if kf.preceding_keyframe is not None: 223 | self.add_edge( 224 | vertices=(kf.preceding_keyframe.id, kf.id), 225 | measurement=kf.preceding_constraint) 226 | 227 | if (kf.reference_keyframe is not None and 228 | kf.reference_keyframe != kf.preceding_keyframe): 229 | self.add_edge( 230 | vertices=(kf.reference_keyframe.id, kf.id), 231 | measurement=kf.reference_constraint) 232 | 233 | for kf, kf2, meas in loops: 234 | self.add_edge((kf.id, kf2.id), measurement=meas) 235 | 236 | 237 | def update_poses_and_points( 238 | self, keyframes, correction=None, exclude=set()): 239 | 240 | for kf in keyframes: 241 | if len(exclude) > 0 and kf in exclude: 242 | continue 243 | uncorrected = g2o.Isometry3d(kf.orientation, kf.position) 244 | if correction is None: 245 | vertex = self.vertex(kf.id) 246 | if vertex.fixed(): 247 | continue 248 | corrected = vertex.estimate() 249 | else: 250 | corrected = uncorrected * correction 251 | 252 | delta = uncorrected.inverse() * corrected 253 | if (g2o.AngleAxis(delta.rotation()).angle() < 0.02 and 254 | np.linalg.norm(delta.translation()) < 0.03): # 1°, 3cm 255 | continue 256 | 257 | for m in kf.measurements(): 258 | if m.from_triangulation(): 259 | old = m.mappoint.position 260 | new = corrected * (uncorrected.inverse() * old) 261 | m.mappoint.update_position(new) 262 | # update normal ? 263 | kf.update_pose(corrected) -------------------------------------------------------------------------------- /params.py: -------------------------------------------------------------------------------- 1 | import cv2 2 | 3 | 4 | 5 | class Params(object): 6 | def __init__(self): 7 | 8 | self.pnp_min_measurements = 10 9 | self.pnp_max_iterations = 10 10 | self.init_min_points = 10 11 | 12 | self.local_window_size = 10 13 | self.ba_max_iterations = 10 14 | 15 | self.min_tracked_points_ratio = 0.5 16 | 17 | self.lc_min_inbetween_frames = 10 # frames 18 | self.lc_max_inbetween_distance = 3 # meters 19 | self.lc_embedding_distance = 22.0 20 | self.lc_inliers_threshold = 15 21 | self.lc_inliers_ratio = 0.5 22 | self.lc_distance_threshold = 2 # meters 23 | self.lc_max_iterations = 20 24 | 25 | self.ground = False 26 | 27 | self.view_camera_size = 1 28 | 29 | 30 | 31 | class ParamsEuroc(Params): 32 | 33 | def __init__(self, config='GFTT-BRIEF'): 34 | super().__init__() 35 | 36 | if config == 'GFTT-BRIEF': 37 | self.feature_detector = cv2.GFTTDetector_create( 38 | maxCorners=1000, minDistance=15.0, 39 | qualityLevel=0.001, useHarrisDetector=False) 40 | 41 | self.descriptor_extractor = cv2.xfeatures2d.BriefDescriptorExtractor_create( 42 | bytes=32, use_orientation=False) 43 | 44 | elif config == 'ORB-BRIEF': 45 | self.feature_detector = cv2.ORB_create( 46 | nfeatures=200, scaleFactor=1.2, nlevels=1, edgeThreshold=31) 47 | 48 | self.descriptor_extractor = cv2.xfeatures2d.BriefDescriptorExtractor_create( 49 | bytes=32, use_orientation=False) 50 | 51 | else: 52 | raise NotImplementedError 53 | 54 | self.descriptor_matcher = cv2.BFMatcher(cv2.NORM_HAMMING, crossCheck=False) 55 | 56 | self.matching_cell_size = 15 # pixels 57 | self.matching_neighborhood = 2 58 | self.matching_distance = 25 59 | 60 | self.frustum_near = 0.1 # meters 61 | self.frustum_far = 50.0 62 | 63 | self.lc_max_inbetween_distance = 4 # meters 64 | self.lc_distance_threshold = 1.5 65 | self.lc_embedding_distance = 22.0 66 | 67 | self.view_image_width = 400 68 | self.view_image_height = 250 69 | self.view_camera_width = 0.1 70 | self.view_viewpoint_x = 0 71 | self.view_viewpoint_y = -1 72 | self.view_viewpoint_z = -10 73 | self.view_viewpoint_f = 2000 74 | 75 | 76 | 77 | 78 | class ParamsKITTI(Params): 79 | def __init__(self, config='GFTT-BRIEF'): 80 | super().__init__() 81 | 82 | if config == 'GFTT-BRIEF': 83 | self.feature_detector = cv2.GFTTDetector_create( 84 | maxCorners=1000, minDistance=12.0, 85 | qualityLevel=0.001, useHarrisDetector=False) 86 | 87 | self.descriptor_extractor = cv2.xfeatures2d.BriefDescriptorExtractor_create( 88 | bytes=32, use_orientation=False) 89 | 90 | elif config == 'GFTT-BRISK': 91 | self.feature_detector = cv2.GFTTDetector_create( 92 | maxCorners=2000, minDistance=15.0, 93 | qualityLevel=0.01, useHarrisDetector=False) 94 | 95 | self.descriptor_extractor = cv2.BRISK_create() 96 | 97 | elif config == 'ORB-ORB': 98 | self.feature_detector = cv2.ORB_create( 99 | nfeatures=1000, scaleFactor=1.2, nlevels=1, edgeThreshold=31) 100 | self.descriptor_extractor = self.feature_detector 101 | 102 | else: 103 | raise NotImplementedError 104 | 105 | self.descriptor_matcher = cv2.BFMatcher(cv2.NORM_HAMMING, crossCheck=False) 106 | 107 | self.matching_cell_size = 15 # pixels 108 | self.matching_neighborhood = 3 109 | self.matching_distance = 30 110 | 111 | self.frustum_near = 0.1 # meters 112 | self.frustum_far = 1000.0 113 | 114 | self.ground = True 115 | 116 | self.lc_max_inbetween_distance = 50 117 | self.lc_distance_threshold = 15 118 | self.lc_embedding_distance = 20.0 119 | 120 | self.view_image_width = 400 121 | self.view_image_height = 130 122 | self.view_camera_width = 0.75 123 | self.view_viewpoint_x = 0 124 | self.view_viewpoint_y = -500 # -10 125 | self.view_viewpoint_z = -100 # -0.1 126 | self.view_viewpoint_f = 2000 -------------------------------------------------------------------------------- /sptam.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | import time 4 | from itertools import chain 5 | from collections import defaultdict 6 | 7 | from covisibility import CovisibilityGraph 8 | from optimization import BundleAdjustment 9 | from mapping import Mapping 10 | from mapping import MappingThread 11 | from components import Measurement 12 | from motion import MotionModel 13 | from loopclosing import LoopClosing 14 | 15 | 16 | 17 | class Tracking(object): 18 | def __init__(self, params): 19 | self.optimizer = BundleAdjustment() 20 | self.min_measurements = params.pnp_min_measurements 21 | self.max_iterations = params.pnp_max_iterations 22 | 23 | def refine_pose(self, pose, cam, measurements): 24 | assert len(measurements) >= self.min_measurements, ( 25 | 'Not enough points') 26 | 27 | self.optimizer.clear() 28 | self.optimizer.add_pose(0, pose, cam, fixed=False) 29 | 30 | for i, m in enumerate(measurements): 31 | self.optimizer.add_point(i, m.mappoint.position, fixed=True) 32 | self.optimizer.add_edge(0, i, 0, m) 33 | 34 | self.optimizer.optimize(self.max_iterations) 35 | return self.optimizer.get_pose(0) 36 | 37 | 38 | 39 | class SPTAM(object): 40 | def __init__(self, params): 41 | self.params = params 42 | 43 | self.tracker = Tracking(params) 44 | self.motion_model = MotionModel() 45 | 46 | self.graph = CovisibilityGraph() 47 | self.mapping = MappingThread(self.graph, params) 48 | 49 | self.loop_closing = LoopClosing(self, params) 50 | self.loop_correction = None 51 | 52 | self.reference = None # reference keyframe 53 | self.preceding = None # last keyframe 54 | self.current = None # current frame 55 | self.status = defaultdict(bool) 56 | 57 | def stop(self): 58 | self.mapping.stop() 59 | if self.loop_closing is not None: 60 | self.loop_closing.stop() 61 | 62 | def initialize(self, frame): 63 | mappoints, measurements = frame.triangulate() 64 | assert len(mappoints) >= self.params.init_min_points, ( 65 | 'Not enough points to initialize map.') 66 | 67 | keyframe = frame.to_keyframe() 68 | keyframe.set_fixed(True) 69 | self.graph.add_keyframe(keyframe) 70 | self.mapping.add_measurements(keyframe, mappoints, measurements) 71 | if self.loop_closing is not None: 72 | self.loop_closing.add_keyframe(keyframe) 73 | 74 | self.reference = keyframe 75 | self.preceding = keyframe 76 | self.current = keyframe 77 | self.status['initialized'] = True 78 | 79 | self.motion_model.update_pose( 80 | frame.timestamp, frame.position, frame.orientation) 81 | 82 | def track(self, frame): 83 | while self.is_paused(): 84 | time.sleep(1e-4) 85 | self.set_tracking(True) 86 | 87 | self.current = frame 88 | print('Tracking:', frame.idx, ' <- ', self.reference.id, self.reference.idx) 89 | 90 | predicted_pose, _ = self.motion_model.predict_pose(frame.timestamp) 91 | frame.update_pose(predicted_pose) 92 | 93 | if self.loop_closing is not None: 94 | if self.loop_correction is not None: 95 | estimated_pose = g2o.Isometry3d( 96 | frame.orientation, 97 | frame.position) 98 | estimated_pose = estimated_pose * self.loop_correction 99 | frame.update_pose(estimated_pose) 100 | self.motion_model.apply_correction(self.loop_correction) 101 | self.loop_correction = None 102 | 103 | local_mappoints = self.filter_points(frame) 104 | measurements = frame.match_mappoints( 105 | local_mappoints, Measurement.Source.TRACKING) 106 | 107 | print('measurements:', len(measurements), ' ', len(local_mappoints)) 108 | 109 | tracked_map = set() 110 | for m in measurements: 111 | mappoint = m.mappoint 112 | mappoint.update_descriptor(m.get_descriptor()) 113 | mappoint.increase_measurement_count() 114 | tracked_map.add(mappoint) 115 | 116 | try: 117 | self.reference = self.graph.get_reference_frame(tracked_map) 118 | 119 | pose = self.tracker.refine_pose(frame.pose, frame.cam, measurements) 120 | frame.update_pose(pose) 121 | self.motion_model.update_pose( 122 | frame.timestamp, pose.position(), pose.orientation()) 123 | tracking_is_ok = True 124 | except: 125 | tracking_is_ok = False 126 | print('tracking failed!!!') 127 | 128 | if tracking_is_ok and self.should_be_keyframe(frame, measurements): 129 | print('new keyframe', frame.idx) 130 | keyframe = frame.to_keyframe() 131 | keyframe.update_reference(self.reference) 132 | keyframe.update_preceding(self.preceding) 133 | 134 | self.mapping.add_keyframe(keyframe, measurements) 135 | if self.loop_closing is not None: 136 | self.loop_closing.add_keyframe(keyframe) 137 | self.preceding = keyframe 138 | 139 | self.set_tracking(False) 140 | 141 | 142 | def filter_points(self, frame): 143 | local_mappoints = self.graph.get_local_map_v2( 144 | [self.preceding, self.reference])[0] 145 | 146 | can_view = frame.can_view(local_mappoints) 147 | print('filter points:', len(local_mappoints), can_view.sum(), 148 | len(self.preceding.mappoints()), 149 | len(self.reference.mappoints())) 150 | 151 | checked = set() 152 | filtered = [] 153 | for i in np.where(can_view)[0]: 154 | pt = local_mappoints[i] 155 | if pt.is_bad(): 156 | continue 157 | pt.increase_projection_count() 158 | filtered.append(pt) 159 | checked.add(pt) 160 | 161 | for reference in set([self.preceding, self.reference]): 162 | for pt in reference.mappoints(): # neglect can_view test 163 | if pt in checked or pt.is_bad(): 164 | continue 165 | pt.increase_projection_count() 166 | filtered.append(pt) 167 | 168 | return filtered 169 | 170 | 171 | def should_be_keyframe(self, frame, measurements): 172 | if self.adding_keyframes_stopped(): 173 | return False 174 | 175 | n_matches = len(measurements) 176 | n_matches_ref = len(self.reference.measurements()) 177 | 178 | print('keyframe check:', n_matches, ' ', n_matches_ref) 179 | 180 | return ((n_matches / n_matches_ref) < 181 | self.params.min_tracked_points_ratio) or n_matches < 20 182 | 183 | 184 | def set_loop_correction(self, T): 185 | self.loop_correction = T 186 | 187 | def is_initialized(self): 188 | return self.status['initialized'] 189 | 190 | def pause(self): 191 | self.status['paused'] = True 192 | 193 | def unpause(self): 194 | self.status['paused'] = False 195 | 196 | def is_paused(self): 197 | return self.status['paused'] 198 | 199 | def is_tracking(self): 200 | return self.status['tracking'] 201 | 202 | def set_tracking(self, status): 203 | self.status['tracking'] = status 204 | 205 | def stop_adding_keyframes(self): 206 | self.status['adding_keyframes_stopped'] = True 207 | 208 | def resume_adding_keyframes(self): 209 | self.status['adding_keyframes_stopped'] = False 210 | 211 | def adding_keyframes_stopped(self): 212 | return self.status['adding_keyframes_stopped'] 213 | 214 | 215 | 216 | 217 | 218 | if __name__ == '__main__': 219 | import cv2 220 | import g2o 221 | 222 | import os 223 | import sys 224 | import argparse 225 | 226 | from threading import Thread 227 | 228 | from components import Camera 229 | from components import StereoFrame 230 | from feature import ImageFeature 231 | from params import ParamsKITTI, ParamsEuroc 232 | from dataset import KITTIOdometry, EuRoCDataset 233 | 234 | 235 | parser = argparse.ArgumentParser() 236 | parser.add_argument('--no-viz', action='store_true', help='do not visualize') 237 | parser.add_argument('--dataset', type=str, help='dataset (KITTI/EuRoC)', 238 | default='KITTI') 239 | parser.add_argument('--path', type=str, help='dataset path', 240 | default='path/to/your/KITTI_odometry/sequences/00') 241 | args = parser.parse_args() 242 | 243 | if args.dataset.lower() == 'kitti': 244 | params = ParamsKITTI() 245 | dataset = KITTIOdometry(args.path) 246 | elif args.dataset.lower() == 'euroc': 247 | params = ParamsEuroc() 248 | dataset = EuRoCDataset(args.path) 249 | 250 | sptam = SPTAM(params) 251 | 252 | visualize = not args.no_viz 253 | if visualize: 254 | from viewer import MapViewer 255 | viewer = MapViewer(sptam, params) 256 | 257 | 258 | cam = Camera( 259 | dataset.cam.fx, dataset.cam.fy, dataset.cam.cx, dataset.cam.cy, 260 | dataset.cam.width, dataset.cam.height, 261 | params.frustum_near, params.frustum_far, 262 | dataset.cam.baseline) 263 | 264 | 265 | 266 | durations = [] 267 | for i in range(len(dataset))[:100]: 268 | featurel = ImageFeature(dataset.left[i], params) 269 | featurer = ImageFeature(dataset.right[i], params) 270 | timestamp = dataset.timestamps[i] 271 | 272 | time_start = time.time() 273 | 274 | t = Thread(target=featurer.extract) 275 | t.start() 276 | featurel.extract() 277 | t.join() 278 | 279 | frame = StereoFrame(i, g2o.Isometry3d(), featurel, featurer, cam, timestamp=timestamp) 280 | 281 | if not sptam.is_initialized(): 282 | sptam.initialize(frame) 283 | else: 284 | sptam.track(frame) 285 | 286 | 287 | duration = time.time() - time_start 288 | durations.append(duration) 289 | print('duration', duration) 290 | print() 291 | print() 292 | 293 | if visualize: 294 | viewer.update() 295 | 296 | print('num frames', len(durations)) 297 | print('num keyframes', len(sptam.graph.keyframes())) 298 | print('average time', np.mean(durations)) 299 | 300 | 301 | sptam.stop() 302 | if visualize: 303 | viewer.stop() -------------------------------------------------------------------------------- /viewer.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import cv2 3 | 4 | import OpenGL.GL as gl 5 | import pangolin 6 | 7 | import time 8 | from multiprocessing import Process, Queue 9 | 10 | 11 | 12 | class DynamicArray(object): 13 | def __init__(self, shape=3): 14 | if isinstance(shape, int): 15 | shape = (shape,) 16 | assert isinstance(shape, tuple) 17 | 18 | self.data = np.zeros((1000, *shape)) 19 | self.shape = shape 20 | self.ind = 0 21 | 22 | def clear(self): 23 | self.ind = 0 24 | 25 | def append(self, x): 26 | self.extend([x]) 27 | 28 | def extend(self, xs): 29 | if len(xs) == 0: 30 | return 31 | assert np.array(xs[0]).shape == self.shape 32 | 33 | if self.ind + len(xs) >= len(self.data): 34 | self.data.resize( 35 | (2 * len(self.data), *self.shape) , refcheck=False) 36 | 37 | if isinstance(xs, np.ndarray): 38 | self.data[self.ind:self.ind+len(xs)] = xs 39 | else: 40 | for i, x in enumerate(xs): 41 | self.data[self.ind+i] = x 42 | self.ind += len(xs) 43 | 44 | def array(self): 45 | return self.data[:self.ind] 46 | 47 | def __len__(self): 48 | return self.ind 49 | 50 | def __getitem__(self, i): 51 | assert i < self.ind 52 | return self.data[i] 53 | 54 | def __iter__(self): 55 | for x in self.data[:self.ind]: 56 | yield x 57 | 58 | 59 | 60 | 61 | class MapViewer(object): 62 | def __init__(self, system=None, config=None): 63 | self.system = system 64 | self.config = config 65 | 66 | self.saved_keyframes = set() 67 | 68 | # data queue 69 | self.q_pose = Queue() 70 | self.q_active = Queue() 71 | self.q_points = Queue() 72 | self.q_colors = Queue() 73 | self.q_graph = Queue() 74 | self.q_camera = Queue() 75 | self.q_image = Queue() 76 | 77 | # message queue 78 | self.q_refresh = Queue() 79 | # self.q_quit = Queue() 80 | 81 | self.view_thread = Process(target=self.view) 82 | self.view_thread.start() 83 | 84 | def update(self, refresh=False): 85 | while not self.q_refresh.empty(): 86 | refresh = self.q_refresh.get() 87 | 88 | self.q_image.put(self.system.current.image) 89 | self.q_pose.put(self.system.current.pose.matrix()) 90 | 91 | points = [] 92 | for m in self.system.reference.measurements(): 93 | if m.from_triangulation(): 94 | points.append(m.mappoint.position) 95 | self.q_active.put(points) 96 | 97 | lines = [] 98 | for kf in self.system.graph.keyframes(): 99 | if kf.reference_keyframe is not None: 100 | lines.append(([*kf.position, *kf.reference_keyframe.position], 0)) 101 | if kf.preceding_keyframe != kf.reference_keyframe: 102 | lines.append(([*kf.position, *kf.preceding_keyframe.position], 1)) 103 | if kf.loop_keyframe is not None: 104 | lines.append(([*kf.position, *kf.loop_keyframe.position], 2)) 105 | self.q_graph.put(lines) 106 | 107 | 108 | if refresh: 109 | print('****************************************************************', 'refresh') 110 | cameras = [] 111 | for kf in self.system.graph.keyframes(): 112 | cameras.append(kf.pose.matrix()) 113 | self.q_camera.put(cameras) 114 | 115 | 116 | points = [] 117 | colors = [] 118 | for pt in self.system.graph.mappoints(): 119 | points.append(pt.position) 120 | colors.append(pt.color) 121 | if len(points) > 0: 122 | self.q_points.put((points, 0)) 123 | self.q_colors.put((colors, 0)) 124 | else: 125 | cameras = [] 126 | points = [] 127 | colors = [] 128 | for kf in self.system.graph.keyframes()[-20:]: 129 | if kf.id not in self.saved_keyframes: 130 | cameras.append(kf.pose.matrix()) 131 | self.saved_keyframes.add(kf.id) 132 | for m in kf.measurements(): 133 | if m.from_triangulation(): 134 | points.append(m.mappoint.position) 135 | colors.append(m.mappoint.color) 136 | if len(cameras) > 0: 137 | self.q_camera.put(cameras) 138 | if len(points) > 0: 139 | self.q_points.put((points, 1)) 140 | self.q_colors.put((colors, 1)) 141 | 142 | 143 | def stop(self): 144 | self.update(refresh=True) 145 | self.view_thread.join() 146 | 147 | qtype = type(Queue()) 148 | for x in self.__dict__.values(): 149 | if isinstance(x, qtype): 150 | while not x.empty(): 151 | _ = x.get() 152 | print('viewer stopped') 153 | 154 | 155 | def view(self): 156 | pangolin.CreateWindowAndBind('Viewer', 1024, 768) 157 | 158 | gl.glEnable(gl.GL_DEPTH_TEST) 159 | gl.glEnable(gl.GL_BLEND) 160 | gl.glBlendFunc (gl.GL_SRC_ALPHA, gl.GL_ONE_MINUS_SRC_ALPHA) 161 | 162 | panel = pangolin.CreatePanel('menu') 163 | panel.SetBounds(0.5, 1.0, 0.0, 175 / 1024.) 164 | 165 | # checkbox 166 | m_follow_camera = pangolin.VarBool('menu.Follow Camera', value=True, toggle=True) 167 | m_show_points = pangolin.VarBool('menu.Show Points', True, True) 168 | m_show_keyframes = pangolin.VarBool('menu.Show KeyFrames', True, True) 169 | m_show_graph = pangolin.VarBool('menu.Show Graph', True, True) 170 | m_show_image = pangolin.VarBool('menu.Show Image', True, True) 171 | 172 | # button 173 | m_replay = pangolin.VarBool('menu.Replay', value=False, toggle=False) 174 | m_refresh = pangolin.VarBool('menu.Refresh', False, False) 175 | m_reset = pangolin.VarBool('menu.Reset', False, False) 176 | 177 | if self.config is None: 178 | width, height = 400, 250 179 | viewpoint_x = 0 180 | viewpoint_y = -500 # -10 181 | viewpoint_z = -100 # -0.1 182 | viewpoint_f = 2000 183 | camera_width = 1. 184 | else: 185 | width = self.config.view_image_width 186 | height = self.config.view_image_height 187 | viewpoint_x = self.config.view_viewpoint_x 188 | viewpoint_y = self.config.view_viewpoint_y 189 | viewpoint_z = self.config.view_viewpoint_z 190 | viewpoint_f = self.config.view_viewpoint_f 191 | camera_width = self.config.view_camera_width 192 | 193 | proj = pangolin.ProjectionMatrix( 194 | 1024, 768, viewpoint_f, viewpoint_f, 512, 389, 0.1, 5000) 195 | look_view = pangolin.ModelViewLookAt( 196 | viewpoint_x, viewpoint_y, viewpoint_z, 0, 0, 0, 0, -1, 0) 197 | 198 | # Camera Render Object (for view / scene browsing) 199 | scam = pangolin.OpenGlRenderState(proj, look_view) 200 | 201 | # Add named OpenGL viewport to window and provide 3D Handler 202 | dcam = pangolin.CreateDisplay() 203 | dcam.SetBounds(0.0, 1.0, 175 / 1024., 1.0, -1024 / 768.) 204 | dcam.SetHandler(pangolin.Handler3D(scam)) 205 | 206 | 207 | # image 208 | # width, height = 400, 130 209 | dimg = pangolin.Display('image') 210 | dimg.SetBounds(0, height / 768., 0.0, width / 1024., 1024 / 768.) 211 | dimg.SetLock(pangolin.Lock.LockLeft, pangolin.Lock.LockTop) 212 | 213 | texture = pangolin.GlTexture(width, height, gl.GL_RGB, False, 0, gl.GL_RGB, gl.GL_UNSIGNED_BYTE) 214 | image = np.ones((height, width, 3), 'uint8') 215 | 216 | 217 | 218 | pose = pangolin.OpenGlMatrix() # identity matrix 219 | following = True 220 | 221 | active = [] 222 | replays = [] 223 | graph = [] 224 | loops = [] 225 | mappoints = DynamicArray(shape=(3,)) 226 | colors = DynamicArray(shape=(3,)) 227 | cameras = DynamicArray(shape=(4, 4)) 228 | 229 | 230 | while not pangolin.ShouldQuit(): 231 | 232 | if not self.q_pose.empty(): 233 | pose.m = self.q_pose.get() 234 | 235 | follow = m_follow_camera.Get() 236 | if follow and following: 237 | scam.Follow(pose, True) 238 | elif follow and not following: 239 | scam.SetModelViewMatrix(look_view) 240 | scam.Follow(pose, True) 241 | following = True 242 | elif not follow and following: 243 | following = False 244 | 245 | 246 | gl.glClear(gl.GL_COLOR_BUFFER_BIT | gl.GL_DEPTH_BUFFER_BIT) 247 | gl.glClearColor(1.0, 1.0, 1.0, 1.0) 248 | dcam.Activate(scam) 249 | 250 | 251 | # show graph 252 | if not self.q_graph.empty(): 253 | graph = self.q_graph.get() 254 | loops = np.array([_[0] for _ in graph if _[1] == 2]) 255 | graph = np.array([_[0] for _ in graph if _[1] < 2]) 256 | if m_show_graph.Get(): 257 | if len(graph) > 0: 258 | gl.glLineWidth(1) 259 | gl.glColor3f(0.0, 1.0, 0.0) 260 | pangolin.DrawLines(graph, 3) 261 | if len(loops) > 0: 262 | gl.glLineWidth(2) 263 | gl.glColor3f(0.0, 0.0, 0.0) 264 | pangolin.DrawLines(loops, 4) 265 | 266 | gl.glPointSize(4) 267 | gl.glColor3f(1.0, 0.0, 0.0) 268 | gl.glBegin(gl.GL_POINTS) 269 | gl.glVertex3d(pose[0, 3], pose[1, 3], pose[2, 3]) 270 | gl.glEnd() 271 | 272 | 273 | # Show mappoints 274 | if not self.q_points.empty(): 275 | pts, code = self.q_points.get() 276 | cls, code = self.q_colors.get() 277 | if code == 1: # append new points 278 | mappoints.extend(pts) 279 | colors.extend(cls) 280 | elif code == 0: # refresh all points 281 | mappoints.clear() 282 | mappoints.extend(pts) 283 | colors.clear() 284 | colors.extend(cls) 285 | 286 | if m_show_points.Get(): 287 | gl.glPointSize(2) 288 | # easily draw millions of points 289 | pangolin.DrawPoints(mappoints.array(), colors.array()) 290 | 291 | 292 | if not self.q_active.empty(): 293 | active = self.q_active.get() 294 | 295 | gl.glPointSize(3) 296 | gl.glBegin(gl.GL_POINTS) 297 | gl.glColor3f(1.0, 0.0, 0.0) 298 | for point in active: 299 | gl.glVertex3f(*point) 300 | gl.glEnd() 301 | 302 | 303 | if len(replays) > 0: 304 | n = 300 305 | gl.glPointSize(4) 306 | gl.glColor3f(1.0, 0.0, 0.0) 307 | gl.glBegin(gl.GL_POINTS) 308 | for point in replays[:n]: 309 | gl.glVertex3f(*point) 310 | gl.glEnd() 311 | replays = replays[n:] 312 | 313 | 314 | # show cameras 315 | if not self.q_camera.empty(): 316 | cams = self.q_camera.get() 317 | if len(cams) > 20: 318 | cameras.clear() 319 | cameras.extend(cams) 320 | 321 | if m_show_keyframes.Get(): 322 | gl.glLineWidth(1) 323 | gl.glColor3f(0.0, 0.0, 1.0) 324 | pangolin.DrawCameras(cameras.array(), camera_width) 325 | 326 | 327 | # show image 328 | if not self.q_image.empty(): 329 | image = self.q_image.get() 330 | if image.ndim == 3: 331 | image = image[::-1, :, ::-1] 332 | else: 333 | image = np.repeat(image[::-1, :, np.newaxis], 3, axis=2) 334 | image = cv2.resize(image, (width, height)) 335 | if m_show_image.Get(): 336 | texture.Upload(image, gl.GL_RGB, gl.GL_UNSIGNED_BYTE) 337 | dimg.Activate() 338 | gl.glColor3f(1.0, 1.0, 1.0) 339 | texture.RenderToViewport() 340 | 341 | 342 | if pangolin.Pushed(m_replay): 343 | replays = mappoints.array() 344 | 345 | if pangolin.Pushed(m_reset): 346 | m_show_graph.SetVal(True) 347 | m_show_keyframes.SetVal(True) 348 | m_show_points.SetVal(True) 349 | m_show_image.SetVal(True) 350 | m_follow_camera.SetVal(True) 351 | follow_camera = True 352 | 353 | if pangolin.Pushed(m_refresh): 354 | self.q_refresh.put(True) 355 | 356 | 357 | 358 | pangolin.FinishFrame() --------------------------------------------------------------------------------