├── Dockerfile ├── LICENSE ├── README.md ├── data_process ├── generate_loop_GT_KITTI.py ├── generate_loop_GT_KITTI360.py ├── remove_ground_plane_kitti.py └── remove_ground_plane_kitti360.py ├── datasets ├── KITTI360Dataset.py └── KITTIDataset.py ├── environment_lcdnet.yml ├── evaluate_kitti.py ├── evaluation ├── inference_loop_closure.py ├── inference_yaw_general.py └── plot_PR_curve.py ├── imgs └── video-preview.png ├── loss.py ├── models ├── backbone3D │ ├── MyVoxelSetAbstraction.py │ ├── NetVlad.py │ ├── PVRCNN.py │ ├── heads.py │ ├── kitti_dataset.yaml │ ├── models_3d.py │ └── pv_rcnn.yaml └── get_models.py ├── requirements.txt ├── training_KITTI_DDP.py ├── triple_selector.py ├── utils ├── data.py ├── geometry.py ├── rotation_conversion.py └── tools.py └── wandb_config.yaml /Dockerfile: -------------------------------------------------------------------------------- 1 | FROM nvidia/cuda:11.3.1-cudnn8-devel-ubuntu20.04 2 | 3 | ENV DEBIAN_FRONTEND noninteractive 4 | ENV DEBCONF_NONINTERACTIVE_SEEN true 5 | 6 | RUN \ 7 | # Update nvidia GPG key 8 | rm /etc/apt/sources.list.d/cuda.list && \ 9 | apt-key del 7fa2af80 && \ 10 | apt-get update && apt-get install -y --no-install-recommends wget && \ 11 | wget https://developer.download.nvidia.com/compute/cuda/repos/ubuntu2004/x86_64/cuda-keyring_1.0-1_all.deb && \ 12 | dpkg -i cuda-keyring_1.0-1_all.deb 13 | 14 | # preesed tzdata, update package index, upgrade packages and install needed software 15 | RUN truncate -s0 /tmp/preseed.cfg; \ 16 | echo "tzdata tzdata/Areas select Europe" >> /tmp/preseed.cfg; \ 17 | echo "tzdata tzdata/Zones/Europe select Berlin" >> /tmp/preseed.cfg; \ 18 | debconf-set-selections /tmp/preseed.cfg && \ 19 | rm -f /etc/timezone /etc/localtime && \ 20 | apt-get update && \ 21 | apt-get install -y tzdata sudo nano git 22 | 23 | 24 | # Install mambaforge 25 | ENV CONDA_DIR=/opt/conda 26 | ENV LANG=C.UTF-8 LC_ALL=C.UTF-8 27 | ENV PATH=${CONDA_DIR}/bin:${PATH} 28 | 29 | RUN apt-get update > /dev/null && \ 30 | apt-get install --no-install-recommends --yes \ 31 | bzip2 ca-certificates \ 32 | libgl1 libglib2.0-0 \ 33 | > /dev/null 34 | RUN wget --no-hsts --quiet https://github.com/conda-forge/miniforge/releases/download/22.11.1-2/Mambaforge-22.11.1-2-Linux-x86_64.sh -O /tmp/mambaforge.sh 35 | RUN /bin/bash /tmp/mambaforge.sh -b -p ${CONDA_DIR} && \ 36 | rm /tmp/mambaforge.sh && \ 37 | conda clean --tarballs --index-cache --packages --yes && \ 38 | find ${CONDA_DIR} -follow -type f -name '*.a' -delete && \ 39 | find ${CONDA_DIR} -follow -type f -name '*.pyc' -delete && \ 40 | conda clean --force-pkgs-dirs --all 41 | RUN mamba init bash 42 | 43 | # LCDNet environment 44 | COPY environment_lcdnet.yml /environment_lcdnet.yml 45 | RUN mamba env create -f /environment_lcdnet.yml -n lcdnet 46 | 47 | SHELL ["mamba", "run", "-n", "lcdnet", "/bin/bash", "-c"] 48 | 49 | RUN git clone --depth 1 --branch v0.5.2 https://github.com/open-mmlab/OpenPCDet.git 50 | WORKDIR /OpenPCDet 51 | 52 | ENV CUDA_HOME=/usr/local/cuda-11.3/ 53 | ENV TORCH_CUDA_ARCH_LIST="3.5 3.7 5.0 5.2 5.3 6.0 6.1 6.2 7.0 7.5 8.0 8.6+PTX" 54 | RUN python setup.py develop 55 | 56 | WORKDIR / 57 | RUN git clone --depth 1 https://github.com/robot-learning-freiburg/LCDNet.git 58 | RUN mkdir /pretreined_models 59 | COPY LCDNet-kitti360.tar /pretreined_models/LCDNet-kitti360.tar 60 | 61 | # cleanup of files from setup 62 | RUN apt-get clean 63 | RUN rm -rf /var/lib/apt/lists/* /tmp/* /var/tmp/* 64 | RUN mamba clean -afy 65 | 66 | -------------------------------------------------------------------------------- /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 | # LCDNet: Deep Loop Closure Detection and Point Cloud Registration for LiDAR SLAM (IEEE T-RO 2022) 2 | 3 | Official PyTorch implementation of LCDNet. 4 | 5 | [![](imgs/video-preview.png)](https://www.youtube.com/watch?v=nAvTdEFRh_s) 6 | 7 | ## Installation 8 | 9 | You can install LCDNet locally on your machine, or use the provided Dockerfile to run it in a container. The `environment_lcdnet.yml` file is meant to be used with docker, as it contains version of packages that are specific to a CUDA version. We don't recommend using it for local installation. 10 | 11 | ### Local Installation 12 | 13 | 1. Install [PyTorch](https://pytorch.org/) (make sure to select the correct cuda version) 14 | 2. Install the requirements 15 | ```pip install -r requirements.txt``` 16 | 3. Install [spconv](https://github.com/traveller59/spconv) <= 2.1.25 (make sure to select the correct cuda version, for example ```pip install spconv-cu113==2.1.25``` for cuda 11.3) 17 | 4. Install [OpenPCDet](https://github.com/open-mmlab/OpenPCDet) 18 | 5. Install [faiss-cpu](https://github.com/facebookresearch/faiss/blob/main/INSTALL.md) - NOTE: avoid installing faiss via pip, use the conda version, or build it from source alternatively. 19 | ```conda install -c pytorch faiss-cpu==1.7.3``` 20 | 21 | Tested in the following environments: 22 | * Ubuntu 18.04/20.04/22.04 23 | * Python 3.7 24 | * cuda 10.2/11.1/11.3 25 | * pytorch 1.8/1.9/1.10 26 | * Open3D 0.12.0/0.14.1 27 | 28 | #### :warning: Note 29 | We noticed that the RANSAC implementation in Open3D version >=0.15 achieves bad results. We tested our code with Open3D versions between 0.12.0 and 0.14.1, please use one of these versions, as results might be very different otherwise. 30 | 31 | We also noticed that spconv version 2.2 or higher is not compatible with the pretrained weights provided with this repository. Spconv version 2.1.25 or lower is required to properly load the pretrained model. 32 | 33 | ### Docker 34 | 35 | 1. Install Docker and NVIDIA-Docker (see [here](https://docs.nvidia.com/datacenter/cloud-native/container-toolkit/install-guide.html#docker) for instructions) 36 | 2. Download the pretrained model (see [Pretrained model](#pretrained-model) section) in the same folder as the Dockerfile 37 | 3. Build the docker image ```docker build --tag lcdnet -f Dockerfile .``` 38 | 4. Run the docker container ```docker run --gpus all -it --rm -v KITTI_ROOT:/data/KITTI lcdnet``` 39 | 5. From inside the container, activate the anaconda environment ```conda activate lcdnet``` and change directory to the LCDNet folder ```cd LCDNet``` 40 | 7. Run the training or evaluation scripts (see [Training](#training) and [Evaluation](#evaluation) sections). The weights of the pretrained model are copied inside the container under ```/pretreined_models/LCDNet-kitti360.tar```. 41 | 42 | ## Preprocessing 43 | 44 | ### KITTI 45 | 46 | Download the [SemanticKITTI](http://semantic-kitti.org/dataset.html#download) dataset and generate the loop ground truths: 47 | 48 | ```python -m data_process.generate_loop_GT_KITTI --root_folder KITTI_ROOT``` 49 | 50 | where KITTI_ROOT is the path where you downloaded and extracted the SemanticKITTI dataset. 51 | 52 | NOTE: although the semantic labels are not required to run LCDNet, we use the improved ground truth poses provided with the SemanticKITTI dataset. 53 | 54 | ### KITTI-360 55 | 56 | Download the [KITTI-360](http://www.cvlibs.net/datasets/kitti-360/download.php) dataset (raw velodyne scans, calibrations and vehicle poses) and generate the loop ground truths: 57 | 58 | ```python -m data_process.generate_loop_GT_KITTI360 --root_folder KITTI360_ROOT``` 59 | 60 | where KITTI360_ROOT is the path where you downloaded and extracted the KITTI-360 dataset. 61 | 62 | ### Optional: Ground Plane Removal 63 | 64 | To achieve better results, it is suggested to preprocess the datasets by removing the ground plane: 65 | 66 | ```python -m data_process.remove_ground_plane_kitti --root_folder KITT_ROOT``` 67 | 68 | ```python -m data_process.remove_ground_plane_kitti360 --root_folder KITT_ROOT360``` 69 | 70 | If you skip this step, please remove the option ```--without_ground``` in all the following steps. 71 | 72 | ## Training 73 | 74 | The training script will use all the available GPUs, if you want to use only a subset of the GPUs, use the environment variable ```CUDA_VISIBLE_DEVICES```. If you don't know how to do that, check [here](https://docs.nvidia.com/cuda/cuda-c-programming-guide/index.html#env-vars). 75 | 76 | To train on the KITTI dataset: 77 | 78 | ```python -m training_KITTI_DDP --root_folder KITTI_ROOT --dataset kitti --batch_size B --without_ground``` 79 | 80 | To train on the KITTI-360 dataset: 81 | 82 | ```python -m training_KITTI_DDP --root_folder KITTI360_ROOT --dataset kitti360 --batch_size B --without_ground``` 83 | 84 | To track the training progress using [Weights & Biases](https://wandb.ai/), add the argument ```--wandb```. 85 | The per-GPU batch size B must be at least 2, and a GPU with at least 8GB of RAM is required (12GB or more is preferred). In our experiments we used a batch size of 6 on 4 x 24GB GPUs, for a total batch size of 24. 86 | 87 | The network's weights will be saved in the folder ```./checkpoints``` (you can change this folder with the argument ```--checkpoints_dest```), inside a subfolder named with the starting date and time of the training (format ```%d-%m-%Y_%H-%M-%S```), for example: ```20-02-2022_16-38-24``` 88 | 89 | ## Evaluation 90 | 91 | ### Loop Closure 92 | 93 | To evaluate the loop closure performance of the trained model on the KITTI dataset: 94 | 95 | ```python -m evaluation.inference_loop_closure --root_folder KITTI_ROOT --dataset kitti --validation_sequence 08 --weights_path WEIGHTS --without_ground``` 96 | 97 | where WEIGHTS is the path of the pretrained model, for example ```./checkpoints/20-02-2022_16-38-24/checkpoint_last_iter.tar``` 98 | 99 | Similarly, on the KITTI-360 dataset: 100 | 101 | ```python -m evaluation.inference_loop_closure --root_folder KITTI360_ROOT --dataset kitti360 --validation_sequence 2013_05_28_drive_0002_sync --weights_path WEIGHTS --without_ground``` 102 | 103 | ### Point Cloud Registration 104 | 105 | To evaluate the loop closure performance of the trained model on the KITTI and KITTI-360 dataset: 106 | 107 | ```python -m evaluation.inference_yaw_general --root_folder KITTI_ROOT --dataset kitti --validation_sequence 08 --weights_path WEIGHTS --ransac --without_ground``` 108 | 109 | ```python -m evaluation.inference_yaw_general --root_folder KITTI360_ROOT --dataset kitti360 --validation_sequence 2013_05_28_drive_0002_sync --weights_path WEIGHTS --ransac --without_ground``` 110 | 111 | To evaluate LCDNet (fast), remove the ```--ransac``` argument. 112 | 113 | ## Pretrained Model 114 | 115 | A model pretrained on the KITTI-360 dataset can be found [here](https://drive.google.com/file/d/176dQn6QhFoolu3bcGvyGuBxaCQY42kNn/view?usp=sharing) 116 | 117 | ## Paper 118 | 119 | "LCDNet: Deep Loop Closure Detection and Point Cloud Registration for LiDAR SLAM" 120 | * [IEEEXplore](https://ieeexplore.ieee.org/document/9723505) 121 | * [Arxiv](https://arxiv.org/abs/2103.05056) 122 | * [Video](https://www.youtube.com/watch?v=nAvTdEFRh_s) 123 | 124 | If you use LCDnet, please cite: 125 | ``` 126 | @ARTICLE{cattaneo2022tro, 127 | author={Cattaneo, Daniele and Vaghi, Matteo and Valada, Abhinav}, 128 | journal={IEEE Transactions on Robotics}, 129 | title={LCDNet: Deep Loop Closure Detection and Point Cloud Registration for LiDAR SLAM}, 130 | year={2022}, 131 | volume={38}, 132 | number={4}, 133 | pages={2074-2093}, 134 | doi={10.1109/TRO.2022.3150683} 135 | } 136 | ``` 137 | 138 | ## Contacts 139 | * [Daniele Cattaneo](https://rl.uni-freiburg.de/people/cattaneo) 140 | * [Abhinav Valada](https://rl.uni-freiburg.de/people/valada) 141 | 142 | ## License 143 | For academic usage, the code is released under the [GPLv3](https://www.gnu.org/licenses/gpl-3.0.en.html) license. For any commercial purpose, please contact the authors. 144 | -------------------------------------------------------------------------------- /data_process/generate_loop_GT_KITTI.py: -------------------------------------------------------------------------------- 1 | import argparse 2 | import torch 3 | from torch.utils.data import Dataset 4 | import pykitti 5 | import os 6 | from sklearn.neighbors import KDTree 7 | import pickle 8 | import numpy as np 9 | 10 | 11 | class KITTILoader3DPosesOnlyLoopPositives(Dataset): 12 | 13 | def __init__(self, dir, sequence, poses, positive_range=5., negative_range=25., hard_range=None): 14 | super(KITTILoader3DPosesOnlyLoopPositives, self).__init__() 15 | 16 | self.positive_range = positive_range 17 | self.negative_range = negative_range 18 | self.hard_range = hard_range 19 | self.dir = dir 20 | self.sequence = sequence 21 | self.data = pykitti.odometry(dir, sequence) 22 | poses2 = [] 23 | T_cam_velo = np.array(self.data.calib.T_cam0_velo) 24 | with open(poses, 'r') as f: 25 | for x in f: 26 | x = x.strip().split() 27 | x = [float(v) for v in x] 28 | pose = np.zeros((4, 4)) 29 | pose[0, 0:4] = np.array(x[0:4]) 30 | pose[1, 0:4] = np.array(x[4:8]) 31 | pose[2, 0:4] = np.array(x[8:12]) 32 | pose[3, 3] = 1.0 33 | pose = np.linalg.inv(T_cam_velo) @ (pose @ T_cam_velo) 34 | poses2.append(pose) 35 | self.poses = np.stack(poses2) 36 | self.kdtree = KDTree(self.poses[:, :3, 3]) 37 | 38 | def __len__(self): 39 | return len(self.data.timestamps) 40 | 41 | def __getitem__(self, idx): 42 | 43 | x = self.poses[idx, 0, 3] 44 | y = self.poses[idx, 1, 3] 45 | z = self.poses[idx, 2, 3] 46 | 47 | anchor_pose = torch.tensor([x, y, z]) 48 | 49 | indices = self.kdtree.query_radius(anchor_pose.unsqueeze(0).numpy(), self.positive_range) 50 | min_range = max(0, idx-50) 51 | max_range = min(idx+50, len(self.data.timestamps)) 52 | positive_idxs = list(set(indices[0]) - set(range(min_range, max_range))) 53 | positive_idxs.sort() 54 | num_loop = len(positive_idxs) 55 | 56 | indices = self.kdtree.query_radius(anchor_pose.unsqueeze(0).numpy(), self.negative_range) 57 | indices = set(indices[0]) 58 | negative_idxs = set(range(len(self.data.timestamps))) - indices 59 | negative_idxs = list(negative_idxs) 60 | negative_idxs.sort() 61 | 62 | hard_idxs = None 63 | if self.hard_range is not None: 64 | inner_indices = self.kdtree.query_radius(anchor_pose.unsqueeze(0).numpy(), self.hard_range[0]) 65 | outer_indices = self.kdtree.query_radius(anchor_pose.unsqueeze(0).numpy(), self.hard_range[1]) 66 | hard_idxs = set(outer_indices[0]) - set(inner_indices[0]) 67 | pass 68 | 69 | return num_loop, positive_idxs, negative_idxs, hard_idxs 70 | 71 | 72 | if __name__ == '__main__': 73 | 74 | parser = argparse.ArgumentParser() 75 | parser.add_argument('--root_folder', default='./KITTI', help='dataset directory') 76 | args = parser.parse_args() 77 | 78 | base_dir = args.root_folder 79 | for sequence in ["00", "03", "04", "05", "06", "07", "08", "09"]: 80 | poses_file = base_dir + "/sequences/" + sequence + "/poses.txt" 81 | 82 | dataset = KITTILoader3DPosesOnlyLoopPositives(base_dir, sequence, poses_file, 4, 10, [6, 10]) 83 | lc_gt = [] 84 | lc_gt_file = os.path.join(base_dir, 'sequences', sequence, 'loop_GT_4m.pickle') 85 | 86 | for i in range(len(dataset)): 87 | 88 | sample, pos, neg, hard = dataset[i] 89 | if sample > 0.: 90 | sample_dict = {} 91 | sample_dict['idx'] = i 92 | sample_dict['positive_idxs'] = pos 93 | sample_dict['negative_idxs'] = neg 94 | sample_dict['hard_idxs'] = hard 95 | lc_gt.append(sample_dict) 96 | with open(lc_gt_file, 'wb') as f: 97 | pickle.dump(lc_gt, f) 98 | print(f'Sequence {sequence} done') 99 | -------------------------------------------------------------------------------- /data_process/generate_loop_GT_KITTI360.py: -------------------------------------------------------------------------------- 1 | import argparse 2 | import torch 3 | from torch.utils.data import Dataset 4 | import os 5 | from sklearn.neighbors import KDTree 6 | import pickle 7 | import numpy as np 8 | 9 | 10 | class KITTI360(Dataset): 11 | """KITTI ODOMETRY DATASET""" 12 | 13 | def __init__(self, dir, sequence, positive_range=5., negative_range=25., hard_range=None): 14 | """ 15 | 16 | :param dataset: directory where dataset is located 17 | :param sequence: KITTI sequence 18 | :param poses: csv with data poses 19 | """ 20 | 21 | self.positive_range = positive_range 22 | self.negative_range = negative_range 23 | self.hard_range = hard_range 24 | self.dir = dir 25 | self.sequence = sequence 26 | calib_file = os.path.join(dir, 'calibration', 'calib_cam_to_velo.txt') 27 | with open(calib_file, 'r') as f: 28 | for line in f.readlines(): 29 | data = np.array([float(x) for x in line.split()]) 30 | 31 | cam0_to_velo = np.reshape(data, (3, 4)) 32 | cam0_to_velo = np.vstack([cam0_to_velo, [0, 0, 0, 1]]) 33 | cam0_to_velo = torch.tensor(cam0_to_velo) 34 | 35 | self.frames_with_gt = [] 36 | poses2 = [] 37 | poses = os.path.join(dir, 'data_poses', sequence, 'cam0_to_world.txt') 38 | with open(poses, 'r') as f: 39 | for x in f: 40 | x = x.strip().split() 41 | x = [float(v) for v in x] 42 | self.frames_with_gt.append(int(x[0])) 43 | pose = torch.zeros((4, 4), dtype=torch.float64) 44 | pose[0, 0:4] = torch.tensor(x[1:5]) 45 | pose[1, 0:4] = torch.tensor(x[5:9]) 46 | pose[2, 0:4] = torch.tensor(x[9:13]) 47 | pose[3, 3] = 1.0 48 | pose = pose @ cam0_to_velo.inverse() 49 | poses2.append(pose.float().numpy()) 50 | self.frames_with_gt = np.array(self.frames_with_gt, dtype=np.int16) 51 | poses2 = np.stack(poses2) 52 | self.poses = poses2 53 | self.kdtree = KDTree(self.poses[:, :3, 3]) 54 | 55 | def __len__(self): 56 | return self.poses.shape[0] 57 | 58 | def __getitem__(self, idx): 59 | 60 | x = self.poses[idx, 0, 3] 61 | y = self.poses[idx, 1, 3] 62 | z = self.poses[idx, 2, 3] 63 | 64 | anchor_pose = torch.tensor([x, y, z]) 65 | 66 | indices = self.kdtree.query_radius(anchor_pose.unsqueeze(0).numpy(), self.positive_range) 67 | min_range = max(0, idx-50) 68 | max_range = min(idx+50, self.poses.shape[0]) 69 | positive_idxs = list(set(indices[0]) - set(range(min_range, max_range))) 70 | positive_idxs.sort() 71 | num_loop = len(positive_idxs) 72 | if num_loop > 0: 73 | positive_idxs = list(self.frames_with_gt[np.array(positive_idxs)]) 74 | 75 | indices = self.kdtree.query_radius(anchor_pose.unsqueeze(0).numpy(), self.negative_range) 76 | indices = set(indices[0]) 77 | negative_idxs = set(range(self.poses.shape[0])) - indices 78 | negative_idxs = list(negative_idxs) 79 | negative_idxs.sort() 80 | 81 | hard_idxs = None 82 | if self.hard_range is not None: 83 | inner_indices = self.kdtree.query_radius(anchor_pose.unsqueeze(0).numpy(), self.hard_range[0]) 84 | outer_indices = self.kdtree.query_radius(anchor_pose.unsqueeze(0).numpy(), self.hard_range[1]) 85 | hard_idxs = set(outer_indices[0]) - set(inner_indices[0]) 86 | hard_idxs = list(self.frames_with_gt[np.array(list(hard_idxs))]) 87 | pass 88 | 89 | return num_loop, positive_idxs,\ 90 | list(self.frames_with_gt[np.array(negative_idxs)]),\ 91 | hard_idxs 92 | 93 | 94 | if __name__ == '__main__': 95 | parser = argparse.ArgumentParser() 96 | parser.add_argument('--root_folder', default='./KITTI-360', help='dataset directory') 97 | args = parser.parse_args() 98 | 99 | base_dir = args.root_folder 100 | for sequence in ["2013_05_28_drive_0000_sync", "2013_05_28_drive_0002_sync", "2013_05_28_drive_0003_sync", 101 | "2013_05_28_drive_0004_sync", "2013_05_28_drive_0005_sync", "2013_05_28_drive_0006_sync", 102 | "2013_05_28_drive_0007_sync", "2013_05_28_drive_0009_sync", "2013_05_28_drive_0010_sync"]: 103 | dataset = KITTI360(base_dir, sequence, 4, 10, [6, 10]) 104 | lc_gt = [] 105 | lc_gt_file = os.path.join(base_dir, 'data_poses', sequence, 'loop_GT_4m.pickle') 106 | 107 | for i in range(len(dataset)): 108 | sample, pos, neg, hard = dataset[i] 109 | if sample > 0.: 110 | idx = dataset.frames_with_gt[i] 111 | sample_dict = {} 112 | sample_dict['idx'] = idx 113 | sample_dict['positive_idxs'] = pos 114 | sample_dict['negative_idxs'] = neg 115 | sample_dict['hard_idxs'] = hard 116 | lc_gt.append(sample_dict) 117 | with open(lc_gt_file, 'wb') as f: 118 | pickle.dump(lc_gt, f) 119 | print(f'Sequence {sequence} done') 120 | -------------------------------------------------------------------------------- /data_process/remove_ground_plane_kitti.py: -------------------------------------------------------------------------------- 1 | import argparse 2 | import h5py 3 | import open3d as o3d 4 | import os 5 | import torch 6 | from tqdm import tqdm 7 | import numpy as np 8 | 9 | parser = argparse.ArgumentParser() 10 | parser.add_argument('--root_folder', default='./KITTI', help='dataset directory') 11 | args = parser.parse_args() 12 | base_dir = args.root_folder 13 | out_dir = 'velodyne_no_ground' 14 | 15 | for sequence in [9]: 16 | if not os.path.exists(os.path.join(base_dir, 'sequences', f'{sequence:02d}', out_dir)): 17 | os.mkdir(os.path.join(base_dir, 'sequences', f'{sequence:02d}', out_dir)) 18 | f = open(f"./failed_frames_{sequence:02d}.txt", "w") 19 | 20 | file_list = os.listdir(os.path.join(base_dir, 'sequences', f'{sequence:02d}', 'velodyne')) 21 | for idx in tqdm(range(len(file_list))): 22 | velo_path = os.path.join(base_dir, 'sequences', f'{sequence:02d}', 'velodyne', f'{idx:06d}.bin') 23 | save_file = os.path.join(base_dir, 'sequences', f'{sequence:02d}', out_dir, f'{idx:06d}.npy') 24 | if os.path.exists(save_file): 25 | continue 26 | scan = np.fromfile(velo_path, dtype=np.float32) 27 | scan = scan.reshape((-1, 4)) 28 | pcd = o3d.geometry.PointCloud() 29 | pcd.points = o3d.utility.Vector3dVector(scan[:,:3]) 30 | plane_model, inliers = pcd.segment_plane(distance_threshold=0.2, 31 | ransac_n=3, 32 | num_iterations=1000) 33 | i = 0 34 | while np.argmax(plane_model[:-1]) != 2: 35 | i += 1 36 | pcd = pcd.select_down_sample(inliers, invert=True) 37 | plane_model, inliers = pcd.segment_plane(distance_threshold=0.2, 38 | ransac_n=3, 39 | num_iterations=10000) 40 | if i == 5: 41 | f.write(f'{idx:06d}.bin\n') 42 | f.flush() 43 | break 44 | outliers_index = set(range(scan.shape[0])) - set(inliers) 45 | outliers_index = list(outliers_index) 46 | no_ground_scan = scan[outliers_index] 47 | 48 | with h5py.File(save_file, 'w') as hf: 49 | hf.create_dataset('PC', data=no_ground_scan, compression='lzf', shuffle=True) 50 | f.close() 51 | -------------------------------------------------------------------------------- /data_process/remove_ground_plane_kitti360.py: -------------------------------------------------------------------------------- 1 | import argparse 2 | import h5py 3 | import open3d as o3d 4 | import os 5 | import torch 6 | from tqdm import tqdm 7 | import numpy as np 8 | 9 | parser = argparse.ArgumentParser() 10 | parser.add_argument('--root_folder', default='./KITTI', help='dataset directory') 11 | args = parser.parse_args() 12 | base_dir = args.root_folder 13 | out_dir = 'velodyne_no_ground' 14 | 15 | for sequence in ["2013_05_28_drive_0000_sync", "2013_05_28_drive_0002_sync", 16 | "2013_05_28_drive_0004_sync", "2013_05_28_drive_0005_sync", 17 | "2013_05_28_drive_0006_sync"]: 18 | if not os.path.exists(os.path.join(base_dir, 'data_3d_raw', sequence, out_dir)): 19 | os.mkdir(os.path.join(base_dir, 'data_3d_raw', sequence, out_dir)) 20 | f = open(f"./failed_frames_{sequence}.txt", "w") 21 | 22 | file_list = os.listdir(os.path.join(base_dir, 'data_3d_raw', sequence, 'velodyne_points', 'data')) 23 | for idx in tqdm(range(len(file_list))): 24 | velo_path = os.path.join(base_dir, 'data_3d_raw', sequence, 'velodyne_points', 'data', f'{idx:010d}.bin') 25 | save_file = os.path.join(base_dir, 'data_3d_raw', sequence, out_dir, f'{idx:010d}.npy') 26 | if os.path.exists(save_file): 27 | continue 28 | scan = np.fromfile(velo_path, dtype=np.float32) 29 | scan = scan.reshape((-1, 4)) 30 | pcd = o3d.geometry.PointCloud() 31 | pcd.points = o3d.utility.Vector3dVector(scan[:,:3]) 32 | plane_model, inliers = pcd.segment_plane(distance_threshold=0.2, 33 | ransac_n=3, 34 | num_iterations=1000) 35 | i = 0 36 | while np.argmax(plane_model[:-1]) != 2: 37 | i += 1 38 | pcd = pcd.select_down_sample(inliers, invert=True) 39 | plane_model, inliers = pcd.segment_plane(distance_threshold=0.2, 40 | ransac_n=3, 41 | num_iterations=10000) 42 | if i == 5: 43 | f.write(f'{idx:06d}.bin\n') 44 | f.flush() 45 | break 46 | outliers_index = set(range(scan.shape[0])) - set(inliers) 47 | outliers_index = list(outliers_index) 48 | no_ground_scan = scan[outliers_index] 49 | 50 | with h5py.File(save_file, 'w') as hf: 51 | hf.create_dataset('PC', data=no_ground_scan, compression='lzf', shuffle=True) 52 | f.close() 53 | -------------------------------------------------------------------------------- /datasets/KITTI360Dataset.py: -------------------------------------------------------------------------------- 1 | import h5py 2 | import torch 3 | from torch.utils.data import Dataset 4 | 5 | import os, os.path 6 | import numpy as np 7 | import random 8 | import pickle 9 | 10 | import utils.rotation_conversion as RT 11 | 12 | 13 | def get_velo(idx, dir, sequence, jitter=False, remove_random_angle=-1, without_ground=False): 14 | if without_ground: 15 | velo_path = os.path.join(dir, 'data_3d_raw', sequence, 16 | 'velodyne_no_ground', f'{idx:010d}.npy') 17 | with h5py.File(velo_path, 'r') as hf: 18 | scan = hf['PC'][:] 19 | else: 20 | velo_path = os.path.join(dir, 'data_3d_raw', sequence, 21 | 'velodyne_points', 'data', f'{idx:010d}.bin') 22 | scan = np.fromfile(velo_path, dtype=np.float32) 23 | scan = scan.reshape((-1, 4)) 24 | 25 | if jitter: 26 | noise = 0.01 * np.random.randn(scan.shape[0], scan.shape[1]).astype(np.float32) 27 | noise = np.clip(noise, -0.05, 0.05) 28 | scan = scan + noise 29 | 30 | if remove_random_angle > 0: 31 | azi = np.arctan2(scan[..., 1], scan[..., 0]) 32 | cols = 2084 * (np.pi - azi) / (2 * np.pi) 33 | cols = np.minimum(cols, 2084 - 1) 34 | cols = np.int32(cols) 35 | start_idx = np.random.randint(0, 2084) 36 | end_idx = start_idx + (remove_random_angle / (360.0/2084)) 37 | end_idx = int(end_idx % 2084) 38 | remove_idxs = cols > start_idx 39 | remove_idxs = remove_idxs & (cols < end_idx) 40 | scan = scan[np.logical_not(remove_idxs)] 41 | 42 | return scan 43 | 44 | 45 | class KITTI3603DPoses(Dataset): 46 | """KITTI ODOMETRY DATASET""" 47 | 48 | def __init__(self, dir, sequence, train=False, loop_file='loop_GT', 49 | jitter=False, remove_random_angle=-1, without_ground=False): 50 | """ 51 | 52 | :param dataset: directory where dataset is located 53 | :param sequence: KITTI sequence 54 | :param poses: csv with data poses 55 | """ 56 | super(KITTI3603DPoses, self).__init__() 57 | 58 | self.dir = dir 59 | self.sequence = sequence 60 | self.jitter = jitter 61 | self.remove_random_angle = remove_random_angle 62 | self.without_ground = without_ground 63 | calib_file = os.path.join(dir, 'calibration', 'calib_cam_to_velo.txt') 64 | with open(calib_file, 'r') as f: 65 | for line in f.readlines(): 66 | data = np.array([float(x) for x in line.split()]) 67 | 68 | cam0_to_velo = np.reshape(data, (3, 4)) 69 | cam0_to_velo = np.vstack([cam0_to_velo, [0, 0, 0, 1]]) 70 | cam0_to_velo = torch.tensor(cam0_to_velo) 71 | 72 | self.frames_with_gt = [] 73 | poses2 = [] 74 | poses = os.path.join(dir, 'data_poses', sequence, 'cam0_to_world.txt') 75 | with open(poses, 'r') as f: 76 | for x in f: 77 | x = x.strip().split() 78 | x = [float(v) for v in x] 79 | self.frames_with_gt.append(int(x[0])) 80 | pose = torch.zeros((4, 4), dtype=torch.float64) 81 | pose[0, 0:4] = torch.tensor(x[1:5]) 82 | pose[1, 0:4] = torch.tensor(x[5:9]) 83 | pose[2, 0:4] = torch.tensor(x[9:13]) 84 | pose[3, 3] = 1.0 85 | pose = pose @ cam0_to_velo.inverse() 86 | poses2.append(pose.float().numpy()) 87 | self.poses = poses2 88 | self.train = train 89 | 90 | gt_file = os.path.join(dir, 'data_poses', sequence, f'{loop_file}.pickle') 91 | self.loop_gt = [] 92 | with open(gt_file, 'rb') as f: 93 | temp = pickle.load(f) 94 | for elem in temp: 95 | temp_dict = {'idx': elem['idx'], 'positive_idxs': elem['positive_idxs']} 96 | self.loop_gt.append(temp_dict) 97 | del temp 98 | self.have_matches = [] 99 | for i in range(len(self.loop_gt)): 100 | self.have_matches.append(self.loop_gt[i]['idx']) 101 | 102 | def __len__(self): 103 | return len(self.frames_with_gt) 104 | 105 | def __getitem__(self, idx): 106 | frame_idx = self.frames_with_gt[idx] 107 | 108 | anchor_pcd = torch.from_numpy(get_velo(frame_idx, self.dir, self.sequence, 109 | self.jitter, self.remove_random_angle, self.without_ground)) 110 | 111 | sample = {'anchor': anchor_pcd} 112 | 113 | return sample 114 | 115 | 116 | class KITTI3603DDictPairs(Dataset): 117 | """KITTI ODOMETRY DATASET""" 118 | 119 | def __init__(self, dir, sequence, loop_file='loop_GT', jitter=False, without_ground=False): 120 | """ 121 | 122 | :param dataset: directory where dataset is located 123 | :param sequence: KITTI sequence 124 | :param poses: csv with data poses 125 | """ 126 | 127 | super(KITTI3603DDictPairs, self).__init__() 128 | 129 | self.jitter = jitter 130 | self.dir = dir 131 | self.sequence = sequence 132 | self.sequence_int = int(sequence[-8:-5]) 133 | self.without_ground = without_ground 134 | calib_file = os.path.join(dir, 'calibration', 'calib_cam_to_velo.txt') 135 | with open(calib_file, 'r') as f: 136 | for line in f.readlines(): 137 | data = np.array([float(x) for x in line.split()]) 138 | 139 | cam0_to_velo = np.reshape(data, (3, 4)) 140 | cam0_to_velo = np.vstack([cam0_to_velo, [0, 0, 0, 1]]) 141 | cam0_to_velo = torch.tensor(cam0_to_velo) 142 | 143 | self.frames_with_gt = [] 144 | poses2 = {} 145 | poses = os.path.join(dir, 'data_poses', sequence, 'cam0_to_world.txt') 146 | with open(poses, 'r') as f: 147 | for x in f: 148 | x = x.strip().split() 149 | x = [float(v) for v in x] 150 | self.frames_with_gt.append(int(x[0])) 151 | pose = torch.zeros((4, 4), dtype=torch.float64) 152 | pose[0, 0:4] = torch.tensor(x[1:5]) 153 | pose[1, 0:4] = torch.tensor(x[5:9]) 154 | pose[2, 0:4] = torch.tensor(x[9:13]) 155 | pose[3, 3] = 1.0 156 | pose = pose @ cam0_to_velo.inverse() 157 | poses2[int(x[0])] = pose.float().numpy() 158 | self.poses = poses2 159 | gt_file = os.path.join(dir, 'data_poses', sequence, f'{loop_file}.pickle') 160 | self.loop_gt = [] 161 | with open(gt_file, 'rb') as f: 162 | temp = pickle.load(f) 163 | for elem in temp: 164 | temp_dict = {'idx': elem['idx'], 'positive_idxs': elem['positive_idxs']} 165 | self.loop_gt.append(temp_dict) 166 | del temp 167 | self.have_matches = [] 168 | for i in range(len(self.loop_gt)): 169 | self.have_matches.append(self.loop_gt[i]['idx']) 170 | 171 | def __len__(self): 172 | return len(self.loop_gt) 173 | 174 | def __getitem__(self, idx): 175 | frame_idx = self.loop_gt[idx]['idx'] 176 | if frame_idx not in self.poses: 177 | print(f"ERROR: sequence {self.sequence}, frame idx {frame_idx} ") 178 | 179 | anchor_pcd = torch.from_numpy(get_velo(frame_idx, self.dir, self.sequence, self.jitter, self.without_ground)) 180 | 181 | anchor_pose = self.poses[frame_idx] 182 | anchor_transl = torch.tensor(anchor_pose[:3, 3], dtype=torch.float32) 183 | 184 | positive_idx = np.random.choice(self.loop_gt[idx]['positive_idxs']) 185 | positive_pcd = torch.from_numpy(get_velo(positive_idx, self.dir, self.sequence, self.jitter, self.without_ground)) 186 | 187 | if positive_idx not in self.poses: 188 | print(f"ERRORE: sequence {self.sequence}, positive idx {positive_idx} ") 189 | positive_pose = self.poses[positive_idx] 190 | positive_transl = torch.tensor(positive_pose[:3, 3], dtype=torch.float32) 191 | 192 | r_anch = anchor_pose 193 | r_pos = positive_pose 194 | r_anch = RT.npto_XYZRPY(r_anch)[3:] 195 | r_pos = RT.npto_XYZRPY(r_pos)[3:] 196 | 197 | anchor_rot_torch = torch.tensor(r_anch.copy(), dtype=torch.float32) 198 | positive_rot_torch = torch.tensor(r_pos.copy(), dtype=torch.float32) 199 | 200 | sample = {'anchor': anchor_pcd, 201 | 'positive': positive_pcd, 202 | 'sequence': self.sequence_int, 203 | 'anchor_pose': anchor_transl, 204 | 'positive_pose': positive_transl, 205 | 'anchor_rot': anchor_rot_torch, 206 | 'positive_rot': positive_rot_torch, 207 | 'anchor_idx': frame_idx, 208 | 'positive_idx': positive_idx 209 | } 210 | 211 | return sample 212 | -------------------------------------------------------------------------------- /datasets/KITTIDataset.py: -------------------------------------------------------------------------------- 1 | import h5py 2 | import torch 3 | from pykitti.utils import read_calib_file 4 | from torch.utils.data import Dataset 5 | import os, os.path 6 | import numpy as np 7 | import random 8 | import pickle 9 | 10 | import utils.rotation_conversion as RT 11 | 12 | 13 | def get_velo(idx, dir, sequence, jitter=False, remove_random_angle=-1, without_ground=False): 14 | if without_ground: 15 | velo_path = os.path.join(dir, 'sequences', f'{int(sequence):02d}', 16 | 'velodyne_no_ground', f'{idx:06d}.h5') 17 | with h5py.File(velo_path, 'r') as hf: 18 | scan = hf['PC'][:] 19 | else: 20 | velo_path = os.path.join(dir, 'sequences', f'{int(sequence):02d}', 'velodyne', f'{idx:06d}.bin') 21 | scan = np.fromfile(velo_path, dtype=np.float32) 22 | scan = scan.reshape((-1, 4)) 23 | 24 | if jitter: 25 | noise = 0.01 * np.random.randn(scan.shape[0], scan.shape[1]).astype(np.float32) 26 | noise = np.clip(noise, -0.05, 0.05) 27 | scan = scan + noise 28 | 29 | if remove_random_angle > 0: 30 | azi = np.arctan2(scan[..., 1], scan[..., 0]) 31 | cols = 2084 * (np.pi - azi) / (2 * np.pi) 32 | cols = np.minimum(cols, 2084 - 1) 33 | cols = np.int32(cols) 34 | start_idx = np.random.randint(0, 2084) 35 | end_idx = start_idx + (remove_random_angle / (360.0/2084)) 36 | end_idx = int(end_idx % 2084) 37 | remove_idxs = cols > start_idx 38 | remove_idxs = remove_idxs & (cols < end_idx) 39 | scan = scan[np.logical_not(remove_idxs)] 40 | 41 | return scan 42 | 43 | 44 | class KITTILoader3DPoses(Dataset): 45 | """KITTI ODOMETRY DATASET""" 46 | 47 | def __init__(self, dir, sequence, poses, train=False, loop_file='loop_GT', 48 | jitter=False, remove_random_angle=-1, without_ground=False): 49 | """ 50 | 51 | :param dir: directory where dataset is located 52 | :param sequence: KITTI sequence 53 | :param poses: semantic-KITTI ground truth poses file 54 | """ 55 | 56 | self.dir = dir 57 | self.sequence = sequence 58 | self.jitter = jitter 59 | self.remove_random_angle = remove_random_angle 60 | self.without_ground = without_ground 61 | data = read_calib_file(os.path.join(dir, 'sequences', sequence, 'calib.txt')) 62 | cam0_to_velo = np.reshape(data['Tr'], (3, 4)) 63 | cam0_to_velo = np.vstack([cam0_to_velo, [0, 0, 0, 1]]) 64 | cam0_to_velo = torch.tensor(cam0_to_velo) 65 | poses2 = [] 66 | with open(poses, 'r') as f: 67 | for x in f: 68 | x = x.strip().split() 69 | x = [float(v) for v in x] 70 | pose = torch.zeros((4, 4), dtype=torch.float64) 71 | pose[0, 0:4] = torch.tensor(x[0:4]) 72 | pose[1, 0:4] = torch.tensor(x[4:8]) 73 | pose[2, 0:4] = torch.tensor(x[8:12]) 74 | pose[3, 3] = 1.0 75 | pose = cam0_to_velo.inverse() @ (pose @ cam0_to_velo) 76 | poses2.append(pose.float().numpy()) 77 | self.poses = poses2 78 | self.train = train 79 | 80 | gt_file = os.path.join(dir, 'sequences', sequence, f'{loop_file}.pickle') 81 | with open(gt_file, 'rb') as f: 82 | self.loop_gt = pickle.load(f) 83 | self.have_matches = [] 84 | for i in range(len(self.loop_gt)): 85 | self.have_matches.append(self.loop_gt[i]['idx']) 86 | 87 | def __len__(self): 88 | return len(self.poses) 89 | 90 | def __getitem__(self, idx): 91 | 92 | anchor_pcd = torch.from_numpy(get_velo(idx, self.dir, self.sequence, self.jitter, 93 | self.remove_random_angle, self.without_ground)) 94 | 95 | sample = {'anchor': anchor_pcd} 96 | 97 | return sample 98 | 99 | 100 | class KITTILoader3DDictPairs(Dataset): 101 | """KITTI ODOMETRY DATASET""" 102 | 103 | def __init__(self, dir, sequence, poses, loop_file='loop_GT', jitter=False, without_ground=False): 104 | """ 105 | 106 | :param dataset: directory where dataset is located 107 | :param sequence: KITTI sequence 108 | :param poses: csv with data poses 109 | """ 110 | 111 | super(KITTILoader3DDictPairs, self).__init__() 112 | 113 | self.jitter = jitter 114 | self.dir = dir 115 | self.sequence = int(sequence) 116 | self.without_ground = without_ground 117 | data = read_calib_file(os.path.join(dir, 'sequences', sequence, 'calib.txt')) 118 | cam0_to_velo = np.reshape(data['Tr'], (3, 4)) 119 | cam0_to_velo = np.vstack([cam0_to_velo, [0, 0, 0, 1]]) 120 | cam0_to_velo = torch.tensor(cam0_to_velo) 121 | poses2 = [] 122 | with open(poses, 'r') as f: 123 | for x in f: 124 | x = x.strip().split() 125 | x = [float(v) for v in x] 126 | pose = torch.zeros((4, 4), dtype=torch.float64) 127 | pose[0, 0:4] = torch.tensor(x[0:4]) 128 | pose[1, 0:4] = torch.tensor(x[4:8]) 129 | pose[2, 0:4] = torch.tensor(x[8:12]) 130 | pose[3, 3] = 1.0 131 | pose = cam0_to_velo.inverse() @ (pose @ cam0_to_velo) 132 | poses2.append(pose.float().numpy()) 133 | self.poses = poses2 134 | gt_file = os.path.join(dir, 'sequences', sequence, f'{loop_file}.pickle') 135 | with open(gt_file, 'rb') as f: 136 | self.loop_gt = pickle.load(f) 137 | self.have_matches = [] 138 | for i in range(len(self.loop_gt)): 139 | self.have_matches.append(self.loop_gt[i]['idx']) 140 | 141 | def __len__(self): 142 | return len(self.loop_gt) 143 | 144 | def __getitem__(self, idx): 145 | frame_idx = self.loop_gt[idx]['idx'] 146 | if frame_idx >= len(self.poses): 147 | print(f"ERRORE: sequence {self.sequence}, frame idx {frame_idx} ") 148 | 149 | anchor_pcd = torch.from_numpy(get_velo(frame_idx, self.dir, self.sequence, self.jitter, self.without_ground)) 150 | 151 | #Random permute points 152 | random_permute = torch.randperm(anchor_pcd.shape[0]) 153 | anchor_pcd = anchor_pcd[random_permute] 154 | 155 | anchor_pose = self.poses[frame_idx] 156 | anchor_transl = torch.tensor(anchor_pose[:3, 3], dtype=torch.float32) 157 | 158 | positive_idx = np.random.choice(self.loop_gt[idx]['positive_idxs']) 159 | 160 | positive_pcd = torch.from_numpy(get_velo(positive_idx, self.dir, self.sequence, self.jitter, self.without_ground)) 161 | 162 | #Random permute points 163 | random_permute = torch.randperm(positive_pcd.shape[0]) 164 | positive_pcd = positive_pcd[random_permute] 165 | 166 | 167 | if positive_idx >= len(self.poses): 168 | print(f"ERROR: sequence {self.sequence}, positive idx {positive_idx} ") 169 | positive_pose = self.poses[positive_idx] 170 | positive_transl = torch.tensor(positive_pose[:3, 3], dtype=torch.float32) 171 | 172 | r_anch = anchor_pose 173 | r_pos = positive_pose 174 | r_anch = RT.npto_XYZRPY(r_anch)[3:] 175 | r_pos = RT.npto_XYZRPY(r_pos)[3:] 176 | 177 | anchor_rot_torch = torch.tensor(r_anch.copy(), dtype=torch.float32) 178 | positive_rot_torch = torch.tensor(r_pos.copy(), dtype=torch.float32) 179 | 180 | sample = {'anchor': anchor_pcd, 181 | 'positive': positive_pcd, 182 | 'sequence': self.sequence, 183 | 'anchor_pose': anchor_transl, 184 | 'positive_pose': positive_transl, 185 | 'anchor_rot': anchor_rot_torch, 186 | 'positive_rot': positive_rot_torch, 187 | 'anchor_idx': frame_idx, 188 | 'positive_idx': positive_idx 189 | } 190 | 191 | return sample 192 | -------------------------------------------------------------------------------- /environment_lcdnet.yml: -------------------------------------------------------------------------------- 1 | name: lcdnet 2 | channels: 3 | - pytorch 4 | - conda-forge 5 | - defaults 6 | dependencies: 7 | - _libgcc_mutex=0.1=main 8 | - _openmp_mutex=5.1=1_gnu 9 | - blas=1.0=mkl 10 | - bzip2=1.0.8=h7f98852_4 11 | - ca-certificates=2023.01.10=h06a4308_0 12 | - certifi=2022.12.7=py37h06a4308_0 13 | - cudatoolkit=11.3.1=h9edb442_10 14 | - faiss-cpu=1.7.3=py3.7_h2a577fa_0_cpu 15 | - ffmpeg=4.3=hf484d3e_0 16 | - freetype=2.10.4=h0708190_1 17 | - gmp=6.2.1=h58526e2_0 18 | - gnutls=3.6.13=h85f3911_1 19 | - intel-openmp=2021.4.0=h06a4308_3561 20 | - jbig=2.1=h7f98852_2003 21 | - jpeg=9e=h166bdaf_1 22 | - lame=3.100=h7f98852_1001 23 | - lcms2=2.12=hddcbb42_0 24 | - ld_impl_linux-64=2.38=h1181459_1 25 | - lerc=2.2.1=h9c3ff4c_0 26 | - libdeflate=1.7=h7f98852_5 27 | - libfaiss=1.7.3=h2bc3f7f_0_cpu 28 | - libffi=3.4.2=h6a678d5_6 29 | - libgcc-ng=11.2.0=h1234567_1 30 | - libgomp=11.2.0=h1234567_1 31 | - libiconv=1.17=h166bdaf_0 32 | - libpng=1.6.37=h21135ba_2 33 | - libstdcxx-ng=11.2.0=h1234567_1 34 | - libtiff=4.3.0=hf544144_1 35 | - libuv=1.43.0=h7f98852_0 36 | - libwebp-base=1.2.2=h7f98852_1 37 | - lz4-c=1.9.3=h9c3ff4c_1 38 | - mkl=2021.4.0=h06a4308_640 39 | - mkl-service=2.4.0=py37h402132d_0 40 | - mkl_fft=1.3.1=py37h3e078e5_1 41 | - mkl_random=1.2.2=py37h219a48f_0 42 | - ncurses=6.4=h6a678d5_0 43 | - nettle=3.6=he412f7d_0 44 | - numpy-base=1.21.5=py37ha15fc14_3 45 | - olefile=0.46=pyh9f0ad1d_1 46 | - openh264=2.1.1=h780b84a_0 47 | - openjpeg=2.4.0=hb52868f_1 48 | - openssl=1.1.1s=h7f8727e_0 49 | - pillow=8.3.2=py37h0f21c89_0 50 | - pip=22.3.1=py37h06a4308_0 51 | - python=3.7.16=h7a1cb2a_0 52 | - python_abi=3.7=2_cp37m 53 | - pytorch=1.10.1=py3.7_cuda11.3_cudnn8.2.0_0 54 | - pytorch-mutex=1.0=cuda 55 | - readline=8.2=h5eee18b_0 56 | - setuptools=65.6.3=py37h06a4308_0 57 | - six=1.16.0=pyh6c4a22f_0 58 | - sqlite=3.40.1=h5082296_0 59 | - tk=8.6.12=h1ccaba5_0 60 | - torchaudio=0.10.1=py37_cu113 61 | - torchvision=0.11.2=py37_cu113 62 | - typing_extensions=4.4.0=pyha770c72_0 63 | - wheel=0.37.1=pyhd3eb1b0_0 64 | - xz=5.2.10=h5eee18b_1 65 | - zlib=1.2.13=h5eee18b_0 66 | - zstd=1.5.0=ha95c52a_0 67 | - pip: 68 | - addict==2.4.0 69 | - aiofiles==22.1.0 70 | - aiosqlite==0.18.0 71 | - anyio==3.6.2 72 | - appdirs==1.4.4 73 | - argon2-cffi==21.3.0 74 | - argon2-cffi-bindings==21.2.0 75 | - arrow==1.2.3 76 | - attrs==22.2.0 77 | - babel==2.11.0 78 | - backcall==0.2.0 79 | - beautifulsoup4==4.11.2 80 | - beniget==0.4.1 81 | - bleach==6.0.0 82 | - cached-property==1.5.2 83 | - ccimport==0.3.7 84 | - cffi==1.15.1 85 | - charset-normalizer==3.0.1 86 | - click==8.1.3 87 | - cloudpickle==2.2.1 88 | - configargparse==1.5.3 89 | - cumm-cu113==0.2.9 90 | - cycler==0.11.0 91 | - cython==0.29.33 92 | - dash==2.8.1 93 | - dash-core-components==2.0.0 94 | - dash-html-components==2.0.0 95 | - dash-table==5.0.0 96 | - debugpy==1.6.6 97 | - decorator==5.1.1 98 | - defusedxml==0.7.1 99 | - deprecation==2.1.0 100 | - docker-pycreds==0.4.0 101 | - easydict==1.10 102 | - entrypoints==0.4 103 | - fastjsonschema==2.16.2 104 | - fire==0.5.0 105 | - flask==2.2.2 106 | - fonttools==4.38.0 107 | - fqdn==1.5.1 108 | - gast==0.5.3 109 | - gitdb==4.0.10 110 | - gitpython==3.1.30 111 | - h5py==3.8.0 112 | - idna==3.4 113 | - importlib-metadata==6.0.0 114 | - importlib-resources==5.10.2 115 | - ipykernel==6.16.2 116 | - ipython==7.34.0 117 | - ipython-genutils==0.2.0 118 | - ipywidgets==8.0.4 119 | - isoduration==20.11.0 120 | - itsdangerous==2.1.2 121 | - jedi==0.18.2 122 | - jinja2==3.1.2 123 | - joblib==1.2.0 124 | - json5==0.9.11 125 | - jsonpointer==2.3 126 | - jsonschema==4.17.3 127 | - jupyter-client==7.4.9 128 | - jupyter-core==4.12.0 129 | - jupyter-events==0.5.0 130 | - jupyter-packaging==0.12.3 131 | - jupyter-server==1.23.5 132 | - jupyter-server-fileid==0.6.0 133 | - jupyter-server-ydoc==0.6.1 134 | - jupyter-ydoc==0.2.2 135 | - jupyterlab==3.6.1 136 | - jupyterlab-pygments==0.2.2 137 | - jupyterlab-server==2.19.0 138 | - jupyterlab-widgets==3.0.5 139 | - kiwisolver==1.4.4 140 | - lark==1.1.5 141 | - llvmlite==0.39.1 142 | - markupsafe==2.1.2 143 | - matplotlib==3.5.3 144 | - matplotlib-inline==0.1.6 145 | - mistune==2.0.4 146 | - nbclassic==0.5.1 147 | - nbclient==0.7.2 148 | - nbconvert==7.2.9 149 | - nbformat==5.5.0 150 | - nest-asyncio==1.5.6 151 | - networkx==2.6.3 152 | - ninja==1.11.1 153 | - notebook==6.5.2 154 | - notebook-shim==0.2.2 155 | - numba==0.56.4 156 | - numpy==1.20.0 157 | - open3d==0.14.1 158 | - opencv-python==4.7.0.68 159 | - packaging==23.0 160 | - pandas==1.3.5 161 | - pandocfilters==1.5.0 162 | - parso==0.8.3 163 | - pathtools==0.1.2 164 | - pccm==0.3.4 165 | - pexpect==4.8.0 166 | - pickleshare==0.7.5 167 | - pkgutil-resolve-name==1.3.10 168 | - plotly==5.13.0 169 | - ply==3.11 170 | - portalocker==2.7.0 171 | - prometheus-client==0.16.0 172 | - prompt-toolkit==3.0.36 173 | - protobuf==3.20.1 174 | - psutil==5.9.4 175 | - ptyprocess==0.7.0 176 | - pybind11==2.10.3 177 | - pycparser==2.21 178 | - pygments==2.14.0 179 | - pykitti==0.3.1 180 | - pyparsing==3.0.9 181 | - pyquaternion==0.9.9 182 | - pyrsistent==0.19.3 183 | - python-dateutil==2.8.2 184 | - python-json-logger==2.0.4 185 | - pythran==0.12.1 186 | - pytorch-metric-learning==2.0.0 187 | - pytz==2022.7.1 188 | - pywavelets==1.3.0 189 | - pyyaml==6.0 190 | - pyzmq==25.0.0 191 | - requests==2.28.2 192 | - rfc3339-validator==0.1.4 193 | - rfc3986-validator==0.1.1 194 | - scikit-image==0.14.5 195 | - scikit-learn==1.0.2 196 | - scipy==1.7.3 197 | - send2trash==1.8.0 198 | - sentry-sdk==1.14.0 199 | - setproctitle==1.3.2 200 | - sharedarray==3.2.2 201 | - smmap==5.0.0 202 | - sniffio==1.3.0 203 | - soupsieve==2.3.2.post1 204 | - spconv-cu113==2.1.25 205 | - tenacity==8.1.0 206 | - tensorboardx==2.5.1 207 | - termcolor==2.2.0 208 | - terminado==0.17.1 209 | - threadpoolctl==3.1.0 210 | - tinycss2==1.2.1 211 | - tomli==2.0.1 212 | - tomlkit==0.11.6 213 | - tornado==6.2 214 | - tqdm==4.64.1 215 | - traitlets==5.9.0 216 | - uri-template==1.2.0 217 | - urllib3==1.26.14 218 | - wandb==0.13.9 219 | - wcwidth==0.2.6 220 | - webcolors==1.12 221 | - webencodings==0.5.1 222 | - websocket-client==1.5.0 223 | - werkzeug==2.2.2 224 | - widgetsnbextension==4.0.5 225 | - y-py==0.5.5 226 | - ypy-websocket==0.8.2 227 | - zipp==3.12.0 228 | -------------------------------------------------------------------------------- /evaluate_kitti.py: -------------------------------------------------------------------------------- 1 | import faiss 2 | import torch 3 | import torch.utils.data 4 | import numpy as np 5 | from sklearn.metrics import precision_recall_curve, average_precision_score, auc 6 | from sklearn.neighbors import KDTree 7 | 8 | 9 | def evaluate_model_with_emb(emb_list, datasets_list, positive_distance=5.): 10 | recall_sum = 0. 11 | start_idx = 0 12 | cont = 0 13 | F1_sum = 0. 14 | auc_sum = 0. 15 | auc_sum2 = 0. 16 | 17 | emb_list = emb_list.cpu().numpy() 18 | 19 | for dataset in datasets_list: 20 | poses = dataset.poses 21 | samples_num = len(dataset) 22 | finish_idx = start_idx + samples_num 23 | emb_sublist = emb_list[start_idx:finish_idx] 24 | 25 | recall, maxF1, wrong_auc, real_auc = compute_recall(emb_sublist, poses, dataset, positive_distance) 26 | recall_sum = recall_sum + recall 27 | F1_sum += maxF1 28 | auc_sum += wrong_auc 29 | auc_sum2 += real_auc 30 | 31 | start_idx = finish_idx 32 | cont += 1 33 | 34 | final_recall = recall_sum / cont 35 | return final_recall, F1_sum / cont, auc_sum / cont, auc_sum2 / cont 36 | 37 | 38 | def compute_recall(emb_list, poses, dataset, positive_distance=5.): 39 | print('compute_recall') 40 | have_matches = dataset.have_matches 41 | num_neighbors = 25 42 | recall_at_k = [0] * num_neighbors 43 | 44 | num_evaluated = 0 45 | emb_list = np.asarray(emb_list) 46 | 47 | for i in range(len(emb_list)): 48 | if hasattr(dataset, 'frames_with_gt'): 49 | if dataset.frames_with_gt[i] not in have_matches: 50 | continue 51 | elif i not in have_matches: 52 | continue 53 | min_range = max(0, i-50) 54 | max_range = min(i+50, len(emb_list)) 55 | ignored_idxs = set(range(min_range, max_range)) 56 | valid_idx = set(range(len(emb_list))) - ignored_idxs 57 | valid_idx = list(valid_idx) 58 | 59 | # tr = KDTree(emb_list[valid_idx]) 60 | 61 | index = faiss.IndexFlatL2(emb_list.shape[1]) 62 | index.add(emb_list[valid_idx]) 63 | 64 | x = poses[i][0, 3] 65 | y = poses[i][1, 3] 66 | z = poses[i][2, 3] 67 | anchor_pose = torch.tensor([x, y, z]) 68 | num_evaluated += 1 69 | # distances, indices = tr.query(np.array([emb_list[i]]), k=num_neighbors) 70 | 71 | D, I = index.search(emb_list[i:i+1], num_neighbors) 72 | 73 | indices = I[0] 74 | for j in range(len(indices)): 75 | 76 | m = valid_idx[indices[j]] 77 | x = poses[m][0, 3] 78 | y = poses[m][1, 3] 79 | z = poses[m][2, 3] 80 | possible_match_pose = torch.tensor([x, y, z]) 81 | distance = torch.norm(anchor_pose - possible_match_pose) 82 | if distance <= positive_distance: 83 | # if j == 0: 84 | # similarity = np.dot(queries_output[i], database_output[indices[0][j]]) 85 | # top1_similarity_score.append(similarity) 86 | recall_at_k[j] += 1 87 | break 88 | recall_at_k = (np.cumsum(recall_at_k) / float(num_evaluated)) * 100 89 | 90 | map_tree_poses = KDTree(np.stack(poses)[:, :3, 3]) 91 | 92 | index = faiss.IndexFlatL2(emb_list.shape[1]) 93 | index.add(emb_list[:50]) 94 | 95 | real_loop = [] 96 | detected_loop = [] 97 | distances = [] 98 | total_frame = 0 99 | for i in range(100, emb_list.shape[0]): 100 | min_range = max(0, i-50) # Scan Context 101 | current_pose = torch.tensor(poses[i][:3, 3]) 102 | 103 | indices = map_tree_poses.query_radius(np.expand_dims(current_pose, 0), positive_distance) 104 | valid_idxs = list(set(indices[0]) - set(range(min_range, emb_list.shape[0]))) 105 | if len(valid_idxs) > 0: 106 | real_loop.append(1) 107 | else: 108 | real_loop.append(0) 109 | 110 | index.add(emb_list[i-50:i-49]) 111 | nearest = index.search(emb_list[i:i+1], 1) 112 | 113 | total_frame += 1 114 | detected_loop.append(-nearest[0][0][0]) 115 | candidate_pose = torch.tensor(poses[nearest[1][0][0]][:3, 3]) 116 | distances.append((current_pose - candidate_pose).norm()) 117 | 118 | 119 | precision, recall, _ = precision_recall_curve(real_loop, detected_loop) 120 | wrong_auc = average_precision_score(real_loop, detected_loop) 121 | F1 = [2*((precision[i]*recall[i])/(precision[i]+recall[i])) for i in range(len(precision))] 122 | 123 | distances = np.array(distances) 124 | detected_loop = -np.array(detected_loop) 125 | real_loop = np.array(real_loop) 126 | precision2 = [] 127 | recall2 = [] 128 | for thr in np.unique(detected_loop): 129 | tp = detected_loop <= thr 130 | tp = tp & real_loop 131 | tp = tp & (distances <= 4) 132 | tp = tp.sum() 133 | fp = (detected_loop 0.: 136 | precision2.append(tp/(tp+fp)) 137 | else: 138 | precision2.append(1.) 139 | recall2.append(tp/(tp+fn)) 140 | real_auc = auc(recall2, precision2) 141 | 142 | return recall_at_k, np.array(F1).max(), wrong_auc, real_auc 143 | -------------------------------------------------------------------------------- /evaluation/inference_loop_closure.py: -------------------------------------------------------------------------------- 1 | import argparse 2 | import os 3 | import pickle 4 | import time 5 | from collections import OrderedDict 6 | 7 | import faiss 8 | import matplotlib.pyplot as plt 9 | import numpy as np 10 | import torch 11 | import torch.nn.parallel 12 | import torch.utils.data 13 | from pcdet.datasets.kitti.kitti_dataset import KittiDataset 14 | import random 15 | 16 | from sklearn.metrics import precision_recall_curve, average_precision_score 17 | from sklearn.neighbors import KDTree 18 | from torch.utils.data.sampler import Sampler, BatchSampler 19 | from tqdm import tqdm 20 | 21 | from datasets.KITTI360Dataset import KITTI3603DPoses 22 | from datasets.KITTIDataset import KITTILoader3DPoses 23 | from evaluation.plot_PR_curve import compute_PR, compute_AP, compute_PR_pairs 24 | from models.get_models import get_model 25 | from utils.data import merge_inputs 26 | from datetime import datetime 27 | 28 | torch.backends.cudnn.benchmark = True 29 | 30 | EPOCH = 1 31 | 32 | 33 | def _init_fn(worker_id, epoch=0, seed=0): 34 | seed = seed + worker_id + epoch * 100 35 | seed = seed % (2**32 - 1) 36 | print(f"Init worker {worker_id} with seed {seed}") 37 | torch.manual_seed(seed) 38 | np.random.seed(seed) 39 | random.seed(seed) 40 | 41 | 42 | def prepare_input(model, samples, exp_cfg, device): 43 | anchor_list = [] 44 | for point_cloud in samples: 45 | anchor_i = point_cloud 46 | 47 | anchor_list.append(model.backbone.prepare_input(anchor_i)) 48 | del anchor_i 49 | 50 | model_in = KittiDataset.collate_batch(anchor_list) 51 | for key, val in model_in.items(): 52 | if not isinstance(val, np.ndarray): 53 | continue 54 | model_in[key] = torch.from_numpy(val).float().to(device) 55 | return model_in 56 | 57 | 58 | class SamplePairs(Sampler): 59 | 60 | def __init__(self, data_source, pairs): 61 | super(SamplePairs, self).__init__(data_source) 62 | self.pairs = pairs 63 | 64 | def __len__(self): 65 | return len(self.pairs) 66 | 67 | def __iter__(self): 68 | return [self.pairs[i, 0] for i in range(len(self.pairs))] 69 | 70 | 71 | class BatchSamplePairs(BatchSampler): 72 | 73 | def __init__(self, data_source, pairs, batch_size): 74 | # super(BatchSamplePairs, self).__init__(batch_size, True) 75 | self.data_source = data_source 76 | self.pairs = pairs 77 | self.batch_size = batch_size 78 | self.count = 0 79 | 80 | def __len__(self): 81 | return 2*len(self.pairs) 82 | 83 | def __iter__(self): 84 | self.count = 0 85 | while 2*self.count + self.batch_size < 2*len(self.pairs): 86 | current_batch = [] 87 | for i in range(self.batch_size//2): 88 | current_batch.append(self.pairs[self.count+i, 0]) 89 | for i in range(self.batch_size//2): 90 | current_batch.append(self.pairs[self.count+i, 1]) 91 | yield current_batch 92 | self.count += self.batch_size//2 93 | if 2*self.count < 2*len(self.pairs): 94 | diff = 2*len(self.pairs)-2*self.count 95 | current_batch = [] 96 | for i in range(diff//2): 97 | current_batch.append(self.pairs[self.count+i, 0]) 98 | for i in range(diff//2): 99 | current_batch.append(self.pairs[self.count+i, 1]) 100 | yield current_batch 101 | 102 | 103 | 104 | def main_process(gpu, weights_path, args): 105 | global EPOCH 106 | 107 | torch.cuda.set_device(gpu) 108 | device = torch.device(gpu) 109 | 110 | saved_params = torch.load(weights_path, map_location='cpu') 111 | 112 | exp_cfg = saved_params['config'] 113 | exp_cfg['batch_size'] = 6 114 | exp_cfg['loop_file'] = 'loop_GT_4m' 115 | exp_cfg['head'] = 'UOTHead' 116 | 117 | validation_sequences = [args.validation_sequence] 118 | if args.dataset == 'kitti': 119 | validation_dataset = KITTILoader3DPoses(args.root_folder, validation_sequences[0], 120 | os.path.join(args.root_folder, 'sequences', validation_sequences[0],'poses.txt'), 121 | train=False, 122 | loop_file=exp_cfg['loop_file'], 123 | remove_random_angle=args.remove_random_angle, 124 | without_ground=args.without_ground) 125 | elif args.dataset == 'kitti360': 126 | validation_dataset = KITTI3603DPoses(args.root_folder, validation_sequences[0], 127 | train=False, loop_file='loop_GT_4m_noneg', 128 | remove_random_angle=args.remove_random_angle, 129 | without_ground=args.without_ground) 130 | else: 131 | raise argparse.ArgumentTypeError("Unknown dataset") 132 | 133 | ValidationLoader = torch.utils.data.DataLoader(dataset=validation_dataset, 134 | batch_size=exp_cfg['batch_size'], 135 | num_workers=2, 136 | shuffle=False, 137 | collate_fn=merge_inputs, 138 | pin_memory=True) 139 | 140 | model = get_model(exp_cfg, is_training=False) 141 | renamed_dict = OrderedDict() 142 | for key in saved_params['state_dict']: 143 | if not key.startswith('module'): 144 | renamed_dict = saved_params['state_dict'] 145 | break 146 | else: 147 | renamed_dict[key[7:]] = saved_params['state_dict'][key] 148 | 149 | # Convert shape from old OpenPCDet 150 | if renamed_dict['backbone.backbone.conv_input.0.weight'].shape != model.state_dict()['backbone.backbone.conv_input.0.weight'].shape: 151 | for key in renamed_dict: 152 | if key.startswith('backbone.backbone.conv') and key.endswith('weight'): 153 | if len(renamed_dict[key].shape) == 5: 154 | renamed_dict[key] = renamed_dict[key].permute(-1, 0, 1, 2, 3) 155 | 156 | res = model.load_state_dict(renamed_dict, strict=True) 157 | 158 | if len(res[0]) > 0: 159 | print(f"WARNING: MISSING {len(res[0])} KEYS, MAYBE WEIGHTS LOADING FAILED") 160 | 161 | model.train() 162 | model = model.to(device) 163 | 164 | map_tree_poses = KDTree(np.stack(validation_dataset.poses)[:, :3, 3]) 165 | 166 | emb_list_map = [] 167 | rot_errors = [] 168 | transl_errors = [] 169 | time_descriptors = [] 170 | for i in range(args.num_iters): 171 | rot_errors.append([]) 172 | transl_errors.append([]) 173 | 174 | for batch_idx, sample in enumerate(tqdm(ValidationLoader)): 175 | 176 | model.eval() 177 | time1 = time.time() 178 | with torch.no_grad(): 179 | 180 | anchor_list = [] 181 | for i in range(len(sample['anchor'])): 182 | anchor = sample['anchor'][i].to(device) 183 | 184 | anchor_i = anchor 185 | 186 | anchor_list.append(model.backbone.prepare_input(anchor_i)) 187 | del anchor_i 188 | 189 | model_in = KittiDataset.collate_batch(anchor_list) 190 | for key, val in model_in.items(): 191 | if not isinstance(val, np.ndarray): 192 | continue 193 | model_in[key] = torch.from_numpy(val).float().to(device) 194 | 195 | batch_dict = model(model_in, metric_head=False, compute_rotation=False, compute_transl=False) 196 | 197 | emb = batch_dict['out_embedding'] 198 | emb_list_map.append(emb) 199 | 200 | time2 = time.time() 201 | time_descriptors.append(time2-time1) 202 | 203 | emb_list_map = torch.cat(emb_list_map).cpu().numpy() 204 | 205 | emb_list_map_norm = emb_list_map / np.linalg.norm(emb_list_map, axis=1, keepdims=True) 206 | pair_dist = faiss.pairwise_distances(emb_list_map_norm, emb_list_map_norm) 207 | 208 | poses = np.stack(validation_dataset.poses) 209 | _, _, precision_ours_fp, recall_ours_fp = compute_PR(pair_dist, poses, map_tree_poses) 210 | ap_ours_fp = compute_AP(precision_ours_fp, recall_ours_fp) 211 | print(weights_path) 212 | print(validation_sequences) 213 | print("Protocol 1 - Average Precision", ap_ours_fp) 214 | 215 | marker = 'x' 216 | markevery = 0.03 217 | plt.clf() 218 | plt.rcParams.update({'font.size': 16}) 219 | fig = plt.figure() 220 | plt.plot(recall_ours_fp, precision_ours_fp, label='LCDNet (Protocol 1)', marker=marker, markevery=markevery) 221 | plt.xlim([0, 1]) 222 | plt.ylim([0, 1]) 223 | plt.xlabel("Recall [%]") 224 | plt.ylabel("Precision [%]") 225 | plt.xticks([0, 0.2, 0.4, 0.6, 0.8, 1.0], ["0", "20", "40", "60", "80", "100"]) 226 | plt.yticks([0, 0.2, 0.4, 0.6, 0.8, 1.0], ["0", "20", "40", "60", "80", "100"]) 227 | plt.show() 228 | # fig.savefig(f'./precision_recall_curve.pdf', bbox_inches='tight', pad_inches=0) 229 | 230 | if __name__ == '__main__': 231 | parser = argparse.ArgumentParser() 232 | parser.add_argument('--root_folder', default='./KITTI', 233 | help='dataset directory') 234 | parser.add_argument('--weights_path', default='./checkpoints') 235 | parser.add_argument('--num_iters', type=int, default=1) 236 | parser.add_argument('--dataset', type=str, default='kitti') 237 | parser.add_argument('--remove_random_angle', type=int, default=-1) 238 | parser.add_argument('--validation_sequence', type=str, default='08') 239 | parser.add_argument('--without_ground', action='store_true', default=False, 240 | help='Use preprocessed point clouds with ground plane removed') 241 | args = parser.parse_args() 242 | 243 | main_process(0, args.weights_path, args) 244 | -------------------------------------------------------------------------------- /evaluation/inference_yaw_general.py: -------------------------------------------------------------------------------- 1 | import argparse 2 | import os 3 | from collections import OrderedDict 4 | 5 | import faiss 6 | import numpy as np 7 | import torch 8 | import torch.nn.parallel 9 | import torch.utils.data 10 | from pcdet.datasets.kitti.kitti_dataset import KittiDataset 11 | import random 12 | 13 | from scipy.spatial import KDTree 14 | from torch.utils.data.sampler import Sampler, BatchSampler 15 | from tqdm import tqdm 16 | 17 | from datasets.KITTI360Dataset import KITTI3603DPoses 18 | from datasets.KITTIDataset import KITTILoader3DPoses 19 | from models.get_models import get_model 20 | from utils.data import merge_inputs, Timer 21 | from datetime import datetime 22 | from utils.geometry import mat2xyzrpy 23 | import utils.rotation_conversion as RT 24 | 25 | import open3d as o3d 26 | if hasattr(o3d, 'pipelines'): 27 | reg_module = o3d.pipelines.registration 28 | else: 29 | reg_module = o3d.registration 30 | 31 | torch.backends.cudnn.benchmark = True 32 | 33 | EPOCH = 1 34 | 35 | 36 | def _init_fn(worker_id, epoch=0, seed=0): 37 | seed = seed + worker_id + epoch * 100 38 | seed = seed % (2**32 - 1) 39 | print(f"Init worker {worker_id} with seed {seed}") 40 | torch.manual_seed(seed) 41 | np.random.seed(seed) 42 | random.seed(seed) 43 | 44 | 45 | def get_database_embs(model, sample, exp_cfg, device): 46 | model.eval() 47 | margin = exp_cfg['margin'] 48 | 49 | with torch.no_grad(): 50 | anchor_list = [] 51 | for i in range(len(sample['anchor'])): 52 | anchor = sample['anchor'][i].to(device) 53 | 54 | anchor_i = anchor 55 | 56 | anchor_list.append(model.backbone.prepare_input(anchor_i)) 57 | del anchor_i 58 | 59 | model_in = KittiDataset.collate_batch(anchor_list) 60 | for key, val in model_in.items(): 61 | if not isinstance(val, np.ndarray): 62 | continue 63 | model_in[key] = torch.from_numpy(val).float().to(device) 64 | 65 | batch_dict = model(model_in, metric_head=False) 66 | anchor_out = batch_dict['out_embedding'] 67 | 68 | 69 | if exp_cfg['norm_embeddings']: 70 | anchor_out = anchor_out / anchor_out.norm(dim=1, keepdim=True) 71 | return anchor_out 72 | 73 | 74 | class SamplePairs(Sampler): 75 | 76 | def __init__(self, data_source, pairs): 77 | super(SamplePairs, self).__init__(data_source) 78 | self.pairs = pairs 79 | 80 | def __len__(self): 81 | return len(self.pairs) 82 | 83 | def __iter__(self): 84 | return [self.pairs[i, 0] for i in range(len(self.pairs))] 85 | 86 | 87 | class BatchSamplePairs(BatchSampler): 88 | 89 | def __init__(self, data_source, pairs, batch_size): 90 | # super(BatchSamplePairs, self).__init__(batch_size, True) 91 | self.pairs = pairs 92 | self.batch_size = batch_size 93 | self.count = 0 94 | 95 | def __len__(self): 96 | tot = 2*len(self.pairs) 97 | ret = (tot + self.batch_size - 1) // self.batch_size 98 | return ret 99 | 100 | def __iter__(self): 101 | self.count = 0 102 | while 2*self.count + self.batch_size < 2*len(self.pairs): 103 | current_batch = [] 104 | for i in range(self.batch_size//2): 105 | current_batch.append(self.pairs[self.count+i, 0]) 106 | for i in range(self.batch_size//2): 107 | current_batch.append(self.pairs[self.count+i, 1]) 108 | yield current_batch 109 | self.count += self.batch_size//2 110 | if 2*self.count < 2*len(self.pairs): 111 | diff = 2*len(self.pairs)-2*self.count 112 | current_batch = [] 113 | for i in range(diff//2): 114 | current_batch.append(self.pairs[self.count+i, 0]) 115 | for i in range(diff//2): 116 | current_batch.append(self.pairs[self.count+i, 1]) 117 | yield current_batch 118 | 119 | 120 | def main_process(gpu, weights_path, args): 121 | global EPOCH 122 | rank = gpu 123 | 124 | torch.cuda.set_device(gpu) 125 | device = torch.device(gpu) 126 | 127 | saved_params = torch.load(weights_path, map_location='cpu') 128 | exp_cfg = saved_params['config'] 129 | exp_cfg['batch_size'] = 2 130 | 131 | exp_cfg['loop_file'] = 'loop_GT_4m' 132 | exp_cfg['head'] = 'UOTHead' 133 | exp_cfg['sinkhorn_type'] = 'unbalanced' 134 | 135 | current_date = datetime.now() 136 | 137 | exp_cfg['test_sequence'] = args.validation_sequence 138 | sequences_validation = [exp_cfg['test_sequence']] 139 | exp_cfg['sinkhorn_iter'] = 5 140 | 141 | if args.dataset == 'kitti': 142 | dataset_for_recall = KITTILoader3DPoses(args.root_folder, sequences_validation[0], 143 | os.path.join(args.root_folder, 'sequences', sequences_validation[0],'poses.txt'), 144 | train=False, loop_file=exp_cfg['loop_file'], 145 | remove_random_angle=args.remove_random_angle, 146 | without_ground=args.without_ground) 147 | else: 148 | dataset_for_recall = KITTI3603DPoses(args.root_folder, sequences_validation[0], 149 | train=False, loop_file='loop_GT_4m_noneg', 150 | remove_random_angle=args.remove_random_angle, 151 | without_ground=args.without_ground) 152 | 153 | test_pair_idxs = [] 154 | index = faiss.IndexFlatL2(3) 155 | poses = np.stack(dataset_for_recall.poses).copy() 156 | index.add(poses[:50, :3, 3].copy()) 157 | num_frames_with_loop = 0 158 | num_frames_with_reverse_loop = 0 159 | for i in tqdm(range(100, len(dataset_for_recall.poses))): 160 | current_pose = poses[i:i+1, :3, 3].copy() 161 | index.add(poses[i-50:i-49, :3, 3].copy()) 162 | lims, D, I = index.range_search(current_pose, 4.**2) 163 | for j in range(lims[0], lims[1]): 164 | if j == 0: 165 | num_frames_with_loop += 1 166 | yaw_diff = RT.npto_XYZRPY(np.linalg.inv(poses[I[j]]) @ poses[i])[-1] 167 | yaw_diff = yaw_diff % (2 * np.pi) 168 | if 0.79 <= yaw_diff <= 5.5: 169 | num_frames_with_reverse_loop += 1 170 | 171 | test_pair_idxs.append([I[j], i]) 172 | test_pair_idxs = np.array(test_pair_idxs) 173 | 174 | batch_sampler = BatchSamplePairs(dataset_for_recall, test_pair_idxs, exp_cfg['batch_size']) 175 | RecallLoader = torch.utils.data.DataLoader(dataset=dataset_for_recall, 176 | # batch_size=exp_cfg['batch_size'], 177 | num_workers=2, 178 | # sampler=sampler, 179 | batch_sampler=batch_sampler, 180 | # worker_init_fn=init_fn, 181 | collate_fn=merge_inputs, 182 | pin_memory=True) 183 | 184 | model = get_model(exp_cfg) 185 | 186 | renamed_dict = OrderedDict() 187 | for key in saved_params['state_dict']: 188 | if not key.startswith('module'): 189 | renamed_dict = saved_params['state_dict'] 190 | break 191 | else: 192 | renamed_dict[key[7:]] = saved_params['state_dict'][key] 193 | 194 | # Convert shape from old OpenPCDet 195 | if renamed_dict['backbone.backbone.conv_input.0.weight'].shape != model.state_dict()['backbone.backbone.conv_input.0.weight'].shape: 196 | for key in renamed_dict: 197 | if key.startswith('backbone.backbone.conv') and key.endswith('weight'): 198 | if len(renamed_dict[key].shape) == 5: 199 | renamed_dict[key] = renamed_dict[key].permute(-1, 0, 1, 2, 3) 200 | 201 | model.load_state_dict(renamed_dict, strict=True) 202 | 203 | model = model.to(device) 204 | 205 | rot_errors = [] 206 | transl_errors = [] 207 | yaw_error = [] 208 | for i in range(args.num_iters): 209 | rot_errors.append([]) 210 | transl_errors.append([]) 211 | 212 | # Testing 213 | if exp_cfg['weight_rot'] > 0: 214 | current_frame = 0 215 | yaw_preds = torch.zeros((len(dataset_for_recall.poses), len(dataset_for_recall.poses))) 216 | transl_errors = [] 217 | for batch_idx, sample in enumerate(tqdm(RecallLoader)): 218 | 219 | model.eval() 220 | with torch.no_grad(): 221 | 222 | anchor_list = [] 223 | for i in range(len(sample['anchor'])): 224 | anchor = sample['anchor'][i].to(device) 225 | 226 | anchor_i = anchor 227 | 228 | anchor_list.append(model.backbone.prepare_input(anchor_i)) 229 | del anchor_i 230 | 231 | model_in = KittiDataset.collate_batch(anchor_list) 232 | for key, val in model_in.items(): 233 | if not isinstance(val, np.ndarray): 234 | continue 235 | model_in[key] = torch.from_numpy(val).float().to(device) 236 | 237 | torch.cuda.synchronize() 238 | batch_dict = model(model_in, metric_head=True) 239 | torch.cuda.synchronize() 240 | pred_transl = [] 241 | yaw = batch_dict['out_rotation'] 242 | 243 | if not args.ransac: 244 | transformation = batch_dict['transformation'] 245 | homogeneous = torch.tensor([0., 0., 0., 1.]).repeat(transformation.shape[0], 1, 1).to(transformation.device) 246 | transformation = torch.cat((transformation, homogeneous), dim=1) 247 | transformation = transformation.inverse() 248 | for i in range(batch_dict['batch_size'] // 2): 249 | yaw_preds[test_pair_idxs[current_frame+i, 0], test_pair_idxs[current_frame+i, 1]] = mat2xyzrpy(transformation[i])[-1].item() 250 | pred_transl.append(transformation[i][:3, 3].detach().cpu()) 251 | elif args.ransac: 252 | coords = batch_dict['point_coords'].view(batch_dict['batch_size'], -1, 4) 253 | feats = batch_dict['point_features'].squeeze(-1) 254 | for i in range(batch_dict['batch_size'] // 2): 255 | coords1 = coords[i] 256 | coords2 = coords[i + batch_dict['batch_size'] // 2] 257 | feat1 = feats[i] 258 | feat2 = feats[i + batch_dict['batch_size'] // 2] 259 | pcd1 = o3d.geometry.PointCloud() 260 | pcd1.points = o3d.utility.Vector3dVector(coords1[:, 1:].cpu().numpy()) 261 | pcd2 = o3d.geometry.PointCloud() 262 | pcd2.points = o3d.utility.Vector3dVector(coords2[:, 1:].cpu().numpy()) 263 | pcd1_feat = reg_module.Feature() 264 | pcd1_feat.data = feat1.permute(0, 1).cpu().numpy() 265 | pcd2_feat = reg_module.Feature() 266 | pcd2_feat.data = feat2.permute(0, 1).cpu().numpy() 267 | 268 | torch.cuda.synchronize() 269 | try: 270 | result = reg_module.registration_ransac_based_on_feature_matching( 271 | pcd2, pcd1, pcd2_feat, pcd1_feat, True, 272 | 0.6, 273 | reg_module.TransformationEstimationPointToPoint(False), 274 | 3, [], 275 | reg_module.RANSACConvergenceCriteria(5000)) 276 | except: 277 | result = reg_module.registration_ransac_based_on_feature_matching( 278 | pcd2, pcd1, pcd2_feat, pcd1_feat, 279 | 0.6, 280 | reg_module.TransformationEstimationPointToPoint(False), 281 | 3, [], 282 | reg_module.RANSACConvergenceCriteria(5000)) 283 | 284 | transformation = torch.tensor(result.transformation.copy()) 285 | if args.icp: 286 | p1 = o3d.geometry.PointCloud() 287 | p1.points = o3d.utility.Vector3dVector(sample['anchor'][i][:, :3].cpu().numpy()) 288 | p2 = o3d.geometry.PointCloud() 289 | p2.points = o3d.utility.Vector3dVector( 290 | sample['anchor'][i + batch_dict['batch_size'] // 2][:, :3].cpu().numpy()) 291 | result2 = reg_module.registration_icp( 292 | p2, p1, 0.1, result.transformation, 293 | reg_module.TransformationEstimationPointToPoint()) 294 | transformation = torch.tensor(result2.transformation.copy()) 295 | yaw_preds[test_pair_idxs[current_frame + i, 0], test_pair_idxs[current_frame + i, 1]] = \ 296 | mat2xyzrpy(transformation)[-1].item() 297 | pred_transl.append(transformation[:3, 3].detach().cpu()) 298 | 299 | for i in range(batch_dict['batch_size'] // 2): 300 | pose1 = dataset_for_recall.poses[test_pair_idxs[current_frame+i, 0]] 301 | pose2 = dataset_for_recall.poses[test_pair_idxs[current_frame+i, 1]] 302 | delta_pose = np.linalg.inv(pose1) @ pose2 303 | transl_error = torch.tensor(delta_pose[:3, 3]) - pred_transl[i] 304 | transl_errors.append(transl_error.norm()) 305 | 306 | yaw_pred = yaw_preds[test_pair_idxs[current_frame+i, 0], test_pair_idxs[current_frame+i, 1]] 307 | yaw_pred = yaw_pred % (2 * np.pi) 308 | delta_yaw = RT.npto_XYZRPY(delta_pose)[-1] 309 | delta_yaw = delta_yaw % (2 * np.pi) 310 | diff_yaw = abs(delta_yaw - yaw_pred) 311 | diff_yaw = diff_yaw % (2 * np.pi) 312 | diff_yaw = (diff_yaw * 180) / np.pi 313 | if diff_yaw > 180.: 314 | diff_yaw = 360 - diff_yaw 315 | yaw_error.append(diff_yaw) 316 | 317 | current_frame += batch_dict['batch_size'] // 2 318 | 319 | 320 | print(weights_path) 321 | print(exp_cfg['test_sequence']) 322 | 323 | transl_errors = np.array(transl_errors) 324 | yaw_error = np.array(yaw_error) 325 | print("Mean rotation error: ", yaw_error.mean()) 326 | print("Median rotation error: ", np.median(yaw_error)) 327 | print("STD rotation error: ", yaw_error.std()) 328 | print("Mean translation error: ", transl_errors.mean()) 329 | print("Median translation error: ", np.median(transl_errors)) 330 | print("STD translation error: ", transl_errors.std()) 331 | # save_dict = {'rot': yaw_error, 'transl': transl_errors} 332 | # save_path = f'./results_for_paper/lcdnet00+08_{exp_cfg["test_sequence"]}' 333 | if '360' in weights_path: 334 | save_path = f'./results_for_paper/lcdnet++_{exp_cfg["test_sequence"]}' 335 | else: 336 | save_path = f'./results_for_paper/lcdnet00+08_{exp_cfg["test_sequence"]}' 337 | if args.remove_random_angle > 0: 338 | save_path = save_path + f'_remove{args.remove_random_angle}' 339 | if args.icp: 340 | save_path = save_path+'_icp' 341 | elif args.ransac: 342 | save_path = save_path+'_ransac' 343 | if args.teaser: 344 | save_path = save_path + '_teaser' 345 | # print("Saving to ", save_path) 346 | # with open(f'{save_path}.pickle', 'wb') as f: 347 | # pickle.dump(save_dict, f) 348 | valid = yaw_error <= 5. 349 | valid = valid & (np.array(transl_errors) <= 2.) 350 | succ_rate = valid.sum() / valid.shape[0] 351 | rte_suc = transl_errors[valid].mean() 352 | rre_suc = yaw_error[valid].mean() 353 | print(f"Success Rate: {succ_rate}, RTE: {rte_suc}, RRE: {rre_suc}") 354 | 355 | 356 | if __name__ == '__main__': 357 | parser = argparse.ArgumentParser() 358 | parser.add_argument('--root_folder', default='/home/cattaneo/Datasets/KITTI', 359 | help='dataset directory') 360 | parser.add_argument('--weights_path', default='/home/cattaneo/checkpoints/deep_lcd') 361 | parser.add_argument('--num_iters', type=int, default=1) 362 | parser.add_argument('--dataset', type=str, default='kitti') 363 | parser.add_argument('--ransac', action='store_true', default=False) 364 | parser.add_argument('--teaser', action='store_true', default=False) 365 | parser.add_argument('--icp', action='store_true', default=False) 366 | parser.add_argument('--remove_random_angle', type=int, default=-1) 367 | parser.add_argument('--validation_sequence', type=str, default='08') 368 | parser.add_argument('--without_ground', action='store_true', default=False, 369 | help='Use preprocessed point clouds with ground plane removed') 370 | args = parser.parse_args() 371 | 372 | main_process(0, args.weights_path, args) 373 | -------------------------------------------------------------------------------- /evaluation/plot_PR_curve.py: -------------------------------------------------------------------------------- 1 | import matplotlib 2 | import matplotlib.pyplot as plt 3 | matplotlib.use("Agg") 4 | 5 | import numpy as np 6 | import pickle 7 | import os 8 | import scipy.io as sio 9 | from sklearn.metrics import precision_recall_curve, auc, average_precision_score 10 | from sklearn.neighbors import KDTree 11 | from tqdm import tqdm 12 | import torch 13 | 14 | from datasets.KITTIDataset import KITTILoader3DPoses 15 | 16 | 17 | def compute_PR(pair_dist, poses, map_tree_poses, is_distance=True, ignore_last=False): 18 | real_loop = [] 19 | detected_loop = [] 20 | distances = [] 21 | last = poses.shape[0] 22 | if ignore_last: 23 | last = last-1 24 | 25 | for i in tqdm(range(100, last)): 26 | min_range = max(0, i-50) 27 | current_pose = poses[i][:3, 3] 28 | indices = map_tree_poses.query_radius(np.expand_dims(current_pose, 0), 4) 29 | valid_idxs = list(set(indices[0]) - set(range(min_range, last))) 30 | if len(valid_idxs) > 0: 31 | real_loop.append(1) 32 | else: 33 | real_loop.append(0) 34 | 35 | if is_distance: 36 | candidate = pair_dist[i, :i-50].argmin() 37 | detected_loop.append(-pair_dist[i, candidate]) 38 | else: 39 | candidate = pair_dist[i, :i-50].argmax() 40 | detected_loop.append(pair_dist[i, candidate]) 41 | candidate_pose = poses[candidate][:3, 3] 42 | distances.append(np.linalg.norm(candidate_pose-current_pose)) 43 | 44 | distances = np.array(distances) 45 | detected_loop = -np.array(detected_loop) 46 | real_loop = np.array(real_loop) 47 | precision_fn = [] 48 | recall_fn = [] 49 | for thr in np.unique(detected_loop): 50 | asd = detected_loop<=thr 51 | asd = asd & real_loop 52 | asd = asd & (distances <= 4) 53 | tp = asd.sum() 54 | fn = (detected_loop<=thr) & (distances > 4) & real_loop 55 | fn2 = (detected_loop > thr) & real_loop 56 | fn = fn.sum() + fn2.sum() 57 | fp = (detected_loop<=thr) & (distances > 4) & (1 - real_loop) 58 | fp = fp.sum() 59 | if (tp+fp) > 0: 60 | precision_fn.append(tp/(tp+fp)) 61 | else: 62 | precision_fn.append(1.) 63 | recall_fn.append(tp/(tp+fn)) 64 | precision_fp = [] 65 | recall_fp = [] 66 | for thr in np.unique(detected_loop): 67 | asd = detected_loop<=thr 68 | asd = asd & real_loop 69 | asd = asd & (distances <= 4) 70 | tp = asd.sum() 71 | fp = (detected_loop<=thr) & (distances > 4) 72 | fp = fp.sum() 73 | fn = (detected_loop > thr) & (real_loop) 74 | fn = fn.sum() 75 | if (tp+fp) > 0: 76 | precision_fp.append(tp/(tp+fp)) 77 | else: 78 | precision_fp.append(1.) 79 | recall_fp.append(tp/(tp+fn)) 80 | 81 | return precision_fn, recall_fn, precision_fp, recall_fp 82 | 83 | 84 | def compute_PR_pairs(pair_dist, poses, is_distance=True, ignore_last=False, positive_range=4, ignore_below=-1): 85 | real_loop = [] 86 | detected_loop = [] 87 | last = poses.shape[0] 88 | if ignore_last: 89 | last = last-1 90 | for i in tqdm(range(100, last)): 91 | current_pose = poses[i][:3, 3] 92 | for j in range(i-50): 93 | candidate_pose = poses[j][:3, 3] 94 | dist_pose = np.linalg.norm(candidate_pose-current_pose) 95 | if dist_pose <= positive_range: 96 | real_loop.append(1) 97 | elif dist_pose <= ignore_below: 98 | continue 99 | else: 100 | real_loop.append(0) 101 | if is_distance: 102 | detected_loop.append(-pair_dist[i, j]) 103 | else: 104 | detected_loop.append(pair_dist[i, j]) 105 | precision, recall, _ = precision_recall_curve(real_loop, detected_loop) 106 | 107 | return precision, recall 108 | 109 | 110 | def compute_AP(precision, recall): 111 | ap = 0. 112 | for i in range(1, len(precision)): 113 | ap += (recall[i] - recall[i-1])*precision[i] 114 | return ap 115 | -------------------------------------------------------------------------------- /imgs/video-preview.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robot-learning-freiburg/LCDNet/1fccda268b235220e646f58d72ffff976338f2d1/imgs/video-preview.png -------------------------------------------------------------------------------- /loss.py: -------------------------------------------------------------------------------- 1 | import torch 2 | import torch.nn as nn 3 | import torch.nn.functional as F 4 | 5 | from pytorch_metric_learning import distances 6 | 7 | 8 | class TripletLoss(nn.Module): 9 | def __init__(self, margin: float, triplet_selector, distance: distances.BaseDistance): 10 | super(TripletLoss, self).__init__() 11 | self.margin = margin 12 | self.triplet_selector = triplet_selector 13 | self.distance = distance 14 | 15 | def forward(self, embeddings, pos_mask, neg_mask, other_embeddings=None): 16 | if other_embeddings is None: 17 | other_embeddings = embeddings 18 | dist_mat = self.distance(embeddings, other_embeddings) 19 | triplets = self.triplet_selector(dist_mat, pos_mask, neg_mask, self.distance.is_inverted) 20 | distance_positive = dist_mat[triplets[0], triplets[1]] 21 | if triplets[-1] is None: 22 | if self.distance.is_inverted: 23 | return F.relu(1 - distance_positive).mean() 24 | else: 25 | return F.relu(distance_positive).mean() 26 | distance_negative = dist_mat[triplets[0], triplets[2]] 27 | curr_margin = self.distance.margin(distance_positive, distance_negative) 28 | loss = F.relu(curr_margin + self.margin) 29 | return loss.mean() 30 | 31 | 32 | def sinkhorn_matches_loss(batch_dict, delta_pose, mode='pairs'): 33 | sinkhorn_matches = batch_dict['sinkhorn_matches'] 34 | src_coords = batch_dict['point_coords'] 35 | src_coords = src_coords.clone().view(batch_dict['batch_size'], -1, 4) 36 | B, N_POINT, _ = src_coords.shape 37 | if mode == 'pairs': 38 | B = B // 2 39 | else: 40 | B = B // 3 41 | src_coords = src_coords[:B, :, [1, 2, 3, 0]] 42 | src_coords[:, :, -1] = 1. 43 | gt_dst_coords = torch.bmm(delta_pose.inverse(), src_coords.permute(0, 2, 1)) 44 | gt_dst_coords = gt_dst_coords.permute(0, 2, 1)[:, :, :3] 45 | loss = (gt_dst_coords - sinkhorn_matches).norm(dim=2).mean() 46 | return loss 47 | 48 | 49 | def pose_loss(batch_dict, delta_pose, mode='pairs'): 50 | src_coords = batch_dict['point_coords'] 51 | src_coords = src_coords.clone().view(batch_dict['batch_size'], -1, 4) 52 | B, N_POINT, _ = src_coords.shape 53 | if mode == 'pairs': 54 | B = B // 2 55 | else: 56 | B = B // 3 57 | src_coords = src_coords[:B, :, [1, 2, 3, 0]] 58 | src_coords[:, :, -1] = 1. 59 | delta_pose_inv = delta_pose.double().inverse() 60 | gt_dst_coords = torch.bmm(delta_pose_inv, src_coords.permute(0,2,1).double()).float() 61 | gt_dst_coords = gt_dst_coords.permute(0, 2, 1)[:, :, :3] 62 | # loss = (gt_dst_coords - sinkhorn_matches).norm(dim=2).mean() 63 | transformation = batch_dict['transformation'] 64 | pred_dst_coords = torch.bmm(transformation, src_coords.permute(0,2,1)) 65 | pred_dst_coords = pred_dst_coords.permute(0, 2, 1)[:, :, :3] 66 | loss = torch.mean(torch.abs(pred_dst_coords - gt_dst_coords)) 67 | # loss = (pred_dst_coords - gt_dst_coords).norm(dim=2).mean() 68 | return loss 69 | -------------------------------------------------------------------------------- /models/backbone3D/MyVoxelSetAbstraction.py: -------------------------------------------------------------------------------- 1 | import torch 2 | import torch.nn as nn 3 | 4 | from pcdet.ops.pointnet2.pointnet2_stack import pointnet2_modules as pointnet2_stack_modules 5 | from pcdet.ops.pointnet2.pointnet2_stack import pointnet2_utils as pointnet2_stack_utils 6 | from pcdet.utils import common_utils 7 | 8 | 9 | def bilinear_interpolate_torch(im, x, y): 10 | """ 11 | Args: 12 | im: (H, W, C) [y, x] 13 | x: (N) 14 | y: (N) 15 | 16 | Returns: 17 | 18 | """ 19 | x0 = torch.floor(x).long() 20 | x1 = x0 + 1 21 | 22 | y0 = torch.floor(y).long() 23 | y1 = y0 + 1 24 | 25 | x0 = torch.clamp(x0, 0, im.shape[1] - 1) 26 | x1 = torch.clamp(x1, 0, im.shape[1] - 1) 27 | y0 = torch.clamp(y0, 0, im.shape[0] - 1) 28 | y1 = torch.clamp(y1, 0, im.shape[0] - 1) 29 | 30 | Ia = im[y0, x0] 31 | Ib = im[y1, x0] 32 | Ic = im[y0, x1] 33 | Id = im[y1, x1] 34 | 35 | wa = (x1.type_as(x) - x) * (y1.type_as(y) - y) 36 | wb = (x1.type_as(x) - x) * (y - y0.type_as(y)) 37 | wc = (x - x0.type_as(x)) * (y1.type_as(y) - y) 38 | wd = (x - x0.type_as(x)) * (y - y0.type_as(y)) 39 | ans = torch.t((torch.t(Ia) * wa)) + torch.t(torch.t(Ib) * wb) + torch.t(torch.t(Ic) * wc) + torch.t(torch.t(Id) * wd) 40 | return ans 41 | 42 | 43 | class MyVoxelSetAbstraction(nn.Module): 44 | """ 45 | Modified from https://github.com/open-mmlab/OpenPCDet 46 | """ 47 | def __init__(self, model_cfg, voxel_size, point_cloud_range, num_bev_features=None, 48 | num_rawpoint_features=None, shared_embeddings=False, **kwargs): 49 | super().__init__() 50 | self.model_cfg = model_cfg 51 | self.voxel_size = voxel_size 52 | self.point_cloud_range = point_cloud_range 53 | self.shared_embeddings = shared_embeddings 54 | 55 | SA_cfg = self.model_cfg.SA_LAYER 56 | 57 | self.SA_layers = nn.ModuleList() 58 | self.SA_layer_names = [] 59 | self.downsample_times_map = {} 60 | c_in = 0 61 | for src_name in self.model_cfg.FEATURES_SOURCE: 62 | if src_name in ['bev', 'raw_points']: 63 | continue 64 | self.downsample_times_map[src_name] = SA_cfg[src_name].DOWNSAMPLE_FACTOR 65 | mlps = SA_cfg[src_name].MLPS 66 | for k in range(len(mlps)): 67 | mlps[k] = [mlps[k][0]] + mlps[k] 68 | cur_layer = pointnet2_stack_modules.StackSAModuleMSG( 69 | radii=SA_cfg[src_name].POOL_RADIUS, 70 | nsamples=SA_cfg[src_name].NSAMPLE, 71 | mlps=mlps, 72 | use_xyz=True, 73 | pool_method='max_pool', 74 | ) 75 | self.SA_layers.append(cur_layer) 76 | self.SA_layer_names.append(src_name) 77 | 78 | c_in += sum([x[-1] for x in mlps]) 79 | 80 | if 'bev' in self.model_cfg.FEATURES_SOURCE: 81 | c_bev = num_bev_features 82 | c_in += c_bev 83 | 84 | if 'raw_points' in self.model_cfg.FEATURES_SOURCE: 85 | mlps = SA_cfg['raw_points'].MLPS 86 | for k in range(len(mlps)): 87 | mlps[k] = [num_rawpoint_features - 3] + mlps[k] 88 | 89 | self.SA_rawpoints = pointnet2_stack_modules.StackSAModuleMSG( 90 | radii=SA_cfg['raw_points'].POOL_RADIUS, 91 | nsamples=SA_cfg['raw_points'].NSAMPLE, 92 | mlps=mlps, 93 | use_xyz=True, 94 | pool_method='max_pool' 95 | ) 96 | c_in += sum([x[-1] for x in mlps]) 97 | 98 | self.vsa_point_feature_fusion = nn.Sequential( 99 | nn.Linear(c_in, self.model_cfg.NUM_OUTPUT_FEATURES, bias=False), 100 | nn.BatchNorm1d(self.model_cfg.NUM_OUTPUT_FEATURES), 101 | nn.ReLU(), 102 | ) 103 | if not shared_embeddings: 104 | self.vsa_point_feature_fusion2 = nn.Sequential( 105 | nn.Linear(c_in, self.model_cfg.NUM_OUTPUT_FEATURES, bias=False), 106 | nn.BatchNorm1d(self.model_cfg.NUM_OUTPUT_FEATURES), 107 | nn.ReLU(), 108 | ) 109 | self.num_point_features = self.model_cfg.NUM_OUTPUT_FEATURES 110 | self.num_point_features_before_fusion = c_in 111 | 112 | def interpolate_from_bev_features(self, keypoints, bev_features, batch_size, bev_stride): 113 | x_idxs = (keypoints[:, :, 0] - self.point_cloud_range[0]) / self.voxel_size[0] 114 | y_idxs = (keypoints[:, :, 1] - self.point_cloud_range[1]) / self.voxel_size[1] 115 | x_idxs = x_idxs / bev_stride 116 | y_idxs = y_idxs / bev_stride 117 | 118 | point_bev_features_list = [] 119 | for k in range(batch_size): 120 | cur_x_idxs = x_idxs[k] 121 | cur_y_idxs = y_idxs[k] 122 | cur_bev_features = bev_features[k].permute(1, 2, 0) # (H, W, C) 123 | point_bev_features = bilinear_interpolate_torch(cur_bev_features, cur_x_idxs, cur_y_idxs) 124 | point_bev_features_list.append(point_bev_features.unsqueeze(dim=0)) 125 | 126 | point_bev_features = torch.cat(point_bev_features_list, dim=0) # (B, N, C0) 127 | return point_bev_features 128 | 129 | def get_sampled_points(self, batch_dict): 130 | batch_size = batch_dict['batch_size'] 131 | if self.model_cfg.POINT_SOURCE == 'raw_points': 132 | src_points = batch_dict['points'][:, 1:4] 133 | batch_indices = batch_dict['points'][:, 0].long() 134 | elif self.model_cfg.POINT_SOURCE == 'voxel_centers': 135 | src_points = common_utils.get_voxel_centers( 136 | batch_dict['voxel_coords'][:, 1:4], 137 | downsample_times=1, 138 | voxel_size=self.voxel_size, 139 | point_cloud_range=self.point_cloud_range 140 | ) 141 | batch_indices = batch_dict['voxel_coords'][:, 0].long() 142 | else: 143 | raise NotImplementedError 144 | keypoints_list = [] 145 | for bs_idx in range(batch_size): 146 | bs_mask = (batch_indices == bs_idx) 147 | sampled_points = src_points[bs_mask].unsqueeze(dim=0) # (1, N, 3) 148 | if self.model_cfg.SAMPLE_METHOD == 'FPS': 149 | cur_pt_idxs = pointnet2_stack_utils.furthest_point_sample( 150 | sampled_points[:, :, 0:3].contiguous(), self.model_cfg.NUM_KEYPOINTS 151 | ).long() 152 | 153 | if sampled_points.shape[1] < self.model_cfg.NUM_KEYPOINTS: 154 | empty_num = self.model_cfg.NUM_KEYPOINTS - sampled_points.shape[1] 155 | cur_pt_idxs[0, -empty_num:] = cur_pt_idxs[0, :empty_num] 156 | 157 | keypoints = sampled_points[0][cur_pt_idxs[0]].unsqueeze(dim=0) 158 | 159 | elif self.model_cfg.SAMPLE_METHOD == 'FastFPS': 160 | raise NotImplementedError 161 | else: 162 | raise NotImplementedError 163 | 164 | keypoints_list.append(keypoints) 165 | 166 | keypoints = torch.cat(keypoints_list, dim=0) # (B, M, 3) 167 | return keypoints 168 | 169 | def forward(self, batch_dict, compute_embeddings=True, compute_rotation=True): 170 | """ 171 | Args: 172 | batch_dict: 173 | batch_size: 174 | keypoints: (B, num_keypoints, 3) 175 | multi_scale_3d_features: { 176 | 'x_conv4': ... 177 | } 178 | points: optional (N, 1 + 3 + C) [bs_idx, x, y, z, ...] 179 | spatial_features: optional 180 | spatial_features_stride: optional 181 | 182 | Returns: 183 | point_features: (N, C) 184 | point_coords: (N, 4) 185 | 186 | """ 187 | keypoints = self.get_sampled_points(batch_dict) 188 | 189 | point_features_list = [] 190 | if 'bev' in self.model_cfg.FEATURES_SOURCE: 191 | point_bev_features = self.interpolate_from_bev_features( 192 | keypoints, batch_dict['spatial_features'], batch_dict['batch_size'], 193 | bev_stride=batch_dict['spatial_features_stride'] 194 | ) 195 | point_features_list.append(point_bev_features) 196 | 197 | batch_size, num_keypoints, _ = keypoints.shape 198 | new_xyz = keypoints.view(-1, 3) 199 | new_xyz_batch_cnt = new_xyz.new_zeros(batch_size).int().fill_(num_keypoints) 200 | 201 | if 'raw_points' in self.model_cfg.FEATURES_SOURCE: 202 | raw_points = batch_dict['points'] 203 | xyz = raw_points[:, 1:4] 204 | xyz_batch_cnt = xyz.new_zeros(batch_size).int() 205 | for bs_idx in range(batch_size): 206 | xyz_batch_cnt[bs_idx] = (raw_points[:, 0] == bs_idx).sum() 207 | point_features = raw_points[:, 4:].contiguous() if raw_points.shape[1] > 4 else None 208 | 209 | pooled_points, pooled_features = self.SA_rawpoints( 210 | xyz=xyz.contiguous(), 211 | xyz_batch_cnt=xyz_batch_cnt, 212 | new_xyz=new_xyz, 213 | new_xyz_batch_cnt=new_xyz_batch_cnt, 214 | features=point_features, 215 | ) 216 | point_features_list.append(pooled_features.view(batch_size, num_keypoints, -1)) 217 | 218 | for k, src_name in enumerate(self.SA_layer_names): 219 | cur_coords = batch_dict['multi_scale_3d_features'][src_name].indices 220 | xyz = common_utils.get_voxel_centers( 221 | cur_coords[:, 1:4], 222 | downsample_times=self.downsample_times_map[src_name], 223 | voxel_size=self.voxel_size, 224 | point_cloud_range=self.point_cloud_range 225 | ) 226 | xyz_batch_cnt = xyz.new_zeros(batch_size).int() 227 | for bs_idx in range(batch_size): 228 | xyz_batch_cnt[bs_idx] = (cur_coords[:, 0] == bs_idx).sum() 229 | 230 | pooled_points, pooled_features = self.SA_layers[k]( 231 | xyz=xyz.contiguous(), 232 | xyz_batch_cnt=xyz_batch_cnt, 233 | new_xyz=new_xyz, 234 | new_xyz_batch_cnt=new_xyz_batch_cnt, 235 | features=batch_dict['multi_scale_3d_features'][src_name].features.contiguous(), 236 | ) 237 | point_features_list.append(pooled_features.view(batch_size, num_keypoints, -1)) 238 | 239 | point_features = torch.cat(point_features_list, dim=2) 240 | 241 | batch_idx = torch.arange(batch_size, device=keypoints.device).view(-1, 1).repeat(1, keypoints.shape[1]).view(-1) 242 | point_coords = torch.cat((batch_idx.view(-1, 1).float(), keypoints.view(-1, 3)), dim=1) 243 | 244 | batch_dict['point_features_before_fusion'] = point_features.view(-1, point_features.shape[-1]) 245 | 246 | if not self.shared_embeddings: 247 | if compute_rotation: 248 | point_features_fusion1 = self.vsa_point_feature_fusion(point_features.view(-1, point_features.shape[-1])) 249 | else: 250 | point_features_fusion1 = None 251 | 252 | if compute_embeddings: 253 | point_features_fusion2 = self.vsa_point_feature_fusion2(point_features.view(-1, point_features.shape[-1])) 254 | else: 255 | point_features_fusion2 = None 256 | 257 | batch_dict['point_features'] = point_features_fusion1 # (BxN, C) 258 | batch_dict['point_features_NV'] = point_features_fusion2 # (BxN, C) 259 | else: 260 | point_features_fusion1 = self.vsa_point_feature_fusion(point_features.view(-1, point_features.shape[-1])) 261 | batch_dict['point_features'] = point_features_fusion1 # (BxN, C) 262 | batch_dict['point_features_NV'] = point_features_fusion1 # (BxN, C) 263 | batch_dict['point_coords'] = point_coords # (BxN, 4) 264 | return batch_dict 265 | -------------------------------------------------------------------------------- /models/backbone3D/NetVlad.py: -------------------------------------------------------------------------------- 1 | from __future__ import print_function 2 | 3 | import math 4 | 5 | import torch 6 | import torch.nn as nn 7 | import torch.nn.functional as F 8 | import torch.nn.parallel 9 | import torch.utils.data 10 | 11 | 12 | class NetVLADLoupe(nn.Module): 13 | """ 14 | Original Tensorflow implementation: https://github.com/antoine77340/LOUPE 15 | """ 16 | def __init__(self, feature_size, cluster_size, output_dim, 17 | gating=True, add_norm=True, is_training=True, normalization='batch'): 18 | super(NetVLADLoupe, self).__init__() 19 | self.feature_size = feature_size 20 | self.output_dim = output_dim 21 | self.is_training = is_training 22 | self.gating = gating 23 | self.add_batch_norm = add_norm 24 | self.cluster_size = cluster_size 25 | if normalization == 'instance': 26 | norm = lambda x: nn.LayerNorm(x) 27 | elif normalization == 'group': 28 | norm = lambda x: nn.GroupNorm(8, x) 29 | else: 30 | norm = lambda x: nn.BatchNorm1d(x) 31 | self.softmax = nn.Softmax(dim=-1) 32 | self.cluster_weights = nn.Parameter(torch.randn(feature_size, cluster_size) * 1 / math.sqrt(feature_size)) 33 | self.cluster_weights2 = nn.Parameter(torch.randn(1, feature_size, cluster_size) * 1 / math.sqrt(feature_size)) 34 | self.hidden1_weights = nn.Parameter( 35 | torch.randn(cluster_size * feature_size, output_dim) * 1 / math.sqrt(feature_size)) 36 | 37 | if add_norm: 38 | self.cluster_biases = None 39 | self.bn1 = norm(cluster_size) 40 | else: 41 | self.cluster_biases = nn.Parameter(torch.randn(cluster_size) * 1 / math.sqrt(feature_size)) 42 | self.bn1 = None 43 | 44 | self.bn2 = norm(output_dim) 45 | 46 | if gating: 47 | self.context_gating = GatingContext(output_dim, add_batch_norm=add_norm, normalization=normalization) 48 | 49 | def forward(self, x): 50 | x = x.transpose(1, 3).contiguous() 51 | batch_size = x.shape[0] 52 | feature_size = x.shape[-1] 53 | x = x.view((batch_size, -1, feature_size)) 54 | max_samples = x.shape[1] 55 | activation = torch.matmul(x, self.cluster_weights) 56 | if self.add_batch_norm: 57 | activation = activation.view(-1, self.cluster_size) 58 | activation = self.bn1(activation) 59 | activation = activation.view(-1, max_samples, self.cluster_size) 60 | else: 61 | activation = activation + self.cluster_biases 62 | activation = self.softmax(activation) 63 | 64 | a_sum = activation.sum(-2, keepdim=True) 65 | a = a_sum * self.cluster_weights2 66 | 67 | activation = torch.transpose(activation, 2, 1) 68 | x = x.view((-1, max_samples, self.feature_size)) 69 | vlad = torch.matmul(activation, x) 70 | vlad = torch.transpose(vlad, 2, 1).contiguous() 71 | vlad0 = vlad - a 72 | 73 | vlad1 = F.normalize(vlad0, dim=1, p=2, eps=1e-6) 74 | vlad2 = vlad1.view((-1, self.cluster_size * self.feature_size)) 75 | vlad = F.normalize(vlad2, dim=1, p=2, eps=1e-6) 76 | 77 | vlad = torch.matmul(vlad, self.hidden1_weights) 78 | 79 | vlad = self.bn2(vlad) 80 | 81 | if self.gating: 82 | vlad = self.context_gating(vlad) 83 | 84 | return vlad 85 | 86 | 87 | class GatingContext(nn.Module): 88 | """ 89 | Original Tensorflow implementation: https://github.com/antoine77340/LOUPE 90 | """ 91 | def __init__(self, dim, add_batch_norm=True, normalization='batch'): 92 | super(GatingContext, self).__init__() 93 | self.dim = dim 94 | self.add_batch_norm = add_batch_norm 95 | if normalization == 'instance': 96 | norm = lambda x: nn.LayerNorm(x) 97 | elif normalization == 'group': 98 | norm = lambda x: nn.GroupNorm(8, x) 99 | else: 100 | norm = lambda x: nn.BatchNorm1d(x) 101 | self.gating_weights = nn.Parameter(torch.randn(dim, dim) * 1 / math.sqrt(dim)) 102 | self.sigmoid = nn.Sigmoid() 103 | 104 | if add_batch_norm: 105 | self.gating_biases = None 106 | self.bn1 = norm(dim) 107 | else: 108 | self.gating_biases = nn.Parameter(torch.randn(dim) * 1 / math.sqrt(dim)) 109 | self.bn1 = None 110 | 111 | def forward(self, x): 112 | gates = torch.matmul(x, self.gating_weights) 113 | 114 | if self.add_batch_norm: 115 | gates = self.bn1(gates) 116 | else: 117 | gates = gates + self.gating_biases 118 | 119 | gates = self.sigmoid(gates) 120 | 121 | activation = x * gates 122 | 123 | return activation 124 | -------------------------------------------------------------------------------- /models/backbone3D/PVRCNN.py: -------------------------------------------------------------------------------- 1 | from functools import partial 2 | 3 | import numpy as np 4 | import torch 5 | import torch.nn as nn 6 | from pcdet.config import cfg_from_yaml_file, cfg 7 | from pcdet.datasets.processor.data_processor import DataProcessor 8 | 9 | from pcdet.models.backbones_2d.map_to_bev import HeightCompression 10 | from pcdet.models.backbones_3d import VoxelBackBone8x 11 | from pcdet.models.backbones_3d.vfe import MeanVFE 12 | 13 | from models.backbone3D.MyVoxelSetAbstraction import MyVoxelSetAbstraction 14 | 15 | 16 | class ReduceInputDimensionality(nn.Module): 17 | def __init__(self, in_dim, out_dim): 18 | super(ReduceInputDimensionality, self).__init__() 19 | self.out_dim = out_dim 20 | self.mlp = nn.Linear(in_dim, out_dim) 21 | self.activation = nn.ReLU() 22 | 23 | def forward(self, batch_dict): 24 | x = batch_dict['voxel_features'] 25 | B = x.shape[0] 26 | coords = x[:, :4] 27 | out = self.activation(self.mlp(x[:, 4:])) 28 | out = torch.cat((coords, out), dim=1) 29 | batch_dict['voxel_features'] = out.view(B, self.out_dim+4) 30 | 31 | x2 = batch_dict['points'] 32 | B = x2.shape[0] 33 | coords2 = x2[:, :5] 34 | out2 = self.activation(self.mlp(x2[:, 5:])) 35 | out2 = torch.cat((coords2, out2), dim=1) 36 | batch_dict['points'] = out2.view(B, self.out_dim+5) 37 | 38 | return batch_dict 39 | 40 | 41 | class PVRCNN(nn.Module): 42 | """ 43 | Modified from https://github.com/open-mmlab/OpenPCDet 44 | """ 45 | def __init__(self, model_cfg, training=True, norm=None, shared_embeddings=False): 46 | super(PVRCNN, self).__init__() 47 | point_cloud_range = np.array(model_cfg.DATA_CONFIG.POINT_CLOUD_RANGE) 48 | voxel_size = model_cfg.DATA_CONFIG.DATA_PROCESSOR[2]['VOXEL_SIZE'] 49 | grid_size = (point_cloud_range[3:6] - point_cloud_range[0:3]) / np.array(voxel_size) 50 | 51 | norm_fn = partial(nn.BatchNorm1d, eps=1e-3, momentum=0.01) 52 | if norm == 'instance': 53 | norm_fn = partial(nn.InstanceNorm2d, affine=True) 54 | if norm == 'group': 55 | norm_fn = partial(nn.GroupNorm, 8) 56 | 57 | raw_in_channel = len(model_cfg.DATA_CONFIG.POINT_FEATURE_ENCODING.src_feature_list) 58 | in_channel = raw_in_channel 59 | self.point_feature_size = model_cfg.MODEL.PFE.NUM_OUTPUT_FEATURES 60 | 61 | self.data_processor = DataProcessor(model_cfg.DATA_CONFIG.DATA_PROCESSOR, point_cloud_range, training, 4) 62 | self.vfe = MeanVFE(model_cfg, in_channel) 63 | 64 | self.reduce_input_dimensionality = False 65 | 66 | self.backbone = VoxelBackBone8x(model_cfg, in_channel, grid_size.astype(np.int64), norm_fn=norm_fn) 67 | self.to_bev = HeightCompression(model_cfg.MODEL.MAP_TO_BEV) 68 | self.vsa = MyVoxelSetAbstraction(model_cfg.MODEL.PFE, voxel_size, 69 | point_cloud_range, 256, in_channel, shared_embeddings) 70 | 71 | def prepare_input(self, point_cloud): 72 | batch_dict = {'points': point_cloud.cpu().numpy(), 'use_lead_xyz': True} 73 | batch_dict = self.data_processor.forward(batch_dict) 74 | return batch_dict 75 | 76 | def forward(self, batch_dict, compute_embeddings=True, compute_rotation=True): 77 | batch_dict = self.vfe(batch_dict) 78 | 79 | if self.reduce_input_dimensionality: 80 | batch_dict = self.reduce_input(batch_dict) 81 | batch_dict = self.backbone(batch_dict) 82 | batch_dict = self.to_bev(batch_dict) 83 | batch_dict = self.vsa(batch_dict, compute_embeddings, compute_rotation) 84 | 85 | if compute_rotation: 86 | out = batch_dict['point_features'].view(batch_dict['batch_size'], -1, self.point_feature_size) 87 | out = out.permute(0, 2, 1).unsqueeze(-1) 88 | batch_dict['point_features'] = out 89 | 90 | if compute_embeddings: 91 | out2 = batch_dict['point_features_NV'].view(batch_dict['batch_size'], -1, self.point_feature_size) 92 | out2 = out2.permute(0, 2, 1).unsqueeze(-1) 93 | batch_dict['point_features_NV'] = out2 94 | 95 | return batch_dict 96 | -------------------------------------------------------------------------------- /models/backbone3D/heads.py: -------------------------------------------------------------------------------- 1 | import torch 2 | import torch.nn as nn 3 | import torch.nn.functional as F 4 | 5 | 6 | class PointNetHead(nn.Module): 7 | 8 | def __init__(self, input_dim, points_num, rotation_parameters=2): 9 | super().__init__() 10 | self.relu = nn.ReLU() 11 | self.FC1 = nn.Linear(input_dim * 2, 1024) 12 | self.FC2 = nn.Linear(1024, 512) 13 | self.FC_transl = nn.Linear(512, 3) 14 | self.FC_rot = nn.Linear(512, rotation_parameters) 15 | 16 | def forward(self, x, compute_transl=True, compute_rotation=True): 17 | x = x.view(x.shape[0], -1) 18 | 19 | x = self.relu(self.FC1(x)) 20 | x = self.relu(self.FC2(x)) 21 | if compute_transl: 22 | transl = self.FC_transl(x) 23 | else: 24 | transl = None 25 | 26 | if compute_rotation: 27 | yaw = self.FC_rot(x) 28 | else: 29 | yaw = None 30 | # print(transl, yaw) 31 | return transl, yaw 32 | 33 | 34 | def compute_rigid_transform(points1, points2, weights): 35 | """Compute rigid transforms between two point clouds via weighted SVD. 36 | Adapted from https://github.com/yewzijian/RPMNet/ 37 | Args: 38 | points1 (torch.Tensor): (B, M, 3) coordinates of the first point cloud 39 | points2 (torch.Tensor): (B, N, 3) coordinates of the second point cloud 40 | weights (torch.Tensor): (B, M) 41 | Returns: 42 | Transform T (B, 3, 4) to get from points1 to points2, i.e. T*points1 = points2 43 | """ 44 | 45 | weights_normalized = weights[..., None] / (torch.sum(weights[..., None], dim=1, keepdim=True) + 1e-5) 46 | centroid_a = torch.sum(points1 * weights_normalized, dim=1) 47 | centroid_b = torch.sum(points2 * weights_normalized, dim=1) 48 | a_centered = points1 - centroid_a[:, None, :] 49 | b_centered = points2 - centroid_b[:, None, :] 50 | cov = a_centered.transpose(-2, -1) @ (b_centered * weights_normalized) 51 | 52 | # Compute rotation using Kabsch algorithm. Will compute two copies with +/-V[:,:3] 53 | # and choose based on determinant to avoid flips 54 | u, s, v = torch.svd(cov, some=False, compute_uv=True) 55 | rot_mat_pos = v @ u.transpose(-1, -2) 56 | v_neg = v.clone() 57 | v_neg[:, :, 2] *= -1 58 | rot_mat_neg = v_neg @ u.transpose(-1, -2) 59 | rot_mat = torch.where(torch.det(rot_mat_pos)[:, None, None] > 0, rot_mat_pos, rot_mat_neg) 60 | assert torch.all(torch.det(rot_mat) > 0) 61 | 62 | # Compute translation (uncenter centroid) 63 | translation = -rot_mat @ centroid_a[:, :, None] + centroid_b[:, :, None] 64 | 65 | transform = torch.cat((rot_mat, translation), dim=2) 66 | return transform 67 | 68 | 69 | def sinkhorn_slack_variables(feature1, feature2, beta, alpha, n_iters = 5, slack = True): 70 | """ Run sinkhorn iterations to generate a near doubly stochastic matrix using slack variables (dustbins) 71 | Adapted from https://github.com/yewzijian/RPMNet/ 72 | Args: 73 | feature1 (torch.Tensor): point-wise features of the first point cloud. 74 | feature2 (torch.Tensor): point-wise features of the second point cloud. 75 | beta (torch.Tensor): annealing parameter. 76 | alpha (torch.Tensor): matching rejection parameter. 77 | n_iters (int): Number of normalization iterations. 78 | slack (bool): Whether to include slack row and column. 79 | Returns: 80 | log(perm_matrix): (B, J, K) Doubly stochastic matrix. 81 | """ 82 | 83 | B, N, _ = feature1.shape 84 | _, M, _ = feature2.shape 85 | 86 | # Feature normalization 87 | feature1 = feature1 / feature1.norm(dim=-1, keepdim=True) 88 | feature2 = feature2 / feature2.norm(dim=-1, keepdim=True) 89 | 90 | dist = -2 * torch.matmul(feature1, feature2.permute(0, 2, 1)) 91 | dist += torch.sum(feature1 ** 2, dim=-1)[:, :, None] 92 | dist += torch.sum(feature2 ** 2, dim=-1)[:, None, :] 93 | 94 | log_alpha = -beta * (dist - alpha) 95 | 96 | # Sinkhorn iterations 97 | if slack: 98 | zero_pad = nn.ZeroPad2d((0, 1, 0, 1)) 99 | log_alpha_padded = zero_pad(log_alpha[:, None, :, :]) 100 | 101 | log_alpha_padded = torch.squeeze(log_alpha_padded, dim=1) 102 | 103 | for i in range(n_iters): 104 | # Row normalization 105 | log_alpha_padded = torch.cat(( 106 | log_alpha_padded[:, :-1, :] - (torch.logsumexp(log_alpha_padded[:, :-1, :], dim=2, keepdim=True)), 107 | log_alpha_padded[:, -1, None, :]), # Don't normalize last row 108 | dim=1) 109 | 110 | # Column normalization 111 | log_alpha_padded = torch.cat(( 112 | log_alpha_padded[:, :, :-1] - (torch.logsumexp(log_alpha_padded[:, :, :-1], dim=1, keepdim=True)), 113 | log_alpha_padded[:, :, -1, None]), # Don't normalize last column 114 | dim=2) 115 | 116 | log_alpha = log_alpha_padded[:, :-1, :-1] 117 | else: 118 | for i in range(n_iters): 119 | # Row normalization 120 | log_alpha = log_alpha - (torch.logsumexp(log_alpha, dim=2, keepdim=True)) 121 | 122 | # Column normalization 123 | log_alpha = log_alpha - (torch.logsumexp(log_alpha, dim=1, keepdim=True)) 124 | 125 | return log_alpha 126 | 127 | 128 | def sinkhorn_unbalanced(feature1, feature2, epsilon, gamma, max_iter): 129 | """ 130 | Sinkhorn algorithm for Unbalanced Optimal Transport. 131 | Modified from https://github.com/valeoai/FLOT/ 132 | Args: 133 | feature1 (torch.Tensor): 134 | (B, N, C) Point-wise features for points cloud 1. 135 | feature2 (torch.Tensor): 136 | (B, M, C) Point-wise features for points cloud 2. 137 | epsilon (torch.Tensor): 138 | Entropic regularization. 139 | gamma (torch.Tensor): 140 | Mass regularization. 141 | max_iter (int): 142 | Number of iteration of the Sinkhorn algorithm. 143 | Returns: 144 | T (torch.Tensor): 145 | (B, N, M) Transport plan between point cloud 1 and 2. 146 | """ 147 | 148 | # Transport cost matrix 149 | feature1 = feature1 / torch.sqrt(torch.sum(feature1 ** 2, -1, keepdim=True) + 1e-8) 150 | feature2 = feature2 / torch.sqrt(torch.sum(feature2 ** 2, -1, keepdim=True) + 1e-8) 151 | C = 1.0 - torch.bmm(feature1, feature2.transpose(1, 2)) 152 | 153 | # Entropic regularisation 154 | K = torch.exp(-C / epsilon) #* support 155 | 156 | # Early return if no iteration 157 | if max_iter == 0: 158 | return K 159 | 160 | # Init. of Sinkhorn algorithm 161 | power = gamma / (gamma + epsilon) 162 | a = ( 163 | torch.ones( 164 | (K.shape[0], K.shape[1], 1), device=feature1.device, dtype=feature1.dtype 165 | ) 166 | / K.shape[1] 167 | ) 168 | prob1 = ( 169 | torch.ones( 170 | (K.shape[0], K.shape[1], 1), device=feature1.device, dtype=feature1.dtype 171 | ) 172 | / K.shape[1] 173 | ) 174 | prob2 = ( 175 | torch.ones( 176 | (K.shape[0], K.shape[2], 1), device=feature2.device, dtype=feature2.dtype 177 | ) 178 | / K.shape[2] 179 | ) 180 | 181 | # Sinkhorn algorithm 182 | for _ in range(max_iter): 183 | # Update b 184 | KTa = torch.bmm(K.transpose(1, 2), a) 185 | b = torch.pow(prob2 / (KTa + 1e-8), power) 186 | # Update a 187 | Kb = torch.bmm(K, b) 188 | a = torch.pow(prob1 / (Kb + 1e-8), power) 189 | 190 | # Transportation map 191 | T = torch.mul(torch.mul(a, K), b.transpose(1, 2)) 192 | 193 | return T 194 | 195 | 196 | class UOTHead(nn.Module): 197 | 198 | def __init__(self, nb_iter=5, use_svd=False, sinkhorn_type='unbalanced'): 199 | super().__init__() 200 | self.epsilon = torch.nn.Parameter(torch.zeros(1)) # Entropic regularisation 201 | self.gamma = torch.nn.Parameter(torch.zeros(1)) # Mass regularisation 202 | self.nb_iter = nb_iter 203 | self.use_svd = use_svd 204 | self.sinkhorn_type = sinkhorn_type 205 | 206 | if not use_svd: 207 | raise NotImplementedError() 208 | 209 | def forward(self, batch_dict, compute_transl=True, compute_rotation=True, src_coords=None, mode='pairs'): 210 | 211 | feats = batch_dict['point_features'].squeeze(-1) 212 | B, C, NUM = feats.shape 213 | 214 | assert B % 2 == 0, "Batch size must be multiple of 2: B anchor + B positive samples" 215 | B = B // 2 216 | feat1 = feats[:B] 217 | feat2 = feats[B:] 218 | 219 | coords = batch_dict['point_coords'].view(2*B, -1, 4) 220 | coords1 = coords[:B, :, 1:] 221 | coords2 = coords[B:, :, 1:] 222 | 223 | if self.sinkhorn_type == 'unbalanced': 224 | transport = sinkhorn_unbalanced( 225 | feat1.permute(0, 2, 1), 226 | feat2.permute(0, 2, 1), 227 | epsilon=torch.exp(self.epsilon) + 0.03, 228 | gamma=torch.exp(self.gamma), 229 | max_iter=self.nb_iter, 230 | ) 231 | else: 232 | transport = sinkhorn_slack_variables( 233 | feat1.permute(0, 2, 1), 234 | feat2.permute(0, 2, 1), 235 | F.softplus(self.epsilon), 236 | F.softplus(self.gamma), 237 | self.nb_iter, 238 | ) 239 | transport = torch.exp(transport) 240 | 241 | row_sum = transport.sum(-1, keepdim=True) 242 | 243 | # Compute the "projected" coordinates of point cloud 1 in 244 | # point cloud 2 based on the optimal transport plan 245 | sinkhorn_matches = (transport @ coords2) / (row_sum + 1e-8) 246 | 247 | batch_dict['sinkhorn_matches'] = sinkhorn_matches 248 | batch_dict['transport'] = transport 249 | 250 | if not self.use_svd: 251 | raise NotImplementedError() 252 | else: 253 | if src_coords is None: 254 | src_coords = coords1 255 | transformation = compute_rigid_transform(src_coords, sinkhorn_matches, row_sum.squeeze(-1)) 256 | batch_dict['transformation'] = transformation 257 | batch_dict['out_rotation'] = None 258 | batch_dict['out_translation'] = None 259 | 260 | return batch_dict 261 | -------------------------------------------------------------------------------- /models/backbone3D/kitti_dataset.yaml: -------------------------------------------------------------------------------- 1 | DATASET: 'KittiDataset' 2 | 3 | POINT_CLOUD_RANGE: [-70.4, -70.4, -3, 70.4, 70.4, 1] 4 | 5 | 6 | POINT_FEATURE_ENCODING: { 7 | encoding_type: absolute_coordinates_encoding, 8 | used_feature_list: ['x', 'y', 'z', 'intensity'], 9 | src_feature_list: ['x', 'y', 'z', 'intensity'], 10 | } 11 | 12 | 13 | DATA_PROCESSOR: 14 | - NAME: mask_points_and_boxes_outside_range 15 | REMOVE_OUTSIDE_BOXES: True 16 | 17 | - NAME: shuffle_points 18 | SHUFFLE_ENABLED: { 19 | 'train': True, 20 | 'test': False 21 | } 22 | 23 | - NAME: transform_points_to_voxels 24 | VOXEL_SIZE: [0.1, 0.1, 0.1] 25 | MAX_POINTS_PER_VOXEL: 5 26 | MAX_NUMBER_OF_VOXELS: { 27 | 'train': 40000, 28 | 'test': 40000 29 | } 30 | -------------------------------------------------------------------------------- /models/backbone3D/models_3d.py: -------------------------------------------------------------------------------- 1 | import torch 2 | import torch.nn as nn 3 | import torch.nn.functional as F 4 | from models.backbone3D.heads import PointNetHead, UOTHead 5 | 6 | 7 | class FCLayer(nn.Module): 8 | def __init__(self, input_dim, output_dim, h_n1=1024, h_n2=512): 9 | super().__init__() 10 | self.FC1 = nn.Linear(input_dim, h_n1) 11 | self.FC2 = nn.Linear(h_n1, h_n2) 12 | self.FC3 = nn.Linear(h_n2, output_dim) 13 | self.dropout = nn.Dropout(0.2, True) 14 | 15 | def forward(self, x): 16 | x = x.view(x.size(0), -1) 17 | x = F.relu(self.FC1(x)) 18 | x = self.dropout(x) 19 | x = F.relu(self.FC2(x)) 20 | x = self.dropout(x) 21 | x = self.FC3(x) 22 | return x 23 | 24 | 25 | class NetVlad(nn.Module): 26 | def __init__(self, backbone, NV, feature_norm=False): 27 | super().__init__() 28 | self.backbone = backbone 29 | self.NV = NV 30 | self.feature_norm = feature_norm 31 | 32 | def forward(self, x): 33 | x = self.backbone(x) 34 | if self.feature_norm: 35 | x = F.normalize(x, p=2, dim=1) 36 | x = self.NV(x) 37 | return x 38 | 39 | 40 | class NetVladCustom(nn.Module): 41 | def __init__(self, backbone, NV, feature_norm=False, fc_input_dim=256, 42 | points_num=4096, head='UOTHead', rotation_parameters=2, 43 | sinkhorn_iter=5, use_svd=True, sinkhorn_type='unbalanced'): 44 | super().__init__() 45 | self.backbone = backbone 46 | self.NV = NV 47 | self.feature_norm = feature_norm 48 | self.head = head 49 | 50 | # PointNetHead 51 | if head == 'PointNet': 52 | self.pose_head = PointNetHead(fc_input_dim, points_num, rotation_parameters) 53 | self.mp1 = torch.nn.MaxPool2d((points_num, 1), 1) 54 | # UOTHead 55 | elif head == 'UOTHead': 56 | self.pose_head = UOTHead(sinkhorn_iter, use_svd, sinkhorn_type) 57 | 58 | def forward(self, batch_dict, metric_head=True, compute_embeddings=True, compute_transl=True, 59 | compute_rotation=True, compute_backbone=True, mode='pairs'): 60 | if compute_backbone: 61 | batch_dict = self.backbone(batch_dict, compute_embeddings, compute_rotation) 62 | 63 | if self.feature_norm: 64 | if compute_rotation: 65 | batch_dict['point_features'] = F.normalize(batch_dict['point_features'], p=2, dim=1) 66 | if compute_embeddings: 67 | batch_dict['point_features_NV'] = F.normalize(batch_dict['point_features_NV'], p=2, dim=1) 68 | 69 | if self.head == 'PointNet++': 70 | batch_dict['point_features'] = batch_dict['point_features'].permute(0, 2, 1, 3) 71 | batch_dict['point_features_NV'] = batch_dict['point_features_NV'].permute(0, 2, 1, 3) 72 | 73 | if compute_embeddings: 74 | embedding = self.NV(batch_dict['point_features_NV']) 75 | 76 | else: 77 | embedding = None 78 | batch_dict['out_embedding'] = embedding 79 | 80 | if metric_head: 81 | # PointNetHead 82 | if self.head == 'PointNet': 83 | B, C, NUM, _ = batch_dict['point_features'].shape 84 | if mode == 'pairs': 85 | assert B % 2 == 0, "Batch size must be multiple of 2: B anchor + B positive samples" 86 | B = B // 2 87 | anchors_feature_maps = batch_dict['point_features'][:B, :, :] 88 | positives_feature_maps = batch_dict['point_features'][B:, :, :] 89 | else: 90 | assert B % 3 == 0, "Batch size must be multiple of 3: B anchor + B positive + B negative samples" 91 | B = B // 3 92 | anchors_feature_maps = batch_dict['point_features'][:B, :, :] 93 | positives_feature_maps = batch_dict['point_features'][B:2*B, :, :] 94 | anchors_feature_maps = self.mp1(anchors_feature_maps) 95 | positives_feature_maps = self.mp1(positives_feature_maps) 96 | pose_head_in = torch.cat((anchors_feature_maps, positives_feature_maps), 1) 97 | transl, yaw = self.pose_head(pose_head_in, compute_transl, compute_rotation) 98 | batch_dict['out_rotation'] = yaw 99 | batch_dict['out_translation'] = transl 100 | # UOTHead 101 | elif self.head == 'UOTHead': 102 | batch_dict = self.pose_head(batch_dict, compute_transl, compute_rotation, mode=mode) 103 | 104 | else: 105 | batch_dict['out_rotation'] = None 106 | batch_dict['out_translation'] = None 107 | 108 | return batch_dict 109 | -------------------------------------------------------------------------------- /models/backbone3D/pv_rcnn.yaml: -------------------------------------------------------------------------------- 1 | CLASS_NAMES: ['Car', 'Pedestrian', 'Cyclist'] 2 | 3 | DATA_CONFIG: 4 | _BASE_CONFIG_: models/backbone3D/kitti_dataset.yaml 5 | DATA_AUGMENTOR: 6 | DISABLE_AUG_LIST: ['placeholder'] 7 | AUG_CONFIG_LIST: 8 | - NAME: gt_sampling 9 | USE_ROAD_PLANE: True 10 | DB_INFO_PATH: 11 | - kitti_dbinfos_train.pkl 12 | PREPARE: { 13 | filter_by_min_points: ['Car:5', 'Pedestrian:5', 'Cyclist:5'], 14 | filter_by_difficulty: [-1], 15 | } 16 | 17 | SAMPLE_GROUPS: ['Car:15','Pedestrian:10', 'Cyclist:10'] 18 | NUM_POINT_FEATURES: 4 19 | DATABASE_WITH_FAKELIDAR: False 20 | REMOVE_EXTRA_WIDTH: [0.0, 0.0, 0.0] 21 | LIMIT_WHOLE_SCENE: False 22 | 23 | - NAME: random_world_flip 24 | ALONG_AXIS_LIST: ['x'] 25 | 26 | - NAME: random_world_rotation 27 | WORLD_ROT_ANGLE: [-0.78539816, 0.78539816] 28 | 29 | - NAME: random_world_scaling 30 | WORLD_SCALE_RANGE: [0.95, 1.05] 31 | 32 | MODEL: 33 | NAME: PVRCNN 34 | 35 | VFE: 36 | NAME: MeanVFE 37 | 38 | BACKBONE_3D: 39 | NAME: VoxelBackBone8x 40 | 41 | MAP_TO_BEV: 42 | NAME: HeightCompression 43 | NUM_BEV_FEATURES: 256 44 | 45 | BACKBONE_2D: 46 | NAME: BaseBEVBackbone 47 | 48 | LAYER_NUMS: [5, 5] 49 | LAYER_STRIDES: [1, 2] 50 | NUM_FILTERS: [128, 256] 51 | UPSAMPLE_STRIDES: [1, 2] 52 | NUM_UPSAMPLE_FILTERS: [256, 256] 53 | 54 | DENSE_HEAD: 55 | NAME: AnchorHeadSingle 56 | CLASS_AGNOSTIC: False 57 | 58 | USE_DIRECTION_CLASSIFIER: True 59 | DIR_OFFSET: 0.78539 60 | DIR_LIMIT_OFFSET: 0.0 61 | NUM_DIR_BINS: 2 62 | 63 | ANCHOR_GENERATOR_CONFIG: [ 64 | { 65 | 'class_name': 'Car', 66 | 'anchor_sizes': [[3.9, 1.6, 1.56]], 67 | 'anchor_rotations': [0, 1.57], 68 | 'anchor_bottom_heights': [-1.78], 69 | 'align_center': False, 70 | 'feature_map_stride': 8, 71 | 'matched_threshold': 0.6, 72 | 'unmatched_threshold': 0.45 73 | }, 74 | { 75 | 'class_name': 'Pedestrian', 76 | 'anchor_sizes': [[0.8, 0.6, 1.73]], 77 | 'anchor_rotations': [0, 1.57], 78 | 'anchor_bottom_heights': [-0.6], 79 | 'align_center': False, 80 | 'feature_map_stride': 8, 81 | 'matched_threshold': 0.5, 82 | 'unmatched_threshold': 0.35 83 | }, 84 | { 85 | 'class_name': 'Cyclist', 86 | 'anchor_sizes': [[1.76, 0.6, 1.73]], 87 | 'anchor_rotations': [0, 1.57], 88 | 'anchor_bottom_heights': [-0.6], 89 | 'align_center': False, 90 | 'feature_map_stride': 8, 91 | 'matched_threshold': 0.5, 92 | 'unmatched_threshold': 0.35 93 | } 94 | ] 95 | 96 | TARGET_ASSIGNER_CONFIG: 97 | NAME: AxisAlignedTargetAssigner 98 | POS_FRACTION: -1.0 99 | SAMPLE_SIZE: 512 100 | NORM_BY_NUM_EXAMPLES: False 101 | MATCH_HEIGHT: False 102 | BOX_CODER: ResidualCoder 103 | 104 | LOSS_CONFIG: 105 | LOSS_WEIGHTS: { 106 | 'cls_weight': 1.0, 107 | 'loc_weight': 2.0, 108 | 'dir_weight': 0.2, 109 | 'code_weights': [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0] 110 | } 111 | 112 | PFE: 113 | NAME: VoxelSetAbstraction 114 | POINT_SOURCE: raw_points 115 | NUM_KEYPOINTS: 2048 116 | NUM_OUTPUT_FEATURES: 640 117 | SAMPLE_METHOD: FPS 118 | 119 | FEATURES_SOURCE: ['bev', 'x_conv1', 'x_conv2', 'x_conv3', 'x_conv4', 'raw_points'] 120 | SA_LAYER: 121 | raw_points: 122 | MLPS: [[16, 16], [16, 16]] 123 | POOL_RADIUS: [0.4, 0.8] 124 | NSAMPLE: [16, 16] 125 | x_conv1: 126 | DOWNSAMPLE_FACTOR: 1 127 | MLPS: [[16, 16], [16, 16]] 128 | POOL_RADIUS: [0.4, 0.8] 129 | NSAMPLE: [16, 16] 130 | x_conv2: 131 | DOWNSAMPLE_FACTOR: 2 132 | MLPS: [[32, 32], [32, 32]] 133 | POOL_RADIUS: [0.8, 1.2] 134 | NSAMPLE: [16, 32] 135 | x_conv3: 136 | DOWNSAMPLE_FACTOR: 4 137 | MLPS: [[64, 64], [64, 64]] 138 | POOL_RADIUS: [1.2, 2.4] 139 | NSAMPLE: [16, 32] 140 | x_conv4: 141 | DOWNSAMPLE_FACTOR: 8 142 | MLPS: [[64, 64], [64, 64]] 143 | POOL_RADIUS: [2.4, 4.8] 144 | NSAMPLE: [16, 32] 145 | 146 | POINT_HEAD: 147 | NAME: PointHeadSimple 148 | CLS_FC: [256, 256] 149 | CLASS_AGNOSTIC: True 150 | USE_POINT_FEATURES_BEFORE_FUSION: True 151 | TARGET_CONFIG: 152 | GT_EXTRA_WIDTH: [0.2, 0.2, 0.2] 153 | LOSS_CONFIG: 154 | LOSS_REG: smooth-l1 155 | LOSS_WEIGHTS: { 156 | 'point_cls_weight': 1.0, 157 | } 158 | 159 | ROI_HEAD: 160 | NAME: PVRCNNHead 161 | CLASS_AGNOSTIC: True 162 | 163 | SHARED_FC: [256, 256] 164 | CLS_FC: [256, 256] 165 | REG_FC: [256, 256] 166 | DP_RATIO: 0.3 167 | 168 | NMS_CONFIG: 169 | TRAIN: 170 | NMS_TYPE: nms_gpu 171 | MULTI_CLASSES_NMS: False 172 | NMS_PRE_MAXSIZE: 9000 173 | NMS_POST_MAXSIZE: 512 174 | NMS_THRESH: 0.8 175 | TEST: 176 | NMS_TYPE: nms_gpu 177 | MULTI_CLASSES_NMS: False 178 | NMS_PRE_MAXSIZE: 1024 179 | NMS_POST_MAXSIZE: 100 180 | NMS_THRESH: 0.7 181 | 182 | ROI_GRID_POOL: 183 | GRID_SIZE: 6 184 | MLPS: [[64, 64], [64, 64]] 185 | POOL_RADIUS: [0.8, 1.6] 186 | NSAMPLE: [16, 16] 187 | POOL_METHOD: max_pool 188 | 189 | TARGET_CONFIG: 190 | BOX_CODER: ResidualCoder 191 | ROI_PER_IMAGE: 128 192 | FG_RATIO: 0.5 193 | 194 | SAMPLE_ROI_BY_EACH_CLASS: True 195 | CLS_SCORE_TYPE: roi_iou 196 | 197 | CLS_FG_THRESH: 0.75 198 | CLS_BG_THRESH: 0.25 199 | CLS_BG_THRESH_LO: 0.1 200 | HARD_BG_RATIO: 0.8 201 | 202 | REG_FG_THRESH: 0.55 203 | 204 | LOSS_CONFIG: 205 | CLS_LOSS: BinaryCrossEntropy 206 | REG_LOSS: smooth-l1 207 | CORNER_LOSS_REGULARIZATION: True 208 | LOSS_WEIGHTS: { 209 | 'rcnn_cls_weight': 1.0, 210 | 'rcnn_reg_weight': 1.0, 211 | 'rcnn_corner_weight': 1.0, 212 | 'code_weights': [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0] 213 | } 214 | 215 | POST_PROCESSING: 216 | RECALL_THRESH_LIST: [0.3, 0.5, 0.7] 217 | SCORE_THRESH: 0.1 218 | OUTPUT_RAW_SCORE: False 219 | 220 | EVAL_METRIC: kitti 221 | 222 | NMS_CONFIG: 223 | MULTI_CLASSES_NMS: False 224 | NMS_TYPE: nms_gpu 225 | NMS_THRESH: 0.1 226 | NMS_PRE_MAXSIZE: 4096 227 | NMS_POST_MAXSIZE: 500 228 | 229 | 230 | OPTIMIZATION: 231 | BATCH_SIZE_PER_GPU: 2 232 | NUM_EPOCHS: 80 233 | 234 | OPTIMIZER: adam_onecycle 235 | LR: 0.01 236 | WEIGHT_DECAY: 0.01 237 | MOMENTUM: 0.9 238 | 239 | MOMS: [0.95, 0.85] 240 | PCT_START: 0.4 241 | DIV_FACTOR: 10 242 | DECAY_STEP_LIST: [35, 45] 243 | LR_DECAY: 0.1 244 | LR_CLIP: 0.0000001 245 | 246 | LR_WARMUP: False 247 | WARMUP_EPOCH: 1 248 | 249 | GRAD_NORM_CLIP: 10 250 | -------------------------------------------------------------------------------- /models/get_models.py: -------------------------------------------------------------------------------- 1 | from pcdet.config import cfg_from_yaml_file 2 | from pcdet.config import cfg as pvrcnn_cfg 3 | 4 | from models.backbone3D.PVRCNN import PVRCNN 5 | from models.backbone3D.NetVlad import NetVLADLoupe 6 | from models.backbone3D.models_3d import NetVladCustom 7 | 8 | 9 | def get_model(exp_cfg, is_training=True): 10 | rotation_parameters = 1 11 | exp_cfg['use_svd'] = True 12 | 13 | if exp_cfg['3D_net'] == 'PVRCNN': 14 | cfg_from_yaml_file('./models/backbone3D/pv_rcnn.yaml', pvrcnn_cfg) 15 | pvrcnn_cfg.MODEL.PFE.NUM_KEYPOINTS = exp_cfg['num_points'] 16 | if 'PC_RANGE' in exp_cfg: 17 | pvrcnn_cfg.DATA_CONFIG.POINT_CLOUD_RANGE = exp_cfg['PC_RANGE'] 18 | pvrcnn = PVRCNN(pvrcnn_cfg, is_training, exp_cfg['model_norm'], exp_cfg['shared_embeddings']) 19 | net_vlad = NetVLADLoupe(feature_size=pvrcnn_cfg.MODEL.PFE.NUM_OUTPUT_FEATURES, 20 | cluster_size=exp_cfg['cluster_size'], 21 | output_dim=exp_cfg['feature_output_dim_3D'], 22 | gating=True, add_norm=True, is_training=is_training) 23 | model = NetVladCustom(pvrcnn, net_vlad, feature_norm=False, fc_input_dim=640, 24 | points_num=exp_cfg['num_points'], head=exp_cfg['head'], 25 | rotation_parameters=rotation_parameters, sinkhorn_iter=exp_cfg['sinkhorn_iter'], 26 | use_svd=exp_cfg['use_svd'], sinkhorn_type=exp_cfg['sinkhorn_type']) 27 | else: 28 | raise TypeError("Unknown 3D network") 29 | return model 30 | -------------------------------------------------------------------------------- /requirements.txt: -------------------------------------------------------------------------------- 1 | h5py 2 | open3d>=0.12.0,<=0.14.1 3 | pykitti 4 | opencv-python 5 | pytorch_metric_learning 6 | cython 7 | wandb 8 | pyquaternion 9 | easydict 10 | numba 11 | -------------------------------------------------------------------------------- /training_KITTI_DDP.py: -------------------------------------------------------------------------------- 1 | import argparse 2 | import os 3 | import time 4 | from functools import partial 5 | from shutil import copy2 6 | 7 | import yaml 8 | import wandb 9 | import numpy as np 10 | import torch 11 | import torch.nn.parallel 12 | import torch.optim as optim 13 | import torch.utils.data 14 | from pcdet.datasets.kitti.kitti_dataset import KittiDataset 15 | import random 16 | from torch.nn.parallel import DistributedDataParallel 17 | 18 | from datasets.KITTI360Dataset import KITTI3603DDictPairs, KITTI3603DPoses 19 | from datasets.KITTIDataset import KITTILoader3DPoses, KITTILoader3DDictPairs 20 | from loss import TripletLoss, sinkhorn_matches_loss, pose_loss 21 | 22 | from models.get_models import get_model 23 | from triple_selector import hardest_negative_selector, random_negative_selector, \ 24 | semihard_negative_selector 25 | from utils.data import datasets_concat_kitti, merge_inputs, datasets_concat_kitti360 26 | from evaluate_kitti import evaluate_model_with_emb 27 | from datetime import datetime 28 | 29 | from utils.geometry import get_rt_matrix, mat2xyzrpy 30 | from utils.tools import _pairwise_distance 31 | from pytorch_metric_learning import distances 32 | 33 | import torch.distributed as dist 34 | import torch.multiprocessing as mp 35 | 36 | torch.backends.cudnn.benchmark = True 37 | 38 | EPOCH = 1 39 | 40 | 41 | def _init_fn(worker_id, epoch=0, seed=0): 42 | seed = seed + worker_id + epoch * 100 43 | seed = seed % (2**32 - 1) 44 | print(f"Init worker {worker_id} with seed {seed}") 45 | torch.manual_seed(seed) 46 | np.random.seed(seed) 47 | random.seed(seed) 48 | 49 | 50 | def train(model, optimizer, sample, loss_fn, exp_cfg, device, mode='pairs'): 51 | if True: 52 | model.train() 53 | 54 | optimizer.zero_grad() 55 | 56 | if 'sequence' in sample: 57 | neg_mask = sample['sequence'].view(1,-1) != sample['sequence'].view(-1, 1) 58 | else: 59 | neg_mask = torch.zeros((sample['anchor_pose'].shape[0], sample['anchor_pose'].shape[0]), 60 | dtype=torch.bool) 61 | 62 | pair_dist = _pairwise_distance(sample['anchor_pose']) 63 | neg_mask = ((pair_dist > exp_cfg['negative_distance']) | neg_mask) 64 | neg_mask = neg_mask.repeat(2, 2).to(device) 65 | 66 | anchor_transl = sample['anchor_pose'].to(device) 67 | positive_transl = sample['positive_pose'].to(device) 68 | anchor_rot = sample['anchor_rot'].to(device) 69 | positive_rot = sample['positive_rot'].to(device) 70 | 71 | anchor_list = [] 72 | positive_list = [] 73 | 74 | delta_pose = [] 75 | for i in range(anchor_transl.shape[0]): 76 | anchor = sample['anchor'][i].to(device) 77 | positive = sample['positive'][i].to(device) 78 | 79 | anchor_i = anchor 80 | positive_i = positive 81 | anchor_transl_i = anchor_transl[i] 82 | anchor_rot_i = anchor_rot[i] 83 | positive_transl_i = positive_transl[i] 84 | positive_rot_i = positive_rot[i] 85 | 86 | anchor_i_reflectance = anchor_i[:, 3].clone() 87 | positive_i_reflectance = positive_i[:, 3].clone() 88 | anchor_i[:, 3] = 1. 89 | positive_i[:, 3] = 1. 90 | 91 | rt_anchor = get_rt_matrix(anchor_transl_i, anchor_rot_i, rot_parmas='xyz') 92 | rt_positive = get_rt_matrix(positive_transl_i, positive_rot_i, rot_parmas='xyz') 93 | 94 | if exp_cfg['point_cloud_augmentation']: 95 | 96 | rotz = np.random.rand() * 360 - 180 97 | rotz = rotz * (np.pi / 180.0) 98 | 99 | roty = (np.random.rand() * 6 - 3) * (np.pi / 180.0) 100 | rotx = (np.random.rand() * 6 - 3) * (np.pi / 180.0) 101 | 102 | T = torch.rand(3)*3. - 1.5 103 | T[-1] = torch.rand(1)*0.5 - 0.25 104 | T = T.to(device) 105 | 106 | rt_anch_augm = get_rt_matrix(T, torch.tensor([rotx, roty, rotz]).to(device)) 107 | anchor_i = rt_anch_augm.inverse() @ anchor_i.T 108 | anchor_i = anchor_i.T 109 | anchor_i[:, 3] = anchor_i_reflectance.clone() 110 | 111 | rotz = np.random.rand() * 360 - 180 112 | rotz = rotz * (3.141592 / 180.0) 113 | 114 | roty = (np.random.rand() * 6 - 3) * (np.pi / 180.0) 115 | rotx = (np.random.rand() * 6 - 3) * (np.pi / 180.0) 116 | 117 | T = torch.rand(3)*3.-1.5 118 | T[-1] = torch.rand(1)*0.5 - 0.25 119 | T = T.to(device) 120 | 121 | rt_pos_augm = get_rt_matrix(T, torch.tensor([rotx, roty, rotz]).to(device)) 122 | positive_i = rt_pos_augm.inverse() @ positive_i.T 123 | positive_i = positive_i.T 124 | positive_i[:, 3] = positive_i_reflectance.clone() 125 | 126 | rt_anch_concat = rt_anchor @ rt_anch_augm 127 | rt_pos_concat = rt_positive @ rt_pos_augm 128 | 129 | rt_anchor2positive = rt_anch_concat.inverse() @ rt_pos_concat 130 | ext = mat2xyzrpy(rt_anchor2positive) 131 | 132 | else: 133 | raise NotImplementedError() 134 | 135 | anchor_list.append(model.module.backbone.prepare_input(anchor_i)) 136 | positive_list.append(model.module.backbone.prepare_input(positive_i)) 137 | del anchor_i, positive_i 138 | delta_pose.append(rt_anchor2positive.unsqueeze(0)) 139 | 140 | delta_pose = torch.cat(delta_pose) 141 | 142 | model_in = KittiDataset.collate_batch(anchor_list + positive_list) 143 | for key, val in model_in.items(): 144 | if not isinstance(val, np.ndarray): 145 | continue 146 | model_in[key] = torch.from_numpy(val).float().to(device) 147 | 148 | metric_head = True 149 | compute_embeddings = True 150 | compute_transl = True 151 | compute_rotation = True 152 | batch_dict = model(model_in, metric_head, compute_embeddings, 153 | compute_transl, compute_rotation, mode=mode) 154 | 155 | model_out = batch_dict['out_embedding'] 156 | 157 | # Translation loss 158 | total_loss = 0. 159 | 160 | loss_transl = torch.tensor([0.], device=device) 161 | 162 | if exp_cfg['weight_rot'] > 0.: 163 | if exp_cfg['sinkhorn_aux_loss']: 164 | aux_loss = sinkhorn_matches_loss(batch_dict, delta_pose, mode=mode) 165 | else: 166 | aux_loss = torch.tensor([0.], device=device) 167 | loss_rot = pose_loss(batch_dict, delta_pose, mode=mode) 168 | if exp_cfg['sinkhorn_type'] == 'rpm': 169 | inlier_loss = (1 - batch_dict['transport'].sum(dim=1)).mean() 170 | inlier_loss += (1 - batch_dict['transport'].sum(dim=2)).mean() 171 | loss_rot += 0.01 * inlier_loss 172 | 173 | total_loss = total_loss + exp_cfg['weight_rot']*(loss_rot + 0.05*aux_loss) 174 | else: 175 | loss_rot = torch.tensor([0.], device=device) 176 | 177 | if exp_cfg['weight_metric_learning'] > 0.: 178 | if exp_cfg['norm_embeddings']: 179 | model_out = model_out / model_out.norm(dim=1, keepdim=True) 180 | 181 | pos_mask = torch.zeros((model_out.shape[0], model_out.shape[0]), device=device) 182 | 183 | batch_size = (model_out.shape[0]//2) 184 | for i in range(batch_size): 185 | pos_mask[i, i+batch_size] = 1 186 | pos_mask[i+batch_size, i] = 1 187 | 188 | loss_metric_learning = loss_fn(model_out, pos_mask, neg_mask) * exp_cfg['weight_metric_learning'] 189 | total_loss = total_loss + loss_metric_learning 190 | 191 | total_loss.backward() 192 | optimizer.step() 193 | 194 | return total_loss, loss_rot, loss_transl 195 | 196 | 197 | def test(model, sample, exp_cfg, device): 198 | model.eval() 199 | 200 | with torch.no_grad(): 201 | anchor_transl = sample['anchor_pose'].to(device) 202 | positive_transl = sample['positive_pose'].to(device) 203 | anchor_rot = sample['anchor_rot'].to(device) 204 | positive_rot = sample['positive_rot'].to(device) 205 | 206 | anchor_list = [] 207 | positive_list = [] 208 | delta_transl_list = [] 209 | delta_rot_list = [] 210 | delta_pose_list = [] 211 | for i in range(anchor_transl.shape[0]): 212 | anchor = sample['anchor'][i].to(device) 213 | positive = sample['positive'][i].to(device) 214 | 215 | anchor_i = anchor 216 | positive_i = positive 217 | 218 | anchor_transl_i = anchor_transl[i] 219 | anchor_rot_i = anchor_rot[i] 220 | positive_transl_i = positive_transl[i] 221 | positive_rot_i = positive_rot[i] 222 | 223 | rt_anchor = get_rt_matrix(anchor_transl_i, anchor_rot_i, rot_parmas='xyz') 224 | rt_positive = get_rt_matrix(positive_transl_i, positive_rot_i, rot_parmas='xyz') 225 | rt_anchor2positive = rt_anchor.inverse() @ rt_positive 226 | ext = mat2xyzrpy(rt_anchor2positive) 227 | delta_transl_i = ext[0:3] 228 | delta_rot_i = ext[3:] 229 | delta_transl_list.append(delta_transl_i.unsqueeze(0)) 230 | delta_rot_list.append(delta_rot_i.unsqueeze(0)) 231 | delta_pose_list.append(rt_anchor2positive.unsqueeze(0)) 232 | 233 | anchor_list.append(model.module.backbone.prepare_input(anchor_i)) 234 | positive_list.append(model.module.backbone.prepare_input(positive_i)) 235 | del anchor_i, positive_i 236 | 237 | delta_rot = torch.cat(delta_rot_list) 238 | delta_pose_list = torch.cat(delta_pose_list) 239 | 240 | model_in = KittiDataset.collate_batch(anchor_list + positive_list) 241 | for key, val in model_in.items(): 242 | if not isinstance(val, np.ndarray): 243 | continue 244 | model_in[key] = torch.from_numpy(val).float().to(device) 245 | 246 | batch_dict = model(model_in, metric_head=True) 247 | anchor_out = batch_dict['out_embedding'] 248 | 249 | diff_yaws = delta_rot[:, 2] % (2*np.pi) 250 | 251 | transformation = batch_dict['transformation'] 252 | homogeneous = torch.tensor([0., 0., 0., 1.]).repeat(transformation.shape[0], 1, 1).to(transformation.device) 253 | transformation = torch.cat((transformation, homogeneous), dim=1) 254 | transformation = transformation.inverse() 255 | final_yaws = torch.zeros(transformation.shape[0], device=transformation.device, 256 | dtype=transformation.dtype) 257 | for i in range(transformation.shape[0]): 258 | final_yaws[i] = mat2xyzrpy(transformation[i])[-1] 259 | yaw = final_yaws 260 | transl_comps_error = (transformation[:,:3,3] - delta_pose_list[:,:3,3]).norm(dim=1).mean() 261 | 262 | yaw = yaw % (2*np.pi) 263 | yaw_error_deg = torch.abs(diff_yaws - yaw) 264 | yaw_error_deg[yaw_error_deg>np.pi] = 2*np.pi - yaw_error_deg[yaw_error_deg>np.pi] 265 | yaw_error_deg = yaw_error_deg.mean() * 180 / np.pi 266 | 267 | if exp_cfg['norm_embeddings']: 268 | anchor_out = anchor_out / anchor_out.norm(dim=1, keepdim=True) 269 | anchor_out = anchor_out[:anchor_transl.shape[0]] 270 | return anchor_out, transl_comps_error, yaw_error_deg 271 | 272 | 273 | def get_database_embs(model, sample, exp_cfg, device): 274 | model.eval() 275 | 276 | with torch.no_grad(): 277 | anchor_list = [] 278 | for i in range(len(sample['anchor'])): 279 | anchor = sample['anchor'][i].to(device) 280 | 281 | anchor_i = anchor 282 | anchor_list.append(model.module.backbone.prepare_input(anchor_i)) 283 | del anchor_i 284 | 285 | model_in = KittiDataset.collate_batch(anchor_list) 286 | for key, val in model_in.items(): 287 | if not isinstance(val, np.ndarray): 288 | continue 289 | model_in[key] = torch.from_numpy(val).float().to(device) 290 | 291 | batch_dict = model(model_in, metric_head=False) 292 | anchor_out = batch_dict['out_embedding'] 293 | 294 | if exp_cfg['norm_embeddings']: 295 | anchor_out = anchor_out / anchor_out.norm(dim=1, keepdim=True) 296 | return anchor_out 297 | 298 | 299 | def main_process(gpu, exp_cfg, common_seed, world_size, args): 300 | global EPOCH 301 | rank = gpu 302 | 303 | dist.init_process_group( 304 | backend='nccl', 305 | init_method='env://', 306 | world_size=world_size, 307 | rank=rank 308 | ) 309 | local_seed = (common_seed + common_seed ** gpu) ** 2 310 | local_seed = local_seed % (2**32 - 1) 311 | np.random.seed(common_seed) 312 | torch.random.manual_seed(common_seed) 313 | torch.cuda.set_device(gpu) 314 | device = torch.device(gpu) 315 | print(f"Process {rank}, seed {common_seed}") 316 | 317 | current_date = datetime.now() 318 | dt_string = current_date.strftime("%d/%m/%Y %H:%M:%S") 319 | dt_string_folder = current_date.strftime("%d-%m-%Y_%H-%M-%S") 320 | 321 | exp_cfg['effective_batch_size'] = args.batch_size * world_size 322 | if args.wandb and rank == 0: 323 | wandb.init(project="deep_lcd", name=dt_string, config=exp_cfg) 324 | 325 | if args.dataset == 'kitti': 326 | sequences_training = ["05", "06", "07", "09"] 327 | sequences_validation = ["00", "08"] 328 | elif args.dataset == 'kitti360': 329 | sequences_training = ["2013_05_28_drive_0000_sync", "2013_05_28_drive_0004_sync", 330 | "2013_05_28_drive_0005_sync", "2013_05_28_drive_0006_sync"] 331 | sequences_validation = ["2013_05_28_drive_0002_sync", "2013_05_28_drive_0009_sync"] 332 | else: 333 | raise TypeError("Dataset should be either 'kitti' or 'kitti360'") 334 | 335 | data_transform = None 336 | # sequences_training = ["09"] 337 | # sequences_validation = ["09"] 338 | 339 | if args.dataset == 'kitti': 340 | training_dataset, dataset_list_train = datasets_concat_kitti(args.root_folder, 341 | sequences_training, 342 | data_transform, 343 | "3D", 344 | loop_file=exp_cfg['loop_file'], 345 | jitter=exp_cfg['point_cloud_jitter'], 346 | without_ground=args.without_ground) 347 | validation_dataset = KITTILoader3DDictPairs(args.root_folder, sequences_validation[0], 348 | os.path.join(args.root_folder, 'sequences', sequences_validation[0], 'poses.txt'), 349 | loop_file=exp_cfg['loop_file'], without_ground=args.without_ground) 350 | dataset_for_recall = KITTILoader3DPoses(args.root_folder, sequences_validation[0], 351 | os.path.join(args.root_folder, 'sequences', sequences_validation[0], 'poses.txt'), 352 | train=False, loop_file=exp_cfg['loop_file'], without_ground=args.without_ground) 353 | elif args.dataset == 'kitti360': 354 | training_dataset, dataset_list_train = datasets_concat_kitti360(args.root_folder, 355 | sequences_training, 356 | data_transform, 357 | "3D", 358 | loop_file=exp_cfg['loop_file'], 359 | jitter=exp_cfg['point_cloud_jitter'], 360 | without_ground=args.without_ground) 361 | validation_dataset = KITTI3603DDictPairs(args.root_folder, sequences_validation[0], 362 | loop_file=exp_cfg['loop_file'], without_ground=args.without_ground) 363 | dataset_for_recall = KITTI3603DPoses(args.root_folder, sequences_validation[0], 364 | train=False, loop_file=exp_cfg['loop_file'], without_ground=args.without_ground) 365 | 366 | dataset_list_valid = [dataset_for_recall] 367 | 368 | train_indices = list(range(len(training_dataset))) 369 | np.random.shuffle(train_indices) 370 | train_sampler = torch.utils.data.distributed.DistributedSampler( 371 | training_dataset, 372 | num_replicas=world_size, 373 | rank=rank, 374 | seed=common_seed 375 | ) 376 | val_sampler = torch.utils.data.distributed.DistributedSampler( 377 | validation_dataset, 378 | num_replicas=world_size, 379 | rank=rank, 380 | seed=common_seed, 381 | ) 382 | recall_sampler = torch.utils.data.distributed.DistributedSampler( 383 | dataset_for_recall, 384 | num_replicas=world_size, 385 | rank=rank, 386 | seed=common_seed, 387 | shuffle=False 388 | ) 389 | 390 | if exp_cfg['loss_type'].startswith('triplet'): 391 | neg_selector = random_negative_selector 392 | if 'hardest' in exp_cfg['loss_type']: 393 | neg_selector = hardest_negative_selector 394 | if 'semihard' in exp_cfg['loss_type']: 395 | neg_selector = semihard_negative_selector 396 | loss_fn = TripletLoss(exp_cfg['margin'], neg_selector, distances.LpDistance()) 397 | else: 398 | raise NotImplementedError(f"Loss {exp_cfg['loss_type']} not implemented") 399 | 400 | positive_distance = 4. 401 | negative_distance = 10. 402 | 403 | if rank == 0: 404 | print("Positive distance: ", positive_distance) 405 | exp_cfg['negative_distance'] = negative_distance 406 | 407 | final_dest = '' 408 | if rank == 0: 409 | if not os.path.exists(args.checkpoints_dest): 410 | try: 411 | os.mkdir(args.checkpoints_dest) 412 | except: 413 | raise TypeError('Folder for saving checkpoints does not exist!') 414 | final_dest = args.checkpoints_dest + '/' + dt_string_folder 415 | os.mkdir(final_dest) 416 | if args.wandb: 417 | wandb.save(f'{final_dest}/best_model_so_far.tar') 418 | copy2('wandb_config.yaml', f'{final_dest}/wandb_config.yaml') 419 | wandb.save(f'{final_dest}/wandb_config.yaml') 420 | 421 | model = get_model(exp_cfg) 422 | if args.weights is not None: 423 | print('Loading pre-trained params...') 424 | saved_params = torch.load(args.weights, map_location='cpu') 425 | model.load_state_dict(saved_params['state_dict']) 426 | 427 | model = torch.nn.SyncBatchNorm.convert_sync_batchnorm(model) 428 | model.train() 429 | model = DistributedDataParallel(model.to(device), device_ids=[rank], output_device=rank) 430 | 431 | start_full_time = time.time() 432 | 433 | parameters = filter(lambda p: p.requires_grad, model.parameters()) 434 | optimizer = optim.Adam(parameters, lr=exp_cfg['learning_rate'], betas=(exp_cfg['beta1'], exp_cfg['beta2']), 435 | eps=exp_cfg['eps'], weight_decay=exp_cfg['weight_decay'], amsgrad=False) 436 | 437 | starting_epoch = 1 438 | scheduler_epoch = -1 439 | if args.resume: 440 | optimizer.load_state_dict(saved_params['optimizer']) 441 | starting_epoch = saved_params['epoch'] 442 | scheduler_epoch = saved_params['epoch'] 443 | 444 | if exp_cfg['scheduler'] == 'multistep': 445 | scheduler = torch.optim.lr_scheduler.MultiStepLR(optimizer, milestones=[40, 80], gamma=0.5, 446 | last_epoch=scheduler_epoch) 447 | else: 448 | raise RuntimeError("Unknown Scheduler") 449 | 450 | best_rot_error = 1000 451 | max_recall = 0. 452 | max_auc = 0. 453 | old_saved_file = None 454 | old_saved_file_recall = None 455 | old_saved_file_auc = None 456 | 457 | np.random.seed(local_seed) 458 | torch.random.manual_seed(local_seed) 459 | 460 | for epoch in range(starting_epoch, exp_cfg['epochs'] + 1): 461 | dist.barrier() 462 | 463 | train_sampler.set_epoch(epoch) 464 | val_sampler.set_epoch(epoch) 465 | recall_sampler.set_epoch(epoch) 466 | EPOCH = epoch 467 | 468 | init_fn = partial(_init_fn, epoch=epoch, seed=local_seed) 469 | TrainLoader = torch.utils.data.DataLoader(dataset=training_dataset, 470 | sampler=train_sampler, 471 | batch_size=args.batch_size, 472 | num_workers=2, 473 | worker_init_fn=init_fn, 474 | collate_fn=merge_inputs, 475 | pin_memory=True, 476 | drop_last=True) 477 | 478 | TestLoader = torch.utils.data.DataLoader(dataset=validation_dataset, 479 | sampler=val_sampler, 480 | batch_size=args.batch_size, 481 | num_workers=2, 482 | worker_init_fn=init_fn, 483 | collate_fn=merge_inputs, 484 | pin_memory=True) 485 | 486 | RecallLoader = torch.utils.data.DataLoader(dataset=dataset_for_recall, 487 | sampler=recall_sampler, 488 | batch_size=args.batch_size, 489 | num_workers=2, 490 | worker_init_fn=init_fn, 491 | collate_fn=merge_inputs, 492 | pin_memory=True) 493 | 494 | if epoch > starting_epoch: 495 | if exp_cfg['scheduler'] == 'multistep': 496 | scheduler.step() 497 | if args.wandb and rank == 0: 498 | wandb.log({"LR": optimizer.param_groups[0]['lr']}, commit=False) 499 | if rank == 0: 500 | print('This is %d-th epoch' % epoch) 501 | epoch_start_time = time.time() 502 | total_train_loss = 0 503 | total_rot_loss = 0. 504 | total_transl_loss = 0. 505 | local_loss = 0. 506 | local_iter = 0 507 | total_iter = 0 508 | 509 | ## Training ## 510 | for batch_idx, sample in enumerate(TrainLoader): 511 | start_time = time.time() 512 | loss, loss_rot, loss_transl = train(model, optimizer, sample, loss_fn, exp_cfg, 513 | device, mode='pairs') 514 | 515 | if exp_cfg['scheduler'] == 'onecycle': 516 | scheduler.step() 517 | 518 | dist.barrier() 519 | dist.reduce(loss, 0) 520 | dist.reduce(loss_rot, 0) 521 | dist.reduce(loss_transl, 0) 522 | if rank == 0: 523 | loss = (loss / world_size).item() 524 | loss_rot = (loss_rot / world_size).item() 525 | loss_transl = (loss_transl / world_size).item() 526 | local_loss += loss 527 | local_iter += 1 528 | 529 | if batch_idx % 20 == 0 and batch_idx != 0: 530 | print('Iter %d / %d training loss = %.3f , time = %.2f' % (batch_idx, 531 | len(TrainLoader), 532 | local_loss / local_iter, 533 | time.time() - start_time)) 534 | local_loss = 0. 535 | local_iter = 0. 536 | 537 | total_train_loss += loss * sample['anchor_pose'].shape[0] 538 | total_rot_loss += loss_rot * sample['anchor_pose'].shape[0] 539 | total_transl_loss += loss_transl * sample['anchor_pose'].shape[0] 540 | 541 | total_iter += sample['anchor_pose'].shape[0] 542 | 543 | if rank == 0: 544 | print("------------------------------------") 545 | print('epoch %d total training loss = %.3f' % (epoch, total_train_loss / len(train_sampler))) 546 | print('Total epoch time = %.2f' % (time.time() - epoch_start_time)) 547 | print("------------------------------------") 548 | 549 | local_iter = 0. 550 | transl_error_sum = 0 551 | yaw_error_sum = 0 552 | emb_list = [] 553 | 554 | # Testing 555 | if exp_cfg['weight_rot'] > 0: 556 | for batch_idx, sample in enumerate(TestLoader): 557 | start_time = time.time() 558 | _, transl_error, yaw_error = test(model, sample, exp_cfg, device) 559 | dist.barrier() 560 | dist.reduce(transl_error, 0) 561 | dist.reduce(yaw_error, 0) 562 | if rank == 0: 563 | transl_error = (transl_error / world_size).item() 564 | yaw_error = (yaw_error / world_size).item() 565 | transl_error_sum += transl_error 566 | yaw_error_sum += yaw_error 567 | local_iter += 1 568 | 569 | if batch_idx % 20 == 0 and batch_idx != 0: 570 | print('Iter %d time = %.2f' % (batch_idx, 571 | time.time() - start_time)) 572 | local_iter = 0. 573 | 574 | if exp_cfg['weight_metric_learning'] > 0.: 575 | for batch_idx, sample in enumerate(RecallLoader): 576 | emb = get_database_embs(model, sample, exp_cfg, device) 577 | dist.barrier() 578 | out_emb = [torch.zeros_like(emb) for _ in range(world_size)] 579 | dist.all_gather(out_emb, emb) 580 | 581 | if rank == 0: 582 | interleaved_out = torch.empty((emb.shape[0]*world_size, emb.shape[1]), 583 | device=emb.device, dtype=emb.dtype) 584 | for current_rank in range(world_size): 585 | interleaved_out[current_rank::world_size] = out_emb[current_rank] 586 | emb_list.append(interleaved_out.detach().clone()) 587 | 588 | if rank == 0: 589 | if exp_cfg['weight_metric_learning'] > 0.: 590 | emb_list = torch.cat(emb_list) 591 | emb_list = emb_list[:len(dataset_for_recall)] 592 | recall, maxF1, auc, auc2 = evaluate_model_with_emb(emb_list, dataset_list_valid, positive_distance) 593 | final_transl_error = transl_error_sum / len(TestLoader) 594 | final_yaw_error = yaw_error_sum / len(TestLoader) 595 | 596 | if args.wandb: 597 | if exp_cfg['weight_rot'] > 0.: 598 | wandb.log({"Rotation Loss": (total_rot_loss / len(train_sampler)), 599 | "Rotation Mean Error": final_yaw_error}, commit=False) 600 | wandb.log({"Translation Loss": (total_transl_loss / len(train_sampler)), 601 | "Translation Error": final_transl_error}, commit=False) 602 | if exp_cfg['weight_metric_learning'] > 0.: 603 | wandb.log({"Validation Recall @ 1": recall[0], 604 | "Validation Recall @ 5": recall[4], 605 | "Validation Recall @ 10": recall[9], 606 | "Max F1": maxF1, 607 | "AUC": auc2}, commit=False) 608 | wandb.log({"Training Loss": (total_train_loss / len(train_sampler))}) 609 | 610 | print("------------------------------------") 611 | if exp_cfg['weight_metric_learning'] > 0.: 612 | print("Recall@k:") 613 | print(recall) 614 | print("Max F1: ", maxF1) 615 | print("AUC: ", auc2) 616 | print("Translation Error: ", final_transl_error) 617 | print("Rotation Error: ", final_yaw_error) 618 | print("------------------------------------") 619 | 620 | if final_yaw_error < best_rot_error: 621 | best_rot_error = final_yaw_error 622 | 623 | savefilename = f'{final_dest}/checkpoint_{epoch}_rot_{final_yaw_error:.3f}.tar' 624 | best_model = { 625 | 'config': exp_cfg, 626 | 'epoch': epoch, 627 | 'state_dict': model.module.state_dict(), 628 | 'optimizer': optimizer.state_dict(), 629 | "Rotation Mean Error": final_yaw_error 630 | } 631 | torch.save(best_model, savefilename) 632 | if old_saved_file is not None: 633 | os.remove(old_saved_file) 634 | if args.wandb: 635 | wandb.run.summary["best_rot_error"] = final_yaw_error 636 | temp = f'{final_dest}/best_model_so_far_rot.tar' 637 | torch.save(best_model, temp) 638 | wandb.save(temp) 639 | old_saved_file = savefilename 640 | 641 | if exp_cfg['weight_metric_learning'] > 0.: 642 | if auc2 > max_auc: 643 | max_auc = auc2 644 | savefilename_auc = f'{final_dest}/checkpoint_{epoch}_auc_{max_auc:.3f}.tar' 645 | best_model_auc = { 646 | 'epoch': epoch, 647 | 'config': exp_cfg, 648 | 'state_dict': model.module.state_dict(), 649 | 'recall@1': recall[0], 650 | 'max_F1': maxF1, 651 | 'AUC': auc2, 652 | 'optimizer': optimizer.state_dict(), 653 | } 654 | torch.save(best_model_auc, savefilename_auc) 655 | if old_saved_file_auc is not None: 656 | os.remove(old_saved_file_auc) 657 | old_saved_file_auc = savefilename_auc 658 | if args.wandb: 659 | wandb.run.summary["best_auc"] = max_auc 660 | 661 | temp = f'{final_dest}/best_model_so_far_auc.tar' 662 | torch.save(best_model_auc, temp) 663 | wandb.save(temp) 664 | 665 | savefilename = f'{final_dest}/checkpoint_last_iter.tar' 666 | best_model = { 667 | 'config': exp_cfg, 668 | 'epoch': epoch, 669 | 'state_dict': model.module.state_dict(), 670 | 'optimizer': optimizer.state_dict(), 671 | } 672 | torch.save(best_model, savefilename) 673 | 674 | print('full training time = %.2f HR' % ((time.time() - start_full_time) / 3600)) 675 | 676 | 677 | if __name__ == '__main__': 678 | parser = argparse.ArgumentParser() 679 | parser.add_argument('--root_folder', default='./KITTI', 680 | help='dataset directory') 681 | parser.add_argument('--dataset', default='kitti', 682 | help='dataset') 683 | parser.add_argument('--without_ground', action='store_true', default=False, 684 | help='Use preprocessed point clouds with ground plane removed') 685 | parser.add_argument('--batch_size', type=int, default=6, 686 | help='Batch size (per gpu). Minimum 2.') 687 | parser.add_argument('--checkpoints_dest', default='./checkpoints', 688 | help='folder where to save checkpoints') 689 | parser.add_argument('--wandb', action='store_true', default=False, 690 | help='Activate wandb service') 691 | parser.add_argument('--gpu', type=int, default=-1, 692 | help='Only use default value: -1') 693 | parser.add_argument('--gpu_count', type=int, default=-1, 694 | help='Only use default value: -1') 695 | parser.add_argument('--port', type=str, default='8888', 696 | help='port to be used for DDP multi-gpu training') 697 | parser.add_argument('--weights', type=str, default=None, 698 | help='Weights to be loaded, use together with --resume' 699 | 'to resume a previously stopped training') 700 | parser.add_argument('--resume', action='store_true', 701 | help='Add this flag to resume a previously stopped training,' 702 | 'the --weights argument must be provided.') 703 | 704 | args = parser.parse_args() 705 | 706 | if args.batch_size < 2: 707 | raise argparse.ArgumentTypeError("The batch size should be at least 2") 708 | 709 | os.environ['MASTER_ADDR'] = 'localhost' 710 | os.environ['MASTER_PORT'] = args.port 711 | 712 | if not args.wandb: 713 | os.environ['WANDB_MODE'] = 'dryrun' 714 | 715 | with open("wandb_config.yaml", "r") as ymlfile: 716 | cfg = yaml.load(ymlfile, Loader=yaml.SafeLoader) 717 | 718 | if args.gpu_count == -1: 719 | args.gpu_count = torch.cuda.device_count() 720 | if args.gpu == -1: 721 | mp.spawn(main_process, nprocs=args.gpu_count, args=(cfg['experiment'], 42, args.gpu_count, args,)) 722 | else: 723 | main_process(args.gpu, cfg['experiment'], 42, args.gpu_count, args) 724 | -------------------------------------------------------------------------------- /triple_selector.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import torch 3 | 4 | 5 | def get_all_triplets(dist_mat, pos_mask, neg_mask, is_inverted=False, margin=0.5, different_embedding=False): 6 | if not different_embedding: 7 | pos_mask = torch.triu(pos_mask, 1) 8 | triplets = pos_mask.unsqueeze(2) * neg_mask.unsqueeze(1) 9 | return torch.where(triplets) 10 | 11 | 12 | def hardest_negative_selector(dist_mat, pos_mask, neg_mask, is_inverted, margin=0.5, different_embedding=False): 13 | if not different_embedding: 14 | pos_mask = torch.triu(pos_mask, 1) 15 | a, p = torch.where(pos_mask) 16 | if neg_mask.sum() == 0: 17 | return a, p, None 18 | if is_inverted: 19 | dist_neg = dist_mat * neg_mask 20 | n = torch.max(dist_neg, dim=1) 21 | else: 22 | dist_neg = dist_mat.clone() 23 | dist_neg[~neg_mask] = dist_neg.max()+1. 24 | _, n = torch.min(dist_neg, dim=1) 25 | n = n[a] 26 | return a, p, n 27 | 28 | 29 | def random_negative_selector(dist_mat, pos_mask, neg_mask, is_inverted, margin=0.5, different_embedding=False): 30 | if not different_embedding: 31 | pos_mask = torch.triu(pos_mask, 1) 32 | a, p = torch.where(pos_mask) 33 | selected_negs = [] 34 | for i in range(a.shape[0]): 35 | possible_negs = torch.where(neg_mask[a[i]])[0] 36 | if len(possible_negs) == 0: 37 | return a, p, None 38 | 39 | dist_neg = dist_mat[a[i], possible_negs] 40 | if is_inverted: 41 | curr_loss = -dist_mat[a[i], p[i]] + dist_neg + margin 42 | else: 43 | curr_loss = dist_mat[a[i], p[i]] - dist_neg + margin 44 | 45 | if len(possible_negs[curr_loss > 0]) > 0: 46 | possible_negs = possible_negs[curr_loss > 0] 47 | random_neg = np.random.choice(possible_negs.cpu().numpy()) 48 | selected_negs.append(random_neg) 49 | n = torch.tensor(selected_negs, dtype=a.dtype, device=a.device) 50 | return a, p, n 51 | 52 | 53 | def semihard_negative_selector(dist_mat, pos_mask, neg_mask, is_inverted, margin=0.5, different_embedding=False): 54 | if not different_embedding: 55 | pos_mask = torch.triu(pos_mask, 1) 56 | a, p = torch.where(pos_mask) 57 | selected_negs = [] 58 | for i in range(a.shape[0]): 59 | possible_negs = torch.where(neg_mask[a[i]])[0] 60 | if len(possible_negs) == 0: 61 | return a, p, None 62 | 63 | dist_neg = dist_mat[a[i], possible_negs] 64 | if is_inverted: 65 | curr_loss = -dist_mat[a[i], p[i]] + dist_neg + margin 66 | else: 67 | curr_loss = dist_mat[a[i], p[i]] - dist_neg + margin 68 | 69 | semihard_idxs = (curr_loss > 0) & (curr_loss < margin) 70 | if len(possible_negs[semihard_idxs]) > 0: 71 | possible_negs = possible_negs[semihard_idxs] 72 | random_neg = np.random.choice(possible_negs.cpu().numpy()) 73 | selected_negs.append(random_neg) 74 | n = torch.tensor(selected_negs, dtype=a.dtype, device=a.device) 75 | return a, p, n 76 | -------------------------------------------------------------------------------- /utils/data.py: -------------------------------------------------------------------------------- 1 | import time 2 | 3 | import torch 4 | from torch.utils.data.dataloader import default_collate 5 | 6 | from datasets.KITTI360Dataset import KITTI3603DDictPairs 7 | from datasets.KITTIDataset import KITTILoader3DDictPairs 8 | import torch.utils.data 9 | 10 | 11 | def datasets_concat_kitti(data_dir, sequences_list, transforms, data_type, **kwargs): 12 | 13 | dataset_list = [] 14 | 15 | for sequence in sequences_list: 16 | poses_file = data_dir + "/sequences/" + sequence + "/poses.txt" 17 | if data_type == "3D": 18 | d = KITTILoader3DDictPairs(data_dir, sequence, poses_file, **kwargs) 19 | else: 20 | raise TypeError("Unknown data type to load") 21 | 22 | dataset_list.append(d) 23 | 24 | dataset = torch.utils.data.ConcatDataset(dataset_list) 25 | return dataset, dataset_list 26 | 27 | 28 | def datasets_concat_kitti360(data_dir, sequences_list, transforms, data_type, **kwargs): 29 | 30 | dataset_list = [] 31 | 32 | for sequence in sequences_list: 33 | if data_type == "3D": 34 | d = KITTI3603DDictPairs(data_dir, sequence, **kwargs) 35 | else: 36 | raise TypeError("Unknown data type to load") 37 | 38 | dataset_list.append(d) 39 | 40 | dataset = torch.utils.data.ConcatDataset(dataset_list) 41 | return dataset, dataset_list 42 | 43 | 44 | def merge_inputs(queries): 45 | anchors = [] 46 | positives = [] 47 | negatives = [] 48 | anchors_logits = [] 49 | positives_logits = [] 50 | negatives_logits = [] 51 | returns = {key: default_collate([d[key] for d in queries]) for key in queries[0] 52 | if key != 'anchor' and key != 'positive' and key != 'negative' and key != 'anchor_logits' 53 | and key != 'positive_logits' and key != 'negative_logits'} 54 | for input in queries: 55 | if 'anchor' in input: 56 | anchors.append(input['anchor']) 57 | if 'positive' in input: 58 | positives.append(input['positive']) 59 | if 'negative' in input: 60 | negatives.append(input['negative']) 61 | if 'anchor_logits' in input: 62 | anchors_logits.append(input['anchor_logits']) 63 | if 'positive_logits' in input: 64 | positives_logits.append(input['positive_logits']) 65 | if 'negative_logits' in input: 66 | negatives_logits.append(input['negative_logits']) 67 | 68 | if 'anchor' in input: 69 | returns['anchor'] = anchors 70 | if 'positive' in input: 71 | returns['positive'] = positives 72 | if 'negative' in input: 73 | returns['negative'] = negatives 74 | if 'anchor_logits' in input: 75 | returns['anchor_logits'] = anchors_logits 76 | if 'positive_logits' in input: 77 | returns['positive_logits'] = positives_logits 78 | if 'negative_logits' in input: 79 | returns['negative_logits'] = negatives_logits 80 | return returns 81 | 82 | 83 | class Timer(object): 84 | """A simple timer.""" 85 | 86 | def __init__(self, binary_fn=None, init_val=0): 87 | self.total_time = 0. 88 | self.calls = 0 89 | self.start_time = 0. 90 | self.diff = 0. 91 | self.binary_fn = binary_fn 92 | self.tmp = init_val 93 | 94 | def reset(self): 95 | self.total_time = 0 96 | self.calls = 0 97 | self.start_time = 0 98 | self.diff = 0 99 | 100 | @property 101 | def avg(self): 102 | if self.calls > 0: 103 | return self.total_time / self.calls 104 | else: 105 | return 0. 106 | 107 | def tic(self): 108 | # using time.time instead of time.clock because time time.clock 109 | # does not normalize for multithreading 110 | self.start_time = time.time() 111 | 112 | def toc(self, average=True): 113 | self.diff = time.time() - self.start_time 114 | self.total_time += self.diff 115 | self.calls += 1 116 | if self.binary_fn: 117 | self.tmp = self.binary_fn(self.tmp, self.diff) 118 | if average: 119 | return self.avg 120 | else: 121 | return self.diff 122 | 123 | -------------------------------------------------------------------------------- /utils/geometry.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import torch 3 | 4 | from functools import reduce 5 | import torch.utils.data 6 | import math 7 | import open3d as o3d 8 | 9 | 10 | def euler2mat(z, y, x): 11 | Ms = [] 12 | if z: 13 | cosz = math.cos(z) 14 | sinz = math.sin(z) 15 | Ms.append(np.array( 16 | [[cosz, -sinz, 0], 17 | [sinz, cosz, 0], 18 | [0, 0, 1]])) 19 | if y: 20 | cosy = math.cos(y) 21 | siny = math.sin(y) 22 | Ms.append(np.array( 23 | [[cosy, 0, siny], 24 | [0, 1, 0], 25 | [-siny, 0, cosy]])) 26 | if x: 27 | cosx = math.cos(x) 28 | sinx = math.sin(x) 29 | Ms.append(np.array( 30 | [[1, 0, 0], 31 | [0, cosx, -sinx], 32 | [0, sinx, cosx]])) 33 | if Ms: 34 | return reduce(np.dot, Ms[::-1]) 35 | return np.eye(3) 36 | 37 | 38 | def euler2mat_torch(z, y, x): 39 | Ms = [] 40 | if z: 41 | cosz = torch.cos(z) 42 | sinz = torch.sin(z) 43 | Ms.append(torch.tensor( 44 | [[cosz, -sinz, 0], 45 | [sinz, cosz, 0], 46 | [0, 0, 1]]).cuda()) 47 | if y: 48 | cosy = torch.cos(y) 49 | siny = torch.sin(y) 50 | Ms.append(torch.tensor( 51 | [[cosy, 0, siny], 52 | [0, 1, 0], 53 | [-siny, 0, cosy]]).cuda()) 54 | if x: 55 | cosx = torch.cos(x) 56 | sinx = torch.sin(x) 57 | Ms.append(torch.tensor( 58 | [[1, 0, 0], 59 | [0, cosx, -sinx], 60 | [0, sinx, cosx]]).cuda()) 61 | if Ms: 62 | return reduce(torch.matmul, Ms[::-1]) 63 | return torch.eye(3).float().cuda() 64 | 65 | 66 | def mat2xyzrpy(rotmatrix): 67 | """ 68 | Decompose transformation matrix into components 69 | Args: 70 | rotmatrix (torch.Tensor/np.ndarray): [4x4] transformation matrix 71 | Returns: 72 | torch.Tensor: shape=[6], contains xyzrpy 73 | """ 74 | roll = math.atan2(-rotmatrix[1, 2], rotmatrix[2, 2]) 75 | pitch = math.asin(rotmatrix[0, 2]) 76 | yaw = math.atan2(-rotmatrix[0, 1], rotmatrix[0, 0]) 77 | x = rotmatrix[:3, 3][0] 78 | y = rotmatrix[:3, 3][1] 79 | z = rotmatrix[:3, 3][2] 80 | 81 | return torch.tensor([x, y, z, roll, pitch, yaw], device=rotmatrix.device, dtype=rotmatrix.dtype) 82 | 83 | 84 | def rototransl_pc(pc, transl, rot, name): 85 | 86 | pcd = o3d.geometry.PointCloud() 87 | 88 | R_sample = torch.from_numpy(euler2mat(rot[0], rot[1], rot[2])).cuda() 89 | 90 | RT1 = torch.cat((torch.eye(3).double().cuda(), transl.unsqueeze(0).T), 1) 91 | RT1 = torch.cat((RT1, torch.tensor([[0., 0., 0., 1.]]).cuda()), 0) 92 | RT2 = torch.cat((R_sample, torch.zeros((3, 1)).double().cuda()), 1) 93 | RT2 = torch.cat((RT2, torch.tensor([[0., 0., 0., 1.]]).cuda()), 0) 94 | 95 | RT_sample = RT1 @ RT2 96 | 97 | ones = torch.ones((1, pc.shape[0])).double().cuda() 98 | new_pc = RT_sample @ torch.cat((pc.T, ones), 0) 99 | new_pc = new_pc[0:3, :].T 100 | 101 | pcd.points = o3d.utility.Vector3dVector(new_pc.cpu().numpy()) 102 | o3d.io.write_point_cloud(name, pcd) 103 | 104 | return RT_sample 105 | 106 | 107 | def get_rt_matrix(transl, rot, rot_parmas='xyz'): 108 | if rot_parmas == 'xyz': 109 | # R_sample = torch.from_numpy(euler2mat(rot[2], rot[1], rot[0])).cuda() 110 | R_sample = euler2mat_torch(rot[2], rot[1], rot[0]) 111 | elif rot_parmas == 'zyx': 112 | # R_sample = torch.from_numpy(euler2mat(rot[0], rot[1], rot[2])).cuda() 113 | R_sample = euler2mat_torch(rot[0], rot[1], rot[2]) 114 | else: 115 | raise TypeError("Unknown rotation params order") 116 | 117 | RT1 = torch.cat((torch.eye(3).float().cuda(), transl.unsqueeze(0).T), 1) 118 | RT1 = torch.cat((RT1, torch.tensor([[0., 0., 0., 1.]]).cuda()), 0) 119 | RT2 = torch.cat((R_sample, torch.zeros((3, 1)).float().cuda()), 1) 120 | RT2 = torch.cat((RT2, torch.tensor([[0., 0., 0., 1.]]).cuda()), 0) 121 | 122 | RT_sample = RT1.float() @ RT2.float() 123 | 124 | return RT_sample 125 | -------------------------------------------------------------------------------- /utils/rotation_conversion.py: -------------------------------------------------------------------------------- 1 | from functools import reduce 2 | 3 | import torch 4 | import math 5 | import numpy as np 6 | 7 | 8 | def quaternion_from_matrix(matrix): 9 | if matrix.shape == (4, 4): 10 | R = matrix[:-1, :-1] 11 | elif matrix.shape == (3, 3): 12 | R = matrix 13 | else: 14 | raise TypeError("Not a valid rotation matrix") 15 | tr = R[0, 0] + R[1, 1] + R[2, 2] 16 | q = torch.zeros(4, device=matrix.device) 17 | if tr > 0.: 18 | S = (tr+1.0).sqrt() * 2 19 | q[0] = 0.25 * S 20 | q[1] = (R[2, 1] - R[1, 2]) / S 21 | q[2] = (R[0, 2] - R[2, 0]) / S 22 | q[3] = (R[1, 0] - R[0, 1]) / S 23 | elif R[0, 0] > R[1, 1] and R[0, 0] > R[2, 2]: 24 | S = (1.0 + R[0, 0] - R[1, 1] - R[2, 2]).sqrt() * 2 25 | q[0] = (R[2, 1] - R[1, 2]) / S 26 | q[1] = 0.25 * S 27 | q[2] = (R[0, 1] + R[1, 0]) / S 28 | q[3] = (R[0, 2] + R[2, 0]) / S 29 | elif R[1, 1] > R[2, 2]: 30 | S = (1.0 + R[1, 1] - R[0, 0] - R[2, 2]).sqrt() * 2 31 | q[0] = (R[0, 2] - R[2, 0]) / S 32 | q[1] = (R[0, 1] + R[1, 0]) / S 33 | q[2] = 0.25 * S 34 | q[3] = (R[1, 2] + R[2, 1]) / S 35 | else: 36 | S = (1.0 + R[2, 2] - R[0, 0] - R[1, 1]).sqrt() * 2 37 | q[0] = (R[1, 0] - R[0, 1]) / S 38 | q[1] = (R[0, 2] + R[2, 0]) / S 39 | q[2] = (R[1, 2] + R[2, 1]) / S 40 | q[3] = 0.25 * S 41 | return q / q.norm() 42 | 43 | 44 | # porting a numpy di quaternion_from_matrix 45 | def npmat2quat(matrix): 46 | if np.shape(matrix) == (4, 4): 47 | R = matrix[:-1, :-1] 48 | elif np.shape(matrix) == (3, 3): 49 | R = matrix 50 | else: 51 | raise TypeError("Not a valid rotation matrix") 52 | tr = R[0, 0] + R[1, 1] + R[2, 2] 53 | q = np.zeros(4) 54 | if tr > 0.: 55 | S = np.sqrt(tr+1.0) * 2 56 | q[0] = 0.25 * S 57 | q[1] = (R[2, 1] - R[1, 2]) / S 58 | q[2] = (R[0, 2] - R[2, 0]) / S 59 | q[3] = (R[1, 0] - R[0, 1]) / S 60 | elif R[0, 0] > R[1, 1] and R[0, 0] > R[2, 2]: 61 | S = np.sqrt(1.0 + R[0, 0] - R[1, 1] - R[2, 2]) * 2 62 | q[0] = (R[2, 1] - R[1, 2]) / S 63 | q[1] = 0.25 * S 64 | q[2] = (R[0, 1] + R[1, 0]) / S 65 | q[3] = (R[0, 2] + R[2, 0]) / S 66 | elif R[1, 1] > R[2, 2]: 67 | S = np.sqrt(1.0 + R[1, 1] - R[0, 0] - R[2, 2]) * 2 68 | q[0] = (R[0, 2] - R[2, 0]) / S 69 | q[1] = (R[0, 1] + R[1, 0]) / S 70 | q[2] = 0.25 * S 71 | q[3] = (R[1, 2] + R[2, 1]) / S 72 | else: 73 | S = np.sqrt(1.0 + R[2, 2] - R[0, 0] - R[1, 1]) * 2 74 | q[0] = (R[1, 0] - R[0, 1]) / S 75 | q[1] = (R[0, 2] + R[2, 0]) / S 76 | q[2] = (R[1, 2] + R[2, 1]) / S 77 | q[3] = 0.25 * S 78 | return q / np.linalg.norm(q) 79 | 80 | 81 | def euler2mat(z, y, x): 82 | # funziona come matlab con 83 | # m=eul2tform([YAW PITCH ROLL], 'XYZ') 84 | 85 | Ms = [] 86 | if z: 87 | cosz = math.cos(z) 88 | sinz = math.sin(z) 89 | Ms.append(np.array( 90 | [[cosz, -sinz, 0, 0], 91 | [sinz, cosz, 0, 0], 92 | [0, 0, 1, 0], 93 | [0, 0, 0, 1] 94 | ])) 95 | if y: 96 | cosy = math.cos(y) 97 | siny = math.sin(y) 98 | Ms.append(np.array( 99 | [[cosy, 0, siny, 0], 100 | [0, 1, 0, 0], 101 | [-siny, 0, cosy, 0], 102 | [0, 0, 0, 1] 103 | ])) 104 | if x: 105 | cosx = math.cos(x) 106 | sinx = math.sin(x) 107 | Ms.append(np.array( 108 | [[1, 0, 0, 0], 109 | [0, cosx, -sinx, 0], 110 | [0, sinx, cosx, 0], 111 | [0, 0, 0, 1] 112 | ])) 113 | if Ms: 114 | return reduce(np.dot, Ms[::-1]) #equivale a Ms[2]@Ms[1]@Ms[0] 115 | 116 | #nel caso sfigato, restituiscimi una idenatità (era 3x3, diventa 4x4) 117 | return np.eye(4) 118 | 119 | 120 | def quatmultiply(q, r): 121 | """ 122 | Batch quaternion multiplication 123 | Args: 124 | q (torch.Tensor/np.ndarray): shape=[Nx4] 125 | r (torch.Tensor/np.ndarray): shape=[Nx4] 126 | Returns: 127 | torch.Tensor/np.ndarray: shape=[Nx4] 128 | """ 129 | if isinstance(q, torch.Tensor): 130 | t = torch.zeros(q.shape[0], 4, device=q.device) 131 | elif isinstance(q, np.ndarray): 132 | t = np.zeros(q.shape[0], 4) 133 | else: 134 | raise TypeError("Type not supported") 135 | t[:, 0] = r[:, 0] * q[:, 0] - r[:, 1] * q[:, 1] - r[:, 2] * q[:, 2] - r[:, 3] * q[:, 3] 136 | t[:, 1] = r[:, 0] * q[:, 1] + r[:, 1] * q[:, 0] - r[:, 2] * q[:, 3] + r[:, 3] * q[:, 2] 137 | t[:, 2] = r[:, 0] * q[:, 2] + r[:, 1] * q[:, 3] + r[:, 2] * q[:, 0] - r[:, 3] * q[:, 1] 138 | t[:, 3] = r[:, 0] * q[:, 3] - r[:, 1] * q[:, 2] + r[:, 2] * q[:, 1] + r[:, 3] * q[:, 0] 139 | return t 140 | 141 | 142 | def quatinv(q): 143 | """ 144 | Batch quaternion inversion 145 | Args: 146 | q (torch.Tensor/np.ndarray): shape=[Nx4] 147 | Returns: 148 | torch.Tensor/np.ndarray: shape=[Nx4] 149 | """ 150 | if isinstance(q, torch.Tensor): 151 | t = q.clone() 152 | elif isinstance(q, np.ndarray): 153 | t = q.copy() 154 | else: 155 | raise TypeError("Type not supported") 156 | t *= -1 157 | t[:, 0] *= -1 158 | return t 159 | 160 | 161 | def quaternion_atan_loss(q, r): 162 | """ 163 | Batch quaternion distances, used as loss 164 | Args: 165 | q (torch.Tensor): shape=[Nx4] 166 | r (torch.Tensor): shape=[Nx4] 167 | Returns: 168 | torch.Tensor: shape=[N] 169 | """ 170 | t = quatmultiply(q, quatinv(r)) 171 | return 2 * torch.atan2(torch.norm(t[:, 1:], dim=1), torch.abs(t[:, 0])) 172 | 173 | 174 | def quat2mat(q): 175 | assert q.shape == torch.Size([4]), "Not a valid quaternion" 176 | if q.norm() != 1.: 177 | q = q / q.norm() 178 | mat = torch.zeros((4, 4), device=q.device) 179 | mat[0, 0] = 1 - 2*q[2]**2 - 2*q[3]**2 180 | mat[0, 1] = 2*q[1]*q[2] - 2*q[3]*q[0] 181 | mat[0, 2] = 2*q[1]*q[3] + 2*q[2]*q[0] 182 | mat[1, 0] = 2*q[1]*q[2] + 2*q[3]*q[0] 183 | mat[1, 1] = 1 - 2*q[1]**2 - 2*q[3]**2 184 | mat[1, 2] = 2*q[2]*q[3] - 2*q[1]*q[0] 185 | mat[2, 0] = 2*q[1]*q[3] - 2*q[2]*q[0] 186 | mat[2, 1] = 2*q[2]*q[3] + 2*q[1]*q[0] 187 | mat[2, 2] = 1 - 2*q[1]**2 - 2*q[2]**2 188 | mat[3, 3] = 1. 189 | return mat 190 | 191 | 192 | def npquat2mat(q): 193 | q = q / np.linalg.norm(q) 194 | 195 | mat = np.zeros([4, 4]) 196 | #mat = np.zeros([4, 4], dtype=q.dtype) 197 | 198 | mat[0, 0] = 1 - 2*q[2]**2 - 2*q[3]**2 199 | mat[0, 1] = 2*q[1]*q[2] - 2*q[3]*q[0] 200 | mat[0, 2] = 2*q[1]*q[3] + 2*q[2]*q[0] 201 | mat[1, 0] = 2*q[1]*q[2] + 2*q[3]*q[0] 202 | mat[1, 1] = 1 - 2*q[1]**2 - 2*q[3]**2 203 | mat[1, 2] = 2*q[2]*q[3] - 2*q[1]*q[0] 204 | mat[2, 0] = 2*q[1]*q[3] - 2*q[2]*q[0] 205 | mat[2, 1] = 2*q[2]*q[3] + 2*q[1]*q[0] 206 | mat[2, 2] = 1 - 2*q[1]**2 - 2*q[2]**2 207 | mat[3, 3] = 1. 208 | return mat 209 | 210 | def tvector2mat(t): 211 | assert t.shape == torch.Size([3]), "Not a valid translation" 212 | mat = torch.eye(4, device=t.device) 213 | mat[0, 3] = t[0] 214 | mat[1, 3] = t[1] 215 | mat[2, 3] = t[2] 216 | return mat 217 | 218 | def npxyz2mat(x,y,z): 219 | #todo TRANSFORM TO NUMPY -- assert t.shape == torch.Size([3]), "Not a valid translation" 220 | mat = np.eye(4) 221 | mat[0, 3] = x 222 | mat[1, 3] = y 223 | mat[2, 3] = z 224 | return mat 225 | 226 | def npto_XYZRPY(rotmatrix): 227 | ''' 228 | Usa mathutils per trasformare una matrice di trasformazione omogenea in xyzrpy 229 | https://docs.blender.org/api/master/mathutils.html# 230 | WARNING: funziona in 32bits quando le variabili numpy sono a 64 bit 231 | 232 | :param rotmatrix: np array 233 | :return: np array with the xyzrpy 234 | ''' 235 | 236 | # qui sotto corrisponde a 237 | # quat2eul([ 0.997785 -0.0381564 0.0358964 0.041007 ],'XYZ') 238 | roll = math.atan2(-rotmatrix[1, 2], rotmatrix[2, 2]) 239 | pitch = math.asin ( rotmatrix[0, 2]) 240 | yaw = math.atan2(-rotmatrix[0, 1], rotmatrix[0, 0]) 241 | x = rotmatrix[:3,3][0] 242 | y = rotmatrix[:3,3][1] 243 | z = rotmatrix[:3,3][2] 244 | 245 | return np.array([x,y,z,roll,pitch,yaw]) 246 | 247 | def to_rotation_matrix(R, T): 248 | R = quat2mat(R) 249 | T = tvector2mat(T) 250 | RT = torch.mm(T, R) 251 | return RT 252 | 253 | def to_rotation_matrix_XYZRPY(x,y,z,roll,pitch,yaw): 254 | 255 | R = euler2mat(yaw, pitch, roll) # la matrice che viene fuori corrisponde a eul2tform di matlab (Convert Euler angles to homogeneous transformation) 256 | T = npxyz2mat(x,y,z) 257 | RT = np.matmul(T, R) 258 | return RT -------------------------------------------------------------------------------- /utils/tools.py: -------------------------------------------------------------------------------- 1 | import torch 2 | import torch.nn.functional as F 3 | 4 | 5 | def gather_nd(params, indices, name=None): 6 | """ 7 | the input indices must be a 2d tensor in the form of [[a,b,..,c],...], 8 | which represents the location of the elements. 9 | """ 10 | 11 | indices = indices.t().long() 12 | ndim = indices.size(0) 13 | idx = torch.zeros_like(indices[0]).long() 14 | m = 1 15 | 16 | for i in range(ndim)[::-1]: 17 | idx += indices[i] * m 18 | m *= params.size(i) 19 | 20 | return torch.take(params, idx) 21 | 22 | 23 | def pairwise_mse(x, y=None): 24 | """ 25 | Input: x is a Nxd tensor 26 | y is an optional Mxd tensor 27 | Output: dist is a NxM matrix where dist[i,j] is the square norm between x[i,:] and y[j,:] 28 | if y is not given then use 'y=x'. 29 | i.e. dist[i,j] = ||x[i,:]-y[j,:]||^2 30 | """ 31 | x_norm = x.pow(2).sum(1).view(-1, 1) 32 | if y is not None: 33 | y_norm = y.pow(2).sum(1).view(1, -1) 34 | else: 35 | y = x 36 | y_norm = x_norm.view(1, -1) 37 | 38 | dist = x_norm + y_norm - 2.0 * torch.mm(x, torch.transpose(y, 0, 1)) 39 | return dist 40 | 41 | 42 | def pairwise_L2(x, y=None): 43 | if y is None: 44 | y = x 45 | dist = pairwise_mse(x, y) 46 | dist = torch.sqrt(dist + 1e-5) 47 | return dist 48 | 49 | 50 | def pairwise_dot(x, y=None): 51 | if y is None: 52 | y = x 53 | dot = torch.mm(x, torch.transpose(y, 0, 1)) 54 | #return 1. - dot 55 | return dot 56 | 57 | 58 | def pairwise_cosine(x, y=None): 59 | if y is None: 60 | y = x 61 | x_norm = x / x.norm(dim=1).unsqueeze(1) 62 | y_norm = y / y.norm(dim=1).unsqueeze(1) 63 | res = 1. - torch.mm(x_norm, y_norm.transpose(0, 1)) 64 | return res 65 | 66 | 67 | def softmax_cross_entropy(logits, target, mean=True): 68 | loss = torch.sum(- target * F.log_softmax(logits, -1), -1) 69 | if mean: 70 | loss = loss.mean() 71 | return loss 72 | 73 | 74 | def pairwise_batch_mse(x, y=None): 75 | """ 76 | Input: x is a Nxd tensor 77 | y is an optional Mxd tensor 78 | Output: dist is a NxM matrix where dist[i,j] is the square norm between x[i,:] and y[j,:] 79 | if y is not given then use 'y=x'. 80 | i.e. dist[i,j] = ||x[i,:]-y[j,:]||^2 81 | """ 82 | x_norm = x.pow(2).sum(2, True) 83 | if y is not None: 84 | y_norm = y.pow(2).sum(2, True) 85 | else: 86 | y = x 87 | y_norm = x_norm.transpose(1, 2) 88 | 89 | dist = x_norm + y_norm - 2.0 * torch.bmm(x, torch.transpose(y, 1, 2)) 90 | return dist 91 | 92 | 93 | def _pairwise_distance(x, squared=False, eps=1e-16): 94 | # Compute the 2D matrix of distances between all the embeddings. 95 | 96 | cor_mat = torch.matmul(x, x.t()) 97 | norm_mat = cor_mat.diag() 98 | distances = norm_mat.unsqueeze(1) - 2 * cor_mat + norm_mat.unsqueeze(0) 99 | distances = F.relu(distances) 100 | 101 | if not squared: 102 | mask = torch.eq(distances, 0.0).float() 103 | distances = distances + mask * eps 104 | distances = torch.sqrt(distances) 105 | distances = distances * (1.0 - mask) 106 | 107 | return distances 108 | -------------------------------------------------------------------------------- /wandb_config.yaml: -------------------------------------------------------------------------------- 1 | "experiment": 2 | # TRAINING 3 | "epochs" : 150 4 | "learning_rate" : 0.004 5 | "beta1" : 0.9 6 | "beta2" : 0.999 7 | "eps" : 1.e-8 8 | "weight_decay" : 0.000005 9 | # SCHEDULER 10 | "scheduler": "multistep" 11 | # NETWORK PARAMS 12 | "3D_net" : PVRCNN 13 | "head" : UOTHead 14 | "model_norm": "batch" 15 | "num_points" : 4096 16 | "feature_size" : 640 17 | "cluster_size" : 64 18 | "shared_embeddings": True 19 | "feature_output_dim_3D" : 256 20 | "point_cloud_augmentation": True 21 | "point_cloud_jitter": False 22 | # LOSS 23 | "margin": 0.5 24 | "loss_type": "triplet_random" 25 | "norm_embeddings": True 26 | "loop_file": "loop_GT_4m" 27 | "weight_metric_learning": 1 28 | "weight_rot": 1 29 | "sinkhorn_iter": 5 30 | "sinkhorn_aux_loss": True 31 | "sinkhorn_type": "unbalanced" 32 | "hard_mining": False 33 | --------------------------------------------------------------------------------