├── .gitignore ├── CMakeLists.txt ├── LICENSE ├── README.md ├── config └── params.yaml ├── include ├── jsk_recognition_msgs │ ├── CMakeLists.txt │ ├── msg │ │ └── PolygonArray.msg │ └── package.xml ├── patchworkpp │ ├── patchworkpp.hpp │ └── utils.hpp └── tools │ ├── kitti_loader.hpp │ └── pcd_loader.hpp ├── launch ├── demo.launch ├── offline_kitti.launch └── video.launch ├── package.xml ├── pictures └── patchwork++.gif ├── rviz ├── demo.rviz └── patchworkpp_viz.rviz └── src ├── demo.cpp ├── offline_kitti.cpp └── video.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | .vscode/ 2 | 3 | ./launch/video.launch 4 | ./src/video.cpp 5 | 6 | scripts/ -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | option(BUILD_JSK_PKGS "Enable building of required components of jsk_recognition_msgs and jsk_rviz_plugins" ON) 2 | 3 | if(BUILD_JSK_PKGS) 4 | add_subdirectory(include/jsk_recognition_msgs) 5 | # add_subdirectory(include/jsk_rviz_plugins) #TODO: allow building of rviz plugins as well 6 | endif() 7 | 8 | cmake_minimum_required(VERSION 3.0.2) 9 | project(patchworkpp) 10 | 11 | add_compile_options(-std=c++17) 12 | set(CMAKE_BUILD_TYPE "Release") 13 | 14 | set(CMAKE_CXX_STANDARD 14) 15 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 16 | set(CMAKE_CXX_EXTENSIONS OFF) 17 | 18 | find_package(catkin REQUIRED COMPONENTS 19 | roscpp 20 | rospy 21 | std_msgs 22 | roslaunch 23 | cv_bridge 24 | pcl_conversions 25 | pcl_ros 26 | geometry_msgs 27 | laser_geometry 28 | sensor_msgs 29 | message_generation 30 | jsk_recognition_msgs 31 | ) 32 | 33 | find_package(OpenCV REQUIRED) 34 | 35 | generate_messages( 36 | DEPENDENCIES 37 | std_msgs 38 | geometry_msgs 39 | sensor_msgs 40 | ) 41 | 42 | find_package(PCL 1.7 REQUIRED) 43 | find_package(Boost 1.54 REQUIRED) 44 | find_package(OpenMP) 45 | if (OPENMP_FOUND) 46 | set (CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") 47 | set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") 48 | set (CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${OpenMP_EXE_LINKER_FLAGS}") 49 | endif() 50 | 51 | catkin_package( 52 | INCLUDE_DIRS 53 | LIBRARIES 54 | CATKIN_DEPENDS roscpp rospy std_msgs 55 | ) 56 | 57 | include_directories( 58 | ${catkin_INCLUDE_DIRS} 59 | ${PCL_INCLUDE_DIRS} 60 | ${OpenCV_INCLUDE_DIRS} 61 | include 62 | ) 63 | 64 | add_executable(offline_kitti src/offline_kitti.cpp) 65 | target_link_libraries(offline_kitti ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES}) 66 | add_dependencies(offline_kitti patchworkpp_generate_messages_cpp) 67 | 68 | add_executable(demo src/demo.cpp) 69 | target_link_libraries(demo ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES}) 70 | add_dependencies(demo patchworkpp_generate_messages_cpp) 71 | 72 | add_executable(video src/video.cpp) 73 | target_link_libraries(video ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES}) 74 | add_dependencies(video patchworkpp_generate_messages_cpp) 75 | 76 | 77 | # ==== Install ==== 78 | install(TARGETS demo 79 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 80 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 81 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 82 | ) 83 | 84 | install(DIRECTORY include/${PROJECT_NAME}/ 85 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) 86 | 87 | install(DIRECTORY launch config 88 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 89 | ) 90 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | GNU GENERAL PUBLIC LICENSE 2 | Version 3, 29 June 2007 3 | 4 | Copyright (C) 2007 Free Software Foundation, Inc. 5 | Everyone is permitted to copy and distribute verbatim copies 6 | of this license document, but changing it is not allowed. 7 | 8 | Preamble 9 | 10 | The GNU General Public License is a free, copyleft license for 11 | software and other kinds of works. 12 | 13 | The licenses for most software and other practical works are designed 14 | to take away your freedom to share and change the works. By contrast, 15 | the GNU General Public License is intended to guarantee your freedom to 16 | share and change all versions of a program--to make sure it remains free 17 | software for all its users. We, the Free Software Foundation, use the 18 | GNU General Public License for most of our software; it applies also to 19 | any other work released this way by its authors. You can apply it to 20 | your programs, too. 21 | 22 | When we speak of free software, we are referring to freedom, not 23 | price. Our General Public Licenses are designed to make sure that you 24 | have the freedom to distribute copies of free software (and charge for 25 | them if you wish), that you receive source code or can get it if you 26 | want it, that you can change the software or use pieces of it in new 27 | free programs, and that you know you can do these things. 28 | 29 | To protect your rights, we need to prevent others from denying you 30 | these rights or asking you to surrender the rights. Therefore, you have 31 | certain responsibilities if you distribute copies of the software, or if 32 | you modify it: responsibilities to respect the freedom of others. 33 | 34 | For example, if you distribute copies of such a program, whether 35 | gratis or for a fee, you must pass on to the recipients the same 36 | freedoms that you received. You must make sure that they, too, receive 37 | or can get the source code. And you must show them these terms so they 38 | know their rights. 39 | 40 | Developers that use the GNU GPL protect your rights with two steps: 41 | (1) assert copyright on the software, and (2) offer you this License 42 | giving you legal permission to copy, distribute and/or modify it. 43 | 44 | For the developers' and authors' protection, the GPL clearly explains 45 | that there is no warranty for this free software. For both users' and 46 | authors' sake, the GPL requires that modified versions be marked as 47 | changed, so that their problems will not be attributed erroneously to 48 | authors of previous versions. 49 | 50 | Some devices are designed to deny users access to install or run 51 | modified versions of the software inside them, although the manufacturer 52 | can do so. This is fundamentally incompatible with the aim of 53 | protecting users' freedom to change the software. The systematic 54 | pattern of such abuse occurs in the area of products for individuals to 55 | use, which is precisely where it is most unacceptable. Therefore, we 56 | have designed this version of the GPL to prohibit the practice for those 57 | products. If such problems arise substantially in other domains, we 58 | stand ready to extend this provision to those domains in future versions 59 | of the GPL, as needed to protect the freedom of users. 60 | 61 | Finally, every program is threatened constantly by software patents. 62 | States should not allow patents to restrict development and use of 63 | software on general-purpose computers, but in those that do, we wish to 64 | avoid the special danger that patents applied to a free program could 65 | make it effectively proprietary. To prevent this, the GPL assures that 66 | patents cannot be used to render the program non-free. 67 | 68 | The precise terms and conditions for copying, distribution and 69 | modification follow. 70 | 71 | TERMS AND CONDITIONS 72 | 73 | 0. Definitions. 74 | 75 | "This License" refers to version 3 of the GNU General Public License. 76 | 77 | "Copyright" also means copyright-like laws that apply to other kinds of 78 | works, such as semiconductor masks. 79 | 80 | "The Program" refers to any copyrightable work licensed under this 81 | License. Each licensee is addressed as "you". "Licensees" and 82 | "recipients" may be individuals or organizations. 83 | 84 | To "modify" a work means to copy from or adapt all or part of the work 85 | in a fashion requiring copyright permission, other than the making of an 86 | exact copy. The resulting work is called a "modified version" of the 87 | earlier work or a work "based on" the earlier work. 88 | 89 | A "covered work" means either the unmodified Program or a work based 90 | on the Program. 91 | 92 | To "propagate" a work means to do anything with it that, without 93 | permission, would make you directly or secondarily liable for 94 | infringement under applicable copyright law, except executing it on a 95 | computer or modifying a private copy. Propagation includes copying, 96 | distribution (with or without modification), making available to the 97 | public, and in some countries other activities as well. 98 | 99 | To "convey" a work means any kind of propagation that enables other 100 | parties to make or receive copies. Mere interaction with a user through 101 | a computer network, with no transfer of a copy, is not conveying. 102 | 103 | An interactive user interface displays "Appropriate Legal Notices" 104 | to the extent that it includes a convenient and prominently visible 105 | feature that (1) displays an appropriate copyright notice, and (2) 106 | tells the user that there is no warranty for the work (except to the 107 | extent that warranties are provided), that licensees may convey the 108 | work under this License, and how to view a copy of this License. If 109 | the interface presents a list of user commands or options, such as a 110 | menu, a prominent item in the list meets this criterion. 111 | 112 | 1. Source Code. 113 | 114 | The "source code" for a work means the preferred form of the work 115 | for making modifications to it. "Object code" means any non-source 116 | form of a work. 117 | 118 | A "Standard Interface" means an interface that either is an official 119 | standard defined by a recognized standards body, or, in the case of 120 | interfaces specified for a particular programming language, one that 121 | is widely used among developers working in that language. 122 | 123 | The "System Libraries" of an executable work include anything, other 124 | than the work as a whole, that (a) is included in the normal form of 125 | packaging a Major Component, but which is not part of that Major 126 | Component, and (b) serves only to enable use of the work with that 127 | Major Component, or to implement a Standard Interface for which an 128 | implementation is available to the public in source code form. A 129 | "Major Component", in this context, means a major essential component 130 | (kernel, window system, and so on) of the specific operating system 131 | (if any) on which the executable work runs, or a compiler used to 132 | produce the work, or an object code interpreter used to run it. 133 | 134 | The "Corresponding Source" for a work in object code form means all 135 | the source code needed to generate, install, and (for an executable 136 | work) run the object code and to modify the work, including scripts to 137 | control those activities. However, it does not include the work's 138 | System Libraries, or general-purpose tools or generally available free 139 | programs which are used unmodified in performing those activities but 140 | which are not part of the work. For example, Corresponding Source 141 | includes interface definition files associated with source files for 142 | the work, and the source code for shared libraries and dynamically 143 | linked subprograms that the work is specifically designed to require, 144 | such as by intimate data communication or control flow between those 145 | subprograms and other parts of the work. 146 | 147 | The Corresponding Source need not include anything that users 148 | can regenerate automatically from other parts of the Corresponding 149 | Source. 150 | 151 | The Corresponding Source for a work in source code form is that 152 | same work. 153 | 154 | 2. Basic Permissions. 155 | 156 | All rights granted under this License are granted for the term of 157 | copyright on the Program, and are irrevocable provided the stated 158 | conditions are met. This License explicitly affirms your unlimited 159 | permission to run the unmodified Program. The output from running a 160 | covered work is covered by this License only if the output, given its 161 | content, constitutes a covered work. This License acknowledges your 162 | rights of fair use or other equivalent, as provided by copyright law. 163 | 164 | You may make, run and propagate covered works that you do not 165 | convey, without conditions so long as your license otherwise remains 166 | in force. You may convey covered works to others for the sole purpose 167 | of having them make modifications exclusively for you, or provide you 168 | with facilities for running those works, provided that you comply with 169 | the terms of this License in conveying all material for which you do 170 | not control copyright. Those thus making or running the covered works 171 | for you must do so exclusively on your behalf, under your direction 172 | and control, on terms that prohibit them from making any copies of 173 | your copyrighted material outside their relationship with you. 174 | 175 | Conveying under any other circumstances is permitted solely under 176 | the conditions stated below. Sublicensing is not allowed; section 10 177 | makes it unnecessary. 178 | 179 | 3. Protecting Users' Legal Rights From Anti-Circumvention Law. 180 | 181 | No covered work shall be deemed part of an effective technological 182 | measure under any applicable law fulfilling obligations under article 183 | 11 of the WIPO copyright treaty adopted on 20 December 1996, or 184 | similar laws prohibiting or restricting circumvention of such 185 | measures. 186 | 187 | When you convey a covered work, you waive any legal power to forbid 188 | circumvention of technological measures to the extent such circumvention 189 | is effected by exercising rights under this License with respect to 190 | the covered work, and you disclaim any intention to limit operation or 191 | modification of the work as a means of enforcing, against the work's 192 | users, your or third parties' legal rights to forbid circumvention of 193 | technological measures. 194 | 195 | 4. Conveying Verbatim Copies. 196 | 197 | You may convey verbatim copies of the Program's source code as you 198 | receive it, in any medium, provided that you conspicuously and 199 | appropriately publish on each copy an appropriate copyright notice; 200 | keep intact all notices stating that this License and any 201 | non-permissive terms added in accord with section 7 apply to the code; 202 | keep intact all notices of the absence of any warranty; and give all 203 | recipients a copy of this License along with the Program. 204 | 205 | You may charge any price or no price for each copy that you convey, 206 | and you may offer support or warranty protection for a fee. 207 | 208 | 5. Conveying Modified Source Versions. 209 | 210 | You may convey a work based on the Program, or the modifications to 211 | produce it from the Program, in the form of source code under the 212 | terms of section 4, provided that you also meet all of these conditions: 213 | 214 | a) The work must carry prominent notices stating that you modified 215 | it, and giving a relevant date. 216 | 217 | b) The work must carry prominent notices stating that it is 218 | released under this License and any conditions added under section 219 | 7. This requirement modifies the requirement in section 4 to 220 | "keep intact all notices". 221 | 222 | c) You must license the entire work, as a whole, under this 223 | License to anyone who comes into possession of a copy. This 224 | License will therefore apply, along with any applicable section 7 225 | additional terms, to the whole of the work, and all its parts, 226 | regardless of how they are packaged. This License gives no 227 | permission to license the work in any other way, but it does not 228 | invalidate such permission if you have separately received it. 229 | 230 | d) If the work has interactive user interfaces, each must display 231 | Appropriate Legal Notices; however, if the Program has interactive 232 | interfaces that do not display Appropriate Legal Notices, your 233 | work need not make them do so. 234 | 235 | A compilation of a covered work with other separate and independent 236 | works, which are not by their nature extensions of the covered work, 237 | and which are not combined with it such as to form a larger program, 238 | in or on a volume of a storage or distribution medium, is called an 239 | "aggregate" if the compilation and its resulting copyright are not 240 | used to limit the access or legal rights of the compilation's users 241 | beyond what the individual works permit. Inclusion of a covered work 242 | in an aggregate does not cause this License to apply to the other 243 | parts of the aggregate. 244 | 245 | 6. Conveying Non-Source Forms. 246 | 247 | You may convey a covered work in object code form under the terms 248 | of sections 4 and 5, provided that you also convey the 249 | machine-readable Corresponding Source under the terms of this License, 250 | in one of these ways: 251 | 252 | a) Convey the object code in, or embodied in, a physical product 253 | (including a physical distribution medium), accompanied by the 254 | Corresponding Source fixed on a durable physical medium 255 | customarily used for software interchange. 256 | 257 | b) Convey the object code in, or embodied in, a physical product 258 | (including a physical distribution medium), accompanied by a 259 | written offer, valid for at least three years and valid for as 260 | long as you offer spare parts or customer support for that product 261 | model, to give anyone who possesses the object code either (1) a 262 | copy of the Corresponding Source for all the software in the 263 | product that is covered by this License, on a durable physical 264 | medium customarily used for software interchange, for a price no 265 | more than your reasonable cost of physically performing this 266 | conveying of source, or (2) access to copy the 267 | Corresponding Source from a network server at no charge. 268 | 269 | c) Convey individual copies of the object code with a copy of the 270 | written offer to provide the Corresponding Source. This 271 | alternative is allowed only occasionally and noncommercially, and 272 | only if you received the object code with such an offer, in accord 273 | with subsection 6b. 274 | 275 | d) Convey the object code by offering access from a designated 276 | place (gratis or for a charge), and offer equivalent access to the 277 | Corresponding Source in the same way through the same place at no 278 | further charge. You need not require recipients to copy the 279 | Corresponding Source along with the object code. If the place to 280 | copy the object code is a network server, the Corresponding Source 281 | may be on a different server (operated by you or a third party) 282 | that supports equivalent copying facilities, provided you maintain 283 | clear directions next to the object code saying where to find the 284 | Corresponding Source. Regardless of what server hosts the 285 | Corresponding Source, you remain obligated to ensure that it is 286 | available for as long as needed to satisfy these requirements. 287 | 288 | e) Convey the object code using peer-to-peer transmission, provided 289 | you inform other peers where the object code and Corresponding 290 | Source of the work are being offered to the general public at no 291 | charge under subsection 6d. 292 | 293 | A separable portion of the object code, whose source code is excluded 294 | from the Corresponding Source as a System Library, need not be 295 | included in conveying the object code work. 296 | 297 | A "User Product" is either (1) a "consumer product", which means any 298 | tangible personal property which is normally used for personal, family, 299 | or household purposes, or (2) anything designed or sold for incorporation 300 | into a dwelling. In determining whether a product is a consumer product, 301 | doubtful cases shall be resolved in favor of coverage. For a particular 302 | product received by a particular user, "normally used" refers to a 303 | typical or common use of that class of product, regardless of the status 304 | of the particular user or of the way in which the particular user 305 | actually uses, or expects or is expected to use, the product. A product 306 | is a consumer product regardless of whether the product has substantial 307 | commercial, industrial or non-consumer uses, unless such uses represent 308 | the only significant mode of use of the product. 309 | 310 | "Installation Information" for a User Product means any methods, 311 | procedures, authorization keys, or other information required to install 312 | and execute modified versions of a covered work in that User Product from 313 | a modified version of its Corresponding Source. The information must 314 | suffice to ensure that the continued functioning of the modified object 315 | code is in no case prevented or interfered with solely because 316 | modification has been made. 317 | 318 | If you convey an object code work under this section in, or with, or 319 | specifically for use in, a User Product, and the conveying occurs as 320 | part of a transaction in which the right of possession and use of the 321 | User Product is transferred to the recipient in perpetuity or for a 322 | fixed term (regardless of how the transaction is characterized), the 323 | Corresponding Source conveyed under this section must be accompanied 324 | by the Installation Information. But this requirement does not apply 325 | if neither you nor any third party retains the ability to install 326 | modified object code on the User Product (for example, the work has 327 | been installed in ROM). 328 | 329 | The requirement to provide Installation Information does not include a 330 | requirement to continue to provide support service, warranty, or updates 331 | for a work that has been modified or installed by the recipient, or for 332 | the User Product in which it has been modified or installed. Access to a 333 | network may be denied when the modification itself materially and 334 | adversely affects the operation of the network or violates the rules and 335 | protocols for communication across the network. 336 | 337 | Corresponding Source conveyed, and Installation Information provided, 338 | in accord with this section must be in a format that is publicly 339 | documented (and with an implementation available to the public in 340 | source code form), and must require no special password or key for 341 | unpacking, reading or copying. 342 | 343 | 7. Additional Terms. 344 | 345 | "Additional permissions" are terms that supplement the terms of this 346 | License by making exceptions from one or more of its conditions. 347 | Additional permissions that are applicable to the entire Program shall 348 | be treated as though they were included in this License, to the extent 349 | that they are valid under applicable law. If additional permissions 350 | apply only to part of the Program, that part may be used separately 351 | under those permissions, but the entire Program remains governed by 352 | this License without regard to the additional permissions. 353 | 354 | When you convey a copy of a covered work, you may at your option 355 | remove any additional permissions from that copy, or from any part of 356 | it. (Additional permissions may be written to require their own 357 | removal in certain cases when you modify the work.) You may place 358 | additional permissions on material, added by you to a covered work, 359 | for which you have or can give appropriate copyright permission. 360 | 361 | Notwithstanding any other provision of this License, for material you 362 | add to a covered work, you may (if authorized by the copyright holders of 363 | that material) supplement the terms of this License with terms: 364 | 365 | a) Disclaiming warranty or limiting liability differently from the 366 | terms of sections 15 and 16 of this License; or 367 | 368 | b) Requiring preservation of specified reasonable legal notices or 369 | author attributions in that material or in the Appropriate Legal 370 | Notices displayed by works containing it; or 371 | 372 | c) Prohibiting misrepresentation of the origin of that material, or 373 | requiring that modified versions of such material be marked in 374 | reasonable ways as different from the original version; or 375 | 376 | d) Limiting the use for publicity purposes of names of licensors or 377 | authors of the material; or 378 | 379 | e) Declining to grant rights under trademark law for use of some 380 | trade names, trademarks, or service marks; or 381 | 382 | f) Requiring indemnification of licensors and authors of that 383 | material by anyone who conveys the material (or modified versions of 384 | it) with contractual assumptions of liability to the recipient, for 385 | any liability that these contractual assumptions directly impose on 386 | those licensors and authors. 387 | 388 | All other non-permissive additional terms are considered "further 389 | restrictions" within the meaning of section 10. If the Program as you 390 | received it, or any part of it, contains a notice stating that it is 391 | governed by this License along with a term that is a further 392 | restriction, you may remove that term. If a license document contains 393 | a further restriction but permits relicensing or conveying under this 394 | License, you may add to a covered work material governed by the terms 395 | of that license document, provided that the further restriction does 396 | not survive such relicensing or conveying. 397 | 398 | If you add terms to a covered work in accord with this section, you 399 | must place, in the relevant source files, a statement of the 400 | additional terms that apply to those files, or a notice indicating 401 | where to find the applicable terms. 402 | 403 | Additional terms, permissive or non-permissive, may be stated in the 404 | form of a separately written license, or stated as exceptions; 405 | the above requirements apply either way. 406 | 407 | 8. Termination. 408 | 409 | You may not propagate or modify a covered work except as expressly 410 | provided under this License. Any attempt otherwise to propagate or 411 | modify it is void, and will automatically terminate your rights under 412 | this License (including any patent licenses granted under the third 413 | paragraph of section 11). 414 | 415 | However, if you cease all violation of this License, then your 416 | license from a particular copyright holder is reinstated (a) 417 | provisionally, unless and until the copyright holder explicitly and 418 | finally terminates your license, and (b) permanently, if the copyright 419 | holder fails to notify you of the violation by some reasonable means 420 | prior to 60 days after the cessation. 421 | 422 | Moreover, your license from a particular copyright holder is 423 | reinstated permanently if the copyright holder notifies you of the 424 | violation by some reasonable means, this is the first time you have 425 | received notice of violation of this License (for any work) from that 426 | copyright holder, and you cure the violation prior to 30 days after 427 | your receipt of the notice. 428 | 429 | Termination of your rights under this section does not terminate the 430 | licenses of parties who have received copies or rights from you under 431 | this License. If your rights have been terminated and not permanently 432 | reinstated, you do not qualify to receive new licenses for the same 433 | material under section 10. 434 | 435 | 9. Acceptance Not Required for Having Copies. 436 | 437 | You are not required to accept this License in order to receive or 438 | run a copy of the Program. Ancillary propagation of a covered work 439 | occurring solely as a consequence of using peer-to-peer transmission 440 | to receive a copy likewise does not require acceptance. However, 441 | nothing other than this License grants you permission to propagate or 442 | modify any covered work. These actions infringe copyright if you do 443 | not accept this License. Therefore, by modifying or propagating a 444 | covered work, you indicate your acceptance of this License to do so. 445 | 446 | 10. Automatic Licensing of Downstream Recipients. 447 | 448 | Each time you convey a covered work, the recipient automatically 449 | receives a license from the original licensors, to run, modify and 450 | propagate that work, subject to this License. You are not responsible 451 | for enforcing compliance by third parties with this License. 452 | 453 | An "entity transaction" is a transaction transferring control of an 454 | organization, or substantially all assets of one, or subdividing an 455 | organization, or merging organizations. If propagation of a covered 456 | work results from an entity transaction, each party to that 457 | transaction who receives a copy of the work also receives whatever 458 | licenses to the work the party's predecessor in interest had or could 459 | give under the previous paragraph, plus a right to possession of the 460 | Corresponding Source of the work from the predecessor in interest, if 461 | the predecessor has it or can get it with reasonable efforts. 462 | 463 | You may not impose any further restrictions on the exercise of the 464 | rights granted or affirmed under this License. For example, you may 465 | not impose a license fee, royalty, or other charge for exercise of 466 | rights granted under this License, and you may not initiate litigation 467 | (including a cross-claim or counterclaim in a lawsuit) alleging that 468 | any patent claim is infringed by making, using, selling, offering for 469 | sale, or importing the Program or any portion of it. 470 | 471 | 11. Patents. 472 | 473 | A "contributor" is a copyright holder who authorizes use under this 474 | License of the Program or a work on which the Program is based. The 475 | work thus licensed is called the contributor's "contributor version". 476 | 477 | A contributor's "essential patent claims" are all patent claims 478 | owned or controlled by the contributor, whether already acquired or 479 | hereafter acquired, that would be infringed by some manner, permitted 480 | by this License, of making, using, or selling its contributor version, 481 | but do not include claims that would be infringed only as a 482 | consequence of further modification of the contributor version. For 483 | purposes of this definition, "control" includes the right to grant 484 | patent sublicenses in a manner consistent with the requirements of 485 | this License. 486 | 487 | Each contributor grants you a non-exclusive, worldwide, royalty-free 488 | patent license under the contributor's essential patent claims, to 489 | make, use, sell, offer for sale, import and otherwise run, modify and 490 | propagate the contents of its contributor version. 491 | 492 | In the following three paragraphs, a "patent license" is any express 493 | agreement or commitment, however denominated, not to enforce a patent 494 | (such as an express permission to practice a patent or covenant not to 495 | sue for patent infringement). To "grant" such a patent license to a 496 | party means to make such an agreement or commitment not to enforce a 497 | patent against the party. 498 | 499 | If you convey a covered work, knowingly relying on a patent license, 500 | and the Corresponding Source of the work is not available for anyone 501 | to copy, free of charge and under the terms of this License, through a 502 | publicly available network server or other readily accessible means, 503 | then you must either (1) cause the Corresponding Source to be so 504 | available, or (2) arrange to deprive yourself of the benefit of the 505 | patent license for this particular work, or (3) arrange, in a manner 506 | consistent with the requirements of this License, to extend the patent 507 | license to downstream recipients. "Knowingly relying" means you have 508 | actual knowledge that, but for the patent license, your conveying the 509 | covered work in a country, or your recipient's use of the covered work 510 | in a country, would infringe one or more identifiable patents in that 511 | country that you have reason to believe are valid. 512 | 513 | If, pursuant to or in connection with a single transaction or 514 | arrangement, you convey, or propagate by procuring conveyance of, a 515 | covered work, and grant a patent license to some of the parties 516 | receiving the covered work authorizing them to use, propagate, modify 517 | or convey a specific copy of the covered work, then the patent license 518 | you grant is automatically extended to all recipients of the covered 519 | work and works based on it. 520 | 521 | A patent license is "discriminatory" if it does not include within 522 | the scope of its coverage, prohibits the exercise of, or is 523 | conditioned on the non-exercise of one or more of the rights that are 524 | specifically granted under this License. You may not convey a covered 525 | work if you are a party to an arrangement with a third party that is 526 | in the business of distributing software, under which you make payment 527 | to the third party based on the extent of your activity of conveying 528 | the work, and under which the third party grants, to any of the 529 | parties who would receive the covered work from you, a discriminatory 530 | patent license (a) in connection with copies of the covered work 531 | conveyed by you (or copies made from those copies), or (b) primarily 532 | for and in connection with specific products or compilations that 533 | contain the covered work, unless you entered into that arrangement, 534 | or that patent license was granted, prior to 28 March 2007. 535 | 536 | Nothing in this License shall be construed as excluding or limiting 537 | any implied license or other defenses to infringement that may 538 | otherwise be available to you under applicable patent law. 539 | 540 | 12. No Surrender of Others' Freedom. 541 | 542 | If conditions are imposed on you (whether by court order, agreement or 543 | otherwise) that contradict the conditions of this License, they do not 544 | excuse you from the conditions of this License. If you cannot convey a 545 | covered work so as to satisfy simultaneously your obligations under this 546 | License and any other pertinent obligations, then as a consequence you may 547 | not convey it at all. For example, if you agree to terms that obligate you 548 | to collect a royalty for further conveying from those to whom you convey 549 | the Program, the only way you could satisfy both those terms and this 550 | License would be to refrain entirely from conveying the Program. 551 | 552 | 13. Use with the GNU Affero General Public License. 553 | 554 | Notwithstanding any other provision of this License, you have 555 | permission to link or combine any covered work with a work licensed 556 | under version 3 of the GNU Affero General Public License into a single 557 | combined work, and to convey the resulting work. The terms of this 558 | License will continue to apply to the part which is the covered work, 559 | but the special requirements of the GNU Affero General Public License, 560 | section 13, concerning interaction through a network will apply to the 561 | combination as such. 562 | 563 | 14. Revised Versions of this License. 564 | 565 | The Free Software Foundation may publish revised and/or new versions of 566 | the GNU General Public License from time to time. Such new versions will 567 | be similar in spirit to the present version, but may differ in detail to 568 | address new problems or concerns. 569 | 570 | Each version is given a distinguishing version number. If the 571 | Program specifies that a certain numbered version of the GNU General 572 | Public License "or any later version" applies to it, you have the 573 | option of following the terms and conditions either of that numbered 574 | version or of any later version published by the Free Software 575 | Foundation. If the Program does not specify a version number of the 576 | GNU General Public License, you may choose any version ever published 577 | by the Free Software Foundation. 578 | 579 | If the Program specifies that a proxy can decide which future 580 | versions of the GNU General Public License can be used, that proxy's 581 | public statement of acceptance of a version permanently authorizes you 582 | to choose that version for the Program. 583 | 584 | Later license versions may give you additional or different 585 | permissions. However, no additional obligations are imposed on any 586 | author or copyright holder as a result of your choosing to follow a 587 | later version. 588 | 589 | 15. Disclaimer of Warranty. 590 | 591 | THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY 592 | APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT 593 | HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY 594 | OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, 595 | THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 596 | PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM 597 | IS WITH YOU. SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF 598 | ALL NECESSARY SERVICING, REPAIR OR CORRECTION. 599 | 600 | 16. Limitation of Liability. 601 | 602 | IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING 603 | WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS 604 | THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY 605 | GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE 606 | USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF 607 | DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD 608 | PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS), 609 | EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF 610 | SUCH DAMAGES. 611 | 612 | 17. Interpretation of Sections 15 and 16. 613 | 614 | If the disclaimer of warranty and limitation of liability provided 615 | above cannot be given local legal effect according to their terms, 616 | reviewing courts shall apply local law that most closely approximates 617 | an absolute waiver of all civil liability in connection with the 618 | Program, unless a warranty or assumption of liability accompanies a 619 | copy of the Program in return for a fee. 620 | 621 | END OF TERMS AND CONDITIONS 622 | 623 | How to Apply These Terms to Your New Programs 624 | 625 | If you develop a new program, and you want it to be of the greatest 626 | possible use to the public, the best way to achieve this is to make it 627 | free software which everyone can redistribute and change under these terms. 628 | 629 | To do so, attach the following notices to the program. It is safest 630 | to attach them to the start of each source file to most effectively 631 | state the exclusion of warranty; and each file should have at least 632 | the "copyright" line and a pointer to where the full notice is found. 633 | 634 | 635 | Copyright (C) 636 | 637 | This program is free software: you can redistribute it and/or modify 638 | it under the terms of the GNU General Public License as published by 639 | the Free Software Foundation, either version 3 of the License, or 640 | (at your option) any later version. 641 | 642 | This program is distributed in the hope that it will be useful, 643 | but WITHOUT ANY WARRANTY; without even the implied warranty of 644 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 645 | GNU General Public License for more details. 646 | 647 | You should have received a copy of the GNU General Public License 648 | along with this program. If not, see . 649 | 650 | Also add information on how to contact you by electronic and paper mail. 651 | 652 | If the program does terminal interaction, make it output a short 653 | notice like this when it starts in an interactive mode: 654 | 655 | Copyright (C) 656 | This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'. 657 | This is free software, and you are welcome to redistribute it 658 | under certain conditions; type `show c' for details. 659 | 660 | The hypothetical commands `show w' and `show c' should show the appropriate 661 | parts of the General Public License. Of course, your program's commands 662 | might be different; for a GUI interface, you would use an "about box". 663 | 664 | You should also get your employer (if you work as a programmer) or school, 665 | if any, to sign a "copyright disclaimer" for the program, if necessary. 666 | For more information on this, and how to apply and follow the GNU GPL, see 667 | . 668 | 669 | The GNU General Public License does not permit incorporating your program 670 | into proprietary programs. If your program is a subroutine library, you 671 | may consider it more useful to permit linking proprietary applications with 672 | the library. If this is what you want to do, use the GNU Lesser General 673 | Public License instead of this License. But first, please read 674 | . 675 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # patchwork-plusplus-ros 2 | 3 | This is ROS package of Patchwork++ (@ IROS'22), which is a fast and robust ground segmentation method. 4 | 5 |

animated

6 | 7 | > If you are not familiar with ROS, please visit the [original repository][patchwork++link]. 8 | 9 | > If you follow the [repository][patchwork++link], you can run Patchwork++ in Python and C++ easily. 10 | 11 | [patchwork++link]: https://github.com/url-kaist/patchwork-plusplus 12 | 13 | ## :open_file_folder: What's in this repository 14 | 15 | * ROS based Patchwork source code ([patchworkpp.hpp][codelink]) 16 | * Demo launch file ([demo.launch][launchlink]) with sample rosbag file. You can execute Patchwork++ simply! 17 | 18 | [codelink]: https://github.com/url-kaist/patchwork-plusplus-ros/blob/master/include/patchworkpp/patchworkpp.hpp 19 | [launchlink]: https://github.com/url-kaist/patchwork-plusplus-ros/blob/master/launch/demo.launch 20 | 21 | ## :package: Prerequisite packages 22 | You may need to install ROS, PCL, Eigen, ... 23 | 24 | ## :gear: How to build Patchwork++ 25 | To build Patchwork++, you can follow below codes. 26 | 27 | ```bash 28 | $ mkdir -p ~/catkin_ws/src 29 | $ cd ~/catkin_ws 30 | $ catkin build # or catkin_make 31 | ``` 32 | 33 | ## :runner: To run the demo codes 34 | There is a demo which executes Patchwork++ with sample rosbag file. You can download a sample file with the following command. 35 | 36 | > For the sample rosbag data, I utilizes [semantickitti2bag](https://github.com/amslabtech/semantickitti2bag) package. 37 | 38 | ```bash 39 | $ wget https://urserver.kaist.ac.kr/publicdata/patchwork++/kitti_00_sample.bag 40 | ``` 41 | > If you have any trouble to download the file by the above command, please click [here][kitti_sample_link] to download the file directly. 42 | 43 | [kitti_sample_link]: https://urserver.kaist.ac.kr/publicdata/patchwork++/kitti_00_sample.bag 44 | 45 | > The rosbag file is based on the [KITTI][kittilink] dataset. The bin files are merged into the rosbag file format. 46 | 47 | > The sample file contains LiDAR sensor data only. 48 | 49 | [kittilink]: http://www.cvlibs.net/datasets/kitti/raw_data.php 50 | 51 | Then, you can run demo as follows. 52 | 53 | ```bash 54 | # Start Patchwork++ 55 | $ roslaunch patchworkpp demo.launch 56 | # Start the bag file 57 | $ rosbag play kitti_00_sample.bag 58 | ``` 59 | 60 | ## :pushpin: TODO List 61 | - [ ] Update additional demo codes processing data with .bin file format 62 | - [ ] Generalize point type in the source code 63 | - [ ] Add visualization result of demo codes in readme 64 | 65 | ## Citation 66 | If you use our codes, please cite our [paper][patchwork++arXivLink]. 67 | 68 | In addition, you can also check the paper of our baseline(Patchwork) [here][patchworkarXivlink]. 69 | 70 | [patchwork++arXivLink]: https://arxiv.org/abs/2207.11919 71 | [patchworkarXivlink]: https://arxiv.org/abs/2108.05560 72 | 73 | ``` 74 | @inproceedings{lee2022patchworkpp, 75 | title={{Patchwork++: Fast and robust ground segmentation solving partial under-segmentation using 3D point cloud}}, 76 | author={Lee, Seungjae and Lim, Hyungtae and Myung, Hyun}, 77 | booktitle={Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst.}, 78 | year={2022}, 79 | note={{Submitted}} 80 | } 81 | ``` 82 | ``` 83 | @article{lim2021patchwork, 84 | title={Patchwork: Concentric Zone-based Region-wise Ground Segmentation with Ground Likelihood Estimation Using a 3D LiDAR Sensor}, 85 | author={Lim, Hyungtae and Minho, Oh and Myung, Hyun}, 86 | journal={IEEE Robotics and Automation Letters}, 87 | year={2021} 88 | } 89 | ``` 90 | 91 | ## :postbox: Contact 92 | If you have any question, don't be hesitate let us know! 93 | 94 | * [Seungjae Lee][sjlink] :envelope: (sj98lee at kaist.ac.kr) 95 | * [Hyungtae Lim][htlink] :envelope: (shapelim at kaist.ac.kr) 96 | 97 | [sjlink]: https://github.com/seungjae24 98 | [htlink]: https://github.com/LimHyungTae 99 | 100 | -------------------------------------------------------------------------------- /config/params.yaml: -------------------------------------------------------------------------------- 1 | save_flag: true 2 | 3 | # patchworkpp: 4 | 5 | sensor_height: 1.723 6 | 7 | mode: "czm" 8 | verbose: false # To check effect of uprightness/elevation/flatness 9 | visualize: true # Ground Likelihood Estimation is visualized 10 | 11 | # Ground Plane Fitting parameters 12 | num_iter: 3 # Number of iterations for ground plane estimation using PCA. 13 | num_lpr: 20 # Maximum number of points to be selected as lowest points representative. 14 | num_min_pts: 10 # Minimum number of points to be estimated as ground plane in each patch. 15 | th_seeds: 0.3 # threshold for lowest point representatives using in initial seeds selection of ground points. 16 | th_dist: 0.125 # threshold for thickenss of ground. 17 | th_seeds_v: 0.25 # threshold for lowest point representatives using in initial seeds selection of vertical structural points. 18 | th_dist_v: 0.1 # threshold for thickenss of vertical structure. 19 | max_r: 80.0 # max_range of ground estimation area 20 | min_r: 2.7 # min_range of ground estimation area 21 | uprightness_thr: 0.707 # threshold of uprightness using in Ground Likelihood Estimation(GLE). Please refer paper for more information about GLE. 22 | 23 | adaptive_seed_selection_margin: -1.2 # The points below the adaptive_seed_selection_margin * sensor_height are filtered 24 | czm: 25 | num_zones: 4 26 | num_sectors_each_zone: [16, 32, 54, 32] 27 | mum_rings_each_zone: [2, 4, 4, 4] 28 | elevation_thresholds: [0.0, 0.0, 0.0, 0.0] # threshold of elevation for each ring using in GLE. Those values are updated adaptively. 29 | flatness_thresholds: [0.0, 0.0, 0.0, 0.0] # threshold of flatness for each ring using in GLE. Those values are updated adaptively. 30 | 31 | enable_RNR : true 32 | enable_RVPF : true 33 | enable_TGR : true 34 | 35 | RNR_ver_angle_thr : -15.0 # Noise points vertical angle threshold. Downward rays of LiDAR are more likely to generate severe noise points. 36 | RNR_intensity_thr : 0.2 # Noise points intensity threshold. The reflected points have relatively small intensity than others. 37 | -------------------------------------------------------------------------------- /include/jsk_recognition_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(jsk_recognition_msgs) 3 | find_package(catkin REQUIRED 4 | std_msgs sensor_msgs geometry_msgs message_generation pcl_msgs) 5 | 6 | add_message_files( 7 | FILES 8 | PolygonArray.msg 9 | ) 10 | 11 | generate_messages( 12 | DEPENDENCIES 13 | std_msgs sensor_msgs geometry_msgs 14 | ) 15 | catkin_package( 16 | CATKIN_DEPENDS std_msgs sensor_msgs geometry_msgs 17 | ) 18 | -------------------------------------------------------------------------------- /include/jsk_recognition_msgs/msg/PolygonArray.msg: -------------------------------------------------------------------------------- 1 | # PolygonArray is a list of PolygonStamped. 2 | # You can use jsk_rviz_plugins to visualize PolygonArray on rviz. 3 | Header header 4 | geometry_msgs/PolygonStamped[] polygons 5 | uint32[] labels 6 | float32[] likelihood 7 | -------------------------------------------------------------------------------- /include/jsk_recognition_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | jsk_recognition_msgs 4 | 1.2.15 5 | ROS messages for jsk_pcl_ros and jsk_perception. 6 | 7 | http://github.com/jsk-ros-pkg/jsk_recognition 8 | http://github.com/jsk-ros-pkg/jsk_recognition/issues 9 | https://jsk-docs.readthedocs.io/projects/jsk_recognition/en/latest/jsk_recognition_msgs 10 | 11 | 12 | Ryohei Ueda 13 | BSD 14 | catkin 15 | std_msgs 16 | std_msgs 17 | geometry_msgs 18 | geometry_msgs 19 | sensor_msgs 20 | sensor_msgs 21 | message_generation 22 | message_generation 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /include/patchworkpp/patchworkpp.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file patchworkpp.hpp 3 | * @author Seungjae Lee 4 | * @brief 5 | * @version 0.1 6 | * @date 2022-07-20 7 | * 8 | * @copyright Copyright (c) 2022 9 | * 10 | */ 11 | #ifndef PATCHWORKPP_H 12 | #define PATCHWORKPP_H 13 | 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | #include 24 | 25 | #define MARKER_Z_VALUE -2.2 26 | #define UPRIGHT_ENOUGH 0.55 27 | #define FLAT_ENOUGH 0.2 28 | #define TOO_HIGH_ELEVATION 0.0 29 | #define TOO_TILTED 1.0 30 | 31 | #define NUM_HEURISTIC_MAX_PTS_IN_PATCH 3000 32 | 33 | using Eigen::MatrixXf; 34 | using Eigen::JacobiSVD; 35 | using Eigen::VectorXf; 36 | 37 | using namespace std; 38 | 39 | /* 40 | @brief PathWork ROS Node. 41 | */ 42 | template 43 | bool point_z_cmp(PointT a, PointT b) { return a.z < b.z; } 44 | 45 | template 46 | struct RevertCandidate 47 | { 48 | int concentric_idx; 49 | int sector_idx; 50 | double ground_flatness; 51 | double line_variable; 52 | Eigen::Vector4f pc_mean; 53 | pcl::PointCloud regionwise_ground; 54 | 55 | RevertCandidate(int _c_idx, int _s_idx, double _flatness, double _line_var, Eigen::Vector4f _pc_mean, pcl::PointCloud _ground) 56 | : concentric_idx(_c_idx), sector_idx(_s_idx), ground_flatness(_flatness), line_variable(_line_var), pc_mean(_pc_mean), regionwise_ground(_ground) {} 57 | }; 58 | 59 | template 60 | class PatchWorkpp { 61 | 62 | public: 63 | typedef std::vector> Ring; 64 | typedef std::vector Zone; 65 | 66 | PatchWorkpp() {}; 67 | 68 | PatchWorkpp(ros::NodeHandle *nh) : node_handle_(*nh) { 69 | // Init ROS related 70 | ROS_INFO("Inititalizing PatchWork++..."); 71 | 72 | node_handle_.param("verbose", verbose_, false); 73 | 74 | node_handle_.param("sensor_height", sensor_height_, 1.723); 75 | node_handle_.param("num_iter", num_iter_, 3); 76 | node_handle_.param("num_lpr", num_lpr_, 20); 77 | node_handle_.param("num_min_pts", num_min_pts_, 10); 78 | node_handle_.param("th_seeds", th_seeds_, 0.4); 79 | node_handle_.param("th_dist", th_dist_, 0.3); 80 | node_handle_.param("th_seeds_v", th_seeds_v_, 0.4); 81 | node_handle_.param("th_dist_v", th_dist_v_, 0.3); 82 | node_handle_.param("max_r", max_range_, 80.0); 83 | node_handle_.param("min_r", min_range_, 2.7); 84 | node_handle_.param("uprightness_thr", uprightness_thr_, 0.5); 85 | node_handle_.param("adaptive_seed_selection_margin", adaptive_seed_selection_margin_, -1.1); 86 | node_handle_.param("RNR_ver_angle_thr", RNR_ver_angle_thr_, -15.0); 87 | node_handle_.param("RNR_intensity_thr", RNR_intensity_thr_, 0.2); 88 | node_handle_.param("max_flatness_storage", max_flatness_storage_, 1000); 89 | node_handle_.param("max_elevation_storage", max_elevation_storage_, 1000); 90 | node_handle_.param("enable_RNR", enable_RNR_, true); 91 | node_handle_.param("enable_RVPF", enable_RVPF_, true); 92 | node_handle_.param("enable_TGR", enable_TGR_, true); 93 | 94 | ROS_INFO("Sensor Height: %f", sensor_height_); 95 | ROS_INFO("Num of Iteration: %d", num_iter_); 96 | ROS_INFO("Num of LPR: %d", num_lpr_); 97 | ROS_INFO("Num of min. points: %d", num_min_pts_); 98 | ROS_INFO("Seeds Threshold: %f", th_seeds_); 99 | ROS_INFO("Distance Threshold: %f", th_dist_); 100 | ROS_INFO("Max. range:: %f", max_range_); 101 | ROS_INFO("Min. range:: %f", min_range_); 102 | ROS_INFO("Normal vector threshold: %f", uprightness_thr_); 103 | ROS_INFO("adaptive_seed_selection_margin: %f", adaptive_seed_selection_margin_); 104 | 105 | // CZM denotes 'Concentric Zone Model'. Please refer to our paper 106 | node_handle_.getParam("czm/num_zones", num_zones_); 107 | node_handle_.getParam("czm/num_sectors_each_zone", num_sectors_each_zone_); 108 | node_handle_.getParam("czm/mum_rings_each_zone", num_rings_each_zone_); 109 | node_handle_.getParam("czm/elevation_thresholds", elevation_thr_); 110 | node_handle_.getParam("czm/flatness_thresholds", flatness_thr_); 111 | 112 | ROS_INFO("Num. zones: %d", num_zones_); 113 | 114 | if (num_zones_ != 4 || num_sectors_each_zone_.size() != num_rings_each_zone_.size()) { 115 | throw invalid_argument("Some parameters are wrong! Check the num_zones and num_rings/sectors_each_zone"); 116 | } 117 | if (elevation_thr_.size() != flatness_thr_.size()) { 118 | throw invalid_argument("Some parameters are wrong! Check the elevation/flatness_thresholds"); 119 | } 120 | 121 | cout << (boost::format("Num. sectors: %d, %d, %d, %d") % num_sectors_each_zone_[0] % num_sectors_each_zone_[1] % 122 | num_sectors_each_zone_[2] % 123 | num_sectors_each_zone_[3]).str() << endl; 124 | cout << (boost::format("Num. rings: %01d, %01d, %01d, %01d") % num_rings_each_zone_[0] % 125 | num_rings_each_zone_[1] % 126 | num_rings_each_zone_[2] % 127 | num_rings_each_zone_[3]).str() << endl; 128 | cout << (boost::format("elevation_thr_: %0.4f, %0.4f, %0.4f, %0.4f ") % elevation_thr_[0] % elevation_thr_[1] % 129 | elevation_thr_[2] % 130 | elevation_thr_[3]).str() << endl; 131 | cout << (boost::format("flatness_thr_: %0.4f, %0.4f, %0.4f, %0.4f ") % flatness_thr_[0] % flatness_thr_[1] % 132 | flatness_thr_[2] % 133 | flatness_thr_[3]).str() << endl; 134 | num_rings_of_interest_ = elevation_thr_.size(); 135 | 136 | node_handle_.param("visualize", visualize_, true); 137 | 138 | int num_polygons = std::inner_product(num_rings_each_zone_.begin(), num_rings_each_zone_.end(), num_sectors_each_zone_.begin(), 0); 139 | poly_list_.header.frame_id = "map"; 140 | poly_list_.polygons.reserve(num_polygons); 141 | 142 | revert_pc_.reserve(NUM_HEURISTIC_MAX_PTS_IN_PATCH); 143 | ground_pc_.reserve(NUM_HEURISTIC_MAX_PTS_IN_PATCH); 144 | regionwise_ground_.reserve(NUM_HEURISTIC_MAX_PTS_IN_PATCH); 145 | regionwise_nonground_.reserve(NUM_HEURISTIC_MAX_PTS_IN_PATCH); 146 | 147 | PlaneViz = node_handle_.advertise("plane", 100, true); 148 | pub_revert_pc = node_handle_.advertise("revert_pc", 100, true); 149 | pub_reject_pc = node_handle_.advertise("reject_pc", 100, true); 150 | pub_normal = node_handle_.advertise("normals", 100, true); 151 | pub_noise = node_handle_.advertise("noise", 100, true); 152 | pub_vertical = node_handle_.advertise("vertical", 100, true); 153 | 154 | min_range_z2_ = (7 * min_range_ + max_range_) / 8.0; 155 | min_range_z3_ = (3 * min_range_ + max_range_) / 4.0; 156 | min_range_z4_ = (min_range_ + max_range_) / 2.0; 157 | 158 | min_ranges_ = {min_range_, min_range_z2_, min_range_z3_, min_range_z4_}; 159 | ring_sizes_ = {(min_range_z2_ - min_range_) / num_rings_each_zone_.at(0), 160 | (min_range_z3_ - min_range_z2_) / num_rings_each_zone_.at(1), 161 | (min_range_z4_ - min_range_z3_) / num_rings_each_zone_.at(2), 162 | (max_range_ - min_range_z4_) / num_rings_each_zone_.at(3)}; 163 | sector_sizes_ = {2 * M_PI / num_sectors_each_zone_.at(0), 2 * M_PI / num_sectors_each_zone_.at(1), 164 | 2 * M_PI / num_sectors_each_zone_.at(2), 165 | 2 * M_PI / num_sectors_each_zone_.at(3)}; 166 | 167 | cout << "INITIALIZATION COMPLETE" << endl; 168 | 169 | for (int i = 0; i < num_zones_; i++) { 170 | Zone z; 171 | initialize_zone(z, num_sectors_each_zone_[i], num_rings_each_zone_[i]); 172 | ConcentricZoneModel_.push_back(z); 173 | } 174 | } 175 | 176 | void estimate_ground(pcl::PointCloud cloud_in, 177 | pcl::PointCloud &cloud_ground, pcl::PointCloud &cloud_nonground, double &time_taken); 178 | 179 | private: 180 | 181 | // Every private member variable is written with the undescore("_") in its end. 182 | 183 | ros::NodeHandle node_handle_; 184 | 185 | std::recursive_mutex mutex_; 186 | 187 | int num_iter_; 188 | int num_lpr_; 189 | int num_min_pts_; 190 | int num_zones_; 191 | int num_rings_of_interest_; 192 | 193 | double sensor_height_; 194 | double th_seeds_; 195 | double th_dist_; 196 | double th_seeds_v_; 197 | double th_dist_v_; 198 | double max_range_; 199 | double min_range_; 200 | double uprightness_thr_; 201 | double adaptive_seed_selection_margin_; 202 | double min_range_z2_; // 12.3625 203 | double min_range_z3_; // 22.025 204 | double min_range_z4_; // 41.35 205 | double RNR_ver_angle_thr_; 206 | double RNR_intensity_thr_; 207 | 208 | bool verbose_; 209 | bool enable_RNR_; 210 | bool enable_RVPF_; 211 | bool enable_TGR_; 212 | 213 | int max_flatness_storage_, max_elevation_storage_; 214 | std::vector update_flatness_[4]; 215 | std::vector update_elevation_[4]; 216 | 217 | float d_; 218 | 219 | VectorXf normal_; 220 | MatrixXf pnormal_; 221 | VectorXf singular_values_; 222 | Eigen::Matrix3f cov_; 223 | Eigen::Vector4f pc_mean_; 224 | 225 | // For visualization 226 | bool visualize_; 227 | 228 | vector num_sectors_each_zone_; 229 | vector num_rings_each_zone_; 230 | 231 | vector sector_sizes_; 232 | vector ring_sizes_; 233 | vector min_ranges_; 234 | vector elevation_thr_; 235 | vector flatness_thr_; 236 | 237 | queue noise_idxs_; 238 | 239 | vector ConcentricZoneModel_; 240 | 241 | jsk_recognition_msgs::PolygonArray poly_list_; 242 | 243 | ros::Publisher PlaneViz, pub_revert_pc, pub_reject_pc, pub_normal, pub_noise, pub_vertical; 244 | pcl::PointCloud revert_pc_, reject_pc_, noise_pc_, vertical_pc_; 245 | pcl::PointCloud ground_pc_; 246 | 247 | pcl::PointCloud normals_; 248 | 249 | pcl::PointCloud regionwise_ground_, regionwise_nonground_; 250 | 251 | void initialize_zone(Zone &z, int num_sectors, int num_rings); 252 | 253 | void flush_patches_in_zone(Zone &patches, int num_sectors, int num_rings); 254 | void flush_patches(std::vector &czm); 255 | 256 | void pc2czm(const pcl::PointCloud &src, std::vector &czm, pcl::PointCloud &cloud_nonground); 257 | 258 | void reflected_noise_removal(pcl::PointCloud &cloud, pcl::PointCloud &cloud_nonground); 259 | 260 | void temporal_ground_revert(pcl::PointCloud &cloud_ground, pcl::PointCloud &cloud_nonground, 261 | std::vector ring_flatness, std::vector> candidates, 262 | int concentric_idx); 263 | 264 | void calc_mean_stdev(std::vector vec, double &mean, double &stdev); 265 | 266 | void update_elevation_thr(); 267 | void update_flatness_thr(); 268 | 269 | double xy2theta(const double &x, const double &y); 270 | 271 | double xy2radius(const double &x, const double &y); 272 | 273 | void estimate_plane(const pcl::PointCloud &ground); 274 | 275 | void extract_piecewiseground( 276 | const int zone_idx, const pcl::PointCloud &src, 277 | pcl::PointCloud &dst, 278 | pcl::PointCloud &non_ground_dst); 279 | 280 | void extract_initial_seeds( 281 | const int zone_idx, const pcl::PointCloud &p_sorted, 282 | pcl::PointCloud &init_seeds); 283 | 284 | void extract_initial_seeds( 285 | const int zone_idx, const pcl::PointCloud &p_sorted, 286 | pcl::PointCloud &init_seeds, double th_seed); 287 | 288 | /*** 289 | * For visulization of Ground Likelihood Estimation 290 | */ 291 | geometry_msgs::PolygonStamped set_polygons(int zone_idx, int r_idx, int theta_idx, int num_split); 292 | 293 | void set_ground_likelihood_estimation_status( 294 | const int zone_idx, const int ring_idx, 295 | const int concentric_idx, 296 | const double z_vec, 297 | const double z_elevation, 298 | const double ground_flatness); 299 | 300 | }; 301 | 302 | template inline 303 | void PatchWorkpp::initialize_zone(Zone &z, int num_sectors, int num_rings) { 304 | z.clear(); 305 | pcl::PointCloud cloud; 306 | cloud.reserve(1000); 307 | Ring ring; 308 | for (int i = 0; i < num_sectors; i++) { 309 | ring.emplace_back(cloud); 310 | } 311 | for (int j = 0; j < num_rings; j++) { 312 | z.emplace_back(ring); 313 | } 314 | } 315 | 316 | template inline 317 | void PatchWorkpp::flush_patches_in_zone(Zone &patches, int num_sectors, int num_rings) { 318 | for (int i = 0; i < num_sectors; i++) { 319 | for (int j = 0; j < num_rings; j++) { 320 | if (!patches[j][i].points.empty()) patches[j][i].points.clear(); 321 | } 322 | } 323 | } 324 | 325 | template inline 326 | void PatchWorkpp::flush_patches(vector &czm) { 327 | for (int k = 0; k < num_zones_; k++) { 328 | for (int i = 0; i < num_rings_each_zone_[k]; i++) { 329 | for (int j = 0; j < num_sectors_each_zone_[k]; j++) { 330 | if (!czm[k][i][j].points.empty()) czm[k][i][j].points.clear(); 331 | } 332 | } 333 | } 334 | 335 | if( verbose_ ) cout << "Flushed patches" << endl; 336 | } 337 | 338 | template inline 339 | void PatchWorkpp::estimate_plane(const pcl::PointCloud &ground) { 340 | pcl::computeMeanAndCovarianceMatrix(ground, cov_, pc_mean_); 341 | // Singular Value Decomposition: SVD 342 | Eigen::JacobiSVD svd(cov_, Eigen::DecompositionOptions::ComputeFullU); 343 | singular_values_ = svd.singularValues(); 344 | 345 | // use the least singular vector as normal 346 | normal_ = (svd.matrixU().col(2)); 347 | 348 | if (normal_(2) < 0) { for(int i=0; i<3; i++) normal_(i) *= -1; } 349 | 350 | // mean ground seeds value 351 | Eigen::Vector3f seeds_mean = pc_mean_.head<3>(); 352 | 353 | // according to normal.T*[x,y,z] = -d 354 | d_ = -(normal_.transpose() * seeds_mean)(0, 0); 355 | } 356 | 357 | template inline 358 | void PatchWorkpp::extract_initial_seeds( 359 | const int zone_idx, const pcl::PointCloud &p_sorted, 360 | pcl::PointCloud &init_seeds, double th_seed) { 361 | init_seeds.points.clear(); 362 | 363 | // LPR is the mean of low point representative 364 | double sum = 0; 365 | int cnt = 0; 366 | 367 | int init_idx = 0; 368 | if (zone_idx == 0) { 369 | for (int i = 0; i < p_sorted.points.size(); i++) { 370 | if (p_sorted.points[i].z < adaptive_seed_selection_margin_ * sensor_height_) { 371 | ++init_idx; 372 | } else { 373 | break; 374 | } 375 | } 376 | } 377 | 378 | // Calculate the mean height value. 379 | for (int i = init_idx; i < p_sorted.points.size() && cnt < num_lpr_; i++) { 380 | sum += p_sorted.points[i].z; 381 | cnt++; 382 | } 383 | double lpr_height = cnt != 0 ? sum / cnt : 0;// in case divide by 0 384 | 385 | // iterate pointcloud, filter those height is less than lpr.height+th_seeds_ 386 | for (int i = 0; i < p_sorted.points.size(); i++) { 387 | if (p_sorted.points[i].z < lpr_height + th_seed) { 388 | init_seeds.points.push_back(p_sorted.points[i]); 389 | } 390 | } 391 | } 392 | 393 | template inline 394 | void PatchWorkpp::extract_initial_seeds( 395 | const int zone_idx, const pcl::PointCloud &p_sorted, 396 | pcl::PointCloud &init_seeds) { 397 | init_seeds.points.clear(); 398 | 399 | // LPR is the mean of low point representative 400 | double sum = 0; 401 | int cnt = 0; 402 | 403 | int init_idx = 0; 404 | if (zone_idx == 0) { 405 | for (int i = 0; i < p_sorted.points.size(); i++) { 406 | if (p_sorted.points[i].z < adaptive_seed_selection_margin_ * sensor_height_) { 407 | ++init_idx; 408 | } else { 409 | break; 410 | } 411 | } 412 | } 413 | 414 | // Calculate the mean height value. 415 | for (int i = init_idx; i < p_sorted.points.size() && cnt < num_lpr_; i++) { 416 | sum += p_sorted.points[i].z; 417 | cnt++; 418 | } 419 | double lpr_height = cnt != 0 ? sum / cnt : 0;// in case divide by 0 420 | 421 | // iterate pointcloud, filter those height is less than lpr.height+th_seeds_ 422 | for (int i = 0; i < p_sorted.points.size(); i++) { 423 | if (p_sorted.points[i].z < lpr_height + th_seeds_) { 424 | init_seeds.points.push_back(p_sorted.points[i]); 425 | } 426 | } 427 | } 428 | 429 | template inline 430 | void PatchWorkpp::reflected_noise_removal(pcl::PointCloud &cloud_in, pcl::PointCloud &cloud_nonground) 431 | { 432 | for (int i=0; i Pointcloud -> z-value sorted Pointcloud 452 | ->error points removal -> extract ground seeds -> ground plane fit mainloop 453 | */ 454 | 455 | template inline 456 | void PatchWorkpp::estimate_ground( 457 | pcl::PointCloud cloud_in, 458 | pcl::PointCloud &cloud_ground, 459 | pcl::PointCloud &cloud_nonground, 460 | double &time_taken) { 461 | 462 | unique_lock lock(mutex_); 463 | 464 | poly_list_.header.stamp = ros::Time::now(); 465 | poly_list_.header.frame_id = cloud_in.header.frame_id; 466 | if (!poly_list_.polygons.empty()) poly_list_.polygons.clear(); 467 | if (!poly_list_.likelihood.empty()) poly_list_.likelihood.clear(); 468 | 469 | static double start, t0, t1, t2, end; 470 | 471 | double pca_time_ = 0.0; 472 | double t_revert = 0.0; 473 | double t_total_ground = 0.0; 474 | double t_total_estimate = 0.0; 475 | 476 | start = ros::Time::now().toSec(); 477 | 478 | cloud_ground.clear(); 479 | cloud_nonground.clear(); 480 | 481 | // 1. Reflected Noise Removal (RNR) 482 | if (enable_RNR_) reflected_noise_removal(cloud_in, cloud_nonground); 483 | 484 | t1 = ros::Time::now().toSec(); 485 | 486 | // 2. Concentric Zone Model (CZM) 487 | flush_patches(ConcentricZoneModel_); 488 | pc2czm(cloud_in, ConcentricZoneModel_, cloud_nonground); 489 | 490 | t2 = ros::Time::now().toSec(); 491 | 492 | int concentric_idx = 0; 493 | 494 | double t_sort = 0; 495 | 496 | std::vector> candidates; 497 | std::vector ringwise_flatness; 498 | 499 | for (int zone_idx = 0; zone_idx < num_zones_; ++zone_idx) { 500 | 501 | auto zone = ConcentricZoneModel_[zone_idx]; 502 | 503 | for (int ring_idx = 0; ring_idx < num_rings_each_zone_[zone_idx]; ++ring_idx) { 504 | for (int sector_idx = 0; sector_idx < num_sectors_each_zone_[zone_idx]; ++sector_idx) { 505 | 506 | if (zone[ring_idx][sector_idx].points.size() < num_min_pts_) 507 | { 508 | cloud_nonground += zone[ring_idx][sector_idx]; 509 | continue; 510 | } 511 | 512 | // --------- region-wise sorting (faster than global sorting method) ---------------- // 513 | double t_sort_0 = ros::Time::now().toSec(); 514 | 515 | sort(zone[ring_idx][sector_idx].points.begin(), zone[ring_idx][sector_idx].points.end(), point_z_cmp); 516 | 517 | double t_sort_1 = ros::Time::now().toSec(); 518 | t_sort += (t_sort_1 - t_sort_0); 519 | // ---------------------------------------------------------------------------------- // 520 | 521 | double t_tmp0 = ros::Time::now().toSec(); 522 | extract_piecewiseground(zone_idx, zone[ring_idx][sector_idx], regionwise_ground_, regionwise_nonground_); 523 | 524 | double t_tmp1 = ros::Time::now().toSec(); 525 | t_total_ground += t_tmp1 - t_tmp0; 526 | pca_time_ += t_tmp1 - t_tmp0; 527 | 528 | // Status of each patch 529 | // used in checking uprightness, elevation, and flatness, respectively 530 | const double ground_uprightness = normal_(2); 531 | const double ground_elevation = pc_mean_(2, 0); 532 | const double ground_flatness = singular_values_.minCoeff(); 533 | const double line_variable = singular_values_(1) != 0 ? singular_values_(0)/singular_values_(1) : std::numeric_limits::max(); 534 | 535 | double heading = 0.0; 536 | for(int i=0; i<3; i++) heading += pc_mean_(i,0)*normal_(i); 537 | 538 | if (visualize_) { 539 | auto polygons = set_polygons(zone_idx, ring_idx, sector_idx, 3); 540 | polygons.header = poly_list_.header; 541 | poly_list_.polygons.push_back(polygons); 542 | set_ground_likelihood_estimation_status(zone_idx, ring_idx, concentric_idx, ground_uprightness, ground_elevation, ground_flatness); 543 | 544 | pcl::PointXYZINormal tmp_p; 545 | tmp_p.x = pc_mean_(0,0); 546 | tmp_p.y = pc_mean_(1,0); 547 | tmp_p.z = pc_mean_(2,0); 548 | tmp_p.normal_x = normal_(0); 549 | tmp_p.normal_y = normal_(1); 550 | tmp_p.normal_z = normal_(2); 551 | normals_.points.emplace_back(tmp_p); 552 | } 553 | 554 | double t_tmp2 = ros::Time::now().toSec(); 555 | 556 | /* 557 | About 'is_heading_outside' condidition, heading should be smaller than 0 theoretically. 558 | ( Imagine the geometric relationship between the surface normal vector on the ground plane and 559 | the vector connecting the sensor origin and the mean point of the ground plane ) 560 | 561 | However, when the patch is far awaw from the sensor origin, 562 | heading could be larger than 0 even if it's ground due to lack of amount of ground plane points. 563 | 564 | Therefore, we only check this value when concentric_idx < num_rings_of_interest ( near condition ) 565 | */ 566 | bool is_upright = ground_uprightness > uprightness_thr_; 567 | bool is_not_elevated = ground_elevation < elevation_thr_[concentric_idx]; 568 | bool is_flat = ground_flatness < flatness_thr_[concentric_idx]; 569 | bool is_near_zone = concentric_idx < num_rings_of_interest_; 570 | bool is_heading_outside = heading < 0.0; 571 | 572 | /* 573 | Store the elevation & flatness variables 574 | for A-GLE (Adaptive Ground Likelihood Estimation) 575 | and TGR (Temporal Ground Revert). More information in the paper Patchwork++. 576 | */ 577 | if (is_upright && is_not_elevated && is_near_zone) 578 | { 579 | update_elevation_[concentric_idx].push_back(ground_elevation); 580 | update_flatness_[concentric_idx].push_back(ground_flatness); 581 | 582 | ringwise_flatness.push_back(ground_flatness); 583 | } 584 | 585 | // Ground estimation based on conditions 586 | if (!is_upright) 587 | { 588 | cloud_nonground += regionwise_ground_; 589 | } 590 | else if (!is_near_zone) 591 | { 592 | cloud_ground += regionwise_ground_; 593 | } 594 | else if (!is_heading_outside) 595 | { 596 | cloud_nonground += regionwise_ground_; 597 | } 598 | else if (is_not_elevated || is_flat) 599 | { 600 | cloud_ground += regionwise_ground_; 601 | } 602 | else 603 | { 604 | RevertCandidate candidate(concentric_idx, sector_idx, ground_flatness, line_variable, pc_mean_, regionwise_ground_); 605 | candidates.push_back(candidate); 606 | } 607 | // Every regionwise_nonground is considered nonground. 608 | cloud_nonground += regionwise_nonground_; 609 | 610 | double t_tmp3 = ros::Time::now().toSec(); 611 | t_total_estimate += t_tmp3 - t_tmp2; 612 | } 613 | 614 | double t_bef_revert = ros::Time::now().toSec(); 615 | 616 | if (!candidates.empty()) 617 | { 618 | if (enable_TGR_) 619 | { 620 | temporal_ground_revert(cloud_ground, cloud_nonground, ringwise_flatness, candidates, concentric_idx); 621 | } 622 | else 623 | { 624 | for (size_t i=0; i inline 699 | void PatchWorkpp::update_elevation_thr(void) 700 | { 701 | for (int i=0; i 0) update_elevation_[i].erase(update_elevation_[i].begin(), update_elevation_[i].begin() + exceed_num); 717 | } 718 | 719 | if (verbose_) 720 | { 721 | cout << "sensor height: " << sensor_height_ << endl; 722 | cout << (boost::format("elevation_thr_ : %0.4f, %0.4f, %0.4f, %0.4f") 723 | % elevation_thr_[0] % elevation_thr_[1] % elevation_thr_[2] % elevation_thr_[3]).str() << endl; 724 | } 725 | 726 | return; 727 | } 728 | 729 | template inline 730 | void PatchWorkpp::update_flatness_thr(void) 731 | { 732 | for (int i=0; i 0) update_flatness_[i].erase(update_flatness_[i].begin(), update_flatness_[i].begin() + exceed_num); 745 | } 746 | 747 | if (verbose_) 748 | { 749 | cout << (boost::format("flatness_thr_ : %0.4f, %0.4f, %0.4f, %0.4f") 750 | % flatness_thr_[0] % flatness_thr_[1] % flatness_thr_[2] % flatness_thr_[3]).str() << endl; 751 | } 752 | 753 | return; 754 | } 755 | 756 | template inline 757 | void PatchWorkpp::temporal_ground_revert(pcl::PointCloud &cloud_ground, pcl::PointCloud &cloud_nonground, 758 | std::vector ring_flatness, std::vector> candidates, 759 | int concentric_idx) 760 | { 761 | if (verbose_) std::cout << "\033[1;34m" << "=========== Temporal Ground Revert (TGR) ===========" << "\033[0m" << endl; 762 | 763 | double mean_flatness = 0.0, stdev_flatness = 0.0; 764 | calc_mean_stdev(ring_flatness, mean_flatness, stdev_flatness); 765 | 766 | if (verbose_) 767 | { 768 | cout << "[" << candidates[0].concentric_idx << ", " << candidates[0].sector_idx << "]" 769 | << " mean_flatness: " << mean_flatness << ", stdev_flatness: " << stdev_flatness << std::endl; 770 | } 771 | 772 | for( size_t i=0; i candidate = candidates[i]; 775 | 776 | // Debug 777 | if(verbose_) 778 | { 779 | cout << "\033[1;33m" << candidate.sector_idx << "th flat_sector_candidate" 780 | << " / flatness: " << candidate.ground_flatness 781 | << " / line_variable: " << candidate.line_variable 782 | << " / ground_num : " << candidate.regionwise_ground.size() 783 | << "\033[0m" << endl; 784 | } 785 | 786 | double mu_flatness = mean_flatness + 1.5*stdev_flatness; 787 | double prob_flatness = 1/(1+exp( (candidate.ground_flatness-mu_flatness)/(mu_flatness/10) )); 788 | 789 | if (candidate.regionwise_ground.size() > 1500 && candidate.ground_flatness < th_dist_*th_dist_) prob_flatness = 1.0; 790 | 791 | double prob_line = 1.0; 792 | if (candidate.line_variable > 8.0 )//&& candidate.line_dir > M_PI/4)// candidate.ground_elevation > elevation_thr_[concentric_idx]) 793 | { 794 | // if (verbose_) cout << "line_dir: " << candidate.line_dir << endl; 795 | prob_line = 0.0; 796 | } 797 | 798 | bool revert = prob_line*prob_flatness > 0.5; 799 | 800 | if ( concentric_idx < num_rings_of_interest_ ) 801 | { 802 | if (revert) 803 | { 804 | if (verbose_) 805 | { 806 | cout << "\033[1;32m" << "REVERT TRUE" << "\033[0m" << endl; 807 | } 808 | 809 | revert_pc_ += candidate.regionwise_ground; 810 | cloud_ground += candidate.regionwise_ground; 811 | } 812 | else 813 | { 814 | if (verbose_) 815 | { 816 | cout << "\033[1;31m" << "FINAL REJECT" << "\033[0m" << endl; 817 | } 818 | reject_pc_ += candidate.regionwise_ground; 819 | cloud_nonground += candidate.regionwise_ground; 820 | } 821 | } 822 | } 823 | 824 | if (verbose_) std::cout << "\033[1;34m" << "====================================================" << "\033[0m" << endl; 825 | } 826 | 827 | // For adaptive 828 | template inline 829 | void PatchWorkpp::extract_piecewiseground( 830 | const int zone_idx, const pcl::PointCloud &src, 831 | pcl::PointCloud &dst, 832 | pcl::PointCloud &non_ground_dst) { 833 | 834 | // 0. Initialization 835 | if (!ground_pc_.empty()) ground_pc_.clear(); 836 | if (!dst.empty()) dst.clear(); 837 | if (!non_ground_dst.empty()) non_ground_dst.clear(); 838 | 839 | // 1. Region-wise Vertical Plane Fitting (R-VPF) 840 | // : removes potential vertical plane under the ground plane 841 | pcl::PointCloud src_wo_verticals; 842 | src_wo_verticals = src; 843 | 844 | if (enable_RVPF_) 845 | { 846 | for (int i = 0; i < num_iter_; i++) 847 | { 848 | extract_initial_seeds(zone_idx, src_wo_verticals, ground_pc_, th_seeds_v_); 849 | estimate_plane(ground_pc_); 850 | 851 | if (zone_idx == 0 && normal_(2) < uprightness_thr_) 852 | { 853 | pcl::PointCloud src_tmp; 854 | src_tmp = src_wo_verticals; 855 | src_wo_verticals.clear(); 856 | 857 | Eigen::MatrixXf points(src_tmp.points.size(), 3); 858 | int j = 0; 859 | for (auto &p:src_tmp.points) { 860 | points.row(j++) << p.x, p.y, p.z; 861 | } 862 | // ground plane model 863 | Eigen::VectorXf result = points * normal_; 864 | 865 | for (int r = 0; r < result.rows(); r++) { 866 | if (result[r] < th_dist_v_ - d_ && result[r] > -th_dist_v_ - d_) { 867 | non_ground_dst.points.push_back(src_tmp[r]); 868 | vertical_pc_.points.push_back(src_tmp[r]); 869 | } else { 870 | src_wo_verticals.points.push_back(src_tmp[r]); 871 | } 872 | } 873 | } 874 | else break; 875 | } 876 | } 877 | 878 | extract_initial_seeds(zone_idx, src_wo_verticals, ground_pc_); 879 | estimate_plane(ground_pc_); 880 | 881 | // 2. Region-wise Ground Plane Fitting (R-GPF) 882 | // : fits the ground plane 883 | 884 | //pointcloud to matrix 885 | Eigen::MatrixXf points(src_wo_verticals.points.size(), 3); 886 | int j = 0; 887 | for (auto &p:src_wo_verticals.points) { 888 | points.row(j++) << p.x, p.y, p.z; 889 | } 890 | 891 | for (int i = 0; i < num_iter_; i++) { 892 | 893 | ground_pc_.clear(); 894 | 895 | // ground plane model 896 | Eigen::VectorXf result = points * normal_; 897 | // threshold filter 898 | for (int r = 0; r < result.rows(); r++) { 899 | if (i < num_iter_ - 1) { 900 | if (result[r] < th_dist_ - d_ ) { 901 | ground_pc_.points.push_back(src_wo_verticals[r]); 902 | } 903 | } else { // Final stage 904 | if (result[r] < th_dist_ - d_ ) { 905 | dst.points.push_back(src_wo_verticals[r]); 906 | } else { 907 | non_ground_dst.points.push_back(src_wo_verticals[r]); 908 | } 909 | } 910 | } 911 | 912 | if (i < num_iter_ -1) estimate_plane(ground_pc_); 913 | else estimate_plane(dst); 914 | } 915 | } 916 | 917 | template inline 918 | geometry_msgs::PolygonStamped PatchWorkpp::set_polygons(int zone_idx, int r_idx, int theta_idx, int num_split) { 919 | geometry_msgs::PolygonStamped polygons; 920 | // Set point of polygon. Start from RL and ccw 921 | geometry_msgs::Point32 point; 922 | 923 | // RL 924 | double zone_min_range = min_ranges_[zone_idx]; 925 | double r_len = r_idx * ring_sizes_[zone_idx] + zone_min_range; 926 | double angle = theta_idx * sector_sizes_[zone_idx]; 927 | 928 | point.x = r_len * cos(angle); 929 | point.y = r_len * sin(angle); 930 | point.z = MARKER_Z_VALUE; 931 | polygons.polygon.points.push_back(point); 932 | // RU 933 | r_len = r_len + ring_sizes_[zone_idx]; 934 | point.x = r_len * cos(angle); 935 | point.y = r_len * sin(angle); 936 | point.z = MARKER_Z_VALUE; 937 | polygons.polygon.points.push_back(point); 938 | 939 | // RU -> LU 940 | for (int idx = 1; idx <= num_split; ++idx) { 941 | angle = angle + sector_sizes_[zone_idx] / num_split; 942 | point.x = r_len * cos(angle); 943 | point.y = r_len * sin(angle); 944 | point.z = MARKER_Z_VALUE; 945 | polygons.polygon.points.push_back(point); 946 | } 947 | 948 | r_len = r_len - ring_sizes_[zone_idx]; 949 | point.x = r_len * cos(angle); 950 | point.y = r_len * sin(angle); 951 | point.z = MARKER_Z_VALUE; 952 | polygons.polygon.points.push_back(point); 953 | 954 | for (int idx = 1; idx < num_split; ++idx) { 955 | angle = angle - sector_sizes_[zone_idx] / num_split; 956 | point.x = r_len * cos(angle); 957 | point.y = r_len * sin(angle); 958 | point.z = MARKER_Z_VALUE; 959 | polygons.polygon.points.push_back(point); 960 | } 961 | 962 | return polygons; 963 | } 964 | 965 | template inline 966 | void PatchWorkpp::set_ground_likelihood_estimation_status( 967 | const int zone_idx, const int ring_idx, 968 | const int concentric_idx, 969 | const double z_vec, 970 | const double z_elevation, 971 | const double ground_flatness) { 972 | if (z_vec > uprightness_thr_) { //orthogonal 973 | if (concentric_idx < num_rings_of_interest_) { 974 | if (z_elevation > elevation_thr_[concentric_idx]) { 975 | if (flatness_thr_[concentric_idx] > ground_flatness) { 976 | poly_list_.likelihood.push_back(FLAT_ENOUGH); 977 | } else { 978 | poly_list_.likelihood.push_back(TOO_HIGH_ELEVATION); 979 | } 980 | } else { 981 | poly_list_.likelihood.push_back(UPRIGHT_ENOUGH); 982 | } 983 | } else { 984 | poly_list_.likelihood.push_back(UPRIGHT_ENOUGH); 985 | } 986 | } else { // tilted 987 | poly_list_.likelihood.push_back(TOO_TILTED); 988 | } 989 | } 990 | 991 | template inline 992 | void PatchWorkpp::calc_mean_stdev(std::vector vec, double &mean, double &stdev) 993 | { 994 | if (vec.size() <= 1) return; 995 | 996 | mean = std::accumulate(vec.begin(), vec.end(), 0.0) / vec.size(); 997 | 998 | for (int i=0; i inline 1004 | double PatchWorkpp::xy2theta(const double &x, const double &y) { // 0 ~ 2 * PI 1005 | // if (y >= 0) { 1006 | // return atan2(y, x); // 1, 2 quadrant 1007 | // } else { 1008 | // return 2 * M_PI + atan2(y, x);// 3, 4 quadrant 1009 | // } 1010 | 1011 | double angle = atan2(y, x); 1012 | return angle > 0 ? angle : 2*M_PI+angle; 1013 | } 1014 | 1015 | template inline 1016 | double PatchWorkpp::xy2radius(const double &x, const double &y) { 1017 | return sqrt(pow(x, 2) + pow(y, 2)); 1018 | } 1019 | 1020 | template inline 1021 | void PatchWorkpp::pc2czm(const pcl::PointCloud &src, std::vector &czm, pcl::PointCloud &cloud_nonground) { 1022 | 1023 | for (int i=0; i min_range_)) { 1033 | double theta = xy2theta(pt.x, pt.y); 1034 | 1035 | int zone_idx = 0; 1036 | if ( r < min_ranges_[1] ) zone_idx = 0; 1037 | else if ( r < min_ranges_[2] ) zone_idx = 1; 1038 | else if ( r < min_ranges_[3] ) zone_idx = 2; 1039 | else zone_idx = 3; 1040 | 1041 | int ring_idx = min(static_cast(((r - min_ranges_[zone_idx]) / ring_sizes_[zone_idx])), num_rings_each_zone_[zone_idx] - 1); 1042 | int sector_idx = min(static_cast((theta / sector_sizes_[zone_idx])), num_sectors_each_zone_[zone_idx] - 1); 1043 | 1044 | czm[zone_idx][ring_idx][sector_idx].points.emplace_back(pt); 1045 | } 1046 | else { 1047 | cloud_nonground.push_back(pt); 1048 | } 1049 | } 1050 | 1051 | if (verbose_) cout << "[ CZM ] Divides pointcloud into the concentric zone model" << endl; 1052 | } 1053 | 1054 | #endif 1055 | -------------------------------------------------------------------------------- /include/patchworkpp/utils.hpp: -------------------------------------------------------------------------------- 1 | #ifndef COMMON_H 2 | #define COMMON_H 3 | 4 | #include "math.h" 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | #include 26 | // CLASSES 27 | #define SENSOR_HEIGHT 1.73 28 | 29 | #define UNLABELED 0 30 | #define OUTLIER 1 31 | #define NUM_ALL_CLASSES 34 32 | #define ROAD 40 33 | #define PARKING 44 34 | #define SIDEWALKR 48 35 | #define OTHER_GROUND 49 36 | #define BUILDING 50 37 | #define FENSE 51 38 | #define LANE_MARKING 60 39 | #define VEGETATION 70 40 | #define TERRAIN 72 41 | 42 | #define TRUEPOSITIVE 3 43 | #define TRUENEGATIVE 2 44 | #define FALSEPOSITIVE 1 45 | #define FALSENEGATIVE 0 46 | 47 | int NUM_ZEROS = 5; 48 | 49 | using namespace std; 50 | 51 | double VEGETATION_THR = - SENSOR_HEIGHT * 3 / 4; 52 | /** Euclidean Velodyne coordinate, including intensity and ring number, and label. */ 53 | struct PointXYZILID 54 | { 55 | PCL_ADD_POINT4D; // quad-word XYZ 56 | float intensity; ///< laser intensity reading 57 | uint16_t label; ///< point label 58 | uint16_t id; 59 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW // ensure proper alignment 60 | } EIGEN_ALIGN16; 61 | 62 | // Register custom point struct according to PCL 63 | POINT_CLOUD_REGISTER_POINT_STRUCT(PointXYZILID, 64 | (float, x, x) 65 | (float, y, y) 66 | (float, z, z) 67 | (float, intensity, intensity) 68 | (std::uint16_t, label, label) 69 | (std::uint16_t, id, id)) 70 | 71 | 72 | void PointXYZILID2XYZI(pcl::PointCloud& src, 73 | pcl::PointCloud::Ptr dst){ 74 | dst->points.clear(); 75 | for (const auto &pt: src.points){ 76 | pcl::PointXYZI pt_xyzi; 77 | pt_xyzi.x = pt.x; 78 | pt_xyzi.y = pt.y; 79 | pt_xyzi.z = pt.z; 80 | pt_xyzi.intensity = pt.intensity; 81 | dst->points.push_back(pt_xyzi); 82 | } 83 | } 84 | std::vector outlier_classes = {UNLABELED, OUTLIER}; 85 | std::vector ground_classes = {ROAD, PARKING, SIDEWALKR, OTHER_GROUND, LANE_MARKING, VEGETATION, TERRAIN}; 86 | std::vector ground_classes_except_terrain = {ROAD, PARKING, SIDEWALKR, OTHER_GROUND, LANE_MARKING}; 87 | std::vector traversable_ground_classes = {ROAD, PARKING, LANE_MARKING, OTHER_GROUND}; 88 | 89 | int count_num_ground(const pcl::PointCloud& pc){ 90 | int num_ground = 0; 91 | 92 | std::vector::iterator iter; 93 | 94 | for (auto const& pt: pc.points){ 95 | iter = std::find(ground_classes.begin(), ground_classes.end(), pt.label); 96 | if (iter != ground_classes.end()){ // corresponding class is in ground classes 97 | if (pt.label == VEGETATION){ 98 | if (pt.z < VEGETATION_THR){ 99 | num_ground ++; 100 | } 101 | }else num_ground ++; 102 | } 103 | } 104 | return num_ground; 105 | } 106 | 107 | int count_num_ground_without_vegetation(const pcl::PointCloud& pc){ 108 | int num_ground = 0; 109 | 110 | std::vector::iterator iter; 111 | 112 | std::vector classes = {ROAD, PARKING, SIDEWALKR, OTHER_GROUND, LANE_MARKING, TERRAIN}; 113 | 114 | for (auto const& pt: pc.points){ 115 | iter = std::find(classes.begin(), classes.end(), pt.label); 116 | if (iter != classes.end()){ // corresponding class is in ground classes 117 | num_ground ++; 118 | } 119 | } 120 | return num_ground; 121 | } 122 | 123 | std::map set_initial_gt_counts(std::vector& gt_classes){ 124 | map gt_counts; 125 | for (int i = 0; i< gt_classes.size(); ++i){ 126 | gt_counts.insert(pair(gt_classes.at(i), 0)); 127 | } 128 | return gt_counts; 129 | } 130 | 131 | std::map count_num_each_class(const pcl::PointCloud& pc){ 132 | int num_ground = 0; 133 | auto gt_counts = set_initial_gt_counts(ground_classes); 134 | std::vector::iterator iter; 135 | 136 | for (auto const& pt: pc.points){ 137 | iter = std::find(ground_classes.begin(), ground_classes.end(), pt.label); 138 | if (iter != ground_classes.end()){ // corresponding class is in ground classes 139 | if (pt.label == VEGETATION){ 140 | if (pt.z < VEGETATION_THR){ 141 | gt_counts.find(pt.label)->second++; 142 | } 143 | }else gt_counts.find(pt.label)->second++; 144 | } 145 | } 146 | return gt_counts; 147 | } 148 | 149 | int count_num_outliers(const pcl::PointCloud& pc){ 150 | int num_outliers = 0; 151 | 152 | std::vector::iterator iter; 153 | 154 | for (auto const& pt: pc.points){ 155 | iter = std::find(outlier_classes.begin(), outlier_classes.end(), pt.label); 156 | if (iter != outlier_classes.end()){ // corresponding class is in ground classes 157 | num_outliers ++; 158 | } 159 | } 160 | return num_outliers; 161 | } 162 | 163 | 164 | void discern_ground(const pcl::PointCloud& src, pcl::PointCloud& ground, pcl::PointCloud& non_ground){ 165 | ground.clear(); 166 | non_ground.clear(); 167 | std::vector::iterator iter; 168 | for (auto const& pt: src.points){ 169 | if (pt.label == UNLABELED || pt.label == OUTLIER) continue; 170 | iter = std::find(ground_classes.begin(), ground_classes.end(), pt.label); 171 | if (iter != ground_classes.end()){ // corresponding class is in ground classes 172 | if (pt.label == VEGETATION){ 173 | if (pt.z < VEGETATION_THR){ 174 | ground.push_back(pt); 175 | }else non_ground.push_back(pt); 176 | }else ground.push_back(pt); 177 | }else{ 178 | non_ground.push_back(pt); 179 | } 180 | } 181 | } 182 | 183 | void discern_ground_without_vegetation(const pcl::PointCloud& src, pcl::PointCloud& ground, pcl::PointCloud& non_ground){ 184 | ground.clear(); 185 | non_ground.clear(); 186 | std::vector::iterator iter; 187 | for (auto const& pt: src.points){ 188 | if (pt.label == UNLABELED || pt.label == OUTLIER) continue; 189 | iter = std::find(ground_classes.begin(), ground_classes.end(), pt.label); 190 | if (iter != ground_classes.end()){ // corresponding class is in ground classes 191 | if (pt.label != VEGETATION) ground.push_back(pt); 192 | }else{ 193 | non_ground.push_back(pt); 194 | } 195 | } 196 | } 197 | 198 | 199 | void calculate_precision_recall(const pcl::PointCloud& pc_curr, 200 | pcl::PointCloud& ground_estimated, 201 | double & precision, 202 | double& recall, 203 | bool consider_outliers=true){ 204 | 205 | int num_ground_est = ground_estimated.points.size(); 206 | int num_ground_gt = count_num_ground(pc_curr); 207 | int num_TP = count_num_ground(ground_estimated); 208 | if (consider_outliers){ 209 | int num_outliers_est = count_num_outliers(ground_estimated); 210 | precision = (double)(num_TP)/(num_ground_est - num_outliers_est) * 100; 211 | recall = (double)(num_TP)/num_ground_gt * 100; 212 | }else{ 213 | precision = (double)(num_TP)/num_ground_est * 100; 214 | recall = (double)(num_TP)/num_ground_gt * 100; 215 | } 216 | } 217 | 218 | void calculate_precision_recall_without_vegetation(const pcl::PointCloud& pc_curr, 219 | pcl::PointCloud& ground_estimated, 220 | double & precision, 221 | double& recall, 222 | bool consider_outliers=true){ 223 | int num_veg = 0; 224 | for (auto const& pt: ground_estimated.points) 225 | { 226 | if (pt.label == VEGETATION) num_veg++; 227 | } 228 | 229 | int num_ground_est = ground_estimated.size() - num_veg; 230 | int num_ground_gt = count_num_ground_without_vegetation(pc_curr); 231 | int num_TP = count_num_ground_without_vegetation(ground_estimated); 232 | if (consider_outliers){ 233 | int num_outliers_est = count_num_outliers(ground_estimated); 234 | precision = (double)(num_TP)/(num_ground_est - num_outliers_est) * 100; 235 | recall = (double)(num_TP)/num_ground_gt * 100; 236 | }else{ 237 | precision = (double)(num_TP)/num_ground_est * 100; 238 | recall = (double)(num_TP)/num_ground_gt * 100; 239 | } 240 | } 241 | 242 | 243 | void save_all_labels(const pcl::PointCloud& pc, string ABS_DIR, string seq, int count){ 244 | 245 | std::string count_str = std::to_string(count); 246 | std::string count_str_padded = std::string(NUM_ZEROS - count_str.length(), '0') + count_str; 247 | std::string output_filename = ABS_DIR + "/" + seq + "/" + count_str_padded + ".csv"; 248 | ofstream sc_output(output_filename); 249 | 250 | vector labels(NUM_ALL_CLASSES, 0); 251 | for (auto const& pt: pc.points){ 252 | if (pt.label == 0) labels[0]++; 253 | else if (pt.label == 1) labels[1]++; 254 | else if (pt.label == 10) labels[2]++; 255 | else if (pt.label == 11) labels[3]++; 256 | else if (pt.label == 13) labels[4]++; 257 | else if (pt.label == 15) labels[5]++; 258 | else if (pt.label == 16) labels[6]++; 259 | else if (pt.label == 18) labels[7]++; 260 | else if (pt.label == 20) labels[8]++; 261 | else if (pt.label == 30) labels[9]++; 262 | else if (pt.label == 31) labels[10]++; 263 | else if (pt.label == 32) labels[11]++; 264 | else if (pt.label == 40) labels[12]++; 265 | else if (pt.label == 44) labels[13]++; 266 | else if (pt.label == 48) labels[14]++; 267 | else if (pt.label == 49) labels[15]++; 268 | else if (pt.label == 50) labels[16]++; 269 | else if (pt.label == 51) labels[17]++; 270 | else if (pt.label == 52) labels[18]++; 271 | else if (pt.label == 60) labels[19]++; 272 | else if (pt.label == 70) labels[20]++; 273 | else if (pt.label == 71) labels[21]++; 274 | else if (pt.label == 72) labels[22]++; 275 | else if (pt.label == 80) labels[23]++; 276 | else if (pt.label == 81) labels[24]++; 277 | else if (pt.label == 99) labels[25]++; 278 | else if (pt.label == 252) labels[26]++; 279 | else if (pt.label == 253) labels[27]++; 280 | else if (pt.label == 254) labels[28]++; 281 | else if (pt.label == 255) labels[29]++; 282 | else if (pt.label == 256) labels[30]++; 283 | else if (pt.label == 257) labels[31]++; 284 | else if (pt.label == 258) labels[32]++; 285 | else if (pt.label == 259) labels[33]++; 286 | } 287 | 288 | for (uint8_t i=0; i < NUM_ALL_CLASSES;++i){ 289 | if (i!=33){ 290 | sc_output<& pc_curr, 299 | pcl::PointCloud& ground_estimated, string acc_filename, 300 | double& accuracy, map&pc_curr_gt_counts, map&g_est_gt_counts){ 301 | 302 | 303 | // std::cout<<"debug: "<(num_TP + (num_False - num_FP)) / num_total_gt * 100.0; 317 | 318 | pc_curr_gt_counts = count_num_each_class(pc_curr); 319 | g_est_gt_counts = count_num_each_class(ground_estimated); 320 | 321 | // save output 322 | for (auto const& class_id: ground_classes){ 323 | sc_output2<second<<","<second<<","; 324 | } 325 | sc_output2<& TP, const pcl::PointCloud& FP, 331 | const pcl::PointCloud& FN, const pcl::PointCloud& TN, 332 | std::string pcd_filename){ 333 | pcl::PointCloud pc_out; 334 | 335 | for (auto const pt: TP.points){ 336 | pcl::PointXYZI pt_est; 337 | pt_est.x = pt.x; pt_est.y = pt.y; pt_est.z = pt.z; 338 | pt_est.intensity = TRUEPOSITIVE; 339 | pc_out.points.push_back(pt_est); 340 | } 341 | for (auto const pt: FP.points){ 342 | pcl::PointXYZI pt_est; 343 | pt_est.x = pt.x; pt_est.y = pt.y; pt_est.z = pt.z; 344 | pt_est.intensity = FALSEPOSITIVE; 345 | pc_out.points.push_back(pt_est); 346 | } 347 | for (auto const pt: FN.points){ 348 | pcl::PointXYZI pt_est; 349 | pt_est.x = pt.x; pt_est.y = pt.y; pt_est.z = pt.z; 350 | pt_est.intensity = FALSENEGATIVE; 351 | pc_out.points.push_back(pt_est); 352 | } 353 | for (auto const pt: TN.points){ 354 | pcl::PointXYZI pt_est; 355 | pt_est.x = pt.x; pt_est.y = pt.y; pt_est.z = pt.z; 356 | pt_est.intensity = TRUENEGATIVE; 357 | pc_out.points.push_back(pt_est); 358 | } 359 | pc_out.width = pc_out.points.size(); 360 | pc_out.height = 1; 361 | pcl::io::savePCDFileASCII(pcd_filename, pc_out); 362 | 363 | } 364 | 365 | 366 | #endif 367 | -------------------------------------------------------------------------------- /include/tools/kitti_loader.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by shapelim on 6/23/21. 3 | // 4 | #include "patchworkpp/utils.hpp" 5 | 6 | #ifndef PATCHWORK_PCD_LOADER_HPP 7 | #define PATCHWORK_PCD_LOADER_HPP 8 | 9 | 10 | class KittiLoader { 11 | public: 12 | KittiLoader(const std::string &abs_path) { 13 | pc_path_ = abs_path + "/velodyne"; 14 | label_path_ = abs_path + "/labels"; 15 | 16 | for (num_frames_ = 0;; num_frames_++) { 17 | std::string filename = (boost::format("%s/%06d.bin") % pc_path_ % num_frames_).str(); 18 | if (!boost::filesystem::exists(filename)) { 19 | break; 20 | } 21 | } 22 | int num_labels; 23 | for (num_labels = 0;; num_labels++) { 24 | std::string filename = (boost::format("%s/%06d.label") % label_path_ % num_labels).str(); 25 | if (!boost::filesystem::exists(filename)) { 26 | break; 27 | } 28 | } 29 | 30 | if (num_frames_ == 0) { 31 | std::cerr << "Error: no files in " << pc_path_ << std::endl; 32 | } 33 | if (num_frames_ != num_labels) { 34 | std::cerr << "Error: The # of point clouds and # of labels are not same" << std::endl; 35 | } 36 | } 37 | 38 | ~KittiLoader() {} 39 | 40 | size_t size() const { return num_frames_; } 41 | 42 | template 43 | int get_cloud(size_t idx, pcl::PointCloud &cloud) const { 44 | std::string filename = (boost::format("%s/%06d.bin") % pc_path_ % idx).str(); 45 | FILE *file = fopen(filename.c_str(), "rb"); 46 | if (!file) { 47 | std::cerr << "error: failed to load " << filename << std::endl; 48 | return -1; 49 | } 50 | 51 | std::vector buffer(1000000); 52 | size_t num_points = fread(reinterpret_cast(buffer.data()), sizeof(float), buffer.size(), file) / 4; 53 | fclose(file); 54 | 55 | cloud.resize(num_points); 56 | if (std::is_same::value) { 57 | for (int i = 0; i < num_points; i++) { 58 | auto &pt = cloud.at(i); 59 | pt.x = buffer[i * 4]; 60 | pt.y = buffer[i * 4 + 1]; 61 | pt.z = buffer[i * 4 + 2]; 62 | } 63 | } else if (std::is_same::value) { 64 | for (int i = 0; i < num_points; i++) { 65 | auto &pt = cloud.at(i); 66 | pt.x = buffer[i * 4]; 67 | pt.y = buffer[i * 4 + 1]; 68 | pt.z = buffer[i * 4 + 2]; 69 | pt.intensity = buffer[i * 4 + 3]; 70 | } 71 | 72 | } else if (std::is_same::value) { 73 | std::string label_name = (boost::format("%s/%06d.label") % label_path_ % idx).str(); 74 | std::ifstream label_input(label_name, std::ios::binary); 75 | if (!label_input.is_open()) { 76 | std::cerr << "Could not open the label!" << std::endl; 77 | return -1; 78 | } 79 | label_input.seekg(0, std::ios::beg); 80 | 81 | std::vector labels(num_points); 82 | label_input.read((char*)&labels[0], num_points * sizeof(uint32_t)); 83 | 84 | for (int i = 0; i < num_points; i++) { 85 | auto &pt = cloud.at(i); 86 | pt.x = buffer[i * 4]; 87 | pt.y = buffer[i * 4 + 1]; 88 | pt.z = buffer[i * 4 + 2]; 89 | pt.intensity = buffer[i * 4 + 3]; 90 | pt.label = labels[i] & 0xFFFF; 91 | pt.id = labels[i] >> 16; 92 | } 93 | 94 | } 95 | 96 | return 0; 97 | } 98 | 99 | private: 100 | int num_frames_; 101 | std::string label_path_; 102 | std::string pc_path_; 103 | }; 104 | 105 | #endif //PATCHWORK_PCD_LOADER_HPP 106 | -------------------------------------------------------------------------------- /include/tools/pcd_loader.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by shapelim on 6/23/21. 3 | // 4 | 5 | #ifndef PATCHWORK_PCD_LOADER_HPP 6 | #define PATCHWORK_PCD_LOADER_HPP 7 | 8 | template 9 | class PcdLoader { 10 | public: 11 | PcdLoader(const std::string &pcd_path) : pcd_path_(pcd_path){ 12 | for (num_frames_ = 0;; num_frames_++) { 13 | std::string filename = (boost::format("%s/%06d.pcd") % pcd_path % num_frames_).str(); 14 | if (!boost::filesystem::exists(filename)) { 15 | break; 16 | } 17 | } 18 | 19 | if (num_frames_ == 0) { 20 | std::cerr << "error: no files in " << pcd_path << std::endl; 21 | } 22 | } 23 | 24 | ~PcdLoader() {} 25 | 26 | size_t size() const { return num_frames_; } 27 | 28 | pcl::PointCloud::ConstPtr cloud(size_t i) const { 29 | std::string filename = (boost::format("%s/%06d.bin") % pcd_path_ % i).str(); 30 | FILE *file = fopen(filename.c_str(), "rb"); 31 | if (!file) { 32 | std::cerr << "error: failed to load " << filename << std::endl; 33 | return nullptr; 34 | } 35 | 36 | std::vector buffer(1000000); 37 | size_t num_points = fread(reinterpret_cast(buffer.data()), sizeof(float), buffer.size(), file) / 4; 38 | fclose(file); 39 | 40 | pcl::PointCloud::Ptr cloud(new pcl::PointCloud()); 41 | cloud->resize(num_points); 42 | 43 | for (int i = 0; i < num_points; i++) { 44 | auto &pt = cloud->at(i); 45 | pt.x = buffer[i * 4]; 46 | pt.y = buffer[i * 4 + 1]; 47 | pt.z = buffer[i * 4 + 2]; 48 | // pt.intensity = buffer[i * 4 + 3]; 49 | } 50 | 51 | return cloud; 52 | } 53 | 54 | private: 55 | int num_frames_; 56 | std::string pcd_path_; 57 | }; 58 | #endif //PATCHWORK_PCD_LOADER_HPP 59 | -------------------------------------------------------------------------------- /launch/demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /launch/offline_kitti.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | "patchworkpp" 9 | "00" 10 | 0 11 | "/../../seungjae_ssd/data/SemanticKITTI/sequences" 12 | "/data/patchworkpp/" 13 | true 14 | true 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /launch/video.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | "patchworkpp" 9 | "00" 10 | 0 11 | "/../../seungjae_ssd/data/SemanticKITTI/sequences" 12 | "/data/patchworkpp/" 13 | false 14 | false 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | patchworkpp 4 | 0.0.0 5 | patchwork++ package 6 | 7 | Seungjae Lee 8 | 9 | 10 | 11 | 12 | 13 | TODO 14 | catkin 15 | message_generation 16 | libpcl-all 17 | roscpp 18 | rospy 19 | std_msgs 20 | nav_msgs 21 | sensor_msgs 22 | jsk_recognition_msgs 23 | cv_bridge 24 | laser_geometry 25 | pcl_conversions 26 | pcl_ros 27 | roscpp 28 | rospy 29 | std_msgs 30 | nav_msgs 31 | roslaunch 32 | libpcl-all-dev 33 | message_generation 34 | sensor_msgs 35 | jsk_recognition_msgs 36 | cv_bridge 37 | laser_geometry 38 | pcl_conversions 39 | pcl_ros 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | -------------------------------------------------------------------------------- /pictures/patchwork++.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/url-kaist/patchwork-plusplus-ros/6f9b081d68d0c9eddda74d2763de1d3f8e51ac04/pictures/patchwork++.gif -------------------------------------------------------------------------------- /rviz/demo.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | Splitter Ratio: 0.43383947014808655 10 | Tree Height: 796 11 | - Class: rviz/Selection 12 | Name: Selection 13 | - Class: rviz/Tool Properties 14 | Expanded: 15 | - /2D Pose Estimate1 16 | - /2D Nav Goal1 17 | - /Publish Point1 18 | Name: Tool Properties 19 | Splitter Ratio: 0.5886790156364441 20 | - Class: rviz/Views 21 | Expanded: 22 | - /Current View1 23 | Name: Views 24 | Splitter Ratio: 0.5 25 | - Class: rviz/Time 26 | Experimental: false 27 | Name: Time 28 | SyncMode: 0 29 | SyncSource: Ground 30 | Preferences: 31 | PromptSaveOnExit: true 32 | Toolbars: 33 | toolButtonStyle: 2 34 | Visualization Manager: 35 | Class: "" 36 | Displays: 37 | - Class: rviz/Axes 38 | Enabled: true 39 | Length: 3 40 | Name: Axes 41 | Radius: 0.4000000059604645 42 | Reference Frame: 43 | Value: true 44 | - Alpha: 0.5 45 | Cell Size: 5 46 | Class: rviz/Grid 47 | Color: 160; 160; 164 48 | Enabled: false 49 | Line Style: 50 | Line Width: 0.029999999329447746 51 | Value: Lines 52 | Name: Grid 53 | Normal Cell Count: 0 54 | Offset: 55 | X: 0 56 | Y: 0 57 | Z: -3.059999942779541 58 | Plane: XY 59 | Plane Cell Count: 30 60 | Reference Frame: 61 | Value: false 62 | - Alpha: 1 63 | Autocompute Intensity Bounds: true 64 | Autocompute Value Bounds: 65 | Max Value: 2.9036598205566406 66 | Min Value: -11.440068244934082 67 | Value: true 68 | Axis: Z 69 | Channel Name: intensity 70 | Class: rviz/PointCloud2 71 | Color: 211; 215; 207 72 | Color Transformer: RGB8 73 | Decay Time: 0 74 | Enabled: false 75 | Invert Rainbow: false 76 | Max Color: 255; 255; 255 77 | Min Color: 0; 0; 0 78 | Name: Src 79 | Position Transformer: XYZ 80 | Queue Size: 10 81 | Selectable: false 82 | Size (Pixels): 3 83 | Size (m): 0.10000000149011612 84 | Style: Flat Squares 85 | Topic: /kitti/velo/pointcloud 86 | Unreliable: false 87 | Use Fixed Frame: true 88 | Use rainbow: true 89 | Value: false 90 | - Alpha: 1 91 | Autocompute Intensity Bounds: true 92 | Autocompute Value Bounds: 93 | Max Value: 10 94 | Min Value: -10 95 | Value: true 96 | Axis: Z 97 | Channel Name: intensity 98 | Class: rviz/PointCloud2 99 | Color: 15; 155; 0 100 | Color Transformer: FlatColor 101 | Decay Time: 0 102 | Enabled: true 103 | Invert Rainbow: false 104 | Max Color: 255; 255; 255 105 | Min Color: 0; 0; 0 106 | Name: Ground 107 | Position Transformer: XYZ 108 | Queue Size: 10 109 | Selectable: true 110 | Size (Pixels): 3 111 | Size (m): 0.10000000149011612 112 | Style: Flat Squares 113 | Topic: /ground_segmentation/ground 114 | Unreliable: false 115 | Use Fixed Frame: true 116 | Use rainbow: true 117 | Value: true 118 | - Alpha: 1 119 | Autocompute Intensity Bounds: true 120 | Autocompute Value Bounds: 121 | Max Value: 10 122 | Min Value: -10 123 | Value: true 124 | Axis: Z 125 | Channel Name: intensity 126 | Class: rviz/PointCloud2 127 | Color: 204; 0; 0 128 | Color Transformer: FlatColor 129 | Decay Time: 0 130 | Enabled: true 131 | Invert Rainbow: false 132 | Max Color: 255; 255; 255 133 | Min Color: 0; 0; 0 134 | Name: Nonground 135 | Position Transformer: XYZ 136 | Queue Size: 10 137 | Selectable: true 138 | Size (Pixels): 3 139 | Size (m): 0.10000000149011612 140 | Style: Flat Squares 141 | Topic: /ground_segmentation/nonground 142 | Unreliable: false 143 | Use Fixed Frame: true 144 | Use rainbow: true 145 | Value: true 146 | - Alpha: 0.20000000298023224 147 | Class: jsk_rviz_plugin/PolygonArray 148 | Color: 25; 255; 0 149 | Enabled: false 150 | Name: plane_viz 151 | Topic: /patchworkpp/plane 152 | Unreliable: false 153 | Value: false 154 | coloring: Liekelihood 155 | enable lighting: false 156 | normal length: 1 157 | only border: false 158 | show normal: false 159 | - Alpha: 0.20000000298023224 160 | Autocompute Intensity Bounds: true 161 | Autocompute Value Bounds: 162 | Max Value: 10 163 | Min Value: -10 164 | Value: true 165 | Axis: Z 166 | Channel Name: intensity 167 | Class: rviz/PointCloud2 168 | Color: 25; 255; 255 169 | Color Transformer: FlatColor 170 | Decay Time: 0 171 | Enabled: false 172 | Invert Rainbow: false 173 | Max Color: 255; 255; 255 174 | Min Color: 0; 0; 0 175 | Name: revert_pc 176 | Position Transformer: XYZ 177 | Queue Size: 10 178 | Selectable: true 179 | Size (Pixels): 3 180 | Size (m): 0.30000001192092896 181 | Style: Flat Squares 182 | Topic: /patchworkpp/revert_pc 183 | Unreliable: false 184 | Use Fixed Frame: true 185 | Use rainbow: true 186 | Value: false 187 | - Alpha: 1 188 | Autocompute Intensity Bounds: true 189 | Autocompute Value Bounds: 190 | Max Value: 10 191 | Min Value: -10 192 | Value: true 193 | Axis: Z 194 | Channel Name: intensity 195 | Class: rviz/PointCloud2 196 | Color: 255; 25; 255 197 | Color Transformer: FlatColor 198 | Decay Time: 0 199 | Enabled: false 200 | Invert Rainbow: false 201 | Max Color: 255; 255; 255 202 | Min Color: 0; 0; 0 203 | Name: reject_pc 204 | Position Transformer: XYZ 205 | Queue Size: 10 206 | Selectable: true 207 | Size (Pixels): 3 208 | Size (m): 0.15000000596046448 209 | Style: Flat Squares 210 | Topic: /patchworkpp/reject_pc 211 | Unreliable: false 212 | Use Fixed Frame: true 213 | Use rainbow: true 214 | Value: false 215 | - Alpha: 1 216 | Class: jsk_rviz_plugin/NormalDisplay 217 | Color: 0; 0; 0 218 | Display Rate (%): 100 219 | Enabled: false 220 | Max Color: 255; 0; 0 221 | MinColor: 0; 255; 0 222 | Name: NormalDisplay 223 | Scale: 1 224 | Style: FlatColor 225 | Topic: /patchworkpp/normals 226 | Unreliable: false 227 | Use Rainbow: true 228 | Value: false 229 | - Alpha: 1 230 | Autocompute Intensity Bounds: true 231 | Autocompute Value Bounds: 232 | Max Value: 10 233 | Min Value: -10 234 | Value: true 235 | Axis: Z 236 | Channel Name: intensity 237 | Class: rviz/PointCloud2 238 | Color: 255; 0; 127 239 | Color Transformer: FlatColor 240 | Decay Time: 0 241 | Enabled: false 242 | Invert Rainbow: false 243 | Max Color: 239; 41; 41 244 | Min Color: 0; 0; 0 245 | Name: Noise 246 | Position Transformer: XYZ 247 | Queue Size: 10 248 | Selectable: true 249 | Size (Pixels): 3 250 | Size (m): 0.20000000298023224 251 | Style: Flat Squares 252 | Topic: /patchworkpp/noise 253 | Unreliable: false 254 | Use Fixed Frame: true 255 | Use rainbow: false 256 | Value: false 257 | - Alpha: 0.10000000149011612 258 | Autocompute Intensity Bounds: true 259 | Autocompute Value Bounds: 260 | Max Value: 10 261 | Min Value: -10 262 | Value: true 263 | Axis: Z 264 | Channel Name: intensity 265 | Class: rviz/PointCloud2 266 | Color: 252; 233; 79 267 | Color Transformer: FlatColor 268 | Decay Time: 0 269 | Enabled: false 270 | Invert Rainbow: false 271 | Max Color: 255; 255; 255 272 | Min Color: 0; 0; 0 273 | Name: Vertical 274 | Position Transformer: XYZ 275 | Queue Size: 10 276 | Selectable: true 277 | Size (Pixels): 3 278 | Size (m): 0.30000001192092896 279 | Style: Flat Squares 280 | Topic: /patchworkpp/vertical 281 | Unreliable: false 282 | Use Fixed Frame: true 283 | Use rainbow: true 284 | Value: false 285 | Enabled: true 286 | Global Options: 287 | Background Color: 255; 255; 255 288 | Default Light: true 289 | Fixed Frame: map 290 | Frame Rate: 30 291 | Name: root 292 | Tools: 293 | - Class: rviz/Interact 294 | Hide Inactive Objects: true 295 | - Class: rviz/MoveCamera 296 | - Class: rviz/Select 297 | - Class: rviz/FocusCamera 298 | - Class: rviz/Measure 299 | - Class: rviz/SetInitialPose 300 | Theta std deviation: 0.2617993950843811 301 | Topic: /initialpose 302 | X std deviation: 0.5 303 | Y std deviation: 0.5 304 | - Class: rviz/SetGoal 305 | Topic: /move_base_simple/goal 306 | - Class: rviz/PublishPoint 307 | Single click: true 308 | Topic: /clicked_point 309 | Value: true 310 | Views: 311 | Current: 312 | Class: rviz/Orbit 313 | Distance: 76.899658203125 314 | Enable Stereo Rendering: 315 | Stereo Eye Separation: 0.05999999865889549 316 | Stereo Focal Distance: 1 317 | Swap Stereo Eyes: false 318 | Value: false 319 | Focal Point: 320 | X: 0 321 | Y: 0 322 | Z: 0 323 | Focal Shape Fixed Size: false 324 | Focal Shape Size: 0.05000000074505806 325 | Invert Z Axis: false 326 | Name: Current View 327 | Near Clip Distance: 0.009999999776482582 328 | Pitch: 0.7853981852531433 329 | Target Frame: 330 | Value: Orbit (rviz) 331 | Yaw: 0.7853981852531433 332 | Saved: ~ 333 | Window Geometry: 334 | Displays: 335 | collapsed: false 336 | Height: 1025 337 | Hide Left Dock: false 338 | Hide Right Dock: true 339 | QMainWindow State: 000000ff00000000fd0000000400000000000001cf000003a7fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000003a7000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c000002610000000100000100000003a7fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000003a7000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000078000000039fc0100000002fb0000000800540069006d0065020000000000000258000004f5000001e0fb0000000800540069006d00650100000000000004500000000000000000000005ab000003a700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 340 | Selection: 341 | collapsed: false 342 | Time: 343 | collapsed: false 344 | Tool Properties: 345 | collapsed: false 346 | Views: 347 | collapsed: true 348 | Width: 1920 349 | X: 0 350 | Y: 27 351 | -------------------------------------------------------------------------------- /rviz/patchworkpp_viz.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /src1 10 | - /plane_viz1 11 | - /Wall1 12 | Splitter Ratio: 0.6044568419456482 13 | Tree Height: 796 14 | - Class: rviz/Selection 15 | Name: Selection 16 | - Class: rviz/Tool Properties 17 | Expanded: 18 | - /2D Pose Estimate1 19 | - /2D Nav Goal1 20 | - /Publish Point1 21 | Name: Tool Properties 22 | Splitter Ratio: 0.5886790156364441 23 | - Class: rviz/Views 24 | Expanded: 25 | - /Current View1 26 | Name: Views 27 | Splitter Ratio: 0.5 28 | - Class: rviz/Time 29 | Experimental: false 30 | Name: Time 31 | SyncMode: 0 32 | SyncSource: src 33 | Preferences: 34 | PromptSaveOnExit: true 35 | Toolbars: 36 | toolButtonStyle: 2 37 | Visualization Manager: 38 | Class: "" 39 | Displays: 40 | - Alpha: 0.5 41 | Cell Size: 5 42 | Class: rviz/Grid 43 | Color: 160; 160; 164 44 | Enabled: true 45 | Line Style: 46 | Line Width: 0.029999999329447746 47 | Value: Lines 48 | Name: Grid 49 | Normal Cell Count: 0 50 | Offset: 51 | X: 0 52 | Y: 0 53 | Z: -3.059999942779541 54 | Plane: XY 55 | Plane Cell Count: 30 56 | Reference Frame: 57 | Value: true 58 | - Alpha: 1 59 | Autocompute Intensity Bounds: true 60 | Autocompute Value Bounds: 61 | Max Value: 10 62 | Min Value: -10 63 | Value: true 64 | Axis: Z 65 | Channel Name: intensity 66 | Class: rviz/PointCloud2 67 | Color: 0; 0; 0 68 | Color Transformer: FlatColor 69 | Decay Time: 0 70 | Enabled: false 71 | Invert Rainbow: false 72 | Max Color: 255; 255; 255 73 | Min Color: 0; 0; 0 74 | Name: src 75 | Position Transformer: XYZ 76 | Queue Size: 10 77 | Selectable: false 78 | Size (Pixels): 3 79 | Size (m): 0.10000000149011612 80 | Style: Flat Squares 81 | Topic: /benchmark/cloud 82 | Unreliable: false 83 | Use Fixed Frame: true 84 | Use rainbow: true 85 | Value: false 86 | - Alpha: 0.4000000059604645 87 | Class: jsk_rviz_plugin/PolygonArray 88 | Color: 25; 255; 0 89 | Enabled: true 90 | Name: plane_viz 91 | Topic: /patchworkpp/plane 92 | Unreliable: false 93 | Value: true 94 | coloring: Liekelihood 95 | enable lighting: false 96 | normal length: 1 97 | only border: false 98 | show normal: false 99 | - Alpha: 1 100 | Autocompute Intensity Bounds: true 101 | Autocompute Value Bounds: 102 | Max Value: 10 103 | Min Value: -10 104 | Value: true 105 | Axis: Z 106 | Channel Name: intensity 107 | Class: rviz/PointCloud2 108 | Color: 25; 255; 25 109 | Color Transformer: FlatColor 110 | Decay Time: 0 111 | Enabled: true 112 | Invert Rainbow: false 113 | Max Color: 255; 255; 255 114 | Min Color: 0; 0; 0 115 | Name: TP 116 | Position Transformer: XYZ 117 | Queue Size: 10 118 | Selectable: true 119 | Size (Pixels): 3 120 | Size (m): 0.20000000298023224 121 | Style: Flat Squares 122 | Topic: /benchmark/TP 123 | Unreliable: false 124 | Use Fixed Frame: true 125 | Use rainbow: true 126 | Value: true 127 | - Alpha: 1 128 | Autocompute Intensity Bounds: true 129 | Autocompute Value Bounds: 130 | Max Value: 10 131 | Min Value: -10 132 | Value: true 133 | Axis: Z 134 | Channel Name: intensity 135 | Class: rviz/PointCloud2 136 | Color: 25; 25; 255 137 | Color Transformer: FlatColor 138 | Decay Time: 0 139 | Enabled: true 140 | Invert Rainbow: false 141 | Max Color: 25; 25; 255 142 | Min Color: 0; 0; 0 143 | Name: FN 144 | Position Transformer: XYZ 145 | Queue Size: 10 146 | Selectable: true 147 | Size (Pixels): 3 148 | Size (m): 0.20000000298023224 149 | Style: Flat Squares 150 | Topic: /benchmark/FN 151 | Unreliable: false 152 | Use Fixed Frame: true 153 | Use rainbow: true 154 | Value: true 155 | - Alpha: 1 156 | Autocompute Intensity Bounds: true 157 | Autocompute Value Bounds: 158 | Max Value: 10 159 | Min Value: -10 160 | Value: true 161 | Axis: Z 162 | Channel Name: intensity 163 | Class: rviz/PointCloud2 164 | Color: 255; 25; 25 165 | Color Transformer: FlatColor 166 | Decay Time: 0 167 | Enabled: true 168 | Invert Rainbow: false 169 | Max Color: 255; 255; 255 170 | Min Color: 0; 0; 0 171 | Name: FP 172 | Position Transformer: XYZ 173 | Queue Size: 10 174 | Selectable: true 175 | Size (Pixels): 3 176 | Size (m): 0.20000000298023224 177 | Style: Flat Squares 178 | Topic: /benchmark/FP 179 | Unreliable: false 180 | Use Fixed Frame: true 181 | Use rainbow: true 182 | Value: true 183 | - Class: rviz/Marker 184 | Enabled: false 185 | Marker Topic: /precision 186 | Name: Marker 187 | Namespaces: 188 | {} 189 | Queue Size: 100 190 | Value: false 191 | - Class: rviz/Marker 192 | Enabled: false 193 | Marker Topic: /recall 194 | Name: Marker 195 | Namespaces: 196 | {} 197 | Queue Size: 100 198 | Value: false 199 | - Class: rviz/Axes 200 | Enabled: true 201 | Length: 3 202 | Name: Axes 203 | Radius: 0.4000000059604645 204 | Reference Frame: 205 | Value: true 206 | - Alpha: 0.20000000298023224 207 | Autocompute Intensity Bounds: true 208 | Autocompute Value Bounds: 209 | Max Value: 10 210 | Min Value: -10 211 | Value: true 212 | Axis: Z 213 | Channel Name: intensity 214 | Class: rviz/PointCloud2 215 | Color: 25; 255; 255 216 | Color Transformer: FlatColor 217 | Decay Time: 0 218 | Enabled: false 219 | Invert Rainbow: false 220 | Max Color: 255; 255; 255 221 | Min Color: 0; 0; 0 222 | Name: revert_pc 223 | Position Transformer: XYZ 224 | Queue Size: 10 225 | Selectable: true 226 | Size (Pixels): 3 227 | Size (m): 0.30000001192092896 228 | Style: Flat Squares 229 | Topic: /patchworkpp/revert_pc 230 | Unreliable: false 231 | Use Fixed Frame: true 232 | Use rainbow: true 233 | Value: false 234 | - Alpha: 1 235 | Autocompute Intensity Bounds: true 236 | Autocompute Value Bounds: 237 | Max Value: 10 238 | Min Value: -10 239 | Value: true 240 | Axis: Z 241 | Channel Name: intensity 242 | Class: rviz/PointCloud2 243 | Color: 255; 25; 255 244 | Color Transformer: FlatColor 245 | Decay Time: 0 246 | Enabled: false 247 | Invert Rainbow: false 248 | Max Color: 255; 255; 255 249 | Min Color: 0; 0; 0 250 | Name: reject_pc 251 | Position Transformer: XYZ 252 | Queue Size: 10 253 | Selectable: true 254 | Size (Pixels): 3 255 | Size (m): 0.15000000596046448 256 | Style: Flat Squares 257 | Topic: /patchworkpp/reject_pc 258 | Unreliable: false 259 | Use Fixed Frame: true 260 | Use rainbow: true 261 | Value: false 262 | - Alpha: 1 263 | Class: jsk_rviz_plugin/NormalDisplay 264 | Color: 0; 0; 0 265 | Display Rate (%): 100 266 | Enabled: true 267 | Max Color: 255; 0; 0 268 | MinColor: 0; 255; 0 269 | Name: NormalDisplay 270 | Scale: 1 271 | Style: FlatColor 272 | Topic: /patchworkpp/normals 273 | Unreliable: false 274 | Use Rainbow: true 275 | Value: true 276 | - Alpha: 1 277 | Autocompute Intensity Bounds: true 278 | Autocompute Value Bounds: 279 | Max Value: 10 280 | Min Value: -10 281 | Value: true 282 | Axis: Z 283 | Channel Name: intensity 284 | Class: rviz/PointCloud2 285 | Color: 252; 175; 62 286 | Color Transformer: FlatColor 287 | Decay Time: 0 288 | Enabled: true 289 | Invert Rainbow: false 290 | Max Color: 239; 41; 41 291 | Min Color: 0; 0; 0 292 | Name: Noise 293 | Position Transformer: XYZ 294 | Queue Size: 10 295 | Selectable: true 296 | Size (Pixels): 3 297 | Size (m): 0.20000000298023224 298 | Style: Flat Squares 299 | Topic: /patchworkpp/noise 300 | Unreliable: false 301 | Use Fixed Frame: true 302 | Use rainbow: false 303 | Value: true 304 | - Alpha: 0.10000000149011612 305 | Autocompute Intensity Bounds: true 306 | Autocompute Value Bounds: 307 | Max Value: 10 308 | Min Value: -10 309 | Value: true 310 | Axis: Z 311 | Channel Name: intensity 312 | Class: rviz/PointCloud2 313 | Color: 252; 233; 79 314 | Color Transformer: FlatColor 315 | Decay Time: 0 316 | Enabled: true 317 | Invert Rainbow: false 318 | Max Color: 255; 255; 255 319 | Min Color: 0; 0; 0 320 | Name: Wall 321 | Position Transformer: XYZ 322 | Queue Size: 10 323 | Selectable: true 324 | Size (Pixels): 3 325 | Size (m): 0.30000001192092896 326 | Style: Flat Squares 327 | Topic: /patchworkpp/vertical 328 | Unreliable: false 329 | Use Fixed Frame: true 330 | Use rainbow: true 331 | Value: true 332 | Enabled: true 333 | Global Options: 334 | Background Color: 0; 0; 0 335 | Default Light: true 336 | Fixed Frame: map 337 | Frame Rate: 30 338 | Name: root 339 | Tools: 340 | - Class: rviz/Interact 341 | Hide Inactive Objects: true 342 | - Class: rviz/MoveCamera 343 | - Class: rviz/Select 344 | - Class: rviz/FocusCamera 345 | - Class: rviz/Measure 346 | - Class: rviz/SetInitialPose 347 | Theta std deviation: 0.2617993950843811 348 | Topic: /initialpose 349 | X std deviation: 0.5 350 | Y std deviation: 0.5 351 | - Class: rviz/SetGoal 352 | Topic: /move_base_simple/goal 353 | - Class: rviz/PublishPoint 354 | Single click: true 355 | Topic: /clicked_point 356 | Value: true 357 | Views: 358 | Current: 359 | Class: rviz/Orbit 360 | Distance: 93.28809356689453 361 | Enable Stereo Rendering: 362 | Stereo Eye Separation: 0.05999999865889549 363 | Stereo Focal Distance: 1 364 | Swap Stereo Eyes: false 365 | Value: false 366 | Focal Point: 367 | X: 21.806533813476562 368 | Y: 5.058434963226318 369 | Z: -7.474634647369385 370 | Focal Shape Fixed Size: false 371 | Focal Shape Size: 0.05000000074505806 372 | Invert Z Axis: false 373 | Name: Current View 374 | Near Clip Distance: 0.009999999776482582 375 | Pitch: 0.784797728061676 376 | Target Frame: 377 | Value: Orbit (rviz) 378 | Yaw: 3.5003976821899414 379 | Saved: ~ 380 | Window Geometry: 381 | Displays: 382 | collapsed: false 383 | Height: 1025 384 | Hide Left Dock: false 385 | Hide Right Dock: true 386 | QMainWindow State: 000000ff00000000fd000000040000000000000156000003a7fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000003a7000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c000002610000000100000100000003a7fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000003a7000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000078000000039fc0100000002fb0000000800540069006d0065020000000000000258000004f5000001e0fb0000000800540069006d0065010000000000000450000000000000000000000624000003a700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 387 | Selection: 388 | collapsed: false 389 | Time: 390 | collapsed: false 391 | Tool Properties: 392 | collapsed: false 393 | Views: 394 | collapsed: true 395 | Width: 1920 396 | X: 0 397 | Y: 27 398 | -------------------------------------------------------------------------------- /src/demo.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | // For disable PCL complile lib, to use PointXYZIR 3 | #define PCL_NO_PRECOMPILE 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include "patchworkpp/patchworkpp.hpp" 11 | 12 | using PointType = pcl::PointXYZI; 13 | using namespace std; 14 | 15 | boost::shared_ptr> PatchworkppGroundSeg; 16 | 17 | ros::Publisher pub_cloud; 18 | ros::Publisher pub_ground; 19 | ros::Publisher pub_non_ground; 20 | 21 | template 22 | sensor_msgs::PointCloud2 cloud2msg(pcl::PointCloud cloud, const ros::Time& stamp, std::string frame_id = "map") { 23 | sensor_msgs::PointCloud2 cloud_ROS; 24 | pcl::toROSMsg(cloud, cloud_ROS); 25 | cloud_ROS.header.stamp = stamp; 26 | cloud_ROS.header.frame_id = frame_id; 27 | return cloud_ROS; 28 | } 29 | 30 | void callbackCloud(const sensor_msgs::PointCloud2::Ptr &cloud_msg) 31 | { 32 | double time_taken; 33 | 34 | pcl::PointCloud pc_curr; 35 | pcl::PointCloud pc_ground; 36 | pcl::PointCloud pc_non_ground; 37 | 38 | pcl::fromROSMsg(*cloud_msg, pc_curr); 39 | 40 | PatchworkppGroundSeg->estimate_ground(pc_curr, pc_ground, pc_non_ground, time_taken); 41 | 42 | ROS_INFO_STREAM("\033[1;32m" << "Input PointCloud: " << pc_curr.size() << " -> Ground: " << pc_ground.size() << "/ NonGround: " << pc_non_ground.size() 43 | << " (running_time: " << time_taken << " sec)" << "\033[0m"); 44 | 45 | pub_cloud.publish(cloud2msg(pc_curr, cloud_msg->header.stamp, cloud_msg->header.frame_id)); 46 | pub_ground.publish(cloud2msg(pc_ground, cloud_msg->header.stamp, cloud_msg->header.frame_id)); 47 | pub_non_ground.publish(cloud2msg(pc_non_ground, cloud_msg->header.stamp, cloud_msg->header.frame_id)); 48 | } 49 | 50 | int main(int argc, char**argv) { 51 | 52 | ros::init(argc, argv, "Demo"); 53 | ros::NodeHandle nh; 54 | ros::NodeHandle pnh("~"); 55 | 56 | std::string cloud_topic; 57 | pnh.param("cloud_topic", cloud_topic, "/pointcloud"); 58 | 59 | cout << "Operating patchwork++..." << endl; 60 | PatchworkppGroundSeg.reset(new PatchWorkpp(&pnh)); 61 | 62 | pub_cloud = pnh.advertise("cloud", 100, true); 63 | pub_ground = pnh.advertise("ground", 100, true); 64 | pub_non_ground = pnh.advertise("nonground", 100, true); 65 | 66 | ros::Subscriber sub_cloud = nh.subscribe(cloud_topic, 100, callbackCloud); 67 | 68 | ros::spin(); 69 | 70 | return 0; 71 | } 72 | -------------------------------------------------------------------------------- /src/offline_kitti.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | // For disable PCL complile lib, to use PointXYZIR 3 | #define PCL_NO_PRECOMPILE 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include "patchworkpp/patchworkpp.hpp" 11 | 12 | #include "tools/kitti_loader.hpp" 13 | 14 | using PointType = PointXYZILID; 15 | using namespace std; 16 | 17 | void signal_callback_handler(int signum) { 18 | cout << "Caught Ctrl + c " << endl; 19 | // Terminate program 20 | exit(signum); 21 | } 22 | 23 | template 24 | pcl::PointCloud cloudmsg2cloud(sensor_msgs::PointCloud2 cloudmsg) { 25 | pcl::PointCloud cloudresult; 26 | pcl::fromROSMsg(cloudmsg, cloudresult); 27 | return cloudresult; 28 | } 29 | 30 | template 31 | sensor_msgs::PointCloud2 cloud2msg(pcl::PointCloud cloud, std::string frame_id = "map") { 32 | sensor_msgs::PointCloud2 cloud_ROS; 33 | pcl::toROSMsg(cloud, cloud_ROS); 34 | cloud_ROS.header.frame_id = frame_id; 35 | return cloud_ROS; 36 | } 37 | 38 | 39 | int main(int argc, char**argv) { 40 | 41 | ros::Publisher CloudPublisher; 42 | ros::Publisher TPPublisher; 43 | ros::Publisher FPPublisher; 44 | ros::Publisher FNPublisher; 45 | 46 | boost::shared_ptr > PatchworkppGroundSeg; 47 | std::string output_csvpath; 48 | 49 | std::string acc_filename; 50 | std::string pcd_savepath; 51 | std::string data_path; 52 | string algorithm; 53 | string seq; 54 | 55 | bool save_flag; 56 | 57 | int init_idx; 58 | bool save_csv_file; 59 | bool stop_per_each_frame; 60 | 61 | ros::init(argc, argv, "Offline_KITTI"); 62 | 63 | ros::NodeHandle nh; 64 | nh.param("/algorithm", algorithm, "patchworkpp"); 65 | nh.param("/sequence", seq, "00"); 66 | nh.param("/data_path", data_path, "/"); 67 | nh.param("/output_csvpath", output_csvpath, "/data/"); 68 | 69 | nh.param("/init_idx", init_idx, 0); 70 | nh.param("/save_csv_file", save_csv_file, false); 71 | nh.param("/stop_per_each_frame", stop_per_each_frame, false); 72 | 73 | auto t = std::time(nullptr); 74 | auto tm = *std::localtime(&t); 75 | 76 | CloudPublisher = nh.advertise("/benchmark/cloud", 100, true); 77 | TPPublisher = nh.advertise("/benchmark/TP", 100, true); 78 | FPPublisher = nh.advertise("/benchmark/FP", 100, true); 79 | FNPublisher = nh.advertise("/benchmark/FN", 100, true); 80 | 81 | signal(SIGINT, signal_callback_handler); 82 | 83 | PatchworkppGroundSeg.reset(new PatchWorkpp(&nh)); 84 | data_path = data_path + "/" + seq; 85 | KittiLoader loader(data_path); 86 | 87 | int N = loader.size(); 88 | for (int n = init_idx; n < N; ++n) { 89 | 90 | cout << n << "th node come" << endl; 91 | pcl::PointCloud pc_curr; 92 | loader.get_cloud(n, pc_curr); 93 | pcl::PointCloud pc_ground; 94 | pcl::PointCloud pc_non_ground; 95 | 96 | static double time_taken; 97 | cout << "Operating patchwork++..." << endl; 98 | 99 | PatchworkppGroundSeg->estimate_ground(pc_curr, pc_ground, pc_non_ground, time_taken); 100 | 101 | // Estimation 102 | double precision, recall, precision_wo_veg, recall_wo_veg; 103 | calculate_precision_recall(pc_curr, pc_ground, precision, recall); 104 | calculate_precision_recall_without_vegetation(pc_curr, pc_ground, precision_wo_veg, recall_wo_veg); 105 | 106 | cout << "\033[1;32m" << n << "th, " << " takes : " << time_taken << " | " 107 | << pc_curr.size() << " -> " << pc_ground.size() << "\033[0m" << endl; 108 | 109 | cout << "\033[1;32m P: " << precision_wo_veg << " | R: " << recall_wo_veg << "\033[0m" << endl; 110 | 111 | // - - - - - - - - - - - - - - - - - - - - 112 | // If you want to save precision/recall in a text file, revise this part 113 | // - - - - - - - - - - - - - - - - - - - - 114 | if (save_csv_file) 115 | { 116 | ofstream sc_output(output_csvpath + seq + ".csv", ios::app); 117 | sc_output << n << "," << time_taken << "," << precision << "," << recall << "," << precision_wo_veg << "," << recall_wo_veg; 118 | sc_output << std::endl; 119 | sc_output.close(); 120 | } 121 | 122 | // - - - - - - - - - - - - - - - - - - - - 123 | 124 | // Publish msg 125 | pcl::PointCloud TP; 126 | pcl::PointCloud FP; 127 | pcl::PointCloud FN; 128 | pcl::PointCloud TN; 129 | discern_ground_without_vegetation(pc_ground, TP, FP); 130 | discern_ground_without_vegetation(pc_non_ground, FN, TN); 131 | 132 | // - - - - - - - - - - - - - - - - - - - - 133 | // If you want to save the output of pcd, revise this part 134 | // - - - - - - - - - - - - - - - - - - - - 135 | // if (save_flag) { 136 | // std::map pc_curr_gt_counts, g_est_gt_counts; 137 | // double accuracy; 138 | // save_all_accuracy(pc_curr, pc_ground, acc_filename, accuracy, pc_curr_gt_counts, g_est_gt_counts); 139 | // 140 | // std::string count_str = std::to_string(n); 141 | // std::string count_str_padded = std::string(NUM_ZEROS - count_str.length(), '0') + count_str; 142 | // std::string pcd_filename = pcd_savepath + "/" + count_str_padded + ".pcd"; 143 | // pc2pcdfile(TP, FP, FN, TN, pcd_filename); 144 | // } 145 | // - - - - - - - - - - - - - - - - - - - - 146 | 147 | CloudPublisher.publish(cloud2msg(pc_curr)); 148 | TPPublisher.publish(cloud2msg(TP)); 149 | FPPublisher.publish(cloud2msg(FP)); 150 | FNPublisher.publish(cloud2msg(FN)); 151 | ros::spinOnce(); 152 | 153 | if (stop_per_each_frame) cin.ignore(); 154 | } 155 | 156 | return 0; 157 | } 158 | -------------------------------------------------------------------------------- /src/video.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | // For disable PCL complile lib, to use PointXYZIR 3 | #define PCL_NO_PRECOMPILE 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include "patchworkpp/patchworkpp.hpp" 11 | 12 | #include "tools/kitti_loader.hpp" 13 | 14 | using PointType = PointXYZILID; 15 | using namespace std; 16 | 17 | void signal_callback_handler(int signum) { 18 | cout << "Caught Ctrl + c " << endl; 19 | // Terminate program 20 | exit(signum); 21 | } 22 | 23 | template 24 | pcl::PointCloud cloudmsg2cloud(sensor_msgs::PointCloud2 cloudmsg) { 25 | pcl::PointCloud cloudresult; 26 | pcl::fromROSMsg(cloudmsg, cloudresult); 27 | return cloudresult; 28 | } 29 | 30 | template 31 | sensor_msgs::PointCloud2 cloud2msg(pcl::PointCloud cloud, std::string frame_id = "map") { 32 | sensor_msgs::PointCloud2 cloud_ROS; 33 | pcl::toROSMsg(cloud, cloud_ROS); 34 | cloud_ROS.header.frame_id = frame_id; 35 | return cloud_ROS; 36 | } 37 | 38 | 39 | int main(int argc, char**argv) { 40 | 41 | ros::Publisher CloudPublisher; 42 | ros::Publisher GroundPublisher; 43 | ros::Publisher NongroundPublisher; 44 | // ros::Publisher TPPublisher; 45 | // ros::Publisher FPPublisher; 46 | // ros::Publisher FNPublisher; 47 | 48 | boost::shared_ptr > PatchworkppGroundSeg; 49 | std::string output_csvpath; 50 | 51 | std::string acc_filename; 52 | std::string pcd_savepath; 53 | std::string data_path; 54 | string algorithm; 55 | string seq; 56 | 57 | bool save_flag; 58 | 59 | int init_idx; 60 | bool save_csv_file; 61 | bool stop_per_each_frame; 62 | 63 | ros::init(argc, argv, "Video"); 64 | 65 | ros::NodeHandle nh; 66 | nh.param("/algorithm", algorithm, "patchworkpp"); 67 | nh.param("/sequence", seq, "00"); 68 | nh.param("/data_path", data_path, "/"); 69 | nh.param("/output_csvpath", output_csvpath, "/data/"); 70 | 71 | nh.param("/init_idx", init_idx, 0); 72 | nh.param("/save_csv_file", save_csv_file, false); 73 | nh.param("/stop_per_each_frame", stop_per_each_frame, false); 74 | 75 | auto t = std::time(nullptr); 76 | auto tm = *std::localtime(&t); 77 | 78 | string HOME = std::getenv("HOME"); 79 | data_path = HOME + data_path; 80 | output_csvpath = HOME + output_csvpath; 81 | 82 | CloudPublisher = nh.advertise("/demo/cloud", 100, true); 83 | GroundPublisher = nh.advertise("/demo/ground", 100, true); 84 | NongroundPublisher = nh.advertise("/demo/nonground", 100, true); 85 | // TPPublisher = nh.advertise("/benchmark/TP", 100, true); 86 | // FPPublisher = nh.advertise("/benchmark/FP", 100, true); 87 | // FNPublisher = nh.advertise("/benchmark/FN", 100, true); 88 | 89 | signal(SIGINT, signal_callback_handler); 90 | 91 | PatchworkppGroundSeg.reset(new PatchWorkpp(&nh)); 92 | data_path = data_path + "/" + seq; 93 | KittiLoader loader(data_path); 94 | 95 | int N = loader.size(); 96 | for (int n = init_idx; n < N; ++n) { 97 | 98 | cout << n << "th node come" << endl; 99 | pcl::PointCloud pc_curr; 100 | loader.get_cloud(n, pc_curr); 101 | pcl::PointCloud pc_ground; 102 | pcl::PointCloud pc_non_ground; 103 | 104 | static double time_taken; 105 | cout << "Operating patchwork++..." << endl; 106 | 107 | PatchworkppGroundSeg->estimate_ground(pc_curr, pc_ground, pc_non_ground, time_taken); 108 | 109 | // Estimation 110 | // double precision, recall, precision_wo_veg, recall_wo_veg; 111 | // calculate_precision_recall(pc_curr, pc_ground, precision, recall); 112 | // calculate_precision_recall_without_vegetation(pc_curr, pc_ground, precision_wo_veg, recall_wo_veg); 113 | 114 | cout << "\033[1;32m" << n << "th, " << " takes : " << time_taken << " | " 115 | << pc_curr.size() << " -> " << pc_ground.size() << "\033[0m" << endl; 116 | 117 | // cout << "\033[1;32m P: " << precision_wo_veg << " | R: " << recall_wo_veg << "\033[0m" << endl; 118 | 119 | // - - - - - - - - - - - - - - - - - - - - 120 | // If you want to save precision/recall in a text file, revise this part 121 | // - - - - - - - - - - - - - - - - - - - - 122 | // if (save_csv_file) 123 | // { 124 | // ofstream sc_output(output_csvpath + seq + ".csv", ios::app); 125 | // sc_output << n << "," << time_taken << "," << precision << "," << recall << "," << precision_wo_veg << "," << recall_wo_veg; 126 | // sc_output << std::endl; 127 | // sc_output.close(); 128 | // } 129 | 130 | // - - - - - - - - - - - - - - - - - - - - 131 | 132 | // Publish msg 133 | // pcl::PointCloud TP; 134 | // pcl::PointCloud FP; 135 | // pcl::PointCloud FN; 136 | // pcl::PointCloud TN; 137 | // discern_ground_without_vegetation(pc_ground, TP, FP); 138 | // discern_ground_without_vegetation(pc_non_ground, FN, TN); 139 | 140 | // - - - - - - - - - - - - - - - - - - - - 141 | // If you want to save the output of pcd, revise this part 142 | // - - - - - - - - - - - - - - - - - - - - 143 | // if (save_flag) { 144 | // std::map pc_curr_gt_counts, g_est_gt_counts; 145 | // double accuracy; 146 | // save_all_accuracy(pc_curr, pc_ground, acc_filename, accuracy, pc_curr_gt_counts, g_est_gt_counts); 147 | // 148 | // std::string count_str = std::to_string(n); 149 | // std::string count_str_padded = std::string(NUM_ZEROS - count_str.length(), '0') + count_str; 150 | // std::string pcd_filename = pcd_savepath + "/" + count_str_padded + ".pcd"; 151 | // pc2pcdfile(TP, FP, FN, TN, pcd_filename); 152 | // } 153 | // - - - - - - - - - - - - - - - - - - - - 154 | 155 | CloudPublisher.publish(cloud2msg(pc_curr)); 156 | GroundPublisher.publish(cloud2msg(pc_ground)); 157 | NongroundPublisher.publish(cloud2msg(pc_non_ground)); 158 | // TPPublisher.publish(cloud2msg(TP)); 159 | // FPPublisher.publish(cloud2msg(FP)); 160 | // FNPublisher.publish(cloud2msg(FN)); 161 | ros::spinOnce(); 162 | 163 | if (stop_per_each_frame) cin.ignore(); 164 | } 165 | 166 | return 0; 167 | } 168 | --------------------------------------------------------------------------------