├── LICENSE ├── README.md ├── Task_File_SVGS.txt ├── Task_File_VGS.txt ├── point_clouds_IO.cpp ├── point_clouds_IO.h ├── supervoxel_segmentation.cpp ├── supervoxel_segmentation.h ├── supervoxel_segmentation.hpp ├── test ├── voxel_segmentation.cpp ├── voxel_segmentation.h └── voxel_segmentation.hpp /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 | {one line to give the program's name and a brief idea of what it does.} 635 | Copyright (C) {year} {name of author} 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 | {project} Copyright (C) {year} {fullname} 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 | # Voxel- and graph-based point-cloud-segmentation 2 | This is the source code for the algorithm of Voxel- and Graph-based Segmentation (VGS/SVGS). 3 | 4 | ### Environment: 5 | The code is written in C++ and tested in VS2015. 6 | 7 | ### Dependence: 8 | The code depends on the following third-party libraries: 9 | -PCL 1.8.1. 10 | 11 | ### How to use these codes： 12 | please refer to "Use_Test" 13 | 14 | The "input_vector" encodes the input parameters (see "Task_File_VGS.txt" and "Task_File_SVGS.txt"), you can change it according to your demands. 15 | 16 | Special thanks to [M.Sc. Dong Lin](https://tu-dresden.de/bu/umwelt/geo/ipf/photogrammetrie/die-professur) from TU Dresden for correcting bugs in the codes! 17 | 18 | ### Reference: 19 | 20 | Testing dataset (Town_Test.pcd) is cropped from ETH Zurich dataset： http://semantic3d.net/ 21 | 22 | You can find more details about the VGS/SVGS algorithm in our recent publications: 23 | 24 | ``` 25 | @article{xu2017geometric, 26 | title={Geometric primitive extraction from point clouds of construction sites using VGS}, 27 | author={Xu, Yusheng and Tuttas, Sebastian and Hoegner, Ludwig and Stilla, Uwe}, 28 | journal={IEEE Geoscience and Remote Sensing Letters}, 29 | volume={14}, 30 | number={3}, 31 | pages={424--428}, 32 | year={2017}, 33 | publisher={IEEE} 34 | } 35 | @article{xu2018voxel, 36 | title={A voxel- and graph-based strategy for segmenting man-made infrastructures using perceptual grouping laws: comparison and evaluation}, 37 | author={Xu, Yusheng and Hoegner, Ludwig and Tuttas, Sebastian and Stilla, Uwe}, 38 | journal={Photogrammetric Engineering \& Remote Sensing}, 39 | volume={84}, 40 | number={6}, 41 | pages={377--391}, 42 | year={2018}, 43 | publisher={American Society for Photogrammetry and Remote Sensing} 44 | } 45 | ``` 46 | -------------------------------------------------------------------------------- /Task_File_SVGS.txt: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 1 2 | // File: Task_File_SVGS.txt 3 | // Author: Yusheng Xu, TUM_PF (yusheng.xu@tum.de) 4 | // Description: Task file for the point cloud segmentation 5 | // Modified: 26.12.2016 6 | // Copyright (c) 2015-2016 Yusheng Xu (yusheng.xu@tum.de) 7 | //////////////////////////////////////////////////////////////////////////////// 8 | Seg 9 | //Tasks 9 10 | SVGS segmentation, 10th.June 2017 for PE&RS paper 11 | 12 | //Input path 12 13 | D:\\Research\\SV\\Test\\ 14 | 15 | //Input name 15 16 | Town_Test.pcd 17 | 18 | //Output path 18 19 | D:\\Research\\SV\\Test\\ 20 | 21 | //Output name 21 22 | Town_Test_SVGS.pcd 23 | 24 | //Method 24 25 | 3 26 | 27 | //Parameters 27 28 | voxel_size 28 29 | 0.05 30 | seed_size 31 | 0.25 32 | graph_size 32 33 | 0.5 34 | sig_p: Proximity 35 | 0.2 36 | sig_n: Normal angle 36 37 | 0.2 38 | sig_o: Stair-like 39 | 0.2 40 | sig_e: Similarity 40 41 | 0.2 42 | sig_l: Convexity 43 | 0.2 44 | sig_w: Weight 44 45 | 1 46 | sig_a: SV color 47 | 0 48 | sig_b: SV distance 48 49 | 0.25 50 | sig_c: SV normal 51 | 0.75 52 | cut_thred 52 53 | 0.5 54 | points_min 55 | 10 56 | points_min2 56 57 | 10 58 | voxels_min 59 | 3 60 | k_nearest 60 61 | 3 62 | 63 | D:\Research\SV\Test\Task_File_SVGS.txt 64 | 65 | 66 | -------------------------------------------------------------------------------- /Task_File_VGS.txt: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 1 2 | // File: Task_File_VGS.txt 3 | // Author: Yusheng Xu, TUM_PF (yusheng.xu@tum.de) 4 | // Description: Task file for the point cloud segmentation 5 | // Modified: 10.06.2016 6 | // Copyright (c) 2015-2018 Yusheng Xu (yusheng.xu@tum.de) 7 | //////////////////////////////////////////////////////////////////////////////// 8 | Seg 9 | //Tasks 9 10 | VGS segmentation, 10th.June 2017 11 | 12 | //Input path 12 13 | D:\\Research\\SV\\Test\\ 14 | 15 | //Input name 15 16 | Town_Test.pcd 17 | 18 | //Output path 18 19 | D:\\Research\\SV\\Test\\ 20 | 21 | //Output name 21 22 | Town_Test_VGS.pcd 23 | 24 | //Method 24 25 | 2 26 | 27 | //Parameters 27 28 | voxel_size 28 29 | 0.15 30 | graph_size 30 31 | 0.5 32 | sig_p: Proximity 32 33 | 0.2 34 | sig_n: Normal angle 35 | 0.2 36 | sig_o: Stair-like 37 | 0.2 38 | sig_e: Similarity 39 | 0.2 40 | sig_c: Convexity 41 | 0.2 42 | sig_weight: 43 | 2 44 | cut_thred 44 45 | 0.3 46 | points_min 47 | 10 48 | adjacency_min 48 49 | 3 50 | voxels_min 51 | 3 52 | 53 | D:\Research\SV\Test\Task_File_VGS.txt 54 | 55 | 56 | 57 | -------------------------------------------------------------------------------- /point_clouds_IO.cpp: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | // File: point_cloud_IO.cpp 3 | // Author: Yusheng Xu, PF_Technische Universitaet Muenchen (yusheng.xu@tum.de) 4 | // Description: IO operation of the point clouds 5 | // Modified: 28.4.2018 6 | // 7 | // Copyright (c) 2015-2018 Yusheng Xu (yusheng.xu@tum.de) 8 | // 9 | // This library is free software; you can redistribute it and/or 10 | // modify it under the terms of the GNU General Public 11 | // License as published by the Free Software Foundation; either 12 | // Version 3 of the License, or (at your option) any later version. 13 | // 14 | // This library is distributed in the hope that it will be useful, 15 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 16 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 17 | // General Public License for more details. 18 | //////////////////////////////////////////////////////////////////////////////// 19 | 20 | #include "point_clouds_IO.h" 21 | 22 | //IO::save colored clusters in a point cloud 23 | void 24 | saveColoredClusters(string fileoutpath_name, pcl::PointCloud::Ptr input_cloud, std::vector > clusters_points_idx) 25 | { 26 | //Settings 27 | int clusters_num=0; 28 | int points_num=0; 29 | std::vector points_idx; 30 | 31 | pcl::PointCloud colored_cloud; 32 | pcl::PointXYZRGB colored_point; 33 | 34 | //Random colors 35 | std::vector color_map(3); 36 | srand(static_cast (time(0))); 37 | 38 | //Record points in each cluster 39 | clusters_num=clusters_points_idx.size(); 40 | for(int i=0;i points_idx; 43 | points_idx=clusters_points_idx.at(i); 44 | points_num=points_idx.size(); 45 | 46 | color_map[0]=static_cast (rand()%256); 47 | color_map[1]=static_cast (rand()%256); 48 | color_map[2]=static_cast (rand()%256); 49 | 50 | for(int j=0;jpoints[point_idx].x; 54 | colored_point.y=input_cloud->points[point_idx].y; 55 | colored_point.z=input_cloud->points[point_idx].z; 56 | colored_point.r=color_map[0]; 57 | colored_point.g=color_map[1]; 58 | colored_point.b=color_map[2]; 59 | 60 | //Output 61 | colored_cloud.points.push_back(colored_point); 62 | } 63 | } 64 | 65 | //Output point cloud 66 | //std::string fileout_name("Supervoxel_Test_Clustered_Points.pcd"); 67 | //std::string fileoutpath_name=path_name+fileout_name; 68 | colored_cloud.width=colored_cloud.size(); 69 | colored_cloud.height=1; 70 | pcl::io::savePCDFile(fileoutpath_name,colored_cloud); 71 | } 72 | void 73 | saveColoredClusters(string fileoutpath_name, pcl::PointCloud::Ptr colored_cloud) 74 | { 75 | pcl::io::savePCDFile(fileoutpath_name, *colored_cloud); 76 | } 77 | 78 | //IO::show colored results 79 | void 80 | showColoredClusters(string fileoutpath_name, pcl::PointCloud::Ptr input_cloud, std::vector > clusters_points_idx) 81 | { 82 | //Settings 83 | int clusters_num = 0; 84 | int points_num = 0; 85 | std::vector points_idx; 86 | 87 | pcl::PointCloud::Ptr colored_cloud (new pcl::PointCloud); 88 | pcl::PointXYZRGB colored_point; 89 | 90 | //Random colors 91 | std::vector color_map(3); 92 | srand(static_cast (time(0))); 93 | 94 | //Record points in each cluster 95 | clusters_num = clusters_points_idx.size(); 96 | for (int i = 0; i points_idx; 99 | points_idx = clusters_points_idx.at(i); 100 | points_num = points_idx.size(); 101 | 102 | color_map[0] = static_cast (rand() % 256); 103 | color_map[1] = static_cast (rand() % 256); 104 | color_map[2] = static_cast (rand() % 256); 105 | 106 | for (int j = 0; jpoints[point_idx].x; 110 | colored_point.y = input_cloud->points[point_idx].y; 111 | colored_point.z = input_cloud->points[point_idx].z; 112 | colored_point.r = color_map[0]; 113 | colored_point.g = color_map[1]; 114 | colored_point.b = color_map[2]; 115 | 116 | //Output 117 | colored_cloud->points.push_back(colored_point); 118 | } 119 | } 120 | 121 | //Output point cloud 122 | colored_cloud->width = colored_cloud->size(); 123 | colored_cloud->height = 1; 124 | 125 | boost::shared_ptr viewer(new pcl::visualization::PCLVisualizer("3D Viewer")); 126 | viewer->setBackgroundColor(0, 0, 0); 127 | viewer->addPointCloud(colored_cloud, fileoutpath_name); 128 | 129 | while (!viewer->wasStopped()) 130 | { 131 | viewer->spinOnce(100); 132 | } 133 | } 134 | void 135 | showColoredClusters(string fileoutpath_name, pcl::PointCloud::Ptr colored_cloud) 136 | { 137 | boost::shared_ptr viewer(new pcl::visualization::PCLVisualizer("3D Viewer")); 138 | viewer->setBackgroundColor(0, 0, 0); 139 | viewer->addPointCloud(colored_cloud, fileoutpath_name); 140 | 141 | while (!viewer->wasStopped()) 142 | { 143 | viewer->spinOnce(100); 144 | } 145 | } 146 | 147 | //IO::read & save taskfile 148 | std::vector 149 | inputTaskTxtFile(string pathname_file)//Read the task file 150 | { 151 | //Parameters 152 | int type_method=0; 153 | string line_string, temp_string; 154 | std::vector task_vector; 155 | 156 | //Settings 157 | task_vector.clear(); 158 | 159 | //Input file 160 | std::ifstream taskFile; 161 | taskFile.open(pathname_file); 162 | while (std::getline(taskFile, line_string)) 163 | { 164 | task_vector.push_back(line_string); 165 | } 166 | taskFile.close(); 167 | 168 | return(task_vector); 169 | } 170 | -------------------------------------------------------------------------------- /point_clouds_IO.h: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | // File: point_cloud_IO.h 3 | // Author: Yusheng Xu, PF_Technische Universitaet Muechen (yusheng.xu@tum.de) 4 | // Description: IO operation of the point clouds 5 | // Modified: 29.7.2016 6 | // 7 | // Copyright (c) 2015-2017 Yusheng Xu (yusheng.xu@tum.de) 8 | // 9 | // This library is free software; you can redistribute it and/or 10 | // modify it under the terms of the GNU General Public 11 | // License as published by the Free Software Foundation; either 12 | // Version 3 of the License, or (at your option) any later version. 13 | // 14 | // This library is distributed in the hope that it will be useful, 15 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 16 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 17 | // General Public License for more details. 18 | //////////////////////////////////////////////////////////////////////////////// 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | #include 26 | #include 27 | #include 28 | #include 29 | 30 | #include 31 | #include 32 | #include 33 | 34 | #include 35 | #include 36 | #include 37 | 38 | //Type definition 39 | typedef pcl::PointCloud::Ptr PCXYZRGBAPtr; 40 | typedef pcl::PointCloud PCXYZRGBA; 41 | typedef pcl::PointCloud::Ptr PCXYZRGBPtr; 42 | typedef pcl::PointCloud PCXYZRGB; 43 | typedef pcl::PointCloud::Ptr PCXYZPtr; 44 | typedef pcl::PointCloud PCXYZ; 45 | 46 | using namespace std; 47 | 48 | //Declaration 49 | template 50 | int 51 | inputPointCloudData(std::string dataName, PTTypePtr dataCloud); //Input point clouds 52 | 53 | template 54 | int 55 | outputPointCloudData(string outName, PTTypePtr dataCloud);//Output point clouds 56 | 57 | ////////////////Remarks////////////// 58 | // For the use of "template" the 59 | // declaration and definition of 60 | // certain functions & classes 61 | // should in same .h or .cpp files 62 | ///////////////////////////////////// 63 | 64 | //Input point cloud 65 | template 66 | int 67 | inputPointCloudData(string dataName, PTTypePtr dataCloud) 68 | { 69 | //Input the PCD file of datasets 70 | if( pcl::io::loadPCDFile(dataName, *dataCloud) == -1) 71 | { 72 | PCL_ERROR("Couldn't read the PCD file!"); 73 | return(-1); 74 | } 75 | 76 | //Show the information of the points in the input PCD files 77 | size_t dataSize=dataCloud->points.size(); // Size of the point clouds 78 | 79 | return (0); 80 | } 81 | 82 | template 83 | int 84 | inputPointCloudData2(string dataName, PTTypePtr dataCloud) 85 | { 86 | //Input the PCD file of datasets 87 | if (pcl::io::loadPLYFile(dataName, *dataCloud) == -1) 88 | { 89 | PCL_ERROR("Couldn't read the PLY file!"); 90 | return(-1); 91 | } 92 | //Show the information of the points in the input PLY files 93 | size_t dataSize = dataCloud->points.size(); // Size of the point clouds 94 | return (0); 95 | } 96 | 97 | //Output point cloud 98 | template 99 | int 100 | outputPointCloudData(string outName, PTTypePtr dataCloud) 101 | { 102 | if( pcl::io::savePCDFile(outName, *dataCloud) == -1) 103 | { 104 | PCL_ERROR("Couldn't save the PCD file!"); 105 | return(-1); 106 | } 107 | return (0); 108 | } 109 | 110 | 111 | //IO::save colored clusters in a point cloud 112 | void 113 | saveColoredClusters(string path_name, pcl::PointCloud::Ptr input_cloud,std::vector > clusters_points_idx); 114 | void 115 | saveColoredClusters(string fileoutpath_name, pcl::PointCloud::Ptr colored_cloud); 116 | 117 | //IO::show colored results 118 | void 119 | showColoredClusters(string fileoutpath_name, pcl::PointCloud::Ptr input_cloud, std::vector > clusters_points_idx); 120 | void 121 | showColoredClusters(string fileoutpath_name, pcl::PointCloud::Ptr colored_cloud); 122 | 123 | //IO::read & save task file 124 | std::vector 125 | inputTaskTxtFile(string pathname_file);//Read the task file 126 | -------------------------------------------------------------------------------- /supervoxel_segmentation.cpp: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | // File: supervoxel_segmentation.cpp 3 | // Author: Yusheng Xu, PF_Technische Universitaet Muechen (yusheng.xu@tum.de) 4 | // Description: The voxel based segmentation for point cloud 5 | // Modified: 5.6.2016 6 | // 7 | // Copyright (c) 2016 Yusheng Xu (yusheng.xu@tum.de) 8 | // 9 | // This library is free software; you can redistribute it and/or 10 | // modify it under the terms of the GNU General Public 11 | // License as published by the Free Software Foundation; either 12 | // Version 3 of the License, or (at your option) any later version. 13 | // 14 | // This library is distributed in the hope that it will be useful, 15 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 16 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 17 | // General Public License for more details. 18 | // 19 | //////////////////////////////////////////////////////////////////////////////// 20 | 21 | #include "supervoxel_segmentation.h" 22 | #include "supervoxel_segmentation.hpp" 23 | -------------------------------------------------------------------------------- /supervoxel_segmentation.h: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | // File: supervoxel_segmentation.h 3 | // Author: Yusheng Xu, PF_Technische Universitaet Muechen (yusheng.xu@tum.de) 4 | // Description: The supervoxel based segmentation methods for point cloud 5 | // Modified: 29.05.2018, By Dong Lin TU Dresden, Bugs repaired 6 | // 7 | // Copyright (c) 2015-2018 Yusheng Xu (yusheng.xu@tum.de) 8 | // 9 | // This library is free software; you can redistribute it and/or 10 | // modify it under the terms of the GNU General Public 11 | // License as published by the Free Software Foundation; either 12 | // Version 3 of the License, or (at your option) any later version. 13 | // 14 | // This library is distributed in the hope that it will be useful, 15 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 16 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 17 | // General Public License for more details. 18 | //////////////////////////////////////////////////////////////////////////////// 19 | #pragma once 20 | 21 | #ifndef PCL_SEGMENTATION_SUPERVOXEL_H_ 22 | #define PCL_SEGMENTATION_SUPERVOXEL_H_ 23 | 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | 30 | #include 31 | 32 | #include 33 | #include 34 | #include 35 | 36 | #include 37 | #include 38 | #include 39 | #include 40 | 41 | #include 42 | #include 43 | 44 | //Type definition 45 | typedef pcl::PointCloud::Ptr PCXYZRGBAPtr; 46 | typedef pcl::PointCloud PCXYZRGBA; 47 | typedef pcl::PointCloud::Ptr PCXYZRGBPtr; 48 | typedef pcl::PointCloud PCXYZRGB; 49 | typedef pcl::PointCloud::Ptr PCXYZPtr; 50 | typedef pcl::PointCloud PCXYZ; 51 | 52 | typedef pcl::PointCloud::Ptr PTNORMPtr; 53 | typedef pcl::PointCloud PTNORM; 54 | 55 | namespace pcl 56 | { 57 | template 58 | class SuperVoxelBasedSegmentation: public pcl::octree::OctreePointCloud 59 | { 60 | //Inherited 61 | using pcl::octree::OctreePointCloud::input_; 62 | 63 | //New ones 64 | public: 65 | struct 66 | Weight_Index 67 | { 68 | float Weight; 69 | int Index; 70 | }; 71 | 72 | static bool 73 | godown(const Weight_Index & a, const Weight_Index & b) 74 | { 75 | return a.Weight > b.Weight; 76 | } 77 | 78 | static bool 79 | riseup(const Weight_Index & a, const Weight_Index & b) 80 | { 81 | return a.Weight < b.Weight; 82 | } 83 | 84 | //Construction 85 | SuperVoxelBasedSegmentation(double input_resolution):OctreePointCloud(input_resolution) 86 | { 87 | } 88 | 89 | //Destrction 90 | ~SuperVoxelBasedSegmentation() 91 | { 92 | } 93 | 94 | //Member functions 95 | int 96 | getTaskVector(std::vector input_vector) 97 | { 98 | this->task_vector_=input_vector; 99 | } 100 | 101 | int 102 | getCloudPointNum(PCXYZPtr input_data) 103 | { 104 | points_num_=input_data->points.size(); 105 | 106 | points_cloud_= input_data; 107 | 108 | return (points_num_); 109 | } 110 | 111 | int 112 | getVoxelNum() 113 | { 114 | voxels_num_=this->voxel_centers_.size(); 115 | return (voxels_num_); 116 | } 117 | 118 | int 119 | getSuperVoxelNum() 120 | { 121 | return (supervoxels_num_); 122 | } 123 | 124 | int 125 | getClusterNum() 126 | { 127 | return (clusters_num_); 128 | } 129 | 130 | std::vector > 131 | getClusterIdx() 132 | { 133 | return(clusters_point_idx_); 134 | } 135 | 136 | Eigen::MatrixXf 137 | getGlobalGraph() 138 | { 139 | return(global_adjacency_matrix_); 140 | } 141 | 142 | //Voxel & Supervoxel 143 | void 144 | setVoxelSize(double input_resolution,int points_num_min) 145 | { 146 | this->voxel_resolution_=input_resolution; 147 | this->voxel_points_min_=points_num_min; 148 | } 149 | 150 | void 151 | setSupervoxelSize(double input_resolution,int voxels_num_min, int points_num_min, int adjacency_num_min) 152 | { 153 | this->seed_resolution_=input_resolution; 154 | this->supervoxel_voxel_min_=voxels_num_min; 155 | this->supervoxel_point_min_=points_num_min; 156 | this->supervoxel_adjacency_min_=adjacency_num_min; 157 | } 158 | 159 | void 160 | setGraphSize(double small_resolution, double large_resolution) 161 | { 162 | this->graph_resolution_=large_resolution; //Global, for the neighboring SV 163 | this->adjacent_resolution_=small_resolution; //Local, for the adjacent SV 164 | } 165 | 166 | void 167 | setBoundingBox(double min_x, double min_y, double min_z, double max_x, double max_y, double max_z) 168 | { 169 | this->min_x_=min_x; 170 | this->min_y_=min_y; 171 | this->min_z_=min_z; 172 | 173 | this->max_x_=max_x; 174 | this->max_y_=max_y; 175 | this->max_z_=max_z; 176 | } 177 | 178 | void 179 | setSupervoxelCentersCentroids() 180 | { 181 | //Setting 182 | SuperVoxelBasedSegmentation::LeafNodeIterator iter_leaf (this); //Iteration index for all leaf nodes in the tree 183 | 184 | PCXYZPtr temp_voxel_centers_cloud (new PCXYZ); 185 | pcl::PointXYZ voxel_center_pt; 186 | 187 | PCXYZPtr temp_voxel_centroids_cloud (new PCXYZ); 188 | pcl::PointXYZ voxel_centroid_pt; 189 | 190 | PCXYZPtr temp_voxel_points_cloud (new PCXYZ); 191 | 192 | //Tranverse the octree leaves 193 | while (*++iter_leaf) 194 | { 195 | //Search centers from the key of the leaf node 196 | pcl::PointXYZ node_center, node_centroid; 197 | pcl::octree::OctreeKey leaf_key; 198 | leaf_key=iter_leaf.getCurrentOctreeKey(); 199 | 200 | // 根据 leaf node 编号, voxel分辨率 以及 boundary的X,Y,Z的最小值确定 该 leaf node 点的空间坐标 201 | this->getVoxelCenterFromOctreeKey(leaf_key, node_center); 202 | this->voxel_centers_.push_back(node_center); //Center of the voxel 203 | 204 | //Search idx all the points in this leaf node 205 | std::vector points_idx; 206 | points_idx=iter_leaf.getLeafContainer().getPointIndicesVector(); 207 | this->voxels_point_idx_.push_back(points_idx); 208 | 209 | // 根据 voxel 范围内点的坐标(X, Y, Z)平均值, 得到每个体素的质心位置 210 | //Calculate the centroid of the voxel (质心) 211 | int voxel_points_num=points_idx.size(); 212 | for (int i=0;ipush_back(this->points_cloud_->points[points_idx[i]]); 215 | } 216 | node_centroid=this->calculateVoxelCentroid(temp_voxel_points_cloud); 217 | this->voxel_centroids_.push_back(node_centroid); //Centroid of the voxel 218 | temp_voxel_points_cloud->clear(); 219 | } 220 | 221 | //Build cloud 222 | int voxel_centers_cloud_size=this->voxel_centers_.size(); 223 | this->centers_num_=voxel_centers_cloud_size; 224 | 225 | for (int i=0;ivoxel_centers_[i]; 228 | voxel_centroid_pt=this->voxel_centroids_[i]; 229 | temp_voxel_centers_cloud->points.push_back(voxel_center_pt); 230 | temp_voxel_centroids_cloud->points.push_back(voxel_centroid_pt); 231 | } 232 | 233 | temp_voxel_centers_cloud->width = (int)temp_voxel_centers_cloud->points.size(); 234 | temp_voxel_centers_cloud->height = 1; 235 | 236 | temp_voxel_centroids_cloud->width = (int)temp_voxel_centroids_cloud->points.size(); 237 | temp_voxel_centroids_cloud->height = 1; 238 | 239 | //Assignment 240 | this->voxel_centers_cloud_=temp_voxel_centers_cloud; 241 | this->voxel_centroids_cloud_=temp_voxel_centroids_cloud; 242 | } 243 | 244 | //Super-voxelization 245 | void 246 | createSupervoxels() 247 | { 248 | //Point cloud draw 249 | pcl::PointCloud::Ptr labeled_cloud (new pcl::PointCloud); 250 | pcl::PointCloud::Ptr data_cloud (new pcl::PointCloud); 251 | pcl::PointCloud::Ptr voxel_centroid_cloud_insuper (new pcl::PointCloud); 252 | pcl::PointCloud::Ptr cloud_f (new pcl::PointCloud); 253 | 254 | pcl::PointCloud::Ptr input_cloud (new pcl::PointCloud); 255 | 256 | //Point cloud copy 257 | input_cloud=this->points_cloud_; 258 | pcl::copyPointCloud(*input_cloud, *data_cloud); 259 | 260 | //Supervoxels' information 261 | std::vector > supervoxels_points_idx; 262 | 263 | //Creat supervoxel structure 264 | //源自 Papon CVPR 2013 的实现, 好像不能调, 只能后续分析结果... 265 | pcl::SupervoxelClustering super (this->voxel_resolution_, this->seed_resolution_); 266 | 267 | super.setInputCloud (data_cloud); 268 | super.setColorImportance (this->color_impt_); 269 | super.setSpatialImportance (this->spatial_impt_); 270 | super.setNormalImportance (this->normal_impt_); 271 | 272 | // map 的元素以 (key-value) 对的形式存贮 273 | std::map ::Ptr > supervoxel_clusters; 274 | 275 | //Oversegmentation using supervoxel 276 | super.extract (supervoxel_clusters); 277 | super.refineSupervoxels(5,supervoxel_clusters); 278 | this->supervoxels_num_=supervoxel_clusters.size(); 279 | pcl::console::print_info ("Found %d supervoxels\n", supervoxel_clusters.size ()); 280 | 281 | //Record points and voxels of each supervoxel 282 | // labeled_points_cloud->points[点号].label 283 | pcl::PointCloud::Ptr labeled_points_cloud = super.getLabeledCloud (); 284 | int max_label=super.getMaxLabel(); 285 | int size_labeled_points=labeled_points_cloud->points.size(); 286 | std::vector labels_voxels_idxs; 287 | 288 | //Create label maps 289 | std::vector> points_label_map, voxels_label_map; 290 | std::vector label_list,label_temp; 291 | for(int g=0;gpoints[j].label; 303 | if (point_label>0) 304 | { 305 | points_label_.push_back(point_label); 306 | points_label_map[point_label].push_back(j); 307 | } 308 | } 309 | 310 | //Mapping the index between supervoxels, voxels, and points 311 | // points_label_map [label] 表示 一堆点号 312 | int temp_sv_num=0; 313 | for(int k=0;k0) 316 | { 317 | supervoxels_point_idx_.push_back(points_label_map[k]); 318 | supervoxels_label_.push_back(labeled_points_cloud->points[points_label_map[k][0]].label); 319 | // supervoxels_label_.push_back(k); 跟上面的结果应该一样.... 320 | temp_sv_num++; 321 | } 322 | } 323 | supervoxels_num_=temp_sv_num; 324 | 325 | //Test 326 | //Output colored cloud: method 1 327 | pcl::PointCloud::Ptr supervoxel_cloud (new pcl::PointCloud); 328 | pcl::PointXYZRGB temp_colored_point; 329 | std::vector color_map(3); 330 | srand(static_cast (time(0))); 331 | for (int k=0;k (rand()%256); 335 | color_map[1]=static_cast (rand()%256); 336 | color_map[2]=static_cast (rand()%256); 337 | 338 | for(int l=0;lsupervoxels_point_idx_[k].size ();l++) 339 | { 340 | temp_colored_point.x=this->points_cloud_->points[supervoxels_point_idx_[k][l]].x; 341 | temp_colored_point.y=this->points_cloud_->points[supervoxels_point_idx_[k][l]].y; 342 | temp_colored_point.z=this->points_cloud_->points[supervoxels_point_idx_[k][l]].z; 343 | temp_colored_point.r=color_map[0]; 344 | temp_colored_point.g=color_map[1]; 345 | temp_colored_point.b=color_map[2]; 346 | 347 | supervoxel_cloud->push_back(temp_colored_point); 348 | } 349 | } 350 | 351 | //// 点云漫游显示 352 | //boost::shared_ptr viewer(new pcl::visualization::PCLVisualizer("3D Viewer")); 353 | //viewer->setBackgroundColor(0, 0, 0); 354 | //viewer->addPointCloud(supervoxel_cloud, "supervoxel_cloud"); 355 | //while (!viewer->wasStopped()) 356 | //{ 357 | // viewer->spinOnce(100); 358 | //} 359 | } 360 | 361 | //Segmentation 362 | void 363 | segmentSupervoxelCloudWithGraphModel(float sig_a, float sig_b, float sig_l, float cut_thred, 364 | float sig_p, float sig_n, float sig_o, float sig_e, float sig_c, float sig_w) //Segmentation 365 | { 366 | //Parameter setting 367 | this->color_impt_=sig_a; 368 | this->spatial_impt_=sig_b; 369 | this->normal_impt_=sig_l; 370 | 371 | //Create supervoxels 372 | this->createSupervoxels(); 373 | 374 | //Attribute calculation 375 | this->calcualteSupervoxelCloudAttributes(); 376 | 377 | //Find adjacencies 378 | this->findAllSupervoxelAdjacency(); 379 | 380 | //Find neighbors 381 | this->findAllSupervoxelNeighbors(); 382 | 383 | //Connectivity calculation 384 | for(int i=0;isupervoxels_num_;i++) 385 | { 386 | //Judgement 387 | if(this->supervoxel_used_[i])//Supervoxel used or not 388 | { 389 | Eigen::MatrixXf supervoxel_adjgraph; 390 | std::vector supervoxel_connect; 391 | std::vector supervoxel_adjidx; 392 | 393 | //Get neighbors 394 | supervoxel_adjidx=this->getOneSupervoxelNeighbor(i); 395 | 396 | //Graph building 397 | supervoxel_adjgraph=this->buildAdjacencyGraph(supervoxel_adjidx, sig_p, sig_n, sig_o, sig_e, sig_c, sig_w); 398 | 399 | //Graph based segmentation 400 | supervoxel_connect=this->cutGraphSegmentation(cut_thred,supervoxel_adjgraph,supervoxel_adjidx); 401 | 402 | //Store the connection information 403 | supervoxels_connect_idx_.push_back(supervoxel_connect); 404 | 405 | } 406 | else 407 | { 408 | //Store the connection informaiton 409 | std::vector supervoxel_connect; 410 | supervoxels_connect_idx_.push_back(supervoxel_connect); 411 | } 412 | 413 | } 414 | 415 | //Cross validation 416 | this->crossValidation(); 417 | //Closest checking 418 | this->closestCheck(sig_p, sig_n, sig_o, sig_e, sig_c, sig_w); 419 | //Voxel Clustering 420 | this->clusteringSupervoxels(); 421 | } 422 | 423 | //Display 424 | void 425 | drawNormofVoxels(pcl::PolygonMesh::Ptr output_mesh) 426 | { 427 | //Setting 428 | std::vector polygons; 429 | pcl::PointCloud clouds_vertices; 430 | 431 | int spvoxel_ID; 432 | int points_size; 433 | int points_min=this->supervoxel_point_min_; 434 | 435 | //Set random color of points in this voxel 436 | pcl::PointXYZRGB colored_vertex; 437 | std::vector color_map(3); 438 | srand(static_cast (time(0))); 439 | color_map[0]=static_cast (rand()%256); 440 | color_map[1]=static_cast (rand()%256); 441 | color_map[2]=static_cast (rand()%256); 442 | 443 | //Tranverse all the supervoxels 444 | for(int i=0;isupervoxels_num_;i++) 445 | { 446 | int point_num=0; 447 | int point_idx=0; 448 | int node_depth=0; 449 | pcl::PointXYZ spvoxel_center; 450 | 451 | //Vertices of the supervoxel 452 | PCXYZRGBPtr spvoxel_vertices (new PCXYZRGB); 453 | spvoxel_vertices->points.resize(2); 454 | 455 | //Find the atttributes of the supervoxel 456 | spvoxel_ID=i; 457 | points_size=this->supervoxels_point_idx_[spvoxel_ID].size(); 458 | spvoxel_center=this->supervoxel_centroids_cloud_->points[spvoxel_ID]; 459 | 460 | //Traverse the points in node and color them 461 | if(points_size>points_min) 462 | { 463 | //Get the points in the voxel 464 | spvoxel_vertices->points[0].x=spvoxel_center.x;//X 465 | spvoxel_vertices->points[1].x=spvoxel_center.x+this->seed_resolution_*this->supervoxel_norms_[spvoxel_ID].normal_x; 466 | 467 | spvoxel_vertices->points[0].y=spvoxel_center.y;//Y 468 | spvoxel_vertices->points[1].y=spvoxel_center.y+this->seed_resolution_*this->supervoxel_norms_[spvoxel_ID].normal_y; 469 | 470 | spvoxel_vertices->points[0].z=spvoxel_center.z;//Z 471 | spvoxel_vertices->points[1].z=spvoxel_center.z+this->seed_resolution_*this->supervoxel_norms_[spvoxel_ID].normal_z; 472 | 473 | //Color 474 | for(int j=0;j<2;j++) 475 | { 476 | spvoxel_vertices->points[j].r=color_map[0]; 477 | spvoxel_vertices->points[j].g=color_map[1]; 478 | spvoxel_vertices->points[j].b=color_map[2]; 479 | } 480 | 481 | //Input vertices points 482 | for(int j=0;j<2;j++) 483 | { 484 | clouds_vertices.points.push_back(spvoxel_vertices->points[j]); 485 | } 486 | 487 | //Input vertices topology 488 | //center to norm 489 | pcl::Vertices vertice0; 490 | vertice0.vertices.push_back(i*2+0);vertice0.vertices.push_back(i*2+1);vertice0.vertices.push_back(i*2+0); 491 | output_mesh->polygons.push_back(vertice0); 492 | } 493 | } 494 | 495 | //Create polygonmesh 496 | pcl::toPCLPointCloud2(clouds_vertices,output_mesh->cloud); 497 | 498 | } 499 | 500 | void 501 | drawColorMapofPointsinVoxels(pcl::PointCloud::Ptr output_cloud) 502 | { 503 | //Setting 504 | int points_size; 505 | int points_min=this->voxel_points_min_; 506 | 507 | pcl::PointXYZ original_point; 508 | pcl::PointXYZRGB colored_point; 509 | std::vector color_map(3); 510 | srand(static_cast (time(0))); 511 | 512 | //VoxelBasedSegmentation::LeafNodeIterator iter_leaf (this); //Iteration index for all leaf nodes in the tree 513 | 514 | //Tranverse the octree voxel 515 | for(int i=0; ivoxels_num_;i++)//while (*++iter_leaf) 516 | { 517 | int point_num=0; 518 | int point_idx=0; 519 | 520 | //Set random color of points in this voxel 521 | color_map[0]=static_cast (rand()%256); 522 | color_map[1]=static_cast (rand()%256); 523 | color_map[2]=static_cast (rand()%256); 524 | 525 | std::vector index_vector; 526 | PCXYZPtr voxel_cloud (new PCXYZ); 527 | 528 | //Traverse the points in the voxel and color them 529 | bool temp_bool=true; 530 | int points_size=this->voxels_point_idx_[i].size(); 531 | 532 | //if(points_size>points_min) 533 | //{ 534 | //Get the points in the voxel 535 | while(point_numvoxels_point_idx_[i][point_num]; 538 | original_point=this->points_cloud_->points[point_idx];//Find original point 539 | 540 | colored_point.x=original_point.x; 541 | colored_point.y=original_point.y; 542 | colored_point.z=original_point.z; 543 | colored_point.r=color_map[0]; 544 | colored_point.g=color_map[1]; 545 | colored_point.b=color_map[2]; 546 | 547 | //Coloring point 548 | output_cloud->points.push_back(colored_point);//Put colored point in the cloud 549 | 550 | point_num++; 551 | } 552 | //} 553 | } 554 | 555 | output_cloud->width = (int)output_cloud->points.size(); 556 | output_cloud->height = 1; 557 | 558 | } 559 | 560 | void 561 | drawColorMapofPointsinSupervoxels(pcl::PointCloud::Ptr output_cloud) 562 | { 563 | //Setting 564 | int points_size; 565 | int points_min=this->voxel_points_min_; 566 | 567 | pcl::PointXYZ original_point; 568 | pcl::PointXYZRGB colored_point; 569 | std::vector color_map(3); 570 | srand(static_cast (time(0))); 571 | 572 | //Tranverse the octree voxel 573 | for(int i=0; isupervoxels_num_;i++) 574 | { 575 | int point_num=0; 576 | int point_idx=0; 577 | 578 | //Set random color of points in this voxel 579 | color_map[0]=static_cast (rand()%256); 580 | color_map[1]=static_cast (rand()%256); 581 | color_map[2]=static_cast (rand()%256); 582 | 583 | std::vector index_vector; 584 | PCXYZPtr voxel_cloud (new PCXYZ); 585 | 586 | //Traverse the points in the voxel and color them 587 | bool temp_bool=true; 588 | int points_size=this->supervoxels_point_idx_[i].size(); 589 | 590 | while(point_numsupervoxels_point_idx_[i][point_num]; 593 | original_point=this->points_cloud_->points[point_idx];//Find original point 594 | 595 | colored_point.x=original_point.x; 596 | colored_point.y=original_point.y; 597 | colored_point.z=original_point.z; 598 | colored_point.r=color_map[0]; 599 | colored_point.g=color_map[1]; 600 | colored_point.b=color_map[2]; 601 | 602 | //Coloring point 603 | output_cloud->points.push_back(colored_point);//Put colored point in the cloud 604 | 605 | point_num++; 606 | } 607 | } 608 | 609 | output_cloud->width = (int)output_cloud->points.size(); 610 | output_cloud->height = 1; 611 | } 612 | 613 | void 614 | drawColorMapofPointsinClusters(pcl::PointCloud::Ptr output_cloud) 615 | { 616 | //Geometry 617 | std::vector polygons; 618 | pcl::PointCloud clouds_vertices; 619 | std::vector use_or_not; 620 | 621 | //Setting 622 | int i=0; 623 | int points_size=0; 624 | int out_num=0; 625 | 626 | //Random color 627 | pcl::PointXYZRGB colored_point; 628 | pcl::PointXYZ original_point; 629 | std::vector color_map(3); 630 | srand(static_cast (time(0))); 631 | 632 | for (int m = 0; mclusters_supervoxel_idx_.size(); m++) 633 | { 634 | color_map[0] = static_cast (rand() % 256); 635 | color_map[1] = static_cast (rand() % 256); 636 | color_map[2] = static_cast (rand() % 256); 637 | 638 | int supervoxels_size = this->clusters_supervoxel_idx_[m].size(); 639 | 640 | for (int n= 0; nclusters_supervoxel_idx_[m][n]; 643 | //Traverse the points in node and color them 644 | int point_idx = 0; //Get the points in the voxel 645 | std::vector points_idx; 646 | points_idx = this->supervoxels_point_idx_[supervoxel_idx]; 647 | points_size = points_idx.size(); 648 | 649 | //Coloring 650 | for (int i = 0; ipoints_cloud_->points[points_idx[i]]; 653 | colored_point.x = original_point.x; 654 | colored_point.y = original_point.y; 655 | colored_point.z = original_point.z; 656 | colored_point.r = color_map[0]; 657 | colored_point.g = color_map[1]; 658 | colored_point.b = color_map[2]; 659 | output_cloud->push_back(colored_point); 660 | } 661 | } 662 | 663 | } 664 | } 665 | 666 | private: 667 | 668 | //Member variables 669 | int points_num_; //Num of input points 670 | int voxels_num_; //Num of segmented voxels 671 | int supervoxels_num_; //Num of generated supervoxels 672 | int clusters_num_; //Num of obtained clusters 673 | int segments_num_; 674 | int centers_num_; //Centers of all the voxels 675 | 676 | int voxel_points_num_; //Num of points in a voxel 677 | int supervoxel_points_num_; //Num of points in a super voxel 678 | int cluster_points_num_; //Num of points in a cluster 679 | 680 | float voxel_resolution_; //Size of the voxel 681 | float seed_resolution_; //Size of the supervoxel 682 | float adjacent_resolution_; //Radius of the small local graph 683 | float graph_resolution_; //Radius of the large global graph 684 | 685 | int voxel_points_max_; //Max num of points in a voxel 686 | int voxel_points_min_; //Min num of points in a voxel 687 | int voxel_adjacency_max_; //Max num of adjacent voxel 688 | int voxel_adjacency_min_; //Min num of adjacent voxel 689 | int supervoxel_adjacency_max_; //Max num of adjacent supervoxel 690 | int supervoxel_adjacency_min_; //Min num of adjacent supervoxel 691 | int supervoxel_voxel_max_; //Max num of voxels in a supervoxel 692 | int supervoxel_voxel_min_; //Min num of voxels in a supervoxel 693 | int supervoxel_point_max_; //Max num of points in a supervoxel 694 | int supervoxel_point_min_; //Min num of points in a supervoxel 695 | 696 | float min_x_, min_y_, min_z_, max_x_, max_y_, max_z_; 697 | float color_impt_, spatial_impt_, normal_impt_; 698 | 699 | std::vector task_vector_; 700 | 701 | Eigen::MatrixXf global_adjacency_matrix_; 702 | 703 | std::vector > voxel_centers_, supervoxel_centers_; //Center point of the voxels 704 | std::vector > voxel_centroids_, supervoxel_centroids_; //Centroid point of the voxels 705 | std::vector voxel_used_, supervoxel_used_, voxel_clustered_, supervoxel_clustered_; //The voxel has been clutered or not 706 | 707 | std::vector > supervoxel_features_; //FPFHs of all the supervoxels 708 | std::vector > supervoxel_eigens_; //Eigen features of all the supervoxels 709 | std::vector > supervoxel_colors_; //Colors of all the supervoxels 710 | std::vector supervoxel_norms_; //Norms of all the supervoxels 711 | std::vector supervoxel_volumes_; //Volumes of all the supervoxels 712 | 713 | std::vector > voxels_adjacency_idx_, supervoxels_adjacency_idx_, supervoxels_neighbor_idx_; //Idx of the adjacent voxels, adjacent supervoxels, neighboring supervoxels 714 | std::vector > voxels_connect_idx_, supervoxels_connect_idx_; //Idx of the connected voxels, supervoxels 715 | std::vector > voxels_point_idx_, supervoxels_point_idx_, clusters_supervoxel_idx_, clusters_point_idx_; //Idx of points, supervoxels, and cluster 716 | 717 | std::vector supervoxels_label_, voxels_label_, points_label_; //Labels of supervoxels and points 718 | 719 | pcl::KdTreeFLANN point_centers_kdtree_, voxel_centers_kdtree_, voxel_centroids_kdtree_, supervoxel_centers_kdtree_, supervoxel_centroids_kdtree_, cluster_centers_kdtree_; //kd tree 720 | 721 | PCXYZPtr points_cloud_, voxel_centers_cloud_, voxel_centroids_cloud_, supervoxel_centers_cloud_, supervoxel_centroids_cloud_, cluster_centers_cloud_; //PT Ptr of points and centers 722 | 723 | //Member functions 724 | void 725 | buildVoxelCentersKdtree() 726 | { 727 | //Input cloud of voxel centers 728 | this->voxel_centers_kdtree_.setInputCloud (this->voxel_centers_cloud_); 729 | 730 | //Input cloud of voxel centroids 731 | this->voxel_centroids_kdtree_.setInputCloud (this->voxel_centroids_cloud_); 732 | } 733 | 734 | void 735 | buildSupervoxelCentersKdtree() 736 | { 737 | //Input cloud of supervoxel centers 738 | //this->supervoxel_centers_kdtree_.setInputCloud (this->supervoxel_centers_cloud_); 739 | 740 | //Input cloud of supervoxel centroids 741 | this->supervoxel_centroids_kdtree_.setInputCloud (this->supervoxel_centroids_cloud_); 742 | } 743 | 744 | //Features 745 | pcl::PointXYZ 746 | calculateVoxelCentroid(PCXYZPtr input_cloud) 747 | { 748 | //Parameter setting 749 | pcl::PointXYZ output_centroid; 750 | int point_num=input_cloud->points.size(); 751 | float x_sum=0,y_sum=0,z_sum=0; 752 | 753 | for(int i=0;ipoints[i].x; 756 | y_sum=y_sum+input_cloud->points[i].y; 757 | z_sum=z_sum+input_cloud->points[i].z; 758 | } 759 | 760 | output_centroid.x=x_sum/point_num; 761 | output_centroid.y=y_sum/point_num; 762 | output_centroid.z=z_sum/point_num; 763 | 764 | return(output_centroid); 765 | } 766 | 767 | //Eight dimentional Eigen-based features (Weinmann et al. 2015 ISPRS Journal) 768 | std::vector 769 | calculateEigenFeatures(PCXYZPtr input_cloud)//Eigen features 770 | { 771 | //Parameter setting 772 | Eigen::Vector3f eig_values; 773 | Eigen::Matrix3f eig_vectors; 774 | Eigen::Matrix3f *cor_matrix=new Eigen::Matrix3f; 775 | std::vector output_features; 776 | 777 | int point_num=0; 778 | point_num=input_cloud->points.size(); 779 | float eig_e1=0,eig_e2=0,eig_e3=0; 780 | //float *features=new float[8]; 781 | 782 | // 自己构造好协方差矩阵就好, 特征值和特征向量的计算用Eigen库中自带就好 783 | //Weighted corvarance matrix 784 | //this->calculateWeightedCorvariance(input_cloud,cor_matrix); 785 | this->calculateCorvariance(input_cloud,cor_matrix); 786 | 787 | //EVD 788 | pcl::eigen33 (*cor_matrix, eig_vectors, eig_values); 789 | 790 | //Eigen values (normalized) 791 | //一共八维特征...完全按照 Weinmann 2015 ISPRS 文章上面的特征 792 | if(eig_values[0]==0 && eig_values[1]==0 && eig_values[2]==0) 793 | { 794 | for(int i=0;i<8;i++) 795 | { 796 | output_features.push_back(float(0)); 797 | } 798 | } 799 | else 800 | { 801 | //e1>e2>e3 (跑程序时检查下是否满足这个规律...) 802 | eig_e3=(float)eig_values[0]/sqrt(pow(eig_values[0],2)+pow(eig_values[1],2)+pow(eig_values[2],2)); 803 | eig_e2=(float)eig_values[1]/sqrt(pow(eig_values[0],2)+pow(eig_values[1],2)+pow(eig_values[2],2)); 804 | eig_e1=(float)eig_values[2]/sqrt(pow(eig_values[0],2)+pow(eig_values[1],2)+pow(eig_values[2],2)); 805 | 806 | 807 | //Feature calculation 808 | if(eig_e1==0) // e1 应该是最大的??? 809 | { 810 | output_features.push_back(float(0));//Linearity 811 | output_features.push_back(float(1));//Planarity 812 | output_features.push_back(float(0));//Scattering 813 | output_features.push_back(float(0));//Anisotropy 各向异性 814 | } 815 | else 816 | { 817 | output_features.push_back(float(eig_e1-eig_e2)/eig_e1);//Linearity 818 | output_features.push_back(float(eig_e2-eig_e3)/eig_e1);//Planarity 819 | output_features.push_back(float(eig_e3)/eig_e1);//Scattering or Sphericity 820 | output_features.push_back(float(eig_e1 - eig_e3) / eig_e1);//Anisotropy 821 | // 查 Weinmann (2015) ISPRS: 正确形式如上, 原错误代码 (e1 - e3) / e2 822 | } 823 | 824 | output_features.push_back(float(eig_e3)/(eig_e1+eig_e2+eig_e3));//Change of curvature or Surface variation 825 | // 查 Weinmann (2015) ISPRS: 正确形式如上, 原错误代码 float(eig_e1)/(eig_e1+eig_e2+eig_e3) 826 | 827 | if(eig_e1*eig_e2*eig_e3==0) 828 | { 829 | output_features.push_back(float(0));//Eigenentropy 830 | } 831 | else 832 | { 833 | output_features.push_back(-1*(eig_e1*log(eig_e1)+eig_e2*log(eig_e2)+eig_e3*log(eig_e3)));//Eigenentropy 834 | } 835 | 836 | output_features.push_back(eig_e1+eig_e2+eig_e3);//Sum of eigen values 837 | output_features.push_back(pow(float(eig_e1*eig_e2*eig_e3),float(1.0/3)));//Omnivariance 838 | } 839 | 840 | ////Test 841 | //std::cout<<"Eigen Features: "< 850 | calculateFPFHFeatures(PCXYZPtr input_cloud) 851 | { 852 | //Parameter setting 853 | std::vector output_features; 854 | pcl::FPFHEstimation fpfh_descriptor; 855 | pcl::PointCloud::Ptr input_norms (new pcl::PointCloud ()); 856 | pcl::PointCloud::Ptr candidate_cloud (new pcl::PointCloud()); 857 | pcl::Normal candidate_norm; 858 | pcl::PointXYZ candidate_point, sum_point, center_point; 859 | 860 | sum_point.x=0;sum_point.y=0;sum_point.z=0; 861 | 862 | int points_num=0; 863 | float norm_radius=this->voxel_resolution_; //Norm radius = voxel size 864 | float fpfh_radius=this->seed_resolution_; //FPFH radius = supervoxel size 865 | std::vector points_indices; 866 | 867 | //Building a kd tree 868 | pcl::KdTreeFLANN kdtree_cloud; 869 | kdtree_cloud.setInputCloud (input_cloud); 870 | 871 | //Calculate norms of voxels 872 | points_num=input_cloud->points.size(); 873 | for(int i=0;ipoints[i]; 877 | std::vector candidate_cloud_idx; 878 | std::vector candidate_cloud_dis; 879 | 880 | //Select candidate points 881 | if(kdtree_cloud.radiusSearch(candidate_point,norm_radius, candidate_cloud_idx, candidate_cloud_dis)>0) 882 | { 883 | for(size_t j=0;jpush_back(input_cloud->points[candidate_cloud_idx[j]]); 886 | } 887 | } 888 | 889 | //Calculate norm 890 | candidate_norm=this->calculateSupervoxelNorms(candidate_cloud); 891 | 892 | //Record norms 893 | input_norms->push_back(candidate_norm); 894 | points_indices.push_back(i);//Record the indices 895 | 896 | //Sum for the centroid point 897 | sum_point.x=sum_point.x+candidate_point.x; 898 | sum_point.y=sum_point.y+candidate_point.y; 899 | sum_point.z=sum_point.z+candidate_point.z; 900 | } 901 | 902 | //Centroid 903 | pcl::PointXYZ centroid_point; 904 | centroid_point.x=sum_point.x/points_num; 905 | centroid_point.y=sum_point.y/points_num; 906 | centroid_point.z=sum_point.z/points_num; 907 | 908 | //Center candidate selection 909 | std::vector center_point_idx; 910 | std::vector center_point_dis; 911 | kdtree_cloud.nearestKSearch(centroid_point,1,center_point_idx,center_point_dis); 912 | 913 | center_point=input_cloud->points[center_point_idx[0]]; 914 | 915 | //Calculate the FPFH features 916 | Eigen::MatrixXf hist_f1, hist_f2, hist_f3,hist_spfh; 917 | Eigen::VectorXf hist_sum; 918 | hist_f1=Eigen::MatrixXf::Zero(points_num,11); 919 | hist_f2=Eigen::MatrixXf::Zero(points_num,11); 920 | hist_f3=Eigen::MatrixXf::Zero(points_num,11); 921 | hist_spfh=Eigen::MatrixXf::Zero(points_num,33); 922 | hist_sum=Eigen::VectorXf::Zero(33); 923 | std::vector center_candidate_dis; 924 | for(int k=0;kpoints[k].x; 934 | temp_point.y=center_point.y-input_cloud->points[k].y; 935 | temp_point.z=center_point.z-input_cloud->points[k].z; 936 | 937 | temp_dis=sqrt(pow(temp_point.x,2)+pow(temp_point.y,2)+pow(temp_point.z,2)); 938 | center_candidate_dis.push_back(temp_dis); 939 | 940 | //Weighted result 941 | for(int m=0;m<11;m++) 942 | { 943 | 944 | if(temp_dis!=0) 945 | { 946 | hist_spfh(k,m)=hist_f1(k,m)/temp_dis; 947 | hist_spfh(k,m+11)=hist_f2(k,m)/temp_dis; 948 | hist_spfh(k,m+22)=hist_f3(k,m)/temp_dis; 949 | 950 | hist_sum(m)=hist_sum(m)+hist_spfh(k,m); 951 | hist_sum(m+11)=hist_sum(m+11)+hist_spfh(k,m+11); 952 | hist_sum(m+22)=hist_sum(m+22)+hist_spfh(k,m+22); 953 | } 954 | /*std::cout<0) 979 | { 980 | for(int n=0;n<33;n++) 981 | { 982 | output_features[n]/=sum_bin; 983 | } 984 | } 985 | return(output_features); 986 | } 987 | 988 | pcl::Normal 989 | calculateSupervoxelNorms(PCXYZPtr input_cloud) 990 | { 991 | //Parameter setting 992 | Eigen::Vector3f eig_values; 993 | Eigen::Matrix3f eig_vectors; 994 | Eigen::Matrix3f *cor_matrix=new Eigen::Matrix3f; 995 | pcl::Normal output_normal; 996 | pcl::PointXYZ view_point; // Why is [0, 0, 1.5]? 997 | 998 | int point_num=input_cloud->points.size(); 999 | //view_point.x=0-input_cloud->points[0].x; 1000 | //view_point.y=0-input_cloud->points[0].y; 1001 | //view_point.z=0-input_cloud->points[0].z; 1002 | view_point.x = 0 - input_cloud->points[0].x; 1003 | view_point.y = 0 - input_cloud->points[0].y; 1004 | view_point.z = 1.5 - input_cloud->points[0].z; 1005 | float eig_e1=0,eig_e2=0,eig_e3=0; 1006 | 1007 | //Normal corvarance matrix 1008 | this->calculateCorvariance(input_cloud,cor_matrix); 1009 | //Weighted corvarance matrix 1010 | //this->calculateWeightedCorvariance(input_cloud,cor_matrix); 1011 | //EVD 1012 | pcl::eigen33 (*cor_matrix, eig_vectors, eig_values); 1013 | 1014 | //Eigen values (e1 > e2 > e3) 1015 | eig_e1=eig_values[2]; 1016 | eig_e2=eig_values[1]; 1017 | eig_e3=eig_values[0]; 1018 | 1019 | //Feature calculation 1020 | //normal vector: the eigenvector corresponding to the smallest eigenvalue (eig_values[0]) of corvarance matrix 1021 | output_normal.normal_x=eig_vectors(0,0); 1022 | output_normal.normal_y=eig_vectors(1,0); 1023 | output_normal.normal_z=eig_vectors(2,0); 1024 | 1025 | //Direction judgement 1026 | //强制让 视向量 与 体素法向量 同向... 1027 | if((output_normal.normal_x*view_point.x+output_normal.normal_y*view_point.y+output_normal.normal_z*view_point.z)<0) 1028 | { 1029 | output_normal.normal_x=output_normal.normal_x*-1; 1030 | output_normal.normal_y=output_normal.normal_y*-1; 1031 | output_normal.normal_z=output_normal.normal_z*-1; 1032 | } 1033 | 1034 | //Test 1035 | //std::cout<<"Norms: "< 1043 | calculateSupervoxelColors(PCXYZPtr input_cloud) 1044 | { 1045 | //Parameter setting 1046 | std::vector output_colors; 1047 | 1048 | //Test 1049 | //std::cout<<"Norms: "<points.size()>0) 1064 | { 1065 | x_min=input_cloud->points[0].x; 1066 | x_max=input_cloud->points[0].x; 1067 | y_min=input_cloud->points[0].y; 1068 | y_max=input_cloud->points[0].y; 1069 | z_min=input_cloud->points[0].z; 1070 | z_max=input_cloud->points[0].z; 1071 | } 1072 | 1073 | for(int i=1;ipoints.size();i++) 1074 | { 1075 | x_temp=input_cloud->points[i].x; 1076 | y_temp=input_cloud->points[i].y; 1077 | z_temp=input_cloud->points[i].z; 1078 | 1079 | if(x_tempx_max) 1085 | { 1086 | x_max=x_temp; 1087 | } 1088 | 1089 | if(y_tempy_max) 1095 | { 1096 | y_max=y_temp; 1097 | } 1098 | 1099 | if(z_tempz_max) 1105 | { 1106 | z_max=z_temp; 1107 | } 1108 | } 1109 | 1110 | if(z_max==z_min) 1111 | { 1112 | output_size=(x_max-x_min)*(y_max-y_min); 1113 | } 1114 | else if (y_max==y_min) 1115 | { 1116 | output_size=(x_max-x_min)*(z_max-z_min); 1117 | } 1118 | else if (x_max==x_min) 1119 | { 1120 | output_size=(y_max-y_min)*(z_max-z_min); 1121 | } 1122 | else 1123 | { 1124 | output_size=(x_max-x_min)*(y_max-y_min)*(z_max-z_min); 1125 | } 1126 | return(output_size); 1127 | } 1128 | 1129 | void 1130 | setSupervoxelCentroid(int supervoxel_id, pcl::PointXYZ supervoxel_centroid) 1131 | { 1132 | this->supervoxel_centroids_.at(supervoxel_id)=supervoxel_centroid; 1133 | this->supervoxel_centroids_cloud_->points.push_back(supervoxel_centroid); 1134 | } 1135 | 1136 | void 1137 | setSupervoxelCenter(int supervoxel_id, pcl::PointXYZ supervoxel_center) 1138 | { 1139 | this->supervoxel_centers_.at(supervoxel_id)=supervoxel_center; 1140 | this->supervoxel_centers_cloud_->points.push_back(supervoxel_center); 1141 | } 1142 | 1143 | void 1144 | setSupervoxelNorms(int supervoxel_id, pcl::Normal supervoxel_norm) 1145 | { 1146 | this->supervoxel_norms_.at(supervoxel_id)=supervoxel_norm; 1147 | } 1148 | 1149 | void 1150 | setSupervoxelFeatures(int supervoxel_id, std::vector input_features) 1151 | { 1152 | //int features_num=input_features.size(); 1153 | this->supervoxel_features_[supervoxel_id].clear(); 1154 | //for(int i=0;isupervoxel_features_[supervoxel_id]=input_features; 1159 | 1160 | } 1161 | 1162 | void 1163 | setSupervoxelEigens(int supervoxel_id, std::vector input_eigens) 1164 | { 1165 | //int eigens_num=input_eigens.size(); 1166 | this->supervoxel_eigens_[supervoxel_id].clear(); 1167 | //for(int i=0;isupervoxel_eigens_[supervoxel_id]=input_eigens; 1172 | } 1173 | 1174 | void 1175 | setSupervoxelColors(int supervoxel_id, std::vector input_colors) 1176 | { 1177 | this->supervoxel_colors_.at(supervoxel_id)=input_colors; 1178 | } 1179 | 1180 | void 1181 | setSupervoxelVolume(int supervoxel_id, float input_vol) 1182 | { 1183 | this->supervoxel_volumes_.at(supervoxel_id)=input_vol; 1184 | } 1185 | 1186 | void 1187 | setSupervoxelChosen(int supervoxel_idx, int chosen_ornot) 1188 | { 1189 | if(chosen_ornot==0) 1190 | { 1191 | this->supervoxel_used_.push_back(true); 1192 | } 1193 | else 1194 | { 1195 | this->supervoxel_used_.push_back(false); 1196 | } 1197 | } 1198 | 1199 | void 1200 | setSupervoxelClustered(int supervoxel_idx, int clustered_ornot) 1201 | { 1202 | if(clustered_ornot==0) 1203 | { 1204 | this->supervoxel_clustered_.push_back(true); 1205 | } 1206 | else 1207 | { 1208 | this->supervoxel_clustered_.push_back(false); 1209 | } 1210 | } 1211 | 1212 | void 1213 | initialSupervoxelAttributes() 1214 | { 1215 | std::vector empty_attribute(1,0); 1216 | pcl::Normal empty_norm; 1217 | pcl::PointXYZ empty_point; 1218 | float empty_value; 1219 | 1220 | for(int i=0;isupervoxels_num_;i++) 1221 | { 1222 | supervoxel_features_.push_back(empty_attribute); //FPFH 1223 | supervoxel_eigens_.push_back(empty_attribute); //Eigens 1224 | supervoxel_colors_.push_back(empty_attribute); //Color 1225 | supervoxel_norms_.push_back(empty_norm); //Norm 1226 | supervoxel_centroids_.push_back(empty_point); //Centroid 1227 | supervoxel_centers_.push_back(empty_point); //Center 1228 | supervoxel_volumes_.push_back(empty_value); //Volume 1229 | } 1230 | 1231 | //Initialization of clouds 1232 | PCXYZPtr temp_cloud1 (new PCXYZ); 1233 | PCXYZPtr temp_cloud2 (new PCXYZ); 1234 | this->supervoxel_centroids_cloud_=temp_cloud1; 1235 | this->supervoxel_centers_cloud_=temp_cloud2; 1236 | } 1237 | 1238 | void 1239 | calcualteSupervoxelCloudAttributes() 1240 | { 1241 | //Parameters and settings 1242 | int sv_num = 0; 1243 | std::vector supervoxel_features; // FPFH, 33 values 1244 | std::vector supervoxel_eigens; // L, P, S, C, 4 values + the rest of the 5 eigen features from Dr. Martin Weinmann 1245 | std::vector supervoxel_colors; // R,G,B color, 3 values 1246 | pcl::Normal supervoxel_norm; // x,y,z, 3 values 1247 | pcl::PointXYZ supervoxel_centroid; // x,y,z, 3 values 1248 | float supervoxel_volume; // vol, 1 value 1249 | 1250 | //Initial feature 1251 | this->initialSupervoxelAttributes(); 1252 | 1253 | //Traverse all the supervoxels 1254 | int valid_supervoxels_num = 0; 1255 | for (int i=0;isupervoxels_num_;i++) 1256 | { 1257 | 1258 | int points_num=this->supervoxels_point_idx_[i].size(); 1259 | 1260 | valid_supervoxels_num++; 1261 | 1262 | PCXYZPtr supervoxel_cloud (new PCXYZ); 1263 | 1264 | //Get the points in this supervoxel 1265 | for(int j=0;jpush_back(this->points_cloud_->points[this->supervoxels_point_idx_[i][j]]); 1268 | } 1269 | 1270 | //Spatial position 1271 | supervoxel_centroid=this->calculateVoxelCentroid(supervoxel_cloud); 1272 | this->setSupervoxelCentroid(i,supervoxel_centroid); 1273 | this->setSupervoxelCenter(i, supervoxel_centroid); 1274 | 1275 | //Normal vector 1276 | supervoxel_norm=this->calculateSupervoxelNorms(supervoxel_cloud); 1277 | this->setSupervoxelNorms(i, supervoxel_norm); 1278 | 1279 | //Eigen feature 1280 | supervoxel_eigens=this->calculateEigenFeatures(supervoxel_cloud); 1281 | this->setSupervoxelEigens(i, supervoxel_eigens); 1282 | 1283 | //Volume 1284 | supervoxel_volume=this->calculateSupervoxelVolume(supervoxel_cloud); 1285 | this->setSupervoxelVolume(i,supervoxel_volume); 1286 | 1287 | //Set supervoxel chosen or not 1288 | this->setSupervoxelChosen(i, 0); 1289 | 1290 | //Set supervoxel clustered or not 1291 | this->setSupervoxelClustered(i,1); 1292 | 1293 | //Test 1294 | //std::cout<<"Attributes of SV: "<supervoxel_used_[i]< points_id_support; // Neighbors within radius search 1318 | std::vector points_dis_support; // Distance of these neighbors 1319 | 1320 | int num_support=input_cloud->points.size(); //Num of input point 1321 | int num_support_min=3; 1322 | float point_dis=0; 1323 | float sum_dis=0; 1324 | 1325 | //Tranverse in the support region 1326 | if(num_support>num_support_min) 1327 | { 1328 | for (size_t i = 0; i < num_support; i++) 1329 | { 1330 | point_can=input_cloud->points[i]; 1331 | sum_coor[0]=sum_coor[0]+point_can.x; 1332 | sum_coor[1]=sum_coor[1]+point_can.y; 1333 | sum_coor[2]=sum_coor[2]+point_can.z; 1334 | } 1335 | 1336 | //key point 1337 | key_coor[0]=sum_coor[0]/num_support; 1338 | key_coor[1]=sum_coor[1]/num_support; 1339 | key_coor[2]=sum_coor[2]/num_support; 1340 | 1341 | for (size_t j = 0; j < num_support; j++) 1342 | { 1343 | //Get candidate point in support 1344 | point_can=input_cloud->points[j]; 1345 | can_coor[0]=point_can.x; 1346 | can_coor[1]=point_can.y; 1347 | can_coor[2]=point_can.z; 1348 | 1349 | //Coordinate differences 1350 | diff_coor=can_coor-key_coor; 1351 | 1352 | //Distance between the candidate and key points 1353 | point_dis=diff_coor.norm(); 1354 | sum_dis=sum_dis+point_dis; 1355 | 1356 | //CTC 1357 | cor_single=point_dis*diff_coor*diff_coor.transpose(); 1358 | cor_sum=cor_sum+cor_single; 1359 | } 1360 | } 1361 | else 1362 | { 1363 | sum_dis=1; 1364 | cor_sum=Eigen::Matrix3f::Zero(3,3); 1365 | } 1366 | 1367 | //Final covariance matrix 1368 | cor_sum=cor_sum/sum_dis; 1369 | *output_cor=cor_sum; 1370 | } 1371 | 1372 | void 1373 | calculateCorvariance(PCXYZPtr input_cloud, Eigen::Matrix3f *output_cor) 1374 | { 1375 | //Settings 1376 | pcl::PointXYZ point_can; // Candidate point 1377 | Eigen::Vector3f key_coor=Eigen::Vector3f::Zero(3,1); // Coordinates of key point 1378 | Eigen::Vector3f can_coor=Eigen::Vector3f::Zero(3,1); // Coordinates of candidate point 1379 | Eigen::Vector3f sum_coor=Eigen::Vector3f::Zero(3,1); // Sum of coordinates 1380 | Eigen::Vector3f diff_coor=Eigen::Vector3f::Zero(3,1); // Coordinates difference 1381 | Eigen::Matrix3f cor_single=Eigen::Matrix3f::Zero(3,3); // CTC for a point 1382 | Eigen::Matrix3f cor_sum=Eigen::Matrix3f::Zero(3,3); // Sum of all CTC 1383 | 1384 | std::vector points_id_support; // Neighbors within radius search 1385 | std::vector points_dis_support; // Distance of these neighbors 1386 | 1387 | int num_support=input_cloud->points.size(); //Num of input point 1388 | int num_support_min=3; 1389 | float point_dis=0; 1390 | //float sum_dis=0; 1391 | 1392 | //Tranverse in the support region 1393 | if(num_support>num_support_min) 1394 | { 1395 | for (size_t i = 0; i < num_support; i++) 1396 | { 1397 | point_can=input_cloud->points[i]; 1398 | sum_coor[0]=sum_coor[0]+point_can.x; 1399 | sum_coor[1]=sum_coor[1]+point_can.y; 1400 | sum_coor[2]=sum_coor[2]+point_can.z; 1401 | } 1402 | 1403 | //key point 1404 | key_coor[0]=sum_coor[0]/num_support; 1405 | key_coor[1]=sum_coor[1]/num_support; 1406 | key_coor[2]=sum_coor[2]/num_support; 1407 | 1408 | for (size_t j = 0; j < num_support; j++) 1409 | { 1410 | //Get candidate point in support 1411 | point_can=input_cloud->points[j]; 1412 | can_coor[0]=point_can.x; 1413 | can_coor[1]=point_can.y; 1414 | can_coor[2]=point_can.z; 1415 | 1416 | //Coordinate differences 1417 | diff_coor=can_coor-key_coor; 1418 | 1419 | //CTC 1420 | cor_single=diff_coor*diff_coor.transpose(); 1421 | cor_sum=cor_sum+cor_single; 1422 | } 1423 | 1424 | //理论上应该除以点的数量 1425 | cor_sum = cor_sum / num_support; 1426 | } 1427 | else 1428 | { 1429 | //sum_dis=1; 1430 | cor_sum=Eigen::Matrix3f::Zero(3,3); 1431 | } 1432 | 1433 | //Final covariance matrix 1434 | *output_cor=cor_sum; 1435 | } 1436 | 1437 | //Graph 1438 | void 1439 | findAllSupervoxelAdjacency() 1440 | { 1441 | //Build kd-tree of supervoxels' centroids 1442 | this->buildSupervoxelCentersKdtree(); 1443 | 1444 | //Search radius 1445 | double search_radius; 1446 | search_radius=this->adjacent_resolution_; //Here, the adjacent ones mean the ones that really closely stick to the center one 1447 | 1448 | //Tranverse all the centers in the tree 1449 | pcl::PointXYZ search_point; 1450 | for (int i = 0; isupervoxels_num_; i++) 1451 | { 1452 | //Searching points 1453 | search_point=this->supervoxel_centroids_cloud_->points[i]; 1454 | 1455 | //Defining search radius 1456 | std::vector pointIdxRadiusSearch; 1457 | std::vector pointRadiusSquaredDistance; 1458 | 1459 | //Searching results 1460 | //Achtung!!! The FLANN 1.7.1 has a known bug for the search_radius function! Use FLANN 1.8.0! 1461 | if ( supervoxel_centroids_kdtree_.radiusSearch (search_point, search_radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0 ) 1462 | { 1463 | //Add elements in the vector 1464 | this->supervoxels_adjacency_idx_.push_back(vector(pointIdxRadiusSearch.size()+1,0)); 1465 | this->supervoxels_adjacency_idx_[i][0]=pointIdxRadiusSearch.size();//The first element records the num of adjacencies 1466 | 1467 | for (size_t j = 1; j < pointIdxRadiusSearch.size()+1; j++)//The rest records the idx 1468 | { 1469 | //Records 1470 | this->supervoxels_adjacency_idx_[i][j]=pointIdxRadiusSearch[j-1]; 1471 | //std::cout << " " << supervoxels_adjacency_idx_[i][j]<< std::endl; 1472 | } 1473 | } 1474 | } 1475 | } 1476 | 1477 | void 1478 | findAllSupervoxelNeighbors() 1479 | { 1480 | //Build kd-tree of supervoxels' centroids 1481 | this->buildSupervoxelCentersKdtree(); 1482 | 1483 | //Search radius 1484 | double search_radius; 1485 | search_radius=this->graph_resolution_; 1486 | 1487 | //Tranverse all the centers in the tree 1488 | pcl::PointXYZ search_point; 1489 | for (int i = 0; isupervoxels_num_; i++) 1490 | { 1491 | 1492 | //Searching points 1493 | search_point=this->supervoxel_centroids_cloud_->points[i]; 1494 | 1495 | //Defining search radius 1496 | std::vector pointIdxRadiusSearch; 1497 | std::vector pointRadiusSquaredDistance; 1498 | 1499 | //Searching results 1500 | //Achtung!!! The FLANN 1.7.1 has a known bug for the search_radius function! Use FLANN 1.8.0! 1501 | if ( supervoxel_centroids_kdtree_.radiusSearch (search_point, search_radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0 ) 1502 | { 1503 | //Add elements in the vector 1504 | this->supervoxels_neighbor_idx_.push_back(vector(pointIdxRadiusSearch.size()+1,0)); 1505 | this->supervoxels_neighbor_idx_[i][0]=pointIdxRadiusSearch.size();//The first element records the num of adjacencies 1506 | 1507 | for (size_t j = 1; j < pointIdxRadiusSearch.size()+1; j++)//The rest records the idx 1508 | { 1509 | //Records 1510 | this->supervoxels_neighbor_idx_[i][j]=pointIdxRadiusSearch[j-1]; 1511 | //std::cout << " " << supervoxels_neighbor_idx_[i][j]<< std::endl; 1512 | } 1513 | } 1514 | else 1515 | { 1516 | //Add elements in the vector 1517 | this->supervoxels_neighbor_idx_.push_back(vector(pointIdxRadiusSearch.size() + 1, 0)); 1518 | this->supervoxels_neighbor_idx_[i][0] = 1; 1519 | } 1520 | } 1521 | } 1522 | 1523 | std::vector 1524 | getOneSupervoxelAdjacency(int supervoxel_id) 1525 | { 1526 | //Test 1527 | //std::cout << " "<< std::endl; 1528 | //std::cout << " Find adjacent supervoxels of supervoxel "<< supervoxel_id < adjacency_idx; 1531 | 1532 | //Traverse 1533 | for (size_t j = 1; j < this->supervoxels_adjacency_idx_[supervoxel_id][0]+1; j++)//Find the idx of recorded adjacent supervoxels 1534 | { 1535 | //Records 1536 | //std::cout << " " << supervoxels_adjacency_idx_[supervoxel_id][j]<< std::endl; 1537 | adjacency_idx.push_back(this->supervoxels_adjacency_idx_[supervoxel_id][j]); 1538 | //std::cout << " " << adjacency_idx[j-1]<< std::endl; 1539 | } 1540 | 1541 | return(adjacency_idx); 1542 | } 1543 | 1544 | std::vector 1545 | getOneSupervoxelNeighbor(int supervoxel_id) 1546 | { 1547 | //Test 1548 | std::cout << " "<< std::endl; 1549 | std::cout << " Find neighbors of supervoxel "<< supervoxel_id < neighbor_idx; 1552 | 1553 | //Traverse 1554 | for (size_t j = 1; j < this->supervoxels_neighbor_idx_[supervoxel_id][0]+1; j++)//Find the idx of recorded neighboring supervoxels 1555 | { 1556 | //Records 1557 | //std::cout << " " << supervoxels_neighbor_idx_[supervoxel_id][j]<< std::endl; 1558 | neighbor_idx.push_back(this->supervoxels_neighbor_idx_[supervoxel_id][j]); 1559 | //std::cout << " " << neighbor_idx[j-1]<< std::endl; 1560 | } 1561 | 1562 | return(neighbor_idx); 1563 | } 1564 | 1565 | Eigen::MatrixXf 1566 | buildAdjacencyGraph(std::vector spvoxels_idx, float sig_p, float sig_n, float sig_o, float sig_e, float sig_c, float sig_w)//Building simple adjacency Graph: static graph only connectivity considered 1567 | { 1568 | //Settings 1569 | Eigen::MatrixXf adj_matrix; //Adjacency in a n*n*n cube 1570 | std::vector spvoxel1_position, spvoxel1_normal, spvoxel1_eigen; 1571 | std::vector spvoxel2_position, spvoxel2_normal, spvoxel2_eigen; 1572 | pcl::PointXYZ temp_center, empty_center; 1573 | pcl::Normal temp_norm, empty_norm; 1574 | float spvoxel1_vol,spvoxel2_vol; 1575 | 1576 | std::vector dist_all; //Affinity calculated from different cues 1577 | 1578 | double similarity_spvoxels=0; 1579 | int neighbor_num=0; 1580 | empty_center.x=0;empty_center.y=0;empty_center.z=0; 1581 | empty_norm.normal_x=0;empty_norm.normal_y=0;empty_norm.normal_z=0; 1582 | 1583 | //Find idx of adjacent voxels 1584 | neighbor_num=spvoxels_idx.size(); 1585 | 1586 | //Initialization 1587 | adj_matrix=Eigen::MatrixXf::Zero(neighbor_num,neighbor_num); 1588 | 1589 | //Assigning 1590 | for (int i=0;isupervoxel_centroids_[spvoxels_idx[i]]; 1596 | temp_norm=this->supervoxel_norms_[spvoxels_idx[i]]; 1597 | 1598 | if(temp_center.x!=empty_center.x && temp_center.y!=empty_center.y && temp_center.z!=empty_center.z) 1599 | { 1600 | spvoxel1_position.push_back(temp_center.x); 1601 | spvoxel1_position.push_back(temp_center.y); 1602 | spvoxel1_position.push_back(temp_center.z); 1603 | } 1604 | else 1605 | { 1606 | spvoxel1_position.push_back(0); 1607 | } 1608 | 1609 | if(temp_norm.normal_x!=empty_norm.normal_x && temp_norm.normal_y!=empty_norm.normal_y && temp_norm.normal_z!=empty_norm.normal_z) 1610 | { 1611 | spvoxel1_normal.push_back(temp_norm.normal_x); 1612 | spvoxel1_normal.push_back(temp_norm.normal_y); 1613 | spvoxel1_normal.push_back(temp_norm.normal_z); 1614 | } 1615 | else 1616 | { 1617 | spvoxel1_normal.push_back(0); 1618 | } 1619 | 1620 | spvoxel1_eigen=this->supervoxel_eigens_[spvoxels_idx[i]]; 1621 | spvoxel1_vol=this->supervoxel_volumes_[spvoxels_idx[i]]; 1622 | 1623 | for(int j=0;jsupervoxel_centroids_[spvoxels_idx[j]]; 1629 | temp_norm=this->supervoxel_norms_[spvoxels_idx[j]]; 1630 | 1631 | if(temp_center.x!=empty_center.x && temp_center.y!=empty_center.y && temp_center.z!=empty_center.z) 1632 | { 1633 | spvoxel2_position.push_back(temp_center.x); 1634 | spvoxel2_position.push_back(temp_center.y); 1635 | spvoxel2_position.push_back(temp_center.z); 1636 | } 1637 | else 1638 | { 1639 | spvoxel2_position.push_back(0); 1640 | } 1641 | 1642 | if(temp_norm.normal_x!=empty_norm.normal_x && temp_norm.normal_y!=empty_norm.normal_y && temp_norm.normal_z!=empty_norm.normal_z) 1643 | { 1644 | spvoxel2_normal.push_back(temp_norm.normal_x); 1645 | spvoxel2_normal.push_back(temp_norm.normal_y); 1646 | spvoxel2_normal.push_back(temp_norm.normal_z); 1647 | } 1648 | else 1649 | { 1650 | spvoxel2_normal.push_back(0); 1651 | } 1652 | 1653 | spvoxel2_eigen=this->supervoxel_eigens_[spvoxels_idx[j]]; 1654 | spvoxel2_vol=this->supervoxel_volumes_[spvoxels_idx[j]]; 1655 | 1656 | //Similarity measuring 1657 | if(i!=j) 1658 | { 1659 | dist_all=measuringDistance(spvoxel1_position, spvoxel2_position, spvoxel1_normal, spvoxel2_normal, spvoxel1_eigen, spvoxel2_eigen); 1660 | 1661 | similarity_spvoxels=distanceWeight(dist_all, sig_p, sig_n, sig_o, sig_e, sig_c, sig_w); 1662 | 1663 | adj_matrix(i,j)=similarity_spvoxels; 1664 | } 1665 | else 1666 | { 1667 | adj_matrix(i,j)=1; 1668 | } 1669 | 1670 | } 1671 | } 1672 | 1673 | return(adj_matrix); 1674 | } 1675 | 1676 | Eigen::MatrixXf 1677 | findRowMemberMatrix(Eigen::MatrixXf input_matrix, int row_idx) 1678 | { 1679 | Eigen::MatrixXf output_matrix; 1680 | int matrix_size=input_matrix.rows(); 1681 | output_matrix=Eigen::MatrixXf::Zero(matrix_size-1,1); 1682 | 1683 | //Assigning 1684 | int j=0; 1685 | for(int i=0;i > 1698 | supervoxelAttributes(int sv_idx) 1699 | { 1700 | //Parameters and settings 1701 | std::vector > sv_attributes; 1702 | std::vector sv_position, sv_normal, sv_eigen, sv_features, sv_colors, sv_vol;; //Attributes of supervoxels 1703 | pcl::PointXYZ temp_center, empty_center; 1704 | pcl::Normal temp_norm, empty_norm; 1705 | 1706 | empty_center.x=0;empty_center.y=0;empty_center.z=0; 1707 | empty_norm.normal_x=0;empty_norm.normal_y=0;empty_norm.normal_z=0; 1708 | 1709 | temp_center=this->supervoxel_centroids_[sv_idx]; 1710 | temp_norm=this->supervoxel_norms_[sv_idx]; 1711 | 1712 | //Position 1713 | if(temp_center.x!=empty_center.x && temp_center.y!=empty_center.y && temp_center.z!=empty_center.z) 1714 | { 1715 | sv_position.push_back(temp_center.x); 1716 | sv_position.push_back(temp_center.y); 1717 | sv_position.push_back(temp_center.z); 1718 | } 1719 | else 1720 | { 1721 | sv_position.push_back(0); 1722 | } 1723 | //Normal 1724 | if(temp_norm.normal_x!=empty_norm.normal_x && temp_norm.normal_y!=empty_norm.normal_y && temp_norm.normal_z!=empty_norm.normal_z) 1725 | { 1726 | sv_normal.push_back(temp_norm.normal_x); 1727 | sv_normal.push_back(temp_norm.normal_y); 1728 | sv_normal.push_back(temp_norm.normal_z); 1729 | } 1730 | else 1731 | { 1732 | sv_normal.push_back(0); 1733 | } 1734 | 1735 | 1736 | //Eigen 1737 | sv_eigen=this->supervoxel_eigens_[sv_idx]; 1738 | //Feature 1739 | sv_features=this->supervoxel_features_[sv_idx]; 1740 | //Color 1741 | sv_colors=this->supervoxel_colors_[sv_idx]; 1742 | //Volumn 1743 | sv_vol.push_back(this->supervoxel_volumes_[sv_idx]); 1744 | 1745 | //Assigning 1746 | sv_attributes.push_back(sv_position); //0 1747 | sv_attributes.push_back(sv_normal); //1 1748 | sv_attributes.push_back(sv_eigen); //2 1749 | sv_attributes.push_back(sv_features); //3 1750 | sv_attributes.push_back(sv_colors); //4 1751 | sv_attributes.push_back(sv_vol); //5 1752 | return(sv_attributes); 1753 | } 1754 | 1755 | //Graphical method 1756 | std::vector 1757 | measuringDistance(std::vector v1_center, std::vector v2_center, std::vector v1_norm, std::vector v2_norm, std::vector v1_eigen, std::vector v2_eigen) 1758 | { 1759 | //Parameters and settings 1760 | std::vector similarity_dist; 1761 | 1762 | // 初始化一个很大的值, 这样当碰到很小的点集时, 可以直接赋很大的距离值给这些变量.... 1763 | float dist_space = 100; //S: space distance 1764 | float dist_angle = 100; //A: norm angles 1765 | float dist_stair = 100; //T: stair effect 1766 | float dist_eigen = 100; //E: eigen feature 1767 | float dist_convx = 100; //C: convex 1768 | 1769 | float dist_v1_v2=0,dist_v1=0,dist_v2=0,dist_o1=0,dist_o2=0; 1770 | float cos_v1_dist=0, cos_v2_dist=0,cos_v1_v2=0, cos_d_s=0; 1771 | float dist_cos_dist1=0,dist_cos_dist2=0, thred_singular=0, max_singular=0; 1772 | double a_1=0, a_2=0, a_1_2=0, a_d_s=0, a_d_s1=0, a_d_s2=0, PI=3.1415926; 1773 | std::vector norm_v1_v2, product_v1_v2, norm_v2_v1; 1774 | 1775 | //Spacial distance 1776 | if(v1_center.size()>1 && v2_center.size()>1) 1777 | { 1778 | dist_v1_v2=sqrt(pow(v1_center[0]-v2_center[0],2)+pow(v1_center[1]-v2_center[1],2)+pow(v1_center[2]-v2_center[2],2)); 1779 | dist_space=dist_v1_v2; 1780 | 1781 | if(dist_v1_v2!=0) 1782 | { 1783 | norm_v1_v2.clear(); 1784 | norm_v1_v2.push_back((v1_center[0]-v2_center[0])/dist_v1_v2); 1785 | norm_v1_v2.push_back((v1_center[1]-v2_center[1])/dist_v1_v2); 1786 | norm_v1_v2.push_back((v1_center[2]-v2_center[2])/dist_v1_v2); 1787 | 1788 | norm_v2_v1.clear(); 1789 | norm_v2_v1.push_back((v2_center[0]-v1_center[0])/dist_v1_v2); 1790 | norm_v2_v1.push_back((v2_center[1]-v1_center[1])/dist_v1_v2); 1791 | norm_v2_v1.push_back((v2_center[2]-v1_center[2])/dist_v1_v2); 1792 | 1793 | // X1×X2 1794 | product_v1_v2.clear(); 1795 | product_v1_v2.push_back(v1_center[1]*v2_center[2]-v1_center[2]*v2_center[1]); 1796 | product_v1_v2.push_back(v1_center[2]*v2_center[0]-v1_center[0]*v2_center[2]); 1797 | product_v1_v2.push_back(v1_center[0]*v2_center[1]-v1_center[1]*v2_center[0]); 1798 | } 1799 | } 1800 | 1801 | //Normal vectors angle, stair-like distance, and convex 1802 | if(v1_norm.size()>1 && v2_norm.size()>1) 1803 | { 1804 | if (dist_v1_v2 != 0) 1805 | { 1806 | cos_v1_v2 = (v1_norm[0] * v2_norm[0] + v1_norm[1] * v2_norm[1] + v1_norm[2] * v2_norm[2]); // 1807 | cos_v1_dist = (v1_norm[0] * norm_v1_v2[0] + v1_norm[1] * norm_v1_v2[1] + v1_norm[2] * norm_v1_v2[2]); // 1808 | cos_v2_dist = (v2_norm[0] * norm_v1_v2[0] + v2_norm[1] * norm_v1_v2[1] + v2_norm[2] * norm_v1_v2[2]); // 1809 | cos_d_s = (product_v1_v2[0] * norm_v1_v2[0] + product_v1_v2[1] * norm_v1_v2[1] + product_v1_v2[2] * norm_v1_v2[2]); // 1810 | 1811 | a_1 = acos(cos_v1_dist); // alpha1 1812 | a_2 = acos(cos_v2_dist); // alpha2 1813 | a_1_2 = acos(cos_v1_v2); 1814 | 1815 | a_d_s1 = acos(cos_d_s); 1816 | a_d_s2 = PI - a_d_s1; 1817 | 1818 | dist_angle = acos(cos_v1_v2);// norm angle 1819 | 1820 | dist_v1 = v1_norm[0] * v1_center[0] + v1_norm[1] * v1_center[1] + v1_norm[2] * v1_center[2]; // = D1 1821 | dist_v2 = v2_norm[0] * v2_center[0] + v2_norm[1] * v2_center[1] + v2_norm[2] * v2_center[2]; // = D2 1822 | dist_o1 = v1_norm[0] * v2_center[0] + v1_norm[1] * v2_center[1] + v1_norm[2] * v2_center[2]; // 1823 | dist_o2 = v2_norm[0] * v1_center[0] + v2_norm[1] * v1_center[1] + v2_norm[2] * v1_center[2]; // 1824 | 1825 | dist_stair = sqrt(pow((dist_o1 - dist_v1), 2) + pow((dist_o2 - dist_v2), 2)); // sqrt(( - D1)^2 + ( - D2)^2) 1826 | } 1827 | else 1828 | { 1829 | dist_stair = 0; 1830 | } 1831 | 1832 | //Singular judgement 1833 | double temp_a=0.5, temp_off=PI/6, 1834 | max_singular=PI/2; 1835 | thred_singular=(float)max_singular/(1+exp(-1*temp_a*(a_1_2-temp_off))); 1836 | 1837 | a_d_s=a_d_s1; 1838 | if(a_d_s1>a_d_s2) 1839 | { 1840 | a_d_s=a_d_s2; 1841 | } 1842 | 1843 | if(a_d_s>thred_singular) //closure: convexity 1844 | { 1845 | dist_convx = abs(a_1 - a_2); 1846 | } 1847 | else 1848 | { 1849 | dist_convx = PI; 1850 | } 1851 | } 1852 | 1853 | //Eigen features similarity (这里来看其实就用点积的方法,并没有用 histogram intersection kernel ) 1854 | if(v1_eigen.size()>1 && v2_eigen.size()>1) 1855 | { 1856 | float eigen_diff=0, eigen_cos=0, eigen_abs1=0, eigen_abs2=0; 1857 | for(int i=0;i dist_all, float sig_p, float sig_n, float sig_o, float sig_e, float sig_c, float sig_weight) 1882 | { 1883 | //Minkovski distance measure (Chebychev distance) 1884 | 1885 | //Settings 1886 | float similarity_weight=0; 1887 | float similarity_dist=0; 1888 | float dist_space = 1; //Space 1889 | float dist_angle = 1; //Angle 1890 | float dist_stair = 1; //sTair 1891 | float dist_eigen = 1; //Eigen 1892 | float dist_convx = 1; //Convex 1893 | 1894 | dist_space = dist_all[0]; 1895 | dist_angle = dist_all[1]; 1896 | dist_stair = dist_all[2]; 1897 | dist_eigen = dist_all[3]; 1898 | dist_convx = dist_all[4]; 1899 | 1900 | similarity_dist = sqrt(pow(dist_space, 2) / sig_p + pow(dist_angle, 2) / sig_n + pow(dist_eigen, 2) / sig_e + pow(dist_stair, 2) / sig_o); 1901 | 1902 | similarity_weight=exp(-0.5*similarity_dist/pow(sig_weight,2)); 1903 | 1904 | return(similarity_weight); 1905 | } 1906 | 1907 | //Graph segmentation 1908 | std::vector 1909 | cutGraphSegmentation(float thrd_value, Eigen::MatrixXf graph_matrix, std::vector voxel_adjidx) //Efficient graph segmentation algorithm 1910 | { 1911 | //Parameters and settings 1912 | int graph_size=0, weight_size=0; 1913 | float initial_thrd=1; 1914 | graph_size=graph_matrix.rows(); 1915 | weight_size=graph_size*graph_size; 1916 | //std::vector weight_array; 1917 | std::vector connected_voxels; // original_idx, sorted_idx; 1918 | graph_matrix.resize(1,weight_size); 1919 | 1920 | // Sort, descending using sort function 1921 | std::vector weight_index_array; 1922 | Weight_Index temp; 1923 | for (int i = 0; i < weight_size; i++) 1924 | { 1925 | temp.Weight = graph_matrix(0, i); 1926 | temp.Index = i; 1927 | weight_index_array.push_back(temp); 1928 | } 1929 | sort(weight_index_array.begin(), weight_index_array.end(), godown); // riseup 1930 | 1931 | //Sort, descending （bug: sorted_idx is not correct） 1932 | /*for (int i = 0; i < weight_size; i++) 1933 | { 1934 | original_idx.push_back(i); 1935 | sorted_idx.push_back(i); 1936 | weight_array.push_back(graph_matrix(0, i)); 1937 | } 1938 | int c_max,i_temp; 1939 | float w_temp; 1940 | for (int i = 0; i < weight_size - 1; i++) 1941 | { 1942 | c_max = i; 1943 | for (int j = i + 1; j < weight_size; j++) 1944 | { 1945 | if (weight_array[c_max] < weight_array[j]) 1946 | { 1947 | c_max = j; 1948 | } 1949 | } 1950 | i_temp = original_idx[c_max]; 1951 | sorted_idx[i]=i_temp; 1952 | sorted_idx[c_max]=original_idx[i]; 1953 | 1954 | w_temp = weight_array[c_max]; 1955 | weight_array[c_max] = weight_array[i]; 1956 | weight_array[i] = w_temp; 1957 | }*/ 1958 | 1959 | //Merging 1960 | std::vector seg_int; 1961 | std::vector< std::vector > seg_ver_idx; 1962 | std::vector seg_size, ver_seg_idx, vec_temp; 1963 | for (int i = 0; i < graph_size; i++) 1964 | { 1965 | vec_temp.clear();//Temp 1966 | vec_temp.push_back(i); 1967 | 1968 | seg_ver_idx.push_back(vec_temp); // segment has which vertice, if the vertice has been merged, than empty 1969 | ver_seg_idx.push_back(i); // vertice belong to which segment 1970 | seg_size.push_back(1); // How many vertex in the segment 1971 | seg_int.push_back(initial_thrd); // Initial of mint 1972 | } 1973 | 1974 | // 边权值从大到小进行合并... 1975 | // seg_ver_idx [面号]: 当前面包含的所有点 1976 | // ver_seg_idx [点号]: 当前点所属的面片 1977 | // seg_size [面号]: 当前面具有点的数量 1978 | // seg_int [面号]: 当前面的异质性测度(初始值是 1, ) 1979 | for (int i = 0; i < weight_size; i++) 1980 | { 1981 | float w_temp, thred_temp; 1982 | int v1_idx, v2_idx, mint_idx, mint_not_idx; 1983 | v1_idx = floor(double(weight_index_array[i].Index / graph_size)); 1984 | v2_idx = weight_index_array[i].Index - v1_idx*graph_size; 1985 | w_temp = weight_index_array[i].Weight; 1986 | 1987 | if (ver_seg_idx[v1_idx] != ver_seg_idx[v2_idx]) 1988 | { 1989 | float v1_mint, v2_mint; 1990 | int v1_size, v2_size; 1991 | v1_size = seg_size[ver_seg_idx[v1_idx]]; 1992 | v2_size = seg_size[ver_seg_idx[v2_idx]]; 1993 | v1_mint = seg_int[ver_seg_idx[v1_idx]] - thrd_value / v1_size; 1994 | v2_mint = seg_int[ver_seg_idx[v2_idx]] - thrd_value / v2_size; 1995 | 1996 | if (v1_mint >= v2_mint) 1997 | { 1998 | mint_idx = ver_seg_idx[v1_idx]; 1999 | mint_not_idx = ver_seg_idx[v2_idx]; 2000 | thred_temp = v1_mint;//seg_mint[ver_seg_idx[v1_idx]]; 2001 | } 2002 | else 2003 | { 2004 | mint_idx = ver_seg_idx[v2_idx]; 2005 | mint_not_idx = ver_seg_idx[v1_idx]; 2006 | thred_temp = v2_mint;//seg_mint[ver_seg_idx[v2_idx]]; 2007 | } 2008 | 2009 | if (w_temp > thred_temp)//Judgement, merge or not 2010 | { 2011 | seg_int[mint_idx] = w_temp; 2012 | 2013 | for (int j = 0; j < seg_size[mint_not_idx]; j++) 2014 | { 2015 | seg_ver_idx[mint_idx].push_back(seg_ver_idx[mint_not_idx][j]); //Rearrange vertex of merged one 2016 | ver_seg_idx[seg_ver_idx[mint_not_idx][j]]=mint_idx; //Remark the vertex of merged one 2017 | } 2018 | 2019 | seg_size[mint_idx]=seg_size[mint_idx]+seg_size[mint_not_idx]; //Change sizes 2020 | seg_size[mint_not_idx]=0; 2021 | 2022 | seg_ver_idx[mint_not_idx].clear(); 2023 | 2024 | } 2025 | } 2026 | } 2027 | 2028 | //Select 2029 | int seg_con=0; 2030 | for(int i=0;i connects_idx, std::vector *inthis_cluster_idx )//recursion search for finding voxels belonging to one cluster 2059 | { 2060 | int connect_id; 2061 | 2062 | for(int i=0;isupervoxel_clustered_[connects_idx[i]]) 2066 | { 2067 | 2068 | inthis_cluster_idx->push_back(connects_idx[i]); //Record the idx of this voxel 2069 | 2070 | this->supervoxel_clustered_[connects_idx[i]]=true; //change the status 2071 | 2072 | this->recursionSearch(this->supervoxels_connect_idx_[connects_idx[i]],inthis_cluster_idx); //recursion 2073 | } 2074 | 2075 | } 2076 | 2077 | } 2078 | 2079 | void 2080 | clusteringSupervoxels() //Clustering of supervoxels 2081 | { 2082 | //Setting 2083 | int clusters_num=0; 2084 | std::vector cluster_supervoxel; 2085 | 2086 | //Clustering 2087 | int i=0; 2088 | for(int i=0;isupervoxels_num_;i++)// loop + recrusion 2089 | { 2090 | if(!this->supervoxel_clustered_[i]) 2091 | { 2092 | //std::cout< *inthis_cluster_idx=new std::vector; 2095 | 2096 | this->supervoxel_clustered_.at(i)=true; 2097 | 2098 | this->recursionSearch(this->supervoxels_connect_idx_[i],inthis_cluster_idx); 2099 | 2100 | inthis_cluster_idx->push_back(i); //Don't forget to 2101 | 2102 | this->clusters_supervoxel_idx_.push_back(*inthis_cluster_idx); 2103 | } 2104 | } 2105 | 2106 | //Output 2107 | this->clusters_num_=this->clusters_supervoxel_idx_.size(); 2108 | 2109 | for(int m=0;mclusters_num_;m++) 2110 | { 2111 | int supervoxels_size=this->clusters_supervoxel_idx_[m].size(); 2112 | 2113 | if(supervoxels_size>0) 2114 | { 2115 | std::vector cluster_points; 2116 | for(int n=0;nsupervoxels_point_idx_[this->clusters_supervoxel_idx_[m][n]].size(); 2119 | for(int k=0;ksupervoxels_point_idx_[this->clusters_supervoxel_idx_[m][n]][k]); 2122 | } 2123 | } 2124 | this->clusters_point_idx_.push_back(cluster_points); 2125 | } 2126 | } 2127 | 2128 | //Test 2129 | std::cout << "In total " << this->clusters_num_ << " segments\n"; 2130 | } 2131 | 2132 | //Other 2133 | void 2134 | getVoxelCenterFromOctreeKey (pcl::octree::OctreeKey key, pcl::PointXYZ & point) 2135 | { 2136 | // define point to leaf node voxel center 2137 | point.x = static_cast ((static_cast (key.x) + 0.5f) * this->voxel_resolution_ + this->min_x_); 2138 | point.y = static_cast ((static_cast (key.y) + 0.5f) * this->voxel_resolution_ + this->min_y_); 2139 | point.z = static_cast ((static_cast (key.z) + 0.5f) * this->voxel_resolution_ + this->min_z_); 2140 | } 2141 | 2142 | void 2143 | crossValidation() 2144 | { 2145 | //Traverse 2146 | for(int i=0;isupervoxels_num_;i++)// loop all the supervoxels 2147 | { 2148 | int inthis_num=this->supervoxels_connect_idx_[i].size(); 2149 | std::vector new_connect_idx; 2150 | 2151 | if(inthis_num>1) 2152 | { 2153 | for(int j=0;jsupervoxels_connect_idx_[i][j]; 2158 | int search_size=this->supervoxels_connect_idx_[search_idx].size(); 2159 | 2160 | if(search_size>0) 2161 | { 2162 | for(int k=0;ksupervoxels_connect_idx_[search_idx][k]==find_idx) 2165 | { 2166 | find_ornot=true; 2167 | // 感觉搜索到了就不用再搜索了... 2168 | break; 2169 | } 2170 | } 2171 | } 2172 | //Judgement 2173 | if(find_ornot==true) 2174 | { 2175 | new_connect_idx.push_back(search_idx); 2176 | } 2177 | } 2178 | 2179 | //Replace 2180 | this->supervoxels_connect_idx_[i].clear(); 2181 | this->supervoxels_connect_idx_[i]=new_connect_idx; 2182 | } 2183 | } 2184 | } 2185 | 2186 | void 2187 | closestCheck(float sig_p, float sig_n, float sig_o, float sig_e, float sig_c, float sig_weight) 2188 | { 2189 | //Settings 2190 | std::vector voxel1_position, voxel1_normal, voxel1_eigen, voxel1_features, voxel1_colors; 2191 | std::vector voxel2_position, voxel2_normal, voxel2_eigen, voxel2_features, voxel2_colors; 2192 | pcl::PointXYZ temp_center, empty_center; 2193 | pcl::Normal temp_norm, empty_norm; 2194 | std::vector dist_vector; 2195 | empty_center.x = 0; empty_center.y = 0; empty_center.z = 0; 2196 | empty_norm.normal_x = 0; empty_norm.normal_y = 0; empty_norm.normal_z = 0; 2197 | 2198 | //Closet neighbor check 2199 | for (int i = 0; isupervoxels_num_; i++) 2200 | { 2201 | int inthis_num = this->supervoxels_connect_idx_[i].size(); 2202 | std::vector new_connect_idx; 2203 | 2204 | if (inthis_num>0 && inthis_num<2) 2205 | { 2206 | if (this->supervoxels_neighbor_idx_[i].size()>this->supervoxel_adjacency_min_) 2207 | { 2208 | float min_dis = 0, temp_dis = 0; 2209 | int min_idx; 2210 | pcl::PointXYZ temp_p1, temp_p2; 2211 | 2212 | voxel1_position.clear(); 2213 | voxel1_normal.clear(); 2214 | voxel1_eigen.clear(); 2215 | temp_center = this->supervoxel_centroids_[i]; 2216 | temp_norm = this->supervoxel_norms_[i]; 2217 | 2218 | if (temp_center.x != empty_center.x && temp_center.y != empty_center.y && temp_center.z != empty_center.z) 2219 | { 2220 | voxel1_position.push_back(temp_center.x); 2221 | voxel1_position.push_back(temp_center.y); 2222 | voxel1_position.push_back(temp_center.z); 2223 | } 2224 | else 2225 | { 2226 | voxel1_position.push_back(0); 2227 | } 2228 | 2229 | if (temp_norm.normal_x != empty_norm.normal_x && temp_norm.normal_y != empty_norm.normal_y && temp_norm.normal_z != empty_norm.normal_z) 2230 | { 2231 | voxel1_normal.push_back(temp_norm.normal_x); 2232 | voxel1_normal.push_back(temp_norm.normal_y); 2233 | voxel1_normal.push_back(temp_norm.normal_z); 2234 | } 2235 | else 2236 | { 2237 | voxel1_normal.push_back(0); 2238 | } 2239 | voxel1_eigen = this->supervoxel_eigens_[i]; 2240 | 2241 | //Initialization 2242 | min_dis = 0; min_idx = -1; 2243 | 2244 | //Check neighbors 2245 | for (int j = 0; jsupervoxels_neighbor_idx_[i].size(); j++) 2246 | { 2247 | if (this->supervoxels_connect_idx_[this->supervoxels_neighbor_idx_[i][j]].size()>1) 2248 | { 2249 | temp_p2 = this->supervoxel_centroids_[this->supervoxels_neighbor_idx_[i][j]]; 2250 | 2251 | voxel2_position.clear(); 2252 | voxel2_normal.clear(); 2253 | voxel2_eigen.clear(); 2254 | temp_center = this->supervoxel_centroids_[this->supervoxels_neighbor_idx_[i][j]];//temp_center=voxel_centers_[voxels_idx[j]]; 2255 | temp_norm = this->supervoxel_norms_[this->supervoxels_neighbor_idx_[i][j]]; 2256 | 2257 | if (temp_center.x != empty_center.x && temp_center.y != empty_center.y && temp_center.z != empty_center.z) 2258 | { 2259 | voxel2_position.push_back(temp_center.x); 2260 | voxel2_position.push_back(temp_center.y); 2261 | voxel2_position.push_back(temp_center.z); 2262 | } 2263 | else 2264 | { 2265 | voxel2_position.push_back(0); 2266 | } 2267 | 2268 | if (temp_norm.normal_x != empty_norm.normal_x && temp_norm.normal_y != empty_norm.normal_y && temp_norm.normal_z != empty_norm.normal_z) 2269 | { 2270 | voxel2_normal.push_back(temp_norm.normal_x); 2271 | voxel2_normal.push_back(temp_norm.normal_y); 2272 | voxel2_normal.push_back(temp_norm.normal_z); 2273 | } 2274 | else 2275 | { 2276 | voxel2_normal.push_back(0); 2277 | } 2278 | voxel2_eigen = this->supervoxel_eigens_[this->supervoxels_neighbor_idx_[i][j]]; 2279 | 2280 | 2281 | dist_vector = measuringDistance(voxel1_position, voxel2_position, voxel1_normal, voxel2_normal, voxel1_eigen, voxel2_eigen); 2282 | // 测量边权值的方法有所不同 2283 | temp_dis = distanceWeight(dist_vector, sig_p, sig_n, sig_o, sig_e, sig_c, sig_weight); 2284 | 2285 | if (temp_dis >= min_dis) 2286 | { 2287 | min_dis = temp_dis; 2288 | min_idx = this->supervoxels_neighbor_idx_[i][j]; 2289 | } 2290 | 2291 | } 2292 | } 2293 | 2294 | if (min_idx != -1) 2295 | { 2296 | this->supervoxels_connect_idx_[i].push_back(min_idx); 2297 | this->supervoxels_connect_idx_[min_idx].push_back(i); 2298 | } 2299 | } 2300 | } 2301 | 2302 | } 2303 | 2304 | 2305 | } 2306 | }; 2307 | 2308 | } 2309 | 2310 | #endif // PCL_SEGMENTATION_SUPERVOXEL_H_ 2311 | -------------------------------------------------------------------------------- /supervoxel_segmentation.hpp: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | // File: supervoxel_segmentation.hpp 3 | // Author: Yusheng Xu, PF_Technische Universitaet Muechen (yusheng.xu@tum.de) 4 | // Description: The supervoxel based segmentation for point cloud 5 | // Modified: 24.04.2017 6 | // 7 | // Copyright (c) 2015-2017 Yusheng Xu (yusheng.xu@tum.de) 8 | // 9 | // This library is free software; you can redistribute it and/or 10 | // modify it under the terms of the GNU General Public 11 | // License as published by the Free Software Foundation; either 12 | // Version 3 of the License, or (at your option) any later version. 13 | // 14 | // This library is distributed in the hope that it will be useful, 15 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 16 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 17 | // General Public License for more details. 18 | //////////////////////////////////////////////////////////////////////////////// 19 | 20 | #ifndef PCL_SEGMENTATION_SUPERVOXEL_IMPL_H_ 21 | #define PCL_SEGMENTATION_SUPERVOXEL_IMPL_H_ 22 | 23 | 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | #endif // PCL_SEGMENTATION_SUPERVOXEL_H_ 30 | -------------------------------------------------------------------------------- /test: -------------------------------------------------------------------------------- 1 | //Segmentation of points using Voxel- and Graph-based Segmentation 2 | 3 | //The "input_vector" encodes the input parameters, you can change it according to your demands 4 | //See： Task_File_VGS.txt Task_File_SVGS.txt 5 | 6 | //Testing dataset (Town_Test.pcd) is cropped from ETH Zurich dataset： Semantic3D 7 | 8 | //Segmentation of points using Voxel and Graph based Segmentation, VGS 9 | int 10 | segmentationVGS(string path_name, PCXYZPtr input_cloud, std::vector input_vector) 11 | { 12 | //Remarks 13 | pcl::console::print_highlight ("Segmenting the pointcloud by the use of voxel based graph segmentation ...\n"); 14 | //For the algorithm used (improved version): 15 | //Xu et al.,GRSL 2017, Geometric Primitive Extraction From Point Clouds of Construction Sites Using VGS 16 | 17 | //Parameter settings 18 | string file_name; 19 | float voxel_size; 20 | float graph_size; 21 | float sig_p, sig_n, sig_o, sig_e, sig_c, sig_w; 22 | float cut_thred; 23 | int points_min, adjacency_min, voxels_min; 24 | 25 | file_name = input_vector[21]; 26 | voxel_size = std::atof(input_vector[28].c_str()); 27 | graph_size = std::atof(input_vector[30].c_str()); 28 | sig_p = std::atof(input_vector[32].c_str()); 29 | sig_n = std::atof(input_vector[34].c_str()); 30 | sig_o = std::atof(input_vector[36].c_str()); 31 | sig_e = std::atof(input_vector[38].c_str()); 32 | sig_c = std::atof(input_vector[40].c_str()); 33 | sig_w = std::atof(input_vector[42].c_str()); 34 | cut_thred = std::atof(input_vector[44].c_str()); 35 | points_min = std::atoi(input_vector[46].c_str()); 36 | adjacency_min = std::atoi(input_vector[48].c_str()); 37 | voxels_min = std::atoi(input_vector[50].c_str()); 38 | 39 | unsigned int node_ID = 0; 40 | double min_x = 0, min_y = 0, min_z = 0, max_x = 0, max_y = 0, max_z = 0; 41 | 42 | //pcl::PointCloud::Ptr colored_cloud(new PCXYZRGB); 43 | pcl::PointCloud::Ptr clustered_cloud(new PCXYZRGB); 44 | pcl::PolygonMesh::Ptr colored_voxels(new pcl::PolygonMesh); 45 | pcl::PolygonMesh::Ptr frames_voxels(new pcl::PolygonMesh); 46 | pcl::PolygonMesh::Ptr Normes_voxels(new pcl::PolygonMesh); 47 | pcl::PolygonMesh::Ptr clustered_voxels(new pcl::PolygonMesh); 48 | std::vector> voxel_centers; 49 | 50 | //Voxelization 51 | VoxelBasedSegmentation voxel_structure(voxel_size);//Building octree structure 52 | voxel_structure.setInputCloud(input_cloud); //Input point cloud to octree 53 | voxel_structure.getCloudPointNum(input_cloud); 54 | voxel_structure.addPointsFromInputCloud(); 55 | voxel_structure.setVoxelSize(voxel_size, points_min, voxels_min, adjacency_min); 56 | voxel_structure.getBoundingBox(min_x, min_y, min_z, max_x, max_y, max_z); 57 | voxel_structure.setBoundingBox(min_x, min_y, min_z, max_x, max_y, max_z); 58 | 59 | //Get center points of all occupied voxels 60 | voxel_structure.setVoxelCenters(); 61 | voxel_centers = voxel_structure.getVoxelCenters(); 62 | voxel_structure.getVoxelNum(); 63 | 64 | //Features calculation 65 | voxel_structure.calcualteVoxelCloudAttributes(input_cloud); 66 | 67 | //Find adjacencies 68 | voxel_structure.findAllVoxelAdjacency(graph_size); 69 | 70 | //Segmentation 71 | voxel_structure.segmentVoxelCloudWithGraphModel(cut_thred, sig_p, sig_n, sig_o, sig_e, sig_c, sig_w); 72 | 73 | //Output segmented color draw 74 | voxel_structure.drawColorMapofPointsinClusters(clustered_cloud); 75 | std::vector > clusters_points_idx; 76 | clusters_points_idx = voxel_structure.getClusterIdx(); 77 | string file_name1 = file_name; 78 | file_name1 = file_name1.replace(file_name1.length() - 4, file_name1.length(), ".pcd"); 79 | string fileoutpath_name1 = path_name + file_name1; 80 | saveColoredClusters(fileoutpath_name1, input_cloud, clusters_points_idx); 81 | 82 | //Visualization 83 | showColoredClusters(fileoutpath_name1, input_cloud, clusters_points_idx); 84 | 85 | return(0); 86 | } 87 | 88 | 89 | 90 | //Segmentation of points using Supervoxel and Graph based Segmentation 91 | int 92 | segmentationSVGS(string path_name, PCXYZPtr input_cloud, std::vector input_vector) 93 | { 94 | //Remarks 95 | pcl::console::print_highlight("Segmenting the pointcloud by the use of supervoxel based graph segmentation ...\n"); 96 | //For the algorithm used (improved version): 97 | //Xu et al.,ISPRS Annals 2017, Voxel- and Graph-Based Point Cloud Segmentation of 3D Scenes Using Perceptual Grouping Laws 98 | 99 | //Parameters and settings 100 | string file_name; 101 | float voxel_size, seed_size, graph_size; 102 | float sig_p, sig_n, sig_o, sig_e, sig_c, sig_w; 103 | float sig_a, sig_b, sig_c; 104 | float cut_thred; 105 | int points_min, points_min2, voxels_min, adjacency_min; 106 | std::vector > clusters_points_idx; 107 | 108 | file_name = input_vector[21]; 109 | voxel_size = std::atof(input_vector[28].c_str()); 110 | seed_size = std::atof(input_vector[30].c_str()); 111 | graph_size = std::atof(input_vector[32].c_str()); 112 | sig_p = std::atof(input_vector[34].c_str()); 113 | sig_n = std::atof(input_vector[36].c_str()); 114 | sig_o = std::atof(input_vector[38].c_str()); 115 | sig_e = std::atof(input_vector[40].c_str()); 116 | sig_c = std::atof(input_vector[42].c_str()); 117 | sig_w = std::atof(input_vector[44].c_str()); 118 | sig_a = std::atof(input_vector[46].c_str()); 119 | sig_b = std::atof(input_vector[48].c_str()); 120 | sig_c = std::atof(input_vector[50].c_str()); 121 | cut_thred = std::atof(input_vector[52].c_str()); 122 | points_min = std::atof(input_vector[54].c_str()); 123 | points_min2 = std::atof(input_vector[56].c_str()); 124 | voxels_min = std::atoi(input_vector[58].c_str()); 125 | adjacency_min = std::atoi(input_vector[60].c_str()); 126 | 127 | int points_size = 0; 128 | unsigned int node_ID = 0; 129 | 130 | double min_x = 0, min_y = 0, min_z = 0, max_x = 0, max_y = 0, max_z = 0; 131 | 132 | pcl::PointCloud::Ptr colored_voxels(new PCXYZRGB); 133 | pcl::PointCloud::Ptr colored_supervoxels(new PCXYZRGB); 134 | pcl::PointCloud::Ptr clustered_cloud(new PCXYZRGB); 135 | pcl::PolygonMesh::Ptr normes_spvoxels(new pcl::PolygonMesh); 136 | 137 | //Super Voxelization 138 | SuperVoxelBasedSegmentation supervoxel_structure(voxel_size);//Building octree structure 139 | 140 | supervoxel_structure.setInputCloud(input_cloud); //Input point cloud to octree 141 | supervoxel_structure.getCloudPointNum(input_cloud); 142 | supervoxel_structure.addPointsFromInputCloud(); 143 | 144 | supervoxel_structure.setVoxelSize(voxel_size, points_min); 145 | supervoxel_structure.setSupervoxelSize(seed_size, voxels_min, points_min, adjacency_min); 146 | supervoxel_structure.setGraphSize(seed_size * 2, graph_size); 147 | 148 | supervoxel_structure.getBoundingBox(min_x, min_y, min_z, max_x, max_y, max_z); 149 | supervoxel_structure.setBoundingBox(min_x, min_y, min_z, max_x, max_y, max_z); 150 | 151 | //Get center points of all occupied voxels 152 | supervoxel_structure.setSupervoxelCentersCentroids(); 153 | supervoxel_structure.getVoxelNum(); 154 | 155 | //Supervoxel generation 156 | supervoxel_structure.segmentSupervoxelCloudWithGraphModel(sig_a, sig_b, sig_c, cut_thred, sig_p, sig_n, sig_o, sig_e, sig_c, sig_w); 157 | 158 | //Results 159 | supervoxel_structure.drawColorMapofPointsinClusters(clustered_cloud); 160 | clusters_points_idx = supervoxel_structure.getClusterIdx(); 161 | string file_name1 = file_name; 162 | file_name1 = file_name1.replace(file_name1.length() - 4, file_name1.length(), ".pcd"); 163 | string fileoutpath_name1 = path_name + file_name1; 164 | saveColoredClusters(fileoutpath_name1, input_cloud, clusters_points_idx); 165 | 166 | //Visualization 167 | showColoredClusters(fileoutpath_name1, input_cloud, clusters_points_idx); 168 | 169 | return(0); 170 | } 171 | -------------------------------------------------------------------------------- /voxel_segmentation.cpp: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | // File: voxel_segmentation.cpp 3 | // Author: Yusheng Xu, PF_Technische Universitaet Muechen (yusheng.xu@tum.de) 4 | // Description: The voxel based segmentation for point cloud 5 | // Modified: 15.11.2016 6 | // 7 | // Copyright (c) 2016 Yusheng Xu (yusheng.xu@tum.de) 8 | // 9 | // This library is free software; you can redistribute it and/or 10 | // modify it under the terms of the GNU General Public 11 | // License as published by the Free Software Foundation; either 12 | // Version 3 of the License, or (at your option) any later version. 13 | // 14 | // This library is distributed in the hope that it will be useful, 15 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 16 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 17 | // General Public License for more details. 18 | // 19 | //////////////////////////////////////////////////////////////////////////////// 20 | 21 | #include "voxel_segmentation.h" 22 | #include "voxel_segmentation.hpp" 23 | -------------------------------------------------------------------------------- /voxel_segmentation.hpp: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////////// 2 | // File: voxel_segmentation.hpp 3 | // Author: Yusheng Xu, PF_Technische Universitaet Muechen (yusheng.xu@tum.de) 4 | // Description: The voxel based segmentation for point cloud 5 | // Modified: 14.11.2016 6 | // 7 | // Copyright (c) 2016 Yusheng Xu (yusheng.xu@tum.de) 8 | // 9 | // This library is free software; you can redistribute it and/or 10 | // modify it under the terms of the GNU General Public 11 | // License as published by the Free Software Foundation; either 12 | // Version 3 of the License, or (at your option) any later version. 13 | // 14 | // This library is distributed in the hope that it will be useful, 15 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 16 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 17 | // General Public License for more details. 18 | // 19 | //////////////////////////////////////////////////////////////////////////////// 20 | 21 | #ifndef PCL_SEGMENTATION_VOXEL_IMPL_H_ 22 | #define PCL_SEGMENTATION_VOXEL_IMPL_H_ 23 | 24 | 25 | #include 26 | #include 27 | #include 28 | #include 29 | 30 | #endif // PCL_SEGMENTATION_VOXEL_H_ 31 | --------------------------------------------------------------------------------