├── CMakeLists.txt ├── COPYING ├── EXTERNAL ├── gnuplot-iostream.h └── mcqd │ ├── COPYING │ ├── README │ ├── mcqd.cpp │ └── mcqd.h ├── LICENSE ├── README ├── cmake_modules ├── FindEigen3.cmake └── falkolibConfig.cmake ├── imgs ├── falko-bsc.png ├── falko-score.png └── intel-lab-falko-map.png ├── include └── falkolib │ ├── Common │ ├── GeomUtils.h │ ├── HoughSpectrum.h │ ├── LaserScan.h │ └── Point.h │ ├── Feature │ ├── BSC.h │ ├── BSCExtractor.h │ ├── CGH.h │ ├── CGHExtractor.h │ ├── Descriptor.h │ ├── DescriptorExtractor.h │ ├── FALKO.h │ ├── FALKOExtractor.h │ ├── Keypoint.h │ ├── KeypointExtractor.h │ ├── OC.h │ └── OCExtractor.h │ └── Matching │ ├── AHTMatcher.h │ ├── CCDAMatcher.h │ ├── Matcher.h │ └── NNMatcher.h ├── info.xml ├── src ├── Common │ └── HoughSpectrum.cpp └── Feature │ ├── BSC.cpp │ ├── CGH.cpp │ ├── FALKOExtractor.cpp │ └── OCExtractor.cpp └── test ├── testData.cpp ├── testData.h ├── testFalkoAHT.cpp ├── testFalkoCC.cpp ├── testKeypointFalko.cpp └── testKeypointOC.cpp /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8) 2 | project(falkolib) 3 | set(CMAKE_BUILD_TYPE Release) 4 | add_definitions(-std=c++0x) # Enabling c++11 5 | 6 | SET(falkolib_RUNTIME_OUTPUT_DIRECTORY ${falkolib_SOURCE_DIR}/bin CACHE PATH "Target for the binaries") 7 | SET(falkolib_LIBRARY_OUTPUT_DIRECTORY ${falkolib_SOURCE_DIR}/lib CACHE PATH "Target for the libraries") 8 | SET(CMAKE_ARCHIVE_OUTPUT_DIRECTORY ${falkolib_LIBRARY_OUTPUT_DIRECTORY}) 9 | SET(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${falkolib_LIBRARY_OUTPUT_DIRECTORY}) 10 | SET(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${falkolib_RUNTIME_OUTPUT_DIRECTORY}) 11 | LIST(APPEND CMAKE_MODULE_PATH ${falkolib_SOURCE_DIR}/cmake_modules) 12 | 13 | message(STATUS "CMAKE_MODULE_PATH: ${CMAKE_MODULE_PATH}") 14 | 15 | find_package(Boost) 16 | if (${Boost_FOUND}) 17 | message(STATUS "Boost_INCLUDE_DIRS: ${Boost_INCLUDE_DIRS}") 18 | message(STATUS "Boost_LIBRARY_DIRS: ${Boost_LIBRARY_DIRS}") 19 | message(STATUS "Boost_LIBRARIES: ${Boost_LIBRARY_DIRS}") 20 | include_directories(${Boost_INCLUDE_DIRS}) 21 | link_directories(${Boost_LIBRARY_DIRS}) 22 | endif(${Boost_FOUND}) 23 | 24 | find_package(Eigen3 REQUIRED) 25 | if (${EIGEN3_FOUND}) 26 | include_directories(${EIGEN3_INCLUDE_DIR}) 27 | message(STATUS "EIGEN3_INCLUDE_DIR: ${EIGEN3_INCLUDE_DIR}") 28 | else(${EIGEN3_FOUND}) 29 | message(WARNING "Cannot find Eigen3 Library") 30 | endif(${EIGEN3_FOUND}) 31 | 32 | message(STATUS "include dir: ${falkolib_SOURCE_DIR}/include") 33 | message(STATUS "include dir: ${falkolib_SOURCE_DIR}/EXTERNAL") 34 | include_directories(${falkolib_SOURCE_DIR}/include ${falkolib_SOURCE_DIR}/EXTERNAL) 35 | 36 | add_library(falkolib 37 | src/Common/HoughSpectrum.cpp 38 | src/Feature/FALKOExtractor.cpp 39 | src/Feature/OCExtractor.cpp 40 | src/Feature/CGH.cpp 41 | src/Feature/BSC.cpp 42 | ) 43 | 44 | add_executable(testKeypointFalko test/testKeypointFalko.cpp test/testData.cpp) 45 | target_link_libraries(testKeypointFalko falkolib ${Boost_LIBRARIES}) 46 | 47 | add_executable(testFalkoAHT test/testFalkoAHT.cpp test/testData.cpp) 48 | target_link_libraries(testFalkoAHT falkolib ${Boost_LIBRARIES}) 49 | 50 | add_executable(testFalkoCC test/testFalkoCC.cpp test/testData.cpp) 51 | target_link_libraries(testFalkoCC falkolib ${Boost_LIBRARIES}) 52 | 53 | add_executable(testKeypointOC test/testKeypointOC.cpp test/testData.cpp) 54 | target_link_libraries(testKeypointOC falkolib ${Boost_LIBRARIES}) # boost_iostreams boost_system boost_filesystem 55 | 56 | 57 | # Option "make install": copy binaries 58 | INSTALL(TARGETS falkolib 59 | RUNTIME DESTINATION ${CMAKE_INSTALL_PREFIX}/bin 60 | LIBRARY DESTINATION ${CMAKE_INSTALL_PREFIX}/lib 61 | ARCHIVE DESTINATION ${CMAKE_INSTALL_PREFIX}/lib 62 | ) 63 | 64 | # Option "make install": copy headers 65 | FILE(GLOB headers_Common "${CMAKE_CURRENT_SOURCE_DIR}/include/falkolib/Common/*.h") 66 | FILE(GLOB headers_Feature "${CMAKE_CURRENT_SOURCE_DIR}/include/falkolib/Feature/*.h") 67 | FILE(GLOB headers_Matching "${CMAKE_CURRENT_SOURCE_DIR}/include/falkolib/Matching/*.h") 68 | INSTALL(FILES ${headers_Common} DESTINATION ${CMAKE_INSTALL_PREFIX}/include/falkolib/Common) 69 | INSTALL(FILES ${headers_Feature} DESTINATION ${CMAKE_INSTALL_PREFIX}/include/falkolib/Feature) 70 | INSTALL(FILES ${headers_Matching} DESTINATION ${CMAKE_INSTALL_PREFIX}/include/falkolib/Matching) 71 | 72 | # Option "make install": copy cmake script 73 | FILE(GLOB cmake_script "${CMAKE_CURRENT_SOURCE_DIR}/cmake_modules/falkolibConfig.cmake") 74 | message(STATUS "cmake_script " ${cmake_script}) 75 | INSTALL(FILES ${cmake_script} DESTINATION ${CMAKE_INSTALL_PREFIX}/share/falkolib/) 76 | 77 | -------------------------------------------------------------------------------- /COPYING: -------------------------------------------------------------------------------- 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 | -------------------------------------------------------------------------------- /EXTERNAL/mcqd/README: -------------------------------------------------------------------------------- 1 | Compile with: g++ -O3 mcqd.cpp -o mcqd 2 | Run with: mcqd test.clq 3 | mcqd.cpp ... example program that shows how to use class implemented in mcqd.h 4 | mcqd.h ... a class that contains two variants of the MCQD maximum clique algorithm 5 | test.clq ... an example clique in DIMACS format 6 | -------------------------------------------------------------------------------- /EXTERNAL/mcqd/mcqd.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright 2007-2012 Janez Konc 3 | 4 | If you use this program, please cite: 5 | Janez Konc and Dusanka Janezic. An improved branch and bound algorithm for the 6 | maximum clique problem. MATCH Commun. Math. Comput. Chem., 2007, 58, 569-590. 7 | 8 | More information at: http://www.sicmm.org/~konc 9 | 10 | This program is free software: you can redistribute it and/or modify 11 | it under the terms of the GNU General Public License as published by 12 | the Free Software Foundation, either version 3 of the License, or 13 | (at your option) any later version. 14 | 15 | This program is distributed in the hope that it will be useful, 16 | but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | GNU General Public License for more details. 19 | 20 | You should have received a copy of the GNU General Public License 21 | along with this program. If not, see . 22 | */ 23 | 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | 32 | using namespace std; 33 | 34 | void read_dimacs(string name, bool** &conn, int &size) { 35 | ifstream f (name.c_str()); 36 | string buffer; 37 | assert(f.is_open()); 38 | set v; 39 | multimap e; 40 | while (!getline(f, buffer).eof()) { 41 | if (buffer[0] == 'e') { 42 | int vi, vj; 43 | sscanf(buffer.c_str(), "%*c %d %d", &vi, &vj); 44 | v.insert(vi); 45 | v.insert(vj); 46 | e.insert(make_pair(vi, vj)); 47 | } 48 | } 49 | // size = v.size() + 1; 50 | size = *v.rbegin() + 1; 51 | conn = new bool*[size]; 52 | for (int i=0; i < size; i++) { 53 | conn[i] = new bool[size]; 54 | memset(conn[i], 0, size * sizeof(bool)); 55 | } 56 | for (multimap::iterator it = e.begin(); it != e.end(); it++) { 57 | conn[it->first][it->second] = true; 58 | conn[it->second][it->first] = true; 59 | } 60 | cout << "|E| = " << e.size() << " |V| = " << v.size() << " p = " << (double) e.size() / (v.size() * (v.size() - 1) / 2) << endl; 61 | f.close(); 62 | } 63 | 64 | 65 | int main(int argc, char *argv[]) { 66 | assert(argc == 2); 67 | cout << "args = " << argv[1] << endl; 68 | bool **conn; 69 | int size; 70 | read_dimacs(argv[1], conn, size); 71 | cout << "---------- Example 1: run max clique with improved coloring ----------------"<0); 150 | for (int i=0; i < sz; i++) V.push(i); 151 | e = conn; 152 | C = new ColorClass[sz + 1]; 153 | for (int i=0; i < sz + 1; i++) C[i].init(sz + 1); 154 | S = new StepCount[sz + 1]; 155 | } 156 | 157 | void Maxclique::_mcq(int* &maxclique, int &sz, bool dyn) { 158 | V.set_degrees(*this); 159 | V.sort(); 160 | V.init_colors(); 161 | if (dyn) { 162 | for (int i=0; i < V.size() + 1; i++) { 163 | S[i].set_i1(0); 164 | S[i].set_i2(0); 165 | } 166 | expand_dyn(V); 167 | } 168 | else 169 | expand(V); 170 | maxclique = new int[QMAX.size()]; 171 | for (int i=0; i maxno) { 221 | maxno = k; 222 | C[maxno + 1].rewind(); 223 | } 224 | C[k].push(pi); 225 | if (k < min_k) { 226 | R.at(j++).set_i(pi); 227 | } 228 | } 229 | if (j > 0) R.at(j-1).set_degree(0); 230 | if (min_k <= 0) min_k = 1; 231 | for (k = min_k; k <= maxno; k++) 232 | for (int i = 0; i < C[k].size(); i++) { 233 | R.at(j).set_i(C[k].at(i)); 234 | R.at(j++).set_degree(k); 235 | } 236 | } 237 | 238 | void Maxclique::expand(Vertices R) { 239 | while (R.size()) { 240 | if (Q.size() + R.end().get_degree() > QMAX.size()) { 241 | Q.push(R.end().get_i()); 242 | Vertices Rp(R.size()); 243 | cut2(R, Rp); 244 | if (Rp.size()) { 245 | color_sort(Rp); 246 | pk++; 247 | expand(Rp); 248 | } 249 | else if (Q.size() > QMAX.size()) { 250 | //std::cout << "step = " << pk << " current max. clique size = " << Q.size() << std::endl; 251 | QMAX = Q; 252 | } 253 | Rp.dispose(); 254 | Q.pop(); 255 | } 256 | else { 257 | return; 258 | } 259 | R.pop(); 260 | } 261 | } 262 | 263 | void Maxclique::expand_dyn(Vertices R) { 264 | S[level].set_i1(S[level].get_i1() + S[level - 1].get_i1() - S[level].get_i2()); 265 | S[level].set_i2(S[level - 1].get_i1()); 266 | while (R.size()) { 267 | if (Q.size() + R.end().get_degree() > QMAX.size()) { 268 | Q.push(R.end().get_i()); 269 | Vertices Rp(R.size()); 270 | cut2(R, Rp); 271 | if (Rp.size()) { 272 | if ((float)S[level].get_i1()/++pk < Tlimit) { 273 | degree_sort(Rp); 274 | } 275 | color_sort(Rp); 276 | S[level].inc_i1(); 277 | level++; 278 | expand_dyn(Rp); 279 | level--; 280 | } 281 | else if (Q.size() > QMAX.size()) { 282 | //std::cout << "step = " << pk << " current max. clique size = " << Q.size() << std::endl; 283 | QMAX = Q; 284 | } 285 | Rp.dispose(); 286 | Q.pop(); 287 | } 288 | else { 289 | return; 290 | } 291 | R.pop(); 292 | } 293 | } 294 | 295 | #endif 296 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | FALKOLib is released under a GPLv3 license (see COPYING). 2 | Please note that we provide along FALKOLib a version of mcq, Janez Konc's Maximum Clique Algorithm 3 | (see http://www.sicmm.org/konc/ for more information). 4 | 5 | For a closed-source version of FALKOLib for commercial purposes, please contact the authors. 6 | 7 | If you use FALKOLib in an academic work, please cite the most relevant publication associated by visiting: 8 | http://rimlab.ce.unipr.it/. 9 | -------------------------------------------------------------------------------- /README: -------------------------------------------------------------------------------- 1 | * FALKOLib - Fast Adaptive Laser Keypoint Orientation-invariant 2 | * Copyright (C) 2016 Fabjan Kallasi and Dario Lodi Rizzini. 3 | 4 | 5 | OVERVIEW 6 | ------------------------------------------------- 7 | 8 | FALKOLib is a library that implements two keypoint detectors 9 | and two descriptors designed for 2D LIDARs. 10 | The two detectors are: 11 | - FALKO (Fast Adaptive Laser Keypoint Orientation-invariant), a 12 | general purpose keypoint detector which gives the name to 13 | the whole library; 14 | - OC (Orthogonal Corner), a keypoint specific for enviroments 15 | with straight linear walls and architectural elements 16 | arranged along orthogonal directions. 17 | The two descriptors are: 18 | - BSC (Binary Shape Context), a binary version of standard 19 | shape context descriptor; 20 | - CGH (Cumulative Gaussian Histogram), which represents the 21 | neighborhood of a point with an histogram. 22 | 23 | Moreover, the library provides the implementation of methods 24 | for keypoint/feature data association. 25 | In particular, the following methods have been implemented: 26 | - NN (Nearest Neighbor): each keypoint/feature of one set 27 | are associated to the nearest keypoint/features of another set; 28 | - CCDA (Combined Constraint Data Association): features are 29 | associated by finding the maximal sets of corresponding 30 | feature pairs, which are compatible with constraints to 31 | a set; 32 | - AHT (Affine Hough Transform): the corresponding features 33 | are found by voting the affine/rigid transformation 34 | that overlaps them according to Hough technique. 35 | 36 | If you use this library, please cite the following paper: 37 | 38 | F. Kallasi, D. Lodi Rizzini, and S. Caselli. 39 | Fast Keypoint Features from Laser Scanner for Robot Localization and Mapping. 40 | IEEE Robotics and Automation Letters (RA-L), 1(1):176-183, jan 2016. 41 | DOI 10.1109/LRA.2016.2517210 42 | 43 | or the most relevant publication associated by visiting: 44 | http://rimlab.ce.unipr.it/ 45 | 46 | 47 | DEPENDENCIES 48 | ------------------------------------------------- 49 | 50 | The software depends on the following external libraries 51 | 52 | Boost >= 1.36 (submodule lexical_cast) 53 | Eigen 3.0 54 | 55 | The library also requires the third party library mcqd 56 | developed by Janez Konc (see http://www.sicmm.org/konc/), 57 | which has been included in folder EXTERNAL. 58 | 59 | 60 | HOW TO COMPILE 61 | ------------------------------------------------- 62 | 63 | Let ${falkolib_ROOT} be the install directory of your local copy 64 | of library falkolib. 65 | The following standard commands are required to compile it: 66 | 67 | cd ${falkolib_ROOT} 68 | mkdir build 69 | cd build 70 | cmake .. 71 | make 72 | 73 | You can also install the library into a system directory. 74 | To change the install directory you must set cmake environment 75 | variable ${CMAKE_INSTALL_PREFIX} (e.g. using command "ccmake .." 76 | before calling "cmake .."). 77 | Its default value on UNIX-like/Linux systems is "/usr/local". 78 | After compiling library falkolib, run the command: 79 | 80 | sudo make install 81 | 82 | The command "sudo" is required only if ${CMAKE_INSTALL_PREFIX} 83 | is a system diretory managed by administrator user root. 84 | Such command copies: 85 | - header files of ${falkolib_ROOT}/include/falkolib to 86 | ${CMAKE_INSTALL_PREFIX}/include/falkolib/ 87 | - library files ${falkolib_ROOT}/lib/libfalkolib.a to 88 | ${CMAKE_INSTALL_PREFIX}/lib/ 89 | - cmake script ${falkolib_ROOT}/cmake_modules/falkolibConfig.cmake to 90 | ${CMAKE_INSTALL_PREFIX}/share/falkolib/ 91 | 92 | 93 | HOW TO USE LIBRARY falkolib IN YOUR PROJECT 94 | ------------------------------------------------- 95 | 96 | If library falkolib has been installed in system directory "/usr/local", 97 | then it is straighforward to use it in your projects. 98 | You needs to add the following lines to your project as in this example: 99 | 100 | 101 | CMAKE_MINIMUM_REQUIRED(VERSION 2.8) 102 | PROJECT(foobar) 103 | 104 | find_package(falkolib REQUIRED) 105 | message(STATUS "falkolib_FOUND ${falkolib_FOUND}") 106 | message(STATUS "falkolib_INCLUDE_DIRS ${falkolib_INCLUDE_DIRS}") 107 | message(STATUS "falkolib_LIBRARY_DIRS ${falkolib_LIBRARY_DIRS}") 108 | message(STATUS "falkolib_LIBRARIES ${falkolib_LIBRARIES}") 109 | 110 | if(${falkolib_FOUND}) 111 | include_directories(${falkolib_INCLUDE_DIRS}) 112 | link_directories(${falkolib_LIBRARY_DIRS}) 113 | endif() 114 | 115 | add_executable(foobar foobar.cpp) 116 | target_link_libraries(foobar ${falkolib_LIBRARIES}) 117 | 118 | The above example uses the variables defined in falkolibConfig.cmake: 119 | 120 | falkolib_FOUND - system has falkolib module 121 | falkolib_INCLUDE_DIRS - the falkolib include directories 122 | falkolib_LIBRARY_DIRS - the falkolib library directories 123 | falkolib_LIBRARIES - link these to use falkolib 124 | 125 | 126 | -------------------------------------------------------------------------------- /cmake_modules/FindEigen3.cmake: -------------------------------------------------------------------------------- 1 | # - Try to find Eigen3 lib 2 | # 3 | # This module supports requiring a minimum version, e.g. you can do 4 | # find_package(Eigen3 3.1.2) 5 | # to require version 3.1.2 or newer of Eigen3. 6 | # 7 | # Once done this will define 8 | # 9 | # EIGEN3_FOUND - system has eigen lib with correct version 10 | # EIGEN3_INCLUDE_DIR - the eigen include directory 11 | # EIGEN3_VERSION - eigen version 12 | 13 | # Copyright (c) 2006, 2007 Montel Laurent, 14 | # Copyright (c) 2008, 2009 Gael Guennebaud, 15 | # Copyright (c) 2009 Benoit Jacob 16 | # Redistribution and use is allowed according to the terms of the 2-clause BSD license. 17 | 18 | if(NOT Eigen3_FIND_VERSION) 19 | if(NOT Eigen3_FIND_VERSION_MAJOR) 20 | set(Eigen3_FIND_VERSION_MAJOR 2) 21 | endif(NOT Eigen3_FIND_VERSION_MAJOR) 22 | if(NOT Eigen3_FIND_VERSION_MINOR) 23 | set(Eigen3_FIND_VERSION_MINOR 91) 24 | endif(NOT Eigen3_FIND_VERSION_MINOR) 25 | if(NOT Eigen3_FIND_VERSION_PATCH) 26 | set(Eigen3_FIND_VERSION_PATCH 0) 27 | endif(NOT Eigen3_FIND_VERSION_PATCH) 28 | 29 | set(Eigen3_FIND_VERSION "${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}") 30 | endif(NOT Eigen3_FIND_VERSION) 31 | 32 | macro(_eigen3_check_version) 33 | file(READ "${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header) 34 | 35 | string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}") 36 | set(EIGEN3_WORLD_VERSION "${CMAKE_MATCH_1}") 37 | string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}") 38 | set(EIGEN3_MAJOR_VERSION "${CMAKE_MATCH_1}") 39 | string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}") 40 | set(EIGEN3_MINOR_VERSION "${CMAKE_MATCH_1}") 41 | 42 | set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION}) 43 | if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 44 | set(EIGEN3_VERSION_OK FALSE) 45 | else(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 46 | set(EIGEN3_VERSION_OK TRUE) 47 | endif(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 48 | 49 | if(NOT EIGEN3_VERSION_OK) 50 | 51 | message(STATUS "Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, " 52 | "but at least version ${Eigen3_FIND_VERSION} is required") 53 | endif(NOT EIGEN3_VERSION_OK) 54 | endmacro(_eigen3_check_version) 55 | 56 | if (EIGEN3_INCLUDE_DIR) 57 | 58 | # in cache already 59 | _eigen3_check_version() 60 | set(EIGEN3_FOUND ${EIGEN3_VERSION_OK}) 61 | 62 | else (EIGEN3_INCLUDE_DIR) 63 | 64 | # specific additional paths for some OS 65 | if (WIN32) 66 | set(EIGEN_ADDITIONAL_SEARCH_PATHS ${EIGEN_ADDITIONAL_SEARCH_PATHS} "C:/Program Files/Eigen/include" "C:/Program Files (x86)/Eigen/include") 67 | endif(WIN32) 68 | 69 | find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library 70 | PATHS 71 | ${CMAKE_INSTALL_PREFIX}/include 72 | ${EIGEN_ADDITIONAL_SEARCH_PATHS} 73 | ${KDE4_INCLUDE_DIR} 74 | PATH_SUFFIXES eigen3 eigen 75 | ) 76 | 77 | if(EIGEN3_INCLUDE_DIR) 78 | _eigen3_check_version() 79 | endif(EIGEN3_INCLUDE_DIR) 80 | 81 | include(FindPackageHandleStandardArgs) 82 | find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK) 83 | 84 | mark_as_advanced(EIGEN3_INCLUDE_DIR) 85 | 86 | endif(EIGEN3_INCLUDE_DIR) 87 | 88 | -------------------------------------------------------------------------------- /cmake_modules/falkolibConfig.cmake: -------------------------------------------------------------------------------- 1 | # - Try to find Library falkolib 2 | # Once done, this will define 3 | # 4 | # falkolib_FOUND - system has falkolib module 5 | # falkolib_INCLUDE_DIRS - the falkolib include directories 6 | # falkolib_LIBRARY_DIRS - the falkolib library directories 7 | # falkolib_LIBRARIES - link these to use falkolib 8 | 9 | 10 | # Uses directory to search mrf_segmentation directory! 11 | set(falkolib_PREFIX_DIR /usr/local) 12 | message(STATUS "Searching falkolib in directory ${falkolib_PREFIX_DIR}." ) 13 | 14 | # Searches include directory /usr/local/include/falkolib 15 | find_path(falkolib_INCLUDE_DIR falkolib ${falkolib_PREFIX_DIR}/include) 16 | message(STATUS " falkolib_INCLUDE_DIR ${falkolib_INCLUDE_DIR}." ) 17 | set(falkolib_INCLUDE_DIRS ${falkolib_INCLUDE_DIR}) 18 | 19 | # Searches library librimagraph.a in /usr/local/lib 20 | find_path(falkolib_LIBRARY_DIR librimagraph.a ${falkolib_PREFIX_DIR}/lib) 21 | message(STATUS " falkolib_LIBRARY_DIR ${falkolib_LIBRARY_DIR}." ) 22 | set(falkolib_LIBRARY_DIRS ${falkolib_PREFIX_DIR}/lib) 23 | 24 | # Sets the names of library components (actually A name and A component) 25 | find_library(falkolib_LIBRARY falkolib ${falkolib_LIBRARY_DIRS}) 26 | message(STATUS " falkolib_LIBRARY ${falkolib_LIBRARY}." ) 27 | set(falkolib_LIBRARIES ${falkolib_LIBRARY}) 28 | 29 | if(("${falkolib_INCLUDE_DIR}" STREQUAL "falkolib_INCLUDE_DIR-NOTFOUND") OR 30 | ("${falkolib_LIBRARY_DIRS}" STREQUAL "falkolib_LIBRARY_DIRS-NOTFOUND") OR 31 | ("${falkolib_LIBRARY}" STREQUAL "falkolib_LIBRARY-NOTFOUND") 32 | ) 33 | message(STATUS "Library falkolib NOT found") 34 | unset(falkolib_FOUND) 35 | unset(falkolib_INCLUDE_DIR) 36 | unset(falkolib_LIBRARY_DIR) 37 | unset(falkolib_LIBRARY) 38 | unset(falkolib_LIBRARIES) 39 | endif() 40 | 41 | mark_as_advanced(falkolib_INCLUDE_DIRS falkolib_LIBRARY_DIRS falkolib_LIBRARIES) 42 | -------------------------------------------------------------------------------- /imgs/falko-bsc.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OpenSLAM-org/openslam_falkolib/ade778744622862d29d77d817d59c184cf9bffff/imgs/falko-bsc.png -------------------------------------------------------------------------------- /imgs/falko-score.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OpenSLAM-org/openslam_falkolib/ade778744622862d29d77d817d59c184cf9bffff/imgs/falko-score.png -------------------------------------------------------------------------------- /imgs/intel-lab-falko-map.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OpenSLAM-org/openslam_falkolib/ade778744622862d29d77d817d59c184cf9bffff/imgs/intel-lab-falko-map.png -------------------------------------------------------------------------------- /include/falkolib/Common/GeomUtils.h: -------------------------------------------------------------------------------- 1 | /** 2 | * FALKOLib - Fast Adaptive Laser Keypoint Orientation-invariant 3 | * Copyright (C) 2016 Fabjan Kallasi and Dario Lodi Rizzini. 4 | * 5 | * This file is part of FALKOLib. 6 | * 7 | * FALKOLib is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU Lesser General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * FALKOLib is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU Lesser General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU Lesser General Public License 18 | * along with FALKOLib. If not, see . 19 | */ 20 | #pragma once 21 | 22 | /** 23 | * @brief Useful geometric functions 24 | */ 25 | 26 | #include 27 | 28 | #include 29 | 30 | namespace falkolib{ 31 | 32 | /** 33 | * @brief point-to-point distance 34 | * @param p1 point 1 35 | * @param p2 point 2 36 | * @return distance between p1 and p2 37 | */ 38 | template 39 | double pointsDistance(const T& p1, const T& p2){ 40 | return (p2 -p1).norm(); 41 | } 42 | 43 | /** 44 | * @brief angle between two given points 45 | * @param p1 point 1 46 | * @param p2 point 2 47 | * @return angle between p1 and p2 [rad] 48 | */ 49 | template 50 | double angleBetweenPoints(const T& p1, const T& p2) { 51 | double angle = atan2(p2[1] - p1[1], p2[0] - p1[0]); 52 | //numeric problems.......... 53 | if (angle >= M_PI) return M_PI - 0.000001; 54 | if (angle <= -M_PI) return -M_PI + 0.000001; 55 | return angle; 56 | } 57 | 58 | /** 59 | * @brief This function computes the inner area of a triangle defined by 3 vertices 60 | * @param p0 triangle first vertex 61 | * @param p1 triangle second vertex 62 | * @param p2 triangle third vertex 63 | * @return signed area of the inner triangle, p0-p1 as base of the triangle 64 | */ 65 | template 66 | double signedTriangleArea(const T& p0, const T& p1, const T& p2) { 67 | return ((p2[1] - p1[1]) * p0[0] - (p2[0] - p1[0]) * p0[1] + p2[0] * p1[1] - p2[1] * p1[0]); 68 | } 69 | 70 | /** 71 | * @brief Compute Affine transform between two points sets using least square regression 72 | * @param v1 first points set 73 | * @param v2 second points set 74 | * @param indices matching vectors, pair.first corresponds to v1 and pair.second corresponds to v2 75 | * @param transform resulting affine transform which move v2 in v1 frame. 76 | * @return if false, the resulting transform is invalid 77 | */ 78 | template 79 | bool computeTransform(const std::vector& v1, const std::vector& v2, const std::vector >& indices, Eigen::Affine2d& transform) { 80 | 81 | Eigen::Vector2d t1 = Eigen::Vector2d::Zero(); 82 | Eigen::Vector2d t2 = Eigen::Vector2d::Zero(); 83 | Eigen::Matrix2d S = Eigen::Matrix2d::Zero(); 84 | int n = 0; 85 | for (int i = 0; i < (int) indices.size(); ++i) { 86 | if (0 <= indices[i].first && indices[i].first < (int) v1.size() && 87 | 0 <= indices[i].second && indices[i].second < (int) v2.size()) { 88 | t1 += v1[indices[i].first].point; 89 | t2 += v2[indices[i].second].point; 90 | n++; 91 | } 92 | } 93 | if (n == 0) { 94 | return false; 95 | } 96 | t1 = (1.0 / n) * t1; 97 | t2 = (1.0 / n) * t2; 98 | for (int i = 0; i < (int) indices.size(); ++i) { 99 | if (0 <= indices[i].first && indices[i].first < (int) v1.size() && 100 | 0 <= indices[i].second && indices[i].second < (int) v2.size()) { 101 | S += (v2[indices[i].second].point - t2) * ((v1[indices[i].first].point - t1).transpose()); 102 | } 103 | } 104 | double theta = std::atan2(S(0, 1) - S(1, 0), S(0, 0) + S(1, 1)); 105 | Eigen::Rotation2Dd rot(theta); 106 | Eigen::Vector2d transl = t1 - (rot * t2); 107 | transform = Eigen::Affine2d::Identity(); 108 | transform.prerotate(rot); 109 | transform.pretranslate(transl); 110 | 111 | return true; 112 | } 113 | 114 | } -------------------------------------------------------------------------------- /include/falkolib/Common/HoughSpectrum.h: -------------------------------------------------------------------------------- 1 | /** 2 | * FALKOLib - Fast Adaptive Laser Keypoint Orientation-invariant 3 | * Copyright (C) 2016 Fabjan Kallasi and Dario Lodi Rizzini. 4 | * 5 | * This file is part of FALKOLib. 6 | * 7 | * FALKOLib is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU Lesser General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * FALKOLib is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU Lesser General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU Lesser General Public License 18 | * along with FALKOLib. If not, see . 19 | */ 20 | #pragma once 21 | 22 | #include 23 | #include 24 | 25 | namespace falkolib { 26 | 27 | /** Hough transform and spectrum. 28 | */ 29 | class HoughSpectrum { 30 | public: 31 | /** Constructor with deafult parameters. 32 | */ 33 | HoughSpectrum(); 34 | 35 | /** Constructor with the number of theta. 36 | */ 37 | HoughSpectrum(double thetaStep, double rhoStep, double rhoMax); 38 | 39 | /** Inits params. 40 | */ 41 | void init(double thetaStep, double rhoStep, double rhoMax); 42 | 43 | /** Inserts the points and computes Hough Transform and Spectrum. 44 | */ 45 | template 46 | void insertPoint(It pbeg, It pend); 47 | 48 | /** Returns the Hough Transform. 49 | */ 50 | const Eigen::MatrixXd& hough() const { 51 | return hough_; 52 | } 53 | 54 | /** Returns the value of Hough Transform for a specific value of theta and rho. 55 | * If the theta and rho are not in the domain, then it return 0.0. 56 | */ 57 | double hough(double theta, double rho) const { 58 | int ith = thetaToIdx(theta); 59 | int irh = rhoToIdx(theta); 60 | if (0 <= ith && ith < hough_.rows() && 0 <= irh && irh < hough_.cols()) { 61 | return hough_(ith, irh); 62 | } 63 | return 0.0; 64 | } 65 | 66 | /** Returns the spectrum. 67 | */ 68 | const Eigen::VectorXd& spectrum() const { 69 | return spectrum_; 70 | } 71 | 72 | /** Returns the spectrum. 73 | */ 74 | const double spectrum(double theta) const { 75 | int ith = thetaToIdx(theta); 76 | if (0 <= ith && ith < hough_.rows()) { 77 | return spectrum_(ith); 78 | } 79 | return 0.0; 80 | } 81 | 82 | /** Returns the spectrum. 83 | */ 84 | const Eigen::VectorXd& orthoSpectrum() const { 85 | return orthoSpectrum_; 86 | } 87 | 88 | private: 89 | int thetaNum_; 90 | int rhoNum_; 91 | double thetaStep_; 92 | double rhoStep_; 93 | // Hough transform and spectra should be integer types. 94 | // However, since their value may be very large, double type is used instead. 95 | Eigen::MatrixXd hough_; 96 | Eigen::VectorXd spectrum_; 97 | Eigen::VectorXd orthoSpectrum_; 98 | Eigen::VectorXd cosLut_; 99 | Eigen::VectorXd sinLut_; 100 | 101 | double idxToRho(int idx) const { 102 | return (rhoStep_ * (idx - rhoNum_)); 103 | } 104 | 105 | int rhoToIdx(double rho) const { 106 | return ((int) round(rho / rhoStep_) + rhoNum_ / 2); 107 | } 108 | 109 | int thetaToIdx(double theta) const { 110 | int idx = (int) round(theta / thetaStep_); 111 | int thetaNum2 = 2 * thetaNum_; 112 | idx = ((idx % thetaNum2) + thetaNum2) % thetaNum2; 113 | return idx; 114 | } 115 | }; 116 | 117 | // ------------------------------------------------------------- 118 | // TEMPLATE METHOD 119 | // ------------------------------------------------------------- 120 | 121 | template 122 | void HoughSpectrum::insertPoint(It pbeg, It pend) { 123 | double rho; 124 | int irho; 125 | 126 | hough_.fill(0.0); 127 | spectrum_.fill(0.0); 128 | // Computes Hough 129 | for (It pit = pbeg; pit != pend; ++pit) { 130 | for (int i = 0; i < thetaNum_; ++i) { 131 | rho = pit->x() * cosLut_(i) + pit->y() * sinLut_(i); 132 | irho = rhoToIdx(rho); 133 | if (0 <= irho && irho < rhoNum_) { 134 | hough_(i, irho) = hough_(i, irho) + 1; 135 | } 136 | // else { 137 | // std::cerr << "Out-of-bound: rho " << rho << ", theta " << (180.0 / M_PI * thetaStep_ * i) << ", " 138 | // << "point [" << pit->x() << " " << pit->y() << "]\n"; 139 | // } 140 | } 141 | } 142 | spectrum_ = (hough_.array() * hough_.array()).rowwise().sum(); 143 | orthoSpectrum_ = spectrum_.segment(0, thetaNum_ / 2) + spectrum_.segment(thetaNum_ / 2, thetaNum_ / 2); 144 | } 145 | 146 | } // end of namespace 147 | 148 | -------------------------------------------------------------------------------- /include/falkolib/Common/LaserScan.h: -------------------------------------------------------------------------------- 1 | /** 2 | * FALKOLib - Fast Adaptive Laser Keypoint Orientation-invariant 3 | * Copyright (C) 2016 Fabjan Kallasi and Dario Lodi Rizzini. 4 | * 5 | * This file is part of FALKOLib. 6 | * 7 | * FALKOLib is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU Lesser General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * FALKOLib is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU Lesser General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU Lesser General Public License 18 | * along with FALKOLib. If not, see . 19 | */ 20 | #pragma once 21 | 22 | #include 23 | #include 24 | #include 25 | 26 | namespace falkolib { 27 | 28 | /** 29 | * @brief Laser scan container 30 | * 31 | * This class provides an essential interface for laser scan data 32 | */ 33 | class LaserScan { 34 | public: 35 | 36 | /** 37 | * @brief Constructor 38 | */ 39 | LaserScan() { 40 | angleMin = 0; 41 | fov = 0; 42 | angleInc = 0; 43 | numBeams = 0; 44 | timestamp = 0; 45 | } 46 | 47 | /** 48 | * @brief Constructor 49 | * @param _angleMin laser scanner start angle [rad] 50 | * @param _fov laser scanner field of view [rad] 51 | * @param _numBeams laser scanner number of beams 52 | */ 53 | LaserScan(double _angleMin, double _fov, int _numBeams) { 54 | angleMin = _angleMin; 55 | fov = _fov; 56 | angleInc = _fov / _numBeams; 57 | numBeams = _numBeams; 58 | timestamp = 0; 59 | } 60 | 61 | /** @brief Set laser scanner start angle [rad] */ 62 | inline void setAngleMin(double _angleMin) { 63 | angleMin = _angleMin; 64 | }; 65 | 66 | /** @brief Set laser scanner field of view [rad] */ 67 | inline void setLaserFoV(double _fov) { 68 | fov = _fov; 69 | }; 70 | 71 | /** @brief Set laser scanner angle increment [rad] */ 72 | inline void setAngleInc(double _angleInc) { 73 | angleInc = _angleInc; 74 | }; 75 | 76 | /** @brief Set laser scanner number of beams */ 77 | inline void setNumBeams(int _numBeams) { 78 | numBeams = _numBeams; 79 | }; 80 | 81 | /** @brief Set scan beginning timestamp [s] */ 82 | inline void setTimestamp(double _timestamp) { 83 | timestamp = _timestamp; 84 | }; 85 | 86 | /** @brief Get laser scanner number of beams */ 87 | inline int getNumBeams() const { 88 | return numBeams; 89 | }; 90 | 91 | /** @brief Get laser scanner angle increment [rad] */ 92 | inline double getAngleInc() const { 93 | return angleInc; 94 | }; 95 | 96 | /** 97 | * @brief Compute scan points from ranges 98 | * 99 | * @param _ranges plain array of double representing the scan ranges 100 | */ 101 | inline void fromRanges(const double* _ranges) { 102 | fromRanges(std::vector(_ranges, _ranges + numBeams)); 103 | } 104 | 105 | /** 106 | * @brief Compute scan points from ranges 107 | * 108 | * @param _ranges std::vector of double representing the scan ranges 109 | */ 110 | inline void fromRanges(const std::vector& _ranges) { 111 | double theta; 112 | ranges = _ranges; 113 | points.resize(numBeams); 114 | for (int i = 0; i < numBeams; ++i) { 115 | theta = i * angleInc + angleMin; 116 | points[i][0] = ranges[i] * std::cos(theta); 117 | points[i][1] = ranges[i] * std::sin(theta); 118 | } 119 | } 120 | 121 | /** 122 | * @brief compute neighborhood points list given a single point index and a search radius 123 | * @param candIndex index of the central point 124 | * @param radius search euclidean radius [m] 125 | * @param neigh vector of the neighborhood points 126 | * @param midIndex index representing the central point in the neigh vector 127 | */ 128 | void getNeighPoints(int candIndex, double radius, std::vector& neigh, int& midIndex) const { 129 | const Point2d& candPoint = points[candIndex]; 130 | int alpha = std::floor(std::asin(radius / ranges[candIndex]) / angleInc); 131 | int begIndex = std::max(0, candIndex - alpha); 132 | int endIndex = std::min(candIndex + alpha + 1, numBeams); 133 | for (int i = begIndex; i <= endIndex; ++i) { 134 | if (pointsDistance(points[i], candPoint) <= radius) { 135 | if (i == candIndex) { 136 | midIndex = neigh.size(); 137 | } 138 | neigh.push_back(points[i]); 139 | } 140 | } 141 | } 142 | 143 | 144 | std::vector ranges; 145 | std::vector points; 146 | 147 | private: 148 | 149 | double angleMin; 150 | double fov; 151 | double angleInc; 152 | int numBeams; 153 | double timestamp; 154 | }; 155 | } -------------------------------------------------------------------------------- /include/falkolib/Common/Point.h: -------------------------------------------------------------------------------- 1 | /** 2 | * FALKOLib - Fast Adaptive Laser Keypoint Orientation-invariant 3 | * Copyright (C) 2016 Fabjan Kallasi and Dario Lodi Rizzini. 4 | * 5 | * This file is part of FALKOLib. 6 | * 7 | * FALKOLib is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU Lesser General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * FALKOLib is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU Lesser General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU Lesser General Public License 18 | * along with FALKOLib. If not, see . 19 | */ 20 | #pragma once 21 | 22 | #include 23 | 24 | namespace falkolib { 25 | typedef Eigen::Vector2d Point2d; 26 | typedef Eigen::Vector2f Point2f; 27 | typedef Eigen::Vector2i Point2i; 28 | } -------------------------------------------------------------------------------- /include/falkolib/Feature/BSC.h: -------------------------------------------------------------------------------- 1 | /** 2 | * FALKOLib - Fast Adaptive Laser Keypoint Orientation-invariant 3 | * Copyright (C) 2016 Fabjan Kallasi and Dario Lodi Rizzini. 4 | * 5 | * This file is part of FALKOLib. 6 | * 7 | * FALKOLib is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU Lesser General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * FALKOLib is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU Lesser General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU Lesser General Public License 18 | * along with FALKOLib. If not, see . 19 | */ 20 | #pragma once 21 | 22 | #include 23 | #include 24 | #include 25 | 26 | namespace falkolib { 27 | 28 | /** 29 | * @brief Binary Shape Context descriptor 30 | * 31 | * This class represents a BSC Descriptor. 32 | */ 33 | class BSC : public Descriptor { 34 | public: 35 | 36 | /** 37 | * @brief Constructor 38 | * @param _radius descriptor max radius 39 | * @param _circularSectorNumber number of circular sectors 40 | * @param _radialRingNumber number of radial rings 41 | * 42 | * Set the grid dimension and cells resolutions 43 | */ 44 | BSC(double _radius, int _circularSectorNumber, int _radialRingNumber); 45 | 46 | /** 47 | * @brief Compute distance between two descriptors 48 | * @param desc descriptor to measure distance 49 | * @return the distance between *this and desc 50 | * 51 | * Compute the distance between two descriptors of the same type (BSC) 52 | */ 53 | double distance(const Descriptor& desc) const; 54 | 55 | /** 56 | * @brief Rotate the descriptor grid 57 | * @param theta angle of rotation [rad] 58 | * 59 | * Rotate the descriptor grid of a number of circular sector based on theta 60 | */ 61 | void rotate(double theta); 62 | 63 | /** 64 | * @brief Compute the grid descriptor 65 | * @param neigh vector of neighborhood points 66 | * @param centralPointIndex index of the central point in the neigh vector 67 | */ 68 | void compute(std::vector& neigh, int centralPointIndex); 69 | 70 | private: 71 | std::vector > grid; 72 | int circularSectorNumber; 73 | int radialRingNumber; 74 | double sectorResolution; 75 | double ringResolution; 76 | double radius; 77 | 78 | /** @brief compute the Hamming distance between two binary grid*/ 79 | double HammingDistance(const std::vector >& g1, const std::vector >& g2) const; 80 | }; 81 | } -------------------------------------------------------------------------------- /include/falkolib/Feature/BSCExtractor.h: -------------------------------------------------------------------------------- 1 | /** 2 | * FALKOLib - Fast Adaptive Laser Keypoint Orientation-invariant 3 | * Copyright (C) 2016 Fabjan Kallasi and Dario Lodi Rizzini. 4 | * 5 | * This file is part of FALKOLib. 6 | * 7 | * FALKOLib is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU Lesser General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * FALKOLib is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU Lesser General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU Lesser General Public License 18 | * along with FALKOLib. If not, see . 19 | */ 20 | #pragma once 21 | 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | namespace falkolib { 28 | 29 | /** 30 | * @brief class representing a BSC descriptor extractor engine 31 | */ 32 | template 33 | class BSCExtractor : public DescriptorExtractor { 34 | public: 35 | 36 | /** 37 | * @brief Constructor 38 | * @param _circularSectorNumber number of grid circular sector 39 | * @param _radialRingNumber number of grid radial number 40 | * @param _useKeypointRadius if true, the selected neighborhood points search radius is keypoint one 41 | * @param _radius neighborhood points search radius 42 | */ 43 | BSCExtractor(int _circularSectorNumber, int _radialRingNumber, bool _useKeypointRadius = true, double _radius = 0.1) { 44 | circularSectorNumber = _circularSectorNumber; 45 | radialRingNumber = _radialRingNumber; 46 | useKeypointRadius = _useKeypointRadius; 47 | radius = _radius; 48 | }; 49 | 50 | /** 51 | * @brief Extract BSC descriptor from a given scan and a list of keypoints 52 | * @param scan input laser scan 53 | * @param keypoints keypoints list 54 | * @param descriptor extracted from scan and keypoints 55 | */ 56 | void compute(const LaserScan& scan, const std::vector& keypoints, std::vector& descriptors) { 57 | descriptors.reserve(keypoints.size()); 58 | for (int i = 0; i < keypoints.size(); ++i) { 59 | std::vector neigh; 60 | int midIndex; 61 | if (useKeypointRadius) { 62 | scan.getNeighPoints(keypoints[i].index, keypoints[i].radius, neigh, midIndex); 63 | BSC desc(keypoints[i].radius, circularSectorNumber, radialRingNumber); 64 | desc.compute(neigh, midIndex); 65 | descriptors.push_back(std::move(desc)); 66 | } else { 67 | scan.getNeighPoints(keypoints[i].index, radius, neigh, midIndex); 68 | BSC desc(radius, circularSectorNumber, radialRingNumber); 69 | desc.compute(neigh, midIndex); 70 | descriptors.push_back(std::move(desc)); 71 | } 72 | } 73 | }; 74 | 75 | private: 76 | int circularSectorNumber; 77 | int radialRingNumber; 78 | bool useKeypointRadius; 79 | double radius; 80 | }; 81 | } -------------------------------------------------------------------------------- /include/falkolib/Feature/CGH.h: -------------------------------------------------------------------------------- 1 | /** 2 | * FALKOLib - Fast Adaptive Laser Keypoint Orientation-invariant 3 | * Copyright (C) 2016 Fabjan Kallasi and Dario Lodi Rizzini. 4 | * 5 | * This file is part of FALKOLib. 6 | * 7 | * FALKOLib is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU Lesser General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * FALKOLib is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU Lesser General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU Lesser General Public License 18 | * along with FALKOLib. If not, see . 19 | */ 20 | #pragma once 21 | 22 | #include 23 | #include 24 | #include 25 | 26 | namespace falkolib { 27 | 28 | /** 29 | * @brief Cumulative Gaussian Histogram descriptor 30 | * 31 | * This class represents a CGH Descriptor. 32 | */ 33 | class CGH : public Descriptor { 34 | public: 35 | 36 | /** 37 | * @brief Constructor 38 | * @param _radius descriptor max radius 39 | * @param _circularSectorNumber number of histogram bins 40 | * 41 | * Set the histogram dimension and the circular sectors resolutions 42 | */ 43 | CGH(double _radius, int _circularSectorNumber); 44 | 45 | /** 46 | * @brief Compute distance between two descriptors 47 | * @param desc descriptor to measure distance 48 | * @return the distance between *this and desc 49 | * 50 | * Compute the distance between two descriptors of the same type (CGH) 51 | */ 52 | double distance(const Descriptor& desc) const; 53 | 54 | /** 55 | * @brief Rotate the descriptor grid 56 | * @param theta angle of rotation [rad] 57 | * 58 | * Rotate the descriptor grid of a number of circular sector based on theta 59 | */ 60 | void rotate(double theta); 61 | 62 | /** 63 | * @brief Compute the histogram descriptor 64 | * @param neigh vector of neighborhood points 65 | * @param centralPointIndex index of the central point in the neigh vector 66 | */ 67 | void compute(std::vector& neigh, int centralPointIndex); 68 | 69 | private: 70 | std::vector histogram; 71 | int circularSectorNumber; 72 | double sectorResolution; 73 | double radius; 74 | 75 | /** @brief compute the Chi-squared distance between two histograms*/ 76 | double SymmetricChiSquaredDistance(const std::vector& h1, const std::vector& h2) const; 77 | }; 78 | } -------------------------------------------------------------------------------- /include/falkolib/Feature/CGHExtractor.h: -------------------------------------------------------------------------------- 1 | /** 2 | * FALKOLib - Fast Adaptive Laser Keypoint Orientation-invariant 3 | * Copyright (C) 2016 Fabjan Kallasi and Dario Lodi Rizzini. 4 | * 5 | * This file is part of FALKOLib. 6 | * 7 | * FALKOLib is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU Lesser General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * FALKOLib is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU Lesser General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU Lesser General Public License 18 | * along with FALKOLib. If not, see . 19 | */ 20 | #pragma once 21 | 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | namespace falkolib { 28 | 29 | /** 30 | * @brief class representing a CGH descriptor extractor engine 31 | */ 32 | template 33 | class CGHExtractor : public DescriptorExtractor { 34 | public: 35 | 36 | /** 37 | * @brief Constructor 38 | * @param _circularSectorNumber number of grid circular sector 39 | * @param _useKeypointRadius if true, the selected neighborhood points search radius is keypoint one 40 | * @param _radius neighborhood points search radius 41 | */ 42 | CGHExtractor(int _circularSectorNumber, bool _useKeypointRadius = true, double _radius = 0.1) { 43 | circularSectorNumber = _circularSectorNumber; 44 | useKeypointRadius = _useKeypointRadius; 45 | radius = _radius; 46 | }; 47 | 48 | /** 49 | * @brief Extract CGH descriptor from a given scan and a list of keypoints 50 | * @param scan input laser scan 51 | * @param keypoints keypoints list 52 | * @param descriptor extracted from scan and keypoints 53 | */ 54 | void compute(const LaserScan& scan, const std::vector& keypoints, std::vector& descriptors) { 55 | descriptors.reserve(keypoints.size()); 56 | for (int i = 0; i < keypoints.size(); ++i) { 57 | std::vector neigh; 58 | int midIndex; 59 | if (useKeypointRadius) { 60 | scan.getNeighPoints(keypoints[i].index, keypoints[i].radius, neigh, midIndex); 61 | CGH desc(keypoints[i].radius, circularSectorNumber); 62 | desc.compute(neigh, midIndex); 63 | descriptors.push_back(std::move(desc)); 64 | } else { 65 | scan.getNeighPoints(keypoints[i].index, radius, neigh, midIndex); 66 | CGH desc(radius, circularSectorNumber); 67 | desc.compute(neigh, midIndex); 68 | descriptors.push_back(std::move(desc)); 69 | } 70 | } 71 | }; 72 | 73 | private: 74 | int circularSectorNumber; 75 | bool useKeypointRadius; 76 | double radius; 77 | }; 78 | } -------------------------------------------------------------------------------- /include/falkolib/Feature/Descriptor.h: -------------------------------------------------------------------------------- 1 | /** 2 | * FALKOLib - Fast Adaptive Laser Keypoint Orientation-invariant 3 | * Copyright (C) 2016 Fabjan Kallasi and Dario Lodi Rizzini. 4 | * 5 | * This file is part of FALKOLib. 6 | * 7 | * FALKOLib is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU Lesser General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * FALKOLib is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU Lesser General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU Lesser General Public License 18 | * along with FALKOLib. If not, see . 19 | */ 20 | #pragma once 21 | 22 | namespace falkolib { 23 | 24 | /** 25 | * @brief class representing a generic descriptor 26 | * 27 | * No properties are defined for a generic descriptor 28 | */ 29 | 30 | class Descriptor { 31 | protected: 32 | 33 | /** 34 | * @brief Compute distance between two descriptors 35 | * @param desc descriptor to measure distance 36 | * @return the distance between *this and desc 37 | */ 38 | virtual double distance(const Descriptor& desc) const = 0; 39 | }; 40 | } -------------------------------------------------------------------------------- /include/falkolib/Feature/DescriptorExtractor.h: -------------------------------------------------------------------------------- 1 | /** 2 | * FALKOLib - Fast Adaptive Laser Keypoint Orientation-invariant 3 | * Copyright (C) 2016 Fabjan Kallasi and Dario Lodi Rizzini. 4 | * 5 | * This file is part of FALKOLib. 6 | * 7 | * FALKOLib is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU Lesser General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * FALKOLib is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU Lesser General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU Lesser General Public License 18 | * along with FALKOLib. If not, see . 19 | */ 20 | #pragma once 21 | 22 | #include 23 | #include 24 | 25 | namespace falkolib { 26 | 27 | /** 28 | * @brief class representing a descriptor extractor engine 29 | */ 30 | template 31 | class DescriptorExtractor { 32 | protected: 33 | /** 34 | * @brief Extract descriptor from a given scan and a list of keypoints 35 | * @param scan input laser scan 36 | * @param keypoints keypoints list 37 | * @param descriptor extracted from scan and keypoints 38 | */ 39 | virtual void compute(const LaserScan& scan, const std::vector& keypoints, std::vector& descriptors) = 0; 40 | }; 41 | } -------------------------------------------------------------------------------- /include/falkolib/Feature/FALKO.h: -------------------------------------------------------------------------------- 1 | /** 2 | * FALKOLib - Fast Adaptive Laser Keypoint Orientation-invariant 3 | * Copyright (C) 2016 Fabjan Kallasi and Dario Lodi Rizzini. 4 | * 5 | * This file is part of FALKOLib. 6 | * 7 | * FALKOLib is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU Lesser General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * FALKOLib is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU Lesser General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU Lesser General Public License 18 | * along with FALKOLib. If not, see . 19 | */ 20 | #pragma once 21 | 22 | #include 23 | 24 | namespace falkolib{ 25 | 26 | /** 27 | * @brief class representing a FALKO keypoint 28 | * 29 | * FALKO keypoint extends simple keypoint interface. 30 | * The index in the original scan, corner orientation and neighborhood search radius are also preserved. 31 | */ 32 | class FALKO : public Keypoint{ 33 | public: 34 | int index; 35 | double radius; 36 | double orientation; 37 | }; 38 | } -------------------------------------------------------------------------------- /include/falkolib/Feature/FALKOExtractor.h: -------------------------------------------------------------------------------- 1 | /** 2 | * FALKOLib - Fast Adaptive Laser Keypoint Orientation-invariant 3 | * Copyright (C) 2016 Fabjan Kallasi and Dario Lodi Rizzini. 4 | * 5 | * This file is part of FALKOLib. 6 | * 7 | * FALKOLib is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU Lesser General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * FALKOLib is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU Lesser General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU Lesser General Public License 18 | * along with FALKOLib. If not, see . 19 | */ 20 | #pragma once 21 | 22 | #include 23 | #include 24 | #include 25 | 26 | namespace falkolib { 27 | 28 | /** 29 | * @brief class representing a FALKO keypoint extractor engine 30 | */ 31 | class FALKOExtractor : public KeypointExtractor { 32 | public: 33 | 34 | /** 35 | * @brief Constructor 36 | */ 37 | FALKOExtractor(); 38 | 39 | /** 40 | * @brief Extract FALKO keypoints from a given scan 41 | * @param scan input laser scan 42 | * @param features FALKO keypoint extracted from given scan 43 | */ 44 | void extract(const LaserScan& scan, std::vector& keypoints); 45 | 46 | /** @brief Set minimum score threshold for a candidate keypoint [%]*/ 47 | inline void setMinScoreTh(double _minScoreTh) { 48 | minScoreTh = _minScoreTh; 49 | }; 50 | 51 | /** @brief Set minimum extraction range [m]*/ 52 | inline void setMinExtractionRange(double _minExtractionRange) { 53 | minExtractionRange = _minExtractionRange; 54 | }; 55 | 56 | /** @brief Set maximum extraction range [m]*/ 57 | inline void setMaxExtractionRange(double _maxExtractionRange) { 58 | maxExtractionRange = _maxExtractionRange; 59 | }; 60 | 61 | /** @brief Enable subbeam accuracy in keypoint extraction */ 62 | inline void enableSubbeam(bool _subbeam) { 63 | subbeam = _subbeam; 64 | }; 65 | 66 | /** @brief Set Non-Maxima-Suppression radius [m]*/ 67 | inline void setNMSRadius(double _NMSRadius) { 68 | NMSRadius = _NMSRadius; 69 | }; 70 | 71 | /** @brief Set neighA parameter for neighborhood radius computation*/ 72 | inline void setNeighA(double _neighA) { 73 | neighA = _neighA; 74 | }; 75 | 76 | /** @brief Set neighB parameter for neighborhood radius computation*/ 77 | inline void setNeighB(double _neighB) { 78 | neighB = _neighB; 79 | }; 80 | 81 | /** @brief Set minimum neighborhood size for each corner side*/ 82 | inline void setNeighMinPoint(double _neighMinPoint) { 83 | neighMinPoint = _neighMinPoint; 84 | }; 85 | 86 | /** @brief Set b-ratio for geometric corner validation*/ 87 | inline void setBRatio(double _bRatio) { 88 | bRatio = _bRatio; 89 | }; 90 | 91 | /** @brief Set number of circular grid sector for score computation*/ 92 | inline void setGridSectors(double _gridSectors) { 93 | gridSectors = _gridSectors; 94 | }; 95 | 96 | private: 97 | double minScoreTh; 98 | double minExtractionRange; 99 | double maxExtractionRange; 100 | bool subbeam; 101 | double NMSRadius; 102 | double neighA; 103 | double neighB; 104 | int neighMinPoint; 105 | double bRatio; 106 | int gridSectors; 107 | 108 | /** @brief distance between grid circular sectors*/ 109 | int circularSectorDistance(int a1, int a2, int res); 110 | 111 | /** @brief return index of corresponding circular sector in the score grid*/ 112 | int getCircularSectorIndex(const Point2d& p, const Point2d& pmid, double theta); 113 | 114 | /** @brief return neighborhood search radius, different heuristics are used based on rho*/ 115 | double getNeighRadius(double rho); 116 | 117 | /** @brief compute corner orientation given neighborhood points*/ 118 | double getCornerOrientation(const std::vector& neigh, int midIndex); 119 | 120 | /** @brief Non-Maxima-Suppression function for keypoint extraction*/ 121 | void NMSKeypoint(const std::vector& scores, const LaserScan& scan, unsigned int ibeg, unsigned int iend, double radius, int minval, std::vector& peaks); 122 | 123 | /** @brief compute corner position with subbeam accuracy*/ 124 | void subBeamCorner(const LaserScan& scan, int index, double radius, Point2d& p); 125 | 126 | /** @brief generate a line model using least-square*/ 127 | void generateLine(const std::vector& points, Eigen::Vector3d& model); 128 | 129 | /** @brief built-in 2x2 system solver function*/ 130 | bool solveSystem2x2(double* A, double* b, double* x); 131 | }; 132 | } -------------------------------------------------------------------------------- /include/falkolib/Feature/Keypoint.h: -------------------------------------------------------------------------------- 1 | /** 2 | * FALKOLib - Fast Adaptive Laser Keypoint Orientation-invariant 3 | * Copyright (C) 2016 Fabjan Kallasi and Dario Lodi Rizzini. 4 | * 5 | * This file is part of FALKOLib. 6 | * 7 | * FALKOLib is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU Lesser General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * FALKOLib is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU Lesser General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU Lesser General Public License 18 | * along with FALKOLib. If not, see . 19 | */ 20 | #pragma once 21 | 22 | #include 23 | 24 | namespace falkolib { 25 | 26 | /** 27 | * @brief class representing a keypoint 28 | * 29 | * Keypoints are defined as a 2d point 30 | */ 31 | class Keypoint { 32 | public: 33 | Point2d point; 34 | 35 | /** 36 | * @brief measure distance between keypoints 37 | * @param desc external keypoints 38 | * @return measured distance 39 | */ 40 | inline double distance(const Keypoint& desc) const { 41 | return (point - desc.point).norm(); 42 | } 43 | }; 44 | } -------------------------------------------------------------------------------- /include/falkolib/Feature/KeypointExtractor.h: -------------------------------------------------------------------------------- 1 | /** 2 | * FALKOLib - Fast Adaptive Laser Keypoint Orientation-invariant 3 | * Copyright (C) 2016 Fabjan Kallasi and Dario Lodi Rizzini. 4 | * 5 | * This file is part of FALKOLib. 6 | * 7 | * FALKOLib is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU Lesser General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * FALKOLib is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU Lesser General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU Lesser General Public License 18 | * along with FALKOLib. If not, see . 19 | */ 20 | #pragma once 21 | 22 | #include 23 | #include 24 | 25 | namespace falkolib { 26 | 27 | /** 28 | * @brief class representing a keypoints extractor engine 29 | */ 30 | template 31 | class KeypointExtractor { 32 | protected: 33 | /** 34 | * @brief Extract keypoints from a given scan 35 | * @param scan input laser scan 36 | * @param features keypoints extracted from given scan 37 | */ 38 | virtual void extract(const LaserScan& scan, std::vector& keypoints) = 0; 39 | }; 40 | } -------------------------------------------------------------------------------- /include/falkolib/Feature/OC.h: -------------------------------------------------------------------------------- 1 | /** 2 | * FALKOLib - Fast Adaptive Laser Keypoint Orientation-invariant 3 | * Copyright (C) 2016 Fabjan Kallasi and Dario Lodi Rizzini. 4 | * 5 | * This file is part of FALKOLib. 6 | * 7 | * FALKOLib is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU Lesser General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * FALKOLib is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU Lesser General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU Lesser General Public License 18 | * along with FALKOLib. If not, see . 19 | */ 20 | #pragma once 21 | 22 | #include 23 | 24 | namespace falkolib{ 25 | 26 | /** 27 | * @brief class representing a OC keypoint 28 | * 29 | * OC keypoint extends simple keypoint interface. 30 | * The index in the original scan, corner orientation and neighborhood search radius are also preserved. 31 | */ 32 | class OC : public Keypoint{ 33 | public: 34 | int index; 35 | double radius; 36 | double orientation; 37 | }; 38 | } 39 | -------------------------------------------------------------------------------- /include/falkolib/Feature/OCExtractor.h: -------------------------------------------------------------------------------- 1 | /** 2 | * FALKOLib - Fast Adaptive Laser Keypoint Orientation-invariant 3 | * Copyright (C) 2016 Fabjan Kallasi and Dario Lodi Rizzini. 4 | * 5 | * This file is part of FALKOLib. 6 | * 7 | * FALKOLib is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU Lesser General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * FALKOLib is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU Lesser General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU Lesser General Public License 18 | * along with FALKOLib. If not, see . 19 | */ 20 | #pragma once 21 | 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | 28 | namespace falkolib { 29 | 30 | /** 31 | * @brief class representing a OC keypoint extractor engine 32 | */ 33 | class OCExtractor : public KeypointExtractor { 34 | public: 35 | 36 | enum CornerOrientation { 37 | NE = 0, NW, SW, SE 38 | }; 39 | 40 | /** 41 | * @brief Constructor 42 | */ 43 | OCExtractor(); 44 | 45 | /** 46 | * @brief Extract OC keypoints from a given scan 47 | * @param scan input laser scan 48 | * @param features OC keypoint extracted from given scan 49 | */ 50 | void extract(const LaserScan& scan, std::vector& keypoints); 51 | 52 | /** @brief Set the tolerance distance to consider point aligned to corner edge. */ 53 | inline void setTol(double _tol) { 54 | tol = _tol; 55 | }; 56 | 57 | /** @brief Set the angular resolution. */ 58 | inline void setAngleRes(double _angleRes) { 59 | angleRes = _angleRes; 60 | houghSpectrum.init(angleRes, rangeRes, rangeMax); 61 | }; 62 | 63 | /** @brief Set the range/distance resolution used for Hough rho size. */ 64 | inline void setRangeRes(double _rangeRes) { 65 | rangeRes = _rangeRes; 66 | houghSpectrum.init(angleRes, rangeRes, rangeMax); 67 | }; 68 | 69 | /** @brief Set the maximum range for Hough rho cell grid. */ 70 | inline void setRangeMax(double _rangeMax) { 71 | rangeMax = _rangeMax; 72 | houghSpectrum.init(angleRes, rangeRes, rangeMax); 73 | }; 74 | 75 | /** @brief Set the non maxima suppression radius. */ 76 | inline void setNMSRadius(double _nmsRadius) { 77 | nmsRadius = _nmsRadius; 78 | }; 79 | 80 | /** @brief Set neighA parameter for neighborhood radius computation*/ 81 | inline void setNeighA(double _neighA) { 82 | neighA = _neighA; 83 | }; 84 | 85 | /** @brief Set neighB parameter for neighborhood radius computation*/ 86 | inline void setNeighB(double _neighB) { 87 | neighB = _neighB; 88 | }; 89 | 90 | /** @brief Set minimum neighborhood size for each corner side*/ 91 | inline void setNeighMinPoint(int _neighMinPoint) { 92 | neighMinPoint = _neighMinPoint; 93 | }; 94 | 95 | 96 | private: 97 | HoughSpectrum houghSpectrum; 98 | double tol; 99 | double angleRes; 100 | double rangeRes; 101 | double rangeMax; 102 | double nmsRadius; 103 | double neighA; 104 | double neighB; 105 | int neighMinPoint; 106 | 107 | /** @brief Computes the dominant direction of a scan using Hough Spectrum. 108 | */ 109 | double computeDominantAngle(const std::vector& points); 110 | 111 | /** @brief Rotates points. 112 | */ 113 | void rotatePoints(const std::vector& pointsIn, double angle, std::vector& pointsOut); 114 | 115 | /** @brief Computes the corner score, position and orientation of point with given index. */ 116 | double computeCornerScore(const std::vector& scan, int index, OC& keypoint); 117 | 118 | /** @brief Finds the neighborhood of point with the given index in vector. 119 | * @param pointsIn vector of all the points 120 | * @param index index of the center point in the 121 | * @param neigh found neighborhood 122 | * @param midIndex index of the center point in neigh vector 123 | */ 124 | void getNeighPoints(const std::vector& pointsIn, int index, std::vector& neigh, int& midIndex, double& dist) const; 125 | 126 | /** @brief Finds the peaks of score function using Non-Maxima Suppression (NMS) over a circular neighborhood with given radius. 127 | * It is almost a duplicate of FALKOExtractor similar function. 128 | */ 129 | void NMSKeypoint(const std::vector& scores, const LaserScan& scan, unsigned int ibeg, unsigned int iend, double radius, int minval, std::vector& peaks); 130 | }; 131 | } 132 | -------------------------------------------------------------------------------- /include/falkolib/Matching/AHTMatcher.h: -------------------------------------------------------------------------------- 1 | /** 2 | * FALKOLib - Fast Adaptive Laser Keypoint Orientation-invariant 3 | * Copyright (C) 2016 Fabjan Kallasi and Dario Lodi Rizzini. 4 | * 5 | * This file is part of FALKOLib. 6 | * 7 | * FALKOLib is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU Lesser General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * FALKOLib is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU Lesser General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU Lesser General Public License 18 | * along with FALKOLib. If not, see . 19 | */ 20 | #pragma once 21 | 22 | #include 23 | 24 | #include 25 | #include 26 | #include 27 | 28 | namespace falkolib { 29 | 30 | /** 31 | * @brief class representing a Affine Hough Transform feature matching engine 32 | */ 33 | template 34 | class AHTMatcher : public Matcher { 35 | public: 36 | 37 | /** 38 | * @brief Constructor 39 | */ 40 | AHTMatcher() : AHTMatcher(0.1, 0.1, 0.04, 5, 5, 1.57) { 41 | }; 42 | 43 | /** 44 | * @brief Constructor 45 | */ 46 | AHTMatcher(double _xRes, double _yRes, double _thetaRes, double _xAbsMax, double _yAbsMax, double _thetaAbsMax) { 47 | xRes = _xRes; 48 | yRes = _yRes; 49 | thetaRes = _thetaRes; 50 | xAbsMax = _xAbsMax; 51 | yAbsMax = _yAbsMax; 52 | thetaAbsMax = _thetaAbsMax; 53 | xSize = static_cast (2.0 * xAbsMax / xRes); 54 | ySize = static_cast (2.0 * yAbsMax / yRes); 55 | thetaSize = static_cast (2.0 * thetaAbsMax / thetaRes); 56 | matchesGrid.resize(xSize * ySize * thetaSize); 57 | txMax = 0; 58 | tyMax = 0; 59 | thetaMax = 0; 60 | }; 61 | 62 | /** 63 | * @brief match keypoints between two sets using AHT matcher 64 | * @param v1 first set of keypoints 65 | * @param v2 second set of keypoints 66 | * @param match matching vector representing associations, pair.first corresponds to v1 and pair.second corresponds to v2 67 | * @return number of valid association in match 68 | */ 69 | int match(const std::vector& v1, const std::vector& v2, std::vector >& match) { 70 | std::vector > assoInit; 71 | for (int i1 = 0; i1 < v1.size(); ++i1) { 72 | for (int i2 = 0; i2 < v2.size(); ++i2) { 73 | if (v1[i1].distance(v2[i2]) < distTh) { 74 | assoInit.push_back(std::make_pair(i1, i2)); 75 | } 76 | } 77 | } 78 | 79 | return getBestMatching(v1, v2, assoInit, match); 80 | } 81 | 82 | /** 83 | * @brief match keypoints between two sets using descriptors 84 | * @param keypoints1 first set of keypoints 85 | * @param descriptors1 first set of descriptors 86 | * @param keypoints2 second set of keypoints 87 | * @param descriptors2 second set of descriptors 88 | * @param match matching vector representing associations, pair.first corresponds to v1 and pair.second corresponds to v2 89 | * @return number of valid association in match 90 | */ 91 | int match(const std::vector& keypoints1, const std::vector& descriptors1, const std::vector& keypoints2, const std::vector& descriptors2, std::vector >& match) { 92 | std::vector > assoInit; 93 | for (int i1 = 0; i1 < keypoints1.size(); ++i1) { 94 | for (int i2 = 0; i2 < keypoints2.size(); ++i2) { 95 | if (keypoints1[i1].distance(keypoints2[i2]) < distTh && descriptors1[i1].distance(descriptors2[i2]) < descTh) { 96 | assoInit.push_back(std::make_pair(i1, i2)); 97 | } 98 | } 99 | } 100 | 101 | return getBestMatching(keypoints1, keypoints2, assoInit, match); 102 | } 103 | 104 | /** @brief set euclidean distance threshold for keypoints distance measurement*/ 105 | void setDistanceThreshold(double _th) { 106 | distTh = _th; 107 | } 108 | 109 | /** @brief set descriptor threshold for distance measurement*/ 110 | void setDescriptorThreshold(double _th) { 111 | descTh = _th; 112 | } 113 | 114 | 115 | 116 | private: 117 | double distTh; 118 | double descTh; 119 | double xRes; 120 | double yRes; 121 | double thetaRes; 122 | double xAbsMax; 123 | double yAbsMax; 124 | double thetaAbsMax; 125 | int xSize; 126 | int ySize; 127 | int thetaSize; 128 | std::vector > > matchesGrid; 129 | // std::vector countGrid; 130 | int txMax; 131 | int tyMax; 132 | int thetaMax; 133 | 134 | /** @brief get grid index from separated transform indexes*/ 135 | int getGridIndex(int ix, int iy, int it) { 136 | return ix + iy * xSize + it * xSize * ySize; 137 | } 138 | 139 | /** @brief get best matching given point and an initial guest associations list*/ 140 | int getBestMatching(const std::vector& v1, const std::vector& v2, const std::vector >& init, std::vector >& match) { 141 | for (auto& asso : init) { 142 | for (int it = 0; it < thetaSize; ++it) { 143 | double theta = thetaRes * (it - thetaSize / 2); 144 | const Point2d& p1 = v1[asso.first].point; 145 | const Point2d& p2 = v2[asso.second].point; 146 | double tx = p1[0] - p2[0] * std::cos(theta) + p2[1] * std::sin(theta); 147 | double ty = p1[1] - p2[0] * std::sin(theta) - p2[1] * std::cos(theta); 148 | 149 | int ix = static_cast (tx / xRes + xSize / 2); 150 | int iy = static_cast (ty / yRes + ySize / 2); 151 | 152 | if (ix >= 0 && ix < xSize && iy >= 0 && iy < ySize && it >= 0 && it < thetaSize) { 153 | // std::cout << "ind: " << getGridIndex(ix, iy, it) << "\t" << ix << "\t" << iy << "\t" << it << "\tpoints: " << p1.transpose() << "\t" << p2.transpose() << std::endl; 154 | matchesGrid[getGridIndex(ix, iy, it)].push_back(asso); 155 | 156 | if (matchesGrid[getGridIndex(ix, iy, it)].size() >= matchesGrid[getGridIndex(txMax, tyMax, thetaMax)].size()) { 157 | txMax = ix; 158 | tyMax = iy; 159 | thetaMax = it; 160 | } 161 | } 162 | } 163 | } 164 | 165 | match = matchesGrid[getGridIndex(txMax, tyMax, thetaMax)]; 166 | int numAsso = match.size(); 167 | for (int i = 0; i < v1.size(); ++i) { 168 | bool found = false; 169 | for (int j = 0; j < match.size(); ++j) { 170 | if (i == match[j].first) { 171 | found = true; 172 | break; 173 | } 174 | } 175 | if (not found) { 176 | match.push_back(std::make_pair(i, -1)); 177 | } 178 | } 179 | return numAsso; 180 | } 181 | }; 182 | } -------------------------------------------------------------------------------- /include/falkolib/Matching/CCDAMatcher.h: -------------------------------------------------------------------------------- 1 | /** 2 | * FALKOLib - Fast Adaptive Laser Keypoint Orientation-invariant 3 | * Copyright (C) 2016 Fabjan Kallasi and Dario Lodi Rizzini. 4 | * 5 | * This file is part of FALKOLib. 6 | * 7 | * FALKOLib is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU Lesser General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * FALKOLib is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU Lesser General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU Lesser General Public License 18 | * along with FALKOLib. If not, see . 19 | */ 20 | #pragma once 21 | 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | namespace falkolib { 28 | 29 | /** @brief This class implements the Combined Constraint Data Association (CCDA) 30 | * proposed by Tim Bailey. The best reference is: 31 | * 32 | * T. Bailey, "Mobile Robot Localisation and Mapping in Extensive Outdoor Environments", 33 | * PhD Thesis, University of Sydney, 2002. 34 | * 35 | * Of course, there are the related papers. 36 | * The method is based on the so called CorrespondenceGraph built on two point sets. 37 | * Data Association is computed as the maximum clique CorrespondenceGraph. 38 | * 39 | * Maximum clique has been computed according to T. Bailey suggestions. 40 | * However, for different approaches see: 41 | * http://www.dharwadker.org/clique/ 42 | * http://www.sanfoundry.com/cpp-program-find-maximum-size-clique-graph/ 43 | * http://www.sicmm.org/~konc/maxclique/mcqd/mcqd.h 44 | */ 45 | template 46 | class CCDAMatcher : public Matcher { 47 | public: 48 | 49 | /** Node of correspondence graph represents an association 50 | * between an input point and a target point. 51 | */ 52 | struct Node { 53 | int index; // global index (that should correspond to position in nodes vector) 54 | int inputId; // input point ID 55 | int targetId; // target point ID 56 | std::vector adjacents; 57 | 58 | bool operator<(const Node& n) const { 59 | return (index < n.index); 60 | } 61 | 62 | int degree() const { 63 | return adjacents.size(); 64 | } 65 | }; 66 | 67 | /** Distance constraint between the point belonging to the same set. 68 | */ 69 | struct Constraint { 70 | int i; 71 | int j; 72 | double dist; 73 | 74 | bool operator<(const Constraint& c) const { 75 | return (dist < c.dist); 76 | } 77 | }; 78 | 79 | /** @brief Default constructor. */ 80 | CCDAMatcher() : distTol(0.10), distMin(0.0) { 81 | } 82 | 83 | /** @brief Sets the tolerance on distances differences to be compatible. See CCDA details. */ 84 | void setDistTol(double _distTol) { 85 | distTol = _distTol; 86 | } 87 | 88 | /** @brief Sets the minimum distance between two keypoints to be a constraint. */ 89 | void setDistMin(double _distMin) { 90 | distMin = _distMin; 91 | } 92 | 93 | /** Computes the value 94 | */ 95 | int match(const std::vector& v1, const std::vector& v2, std::vector >& match) { 96 | std::vector nodes; 97 | std::vector > edges; 98 | std::vector constraints1; 99 | std::vector constraints2; 100 | Constraint constrTmp; 101 | std::vector cliqueMax; 102 | int num1 = v1.size(); 103 | int num2 = v2.size(); 104 | int isrc, idst; 105 | 106 | match.clear(); 107 | // Creates constraints inside each group of keypoints. 108 | // A constraint is the geometric distance (must be > distMin!) between two internal keypoints of a given set. 109 | makeRelativeConstraints(v1, constraints1); 110 | makeRelativeConstraints(v2, constraints2); 111 | 112 | // Creates correspondence graph based on constraints: 113 | // * node: association hypothesis n=(k1,k2) where k1 in set1, k2 in set2; 114 | // * edge: connects 2 compatible association hypotheses (compatibility (na,nb) if ||na.k1-na.k2| - |nb.k1-nb.k2|| < distToll); 115 | 116 | // Nodes 117 | makeNodeSet(v1, v2, nodes); 118 | 119 | // Edges 120 | for (auto& constrCurr : constraints1) { 121 | // Finds the candidate target constraints, i.e. pair of target points with similar 122 | // distance, and visits them 123 | constrTmp.dist = constrCurr.dist - distTol; 124 | typename std::vector::iterator it = std::upper_bound(constraints2.begin(), constraints2.end(), constrTmp); 125 | for (; it != constraints2.end() && it->dist < constrCurr.dist + distTol; ++it) { 126 | //std::cout << " target constr (" << it->i << "," << it->j << "): " << it->dist << std::endl; 127 | if (std::abs(it->dist - constrCurr.dist) < distTol && (it->dist + constrCurr.dist) > distMin) { 128 | assert(constrCurr.i < num1 && constrCurr.j < num1); 129 | assert(it->i < num2 && it->j < num2); 130 | // Match 1 131 | isrc = it->i + constrCurr.i * num2; 132 | idst = it->j + constrCurr.j * num2; 133 | assert(isrc < nodes.size() && idst < nodes.size()); 134 | assert(nodes[isrc].inputId == constrCurr.i && nodes[isrc].targetId == it->i); 135 | assert(nodes[idst].inputId == constrCurr.j && nodes[idst].targetId == it->j); 136 | nodes[isrc].adjacents.push_back(idst); 137 | nodes[idst].adjacents.push_back(isrc); 138 | // Match 2 139 | isrc = it->i + constrCurr.j * num2; 140 | idst = it->j + constrCurr.i * num2; 141 | assert(isrc < nodes.size() && idst < nodes.size()); 142 | assert(nodes[isrc].inputId == constrCurr.j && nodes[isrc].targetId == it->i); 143 | assert(nodes[idst].inputId == constrCurr.i && nodes[idst].targetId == it->j); 144 | nodes[isrc].adjacents.push_back(idst); 145 | nodes[idst].adjacents.push_back(isrc); 146 | } 147 | } 148 | } 149 | 150 | // Finds maximum clique 151 | findCliqueDyn(nodes, cliqueMax); 152 | match.reserve(cliqueMax.size()); 153 | for (auto& id : cliqueMax) { 154 | match.push_back(std::make_pair(nodes[id].inputId, nodes[id].targetId)); 155 | } 156 | } 157 | 158 | private: 159 | double distTol; 160 | double distMin; 161 | 162 | void makeNodeSet(const std::vector& points1, const std::vector& points2, std::vector& nodes) { 163 | Node corresp; 164 | int index; 165 | nodes.clear(); 166 | nodes.resize(points1.size() * points2.size()); 167 | for (int i = 0; i < points1.size(); ++i) { 168 | for (int j = 0; j < points2.size(); ++j) { 169 | index = i * points2.size() + j; 170 | nodes[index].inputId = i; 171 | nodes[index].targetId = j; 172 | nodes[index].index = index; 173 | nodes[index].adjacents.clear(); 174 | } 175 | } 176 | // for (int i = 0; i < nodes.size(); ++i) { 177 | // if (nodes[i].index != i) { 178 | // std::cerr << __PRETTY_FUNCTION__ << ": difference bewteen node index " << nodes[i].index << " and its position " << i << " in node vector" << std::endl; 179 | // } 180 | // } 181 | } 182 | 183 | void makeRelativeConstraints(const std::vector& points, std::vector& constraints) { 184 | Constraint constraint; 185 | int n = points.size(); 186 | for (int i = 0; i < n; ++i) { 187 | for (int j = i + 1; j < n; ++j) { 188 | constraint.i = i; 189 | constraint.j = j; 190 | constraint.dist = points[i].distance(points[j]); 191 | constraints.push_back(constraint); 192 | } 193 | } 194 | std::sort(constraints.begin(), constraints.end()); 195 | } 196 | 197 | void findCliqueDyn(const std::vector& nodes, std::vector& cliqueMax) { 198 | bool **conn; 199 | int nsize = nodes.size(); 200 | int *qmax; 201 | int qsize; 202 | 203 | if (nsize == 0) { 204 | return; 205 | } 206 | 207 | // Allocates space for adjacence matrix 208 | conn = new bool*[nsize]; 209 | for (int i = 0; i < nsize; ++i) { 210 | conn[i] = new bool[nsize]; 211 | memset(conn[i], 0, nsize * sizeof (bool)); 212 | } 213 | // Fills adjacence matrix 214 | for (int i = 0; i < nodes.size(); ++i) { 215 | assert(nodes[i].index == i); 216 | for (auto& j : nodes[i].adjacents) { 217 | conn[i][j] = true; 218 | } 219 | } 220 | // Computes maximum clique 221 | Maxclique maxcl(conn, nsize); 222 | maxcl.mcq(qmax, qsize); 223 | cliqueMax.resize(qsize); 224 | for (int i = 0; i < qsize; ++i) { 225 | cliqueMax[i] = qmax[i]; 226 | } 227 | 228 | delete [] qmax; 229 | for (int i = 0; i < nsize; ++i) 230 | delete [] conn[i]; 231 | delete [] conn; 232 | } 233 | 234 | }; 235 | } 236 | -------------------------------------------------------------------------------- /include/falkolib/Matching/Matcher.h: -------------------------------------------------------------------------------- 1 | /** 2 | * FALKOLib - Fast Adaptive Laser Keypoint Orientation-invariant 3 | * Copyright (C) 2016 Fabjan Kallasi and Dario Lodi Rizzini. 4 | * 5 | * This file is part of FALKOLib. 6 | * 7 | * FALKOLib is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU Lesser General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * FALKOLib is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU Lesser General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU Lesser General Public License 18 | * along with FALKOLib. If not, see . 19 | */ 20 | #pragma once 21 | 22 | #include 23 | 24 | namespace falkolib { 25 | 26 | /** 27 | * @brief class representing a simple feature matching engine 28 | */ 29 | template 30 | class Matcher { 31 | protected: 32 | 33 | /** 34 | * @brief match features between two sets 35 | * @param v1 first set of features 36 | * @param v2 second set of features 37 | * @param match matching vector representing associations, pair.first corresponds to v1 and pair.second corresponds to v2 38 | * @return number of valid association in match 39 | */ 40 | virtual int match(const std::vector& v1, const std::vector& v2, std::vector >& match) = 0; 41 | }; 42 | } -------------------------------------------------------------------------------- /include/falkolib/Matching/NNMatcher.h: -------------------------------------------------------------------------------- 1 | /** 2 | * FALKOLib - Fast Adaptive Laser Keypoint Orientation-invariant 3 | * Copyright (C) 2016 Fabjan Kallasi and Dario Lodi Rizzini. 4 | * 5 | * This file is part of FALKOLib. 6 | * 7 | * FALKOLib is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU Lesser General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * FALKOLib is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU Lesser General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU Lesser General Public License 18 | * along with FALKOLib. If not, see . 19 | */ 20 | #pragma once 21 | 22 | #include 23 | 24 | #include 25 | #include 26 | #include 27 | 28 | namespace falkolib { 29 | 30 | /** 31 | * @brief class representing a simple Nearest-Neighborhood feature matching engine 32 | */ 33 | template 34 | class NNMatcher : public Matcher { 35 | public: 36 | 37 | /** 38 | * @brief Constructor 39 | */ 40 | NNMatcher() { 41 | 42 | }; 43 | 44 | /** 45 | * @brief match keypoints or descriptors between two sets 46 | * @param v1 first set of keypoints or descriptors 47 | * @param v2 second set of keypoints or descriptors 48 | * @param match matching vector representing associations, pair.first corresponds to v1 and pair.second corresponds to v2 49 | * @return number of valid association in match 50 | */ 51 | int match(const std::vector& v1, const std::vector& v2, std::vector >& match) { 52 | match.clear(); 53 | int imin; 54 | double dmin, d; 55 | int counter = 0; 56 | std::vector matched(v2.size(), false); 57 | for (int i1 = 0; i1 < (int) v1.size(); ++i1) { 58 | imin = -1; 59 | dmin = 1.05 * distTh; 60 | for (int i2 = 0; i2 < (int) v2.size(); ++i2) { 61 | if (not matched[i2]) { 62 | d = v1[i1].distance(v2[i2]); 63 | if (d < dmin) { 64 | imin = i2; 65 | dmin = d; 66 | } 67 | } 68 | } 69 | if (dmin < distTh) { 70 | match.push_back(std::make_pair(i1, imin)); 71 | matched[imin] = true; 72 | counter++; 73 | } else { 74 | match.push_back(std::make_pair(i1, -1)); 75 | } 76 | } 77 | 78 | return counter; 79 | } 80 | 81 | /** 82 | * @brief match keypoints between two sets using descriptors 83 | * @param keypoints1 first set of keypoints 84 | * @param descriptors1 first set of descriptors 85 | * @param keypoints2 second set of keypoints 86 | * @param descriptors2 second set of descriptors 87 | * @param match matching vector representing associations, pair.first corresponds to v1 and pair.second corresponds to v2 88 | * @return number of valid association in match 89 | */ 90 | int match(const std::vector& keypoints1, const std::vector& descriptors1, const std::vector& keypoints2, const std::vector& descriptors2, std::vector >& match) { 91 | match.clear(); 92 | int imin; 93 | double dmin, d, ddesc; 94 | int counter = 0; 95 | std::vector matched(keypoints2.size(), false); 96 | for (int i1 = 0; i1 < (int) keypoints1.size(); ++i1) { 97 | imin = -1; 98 | dmin = 1.05 * distTh; 99 | for (int i2 = 0; i2 < (int) keypoints2.size(); ++i2) { 100 | if (not matched[i2]) { 101 | d = keypoints1[i1].distance(keypoints2[i2]); 102 | ddesc = descriptors1[i1].distance(descriptors2[i2]); 103 | if (d < dmin && ddesc < descTh) { 104 | imin = i2; 105 | dmin = d; 106 | } 107 | } 108 | } 109 | if (dmin < distTh && descriptors1[i1].distance(descriptors2[imin]) < descTh) { 110 | match.push_back(std::make_pair(i1, imin)); 111 | matched[imin] = true; 112 | counter++; 113 | } else { 114 | match.push_back(std::make_pair(i1, -1)); 115 | } 116 | } 117 | 118 | return counter; 119 | } 120 | 121 | /** @brief set euclidean distance threshold for keypoints distance measurements*/ 122 | void setDistanceThreshold(double _th) { 123 | distTh = _th; 124 | } 125 | 126 | /** #brief set descriptor threshold for distance measurements*/ 127 | void setDescriptorThreshold(double _th) { 128 | descTh = _th; 129 | } 130 | 131 | private: 132 | double distTh; 133 | double descTh; 134 | 135 | }; 136 | } -------------------------------------------------------------------------------- /info.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | FALKOLib 5 | 6 | 7 | Fabjan Kallasi 8 | http://rimlab.ce.unipr.it/Kallasi.html 9 | 10 | 11 | Dario Lodi Rizzini 12 | http://rimlab.ce.unipr.it/LodiRizzini.html 13 | 14 | 15 | Stefano Caselli 16 | http://rimlab.ce.unipr.it/Caselli.html 17 | 18 | 19 | http://rimlab.ce.unipr.it/ 20 | 21 | 22 | FALKOLib is a library containing keypoint detectors for the stable detection 23 | of interest points in laser measurements and two descriptors for robust associations. 24 | 25 | 26 | 27 | Detecting features in sensor measurements and distinguishing among 28 | them is an important capability for robot localization and navigation. 29 | Despite the wide diffusion of range finders, there are few works 30 | on keypoint features for 2D LIDAR and there is potential for improvement 31 | over the existing methods. 32 | We propose two novel keypoint detectors for the stable detection of 33 | interest points in laser measurements (named FALKO and OC) 34 | and two descriptors for robust associations (named BSC and CGH). 35 | The features defined by combining keypoints and descriptors allow 36 | stable and efficient place recognition. 37 | 38 | 39 | 40 | Developed and tested under Linux (tested with GCC >= 4.8). 41 | Library dependencies: Eigen 3.0, Boost. 42 | CMake is needed for building. 43 | 44 | feature maps 45 | 46 | The approach takes raw laser range data. 47 | Laser scans with higher resolution (0.25 deg) enables richer description of the environment. 48 | Odometry can be used for testing the association between features 49 | 50 | 51 | 52 | Carmen log format. Raw scan data. 53 | 54 | 55 | 56 | http://rimlab.ce.unipr.it/img/falko-bsc.png 57 | Example of feature association with FALKO detector and BSC descriptors. 58 | http://rimlab.ce.unipr.it/img/intel-lab-falko-map.png 59 | Example of FALKO keypoint (red points) extraction in intel-lab dataset 60 | 61 | 62 | 63 | Fast Keypoint Features from Laser Scanner for Robot Localization and Mapping 64 | F. Kallasi, D. Lodi Rizzini, and S. Caselli. 65 | IEEE Robotics and Automation Letters (RA-L), 1(1):176-183 66 | 2016 67 | http://dx.doi.org/10.1109/LRA.2016.2517210 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | FALKOLib is licenced under the Creative Commons 78 | (Attribution-NonCommercial-ShareAlike). 79 | 80 | 81 | FALKOLib is designed as a stand-alone library that can be included in your projects. 82 | For any questions, comments or installation problems please contact either kallasi@ce.unipr.it or dlr@ce.unipr.it. 83 | 84 | 85 | 86 | -------------------------------------------------------------------------------- /src/Common/HoughSpectrum.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * FALKOLib - Fast Adaptive Laser Keypoint Orientation-invariant 3 | * Copyright (C) 2016 Fabjan Kallasi and Dario Lodi Rizzini. 4 | * 5 | * This file is part of FALKOLib. 6 | * 7 | * FALKOLib is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU Lesser General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * FALKOLib is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU Lesser General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU Lesser General Public License 18 | * along with FALKOLib. If not, see . 19 | */ 20 | #include 21 | #include 22 | 23 | using namespace std; 24 | 25 | namespace falkolib { 26 | 27 | HoughSpectrum::HoughSpectrum() 28 | : thetaNum_(360), 29 | rhoNum_(1000), 30 | thetaStep_(0.0087266), rhoStep_(0.02), 31 | hough_(thetaNum_, rhoNum_), 32 | spectrum_(thetaNum_), 33 | orthoSpectrum_((thetaNum_ / 2) + (thetaNum_ % 2)), 34 | cosLut_(thetaNum_), sinLut_(thetaNum_) { 35 | for (unsigned int i = 0; i < thetaNum_; ++i) { 36 | cosLut_(i) = cos(thetaStep_ * i); 37 | sinLut_(i) = sin(thetaStep_ * i); 38 | } 39 | } 40 | 41 | HoughSpectrum::HoughSpectrum(double thetaStep, double rhoStep, double rhoMax) 42 | : thetaNum_(static_cast (ceil(M_PI / thetaStep))), 43 | rhoNum_(2 * static_cast (ceil(rhoMax / rhoStep))), 44 | thetaStep_(thetaStep), rhoStep_(rhoStep), 45 | hough_(thetaNum_, rhoNum_), 46 | spectrum_(thetaNum_), 47 | orthoSpectrum_((thetaNum_ / 2) + (thetaNum_ % 2)), 48 | cosLut_(thetaNum_), sinLut_(thetaNum_) { 49 | for (unsigned int i = 0; i < thetaNum_; ++i) { 50 | cosLut_(i) = cos(thetaStep_ * i); 51 | sinLut_(i) = sin(thetaStep_ * i); 52 | } 53 | } 54 | 55 | void HoughSpectrum::init(double thetaStep, double rhoStep, double rhoMax) { 56 | thetaStep_ = thetaStep; 57 | rhoStep_ = rhoStep; 58 | thetaNum_ = (unsigned int) ceil(M_PI / thetaStep); 59 | rhoNum_ = (unsigned int) ceil(rhoMax / rhoStep); 60 | 61 | hough_.resize(thetaNum_, rhoNum_); 62 | spectrum_.resize(thetaNum_); 63 | orthoSpectrum_.resize(thetaNum_ / 2); 64 | cosLut_.resize(thetaNum_); 65 | sinLut_.resize(thetaNum_); 66 | 67 | for (unsigned int i = 0; i < thetaNum_; ++i) { 68 | cosLut_(i) = cos(thetaStep_ * i); 69 | sinLut_(i) = sin(thetaStep_ * i); 70 | } 71 | } 72 | 73 | } // end of namespace 74 | 75 | -------------------------------------------------------------------------------- /src/Feature/BSC.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * FALKOLib - Fast Adaptive Laser Keypoint Orientation-invariant 3 | * Copyright (C) 2016 Fabjan Kallasi and Dario Lodi Rizzini. 4 | * 5 | * This file is part of FALKOLib. 6 | * 7 | * FALKOLib is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU Lesser General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * FALKOLib is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU Lesser General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU Lesser General Public License 18 | * along with FALKOLib. If not, see . 19 | */ 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | using namespace std; 28 | 29 | namespace falkolib { 30 | 31 | BSC::BSC(double _radius, int _circularSectorNumber, int _radialRingNumber) { 32 | radius = _radius; 33 | circularSectorNumber = _circularSectorNumber; 34 | sectorResolution = 2.0 * M_PI / circularSectorNumber; 35 | radialRingNumber = _radialRingNumber; 36 | ringResolution = radius / radialRingNumber; 37 | } 38 | 39 | void BSC::compute(std::vector& neigh, int centralPointIndex) { 40 | const int size = neigh.size(); 41 | grid.resize(radialRingNumber, std::vector(circularSectorNumber, 0)); 42 | for (int i = 0; i < size; ++i) { 43 | if (i != centralPointIndex) { 44 | int col = static_cast (floor((angleBetweenPoints(neigh[i], neigh[centralPointIndex]) + M_PI) / sectorResolution)); 45 | assert(col < circularSectorNumber); 46 | int row = static_cast (floor(((neigh[i] - neigh[centralPointIndex]).norm()) / ringResolution)); 47 | assert(row < radialRingNumber); 48 | grid[row][col] = 1; 49 | } 50 | } 51 | } 52 | 53 | double BSC::distance(const Descriptor& desc) const{ 54 | try { 55 | const BSC& d2 = dynamic_cast (desc); 56 | assert(circularSectorNumber == d2.circularSectorNumber); 57 | assert(radialRingNumber == d2.radialRingNumber); 58 | if (circularSectorNumber == d2.circularSectorNumber && radialRingNumber == d2.radialRingNumber) { 59 | return HammingDistance(grid, d2.grid); 60 | } 61 | } catch (const std::bad_cast& e){ 62 | ; 63 | } 64 | return numeric_limits::max(); 65 | } 66 | 67 | double BSC::HammingDistance(const std::vector >& g1, const std::vector >& g2) const { 68 | double diff = 0.0; 69 | for (int i = 0; i < g1.size(); ++i) { 70 | for (int j = 0; j < g1[i].size(); ++j) { 71 | if (g1[i][j] != g2[i][j]) { 72 | diff++; 73 | } 74 | } 75 | } 76 | return diff; 77 | } 78 | 79 | void BSC::rotate(double theta) { 80 | int rot = static_cast (floor((theta + M_PI) / sectorResolution)) % circularSectorNumber; 81 | for (auto& i : grid) { 82 | std::rotate(i.begin(), i.begin() + rot, i.end()); 83 | } 84 | } 85 | } -------------------------------------------------------------------------------- /src/Feature/CGH.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * FALKOLib - Fast Adaptive Laser Keypoint Orientation-invariant 3 | * Copyright (C) 2016 Fabjan Kallasi and Dario Lodi Rizzini. 4 | * 5 | * This file is part of FALKOLib. 6 | * 7 | * FALKOLib is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU Lesser General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * FALKOLib is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU Lesser General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU Lesser General Public License 18 | * along with FALKOLib. If not, see . 19 | */ 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | #include 28 | 29 | using namespace std; 30 | 31 | namespace falkolib { 32 | 33 | CGH::CGH(double _radius, int _circularSectorNumber) { 34 | radius = _radius; 35 | circularSectorNumber = _circularSectorNumber; 36 | sectorResolution = 2.0 * M_PI / circularSectorNumber; 37 | } 38 | 39 | void CGH::compute(std::vector& neigh, int centralPointIndex) { 40 | const int size = neigh.size(); 41 | histogram.resize(circularSectorNumber, 0); 42 | for (int i = 0; i < size; ++i) { 43 | if (i != centralPointIndex) { 44 | int ind = static_cast (floor((angleBetweenPoints(neigh[i], neigh[centralPointIndex]) + M_PI) / sectorResolution)); 45 | assert(ind < circularSectorNumber); 46 | if (ind == 0) { 47 | histogram[circularSectorNumber - 1]++; 48 | } else { 49 | histogram[ind - 1]++; 50 | } 51 | histogram[ind] += 5; 52 | if (ind == (circularSectorNumber - 1)) { 53 | histogram[0]++; 54 | } else { 55 | histogram[ind + 1]++; 56 | } 57 | } 58 | } 59 | double sum = 0; 60 | for (auto& l : histogram) { 61 | sum += l; 62 | } 63 | for (auto& l : histogram) { 64 | l /= sum; 65 | } 66 | } 67 | 68 | double CGH::distance(const Descriptor& desc) const { 69 | try { 70 | const CGH& d2 = dynamic_cast (desc); 71 | assert(circularSectorNumber == d2.circularSectorNumber); 72 | if (circularSectorNumber == d2.circularSectorNumber) { 73 | return SymmetricChiSquaredDistance(histogram, d2.histogram); 74 | } 75 | } catch (const std::bad_cast& e){ 76 | ; 77 | } 78 | return numeric_limits::max(); 79 | } 80 | 81 | double CGH::SymmetricChiSquaredDistance(const std::vector& h1, const std::vector& h2) const { 82 | double sum1 = 0.0; 83 | for (int i = 0; i < h1.size(); ++i) { 84 | sum1 += ((h1[i] - h2[i])*(h1[i] - h2[i]) / (h1[i] + h2[i] + 0.00000001)); 85 | } 86 | return sum1/2.0; 87 | } 88 | 89 | void CGH::rotate(double theta) { 90 | int rot = (static_cast (floor((theta + M_PI) / sectorResolution)) % circularSectorNumber); 91 | std::rotate(histogram.begin(), histogram.begin()+rot, histogram.end()); 92 | } 93 | } -------------------------------------------------------------------------------- /src/Feature/FALKOExtractor.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * FALKOLib - Fast Adaptive Laser Keypoint Orientation-invariant 3 | * Copyright (C) 2016 Fabjan Kallasi and Dario Lodi Rizzini. 4 | * 5 | * This file is part of FALKOLib. 6 | * 7 | * FALKOLib is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU Lesser General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * FALKOLib is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU Lesser General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU Lesser General Public License 18 | * along with FALKOLib. If not, see . 19 | */ 20 | #include 21 | 22 | using namespace std; 23 | 24 | namespace falkolib { 25 | 26 | FALKOExtractor::FALKOExtractor() { 27 | minScoreTh = 50; 28 | minExtractionRange = 0; 29 | maxExtractionRange = 30; 30 | subbeam = true; 31 | NMSRadius = 0.1; 32 | neighA = 0.1; 33 | neighB = 0.07; 34 | neighMinPoint = 2; 35 | bRatio = 2.5; 36 | gridSectors = 16; 37 | } 38 | 39 | void FALKOExtractor::extract(const LaserScan& scan, std::vector& keypoints) { 40 | const int numBeams = scan.getNumBeams(); 41 | vector scores(numBeams, -10); 42 | vector neigh; 43 | vector radius(numBeams); 44 | int neighSize; 45 | int neighSizeL; 46 | int neighSizeR; 47 | int midIndex; 48 | double triangleBLength; 49 | double triangleHLength; 50 | vector thetaCorner(numBeams); 51 | int scoreL; 52 | int scoreR; 53 | int scoreMax = 0; 54 | vector peaks; 55 | vector neighCircularIndexesL; 56 | vector neighCircularIndexesR; 57 | 58 | for (int ind = 0; ind < numBeams; ++ind) { 59 | if (scan.ranges[ind] < minExtractionRange || scan.ranges[ind] > maxExtractionRange) { 60 | scores[ind] = -10; 61 | continue; 62 | } 63 | neigh.clear(); 64 | radius[ind] = getNeighRadius(scan.ranges[ind]); 65 | scan.getNeighPoints(ind, radius[ind], neigh, midIndex); 66 | neighSize = neigh.size(); 67 | 68 | neighSizeL = midIndex; 69 | neighSizeR = neighSize - midIndex - 1; 70 | 71 | if (neighSizeL < neighMinPoint || neighSizeR < neighMinPoint) { 72 | scores[ind] = -10; 73 | continue; 74 | } 75 | 76 | triangleBLength = pointsDistance(neigh.front(), neigh.back()); 77 | triangleHLength = std::abs(signedTriangleArea(neigh[midIndex], neigh.front(), neigh.back())) / triangleBLength; 78 | 79 | if (triangleBLength < (radius[ind] / bRatio) || triangleHLength < (radius[ind] / bRatio)) { 80 | scores[ind] = -10; 81 | continue; 82 | } 83 | 84 | thetaCorner[ind] = getCornerOrientation(neigh, midIndex); 85 | 86 | neighCircularIndexesL.resize(neighSizeL, 0); 87 | neighCircularIndexesR.resize(neighSizeR, 0); 88 | 89 | for (int i = 0; i < neighSizeL; ++i) { 90 | neighCircularIndexesL[i] = getCircularSectorIndex(neigh[i], neigh[midIndex], thetaCorner[ind]); 91 | } 92 | for (int i = 0; i < neighSizeR; ++i) { 93 | neighCircularIndexesR[i] = getCircularSectorIndex(neigh[midIndex + i + 1], neigh[midIndex], thetaCorner[ind]); 94 | } 95 | 96 | scoreL = 0; 97 | scoreR = 0; 98 | 99 | for (int i = midIndex - 1; i >= 0; --i) { 100 | for (int j = i; j >= 0; --j) { 101 | scoreL += circularSectorDistance(neighCircularIndexesL[i], neighCircularIndexesL[j], gridSectors); 102 | } 103 | } 104 | 105 | for (int i = midIndex + 1; i < neighSize; ++i) { 106 | for (int j = i; j < neighSize; ++j) { 107 | scoreR += circularSectorDistance(neighCircularIndexesR[i - midIndex - 1], neighCircularIndexesR[j - midIndex - 1], gridSectors); 108 | } 109 | } 110 | 111 | scores[ind] = scoreL + scoreR; 112 | 113 | if (scores[ind] > scoreMax) { 114 | scoreMax = scores[ind]; 115 | } 116 | } 117 | 118 | for (int ind = 0; ind < numBeams; ++ind) { 119 | if (scores[ind] < 0) { 120 | scores[ind] = scoreMax; 121 | } 122 | scores[ind] = scoreMax - scores[ind]; 123 | } 124 | 125 | NMSKeypoint(scores, scan, 0, numBeams, NMSRadius, (scoreMax * minScoreTh / 100.0), peaks); 126 | 127 | for (int i = 0; i < peaks.size(); ++i) { 128 | FALKO kp; 129 | kp.index = peaks[i]; 130 | kp.orientation = thetaCorner[peaks[i]]; 131 | kp.radius = radius[peaks[i]]; 132 | 133 | if (subbeam) { 134 | subBeamCorner(scan, peaks[i], radius[peaks[i]], kp.point); 135 | } else { 136 | kp.point = scan.points[peaks[i]]; 137 | } 138 | 139 | keypoints.push_back(std::move(kp)); 140 | } 141 | 142 | } 143 | 144 | int FALKOExtractor::circularSectorDistance(int a1, int a2, int res) { 145 | const int r2 = res / 2; 146 | return std::abs(((a1 - a2) + r2) % res - r2); 147 | } 148 | 149 | int FALKOExtractor::getCircularSectorIndex(const Point2d& p, const Point2d& pmid, double theta) { 150 | return (int) floor((angleBetweenPoints(p, pmid) + M_PI - theta) / (2.0 * M_PI / gridSectors)); 151 | } 152 | 153 | double FALKOExtractor::getNeighRadius(double rho) { 154 | double radius = neighA * std::exp(neighB * rho); 155 | if (radius >= rho) { 156 | return rho * 0.8; 157 | } 158 | return radius; 159 | } 160 | 161 | double FALKOExtractor::getCornerOrientation(const std::vector& neigh, int midIndex) { 162 | Point2d oriL(0, 0); 163 | Point2d oriR(0, 0); 164 | int size = neigh.size(); 165 | for (int i = 0; i < size; ++i) { 166 | if (i < midIndex) { 167 | oriL += (neigh[i] - neigh[midIndex]); // / (s[i] - s[mid]).norm(); 168 | } else if (i > midIndex) { 169 | oriR += (neigh[i] - neigh[midIndex]); // / (s[i] - s[mid]).norm(); 170 | } 171 | } 172 | oriL /= midIndex; 173 | oriR /= (size - (midIndex + 1)); 174 | Point2d ori = oriL + oriR; 175 | double theta = atan2(ori(1), ori(0)); 176 | } 177 | 178 | void FALKOExtractor::NMSKeypoint(const std::vector& scores, const LaserScan& scan, unsigned int ibeg, unsigned int iend, double radius, int minval, std::vector& peaks) { 179 | unsigned i, imax; 180 | unsigned j, jbeg, jend, jmax; 181 | std::vector candidates; 182 | peaks.clear(); 183 | 184 | i = ibeg; 185 | imax = ibeg; 186 | while (i < iend) { 187 | int win; 188 | if (radius >= scan.ranges[i]) { 189 | win = std::floor(std::asin(0.8) / scan.getAngleInc()); 190 | } else { 191 | win = std::floor(std::asin(radius / scan.ranges[i]) / scan.getAngleInc()); 192 | } 193 | 194 | jmax = i; 195 | 196 | if (imax + win < i) { 197 | jbeg = (i >= win ? i - win : 0); 198 | imax = i; 199 | } else { 200 | jbeg = i; 201 | } 202 | 203 | jend = (i + win + 1 < iend ? i + win + 1 : iend); 204 | for (j = jbeg; j < jend; ++j) { 205 | if (scores[j] > scores[jmax]) { 206 | jmax = j; 207 | } 208 | } 209 | imax = (scores[jmax] >= scores[imax] ? jmax : imax); 210 | 211 | if (i == imax && scores[i] > minval) { 212 | candidates.push_back(i); 213 | } 214 | if (jmax > i) i = jmax; 215 | else ++i; 216 | } 217 | 218 | int i1 = 0; 219 | int i2 = 0; 220 | int counter = 0; 221 | 222 | for (i1 = 0, i2 = 0; i1 < candidates.size(); ++i1) { 223 | if (scores[candidates[i2]] == scores[candidates[i1]]) { 224 | counter++; 225 | if (2 * abs(i2 - i1) > counter) { 226 | ++i2; 227 | } 228 | } else { 229 | peaks.push_back(candidates[i2]); 230 | i2 = i1; 231 | counter = 0; 232 | } 233 | } 234 | if (i2 != candidates.size()) { 235 | peaks.push_back(candidates[i2]); 236 | } 237 | } 238 | 239 | void FALKOExtractor::subBeamCorner(const LaserScan& scan, int index, double radius, Point2d& p) { 240 | vector neigh; 241 | int midIndex; 242 | scan.getNeighPoints(index, radius, neigh, midIndex); 243 | int neighSize = neigh.size(); 244 | 245 | Eigen::Vector3d leftLine; 246 | Eigen::Vector3d rightLine; 247 | 248 | std::vector leftSide; 249 | for (int i = 0; i <= midIndex; ++i) { 250 | leftSide.push_back(neigh[i]); 251 | } 252 | 253 | std::vector rightSide; 254 | for (int i = midIndex; i < neighSize; ++i) { 255 | rightSide.push_back(neigh[i]); 256 | } 257 | 258 | generateLine(leftSide, leftLine); 259 | generateLine(rightSide, rightLine); 260 | 261 | double A[4]; 262 | double b[2]; 263 | double x[2]; 264 | A[0] = leftLine[0]; 265 | A[1] = leftLine[1]; 266 | A[2] = rightLine[0]; 267 | A[3] = rightLine[1]; 268 | 269 | b[0] = -leftLine[2]; 270 | b[1] = -rightLine[2]; 271 | 272 | bool valid = solveSystem2x2(A, b, x); 273 | 274 | if (not valid) { 275 | p = neigh[midIndex]; 276 | return; 277 | } 278 | 279 | Point2d temp(x[0], x[1]); 280 | 281 | if (pointsDistance(neigh[midIndex], temp) < 0.20) { 282 | p = temp; 283 | } else { 284 | p = neigh[midIndex]; 285 | } 286 | } 287 | 288 | void FALKOExtractor::generateLine(const std::vector& points, Eigen::Vector3d& model) { 289 | double sxx = 0.0; 290 | double syy = 0.0; 291 | double sxy = 0.0; 292 | double sx = 0.0; 293 | double sy = 0.0; 294 | int num = 0; 295 | for (unsigned int i = 0; i < points.size(); ++i) { 296 | sxx += points[i](0) * points[i](0); 297 | syy += points[i](1) * points[i](1); 298 | sxy += points[i](0) * points[i](1); 299 | sx += points[i](0); 300 | sy += points[i](1); 301 | ++num; 302 | } 303 | 304 | double msxx = (sxx - sx * sx / num) / num; 305 | double msyy = (syy - sy * sy / num) / num; 306 | double msxy = (sxy - sx * sy / num) / num; 307 | double b = 2.0 * msxy; 308 | double a = msxx; 309 | double c = msyy; 310 | double theta = 0.5 * (atan2(b, a - c) + M_PI); 311 | theta = atan2(sin(theta), cos(theta)); 312 | double rho = (sx * cos(theta) + sy * sin(theta)) / num; 313 | 314 | if (rho < 0) { 315 | theta = atan2(sin(theta + M_PI), cos(theta + M_PI)); 316 | rho = -rho; 317 | } 318 | model(0) = cos(theta); 319 | model(1) = sin(theta); 320 | model(2) = -rho; 321 | } 322 | 323 | bool FALKOExtractor::solveSystem2x2(double* A, double* b, double* x) { 324 | double det = A[0] * A[3] - A[1] * A[2]; 325 | if (std::abs(det) < 0.0000001) 326 | return false; 327 | x[0] = (b[0] * A[3] - A[1] * b[1]) / det; 328 | x[1] = (A[0] * b[1] - b[0] * A[2]) / det; 329 | return true; 330 | } 331 | } -------------------------------------------------------------------------------- /src/Feature/OCExtractor.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * FALKOLib - Fast Adaptive Laser Keypoint Orientation-invariant 3 | * Copyright (C) 2016 Fabjan Kallasi and Dario Lodi Rizzini. 4 | * 5 | * This file is part of FALKOLib. 6 | * 7 | * FALKOLib is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU Lesser General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * FALKOLib is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU Lesser General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU Lesser General Public License 18 | * along with FALKOLib. If not, see . 19 | */ 20 | #include 21 | #include 22 | 23 | using namespace std; 24 | 25 | namespace falkolib { 26 | 27 | OCExtractor::OCExtractor() 28 | //: houghSpectrum((M_PI / 180.0) * 0.5, 0.02, 10.0) { 29 | { 30 | tol = 0.1; 31 | angleRes = (M_PI / 180.0) * 0.5; 32 | rangeRes = 0.02; 33 | rangeMax = 10.0; 34 | nmsRadius = 0.1; 35 | neighA = 0.1; 36 | neighB = 0.07; 37 | neighMinPoint = 2; 38 | houghSpectrum.init(angleRes, rangeRes, rangeMax); 39 | } 40 | 41 | void OCExtractor::extract(const LaserScan& scan, std::vector& keypoints) { 42 | std::vector pointRot; 43 | std::vector keypointCandidates; 44 | std::vector scores; 45 | std::vector maxima; 46 | OC keypoint; 47 | double angle, score; 48 | int win; 49 | Eigen::Affine2d rot2orth, rot2inve; 50 | OC kp; 51 | 52 | // Computes dominant orientation angle and rotates the points 53 | angle = computeDominantAngle(scan.points); 54 | rotatePoints(scan.points, -angle, pointRot); 55 | // std::cout << "Rotated " << scan.points.size() << " by angle " << (180.0 / M_PI * angle) << " deg" << std::endl; 56 | rot2orth = Eigen::Affine2d::Identity(); 57 | rot2orth.prerotate(angle); 58 | rot2inve = rot2orth.inverse(); 59 | 60 | // For each point, computes the corresponding corner and score 61 | keypointCandidates.reserve(pointRot.size()); 62 | scores.reserve(pointRot.size()); 63 | for (int idx = 0; idx < pointRot.size(); ++idx) { 64 | score = computeCornerScore(pointRot, idx, keypoint); 65 | keypointCandidates.push_back(keypoint); 66 | scores.push_back(score); 67 | } 68 | 69 | // Extracts the maxima using non-maxima suppression (NMS) 70 | // assert(scores.size() == scan.getNumBeams(); 71 | NMSKeypoint(scores, scan, 0, scan.getNumBeams(), nmsRadius, 1.0, maxima); 72 | 73 | keypoints.clear(); 74 | keypoints.reserve(maxima.size()); 75 | for (auto& idx : maxima) { 76 | kp = keypointCandidates[idx]; 77 | kp.point = rot2orth * keypointCandidates[idx].point; 78 | double a = keypointCandidates[idx].orientation + angle; 79 | kp.orientation = atan2(sin(a), cos(a)); 80 | keypoints.push_back(kp); 81 | } 82 | } 83 | 84 | void OCExtractor::rotatePoints(const std::vector& pointsIn, double angle, std::vector& pointsOut) { 85 | Point2d p; 86 | double ct = cos(angle); 87 | double st = sin(angle); 88 | 89 | pointsOut.clear(); 90 | pointsOut.reserve(pointsIn.size()); 91 | for (int i = 0; i < pointsIn.size(); ++i) { 92 | p.x() = pointsIn[i].x() * ct - pointsIn[i].y() * st; 93 | p.y() = pointsIn[i].x() * st + pointsIn[i].y() * ct; 94 | pointsOut.push_back(p); 95 | } 96 | } 97 | 98 | double OCExtractor::computeDominantAngle(const std::vector& points) { 99 | int maxIdx; 100 | double angle; 101 | 102 | houghSpectrum.insertPoint(points.begin(), points.end()); 103 | houghSpectrum.orthoSpectrum().maxCoeff(&maxIdx); 104 | angle = angleRes * maxIdx; 105 | return angle; 106 | } 107 | 108 | double OCExtractor::computeCornerScore(const std::vector& pointsIn, int index, OC& keypoint) { 109 | vector neigh; 110 | double range, distNeigh; 111 | double score, dx, dy; 112 | int midIndex, orientationMax; 113 | int neighSize, neighSizeL, neighSizeR; 114 | int xnum, ynum; 115 | int orientationBin[4] = {0, 0, 0, 0}; 116 | 117 | // Computes the neighborhood 118 | neigh.clear(); 119 | getNeighPoints(pointsIn, index, neigh, midIndex, distNeigh); 120 | 121 | // Check if it current point is in the center of the neighbor interval 122 | neighSize = neigh.size(); 123 | neighSizeL = midIndex; 124 | neighSizeR = neighSize - midIndex - 1; 125 | // std::cout << __FILE__ << "," << __LINE__ << ": distNeigh " << distNeigh << ", neighSize " << neighSize << ", neighSizeL " << neighSizeL << ", neighSizeR " << neighSizeR << std::endl; 126 | if (neighSizeL < neighMinPoint || neighSizeR < neighMinPoint) { 127 | // std::cout << " unbalanced neighborhood: distNeigh " << distNeigh << ", neighSize " << neighSize << ", neighSizeL " << neighSizeL << ", neighSizeR " << neighSizeR << ", neighMinPoint " << neighMinPoint << std::endl; 128 | return 0.0; 129 | } 130 | 131 | // Check that the neighborhood has a triangular shape rather than a flat one 132 | // double triangleBLength = pointsDistance(neigh.front(), neigh.back()); 133 | // double triangleHLength = std::abs(signedTriangleArea(neigh[midIndex], neigh.front(), neigh.back())) / triangleBLength; 134 | // std::cout << __FILE__ << "," << __LINE__ << ": triangleBLength " << triangleBLength << " > " << (distNeigh / bRatio) << ", triangleHLength " << triangleHLength << ", (distNeigh / bRatio) " << (distNeigh / bRatio) << std::endl; 135 | // if (triangleBLength < (distNeigh / bRatio) || triangleHLength < (distNeigh / bRatio)) { 136 | // return 0.0; 137 | // } 138 | 139 | // Computes the keypoint/corner 140 | keypoint.index = index; 141 | keypoint.point.x() = 0.0; 142 | keypoint.point.y() = 0.0; 143 | keypoint.index = index; 144 | keypoint.radius = distNeigh; 145 | xnum = 0; 146 | ynum = 0; 147 | for (auto& np : neigh) { 148 | dx = np.x() - pointsIn[index].x(); 149 | dy = np.y() - pointsIn[index].y(); 150 | // a. Neighboor pointIn[i] has similar x coordinate and different y coordinate. 151 | // Condition std::abs(dy) < distNeigh means that is inside circle with radius distNeigh. 152 | // b. Neighboor pointIn[i] has similar y coordinate and different x coordinate. 153 | // Condition std::abs(dx) < distNeigh means that is inside circle with radius distNeigh. 154 | if (std::abs(dx) < tol && tol < std::abs(dy) && std::abs(dy) < distNeigh) { 155 | xnum++; 156 | keypoint.point.x() += (np.x() - keypoint.point.x()) / xnum; 157 | if (dy > 0) { 158 | orientationBin[NE]++; // NE 159 | orientationBin[NW]++; // NW 160 | } else { 161 | orientationBin[SW]++; // SW 162 | orientationBin[SE]++; // SE 163 | } 164 | } 165 | if (std::abs(dy) < tol && tol < std::abs(dx) && std::abs(dx) < distNeigh) { 166 | ynum++; 167 | keypoint.point.y() += (np.y() - keypoint.point.y()) / ynum; 168 | if (dx > 0) { 169 | orientationBin[NE]++; // NE 170 | orientationBin[SE]++; // SE 171 | } else { 172 | orientationBin[NW]++; // NW 173 | orientationBin[SW]++; // SW 174 | } 175 | } 176 | // std::cout << " index " << index << " dx " << dx << ", dy " << dy << ", xnum " << xnum << ", ynum " << ynum << "\n"; 177 | } 178 | if (xnum == 0) { 179 | keypoint.point.x() = pointsIn[index].x(); 180 | } 181 | if (ynum == 0) { 182 | keypoint.point.y() = pointsIn[index].y(); 183 | } 184 | 185 | score = 1.0 * (xnum + ynum) / (0.1 + std::abs(xnum - ynum)); 186 | // if (score > 1.0 || index == 1197 || (1304 <= index && index <= 1305) || (117 <= index && index <= 119)) { 187 | // std::cout << __FILE__ << "," << __LINE__ << "\n index " << index << ", score " << score << ", xnum " << xnum << ", ynum " << ynum 188 | // << "\n neighSize " << neighSize << ", neighSizeL " << neighSizeL << ", neighSizeR " << neighSizeR << ", neighMinPoint " << neighMinPoint << std::endl; 189 | // } 190 | 191 | // Computes keypoint orientation according to maximum of orientationBin[] histogram: 192 | // * orientationBin[0]: I quadrant, 45 deg 193 | // * orientationBin[1]: II quadrant, 135 deg 194 | // * orientationBin[2]: III quadrant, 225 deg (or -135 deg) 195 | // * orientationBin[3]: IV quadrant, 315 deg (or -45 deg) 196 | orientationMax = 0; 197 | for (int i = 1; i < 4; ++i) { 198 | if (orientationBin[i] > orientationBin[orientationMax]) { 199 | orientationMax = i; 200 | } 201 | } 202 | keypoint.orientation = M_PI * (0.25 + 0.5 * orientationMax); 203 | keypoint.orientation = atan2(sin(keypoint.orientation), cos(keypoint.orientation)); 204 | return score; 205 | } 206 | 207 | void OCExtractor::getNeighPoints(const std::vector& pointsIn, int index, std::vector& neigh, int& midIndex, double& dist) const { 208 | double range; 209 | int imin, imax, win; 210 | 211 | neigh.clear(); 212 | range = pointsIn[index].norm(); 213 | dist = neighA * exp(neighB * range); 214 | if (range > dist) { 215 | win = (int) ceil(asin(dist / range) / angleRes); 216 | } else { 217 | win = pointsIn.size(); 218 | } 219 | imin = std::max(index - win, 0); 220 | imax = std::min(index + win + 1, (int) pointsIn.size()); 221 | for (int i = imin; i < imax; ++i) { 222 | if ((pointsIn[i] - pointsIn[index]).norm() < dist) { 223 | neigh.push_back(pointsIn[i]); 224 | if (i == index) { 225 | midIndex = neigh.size(); 226 | } 227 | } 228 | } 229 | } 230 | 231 | void OCExtractor::NMSKeypoint(const std::vector& scores, const LaserScan& scan, unsigned int ibeg, unsigned int iend, double radius, int minval, std::vector& peaks) { 232 | unsigned i, imax; 233 | unsigned j, jbeg, jend, jmax; 234 | std::vector candidates; 235 | peaks.clear(); 236 | 237 | i = ibeg; 238 | imax = ibeg; 239 | while (i < iend) { 240 | int win; 241 | if (radius >= scan.ranges[i]) { 242 | win = std::floor(std::asin(0.8) / scan.getAngleInc()); 243 | } else { 244 | win = std::floor(std::asin(radius / scan.ranges[i]) / scan.getAngleInc()); 245 | } 246 | 247 | jmax = i; 248 | 249 | if (imax + win < i) { 250 | jbeg = (i >= win ? i - win : 0); 251 | imax = i; 252 | } else { 253 | jbeg = i; 254 | } 255 | 256 | jend = (i + win + 1 < iend ? i + win + 1 : iend); 257 | for (j = jbeg; j < jend; ++j) { 258 | if (scores[j] > scores[jmax]) { 259 | jmax = j; 260 | } 261 | } 262 | imax = (scores[jmax] >= scores[imax] ? jmax : imax); 263 | 264 | if (i == imax && scores[i] > minval) { 265 | candidates.push_back(i); 266 | } 267 | if (jmax > i) i = jmax; 268 | else ++i; 269 | } 270 | 271 | int i1 = 0; 272 | int i2 = 0; 273 | int counter = 0; 274 | 275 | for (i1 = 0, i2 = 0; i1 < candidates.size(); ++i1) { 276 | if (scores[candidates[i2]] == scores[candidates[i1]]) { 277 | counter++; 278 | if (2 * abs(i2 - i1) > counter) { 279 | ++i2; 280 | } 281 | } else { 282 | peaks.push_back(candidates[i2]); 283 | i2 = i1; 284 | counter = 0; 285 | } 286 | } 287 | if (i2 != candidates.size()) { 288 | peaks.push_back(candidates[i2]); 289 | } 290 | } 291 | 292 | } 293 | 294 | -------------------------------------------------------------------------------- /test/testData.h: -------------------------------------------------------------------------------- 1 | /** 2 | * FALKOLib - Fast Adaptive Laser Keypoint Orientation-invariant 3 | * Copyright (C) 2016 Fabjan Kallasi and Dario Lodi Rizzini. 4 | * 5 | * This file is part of FALKOLib. 6 | * 7 | * FALKOLib is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU Lesser General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * FALKOLib is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU Lesser General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU Lesser General Public License 18 | * along with FALKOLib. If not, see . 19 | */ 20 | #pragma once 21 | 22 | extern double testRanges[]; 23 | 24 | extern double testRanges2[]; 25 | 26 | extern double testRangesOrtho1[]; 27 | 28 | extern double testRangesOrtho2[]; 29 | -------------------------------------------------------------------------------- /test/testFalkoAHT.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * FALKOLib - Fast Adaptive Laser Keypoint Orientation-invariant 3 | * Copyright (C) 2016 Fabjan Kallasi and Dario Lodi Rizzini. 4 | * 5 | * This file is part of FALKOLib. 6 | * 7 | * FALKOLib is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU Lesser General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * FALKOLib is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU Lesser General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU Lesser General Public License 18 | * along with FALKOLib. If not, see . 19 | */ 20 | #include 21 | 22 | 23 | #include 24 | #include 25 | #include 26 | 27 | #include 28 | 29 | #include 30 | #include 31 | 32 | #include "testData.h" 33 | 34 | using namespace std; 35 | using namespace falkolib; 36 | 37 | int main(int argc, char** argv) { 38 | LaserScan scan1(-0.003316126, 2.0 * M_PI, 1440); 39 | scan1.fromRanges(testRanges); 40 | LaserScan scan2(-0.003316126, 2.0 * M_PI, 1440); 41 | scan2.fromRanges(testRanges2); 42 | 43 | FALKOExtractor fe; 44 | fe.setMinExtractionRange(0); 45 | fe.setMaxExtractionRange(30); 46 | fe.enableSubbeam(true); 47 | fe.setNMSRadius(0.1); 48 | fe.setNeighB(0.07); 49 | fe.setBRatio(2.5); 50 | fe.setGridSectors(16); 51 | 52 | std::vector keypoints1; 53 | std::vector keypoints2; 54 | 55 | 56 | fe.extract(scan1, keypoints1); 57 | fe.extract(scan2, keypoints2); 58 | 59 | cout << "num keypoints1 extracted: " << keypoints1.size() << endl; 60 | cout << "num keypoints2 extracted: " << keypoints2.size() << endl; 61 | 62 | BSCExtractor bsc(16, 8); 63 | vector bscDesc1; 64 | vector bscDesc2; 65 | 66 | 67 | bsc.compute(scan1, keypoints1, bscDesc1); 68 | bsc.compute(scan2, keypoints2, bscDesc2); 69 | 70 | 71 | 72 | 73 | NNMatcher matcher; 74 | matcher.setDistanceThreshold(0.1); 75 | std::vector > assoNN; 76 | std::cout << "num matching NN: " << matcher.match(keypoints1, keypoints2, assoNN) << endl; 77 | for (auto& match : assoNN) { 78 | if (match.second >= 0) { 79 | int i1 = match.first; 80 | int i2 = match.second; 81 | std::cout << "i1: " << i1 << "\ti2: " << i2 << "\t keypoints distance: " << (keypoints1[i1].distance(keypoints2[i2])) << endl; 82 | } 83 | } 84 | 85 | cout << endl; 86 | AHTMatcher aht(0.01, 0.01, 0.001, 0.2, 0.2, 0.05); 87 | aht.setDistanceThreshold(0.1); 88 | std::vector > assoAHT; 89 | std::cout << "num matching AHT: " << aht.match(keypoints1, keypoints2, assoAHT) << endl; 90 | for (auto& match : assoAHT) { 91 | if (match.second >= 0) { 92 | int i1 = match.first; 93 | int i2 = match.second; 94 | std::cout << "i1: " << i1 << "\ti2: " << i2 << "\t keypoints distance: " << (keypoints1[i1].distance(keypoints2[i2])) << endl; 95 | } 96 | } 97 | 98 | Eigen::Affine2d transformNN; 99 | Eigen::Affine2d transformAHT; 100 | computeTransform(keypoints1, keypoints2, assoNN, transformNN); 101 | computeTransform(keypoints1, keypoints2, assoAHT, transformAHT); 102 | 103 | std::cout << "NN transform: " << std::endl; 104 | std::cout << transformNN.inverse().matrix() << std::endl; 105 | std::cout << "AHT transform: " << std::endl; 106 | std::cout << transformAHT.inverse().matrix() << std::endl; 107 | 108 | return 0; 109 | } 110 | 111 | -------------------------------------------------------------------------------- /test/testFalkoCC.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * FALKOLib - Fast Adaptive Laser Keypoint Orientation-invariant 3 | * Copyright (C) 2016 Fabjan Kallasi and Dario Lodi Rizzini. 4 | * 5 | * This file is part of FALKOLib. 6 | * 7 | * FALKOLib is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU Lesser General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * FALKOLib is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU Lesser General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU Lesser General Public License 18 | * along with FALKOLib. If not, see . 19 | */ 20 | #include 21 | 22 | 23 | #include 24 | #include 25 | #include 26 | 27 | #include 28 | 29 | #include 30 | #include 31 | 32 | #include "testData.h" 33 | 34 | using namespace std; 35 | using namespace falkolib; 36 | 37 | int main(int argc, char** argv) { 38 | LaserScan scan1(-0.003316126, 2.0 * M_PI, 1440); 39 | scan1.fromRanges(testRanges); 40 | LaserScan scan2(-0.003316126, 2.0 * M_PI, 1440); 41 | scan2.fromRanges(testRanges2); 42 | 43 | FALKOExtractor fe; 44 | fe.setMinExtractionRange(0); 45 | fe.setMaxExtractionRange(30); 46 | fe.enableSubbeam(true); 47 | fe.setNMSRadius(0.1); 48 | fe.setNeighB(0.07); 49 | fe.setBRatio(2.5); 50 | fe.setGridSectors(16); 51 | 52 | std::vector keypoints1; 53 | std::vector keypoints2; 54 | 55 | 56 | fe.extract(scan1, keypoints1); 57 | fe.extract(scan2, keypoints2); 58 | 59 | cout << "num keypoints1 extracted: " << keypoints1.size() << endl; 60 | cout << "num keypoints2 extracted: " << keypoints2.size() << endl; 61 | 62 | BSCExtractor bsc(16, 8); 63 | vector bscDesc1; 64 | vector bscDesc2; 65 | 66 | 67 | bsc.compute(scan1, keypoints1, bscDesc1); 68 | bsc.compute(scan2, keypoints2, bscDesc2); 69 | 70 | 71 | 72 | 73 | NNMatcher matcher; 74 | matcher.setDistanceThreshold(0.1); 75 | std::vector > assoNN; 76 | std::cout << "num matching NN: " << matcher.match(keypoints1, keypoints2, assoNN) << endl; 77 | for (auto& match : assoNN) { 78 | if (match.second >= 0) { 79 | int i1 = match.first; 80 | int i2 = match.second; 81 | std::cout << "i1: " << i1 << "\ti2: " << i2 << "\t keypoints distance: " << (keypoints1[i1].distance(keypoints2[i2])) << endl; 82 | } 83 | } 84 | 85 | cout << endl; 86 | CCDAMatcher cc; 87 | cc.setDistMin(0.05); 88 | cc.setDistTol(0.1); 89 | std::vector > assoCC; 90 | std::cout << "num matching CC: " << cc.match(keypoints1, keypoints2, assoCC) << endl; 91 | for (auto& match : assoCC) { 92 | if (match.second >= 0) { 93 | int i1 = match.first; 94 | int i2 = match.second; 95 | std::cout << "i1: " << i1 << "\ti2: " << i2 << "\t keypoints distance: " << (keypoints1[i1].distance(keypoints2[i2])) << endl; 96 | } 97 | } 98 | 99 | // Eigen::Affine2d transformNN; 100 | // Eigen::Affine2d transformCC; 101 | // computeTransform(keypoints1, keypoints2, assoNN, transformNN); 102 | // computeTransform(keypoints1, keypoints2, assoCC, transformCC); 103 | // 104 | // std::cout << "NN transform: " << std::endl; 105 | // std::cout << transformNN.inverse().matrix() << std::endl; 106 | // std::cout << "AHT transform: " << std::endl; 107 | // std::cout << transformCC.inverse().matrix() << std::endl; 108 | 109 | return 0; 110 | } 111 | 112 | -------------------------------------------------------------------------------- /test/testKeypointFalko.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * FALKOLib - Fast Adaptive Laser Keypoint Orientation-invariant 3 | * Copyright (C) 2016 Fabjan Kallasi and Dario Lodi Rizzini. 4 | * 5 | * This file is part of FALKOLib. 6 | * 7 | * FALKOLib is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU Lesser General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * FALKOLib is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU Lesser General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU Lesser General Public License 18 | * along with FALKOLib. If not, see . 19 | */ 20 | #include 21 | 22 | 23 | #include 24 | #include 25 | #include 26 | #include 27 | 28 | #include 29 | #include 30 | 31 | #include 32 | #include 33 | 34 | #include "testData.h" 35 | 36 | using namespace std; 37 | using namespace falkolib; 38 | 39 | int main(int argc, char** argv) { 40 | FALKOExtractor fe; 41 | fe.setMinExtractionRange(1); 42 | fe.setMaxExtractionRange(30); 43 | fe.enableSubbeam(true); 44 | fe.setNMSRadius(0.1); 45 | fe.setNeighB(0.07); 46 | fe.setBRatio(2.5); 47 | fe.setGridSectors(16); 48 | 49 | LaserScan scan1(-0.003316126, 2.0 * M_PI, 1440); 50 | scan1.fromRanges(testRanges); 51 | LaserScan scan2(-0.003316126, 2.0 * M_PI, 1440); 52 | scan2.fromRanges(testRanges2); 53 | 54 | std::vector keypoints1; 55 | std::vector keypoints2; 56 | 57 | 58 | fe.extract(scan1, keypoints1); 59 | fe.extract(scan2, keypoints2); 60 | 61 | cout << "num keypoints1 extracted: " << keypoints1.size() << endl; 62 | cout << "num keypoints2 extracted: " << keypoints2.size() << endl; 63 | 64 | CGHExtractor cgh(16); 65 | BSCExtractor bsc(16, 8); 66 | 67 | vector cghDesc1; 68 | vector cghDesc2; 69 | vector bscDesc1; 70 | vector bscDesc2; 71 | 72 | 73 | cgh.compute(scan1, keypoints1, cghDesc1); 74 | bsc.compute(scan1, keypoints1, bscDesc1); 75 | cgh.compute(scan2, keypoints2, cghDesc2); 76 | bsc.compute(scan2, keypoints2, bscDesc2); 77 | 78 | 79 | 80 | cout << endl; 81 | NNMatcher matcher; 82 | matcher.setDistanceThreshold(0.1); 83 | std::vector > asso; 84 | std::cout << "num matching NN: " << matcher.match(keypoints1, keypoints2, asso) << endl; 85 | for (auto& match : asso) { 86 | if (match.second >= 0) { 87 | int i1 = match.first; 88 | int i2 = match.second; 89 | std::cout << "i1: " << i1 << "\ti2: " << i2 << "\t keypoints distance: " << (keypoints1[i1].distance(keypoints2[i2])) << "\t CHG Distance: " << (cghDesc1[i1].distance(cghDesc2[i2])) << "\t BSC Distance: " << (bscDesc1[i1].distance(bscDesc2[i2])) << endl; 90 | } 91 | } 92 | 93 | cout << endl; 94 | NNMatcher matcherFALKOBSC; 95 | matcherFALKOBSC.setDistanceThreshold(0.1); 96 | matcherFALKOBSC.setDescriptorThreshold(15); 97 | std::cout << "num matching NN FALKO BSC: " << matcherFALKOBSC.match(keypoints1, bscDesc1, keypoints2, bscDesc2, asso) << endl; 98 | for (auto& match : asso) { 99 | if (match.second >= 0) { 100 | int i1 = match.first; 101 | int i2 = match.second; 102 | std::cout << "i1: " << i1 << "\ti2: " << i2 << "\t keypoints distance: " << (keypoints1[i1].distance(keypoints2[i2])) << "\t CHG Distance: " << (cghDesc1[i1].distance(cghDesc2[i2])) << "\t BSC Distance: " << (bscDesc1[i1].distance(bscDesc2[i2])) << endl; 103 | } 104 | } 105 | 106 | cout << endl; 107 | NNMatcher matcherFALKOCGH; 108 | matcherFALKOCGH.setDistanceThreshold(0.1); 109 | matcherFALKOCGH.setDescriptorThreshold(0.2); 110 | std::cout << "num matching NN FALKO CGH: " << matcherFALKOCGH.match(keypoints1, cghDesc1, keypoints2, cghDesc2, asso) << endl; 111 | for (auto& match : asso) { 112 | if (match.second >= 0) { 113 | int i1 = match.first; 114 | int i2 = match.second; 115 | std::cout << "i1: " << i1 << "\ti2: " << i2 << "\t keypoints distance: " << (keypoints1[i1].distance(keypoints2[i2])) << "\t CHG Distance: " << (cghDesc1[i1].distance(cghDesc2[i2])) << "\t BSC Distance: " << (bscDesc1[i1].distance(bscDesc2[i2])) << endl; 116 | } 117 | } 118 | 119 | cout << endl; 120 | NNMatcher matcherBSC; 121 | matcherBSC.setDistanceThreshold(15); 122 | std::cout << "num matching NN BSC: " << matcherBSC.match(bscDesc1, bscDesc2, asso) << endl; 123 | for (auto& match : asso) { 124 | if (match.second >= 0) { 125 | int i1 = match.first; 126 | int i2 = match.second; 127 | std::cout << "i1: " << i1 << "\ti2: " << i2 << "\t keypoints distance: " << (keypoints1[i1].distance(keypoints2[i2])) << "\t CHG Distance: " << (cghDesc1[i1].distance(cghDesc2[i2])) << "\t BSC Distance: " << (bscDesc1[i1].distance(bscDesc2[i2])) << endl; 128 | } 129 | } 130 | cout << endl; 131 | NNMatcher matcherCGH; 132 | matcherCGH.setDistanceThreshold(0.2); 133 | std::cout << "num matching NN CGH: " << matcherCGH.match(cghDesc1, cghDesc2, asso) << endl; 134 | for (auto& match : asso) { 135 | if (match.second >= 0) { 136 | int i1 = match.first; 137 | int i2 = match.second; 138 | std::cout << "i1: " << i1 << "\ti2: " << i2 << "\t keypoints distance: " << (keypoints1[i1].distance(keypoints2[i2])) << "\t CHG Distance: " << (cghDesc1[i1].distance(cghDesc2[i2])) << "\t BSC Distance: " << (bscDesc1[i1].distance(bscDesc2[i2])) << endl; 139 | } 140 | } 141 | 142 | AHTMatcher aht; 143 | 144 | 145 | return 0; 146 | } 147 | 148 | -------------------------------------------------------------------------------- /test/testKeypointOC.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | /** 3 | * FALKOLib - Fast Adaptive Laser Keypoint Orientation-invariant 4 | * Copyright (C) 2016 Fabjan Kallasi and Dario Lodi Rizzini. 5 | * 6 | * This file is part of FALKOLib. 7 | * 8 | * FALKOLib is free software: you can redistribute it and/or modify 9 | * it under the terms of the GNU Lesser General Public License as published by 10 | * the Free Software Foundation, either version 3 of the License, or 11 | * (at your option) any later version. 12 | * 13 | * FALKOLib is distributed in the hope that it will be useful, 14 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | * GNU Lesser General Public License for more details. 17 | * 18 | * You should have received a copy of the GNU Lesser General Public License 19 | * along with FALKOLib. If not, see . 20 | */ 21 | #include 22 | #include 23 | #include 24 | 25 | #include 26 | #include 27 | #include 28 | 29 | #include 30 | #include 31 | 32 | #include "testData.h" 33 | 34 | //#ifdef FEATURELIB_DEBUG 35 | //#include 36 | //#endif 37 | 38 | using namespace std; 39 | using namespace falkolib; 40 | 41 | int main(int argc, char** argv) { 42 | OCExtractor oe; 43 | oe.setAngleRes(M_PI / 180.0 * 0.25); 44 | oe.setTol(0.1); 45 | oe.setRangeRes(0.05); 46 | oe.setRangeMax(10.0); 47 | oe.setNMSRadius(0.3); 48 | oe.setNeighMinPoint(2); 49 | 50 | LaserScan scan1(-0.003316126, 2.0 * M_PI, 1440); 51 | scan1.fromRanges(testRangesOrtho1); 52 | LaserScan scan2(-0.003316126, 2.0 * M_PI, 1440); 53 | scan2.fromRanges(testRangesOrtho2); 54 | 55 | std::vector keypoints1; 56 | std::vector keypoints2; 57 | 58 | std::cout << "extract OC keypoints: " << std::endl; 59 | oe.extract(scan1, keypoints1); 60 | oe.extract(scan2, keypoints2); 61 | 62 | cout << "num keypoints1 extracted: " << keypoints1.size() << endl; 63 | for (auto& kp : keypoints1) { 64 | std::cout << kp.point.transpose() << ", index " << kp.index << ", radius " << kp.radius << ", orientation [deg] " << (180.0 / M_PI * kp.orientation) << std::endl; 65 | } 66 | 67 | cout << "num keypoints2 extracted: " << keypoints2.size() << endl; 68 | for (auto& kp : keypoints2) { 69 | std::cout << kp.point.transpose() << ", index " << kp.index << ", radius " << kp.radius << ", orientation [deg] " << (180.0 / M_PI * kp.orientation) << std::endl; 70 | } 71 | 72 | //#ifdef FEATURELIB_DEBUG 73 | // Gnuplot gp("gnuplot -persist"); 74 | // gp << "set size ratio -1\n"; 75 | // for (int i = 0; i < scan1.points.size(); i += 10) { 76 | // gp << "set label \"" << i << "\" at " << scan1.points[i].x() << "," << scan1.points[i].y() << "\n"; 77 | // } 78 | // gp << "plot '-' u 1:2 notitle pt 7 w p, '-' u 1:2 notitle pt 7 w p\n"; 79 | // for (auto& p : scan1.points) { 80 | // gp << p.x() << " " << p.y() << "\n"; 81 | // } 82 | // gp << "e" << std::endl; 83 | // for (auto& kp : keypoints1) { 84 | // gp << kp.point.x() << " " << kp.point.y() << "\n"; 85 | // } 86 | // gp << "e" << std::endl; 87 | //#endif 88 | 89 | CGHExtractor cgh(16); 90 | BSCExtractor bsc(16, 8); 91 | 92 | vector cghDesc1; 93 | vector cghDesc2; 94 | vector bscDesc1; 95 | vector bscDesc2; 96 | 97 | 98 | cgh.compute(scan1, keypoints1, cghDesc1); 99 | bsc.compute(scan1, keypoints1, bscDesc1); 100 | cgh.compute(scan2, keypoints2, cghDesc2); 101 | bsc.compute(scan2, keypoints2, bscDesc2); 102 | 103 | return 0; 104 | } 105 | 106 | --------------------------------------------------------------------------------