├── CMakeLists.txt ├── ConfigMap.cpp ├── ConfigMap.hpp ├── LICENSE ├── README.md ├── StringTokenizer.cpp ├── StringTokenizer.hpp ├── bunny.png ├── config_example.txt ├── demo ├── config.txt ├── data_bunny.txt ├── data_rand.txt ├── demo.m ├── model_bunny.txt ├── model_rand.txt ├── output.txt └── readpoints.m ├── jly_3ddt.cpp ├── jly_3ddt.h ├── jly_goicp.cpp ├── jly_goicp.h ├── jly_icp3d.hpp ├── jly_main.cpp ├── jly_sorting.hpp ├── matrix.cpp ├── matrix.h └── nanoflann.hpp /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | project(GoICP) 2 | 3 | cmake_minimum_required(VERSION 2.8 FATAL_ERROR) 4 | 5 | add_executable(GoICP 6 | jly_main.cpp 7 | jly_goicp.cpp 8 | jly_3ddt.cpp 9 | matrix.cpp 10 | ConfigMap.cpp 11 | StringTokenizer.cpp 12 | ) 13 | -------------------------------------------------------------------------------- /ConfigMap.cpp: -------------------------------------------------------------------------------- 1 | #include "ConfigMap.hpp" 2 | 3 | ConfigMap::ConfigMap() { 4 | }; 5 | 6 | ConfigMap::ConfigMap(const char * config_file) { 7 | 8 | std::ifstream fin(config_file); 9 | 10 | if (!fin.is_open()) { 11 | std::cout << "Unable to open config file '" << config_file << "'" << std::endl; 12 | exit(-2); 13 | } 14 | else { 15 | 16 | std::string buffer; 17 | 18 | while(std::getline(fin, buffer)) { 19 | // Ignore comments 20 | if(buffer.c_str()[0] == '#') { 21 | } 22 | else { 23 | this->addLine(buffer); 24 | } 25 | } 26 | 27 | fin.close(); 28 | } 29 | }; 30 | 31 | ConfigMap::~ConfigMap() { 32 | 33 | for (std::list::iterator iter = allocated_memory_collector.begin(); 34 | iter != allocated_memory_collector.end(); iter++) { 35 | delete [] (*iter); 36 | } 37 | }; 38 | 39 | void ConfigMap::addLine(std::string line_) { 40 | 41 | // Swallow last character (carriage return: ASCII 13) 42 | if(line_.size() > 0) 43 | { 44 | if((int)line_.c_str()[line_.size() - 1] == 13) 45 | { 46 | line_.resize (line_.size () - 1); 47 | } 48 | } 49 | 50 | StringTokenizer st(line_, const_cast(" =;")); 51 | 52 | if (st.numberOfTokens() != 2) { 53 | return; 54 | } 55 | 56 | std::string key = st.nextToken(); 57 | std::string val = st.nextToken(); 58 | 59 | addPair(key, val); 60 | }; 61 | 62 | void ConfigMap::addPair(std::string key_, std::string value_) { 63 | 64 | mappings[key_] = value_; 65 | 66 | }; 67 | 68 | char * ConfigMap::get(char * key_) { 69 | 70 | std::string key(key_); 71 | 72 | #ifdef VERBOSE 73 | std::cout << "DEBUG::ConfigMap.get()::key is `" << key_ << "'" << std::endl; 74 | 75 | std::string val = mappings[key]; 76 | std::cout << "DEBUG::Requesting (" << key_ << ")->(" << val << ")" << std::endl; 77 | 78 | return const_cast(val.c_str()); 79 | #else 80 | return const_cast(mappings[key].c_str()); 81 | #endif 82 | 83 | }; 84 | 85 | char * ConfigMap::get(const char * key_) { 86 | return get(const_cast(key_)); 87 | }; 88 | 89 | int ConfigMap::getI(char * key_) { 90 | 91 | char * str_val = get(key_); 92 | 93 | if (str_val == NULL) { 94 | return 0; 95 | } 96 | else { 97 | return atoi(str_val); 98 | } 99 | }; 100 | 101 | int ConfigMap::getI(const char * key_) { 102 | return getI(const_cast(key_)); 103 | }; 104 | 105 | double ConfigMap::getF(char *key_) { 106 | 107 | char * str_val = get(key_); 108 | 109 | if (str_val == NULL) { 110 | return 0; 111 | } else { 112 | return atof(str_val); 113 | } 114 | }; 115 | 116 | double ConfigMap::getF(const char * key_) { 117 | return getF(const_cast(key_)); 118 | }; 119 | 120 | float * ConfigMap::getVector(char * key_) { 121 | 122 | std::string key(key_); 123 | std::string val = mappings[key]; 124 | 125 | if (val.empty()) { 126 | return NULL; 127 | } 128 | 129 | StringTokenizer st(val, const_cast("(,)")); 130 | 131 | float * return_vector = new float[st.numberOfTokens()]; 132 | allocated_memory_collector.push_back(return_vector); 133 | 134 | for (int i = 0; st.numberOfTokens() > 0; i++) { 135 | return_vector[i] = (float)atof(st.nextToken().c_str()); 136 | } 137 | 138 | return return_vector; 139 | }; 140 | 141 | float * ConfigMap::getVector(const char * key_) { 142 | return getVector(const_cast(key_)); 143 | }; 144 | 145 | void ConfigMap::print() { 146 | 147 | for (std::map::const_iterator iter = mappings.begin(); iter != mappings.end(); iter++) 148 | { 149 | std::cout << "(" << iter->first << ")->(" << iter->second << ")" << std::endl; 150 | } 151 | }; 152 | -------------------------------------------------------------------------------- /ConfigMap.hpp: -------------------------------------------------------------------------------- 1 | 2 | #ifndef CONFIGMAP_H 3 | #define CONFIGMAP_H 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #include "StringTokenizer.hpp" 13 | 14 | class ConfigMap { 15 | 16 | private: 17 | std::map mappings; 18 | std::list allocated_memory_collector; 19 | 20 | public: 21 | ConfigMap(); 22 | ConfigMap(const char * config_file); 23 | ~ConfigMap(); 24 | 25 | // adds a config line to the map 26 | // (auto handles tokenising around '=' character) 27 | void addLine(std::string line_); 28 | 29 | // adds a key->value mapping 30 | void addPair(std::string key_, std::string value_); 31 | 32 | // gets the string value at a given key 33 | char * get(char * key_); 34 | char * get(const char * key_); 35 | 36 | // gets the integer value at a given key 37 | // (uses the stdlib function 'atoi' to convert) 38 | int getI(char * key_); 39 | int getI(const char * key_); 40 | 41 | // gets the double value at a given key 42 | // (uses the stdlib function 'atof' to convert) 43 | double getF(char * key_); 44 | double getF(const char * key_); 45 | 46 | // gets a vector of floats at the given key 47 | float * getVector(char * key_); 48 | float * getVector(const char * key_); 49 | 50 | // prints the contents of the map list to stdout 51 | void print(); 52 | 53 | }; 54 | 55 | #endif // for CONFIGMAP_H 56 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | GNU GENERAL PUBLIC LICENSE 2 | Version 3, 29 June 2007 3 | 4 | Copyright (C) 2007 Free Software Foundation, Inc. 5 | Everyone is permitted to copy and distribute verbatim copies 6 | of this license document, but changing it is not allowed. 7 | 8 | Preamble 9 | 10 | The GNU General Public License is a free, copyleft license for 11 | software and other kinds of works. 12 | 13 | The licenses for most software and other practical works are designed 14 | to take away your freedom to share and change the works. By contrast, 15 | the GNU General Public License is intended to guarantee your freedom to 16 | share and change all versions of a program--to make sure it remains free 17 | software for all its users. We, the Free Software Foundation, use the 18 | GNU General Public License for most of our software; it applies also to 19 | any other work released this way by its authors. You can apply it to 20 | your programs, too. 21 | 22 | When we speak of free software, we are referring to freedom, not 23 | price. Our General Public Licenses are designed to make sure that you 24 | have the freedom to distribute copies of free software (and charge for 25 | them if you wish), that you receive source code or can get it if you 26 | want it, that you can change the software or use pieces of it in new 27 | free programs, and that you know you can do these things. 28 | 29 | To protect your rights, we need to prevent others from denying you 30 | these rights or asking you to surrender the rights. Therefore, you have 31 | certain responsibilities if you distribute copies of the software, or if 32 | you modify it: responsibilities to respect the freedom of others. 33 | 34 | For example, if you distribute copies of such a program, whether 35 | gratis or for a fee, you must pass on to the recipients the same 36 | freedoms that you received. You must make sure that they, too, receive 37 | or can get the source code. And you must show them these terms so they 38 | know their rights. 39 | 40 | Developers that use the GNU GPL protect your rights with two steps: 41 | (1) assert copyright on the software, and (2) offer you this License 42 | giving you legal permission to copy, distribute and/or modify it. 43 | 44 | For the developers' and authors' protection, the GPL clearly explains 45 | that there is no warranty for this free software. For both users' and 46 | authors' sake, the GPL requires that modified versions be marked as 47 | changed, so that their problems will not be attributed erroneously to 48 | authors of previous versions. 49 | 50 | Some devices are designed to deny users access to install or run 51 | modified versions of the software inside them, although the manufacturer 52 | can do so. This is fundamentally incompatible with the aim of 53 | protecting users' freedom to change the software. The systematic 54 | pattern of such abuse occurs in the area of products for individuals to 55 | use, which is precisely where it is most unacceptable. Therefore, we 56 | have designed this version of the GPL to prohibit the practice for those 57 | products. If such problems arise substantially in other domains, we 58 | stand ready to extend this provision to those domains in future versions 59 | of the GPL, as needed to protect the freedom of users. 60 | 61 | Finally, every program is threatened constantly by software patents. 62 | States should not allow patents to restrict development and use of 63 | software on general-purpose computers, but in those that do, we wish to 64 | avoid the special danger that patents applied to a free program could 65 | make it effectively proprietary. To prevent this, the GPL assures that 66 | patents cannot be used to render the program non-free. 67 | 68 | The precise terms and conditions for copying, distribution and 69 | modification follow. 70 | 71 | TERMS AND CONDITIONS 72 | 73 | 0. Definitions. 74 | 75 | "This License" refers to version 3 of the GNU General Public License. 76 | 77 | "Copyright" also means copyright-like laws that apply to other kinds of 78 | works, such as semiconductor masks. 79 | 80 | "The Program" refers to any copyrightable work licensed under this 81 | License. Each licensee is addressed as "you". "Licensees" and 82 | "recipients" may be individuals or organizations. 83 | 84 | To "modify" a work means to copy from or adapt all or part of the work 85 | in a fashion requiring copyright permission, other than the making of an 86 | exact copy. The resulting work is called a "modified version" of the 87 | earlier work or a work "based on" the earlier work. 88 | 89 | A "covered work" means either the unmodified Program or a work based 90 | on the Program. 91 | 92 | To "propagate" a work means to do anything with it that, without 93 | permission, would make you directly or secondarily liable for 94 | infringement under applicable copyright law, except executing it on a 95 | computer or modifying a private copy. Propagation includes copying, 96 | distribution (with or without modification), making available to the 97 | public, and in some countries other activities as well. 98 | 99 | To "convey" a work means any kind of propagation that enables other 100 | parties to make or receive copies. Mere interaction with a user through 101 | a computer network, with no transfer of a copy, is not conveying. 102 | 103 | An interactive user interface displays "Appropriate Legal Notices" 104 | to the extent that it includes a convenient and prominently visible 105 | feature that (1) displays an appropriate copyright notice, and (2) 106 | tells the user that there is no warranty for the work (except to the 107 | extent that warranties are provided), that licensees may convey the 108 | work under this License, and how to view a copy of this License. If 109 | the interface presents a list of user commands or options, such as a 110 | menu, a prominent item in the list meets this criterion. 111 | 112 | 1. Source Code. 113 | 114 | The "source code" for a work means the preferred form of the work 115 | for making modifications to it. "Object code" means any non-source 116 | form of a work. 117 | 118 | A "Standard Interface" means an interface that either is an official 119 | standard defined by a recognized standards body, or, in the case of 120 | interfaces specified for a particular programming language, one that 121 | is widely used among developers working in that language. 122 | 123 | The "System Libraries" of an executable work include anything, other 124 | than the work as a whole, that (a) is included in the normal form of 125 | packaging a Major Component, but which is not part of that Major 126 | Component, and (b) serves only to enable use of the work with that 127 | Major Component, or to implement a Standard Interface for which an 128 | implementation is available to the public in source code form. A 129 | "Major Component", in this context, means a major essential component 130 | (kernel, window system, and so on) of the specific operating system 131 | (if any) on which the executable work runs, or a compiler used to 132 | produce the work, or an object code interpreter used to run it. 133 | 134 | The "Corresponding Source" for a work in object code form means all 135 | the source code needed to generate, install, and (for an executable 136 | work) run the object code and to modify the work, including scripts to 137 | control those activities. However, it does not include the work's 138 | System Libraries, or general-purpose tools or generally available free 139 | programs which are used unmodified in performing those activities but 140 | which are not part of the work. For example, Corresponding Source 141 | includes interface definition files associated with source files for 142 | the work, and the source code for shared libraries and dynamically 143 | linked subprograms that the work is specifically designed to require, 144 | such as by intimate data communication or control flow between those 145 | subprograms and other parts of the work. 146 | 147 | The Corresponding Source need not include anything that users 148 | can regenerate automatically from other parts of the Corresponding 149 | Source. 150 | 151 | The Corresponding Source for a work in source code form is that 152 | same work. 153 | 154 | 2. Basic Permissions. 155 | 156 | All rights granted under this License are granted for the term of 157 | copyright on the Program, and are irrevocable provided the stated 158 | conditions are met. This License explicitly affirms your unlimited 159 | permission to run the unmodified Program. The output from running a 160 | covered work is covered by this License only if the output, given its 161 | content, constitutes a covered work. This License acknowledges your 162 | rights of fair use or other equivalent, as provided by copyright law. 163 | 164 | You may make, run and propagate covered works that you do not 165 | convey, without conditions so long as your license otherwise remains 166 | in force. You may convey covered works to others for the sole purpose 167 | of having them make modifications exclusively for you, or provide you 168 | with facilities for running those works, provided that you comply with 169 | the terms of this License in conveying all material for which you do 170 | not control copyright. Those thus making or running the covered works 171 | for you must do so exclusively on your behalf, under your direction 172 | and control, on terms that prohibit them from making any copies of 173 | your copyrighted material outside their relationship with you. 174 | 175 | Conveying under any other circumstances is permitted solely under 176 | the conditions stated below. Sublicensing is not allowed; section 10 177 | makes it unnecessary. 178 | 179 | 3. Protecting Users' Legal Rights From Anti-Circumvention Law. 180 | 181 | No covered work shall be deemed part of an effective technological 182 | measure under any applicable law fulfilling obligations under article 183 | 11 of the WIPO copyright treaty adopted on 20 December 1996, or 184 | similar laws prohibiting or restricting circumvention of such 185 | measures. 186 | 187 | When you convey a covered work, you waive any legal power to forbid 188 | circumvention of technological measures to the extent such circumvention 189 | is effected by exercising rights under this License with respect to 190 | the covered work, and you disclaim any intention to limit operation or 191 | modification of the work as a means of enforcing, against the work's 192 | users, your or third parties' legal rights to forbid circumvention of 193 | technological measures. 194 | 195 | 4. Conveying Verbatim Copies. 196 | 197 | You may convey verbatim copies of the Program's source code as you 198 | receive it, in any medium, provided that you conspicuously and 199 | appropriately publish on each copy an appropriate copyright notice; 200 | keep intact all notices stating that this License and any 201 | non-permissive terms added in accord with section 7 apply to the code; 202 | keep intact all notices of the absence of any warranty; and give all 203 | recipients a copy of this License along with the Program. 204 | 205 | You may charge any price or no price for each copy that you convey, 206 | and you may offer support or warranty protection for a fee. 207 | 208 | 5. Conveying Modified Source Versions. 209 | 210 | You may convey a work based on the Program, or the modifications to 211 | produce it from the Program, in the form of source code under the 212 | terms of section 4, provided that you also meet all of these conditions: 213 | 214 | a) The work must carry prominent notices stating that you modified 215 | it, and giving a relevant date. 216 | 217 | b) The work must carry prominent notices stating that it is 218 | released under this License and any conditions added under section 219 | 7. This requirement modifies the requirement in section 4 to 220 | "keep intact all notices". 221 | 222 | c) You must license the entire work, as a whole, under this 223 | License to anyone who comes into possession of a copy. This 224 | License will therefore apply, along with any applicable section 7 225 | additional terms, to the whole of the work, and all its parts, 226 | regardless of how they are packaged. This License gives no 227 | permission to license the work in any other way, but it does not 228 | invalidate such permission if you have separately received it. 229 | 230 | d) If the work has interactive user interfaces, each must display 231 | Appropriate Legal Notices; however, if the Program has interactive 232 | interfaces that do not display Appropriate Legal Notices, your 233 | work need not make them do so. 234 | 235 | A compilation of a covered work with other separate and independent 236 | works, which are not by their nature extensions of the covered work, 237 | and which are not combined with it such as to form a larger program, 238 | in or on a volume of a storage or distribution medium, is called an 239 | "aggregate" if the compilation and its resulting copyright are not 240 | used to limit the access or legal rights of the compilation's users 241 | beyond what the individual works permit. Inclusion of a covered work 242 | in an aggregate does not cause this License to apply to the other 243 | parts of the aggregate. 244 | 245 | 6. Conveying Non-Source Forms. 246 | 247 | You may convey a covered work in object code form under the terms 248 | of sections 4 and 5, provided that you also convey the 249 | machine-readable Corresponding Source under the terms of this License, 250 | in one of these ways: 251 | 252 | a) Convey the object code in, or embodied in, a physical product 253 | (including a physical distribution medium), accompanied by the 254 | Corresponding Source fixed on a durable physical medium 255 | customarily used for software interchange. 256 | 257 | b) Convey the object code in, or embodied in, a physical product 258 | (including a physical distribution medium), accompanied by a 259 | written offer, valid for at least three years and valid for as 260 | long as you offer spare parts or customer support for that product 261 | model, to give anyone who possesses the object code either (1) a 262 | copy of the Corresponding Source for all the software in the 263 | product that is covered by this License, on a durable physical 264 | medium customarily used for software interchange, for a price no 265 | more than your reasonable cost of physically performing this 266 | conveying of source, or (2) access to copy the 267 | Corresponding Source from a network server at no charge. 268 | 269 | c) Convey individual copies of the object code with a copy of the 270 | written offer to provide the Corresponding Source. This 271 | alternative is allowed only occasionally and noncommercially, and 272 | only if you received the object code with such an offer, in accord 273 | with subsection 6b. 274 | 275 | d) Convey the object code by offering access from a designated 276 | place (gratis or for a charge), and offer equivalent access to the 277 | Corresponding Source in the same way through the same place at no 278 | further charge. You need not require recipients to copy the 279 | Corresponding Source along with the object code. If the place to 280 | copy the object code is a network server, the Corresponding Source 281 | may be on a different server (operated by you or a third party) 282 | that supports equivalent copying facilities, provided you maintain 283 | clear directions next to the object code saying where to find the 284 | Corresponding Source. Regardless of what server hosts the 285 | Corresponding Source, you remain obligated to ensure that it is 286 | available for as long as needed to satisfy these requirements. 287 | 288 | e) Convey the object code using peer-to-peer transmission, provided 289 | you inform other peers where the object code and Corresponding 290 | Source of the work are being offered to the general public at no 291 | charge under subsection 6d. 292 | 293 | A separable portion of the object code, whose source code is excluded 294 | from the Corresponding Source as a System Library, need not be 295 | included in conveying the object code work. 296 | 297 | A "User Product" is either (1) a "consumer product", which means any 298 | tangible personal property which is normally used for personal, family, 299 | or household purposes, or (2) anything designed or sold for incorporation 300 | into a dwelling. In determining whether a product is a consumer product, 301 | doubtful cases shall be resolved in favor of coverage. For a particular 302 | product received by a particular user, "normally used" refers to a 303 | typical or common use of that class of product, regardless of the status 304 | of the particular user or of the way in which the particular user 305 | actually uses, or expects or is expected to use, the product. A product 306 | is a consumer product regardless of whether the product has substantial 307 | commercial, industrial or non-consumer uses, unless such uses represent 308 | the only significant mode of use of the product. 309 | 310 | "Installation Information" for a User Product means any methods, 311 | procedures, authorization keys, or other information required to install 312 | and execute modified versions of a covered work in that User Product from 313 | a modified version of its Corresponding Source. The information must 314 | suffice to ensure that the continued functioning of the modified object 315 | code is in no case prevented or interfered with solely because 316 | modification has been made. 317 | 318 | If you convey an object code work under this section in, or with, or 319 | specifically for use in, a User Product, and the conveying occurs as 320 | part of a transaction in which the right of possession and use of the 321 | User Product is transferred to the recipient in perpetuity or for a 322 | fixed term (regardless of how the transaction is characterized), the 323 | Corresponding Source conveyed under this section must be accompanied 324 | by the Installation Information. But this requirement does not apply 325 | if neither you nor any third party retains the ability to install 326 | modified object code on the User Product (for example, the work has 327 | been installed in ROM). 328 | 329 | The requirement to provide Installation Information does not include a 330 | requirement to continue to provide support service, warranty, or updates 331 | for a work that has been modified or installed by the recipient, or for 332 | the User Product in which it has been modified or installed. Access to a 333 | network may be denied when the modification itself materially and 334 | adversely affects the operation of the network or violates the rules and 335 | protocols for communication across the network. 336 | 337 | Corresponding Source conveyed, and Installation Information provided, 338 | in accord with this section must be in a format that is publicly 339 | documented (and with an implementation available to the public in 340 | source code form), and must require no special password or key for 341 | unpacking, reading or copying. 342 | 343 | 7. Additional Terms. 344 | 345 | "Additional permissions" are terms that supplement the terms of this 346 | License by making exceptions from one or more of its conditions. 347 | Additional permissions that are applicable to the entire Program shall 348 | be treated as though they were included in this License, to the extent 349 | that they are valid under applicable law. If additional permissions 350 | apply only to part of the Program, that part may be used separately 351 | under those permissions, but the entire Program remains governed by 352 | this License without regard to the additional permissions. 353 | 354 | When you convey a copy of a covered work, you may at your option 355 | remove any additional permissions from that copy, or from any part of 356 | it. (Additional permissions may be written to require their own 357 | removal in certain cases when you modify the work.) You may place 358 | additional permissions on material, added by you to a covered work, 359 | for which you have or can give appropriate copyright permission. 360 | 361 | Notwithstanding any other provision of this License, for material you 362 | add to a covered work, you may (if authorized by the copyright holders of 363 | that material) supplement the terms of this License with terms: 364 | 365 | a) Disclaiming warranty or limiting liability differently from the 366 | terms of sections 15 and 16 of this License; or 367 | 368 | b) Requiring preservation of specified reasonable legal notices or 369 | author attributions in that material or in the Appropriate Legal 370 | Notices displayed by works containing it; or 371 | 372 | c) Prohibiting misrepresentation of the origin of that material, or 373 | requiring that modified versions of such material be marked in 374 | reasonable ways as different from the original version; or 375 | 376 | d) Limiting the use for publicity purposes of names of licensors or 377 | authors of the material; or 378 | 379 | e) Declining to grant rights under trademark law for use of some 380 | trade names, trademarks, or service marks; or 381 | 382 | f) Requiring indemnification of licensors and authors of that 383 | material by anyone who conveys the material (or modified versions of 384 | it) with contractual assumptions of liability to the recipient, for 385 | any liability that these contractual assumptions directly impose on 386 | those licensors and authors. 387 | 388 | All other non-permissive additional terms are considered "further 389 | restrictions" within the meaning of section 10. If the Program as you 390 | received it, or any part of it, contains a notice stating that it is 391 | governed by this License along with a term that is a further 392 | restriction, you may remove that term. If a license document contains 393 | a further restriction but permits relicensing or conveying under this 394 | License, you may add to a covered work material governed by the terms 395 | of that license document, provided that the further restriction does 396 | not survive such relicensing or conveying. 397 | 398 | If you add terms to a covered work in accord with this section, you 399 | must place, in the relevant source files, a statement of the 400 | additional terms that apply to those files, or a notice indicating 401 | where to find the applicable terms. 402 | 403 | Additional terms, permissive or non-permissive, may be stated in the 404 | form of a separately written license, or stated as exceptions; 405 | the above requirements apply either way. 406 | 407 | 8. Termination. 408 | 409 | You may not propagate or modify a covered work except as expressly 410 | provided under this License. Any attempt otherwise to propagate or 411 | modify it is void, and will automatically terminate your rights under 412 | this License (including any patent licenses granted under the third 413 | paragraph of section 11). 414 | 415 | However, if you cease all violation of this License, then your 416 | license from a particular copyright holder is reinstated (a) 417 | provisionally, unless and until the copyright holder explicitly and 418 | finally terminates your license, and (b) permanently, if the copyright 419 | holder fails to notify you of the violation by some reasonable means 420 | prior to 60 days after the cessation. 421 | 422 | Moreover, your license from a particular copyright holder is 423 | reinstated permanently if the copyright holder notifies you of the 424 | violation by some reasonable means, this is the first time you have 425 | received notice of violation of this License (for any work) from that 426 | copyright holder, and you cure the violation prior to 30 days after 427 | your receipt of the notice. 428 | 429 | Termination of your rights under this section does not terminate the 430 | licenses of parties who have received copies or rights from you under 431 | this License. If your rights have been terminated and not permanently 432 | reinstated, you do not qualify to receive new licenses for the same 433 | material under section 10. 434 | 435 | 9. Acceptance Not Required for Having Copies. 436 | 437 | You are not required to accept this License in order to receive or 438 | run a copy of the Program. Ancillary propagation of a covered work 439 | occurring solely as a consequence of using peer-to-peer transmission 440 | to receive a copy likewise does not require acceptance. However, 441 | nothing other than this License grants you permission to propagate or 442 | modify any covered work. These actions infringe copyright if you do 443 | not accept this License. Therefore, by modifying or propagating a 444 | covered work, you indicate your acceptance of this License to do so. 445 | 446 | 10. Automatic Licensing of Downstream Recipients. 447 | 448 | Each time you convey a covered work, the recipient automatically 449 | receives a license from the original licensors, to run, modify and 450 | propagate that work, subject to this License. You are not responsible 451 | for enforcing compliance by third parties with this License. 452 | 453 | An "entity transaction" is a transaction transferring control of an 454 | organization, or substantially all assets of one, or subdividing an 455 | organization, or merging organizations. If propagation of a covered 456 | work results from an entity transaction, each party to that 457 | transaction who receives a copy of the work also receives whatever 458 | licenses to the work the party's predecessor in interest had or could 459 | give under the previous paragraph, plus a right to possession of the 460 | Corresponding Source of the work from the predecessor in interest, if 461 | the predecessor has it or can get it with reasonable efforts. 462 | 463 | You may not impose any further restrictions on the exercise of the 464 | rights granted or affirmed under this License. For example, you may 465 | not impose a license fee, royalty, or other charge for exercise of 466 | rights granted under this License, and you may not initiate litigation 467 | (including a cross-claim or counterclaim in a lawsuit) alleging that 468 | any patent claim is infringed by making, using, selling, offering for 469 | sale, or importing the Program or any portion of it. 470 | 471 | 11. Patents. 472 | 473 | A "contributor" is a copyright holder who authorizes use under this 474 | License of the Program or a work on which the Program is based. The 475 | work thus licensed is called the contributor's "contributor version". 476 | 477 | A contributor's "essential patent claims" are all patent claims 478 | owned or controlled by the contributor, whether already acquired or 479 | hereafter acquired, that would be infringed by some manner, permitted 480 | by this License, of making, using, or selling its contributor version, 481 | but do not include claims that would be infringed only as a 482 | consequence of further modification of the contributor version. For 483 | purposes of this definition, "control" includes the right to grant 484 | patent sublicenses in a manner consistent with the requirements of 485 | this License. 486 | 487 | Each contributor grants you a non-exclusive, worldwide, royalty-free 488 | patent license under the contributor's essential patent claims, to 489 | make, use, sell, offer for sale, import and otherwise run, modify and 490 | propagate the contents of its contributor version. 491 | 492 | In the following three paragraphs, a "patent license" is any express 493 | agreement or commitment, however denominated, not to enforce a patent 494 | (such as an express permission to practice a patent or covenant not to 495 | sue for patent infringement). To "grant" such a patent license to a 496 | party means to make such an agreement or commitment not to enforce a 497 | patent against the party. 498 | 499 | If you convey a covered work, knowingly relying on a patent license, 500 | and the Corresponding Source of the work is not available for anyone 501 | to copy, free of charge and under the terms of this License, through a 502 | publicly available network server or other readily accessible means, 503 | then you must either (1) cause the Corresponding Source to be so 504 | available, or (2) arrange to deprive yourself of the benefit of the 505 | patent license for this particular work, or (3) arrange, in a manner 506 | consistent with the requirements of this License, to extend the patent 507 | license to downstream recipients. "Knowingly relying" means you have 508 | actual knowledge that, but for the patent license, your conveying the 509 | covered work in a country, or your recipient's use of the covered work 510 | in a country, would infringe one or more identifiable patents in that 511 | country that you have reason to believe are valid. 512 | 513 | If, pursuant to or in connection with a single transaction or 514 | arrangement, you convey, or propagate by procuring conveyance of, a 515 | covered work, and grant a patent license to some of the parties 516 | receiving the covered work authorizing them to use, propagate, modify 517 | or convey a specific copy of the covered work, then the patent license 518 | you grant is automatically extended to all recipients of the covered 519 | work and works based on it. 520 | 521 | A patent license is "discriminatory" if it does not include within 522 | the scope of its coverage, prohibits the exercise of, or is 523 | conditioned on the non-exercise of one or more of the rights that are 524 | specifically granted under this License. You may not convey a covered 525 | work if you are a party to an arrangement with a third party that is 526 | in the business of distributing software, under which you make payment 527 | to the third party based on the extent of your activity of conveying 528 | the work, and under which the third party grants, to any of the 529 | parties who would receive the covered work from you, a discriminatory 530 | patent license (a) in connection with copies of the covered work 531 | conveyed by you (or copies made from those copies), or (b) primarily 532 | for and in connection with specific products or compilations that 533 | contain the covered work, unless you entered into that arrangement, 534 | or that patent license was granted, prior to 28 March 2007. 535 | 536 | Nothing in this License shall be construed as excluding or limiting 537 | any implied license or other defenses to infringement that may 538 | otherwise be available to you under applicable patent law. 539 | 540 | 12. No Surrender of Others' Freedom. 541 | 542 | If conditions are imposed on you (whether by court order, agreement or 543 | otherwise) that contradict the conditions of this License, they do not 544 | excuse you from the conditions of this License. If you cannot convey a 545 | covered work so as to satisfy simultaneously your obligations under this 546 | License and any other pertinent obligations, then as a consequence you may 547 | not convey it at all. For example, if you agree to terms that obligate you 548 | to collect a royalty for further conveying from those to whom you convey 549 | the Program, the only way you could satisfy both those terms and this 550 | License would be to refrain entirely from conveying the Program. 551 | 552 | 13. Use with the GNU Affero General Public License. 553 | 554 | Notwithstanding any other provision of this License, you have 555 | permission to link or combine any covered work with a work licensed 556 | under version 3 of the GNU Affero General Public License into a single 557 | combined work, and to convey the resulting work. The terms of this 558 | License will continue to apply to the part which is the covered work, 559 | but the special requirements of the GNU Affero General Public License, 560 | section 13, concerning interaction through a network will apply to the 561 | combination as such. 562 | 563 | 14. Revised Versions of this License. 564 | 565 | The Free Software Foundation may publish revised and/or new versions of 566 | the GNU General Public License from time to time. Such new versions will 567 | be similar in spirit to the present version, but may differ in detail to 568 | address new problems or concerns. 569 | 570 | Each version is given a distinguishing version number. If the 571 | Program specifies that a certain numbered version of the GNU General 572 | Public License "or any later version" applies to it, you have the 573 | option of following the terms and conditions either of that numbered 574 | version or of any later version published by the Free Software 575 | Foundation. If the Program does not specify a version number of the 576 | GNU General Public License, you may choose any version ever published 577 | by the Free Software Foundation. 578 | 579 | If the Program specifies that a proxy can decide which future 580 | versions of the GNU General Public License can be used, that proxy's 581 | public statement of acceptance of a version permanently authorizes you 582 | to choose that version for the Program. 583 | 584 | Later license versions may give you additional or different 585 | permissions. However, no additional obligations are imposed on any 586 | author or copyright holder as a result of your choosing to follow a 587 | later version. 588 | 589 | 15. Disclaimer of Warranty. 590 | 591 | THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY 592 | APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT 593 | HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY 594 | OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, 595 | THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 596 | PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM 597 | IS WITH YOU. SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF 598 | ALL NECESSARY SERVICING, REPAIR OR CORRECTION. 599 | 600 | 16. Limitation of Liability. 601 | 602 | IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING 603 | WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS 604 | THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY 605 | GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE 606 | USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF 607 | DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD 608 | PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS), 609 | EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF 610 | SUCH DAMAGES. 611 | 612 | 17. Interpretation of Sections 15 and 16. 613 | 614 | If the disclaimer of warranty and limitation of liability provided 615 | above cannot be given local legal effect according to their terms, 616 | reviewing courts shall apply local law that most closely approximates 617 | an absolute waiver of all civil liability in connection with the 618 | Program, unless a warranty or assumption of liability accompanies a 619 | copy of the Program in return for a fee. 620 | 621 | END OF TERMS AND CONDITIONS 622 | 623 | How to Apply These Terms to Your New Programs 624 | 625 | If you develop a new program, and you want it to be of the greatest 626 | possible use to the public, the best way to achieve this is to make it 627 | free software which everyone can redistribute and change under these terms. 628 | 629 | To do so, attach the following notices to the program. It is safest 630 | to attach them to the start of each source file to most effectively 631 | state the exclusion of warranty; and each file should have at least 632 | the "copyright" line and a pointer to where the full notice is found. 633 | 634 | 635 | Copyright (C) 636 | 637 | This program is free software: you can redistribute it and/or modify 638 | it under the terms of the GNU General Public License as published by 639 | the Free Software Foundation, either version 3 of the License, or 640 | (at your option) any later version. 641 | 642 | This program is distributed in the hope that it will be useful, 643 | but WITHOUT ANY WARRANTY; without even the implied warranty of 644 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 645 | GNU General Public License for more details. 646 | 647 | You should have received a copy of the GNU General Public License 648 | along with this program. If not, see . 649 | 650 | Also add information on how to contact you by electronic and paper mail. 651 | 652 | If the program does terminal interaction, make it output a short 653 | notice like this when it starts in an interactive mode: 654 | 655 | Copyright (C) 656 | This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'. 657 | This is free software, and you are welcome to redistribute it 658 | under certain conditions; type `show c' for details. 659 | 660 | The hypothetical commands `show w' and `show c' should show the appropriate 661 | parts of the General Public License. Of course, your program's commands 662 | might be different; for a GUI interface, you would use an "about box". 663 | 664 | You should also get your employer (if you work as a programmer) or school, 665 | if any, to sign a "copyright disclaimer" for the program, if necessary. 666 | For more information on this, and how to apply and follow the GNU GPL, see 667 | . 668 | 669 | The GNU General Public License does not permit incorporating your program 670 | into proprietary programs. If your program is a subroutine library, you 671 | may consider it more useful to permit linking proprietary applications with 672 | the library. If this is what you want to do, use the GNU Lesser General 673 | Public License instead of this License. But first, please read 674 | . 675 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Go-ICP for globally optimal 3D pointset registration 2 | 3 | 4 | 5 | 6 | (A demo video can be found on [here](http://jlyang.org/go-icp/).) 7 | 8 | ### Introduction 9 | 10 | This repository contains the C++ code for the Go-ICP algorithm (with trimming strategy for outlier handling). It is free software under the terms of the GNU General Public License (GPL) v3. Details of the Go-ICP algorithm can be found in our papers: 11 | 12 | * J. Yang, H. Li, Y. Jia, *Go-ICP: Solving 3D Registration Efficiently and 13 | Globally Optimally*, International Conference on Computer Vision (__ICCV__), 2013. [PDF](http://jlyang.org/iccv13_go-icp.pdf) 14 | 15 | * J. Yang, H. Li, D. Campbell, Y. Jia, *Go-ICP: A Globally Optimal Solution to 3D ICP Point-Set Registration*, IEEE Transactions on Pattern Analysis and Machine Intelligence (__TPAMI__), 2016. [PDF](http://jlyang.org/tpami16_go-icp_preprint.pdf) 16 | 17 | Please read this file carefully prior to using the code. Some frequently-asked questions have answers here. 18 | 19 | ### Compiling 20 | 21 | Use cmake to generate desired projects on different platforms. 22 | 23 | A pre-built Windows exe file can be found in [this zip file](http://jlyang.org/go-icp/Go-ICP_V1.3.zip). 24 | 25 | ### Terminology 26 | 27 | Data points: points of the source point set to be transformed. 28 | 29 | Model points: points of the target point set. 30 | 31 | ### Notes 32 | 33 | * ___Make sure both model and data points are normalized to fit in \[-1,1\]3 prior to running___ (we recommend first independently centralizing the two point clouds to the origin then simultaneously scaling them). The default initial translation cube is \[-0.5,0.5\]3 (see “config_example.txt”). 34 | 35 | * The convergence threshold is set on the Sum of Squared Error (SSE) as in the code and the paper. For the ease of parameter setting for different data point numbers, we use Mean of Squared Error (MSE) in the configuration (see “config_example.txt”). We use MSE threshold of 0.001 for the demos. ___Try smaller ones if your registration results are not satisfactory___. 36 | 37 | * ___Make sure you tune the trimming percentage in the configuration file properly___, if there are some outliers in the data pointset (i.e., some regions that are not overlapped by the model pointset). Note that a small portion of outliers may lead to competely wrong result if no trimming is used. Refer to our TPAMI paper for more details. 38 | 39 | * ___Do NOT subsample the model points!___ Since we use 3D distance transform for closest distance computation, model point number does not affect running speed. Subsampling the model points may increase the optimal registration error thus slowing down the BnB convergance. 40 | 41 | * Building 3D distance transform with (default) 300 discrete nodes in each dimension takes about 20-25s in our experiments. Using smaller values can reduce memory and building time costs, but it will also degrade the distance accuracy. 42 | 43 | ### Running 44 | 45 | Run the compiled binary with following parameters: \ \ \ \ \, e.g. “./GoICP model data 1000 config output”, “GoICP.exe model.txt data.txt 46 | 500 config.txt output.txt”. 47 | 48 | * \ and \ are the point files of the model and data pointsets respectively. Each point file is in plain text format. It begins with a positive point number N in the first line, followed with N lines of X, Y, Z values of the N points. 49 | 50 | * \ indicates the number of down-sampled data points. The code assumes the input data points are randomly ordered and uses the first \ data points for registration. ___Make sure you randomly permute your data points or change the code for some other sampling strategies.___ 51 | 52 | * \ is the configuration file containing parameters for the algorithm, e.g. initial rotation and translation cubes, convergence threshold and trimming percentage. See “config_example.txt” for example. 53 | 54 | * \ is the output file containing registration results. By default it contains the obtained 3x3 rotation matrix and 3x1 translation vector only. You can adapt the code to output other results as you wish. 55 | 56 | Some sample data and scripts can be found in the /demo folder. 57 | 58 | ### Other langueage 59 | 60 | A python wrapper by @aalavandhaann can be found at https://github.com/aalavandhaann/go-icp_cython 61 | 62 | ### Acknowledgments 63 | 64 | This implementation uses the nanoflann library, and a simple matrix library written by Andreas Geiger. The distance transform implementation is adapted from the code of Alexander Vasilevskiy. 65 | 66 | 67 | ### Change log 68 | V1.3 (26-Jan-2015) 69 | 70 | Implemented the intro-selection algorithm 71 | 72 | Fixed some minor issues 73 | 74 | 75 | V1.2 (12-Jun-2014) 76 | 77 | Refined the quick-selection algorithm 78 | 79 | Added a deconstructor to distance transform class (Thanks to Nima Tajbakhsh) 80 | 81 | 82 | V1.1 (21-Apr-2014) 83 | 84 | Speeded up Trimmed-GoICP (around 2-5 times experimentally) using a quick-selection algorithm 85 | 86 | 87 | V1.0 (13-Feb-2014) 88 | 89 | First complete version for release 90 | 91 | -------------------------------------------------------------------------------- /StringTokenizer.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "StringTokenizer.hpp" 3 | 4 | StringTokenizer::StringTokenizer() { 5 | }; 6 | 7 | StringTokenizer::StringTokenizer(std::string str, char delim) { 8 | int delim_loc; 9 | 10 | while ((delim_loc = str.find_first_of(delim,0)) != std::string::npos) { 11 | 12 | if (str.substr(0, delim_loc).length() > 0) { 13 | tokens.push_back(str.substr(0,delim_loc)); 14 | } 15 | str = str.substr(delim_loc+1, str.length()); 16 | } 17 | 18 | if (str.length() > 0) { 19 | tokens.push_back(str); 20 | } 21 | }; 22 | 23 | StringTokenizer::StringTokenizer(std::string str, char * delims) { 24 | int delim_loc; 25 | 26 | while ((delim_loc = str.find_first_of(delims,0)) != std::string::npos) { 27 | if (str.substr(0, delim_loc).length() > 0) { 28 | tokens.push_back(str.substr(0,delim_loc)); 29 | } 30 | str = str.substr(delim_loc+1, str.length()); 31 | } 32 | if (str.length() > 0) tokens.push_back(str); 33 | }; 34 | 35 | StringTokenizer::~StringTokenizer() { 36 | }; 37 | 38 | std::string StringTokenizer::nextToken() { 39 | if (!hasMoreTokens()) return ""; 40 | std::string return_str(tokens.front()); 41 | tokens.pop_front(); 42 | return return_str; 43 | }; 44 | 45 | bool StringTokenizer::hasMoreTokens() { 46 | return !tokens.empty(); 47 | }; 48 | 49 | int StringTokenizer::numberOfTokens() { 50 | return tokens.size(); 51 | }; 52 | -------------------------------------------------------------------------------- /StringTokenizer.hpp: -------------------------------------------------------------------------------- 1 | 2 | #ifndef STRINGTOKENIZER_H 3 | #define STRINGTOKENIZER_H 4 | 5 | #include 6 | #include 7 | 8 | class StringTokenizer { 9 | 10 | private: 11 | std::list tokens; 12 | 13 | public: 14 | StringTokenizer(); 15 | StringTokenizer(std::string str, char delim = ' '); 16 | StringTokenizer(std::string str, char * delims); 17 | ~StringTokenizer(); 18 | 19 | std::string nextToken(); 20 | bool hasMoreTokens(); 21 | int numberOfTokens(); 22 | }; 23 | 24 | #endif // for STRINGTOKENIZER_H 25 | -------------------------------------------------------------------------------- /bunny.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yangjiaolong/Go-ICP/937f114590f7df8b003d05f594b55527e230fef0/bunny.png -------------------------------------------------------------------------------- /config_example.txt: -------------------------------------------------------------------------------- 1 | # Config file for GO-ICP 2 | 3 | # Mean Squared Error (MSE) convergence threshold 4 | MSEThresh=0.001 5 | 6 | # Smallest rotation value along dimension X of rotation cube (radians) 7 | rotMinX=-3.1416 8 | # Smallest rotation value along dimension Y of rotation cube (radians) 9 | rotMinY=-3.1416 10 | # Smallest rotation value along dimension Z of rotation cube (radians) 11 | rotMinZ=-3.1416 12 | # Side length of each dimension of rotation cube (radians) 13 | rotWidth=6.2832 14 | 15 | # Smallest translation value along dimension X of translation cube 16 | transMinX=-0.5 17 | # Smallest translation value along dimension Y of translation cube 18 | transMinY=-0.5 19 | # Smallest translation value along dimension Z of translation cube 20 | transMinZ=-0.5 21 | # Side length of each dimension of translation cube 22 | transWidth=1.0 23 | 24 | # Set to 0.0 for no trimming 25 | trimFraction=0.0 26 | 27 | # Nodes per dimension of distance transform 28 | distTransSize=300 29 | # DistanceTransformWidth = ExpandFactor x WidthLargestDimension 30 | distTransExpandFactor=2.0 31 | 32 | -------------------------------------------------------------------------------- /demo/config.txt: -------------------------------------------------------------------------------- 1 | # Config file for GO-ICP 2 | 3 | # Mean Squared Error (MSE) convergence threshold 4 | MSEThresh=0.001 5 | 6 | # Smallest rotation value along dimension X of rotation cube (radians) 7 | rotMinX=-3.1416 8 | # Smallest rotation value along dimension Y of rotation cube (radians) 9 | rotMinY=-3.1416 10 | # Smallest rotation value along dimension Z of rotation cube (radians) 11 | rotMinZ=-3.1416 12 | # Side length of each dimension of rotation cube (radians) 13 | rotWidth=6.2832 14 | 15 | # Smallest translation value along dimension X of translation cube 16 | transMinX=-0.5 17 | # Smallest translation value along dimension Y of translation cube 18 | transMinY=-0.5 19 | # Smallest translation value along dimension Z of translation cube 20 | transMinZ=-0.5 21 | # Side length of each dimension of translation cube 22 | transWidth=1.0 23 | 24 | # Set to 0.0 for no trimming 25 | trimFraction=0.0 26 | 27 | # Nodes per dimension of distance transform 28 | distTransSize=300 29 | # DistanceTransformWidth = ExpandFactor x WidthLargestDimension 30 | distTransExpandFactor=2.0 31 | 32 | -------------------------------------------------------------------------------- /demo/data_rand.txt: -------------------------------------------------------------------------------- 1 | 100 2 | -0.390945 -0.253648 -0.693646 3 | -0.209112 -0.788946 0.806259 4 | -0.244454 -0.630994 0.222520 5 | -0.018572 -0.013369 -0.968680 6 | -0.795680 0.213003 0.469272 7 | -0.129410 -0.758468 -0.841533 8 | -0.874140 0.376690 0.073802 9 | 0.972439 0.805720 0.540446 10 | 0.639408 0.517936 -0.085539 11 | 0.041984 -0.567641 0.525445 12 | -0.627868 -0.626682 -0.682663 13 | -0.390540 -0.910843 0.614556 14 | -0.093783 0.383537 0.689436 15 | -0.340680 -0.130823 -0.818663 16 | 0.585048 0.368079 0.830095 17 | -0.524509 -0.227004 -0.694724 18 | 0.805901 0.617765 0.547100 19 | -0.014319 0.920549 -0.620875 20 | -0.948211 -0.705225 -0.308715 21 | 0.598218 -0.846279 -0.971523 22 | 0.866325 -0.425606 -0.320275 23 | 0.878305 -0.543165 0.731280 24 | -0.551634 0.122239 -0.418976 25 | -0.809045 -0.222236 -0.835483 26 | -0.669562 -0.568575 0.902713 27 | 0.640050 -0.124511 0.619277 28 | 0.004342 -0.336516 0.701013 29 | 0.193722 -0.674728 -0.456951 30 | -0.551709 -0.026097 0.762519 31 | 0.220608 0.452412 0.698366 32 | -0.984362 -0.830217 -0.733272 33 | 0.275721 -0.381809 0.669618 34 | 0.893837 -0.663012 -0.430258 35 | 0.929187 0.540413 0.857103 36 | -0.721998 0.609023 0.199136 37 | 0.082503 -0.118703 -0.543592 38 | -0.500554 -0.347211 0.659898 39 | 0.919482 0.094603 0.864206 40 | -0.336649 0.410775 -0.994273 41 | 0.032443 0.941984 -0.214181 42 | -0.328830 0.445457 0.371844 43 | 0.325111 0.235720 -0.881337 44 | -0.460650 -0.230771 -0.525362 45 | -0.930396 0.274761 -0.467282 46 | -0.388081 0.830520 0.556153 47 | -0.569780 0.964451 -0.647045 48 | -0.503215 0.297412 -0.720650 49 | -0.435641 0.552125 0.155869 50 | -0.341545 0.034978 0.636255 51 | 0.263576 -0.400496 -0.873924 52 | -0.432517 -0.689693 0.525743 53 | 0.097252 0.257235 0.200841 54 | -0.659993 -0.858351 -0.422616 55 | 0.553212 -0.617077 0.950776 56 | -0.666821 0.929519 0.154664 57 | -0.155741 -0.279532 -0.726886 58 | -0.541045 -0.541962 0.957706 59 | 0.222851 0.459064 0.493044 60 | 0.025348 0.623475 0.178610 61 | -0.759422 0.186110 -0.718520 62 | -0.688227 0.173035 0.649013 63 | -0.216700 0.694449 0.542953 64 | -0.972530 -0.154074 0.896830 65 | -0.865882 -0.708373 0.356959 66 | -0.741257 0.202092 0.727488 67 | 0.964848 -0.359001 0.269442 68 | 0.652869 0.249336 -0.199819 69 | -0.974141 -0.180387 0.625854 70 | -0.427451 -0.971566 0.629372 71 | 0.911110 0.797895 -0.478867 72 | -0.865690 0.675552 0.929710 73 | 0.366356 0.734283 0.529405 74 | 0.387733 0.478012 0.031460 75 | -0.933848 0.510250 -0.630657 76 | -0.441093 0.965525 -0.582941 77 | 0.219504 -0.735712 -0.541959 78 | -0.387027 0.525647 -0.816564 79 | -0.418526 0.667827 0.209249 80 | -0.504858 0.628997 -0.337728 81 | -0.234295 0.857608 -0.370971 82 | -0.126532 -0.250294 -0.417554 83 | -0.095219 -0.958002 0.881987 84 | -0.606688 0.059193 0.489544 85 | 0.206382 0.132974 0.104455 86 | -0.975392 0.750054 0.938282 87 | 0.498338 0.585213 0.263634 88 | 0.450925 0.285074 0.861187 89 | -0.571254 -0.097008 0.039436 90 | -1.010648 -0.655652 0.887168 91 | -0.830783 0.554990 0.148806 92 | -0.169736 -0.961859 0.291496 93 | 0.772313 0.524162 -0.166129 94 | 0.001199 0.259918 -0.742469 95 | 0.281014 0.447398 -0.913083 96 | -0.619791 -0.259713 0.763154 97 | -0.192184 -0.902064 -0.207704 98 | -0.064109 -0.357203 0.952066 99 | 0.876370 0.603648 -0.897100 100 | -0.824621 -0.587793 -0.818184 101 | 0.535244 -0.763384 -0.909095 102 | -------------------------------------------------------------------------------- /demo/demo.m: -------------------------------------------------------------------------------- 1 | % Simple demos for the GoICP code 2 | % Demo1 uses 100 points randomly drawn in [-1,1]^3 3 | % Demo2 uses the Stanford bunny data normalized in [-1,1]^3 4 | % Gaussian noise is added to the all points 5 | % 6 | % Jiaolong Yang 7 | % Feb 13, 2014 8 | 9 | 10 | % data = 'random points'; 11 | data = 'bunny'; 12 | 13 | if ispc() 14 | cmd = 'GoICP_vc2012.exe'; 15 | else 16 | cmd = './GoICP'; 17 | end 18 | 19 | if strcmp(data, 'random points') 20 | cmd = [cmd ' model_rand.txt data_rand.txt 100 config.txt output.txt']; 21 | model = readpoints('model_rand.txt'); 22 | data = readpoints('data_rand.txt'); 23 | else 24 | cmd = [cmd ' model_bunny.txt data_bunny.txt 1000 config.txt output.txt']; 25 | model = readpoints('model_bunny.txt'); 26 | data = readpoints('data_bunny.txt'); 27 | end 28 | 29 | system(cmd); 30 | 31 | file = fopen('output.txt', 'r'); 32 | t = fscanf(file, '%f', 1); 33 | R = fscanf(file, '%f', [3,3])'; 34 | T = fscanf(file, '%f', [3,1]); 35 | fclose(file); 36 | 37 | figure; 38 | subplot(1,2,1); 39 | plot3(model(1,:), model(2,:), model(3,:), '.r'); 40 | hold on; 41 | plot3(data(1,:), data(2,:), data(3,:), '.b'); 42 | hold off; axis equal; title('Initial Pose'); 43 | subplot(1,2,2); 44 | data_ = bsxfun(@plus, R*data, T); 45 | plot3(model(1,:), model(2,:), model(3,:), '.r'); 46 | hold on; 47 | plot3(data_(1,:), data_(2,:), data_(3,:), '.b'); 48 | hold off; axis equal; title('Result'); 49 | -------------------------------------------------------------------------------- /demo/model_rand.txt: -------------------------------------------------------------------------------- 1 | 100 2 | 0.891472 0.164805 -0.955314 3 | 0.809038 -0.917668 0.198116 4 | 0.900167 -0.474560 -0.255627 5 | 0.852279 0.679822 -0.897447 6 | -0.061554 -0.674927 -0.567396 7 | 1.462851 0.214494 -0.813023 8 | -0.044407 -0.394357 -0.903964 9 | -0.293255 0.621115 0.731754 10 | 0.123039 0.726665 0.099793 11 | 0.809097 -0.491692 0.203452 12 | 1.192926 -0.131155 -1.090970 13 | 0.993202 -0.951571 -0.044605 14 | -0.168530 -0.315542 0.074971 15 | 0.847547 0.344356 -1.046240 16 | -0.081948 0.052107 0.670425 17 | 0.841575 0.114205 -1.096014 18 | -0.161038 0.474140 0.618961 19 | -0.119751 0.820865 -0.786561 20 | 1.038907 -0.611916 -1.096341 21 | 1.716085 0.731287 -0.342688 22 | 1.124265 0.645839 0.251174 23 | 0.832298 -0.081293 0.955536 24 | 0.442408 0.017335 -0.967022 25 | 0.847352 0.000271 -1.385553 26 | 0.535181 -1.178774 -0.109779 27 | 0.467348 -0.010067 0.632341 28 | 0.518452 -0.555805 0.259907 29 | 1.275175 0.213775 -0.329232 30 | 0.130129 -0.783844 -0.176465 31 | -0.166791 -0.070545 0.301661 32 | 1.316648 -0.412634 -1.353849 33 | 0.606552 -0.367068 0.423828 34 | 1.368911 0.624917 0.245115 35 | -0.193887 0.292195 0.933718 36 | -0.279933 -0.279192 -0.765136 37 | 0.804966 0.429169 -0.534185 38 | 0.450879 -0.835961 -0.162066 39 | 0.212349 0.107564 0.997032 40 | 0.417951 0.674364 -1.190164 41 | -0.312481 0.589021 -0.479724 42 | -0.121688 -0.214579 -0.306769 43 | 0.637287 0.953640 -0.618865 44 | 0.821740 0.027142 -0.919133 45 | 0.250653 -0.094283 -1.277081 46 | -0.570891 -0.202971 -0.293967 47 | -0.259160 0.499580 -1.222150 48 | 0.398977 0.314432 -1.119246 49 | -0.200709 -0.095603 -0.543558 50 | 0.129142 -0.582392 -0.110384 51 | 1.234782 0.634074 -0.586755 52 | 0.823509 -0.811335 -0.138905 53 | 0.187958 0.080826 -0.093637 54 | 1.285037 -0.420406 -0.900434 55 | 0.801948 -0.450514 0.813595 56 | -0.531444 -0.093548 -0.763167 57 | 0.995463 0.323762 -0.803779 58 | 0.470887 -1.138835 0.023181 59 | -0.107263 0.081053 0.174508 60 | -0.174405 0.200542 -0.198951 61 | 0.459629 0.113366 -1.337517 62 | -0.033295 -0.742887 -0.364665 63 | -0.420594 -0.171931 -0.151170 64 | 0.110108 -1.208862 -0.362297 65 | 0.836565 -1.024992 -0.607595 66 | -0.118826 -0.805765 -0.353701 67 | 0.869614 0.368124 0.684688 68 | 0.440130 0.712192 0.070918 69 | 0.240290 -1.042213 -0.560852 70 | 1.016407 -1.014421 -0.052200 71 | 0.097433 1.277945 0.045402 72 | -0.636162 -0.838280 -0.410838 73 | -0.321169 0.234910 0.282842 74 | 0.101430 0.469194 0.014662 75 | 0.117664 0.077075 -1.464952 76 | -0.268332 0.549733 -1.102880 77 | 1.376281 0.270486 -0.345013 78 | 0.220909 0.562012 -1.116908 79 | -0.286204 -0.055884 -0.507191 80 | -0.055982 0.210019 -0.940920 81 | -0.203870 0.496085 -0.788243 82 | 0.849596 0.158882 -0.610943 83 | 1.012071 -0.950827 0.381258 84 | 0.130640 -0.615347 -0.396846 85 | 0.365915 0.149899 -0.062392 86 | -0.723044 -0.875811 -0.455569 87 | -0.065262 0.430453 0.224190 88 | -0.040891 -0.102308 0.587432 89 | 0.444330 -0.388311 -0.649969 90 | 0.564429 -1.393917 -0.365897 91 | -0.224104 -0.341029 -0.857896 92 | 1.226559 -0.624932 -0.068242 93 | 0.196959 0.847497 0.144423 94 | 0.506330 0.664479 -0.749120 95 | 0.444183 1.012243 -0.694046 96 | 0.308319 -0.937891 -0.168803 97 | 1.318222 -0.288644 -0.442094 98 | 0.440013 -0.749676 0.372678 99 | 0.402528 1.469447 -0.241091 100 | 1.163921 -0.152044 -1.331625 101 | 1.605365 0.700907 -0.349552 102 | 103 | -------------------------------------------------------------------------------- /demo/output.txt: -------------------------------------------------------------------------------- 1 | 12.365 2 | -0.0101497 0.0017169 0.9999469 3 | -0.0041633 0.9999896 -0.0017597 4 | -0.9999398 -0.0041811 -0.0101425 5 | 0.2163900 6 | -0.1497952 7 | 0.0745708 8 | -------------------------------------------------------------------------------- /demo/readpoints.m: -------------------------------------------------------------------------------- 1 | function [ P ] = readpoints( filename ) 2 | %READPOINTS Summary of this function goes here 3 | % Detailed explanation goes here 4 | 5 | file = fopen(filename, 'r'); 6 | N = fscanf(file, '%d', 1); 7 | P = fscanf(file, '%f%f%f', [3,N]); 8 | fclose(file); 9 | 10 | end 11 | 12 | -------------------------------------------------------------------------------- /jly_3ddt.cpp: -------------------------------------------------------------------------------- 1 | /**************************************************************** 2 | 3D Euclidean Distance Transform Class 3 | Last modified: Feb 13, 2013 4 | 5 | Functions computing DT are derived from "dt.c" written by 6 | Alexander Vasilevskiy. 7 | 8 | Jiaolong Yang 9 | ****************************************************************/ 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | #include "jly_3ddt.h" 18 | 19 | #define FALSE 0 20 | #define TRUE 1 21 | 22 | #define sqrt1(x) sqrt((double)x) 23 | 24 | #define ROUND(x) (int((x)+0.5)) 25 | #define FLOOR(x) (int((x))) 26 | #define CEIL(x) (int((x+0.99999))) 27 | 28 | // ************************************ 29 | // 30 | // D-Euclidean BEGIN 31 | // 32 | // ************************************ 33 | 34 | 35 | void initDE(Array3dDEucl3D & inDE){ 36 | int z,y,x; 37 | for (z=0;z0)&&(y0) { 148 | mask[0].v=inDE[z-1][y][x].v; 149 | mask[0].h=inDE[z-1][y][x].h; 150 | mask[0].d=inDE[z-1][y][x].d+1; 151 | mask[0].distance=sqrt1(mask[0].v*mask[0].v+mask[0].h*mask[0].h+mask[0].d*mask[0].d); 152 | } 153 | else { 154 | mask[0].v=infty; 155 | mask[0].h=infty; 156 | mask[0].d=infty; 157 | mask[0].distance=infty; 158 | } 159 | mask[1].v=inDE[z][y][x].v; 160 | mask[1].h=inDE[z][y][x].h; 161 | mask[1].d=inDE[z][y][x].d; 162 | mask[1].distance=sqrt1(mask[1].v*mask[1].v+mask[1].h*mask[1].h+mask[1].d*mask[1].d); 163 | 164 | for(looper=0;looper<2;looper++) { 165 | if (mask[looper].distance0)&&(y>0)) { 193 | mask[0].v=inDE[z-1][y-1][x].v; 194 | mask[0].h=inDE[z-1][y-1][x].h+1; 195 | mask[0].d=inDE[z-1][y-1][x].d+1; 196 | mask[0].distance=sqrt1(mask[0].v*mask[0].v+mask[0].h*mask[0].h+mask[0].d*mask[0].d); 197 | } 198 | else { 199 | mask[0].v=infty; 200 | mask[0].h=infty; 201 | mask[0].d=infty; 202 | mask[0].distance=infty; 203 | } 204 | if (y>0){ 205 | mask[1].v=inDE[z][y-1][x].v; 206 | mask[1].h=inDE[z][y-1][x].h+1; 207 | mask[1].d=inDE[z][y-1][x].d; 208 | mask[1].distance=sqrt1(mask[1].v*mask[1].v+mask[1].h*mask[1].h+mask[1].d*mask[1].d); 209 | } 210 | else { 211 | mask[1].v=infty; 212 | mask[1].h=infty; 213 | mask[1].d=infty; 214 | mask[1].distance=infty; 215 | 216 | } 217 | if ((z0)) { 218 | mask[2].v=inDE[z+1][y-1][x].v; 219 | mask[2].h=inDE[z+1][y-1][x].h+1; 220 | mask[2].d=inDE[z+1][y-1][x].d+1; 221 | mask[2].distance=sqrt1(mask[2].v*mask[2].v+mask[2].h*mask[2].h+mask[2].d*mask[2].d); 222 | } 223 | else { 224 | mask[2].v=infty; 225 | mask[2].h=infty; 226 | mask[2].d=infty; 227 | mask[2].distance=infty; 228 | } 229 | mask[3].v=inDE[z][y][x].v; 230 | mask[3].h=inDE[z][y][x].h; 231 | mask[3].d=inDE[z][y][x].d; 232 | mask[3].distance=sqrt1(mask[3].v*mask[3].v+mask[3].h*mask[3].h+mask[3].d*mask[3].d); 233 | if (z>0) { 234 | mask[4].v=inDE[z-1][y][x].v; 235 | mask[4].h=inDE[z-1][y][x].h; 236 | mask[4].d=inDE[z-1][y][x].d+1; 237 | mask[4].distance=sqrt1(mask[4].v*mask[4].v+mask[4].h*mask[4].h+mask[4].d*mask[4].d); 238 | } 239 | else { 240 | mask[4].v=infty; 241 | mask[4].h=infty; 242 | mask[4].d=infty; 243 | mask[4].distance=infty; 244 | } 245 | 246 | for(looper=0;looper<5;looper++) { 247 | if (mask[looper].distance0)&&(y>0)&&(x0)&&(x0)&&(x0)&&(x0)&&(y0)&&(y0)&&(y>0)&&(x>0)) { 523 | mask[0].v=inDE[z-1][y-1][x-1].v+1; 524 | mask[0].h=inDE[z-1][y-1][x-1].h+1; 525 | mask[0].d=inDE[z-1][y-1][x-1].d+1; 526 | mask[0].distance=sqrt1(mask[0].v*mask[0].v+mask[0].h*mask[0].h+mask[0].d*mask[0].d); 527 | } 528 | else { 529 | mask[0].v=infty; 530 | mask[0].h=infty; 531 | mask[0].d=infty; 532 | mask[0].distance=infty; 533 | } 534 | if ((y>0)&&(x>0)){ 535 | mask[1].v=inDE[z][y-1][x-1].v+1; 536 | mask[1].h=inDE[z][y-1][x-1].h+1; 537 | mask[1].d=inDE[z][y-1][x-1].d; 538 | mask[1].distance=sqrt1(mask[1].v*mask[1].v+mask[1].h*mask[1].h+mask[1].d*mask[1].d); 539 | } 540 | else { 541 | mask[1].v=infty; 542 | mask[1].h=infty; 543 | mask[1].d=infty; 544 | mask[1].distance=infty; 545 | 546 | } 547 | if ((z0)&&(x>0)) { 548 | mask[2].v=inDE[z+1][y-1][x-1].v+1; 549 | mask[2].h=inDE[z+1][y-1][x-1].h+1; 550 | mask[2].d=inDE[z+1][y-1][x-1].d+1; 551 | mask[2].distance=sqrt1(mask[2].v*mask[2].v+mask[2].h*mask[2].h+mask[2].d*mask[2].d); 552 | } 553 | else { 554 | mask[2].v=infty; 555 | mask[2].h=infty; 556 | mask[2].d=infty; 557 | mask[2].distance=infty; 558 | } 559 | if ((z>0)&&(x>0)) { 560 | mask[3].v=inDE[z-1][y][x-1].v+1; 561 | mask[3].h=inDE[z-1][y][x-1].h; 562 | mask[3].d=inDE[z-1][y][x-1].d+1; 563 | mask[3].distance=sqrt1(mask[3].v*mask[3].v+mask[3].h*mask[3].h+mask[3].d*mask[3].d); 564 | } 565 | else { 566 | mask[3].v=infty; 567 | mask[3].h=infty; 568 | mask[3].d=infty; 569 | mask[3].distance=infty; 570 | } 571 | if (x>0) { 572 | mask[4].v=inDE[z][y][x-1].v+1; 573 | mask[4].h=inDE[z][y][x-1].h; 574 | mask[4].d=inDE[z][y][x-1].d; 575 | mask[4].distance=sqrt1(mask[4].v*mask[4].v+mask[4].h*mask[4].h+mask[4].d*mask[4].d); 576 | } 577 | else { 578 | mask[4].v=infty; 579 | mask[4].h=infty; 580 | mask[4].d=infty; 581 | mask[4].distance=infty; 582 | } 583 | if ((x>0)&&(z0)&&(z>0)&&(y0)&&(y0)&&(y0)&&(y>0)) { 642 | mask[9].v=inDE[z-1][y-1][x].v; 643 | mask[9].h=inDE[z-1][y-1][x].h+1; 644 | mask[9].d=inDE[z-1][y-1][x].d+1; 645 | mask[9].distance=sqrt1(mask[9].v*mask[9].v+mask[9].h*mask[9].h+mask[9].d*mask[9].d); 646 | } 647 | else { 648 | mask[9].v=infty; 649 | mask[9].h=infty; 650 | mask[9].d=infty; 651 | mask[9].distance=infty; 652 | } 653 | 654 | 655 | if (y>0){ 656 | mask[10].v=inDE[z][y-1][x].v; 657 | mask[10].h=inDE[z][y-1][x].h+1; 658 | mask[10].d=inDE[z][y-1][x].d; 659 | mask[10].distance=sqrt1(mask[10].v*mask[10].v+mask[10].h*mask[10].h+mask[10].d*mask[10].d); 660 | } 661 | else { 662 | mask[10].v=infty; 663 | mask[10].h=infty; 664 | mask[10].d=infty; 665 | mask[10].distance=infty; 666 | 667 | } 668 | 669 | 670 | if ((z0)) { 671 | mask[11].v=inDE[z+1][y-1][x].v; 672 | mask[11].h=inDE[z+1][y-1][x].h+1; 673 | mask[11].d=inDE[z+1][y-1][x].d+1; 674 | mask[11].distance=sqrt1(mask[11].v*mask[11].v+mask[11].h*mask[11].h+mask[11].d*mask[11].d); 675 | } 676 | else { 677 | mask[11].v=infty; 678 | mask[11].h=infty; 679 | mask[11].d=infty; 680 | mask[11].distance=infty; 681 | } 682 | 683 | mask[12].v=inDE[z][y][x].v; 684 | mask[12].h=inDE[z][y][x].h; 685 | mask[12].d=inDE[z][y][x].d; 686 | mask[12].distance=sqrt1(mask[12].v*mask[12].v+mask[12].h*mask[12].h+mask[12].d*mask[12].d); 687 | 688 | if (z>0) { 689 | mask[13].v=inDE[z-1][y][x].v; 690 | mask[13].h=inDE[z-1][y][x].h; 691 | mask[13].d=inDE[z-1][y][x].d+1; 692 | mask[13].distance=sqrt1(mask[13].v*mask[13].v+mask[13].h*mask[13].h+mask[13].d*mask[13].d); 693 | } 694 | else { 695 | mask[13].v=infty; 696 | mask[13].h=infty; 697 | mask[13].d=infty; 698 | mask[13].distance=infty; 699 | } 700 | 701 | for(looper=0;looper<14;looper++) { 702 | if (mask[looper].distance-1;z--) inDE[z][y][x]=MINforwardDE2(A, z,y,x); 723 | } 724 | for(y=Ydim-1;y>-1;y--) { 725 | for(z=Zdim-1;z>-1;z--) inDE[z][y][x]=MINforwardDE3(A, z,y,x); 726 | for(z=0;z-1;x--) { 730 | for(y=Ydim-1;y>-1;y--){ 731 | for(z=Zdim-1;z>-1;z--) inDE[z][y][x]=MINbackwardDE1(A, z,y,x); 732 | for(z=0;z-1;z--) inDE[z][y][x]=MINforwardDE2(A, z,y,x); 737 | 738 | } 739 | } 740 | if (Debug) printf("DEuclidean finished\n"); 741 | 742 | } 743 | 744 | // ************************************ 745 | // 746 | // D-Euclidean END 747 | // 748 | // *********************************** 749 | 750 | void printArray3D(const Array3dDEucl3D& A, int x, char type) 751 | { 752 | DEucl3D*** inDE = A.data; 753 | int Xdim = A.Xdim; 754 | int Ydim = A.Ydim; 755 | int Zdim = A.Zdim; 756 | 757 | int z,y; 758 | printf("\nslice x= %d\n",x); 759 | for (y=0;y0)&&(y>0)) mask[0]=inDE[z-1][y-1][x].distance+d2; else mask[0]=infty; 796 | if (y>0) mask[1]=inDE[z][y-1][x].distance+d1; else mask[1]=infty; 797 | if (( z0)) mask[2]=inDE[z+1][y-1][x].distance+d2; else mask[2]=infty; 798 | mask[3]=inDE[z][y][x].distance; 799 | if (z>0) mask[4]=inDE[z-1][y][x].distance+d1; else mask[4]=infty; 800 | 801 | if ((x>0)&&(z>0)&&(y>0)) mask[5]=inDE[z-1][y-1][x-1].distance+d3; else mask[5]=infty; 802 | if ((x>0)&&(y>0)) mask[6]=inDE[z][y-1][x-1].distance+d2; else mask[6]=infty; 803 | if ((x>0)&&(z0)) mask[7]=inDE[z+1][y-1][x-1].distance+d3; else mask[7]=infty; 804 | if ((x>0)&&(z>0)) mask[8]=inDE[z-1][y][x-1].distance+d2; else mask[8]=infty; 805 | if (x>0) mask[9]=inDE[z][y][x-1].distance+d1; else mask[9]=infty; 806 | if ((x>0)&&(z0)&&(z>0)&&(y0)&&(y0)&&(z0)&&(y0)&&(y>0)) mask[5]=inDE[z-1][y-1][x+1].distance+d3; else mask[5]=infty; 842 | if ((x0)) mask[6]=inDE[z][y-1][x+1].distance+d2; else mask[6]=infty; 843 | if ((x0)) mask[7]=inDE[z+1][y-1][x+1].distance+d3; else mask[7]=infty; 844 | if ((x0)) mask[8]=inDE[z-1][y][x+1].distance+d2; else mask[8]=infty; 845 | if (x0)&&(y-1;x--) 872 | for (y=Ydim-1;y>-1;y--) 873 | for(z=Zdim-1;z>-1;z--) inDE[z][y][x].distance=MINbackward3Dfloat(A, d1,d2,d3,z,y,x); 874 | if (Debug) printf("chamfer float finished\n"); 875 | 876 | } 877 | 878 | // *************************** 879 | // 880 | // float functions END 881 | // 882 | // *************************** 883 | 884 | DT3D::DT3D() 885 | { 886 | A.data = NULL; 887 | } 888 | 889 | void DT3D::Build(double* _x, double* _y, double* _z, int num) 890 | { 891 | xMin=_x[0]; xMax=_x[0]; yMin=_y[0]; yMax=_y[0]; zMin=_z[0]; zMax=_z[0]; 892 | int i; 893 | for(i = 1; i < num; i ++) 894 | { 895 | if(xMin > _x[i]) xMin = _x[i]; 896 | if(xMax < _x[i]) xMax = _x[i]; 897 | if(yMin > _y[i]) yMin = _y[i]; 898 | if(yMax < _y[i]) yMax = _y[i]; 899 | if(zMin > _z[i]) zMin = _z[i]; 900 | if(zMax < _z[i]) zMax = _z[i]; 901 | } 902 | 903 | double xCenter = (xMin+xMax)/2; 904 | double yCenter = (yMin+yMax)/2; 905 | double zCenter = (zMin+zMax)/2; 906 | xMin = xCenter - expandFactor*(xMax-xCenter); 907 | xMax = xCenter + expandFactor*(xMax-xCenter); 908 | yMin = yCenter - expandFactor*(yMax-yCenter); 909 | yMax = yCenter + expandFactor*(yMax-yCenter); 910 | zMin = zCenter - expandFactor*(zMax-zCenter); 911 | zMax = zCenter + expandFactor*(zMax-zCenter); 912 | 913 | double max = xMax-xMin > yMax-yMin ? xMax-xMin : yMax-yMin; 914 | max = max > zMax-zMin ? max : zMax-zMin; 915 | 916 | xMin = xCenter - max/2; 917 | xMax = xCenter + max/2; 918 | yMin = yCenter - max/2; 919 | yMax = yCenter + max/2; 920 | zMin = zCenter - max/2; 921 | zMax = zCenter + max/2; 922 | 923 | scale = SIZE/max; 924 | 925 | //printf("DTaccu:%lf\n",sqrt(3.0)/2/scale); 926 | 927 | if(A.data == NULL) 928 | { 929 | A.Init(SIZE, SIZE, SIZE); 930 | } 931 | 932 | int x,y,z; 933 | 934 | DEucl3D*** inDE = A.data; 935 | int Xdim = A.Xdim; 936 | int Ydim = A.Ydim; 937 | int Zdim = A.Zdim; 938 | 939 | for(z=0; z=Xdim || y<0 || y>=Ydim || z<0 || z>=Zdim) 959 | continue; 960 | 961 | inDE[z][y][x].distance=0; 962 | inDE[z][y][x].h=0; 963 | inDE[z][y][x].v=0; 964 | inDE[z][y][x].d=0; 965 | } 966 | 967 | DEuclidean(A); 968 | 969 | for(z=0; z -1 && x < SIZE && y > -1 && y < SIZE && z > -1 && z < SIZE) 989 | return A.data[z][y][x].distance; 990 | 991 | float a = 0, b = 0, c = 0; 992 | if(x < 0) 993 | { 994 | a = x; 995 | x = 0; 996 | } 997 | else if(x >= SIZE) 998 | { 999 | a = x-SIZE+1; 1000 | x = SIZE-1; 1001 | } 1002 | 1003 | if(y < 0) 1004 | { 1005 | b = y; 1006 | y = 0; 1007 | } 1008 | else if(y >= SIZE) 1009 | { 1010 | b = y-SIZE+1; 1011 | y = SIZE-1; 1012 | } 1013 | 1014 | if(z < 0) 1015 | { 1016 | c = z; 1017 | z = 0; 1018 | } 1019 | else if(z >= SIZE) 1020 | { 1021 | c = z-SIZE+1; 1022 | z = SIZE-1; 1023 | } 1024 | 1025 | return sqrt(a*a+b*b+c*c)/scale + A.data[z][y][x].distance; 1026 | } 1027 | -------------------------------------------------------------------------------- /jly_3ddt.h: -------------------------------------------------------------------------------- 1 | /**************************************************************** 2 | 3D Euclidean Distance Transform Class 3 | Last modified: May 1, 2014 4 | 5 | Functions computing DT are derived from "dt.c" written by 6 | Alexander Vasilevskiy. 7 | 8 | Jiaolong Yang 9 | ****************************************************************/ 10 | 11 | #ifndef JLY_3DDT_H 12 | #define JLY_3DDT_H 13 | 14 | #define infty 32767 // Max value for a signed short (2 bytes / 16 bits) 15 | 16 | typedef struct DEucl { 17 | int v,h; 18 | float distance; 19 | }DEucl; 20 | typedef struct DEucl3D { 21 | short v,h,d; 22 | float distance; 23 | }DEucl3D; 24 | 25 | 26 | template 27 | struct Array3d { 28 | int Xdim, Ydim, Zdim; 29 | T *** data; 30 | T * data_array; 31 | 32 | void Init(int x, int y, int z); 33 | 34 | // Call this printslice; 35 | void printArrayDE(int x); 36 | Array3d() 37 | { 38 | data = NULL; 39 | data_array = NULL; 40 | } 41 | ~Array3d() 42 | { 43 | if (data && data[0]) 44 | delete(data[0]); 45 | if(data) 46 | delete(data); 47 | if(data_array) 48 | delete(data_array); 49 | } 50 | }; 51 | 52 | template 53 | void Array3d::Init(int x, int y, int z) 54 | { 55 | Xdim = x; 56 | Ydim = y; 57 | Zdim = z; 58 | // allocate the memory for the first level pointers. 59 | int n1 = z, n2 = y, n3 = x; 60 | 61 | data = new T** [n1]; 62 | 63 | // set the first level pointers and allocate the memory for the second level pointers. 64 | { 65 | data[0] = new T* [n1 * n2]; 66 | for (int row1_index = 0; row1_index < n1; row1_index++) 67 | data [row1_index] = data[0] + n2 * row1_index; 68 | } 69 | 70 | T* array_ptr = new T [n1*n2*n3]; 71 | data_array = array_ptr; 72 | 73 | // set the second level pointers. 74 | for (int row1_index = 0; row1_index < n1; row1_index++) 75 | for (int row2_index = 0; row2_index < n2; row2_index++) { 76 | data [row1_index][row2_index] = array_ptr; 77 | array_ptr += n3; 78 | } 79 | } 80 | 81 | template 82 | void Array3d::printArrayDE(int x) 83 | { 84 | int z,y; 85 | printf("slice %d",x); 86 | for (y=0;y=infty) 90 | printf("(*,*,*)"); 91 | else 92 | printf("(%d,%d,%d)",data[z][y][x].v,data[z][y][x].h,data[z][y][x].d,data[z][y][x].distance); 93 | } 94 | printf("\nDONE\n"); 95 | } 96 | 97 | typedef Array3d Array3dDEucl3D; 98 | typedef Array3d Array3dfloat; 99 | 100 | class DT3D{ 101 | public: 102 | DT3D(); 103 | int SIZE; 104 | double scale; 105 | double expandFactor; 106 | double xMin, xMax, yMin, yMax, zMin, zMax; 107 | void Build(double* x, double* y, double* z, int num); 108 | float Distance(double x, double y, double z); 109 | private: 110 | Array3dDEucl3D A; 111 | }; 112 | 113 | #endif 114 | -------------------------------------------------------------------------------- /jly_goicp.cpp: -------------------------------------------------------------------------------- 1 | /******************************************************************** 2 | Implementation of Go-ICP Algorithm 3 | Last modified: Jun 18, 2014 4 | 5 | "Go-ICP: Solving 3D Registration Efficiently and Globally Optimally" 6 | Jiaolong Yang, Hongdong Li, Yunde Jia 7 | International Conference on Computer Vision (ICCV), 2013 8 | 9 | Copyright (C) 2013 Jiaolong Yang (BIT and ANU) 10 | 11 | This program is free software: you can redistribute it and/or modify 12 | it under the terms of the GNU General Public License as published by 13 | the Free Software Foundation, either version 3 of the License, or 14 | (at your option) any later version. 15 | 16 | This program is distributed in the hope that it will be useful, 17 | but WITHOUT ANY WARRANTY; without even the implied warranty of 18 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 19 | GNU General Public License for more details. 20 | 21 | You should have received a copy of the GNU General Public License 22 | along with this program. If not, see . 23 | *********************************************************************/ 24 | 25 | #include 26 | #include 27 | #include 28 | #include 29 | //using namespace std; 30 | 31 | #include "jly_goicp.h" 32 | #include "jly_sorting.hpp" 33 | 34 | GoICP::GoICP() 35 | { 36 | initNodeRot.a = -PI; 37 | initNodeRot.b = -PI; 38 | initNodeRot.c = -PI; 39 | initNodeRot.w = 2*PI; 40 | initNodeRot.l = 0; 41 | 42 | initNodeRot.lb = 0; 43 | initNodeTrans.lb = 0; 44 | 45 | doTrim = true; 46 | } 47 | 48 | // Build Distance Transform 49 | void GoICP::BuildDT() 50 | { 51 | double* x = (double*)malloc(sizeof(double)*Nm); 52 | double* y = (double*)malloc(sizeof(double)*Nm); 53 | double* z = (double*)malloc(sizeof(double)*Nm); 54 | for(int i = 0; i < Nm; i++) 55 | { 56 | x[i] = pModel[i].x; 57 | y[i] = pModel[i].y; 58 | z[i] = pModel[i].z; 59 | } 60 | dt.Build(x, y, z, Nm); 61 | delete(x); 62 | delete(y); 63 | delete(z); 64 | } 65 | 66 | // Run ICP and calculate sum squared L2 error 67 | float GoICP::ICP(Matrix& R_icp, Matrix& t_icp) 68 | { 69 | int i; 70 | float error, dis; 71 | 72 | icp3d.Run(D_icp, Nd, R_icp, t_icp); // data cloud, # data points, rotation matrix, translation matrix 73 | 74 | // Transform point cloud and use DT to determine the L2 error 75 | error = 0; 76 | for(i = 0; i < Nd; i++) 77 | { 78 | POINT3D& p = pData[i]; 79 | pDataTempICP[i].x = R_icp.val[0][0]*p.x+R_icp.val[0][1]*p.y+R_icp.val[0][2]*p.z + t_icp.val[0][0]; 80 | pDataTempICP[i].y = R_icp.val[1][0]*p.x+R_icp.val[1][1]*p.y+R_icp.val[1][2]*p.z + t_icp.val[1][0]; 81 | pDataTempICP[i].z = R_icp.val[2][0]*p.x+R_icp.val[2][1]*p.y+R_icp.val[2][2]*p.z + t_icp.val[2][0]; 82 | 83 | if(!doTrim) 84 | { 85 | dis = dt.Distance(pDataTempICP[i].x, pDataTempICP[i].y, pDataTempICP[i].z); 86 | error += dis*dis; 87 | } 88 | else 89 | { 90 | minDis[i] = dt.Distance(pDataTempICP[i].x, pDataTempICP[i].y, pDataTempICP[i].z); 91 | } 92 | } 93 | 94 | if(doTrim) 95 | { 96 | //qsort(minDis, Nd, sizeof(float), cmp); 97 | //myqsort(minDis, Nd, inlierNum); 98 | intro_select(minDis,0,Nd-1,inlierNum-1); 99 | for(i = 0; i < inlierNum; i++) 100 | { 101 | error += minDis[i]*minDis[i]; 102 | } 103 | } 104 | 105 | return error; 106 | } 107 | 108 | void GoICP::Initialize() 109 | { 110 | int i, j; 111 | float sigma, maxAngle; 112 | 113 | // Precompute the rotation uncertainty distance (maxRotDis) for each point in the data and each level of rotation subcube 114 | 115 | // Calculate L2 norm of each point in data cloud to origin 116 | normData = (float*)malloc(sizeof(float)*Nd); 117 | for(i = 0; i < Nd; i++) 118 | { 119 | normData[i] = sqrt(pData[i].x*pData[i].x + pData[i].y*pData[i].y + pData[i].z*pData[i].z); 120 | } 121 | 122 | maxRotDis = new float*[MAXROTLEVEL]; 123 | for(i = 0; i < MAXROTLEVEL; i++) 124 | { 125 | maxRotDis[i] = (float*)malloc(sizeof(float*)*Nd); 126 | 127 | sigma = initNodeRot.w/pow(2.0,i)/2.0; // Half-side length of each level of rotation subcube 128 | maxAngle = SQRT3*sigma; 129 | 130 | if(maxAngle > PI) 131 | maxAngle = PI; 132 | for(j = 0; j < Nd; j++) 133 | maxRotDis[i][j] = 2*sin(maxAngle/2)*normData[j]; 134 | } 135 | 136 | // Temporary Variables 137 | minDis = (float*)malloc(sizeof(float)*Nd); 138 | pDataTemp = (POINT3D *)malloc(sizeof(POINT3D)*Nd); 139 | pDataTempICP = (POINT3D *)malloc(sizeof(POINT3D)*Nd); 140 | 141 | // ICP Initialisation 142 | // Copy model and data point clouds to variables for ICP 143 | M_icp = (float*)calloc(3*Nm,sizeof(float)); 144 | D_icp = (float*)calloc(3*Nd,sizeof(float)); 145 | for(i = 0, j = 0; i < Nm; i++) 146 | { 147 | M_icp[j++] = pModel[i].x; 148 | M_icp[j++] = pModel[i].y; 149 | M_icp[j++] = pModel[i].z; 150 | } 151 | for(i = 0, j = 0; i < Nd; i++) 152 | { 153 | D_icp[j++] = pData[i].x; 154 | D_icp[j++] = pData[i].y; 155 | D_icp[j++] = pData[i].z; 156 | } 157 | 158 | // Build ICP kdtree with model dataset 159 | icp3d.Build(M_icp,Nm); 160 | icp3d.err_diff_def = MSEThresh/10000; 161 | icp3d.trim_fraction = trimFraction; 162 | icp3d.do_trim = doTrim; 163 | 164 | // Initialise so-far-best rotation and translation nodes 165 | optNodeRot = initNodeRot; 166 | optNodeTrans = initNodeTrans; 167 | // Initialise so-far-best rotation and translation matrices 168 | optR = Matrix::eye(3); 169 | optT = Matrix::ones(3,1)*0; 170 | 171 | // For untrimmed ICP, use all points, otherwise only use inlierNum points 172 | if(doTrim) 173 | { 174 | // Calculate number of inlier points 175 | inlierNum = (int)(Nd * (1 - trimFraction)); 176 | } 177 | else 178 | { 179 | inlierNum = Nd; 180 | } 181 | SSEThresh = MSEThresh * inlierNum; 182 | } 183 | 184 | void GoICP::Clear() 185 | { 186 | delete(pDataTemp); 187 | delete(pDataTempICP); 188 | delete(normData); 189 | delete(minDis); 190 | for(int i = 0; i < MAXROTLEVEL; i++) 191 | { 192 | delete(maxRotDis[i]); 193 | } 194 | delete(maxRotDis); 195 | delete(M_icp); 196 | delete(D_icp); 197 | } 198 | 199 | // Inner Branch-and-Bound, iterating over the translation space 200 | float GoICP::InnerBnB(float* maxRotDisL, TRANSNODE* nodeTransOut) 201 | { 202 | int i, j; 203 | float transX, transY, transZ; 204 | float lb, ub, optErrorT; 205 | float dis, maxTransDis; 206 | TRANSNODE nodeTrans, nodeTransParent; 207 | priority_queue queueTrans; 208 | 209 | // Set optimal translation error to overall so-far optimal error 210 | // Investigating translation nodes that are sub-optimal overall is redundant 211 | optErrorT = optError; 212 | 213 | // Push top-level translation node into the priority queue 214 | queueTrans.push(initNodeTrans); 215 | 216 | // 217 | while(1) 218 | { 219 | if(queueTrans.empty()) 220 | break; 221 | 222 | nodeTransParent = queueTrans.top(); 223 | queueTrans.pop(); 224 | 225 | if(optErrorT-nodeTransParent.lb < SSEThresh) 226 | { 227 | break; 228 | } 229 | 230 | nodeTrans.w = nodeTransParent.w/2; 231 | maxTransDis = SQRT3/2.0*nodeTrans.w; 232 | 233 | for(j = 0; j < 8; j++) 234 | { 235 | nodeTrans.x = nodeTransParent.x + (j&1)*nodeTrans.w ; 236 | nodeTrans.y = nodeTransParent.y + (j>>1&1)*nodeTrans.w ; 237 | nodeTrans.z = nodeTransParent.z + (j>>2&1)*nodeTrans.w ; 238 | 239 | transX = nodeTrans.x + nodeTrans.w/2; 240 | transY = nodeTrans.y + nodeTrans.w/2; 241 | transZ = nodeTrans.z + nodeTrans.w/2; 242 | 243 | // For each data point, calculate the distance to it's closest point in the model cloud 244 | for(i = 0; i < Nd; i++) 245 | { 246 | // Find distance between transformed point and closest point in model set ||R_r0 * x + t0 - y|| 247 | // pDataTemp is the data points rotated by R0 248 | minDis[i] = dt.Distance(pDataTemp[i].x + transX, pDataTemp[i].y + transY, pDataTemp[i].z + transZ); 249 | 250 | // Subtract the rotation uncertainty radius if calculating the rotation lower bound 251 | // maxRotDisL == NULL when calculating the rotation upper bound 252 | if(maxRotDisL) 253 | minDis[i] -= maxRotDisL[i]; 254 | 255 | if(minDis[i] < 0) 256 | { 257 | minDis[i] = 0; 258 | } 259 | } 260 | 261 | if(doTrim) 262 | { 263 | // Sort by distance 264 | //qsort(minDis, Nd, sizeof(float), cmp); 265 | //myqsort(minDis, Nd, inlierNum); 266 | intro_select(minDis,0,Nd-1,inlierNum-1); 267 | } 268 | 269 | // For each data point, find the incremental upper and lower bounds 270 | ub = 0; 271 | for(i = 0; i < inlierNum; i++) 272 | { 273 | ub += minDis[i]*minDis[i]; 274 | } 275 | 276 | lb = 0; 277 | for(i = 0; i < inlierNum; i++) 278 | { 279 | // Subtract the translation uncertainty radius 280 | dis = minDis[i] - maxTransDis; 281 | if(dis > 0) 282 | lb += dis*dis; 283 | } 284 | 285 | 286 | // If upper bound is better than best, update optErrorT and optTransOut (optimal translation node) 287 | if(ub < optErrorT) 288 | { 289 | optErrorT = ub; 290 | if(nodeTransOut) 291 | *nodeTransOut = nodeTrans; 292 | } 293 | 294 | // Remove subcube from queue if lb is bigger than optErrorT 295 | if(lb >= optErrorT) 296 | { 297 | //discard 298 | continue; 299 | } 300 | 301 | nodeTrans.ub = ub; 302 | nodeTrans.lb = lb; 303 | queueTrans.push(nodeTrans); 304 | } 305 | } 306 | 307 | return optErrorT; 308 | } 309 | 310 | float GoICP::OuterBnB() 311 | { 312 | int i, j; 313 | ROTNODE nodeRot, nodeRotParent; 314 | TRANSNODE nodeTrans; 315 | float v1, v2, v3, t, ct, ct2,st, st2; 316 | float tmp121, tmp122, tmp131, tmp132, tmp231, tmp232; 317 | float R11, R12, R13, R21, R22, R23, R31, R32, R33; 318 | float lb, ub, error, dis; 319 | clock_t clockBeginICP; 320 | priority_queue queueRot; 321 | 322 | // Calculate Initial Error 323 | optError = 0; 324 | 325 | for(i = 0; i < Nd; i++) 326 | { 327 | minDis[i] = dt.Distance(pData[i].x, pData[i].y, pData[i].z); 328 | } 329 | if(doTrim) 330 | { 331 | // Sort by distance 332 | //qsort(minDis, Nd, sizeof(float), cmp); 333 | //myqsort(minDis, Nd, inlierNum); 334 | intro_select(minDis,0,Nd-1,inlierNum-1); 335 | } 336 | for(i = 0; i < inlierNum; i++) 337 | { 338 | optError += minDis[i]*minDis[i]; 339 | } 340 | cout << "Error*: " << optError << " (Init)" << endl; 341 | 342 | Matrix R_icp = optR; 343 | Matrix t_icp = optT; 344 | 345 | // Run ICP from initial state 346 | clockBeginICP = clock(); 347 | error = ICP(R_icp, t_icp); 348 | if(error < optError) 349 | { 350 | optError = error; 351 | optR = R_icp; 352 | optT = t_icp; 353 | cout << "Error*: " << error << " (ICP " << (double)(clock()-clockBeginICP)/CLOCKS_PER_SEC << "s)" << endl; 354 | cout << "ICP-ONLY Rotation Matrix:" << endl; 355 | cout << R_icp << endl; 356 | cout << "ICP-ONLY Translation Vector:" << endl; 357 | cout << t_icp << endl; 358 | } 359 | 360 | // Push top-level rotation node into priority queue 361 | queueRot.push(initNodeRot); 362 | 363 | // Keep exploring rotation space until convergence is achieved 364 | long long count = 0; 365 | while(1) 366 | { 367 | if(queueRot.empty()) 368 | { 369 | cout << "Rotation Queue Empty" << endl; 370 | cout << "Error*: " << optError << ", LB: " << lb << endl; 371 | break; 372 | } 373 | 374 | // Access rotation cube with lowest lower bound... 375 | nodeRotParent = queueRot.top(); 376 | // ...and remove it from the queue 377 | queueRot.pop(); 378 | 379 | // Exit if the optError is less than or equal to the lower bound plus a small epsilon 380 | if((optError-nodeRotParent.lb) <= SSEThresh) 381 | { 382 | cout << "Error*: " << optError << ", LB: " << nodeRotParent.lb << ", epsilon: " << SSEThresh << endl; 383 | break; 384 | } 385 | 386 | if(count>0 && count%300 == 0) 387 | printf("LB=%f L=%d\n",nodeRotParent.lb,nodeRotParent.l); 388 | count ++; 389 | 390 | // Subdivide rotation cube into octant subcubes and calculate upper and lower bounds for each 391 | nodeRot.w = nodeRotParent.w/2; 392 | nodeRot.l = nodeRotParent.l+1; 393 | // For each subcube, 394 | for(j = 0; j < 8; j++) 395 | { 396 | // Calculate the smallest rotation across each dimension 397 | nodeRot.a = nodeRotParent.a + (j&1)*nodeRot.w ; 398 | nodeRot.b = nodeRotParent.b + (j>>1&1)*nodeRot.w ; 399 | nodeRot.c = nodeRotParent.c + (j>>2&1)*nodeRot.w ; 400 | 401 | // Find the subcube centre 402 | v1 = nodeRot.a + nodeRot.w/2; 403 | v2 = nodeRot.b + nodeRot.w/2; 404 | v3 = nodeRot.c + nodeRot.w/2; 405 | 406 | // Skip subcube if it is completely outside the rotation PI-ball 407 | if(sqrt(v1*v1+v2*v2+v3*v3)-SQRT3*nodeRot.w/2 > PI) 408 | { 409 | continue; 410 | } 411 | 412 | // Convert angle-axis rotation into a rotation matrix 413 | t = sqrt(v1*v1 + v2*v2 + v3*v3); 414 | if(t > 0) 415 | { 416 | v1 /= t; 417 | v2 /= t; 418 | v3 /= t; 419 | 420 | ct = cos(t); 421 | ct2 = 1 - ct; 422 | st = sin(t); 423 | st2 = 1 - st; 424 | 425 | tmp121 = v1*v2*ct2; tmp122 = v3*st; 426 | tmp131 = v1*v3*ct2; tmp132 = v2*st; 427 | tmp231 = v2*v3*ct2; tmp232 = v1*st; 428 | 429 | R11 = ct + v1*v1*ct2; R12 = tmp121 - tmp122; R13 = tmp131 + tmp132; 430 | R21 = tmp121 + tmp122; R22 = ct + v2*v2*ct2; R23 = tmp231 - tmp232; 431 | R31 = tmp131 - tmp132; R32 = tmp231 + tmp232; R33 = ct + v3*v3*ct2; 432 | 433 | // Rotate data points by subcube rotation matrix 434 | for(i = 0; i < Nd; i++) 435 | { 436 | POINT3D& p = pData[i]; 437 | pDataTemp[i].x = R11*p.x + R12*p.y + R13*p.z; 438 | pDataTemp[i].y = R21*p.x + R22*p.y + R23*p.z; 439 | pDataTemp[i].z = R31*p.x + R32*p.y + R33*p.z; 440 | } 441 | } 442 | // If t == 0, the rotation angle is 0 and no rotation is required 443 | else 444 | { 445 | memcpy(pDataTemp, pData, sizeof(POINT3D)*Nd); 446 | } 447 | 448 | // Upper Bound 449 | // Run Inner Branch-and-Bound to find rotation upper bound 450 | // Calculates the rotation upper bound by finding the translation upper bound for a given rotation, 451 | // assuming that the rotation is known (zero rotation uncertainty radius) 452 | ub = InnerBnB(NULL /*Rotation Uncertainty Radius*/, &nodeTrans); 453 | 454 | // If the upper bound is the best so far, run ICP 455 | if(ub < optError) 456 | { 457 | // Update optimal error and rotation/translation nodes 458 | optError = ub; 459 | optNodeRot = nodeRot; 460 | optNodeTrans = nodeTrans; 461 | 462 | optR.val[0][0] = R11; optR.val[0][1] = R12; optR.val[0][2] = R13; 463 | optR.val[1][0] = R21; optR.val[1][1] = R22; optR.val[1][2] = R23; 464 | optR.val[2][0] = R31; optR.val[2][1] = R32; optR.val[2][2] = R33; 465 | optT.val[0][0] = optNodeTrans.x+optNodeTrans.w/2; 466 | optT.val[1][0] = optNodeTrans.y+optNodeTrans.w/2; 467 | optT.val[2][0] = optNodeTrans.z+optNodeTrans.w/2; 468 | 469 | cout << "Error*: " << optError << endl; 470 | 471 | // Run ICP 472 | clockBeginICP = clock(); 473 | R_icp = optR; 474 | t_icp = optT; 475 | error = ICP(R_icp, t_icp); 476 | //Our ICP implementation uses kdtree for closest distance computation which is slightly different from DT approximation, 477 | //thus it's possible that ICP failed to decrease the DT error. This is no big deal as the difference should be very small. 478 | if(error < optError) 479 | { 480 | optError = error; 481 | optR = R_icp; 482 | optT = t_icp; 483 | 484 | cout << "Error*: " << error << "(ICP " << (double)(clock() - clockBeginICP)/CLOCKS_PER_SEC << "s)" << endl; 485 | } 486 | 487 | // Discard all rotation nodes with high lower bounds in the queue 488 | priority_queue queueRotNew; 489 | while(!queueRot.empty()) 490 | { 491 | ROTNODE node = queueRot.top(); 492 | queueRot.pop(); 493 | if(node.lb < optError) 494 | queueRotNew.push(node); 495 | else 496 | break; 497 | } 498 | queueRot = queueRotNew; 499 | } 500 | 501 | // Lower Bound 502 | // Run Inner Branch-and-Bound to find rotation lower bound 503 | // Calculates the rotation lower bound by finding the translation upper bound for a given rotation, 504 | // assuming that the rotation is uncertain (a positive rotation uncertainty radius) 505 | // Pass an array of rotation uncertainties for every point in data cloud at this level 506 | lb = InnerBnB(maxRotDis[nodeRot.l], NULL /*Translation Node*/); 507 | 508 | // If the best error so far is less than the lower bound, remove the rotation subcube from the queue 509 | if(lb >= optError) 510 | { 511 | continue; 512 | } 513 | 514 | // Update node and put it in queue 515 | nodeRot.ub = ub; 516 | nodeRot.lb = lb; 517 | queueRot.push(nodeRot); 518 | } 519 | } 520 | 521 | return optError; 522 | } 523 | 524 | float GoICP::Register() 525 | { 526 | Initialize(); 527 | OuterBnB(); 528 | Clear(); 529 | 530 | return optError; 531 | } 532 | -------------------------------------------------------------------------------- /jly_goicp.h: -------------------------------------------------------------------------------- 1 | /******************************************************************** 2 | Header File for Go-ICP Class 3 | Last modified: Apr 21, 2014 4 | 5 | "Go-ICP: Solving 3D Registration Efficiently and Globally Optimally" 6 | Jiaolong Yang, Hongdong Li, Yunde Jia 7 | International Conference on Computer Vision (ICCV), 2013 8 | 9 | Copyright (C) 2013 Jiaolong Yang (BIT and ANU) 10 | 11 | This program is free software: you can redistribute it and/or modify 12 | it under the terms of the GNU General Public License as published by 13 | the Free Software Foundation, either version 3 of the License, or 14 | (at your option) any later version. 15 | 16 | This program is distributed in the hope that it will be useful, 17 | but WITHOUT ANY WARRANTY; without even the implied warranty of 18 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 19 | GNU General Public License for more details. 20 | 21 | You should have received a copy of the GNU General Public License 22 | along with this program. If not, see . 23 | *********************************************************************/ 24 | 25 | #ifndef JLY_GOICP_H 26 | #define JLY_GOICP_H 27 | 28 | #include 29 | using namespace std; 30 | 31 | #include "jly_icp3d.hpp" 32 | #include "jly_3ddt.h" 33 | 34 | #define PI 3.1415926536 35 | #define SQRT3 1.732050808 36 | 37 | typedef struct _POINT3D 38 | { 39 | float x, y, z; 40 | }POINT3D; 41 | 42 | typedef struct _ROTNODE 43 | { 44 | float a, b, c, w; 45 | float ub, lb; 46 | int l; 47 | friend bool operator < (const struct _ROTNODE & n1, const struct _ROTNODE & n2) 48 | { 49 | if(n1.lb != n2.lb) 50 | return n1.lb > n2.lb; 51 | else 52 | return n1.w < n2.w; 53 | //return n1.ub > n2.ub; 54 | } 55 | 56 | }ROTNODE; 57 | 58 | typedef struct _TRANSNODE 59 | { 60 | float x, y, z, w; 61 | float ub, lb; 62 | friend bool operator < (const struct _TRANSNODE & n1, const struct _TRANSNODE & n2) 63 | { 64 | if(n1.lb != n2.lb) 65 | return n1.lb > n2.lb; 66 | else 67 | return n1.w < n2.w; 68 | //return n1.ub > n2.ub; 69 | } 70 | }TRANSNODE; 71 | 72 | /********************************************************/ 73 | 74 | 75 | 76 | /********************************************************/ 77 | 78 | #define MAXROTLEVEL 20 79 | 80 | class GoICP 81 | { 82 | public: 83 | int Nm, Nd; 84 | POINT3D * pModel, * pData; 85 | 86 | ROTNODE initNodeRot; 87 | TRANSNODE initNodeTrans; 88 | 89 | DT3D dt; 90 | 91 | ROTNODE optNodeRot; 92 | TRANSNODE optNodeTrans; 93 | 94 | GoICP(); 95 | float Register(); 96 | void BuildDT(); 97 | 98 | float MSEThresh; 99 | float SSEThresh; 100 | float icpThresh; 101 | 102 | float optError; 103 | Matrix optR; 104 | Matrix optT; 105 | 106 | clock_t clockBegin; 107 | 108 | float trimFraction; 109 | int inlierNum; 110 | bool doTrim; 111 | 112 | private: 113 | //temp variables 114 | float * normData; 115 | float * minDis; 116 | float** maxRotDis; 117 | float * maxRotDisL; 118 | POINT3D * pDataTemp; 119 | POINT3D * pDataTempICP; 120 | 121 | ICP3D icp3d; 122 | float * M_icp; 123 | float * D_icp; 124 | 125 | float ICP(Matrix& R_icp, Matrix& t_icp); 126 | float InnerBnB(float* maxRotDisL, TRANSNODE* nodeTransOut); 127 | float OuterBnB(); 128 | void Initialize(); 129 | void Clear(); 130 | 131 | }; 132 | 133 | /********************************************************/ 134 | 135 | #endif 136 | -------------------------------------------------------------------------------- /jly_icp3d.hpp: -------------------------------------------------------------------------------- 1 | /******************************************************************** 2 | An ICP Implementation for 3D Pointset Registration 3 | Last modified: Feb 13, 2014 4 | 5 | "Go-ICP: Solving 3D Registration Efficiently and Globally Optimally" 6 | Jiaolong Yang, Hongdong Li, Yunde Jia 7 | International Conference on Computer Vision (ICCV), 2013 8 | 9 | Copyright (C) 2013 Jiaolong Yang (BIT and ANU) 10 | 11 | This program is free software: you can redistribute it and/or modify 12 | it under the terms of the GNU General Public License as published by 13 | the Free Software Foundation, either version 3 of the License, or 14 | (at your option) any later version. 15 | 16 | This program is distributed in the hope that it will be useful, 17 | but WITHOUT ANY WARRANTY; without even the implied warranty of 18 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 19 | GNU General Public License for more details. 20 | 21 | You should have received a copy of the GNU General Public License 22 | along with this program. If not, see . 23 | *********************************************************************/ 24 | 25 | #ifndef JLY_ICP3D_HPP 26 | #define JLY_ICP3D_HPP 27 | 28 | #include "matrix.h" 29 | #include "nanoflann.hpp" 30 | using namespace nanoflann; 31 | 32 | 33 | // A custom data set class to use nanoflann 34 | template 35 | struct PointCloud 36 | { 37 | struct Point 38 | { 39 | T x,y,z; 40 | }; 41 | 42 | std::vector pts; 43 | 44 | // Must return the number of data points 45 | inline size_t kdtree_get_point_count() const { return pts.size(); } 46 | 47 | // Returns the distance between the vector "p1[0:size-1]" and the data point with index "idx_p2" stored in the class: 48 | inline T kdtree_distance(const T *p1, const size_t idx_p2,size_t size) const 49 | { 50 | const T d0=p1[0]-pts[idx_p2].x; 51 | const T d1=p1[1]-pts[idx_p2].y; 52 | const T d2=p1[2]-pts[idx_p2].z; 53 | return d0*d0+d1*d1+d2*d2; 54 | } 55 | 56 | // Returns the dim'th component of the idx'th point in the class: 57 | // Since this is inlined and the "dim" argument is typically an immediate value, the 58 | // "if/else's" are actually solved at compile time. 59 | inline T kdtree_get_pt(const size_t idx, int dim) const 60 | { 61 | if (dim==0) return pts[idx].x; 62 | else if (dim==1) return pts[idx].y; 63 | else return pts[idx].z; 64 | } 65 | 66 | // Optional bounding-box computation: return false to default to a standard bbox computation loop. 67 | // Return true if the BBOX was already computed by the class and returned in "bb" so it can be avoided to redo it again. 68 | // Look at bb.size() to find out the expected dimensionality (e.g. 2 or 3 for point clouds) 69 | template 70 | bool kdtree_get_bbox(BBOX &bb) const { return false; } 71 | 72 | }; 73 | 74 | struct POINTREF 75 | { 76 | double dis; 77 | int id_data; 78 | int id_model; 79 | }; 80 | 81 | template 82 | class ICP3D 83 | { 84 | public: 85 | 86 | ICP3D(); 87 | ~ICP3D(); 88 | size_t max_iter_def; 89 | T err_diff_def; 90 | T trim_fraction; 91 | bool do_trim; 92 | void Build(T * model, size_t n); 93 | T Run(T * data, size_t n, Matrix & R, Matrix & t); 94 | T Run(T * data, size_t n, Matrix & R, Matrix & t, size_t max_iter); 95 | T Run(T * data, size_t n, Matrix & R, Matrix & t, T err_diff); 96 | T Run(T * data, size_t n, Matrix & R, Matrix & t, size_t max_iter, T err_diff); 97 | 98 | private: 99 | 100 | static int cmp(const void * a, const void * b); 101 | 102 | PointCloud model_; 103 | 104 | KDTreeSingleIndexAdaptor< 105 | L2_Simple_Adaptor > , 106 | PointCloud, 107 | 3 /* dim */ 108 | > * kdtree; 109 | }; 110 | 111 | template 112 | ICP3D::ICP3D() 113 | { 114 | max_iter_def = 10000; 115 | err_diff_def = 0.000001; 116 | trim_fraction = 0; 117 | do_trim = true; 118 | 119 | kdtree = NULL; 120 | } 121 | 122 | template 123 | ICP3D::~ICP3D() 124 | { 125 | if(kdtree != NULL) 126 | delete(kdtree); 127 | } 128 | 129 | template 130 | void ICP3D::Build(T * model, size_t n) 131 | { 132 | 133 | if(kdtree != NULL) 134 | delete(kdtree); 135 | 136 | model_.pts.resize(n); 137 | 138 | size_t i, idx; 139 | for(i = 0; i < n; i++) 140 | { 141 | idx = i*3; 142 | model_.pts[i].x = model[idx]; 143 | model_.pts[i].y = model[idx+1]; 144 | model_.pts[i].z = model[idx+2]; 145 | } 146 | 147 | kdtree = new KDTreeSingleIndexAdaptor< 148 | L2_Simple_Adaptor > , 149 | PointCloud, 150 | 3 /* dim */ 151 | >(3 /*dim*/, model_, KDTreeSingleIndexAdaptorParams(10 /* max leaf */) ); 152 | 153 | kdtree->buildIndex(); 154 | } 155 | 156 | template 157 | int ICP3D::cmp(const void * a, const void * b) 158 | { 159 | return ((struct POINTREF*)a)->dis > ((struct POINTREF*)b)->dis ? 1:-1; 160 | } 161 | 162 | template 163 | T ICP3D::Run(T * data, size_t n, Matrix & R, Matrix & t) 164 | { 165 | return Run(data, n, R, t, max_iter_def, err_diff_def); 166 | } 167 | 168 | template 169 | T ICP3D::Run(T * data, size_t n, Matrix & R, Matrix & t, size_t max_iter) 170 | { 171 | return Run(data, n, R, t, max_iter, err_diff_def); 172 | } 173 | 174 | template 175 | T ICP3D::Run(T * data, size_t n, Matrix & R, Matrix & t, T err_diff) 176 | { 177 | return Run(data, n, R, t, max_iter_def, err_diff); 178 | } 179 | 180 | template 181 | T ICP3D::Run(T * data, size_t n, Matrix & R, Matrix & t, size_t max_iter, T err_diff) 182 | { 183 | size_t num; 184 | 185 | T query[3]; 186 | std::vector ret_index(1); 187 | std::vector out_dist_sqr(1); 188 | 189 | if(do_trim) 190 | { 191 | num = (int)(n*(1-trim_fraction)); 192 | } 193 | else 194 | { 195 | num = n; 196 | } 197 | 198 | struct POINTREF * points = (struct POINTREF *)malloc(sizeof(struct POINTREF)*n); 199 | 200 | // init matrix for point correspondences 201 | Matrix p_m(num,3); // model 202 | Matrix p_d(num,3); // data 203 | 204 | // init mean 205 | Matrix mu_m(1,3); 206 | Matrix mu_d(1,3); 207 | 208 | size_t iter, idx, i; 209 | T err = -1, err_new; 210 | for(iter = 0; iter < max_iter; iter++) 211 | { 212 | T r00 = R.val[0][0]; T r01 = R.val[0][1]; T r02 = R.val[0][2]; 213 | T r10 = R.val[1][0]; T r11 = R.val[1][1]; T r12 = R.val[1][2]; 214 | T r20 = R.val[2][0]; T r21 = R.val[2][1]; T r22 = R.val[2][2]; 215 | T t0 = t.val[0][0]; T t1 = t.val[1][0]; T t2 = t.val[2][0]; 216 | 217 | err_new = 0; 218 | for(i = 0; i < n; i++) 219 | { 220 | idx = i*3; 221 | 222 | //transform point according to R and T 223 | query[0] = r00*data[idx+0] + r01*data[idx+1] + r02*data[idx+2] + t0; 224 | query[1] = r10*data[idx+0] + r11*data[idx+1] + r12*data[idx+2] + t1; 225 | query[2] = r20*data[idx+0] + r21*data[idx+1] + r22*data[idx+2] + t2; 226 | 227 | 228 | //search nearest neighbor 229 | kdtree->knnSearch(&query[0], 1, &ret_index[0], &out_dist_sqr[0]); 230 | 231 | points[i].dis = out_dist_sqr[0]; 232 | points[i].id_data = i; 233 | points[i].id_model = ret_index[0]; 234 | } 235 | 236 | if(do_trim) 237 | { 238 | qsort(points, n, sizeof(struct POINTREF), cmp); 239 | } 240 | 241 | for(i = 0; i < num; i++) 242 | { 243 | // set model point 244 | p_m.val[i][0] = model_.pts[points[i].id_model].x; mu_m.val[0][0] += p_m.val[i][0]; 245 | p_m.val[i][1] = model_.pts[points[i].id_model].y; mu_m.val[0][1] += p_m.val[i][1]; 246 | p_m.val[i][2] = model_.pts[points[i].id_model].z; mu_m.val[0][2] += p_m.val[i][2]; 247 | 248 | idx = points[i].id_data*3; 249 | // set query point 250 | p_d.val[i][0] = r00*data[idx+0] + r01*data[idx+1] + r02*data[idx+2] + t0; mu_d.val[0][0] += p_d.val[i][0]; 251 | p_d.val[i][1] = r10*data[idx+0] + r11*data[idx+1] + r12*data[idx+2] + t1; mu_d.val[0][1] += p_d.val[i][1]; 252 | p_d.val[i][2] = r20*data[idx+0] + r21*data[idx+1] + r22*data[idx+2] + t2; mu_d.val[0][2] += p_d.val[i][2]; 253 | 254 | err_new += points[i].dis; 255 | } 256 | 257 | if(err > 0 && err - err_new < err_diff*num) 258 | break; 259 | err = err_new; 260 | 261 | // subtract mean 262 | mu_m = mu_m/(T)n; 263 | mu_d = mu_d/(T)n; 264 | Matrix q_m = p_m - Matrix::ones(num,1)*mu_m; 265 | Matrix q_t = p_d - Matrix::ones(num,1)*mu_d; 266 | 267 | // compute relative rotation matrix R and translation vector T 268 | Matrix H = ~q_t*q_m; 269 | Matrix U,W,V; 270 | H.svd(U,W,V); 271 | Matrix R_ = V*~U; 272 | 273 | //There are some problems with Matrix::det(), so it is not used 274 | //R11*(R22*R33-R23*R32) 275 | T a = R_.val[0][0]*(R_.val[1][1]* R_.val[2][2] - R_.val[1][2]*R_.val[2][1]); 276 | //R12*(R21*R33-R23*R31) 277 | T b = -R_.val[0][1]*(R_.val[1][0]* R_.val[2][2] - R_.val[1][2]*R_.val[2][0]); 278 | //R13*(R21*R32-R22*R31) 279 | T c = R_.val[0][2]*(R_.val[1][0]* R_.val[2][1] - R_.val[1][1]*R_.val[2][0]); 280 | T det = a+b+c; 281 | 282 | Matrix tmp = Matrix::eye(3); 283 | tmp.val[2][2] = det; 284 | 285 | R_ = V*tmp*~U; 286 | 287 | Matrix t_ = ~mu_m - R_*~mu_d; 288 | 289 | // compose transformation 290 | R = R_*R; 291 | t = R_*t + t_; 292 | } 293 | 294 | return err_new; 295 | } 296 | 297 | 298 | #endif 299 | -------------------------------------------------------------------------------- /jly_main.cpp: -------------------------------------------------------------------------------- 1 | /******************************************************************** 2 | Main Function for point cloud registration with Go-ICP Algorithm 3 | Last modified: Feb 13, 2014 4 | 5 | "Go-ICP: Solving 3D Registration Efficiently and Globally Optimally" 6 | Jiaolong Yang, Hongdong Li, Yunde Jia 7 | International Conference on Computer Vision (ICCV), 2013 8 | 9 | Copyright (C) 2013 Jiaolong Yang (BIT and ANU) 10 | 11 | This program is free software: you can redistribute it and/or modify 12 | it under the terms of the GNU General Public License as published by 13 | the Free Software Foundation, either version 3 of the License, or 14 | (at your option) any later version. 15 | 16 | This program is distributed in the hope that it will be useful, 17 | but WITHOUT ANY WARRANTY; without even the implied warranty of 18 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 19 | GNU General Public License for more details. 20 | 21 | You should have received a copy of the GNU General Public License 22 | along with this program. If not, see . 23 | *********************************************************************/ 24 | 25 | #include 26 | #include 27 | #include 28 | using namespace std; 29 | 30 | #include "jly_goicp.h" 31 | #include "ConfigMap.hpp" 32 | 33 | #define DEFAULT_OUTPUT_FNAME "output.txt" 34 | #define DEFAULT_CONFIG_FNAME "config.txt" 35 | #define DEFAULT_MODEL_FNAME "model.txt" 36 | #define DEFAULT_DATA_FNAME "data.txt" 37 | 38 | void parseInput(int argc, char **argv, string & modelFName, string & dataFName, int & NdDownsampled, string & configFName, string & outputFName); 39 | void readConfig(string FName, GoICP & goicp); 40 | int loadPointCloud(string FName, int & N, POINT3D ** p); 41 | 42 | int main(int argc, char** argv) 43 | { 44 | int Nm, Nd, NdDownsampled; 45 | clock_t clockBegin, clockEnd; 46 | string modelFName, dataFName, configFName, outputFname; 47 | POINT3D * pModel, * pData; 48 | GoICP goicp; 49 | 50 | parseInput(argc, argv, modelFName, dataFName, NdDownsampled, configFName, outputFname); 51 | readConfig(configFName, goicp); 52 | 53 | // Load model and data point clouds 54 | loadPointCloud(modelFName, Nm, &pModel); 55 | loadPointCloud(dataFName, Nd, &pData); 56 | 57 | goicp.pModel = pModel; 58 | goicp.Nm = Nm; 59 | goicp.pData = pData; 60 | goicp.Nd = Nd; 61 | 62 | // Build Distance Transform 63 | cout << "Building Distance Transform..." << flush; 64 | clockBegin = clock(); 65 | goicp.BuildDT(); 66 | clockEnd = clock(); 67 | cout << (double)(clockEnd - clockBegin)/CLOCKS_PER_SEC << "s (CPU)" << endl; 68 | 69 | // Run GO-ICP 70 | if(NdDownsampled > 0) 71 | { 72 | goicp.Nd = NdDownsampled; // Only use first NdDownsampled data points (assumes data points are randomly ordered) 73 | } 74 | cout << "Model ID: " << modelFName << " (" << goicp.Nm << "), Data ID: " << dataFName << " (" << goicp.Nd << ")" << endl; 75 | cout << "Registering..." << endl; 76 | clockBegin = clock(); 77 | goicp.Register(); 78 | clockEnd = clock(); 79 | double time = (double)(clockEnd - clockBegin)/CLOCKS_PER_SEC; 80 | cout << "Optimal Rotation Matrix:" << endl; 81 | cout << goicp.optR << endl; 82 | cout << "Optimal Translation Vector:" << endl; 83 | cout << goicp.optT << endl; 84 | cout << "Finished in " << time << endl; 85 | 86 | ofstream ofile; 87 | ofile.open(outputFname.c_str(), ofstream::out); 88 | ofile << time << endl; 89 | ofile << goicp.optR << endl; 90 | ofile << goicp.optT << endl; 91 | ofile.close(); 92 | 93 | delete(pModel); 94 | delete(pData); 95 | 96 | return 0; 97 | } 98 | 99 | void parseInput(int argc, char **argv, string & modelFName, string & dataFName, int & NdDownsampled, string & configFName, string & outputFName) 100 | { 101 | // Set default values 102 | modelFName = DEFAULT_MODEL_FNAME; 103 | dataFName = DEFAULT_DATA_FNAME; 104 | configFName = DEFAULT_CONFIG_FNAME; 105 | outputFName = DEFAULT_OUTPUT_FNAME; 106 | NdDownsampled = 0; // No downsampling 107 | 108 | //cout << endl; 109 | //cout << "USAGE:" << "./GOICP " << endl; 110 | //cout << endl; 111 | 112 | if(argc > 5) 113 | { 114 | outputFName = argv[5]; 115 | } 116 | if(argc > 4) 117 | { 118 | configFName = argv[4]; 119 | } 120 | if(argc > 3) 121 | { 122 | NdDownsampled = atoi(argv[3]); 123 | } 124 | if(argc > 2) 125 | { 126 | dataFName = argv[2]; 127 | } 128 | if(argc > 1) 129 | { 130 | modelFName = argv[1]; 131 | } 132 | 133 | cout << "INPUT:" << endl; 134 | cout << "(modelFName)->(" << modelFName << ")" << endl; 135 | cout << "(dataFName)->(" << dataFName << ")" << endl; 136 | cout << "(NdDownsampled)->(" << NdDownsampled << ")" << endl; 137 | cout << "(configFName)->(" << configFName << ")" << endl; 138 | cout << "(outputFName)->(" << outputFName << ")" << endl; 139 | cout << endl; 140 | } 141 | 142 | void readConfig(string FName, GoICP & goicp) 143 | { 144 | // Open and parse the associated config file 145 | ConfigMap config(FName.c_str()); 146 | 147 | goicp.MSEThresh = config.getF("MSEThresh"); 148 | goicp.initNodeRot.a = config.getF("rotMinX"); 149 | goicp.initNodeRot.b = config.getF("rotMinY"); 150 | goicp.initNodeRot.c = config.getF("rotMinZ"); 151 | goicp.initNodeRot.w = config.getF("rotWidth"); 152 | goicp.initNodeTrans.x = config.getF("transMinX"); 153 | goicp.initNodeTrans.y = config.getF("transMinY"); 154 | goicp.initNodeTrans.z = config.getF("transMinZ"); 155 | goicp.initNodeTrans.w = config.getF("transWidth"); 156 | goicp.trimFraction = config.getF("trimFraction"); 157 | // If < 0.1% trimming specified, do no trimming 158 | if(goicp.trimFraction < 0.001) 159 | { 160 | goicp.doTrim = false; 161 | } 162 | goicp.dt.SIZE = config.getI("distTransSize"); 163 | goicp.dt.expandFactor = config.getF("distTransExpandFactor"); 164 | 165 | cout << "CONFIG:" << endl; 166 | config.print(); 167 | //cout << "(doTrim)->(" << goicp.doTrim << ")" << endl; 168 | cout << endl; 169 | } 170 | 171 | int loadPointCloud(string FName, int & N, POINT3D ** p) 172 | { 173 | int i; 174 | ifstream ifile; 175 | 176 | ifile.open(FName.c_str(), ifstream::in); 177 | if(!ifile.is_open()) 178 | { 179 | cout << "Unable to open point file '" << FName << "'" << endl; 180 | exit(-1); 181 | } 182 | ifile >> N; // First line has number of points to follow 183 | *p = (POINT3D *)malloc(sizeof(POINT3D) * N); 184 | for(i = 0; i < N; i++) 185 | { 186 | ifile >> (*p)[i].x >> (*p)[i].y >> (*p)[i].z; 187 | } 188 | 189 | ifile.close(); 190 | 191 | return 0; 192 | } 193 | -------------------------------------------------------------------------------- /jly_sorting.hpp: -------------------------------------------------------------------------------- 1 | /******************************************************************** 2 | Sorting/Selection functions for the Go-ICP Algorithm 3 | Last modified: Jan 27, 2015 4 | 5 | "Go-ICP: Solving 3D Registration Efficiently and Globally Optimally" 6 | Jiaolong Yang, Hongdong Li, Yunde Jia 7 | International Conference on Computer Vision (ICCV), 2013 8 | 9 | Copyright (C) 2013 Jiaolong Yang (BIT and ANU) 10 | 11 | This program is free software: you can redistribute it and/or modify 12 | it under the terms of the GNU General Public License as published by 13 | the Free Software Foundation, either version 3 of the License, or 14 | (at your option) any later version. 15 | 16 | This program is distributed in the hope that it will be useful, 17 | but WITHOUT ANY WARRANTY; without even the implied warranty of 18 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 19 | GNU General Public License for more details. 20 | 21 | You should have received a copy of the GNU General Public License 22 | along with this program. If not, see . 23 | *********************************************************************/ 24 | 25 | #ifndef JLY_SORING_HPP 26 | #define JLY_SORING_HPP 27 | 28 | #define INTRO_K 5 29 | 30 | #define INSERTION_NUM 5 31 | 32 | //sorting in ascending order 33 | 34 | template 35 | inline size_t median_of_st_mid_en(T * data, size_t st, size_t en) 36 | { 37 | size_t mid = (st+en)/2; 38 | if(data[st] < data[en]) 39 | { 40 | if(data[mid] < data[st])//median is data[st] 41 | return st; 42 | else if(data[mid] < data[en]) //median is data[md] 43 | return mid; 44 | else //median is data[en] 45 | return en; 46 | } 47 | else // data[en] <= data[st] 48 | { 49 | if(data[mid] < data[en])//median is data[en] 50 | return en; 51 | else if(data[mid] < data[st]) //median is data[md] 52 | return mid; 53 | else //median is data[st] 54 | return st; 55 | } 56 | } 57 | 58 | //median of 3 numbers 59 | template 60 | inline size_t median_of_3(T * data, size_t st) 61 | { 62 | T* data_ = data + st; 63 | 64 | if(data_[0] < data_[2]) 65 | { 66 | if(data_[1] < data_[0])//median is data[0] 67 | return st; 68 | else if(data_[1] < data_[2]) //median is data[1] 69 | return st + 1; 70 | else //median is data[2] 71 | return st + 2; 72 | } 73 | else // data[2] <= data[0] 74 | { 75 | if(data[1] < data[2])//median is data[2] 76 | return st + 2; 77 | else if(data[1] < data[0]) //median is data[1] 78 | return st + 1; 79 | else //median is data[0] 80 | return st; 81 | } 82 | } 83 | 84 | //median of 5 numbers with 6 comparisons 85 | template 86 | inline size_t median_of_5(T * data, size_t st) 87 | { 88 | T* data_ = data + st; 89 | T tmp; 90 | 91 | if(data_[0] > data_[1]) 92 | { 93 | tmp = data_[0]; 94 | data_[0] = data_[1]; 95 | data_[1] = tmp; 96 | } 97 | 98 | if(data_[2] > data_[3]) 99 | { 100 | tmp = data_[2]; 101 | data_[2] = data_[3]; 102 | data_[3] = tmp; 103 | } 104 | 105 | if(data_[0] < data_[2]) 106 | { 107 | tmp = data_[4]; 108 | data_[4] = data_[0]; 109 | 110 | if(tmp < data_[1]) 111 | data_[0] = tmp; 112 | else 113 | { 114 | data_[0] = data_[1]; 115 | data_[1] = tmp; 116 | } 117 | } 118 | else 119 | { 120 | tmp = data_[4]; 121 | data_[4] = data_[2]; 122 | 123 | if(tmp < data_[3]) 124 | data_[2] = tmp; 125 | else 126 | { 127 | data_[2] = data_[3]; 128 | data_[3] = tmp; 129 | } 130 | } 131 | 132 | if(data_[0] < data_[2]) 133 | { 134 | if(data_[1] < data_[2]) 135 | return st + 1; 136 | else 137 | return st + 2; 138 | } 139 | else 140 | { 141 | if(data_[0] < data_[3]) 142 | return st; 143 | else 144 | return st + 3; 145 | } 146 | } 147 | 148 | template 149 | size_t median_of_medians(T * data, size_t st, size_t en) 150 | { 151 | size_t l = en-st+1; 152 | size_t numof5 = l / 5; 153 | if(l % 5 != 0) 154 | numof5 ++; 155 | 156 | T tmp; 157 | size_t subst = st, suben = st + 4; 158 | size_t i, medind; 159 | 160 | //fist (numof5 - 1) groups 161 | for(i = 0; i < numof5 - 1; i++, subst += 5, suben += 5) 162 | { 163 | medind = median_of_5(data, subst); 164 | 165 | tmp = data[st+i]; 166 | data[st+i] = data[medind]; 167 | data[medind] = tmp; 168 | } 169 | 170 | //last group 171 | { 172 | switch(en-subst+1) 173 | { 174 | case 3: // 3 elements 175 | case 4: // 4 elements 176 | medind = median_of_3(data, subst); 177 | break; 178 | case 5: // 5 elements 179 | medind = median_of_5(data, subst); 180 | break; 181 | default: // 1 or 2 elements 182 | medind = subst; 183 | break; 184 | } 185 | 186 | tmp = data[st+i]; 187 | data[st+i] = data[medind]; 188 | data[medind] = tmp; 189 | } 190 | 191 | //median of medians 192 | if(numof5 > 5) 193 | return median_of_medians(data, st, st + numof5-1); 194 | else 195 | { 196 | switch(numof5) 197 | { 198 | case 3: // 3 elements 199 | case 4: // 4 elements 200 | return median_of_3(data, st); 201 | break; 202 | case 5: // 5 elements 203 | return median_of_5(data, st); 204 | break; 205 | default: // 1 or 2 elements 206 | return st; 207 | break; 208 | } 209 | } 210 | } 211 | 212 | template 213 | void insertion_sort(T * data, size_t st, size_t en) 214 | { 215 | T tmp; 216 | size_t i, j; 217 | for(i = st+1; i <= en; i++) 218 | for(j = i; j > st && data[j-1] > data[j]; j--) 219 | { 220 | tmp = data[j-1]; 221 | data[j-1] = data[j]; 222 | data[j] = tmp; 223 | } 224 | } 225 | 226 | // Sort the given array in ascending order 227 | // Stop immediately after the array is splitted into k small numbers and n-k large numbers 228 | template 229 | void intro_select(T * data, size_t st, size_t en, size_t k) 230 | { 231 | T pivot; 232 | T tmp; 233 | 234 | //for(; st < en && data[st] > 0; st++); 235 | 236 | size_t l_pre = en-st+1; 237 | size_t l; 238 | size_t medind; 239 | 240 | bool quickselect = true; 241 | 242 | size_t i = 0; 243 | while(1) 244 | { 245 | if(st >= en) 246 | break; 247 | 248 | if(en - st <= INSERTION_NUM) 249 | { 250 | insertion_sort(data, st, en); 251 | return; 252 | } 253 | 254 | if(quickselect && i++ == INTRO_K) 255 | { 256 | // switch to 'median of medians' if INTRO_K partations of quickselect fail to half the size 257 | l = en-st+1; 258 | if(l*2 > l_pre) 259 | quickselect = false; 260 | 261 | l_pre = l; 262 | i = 0; 263 | } 264 | 265 | if(quickselect) 266 | //medind = st; 267 | medind = median_of_st_mid_en(data, st, en); 268 | else 269 | medind = median_of_medians(data, st, en); 270 | 271 | if(medind != st) 272 | { 273 | tmp = data[st]; 274 | data[st] = data[medind]; 275 | data[medind] = tmp; 276 | } 277 | 278 | size_t p = st; 279 | size_t left = st+1; 280 | size_t right = en; 281 | pivot = data[p]; 282 | 283 | while(1) 284 | { 285 | while (left < right && pivot >= data[left]) 286 | ++left; 287 | while (left < right && pivot <= data[right]) 288 | --right; 289 | 290 | if (left >= right) 291 | break; 292 | 293 | //swap left & right 294 | tmp = data[left]; 295 | data[left] = data[right]; 296 | data[right] = tmp; 297 | } 298 | 299 | size_t s = left-1; 300 | if(data[left] < pivot) 301 | s = left; 302 | //swap p & s 303 | data[p] = data[s]; 304 | data[s] = pivot; 305 | 306 | if(s < k) 307 | st = s+1; 308 | else if(s > k) 309 | en = s-1; 310 | else //s == k 311 | break; 312 | } 313 | } 314 | 315 | #endif 316 | -------------------------------------------------------------------------------- /matrix.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright 2011. All rights reserved. 3 | Institute of Measurement and Control Systems 4 | Karlsruhe Institute of Technology, Germany 5 | 6 | Authors: Andreas Geiger 7 | 8 | matrix is free software; you can redistribute it and/or modify it under the 9 | terms of the GNU General Public License as published by the Free Software 10 | Foundation; either version 2 of the License, or any later version. 11 | 12 | matrix is distributed in the hope that it will be useful, but WITHOUT ANY 13 | WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A 14 | PARTICULAR PURPOSE. See the GNU General Public License for more details. 15 | 16 | You should have received a copy of the GNU General Public License along with 17 | matrix; if not, write to the Free Software Foundation, Inc., 51 Franklin 18 | Street, Fifth Floor, Boston, MA 02110-1301, USA 19 | */ 20 | 21 | #include "matrix.h" 22 | #include 23 | 24 | #define SWAP(a,b) {temp=a;a=b;b=temp;} 25 | #define SIGN(a,b) ((b) >= 0.0 ? fabs(a) : -fabs(a)) 26 | static FLOAT sqrarg; 27 | #define SQR(a) ((sqrarg=(a)) == 0.0 ? 0.0 : sqrarg*sqrarg) 28 | static FLOAT maxarg1,maxarg2; 29 | #define FMAX(a,b) (maxarg1=(a),maxarg2=(b),(maxarg1) > (maxarg2) ? (maxarg1) : (maxarg2)) 30 | static int32_t iminarg1,iminarg2; 31 | #define IMIN(a,b) (iminarg1=(a),iminarg2=(b),(iminarg1) < (iminarg2) ? (iminarg1) : (iminarg2)) 32 | 33 | 34 | using namespace std; 35 | 36 | Matrix::Matrix () { 37 | m = 0; 38 | n = 0; 39 | val = 0; 40 | } 41 | 42 | Matrix::Matrix (const int32_t m_,const int32_t n_) { 43 | allocateMemory(m_,n_); 44 | } 45 | 46 | Matrix::Matrix (const int32_t m_,const int32_t n_,const FLOAT* val_) { 47 | allocateMemory(m_,n_); 48 | int32_t k=0; 49 | for (int32_t i=0; i0) 71 | for (int32_t i=0; i=m || j1<0 || j2>=n || i2m || j1+M.n>n) { 104 | cerr << "ERROR: Cannot set submatrix [" << i1 << ".." << i1+M.m-1 << 105 | "] x [" << j1 << ".." << j1+M.n-1 << "]" << 106 | " of a (" << m << "x" << n << ") matrix." << endl; 107 | exit(0); 108 | } 109 | for (int32_t i=0; i idx) { 137 | Matrix M(m,idx.size()); 138 | for (int32_t j=0; j1 && M.n==1) { 170 | Matrix D(M.m,M.m); 171 | for (int32_t i=0; i1) { 175 | Matrix D(M.n,M.n); 176 | for (int32_t i=0; i=big) { 455 | big=fabs(A.val[j][k]); 456 | irow=j; 457 | icol=k; 458 | } 459 | ++(ipiv[icol]); 460 | 461 | // We now have the pivot element, so we interchange rows, if needed, to put the pivot 462 | // element on the diagonal. The columns are not physically interchanged, only relabeled. 463 | if (irow != icol) { 464 | for (l=0;l=0;l--) { 498 | if (indxr[l]!=indxc[l]) 499 | for (k=0;kbig) 532 | big = temp; 533 | if (big == 0.0) { // No nonzero largest element. 534 | free(vv); 535 | return false; 536 | } 537 | vv[i] = 1.0/big; // Save the scaling. 538 | } 539 | for (j=0; j=big) { 553 | big = dum; 554 | imax = i; 555 | } 556 | } 557 | if (j!=imax) { // Do we need to interchange rows? 558 | for (k=0; k=0;i--) { // Accumulation of right-hand transformations. 642 | if (i=0;i--) { // Accumulation of left-hand transformations. 658 | l = i+1; 659 | g = w[i]; 660 | for (j=l;j=0;k--) { // Diagonalization of the bidiagonal form: Loop over singular values, 673 | for (its=0;its<30;its++) { // and over allowed iterations. 674 | flag = 1; 675 | for (l=k;l>=0;l--) { // Test for splitting. 676 | nm = l-1; 677 | if ((FLOAT)(fabs(rv1[l])+anorm) == anorm) { flag = 0; break; } 678 | if ((FLOAT)(fabs( w[nm])+anorm) == anorm) { break; } 679 | } 680 | if (flag) { 681 | c = 0.0; // Cancellation of rv1[l], if l > 1. 682 | s = 1.0; 683 | for (i=l;i<=k;i++) { 684 | f = s*rv1[i]; 685 | rv1[i] = c*rv1[i]; 686 | if ((FLOAT)(fabs(f)+anorm) == anorm) break; 687 | g = w[i]; 688 | h = pythag(f,g); 689 | w[i] = h; 690 | h = 1.0/h; 691 | c = g*h; 692 | s = -f*h; 693 | for (j=0;j 1); 789 | for (k=0;k (m+n)/2) { 794 | for (i=0;i absb) 853 | return absa*sqrt(1.0+SQR(absb/absa)); 854 | else 855 | return (absb == 0.0 ? 0.0 : absb*sqrt(1.0+SQR(absa/absb))); 856 | } 857 | 858 | -------------------------------------------------------------------------------- /matrix.h: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright 2011. All rights reserved. 3 | Institute of Measurement and Control Systems 4 | Karlsruhe Institute of Technology, Germany 5 | 6 | Authors: Andreas Geiger 7 | 8 | matrix is free software; you can redistribute it and/or modify it under the 9 | terms of the GNU General Public License as published by the Free Software 10 | Foundation; either version 2 of the License, or any later version. 11 | 12 | matrix is distributed in the hope that it will be useful, but WITHOUT ANY 13 | WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A 14 | PARTICULAR PURPOSE. See the GNU General Public License for more details. 15 | 16 | You should have received a copy of the GNU General Public License along with 17 | matrix; if not, write to the Free Software Foundation, Inc., 51 Franklin 18 | Street, Fifth Floor, Boston, MA 02110-1301, USA 19 | */ 20 | 21 | #ifndef MATRIX_H 22 | #define MATRIX_H 23 | 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | 30 | #ifndef _MSC_VER 31 | #include 32 | #else 33 | typedef __int8 int8_t; 34 | typedef __int16 int16_t; 35 | typedef __int32 int32_t; 36 | typedef __int64 int64_t; 37 | typedef unsigned __int8 uint8_t; 38 | typedef unsigned __int16 uint16_t; 39 | typedef unsigned __int32 uint32_t; 40 | typedef unsigned __int64 uint64_t; 41 | #endif 42 | 43 | #define endll endl << endl // double end line definition 44 | 45 | typedef double FLOAT; // double precision 46 | //typedef float FLOAT; // single precision 47 | 48 | class Matrix { 49 | 50 | public: 51 | 52 | // constructor / deconstructor 53 | Matrix (); // init empty 0x0 matrix 54 | Matrix (const int32_t m,const int32_t n); // init empty mxn matrix 55 | Matrix (const int32_t m,const int32_t n,const FLOAT* val_); // init mxn matrix with values from array 'val' 56 | Matrix (const Matrix &M); // creates deepcopy of M 57 | ~Matrix (); 58 | 59 | // assignment operator, copies contents of M 60 | Matrix& operator= (const Matrix &M); 61 | 62 | // copies submatrix of M into array 'val', default values copy whole row/column/matrix 63 | void getData(FLOAT* val_,int32_t i1=0,int32_t j1=0,int32_t i2=-1,int32_t j2=-1); 64 | 65 | // set or get submatrices of current matrix 66 | Matrix getMat(int32_t i1,int32_t j1,int32_t i2=-1,int32_t j2=-1); 67 | void setMat(const Matrix &M,const int32_t i,const int32_t j); 68 | 69 | // set sub-matrix to scalar (default 0), -1 as end replaces whole row/column/matrix 70 | void setVal(FLOAT s,int32_t i1=0,int32_t j1=0,int32_t i2=-1,int32_t j2=-1); 71 | 72 | // set (part of) diagonal to scalar, -1 as end replaces whole diagonal 73 | void setDiag(FLOAT s,int32_t i1=0,int32_t i2=-1); 74 | 75 | // clear matrix 76 | void zero(); 77 | 78 | // extract columns with given index 79 | Matrix extractCols (std::vector idx); 80 | 81 | // create identity matrix 82 | static Matrix eye (const int32_t m); 83 | void eye (); 84 | 85 | // create matrix with ones 86 | static Matrix ones(const int32_t m,const int32_t n); 87 | 88 | // create diagonal matrix with nx1 or 1xn matrix M as elements 89 | static Matrix diag(const Matrix &M); 90 | 91 | // returns the m-by-n matrix whose elements are taken column-wise from M 92 | static Matrix reshape(const Matrix &M,int32_t m,int32_t n); 93 | 94 | // create 3x3 rotation matrices (convention: http://en.wikipedia.org/wiki/Rotation_matrix) 95 | static Matrix rotMatX(const FLOAT &angle); 96 | static Matrix rotMatY(const FLOAT &angle); 97 | static Matrix rotMatZ(const FLOAT &angle); 98 | 99 | // simple arithmetic operations 100 | Matrix operator+ (const Matrix &M); // add matrix 101 | Matrix operator- (const Matrix &M); // subtract matrix 102 | Matrix operator* (const Matrix &M); // multiply with matrix 103 | Matrix operator* (const FLOAT &s); // multiply with scalar 104 | Matrix operator/ (const Matrix &M); // divide elementwise by matrix (or vector) 105 | Matrix operator/ (const FLOAT &s); // divide by scalar 106 | Matrix operator- (); // negative matrix 107 | Matrix operator~ (); // transpose 108 | FLOAT l2norm (); // euclidean norm (vectors) / frobenius norm (matrices) 109 | FLOAT mean (); // mean of all elements in matrix 110 | 111 | // complex arithmetic operations 112 | static Matrix cross (const Matrix &a, const Matrix &b); // cross product of two vectors 113 | static Matrix inv (const Matrix &M); // invert matrix M 114 | bool inv (); // invert this matrix 115 | FLOAT det (); // returns determinant of matrix 116 | bool solve (const Matrix &M,FLOAT eps=1e-20); // solve linear system M*x=B, replaces *this and M 117 | bool lu(int32_t *idx, FLOAT &d, FLOAT eps=1e-20); // replace *this by lower upper decomposition 118 | void svd(Matrix &U,Matrix &W,Matrix &V); // singular value decomposition *this = U*diag(W)*V^T 119 | 120 | // print matrix to stream 121 | friend std::ostream& operator<< (std::ostream& out,const Matrix& M); 122 | 123 | // direct data access 124 | FLOAT **val; 125 | int32_t m,n; 126 | 127 | private: 128 | 129 | void allocateMemory (const int32_t m_,const int32_t n_); 130 | void releaseMemory (); 131 | inline FLOAT pythag(FLOAT a,FLOAT b); 132 | 133 | }; 134 | 135 | #endif // MATRIX_H 136 | -------------------------------------------------------------------------------- /nanoflann.hpp: -------------------------------------------------------------------------------- 1 | /*********************************************************************** 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright 2008-2009 Marius Muja (mariusm@cs.ubc.ca). All rights reserved. 5 | * Copyright 2008-2009 David G. Lowe (lowe@cs.ubc.ca). All rights reserved. 6 | * Copyright 2011-2013 Jose Luis Blanco (joseluisblancoc@gmail.com). 7 | * All rights reserved. 8 | * 9 | * THE BSD LICENSE 10 | * 11 | * Redistribution and use in source and binary forms, with or without 12 | * modification, are permitted provided that the following conditions 13 | * are met: 14 | * 15 | * 1. Redistributions of source code must retain the above copyright 16 | * notice, this list of conditions and the following disclaimer. 17 | * 2. Redistributions in binary form must reproduce the above copyright 18 | * notice, this list of conditions and the following disclaimer in the 19 | * documentation and/or other materials provided with the distribution. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR 22 | * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES 23 | * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. 24 | * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT 26 | * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 27 | * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 28 | * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 29 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF 30 | * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 31 | *************************************************************************/ 32 | 33 | #ifndef NANOFLANN_HPP_ 34 | #define NANOFLANN_HPP_ 35 | 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include // for fwrite() 41 | #include // for fabs(),... 42 | #include 43 | 44 | // Avoid conflicting declaration of min/max macros in windows headers 45 | #if !defined(NOMINMAX) && (defined(_WIN32) || defined(_WIN32_) || defined(WIN32) || defined(_WIN64)) 46 | # define NOMINMAX 47 | # ifdef max 48 | # undef max 49 | # undef min 50 | # endif 51 | #endif 52 | 53 | namespace nanoflann 54 | { 55 | /** @addtogroup nanoflann_grp nanoflann C++ library for ANN 56 | * @{ */ 57 | 58 | /** Library version: 0xMmP (M=Major,m=minor,P=path) */ 59 | #define NANOFLANN_VERSION 0x116 60 | 61 | /** @addtogroup result_sets_grp Result set classes 62 | * @{ */ 63 | template 64 | class KNNResultSet 65 | { 66 | IndexType * indices; 67 | DistanceType* dists; 68 | CountType capacity; 69 | CountType count; 70 | 71 | public: 72 | inline KNNResultSet(CountType capacity_) : capacity(capacity_), count(0) 73 | { 74 | } 75 | 76 | inline void init(IndexType* indices_, DistanceType* dists_) 77 | { 78 | indices = indices_; 79 | dists = dists_; 80 | count = 0; 81 | dists[capacity-1] = (std::numeric_limits::max)(); 82 | } 83 | 84 | inline CountType size() const 85 | { 86 | return count; 87 | } 88 | 89 | inline bool full() const 90 | { 91 | return count == capacity; 92 | } 93 | 94 | 95 | inline void addPoint(DistanceType dist, IndexType index) 96 | { 97 | CountType i; 98 | for (i=count; i>0; --i) { 99 | #ifdef NANOFLANN_FIRST_MATCH // If defined and two poins have the same distance, the one with the lowest-index will be returned first. 100 | if ( (dists[i-1]>dist) || ((dist==dists[i-1])&&(indices[i-1]>index)) ) { 101 | #else 102 | if (dists[i-1]>dist) { 103 | #endif 104 | if (i 129 | class RadiusResultSet 130 | { 131 | public: 132 | const DistanceType radius; 133 | 134 | std::vector >& m_indices_dists; 135 | 136 | inline RadiusResultSet(DistanceType radius_, std::vector >& indices_dists) : radius(radius_), m_indices_dists(indices_dists) 137 | { 138 | init(); 139 | } 140 | 141 | inline ~RadiusResultSet() { } 142 | 143 | inline void init() { clear(); } 144 | inline void clear() { m_indices_dists.clear(); } 145 | 146 | inline size_t size() const { return m_indices_dists.size(); } 147 | 148 | inline bool full() const { return true; } 149 | 150 | inline void addPoint(DistanceType dist, IndexType index) 151 | { 152 | if (dist(index,dist)); 154 | } 155 | 156 | inline DistanceType worstDist() const { return radius; } 157 | 158 | /** Clears the result set and adjusts the search radius. */ 159 | inline void set_radius_and_clear( const DistanceType r ) 160 | { 161 | radius = r; 162 | clear(); 163 | } 164 | 165 | /** 166 | * Find the worst result (furtherest neighbor) without copying or sorting 167 | * Pre-conditions: size() > 0 168 | */ 169 | std::pair worst_item() const 170 | { 171 | if (m_indices_dists.empty()) throw std::runtime_error("Cannot invoke RadiusResultSet::worst_item() on an empty list of results."); 172 | typedef typename std::vector >::const_iterator DistIt; 173 | DistIt it = std::max_element(m_indices_dists.begin(), m_indices_dists.end()); 174 | return *it; 175 | } 176 | }; 177 | 178 | /** operator "<" for std::sort() */ 179 | struct IndexDist_Sorter 180 | { 181 | /** PairType will be typically: std::pair */ 182 | template 183 | inline bool operator()(const PairType &p1, const PairType &p2) const { 184 | return p1.second < p2.second; 185 | } 186 | }; 187 | 188 | /** @} */ 189 | 190 | 191 | /** @addtogroup loadsave_grp Load/save auxiliary functions 192 | * @{ */ 193 | template 194 | void save_value(FILE* stream, const T& value, size_t count = 1) 195 | { 196 | fwrite(&value, sizeof(value),count, stream); 197 | } 198 | 199 | template 200 | void save_value(FILE* stream, const std::vector& value) 201 | { 202 | size_t size = value.size(); 203 | fwrite(&size, sizeof(size_t), 1, stream); 204 | fwrite(&value[0], sizeof(T), size, stream); 205 | } 206 | 207 | template 208 | void load_value(FILE* stream, T& value, size_t count = 1) 209 | { 210 | size_t read_cnt = fread(&value, sizeof(value), count, stream); 211 | if (read_cnt != count) { 212 | throw std::runtime_error("Cannot read from file"); 213 | } 214 | } 215 | 216 | 217 | template 218 | void load_value(FILE* stream, std::vector& value) 219 | { 220 | size_t size; 221 | size_t read_cnt = fread(&size, sizeof(size_t), 1, stream); 222 | if (read_cnt!=1) { 223 | throw std::runtime_error("Cannot read from file"); 224 | } 225 | value.resize(size); 226 | read_cnt = fread(&value[0], sizeof(T), size, stream); 227 | if (read_cnt!=size) { 228 | throw std::runtime_error("Cannot read from file"); 229 | } 230 | } 231 | /** @} */ 232 | 233 | 234 | /** @addtogroup metric_grp Metric (distance) classes 235 | * @{ */ 236 | 237 | template inline T abs(T x) { return (x<0) ? -x : x; } 238 | template<> inline int abs(int x) { return ::abs(x); } 239 | template<> inline float abs(float x) { return fabsf(x); } 240 | template<> inline double abs(double x) { return fabs(x); } 241 | template<> inline long double abs(long double x) { return fabsl(x); } 242 | 243 | /** Manhattan distance functor (generic version, optimized for high-dimensionality data sets). 244 | * Corresponding distance traits: nanoflann::metric_L1 245 | * \tparam T Type of the elements (e.g. double, float, uint8_t) 246 | * \tparam DistanceType Type of distance variables (must be signed) (e.g. float, double, int64_t) 247 | */ 248 | template 249 | struct L1_Adaptor 250 | { 251 | typedef T ElementType; 252 | typedef _DistanceType DistanceType; 253 | 254 | const DataSource &data_source; 255 | 256 | L1_Adaptor(const DataSource &_data_source) : data_source(_data_source) { } 257 | 258 | inline DistanceType operator()(const T* a, const size_t b_idx, size_t size, DistanceType worst_dist = -1) const 259 | { 260 | DistanceType result = DistanceType(); 261 | const T* last = a + size; 262 | const T* lastgroup = last - 3; 263 | size_t d = 0; 264 | 265 | /* Process 4 items with each loop for efficiency. */ 266 | while (a < lastgroup) { 267 | const DistanceType diff0 = nanoflann::abs(a[0] - data_source.kdtree_get_pt(b_idx,d++)); 268 | const DistanceType diff1 = nanoflann::abs(a[1] - data_source.kdtree_get_pt(b_idx,d++)); 269 | const DistanceType diff2 = nanoflann::abs(a[2] - data_source.kdtree_get_pt(b_idx,d++)); 270 | const DistanceType diff3 = nanoflann::abs(a[3] - data_source.kdtree_get_pt(b_idx,d++)); 271 | result += diff0 + diff1 + diff2 + diff3; 272 | a += 4; 273 | if ((worst_dist>0)&&(result>worst_dist)) { 274 | return result; 275 | } 276 | } 277 | /* Process last 0-3 components. Not needed for standard vector lengths. */ 278 | while (a < last) { 279 | result += nanoflann::abs( *a++ - data_source.kdtree_get_pt(b_idx,d++) ); 280 | } 281 | return result; 282 | } 283 | 284 | template 285 | inline DistanceType accum_dist(const U a, const V b, int ) const 286 | { 287 | return nanoflann::abs(a-b); 288 | } 289 | }; 290 | 291 | /** Squared Euclidean distance functor (generic version, optimized for high-dimensionality data sets). 292 | * Corresponding distance traits: nanoflann::metric_L2 293 | * \tparam T Type of the elements (e.g. double, float, uint8_t) 294 | * \tparam DistanceType Type of distance variables (must be signed) (e.g. float, double, int64_t) 295 | */ 296 | template 297 | struct L2_Adaptor 298 | { 299 | typedef T ElementType; 300 | typedef _DistanceType DistanceType; 301 | 302 | const DataSource &data_source; 303 | 304 | L2_Adaptor(const DataSource &_data_source) : data_source(_data_source) { } 305 | 306 | inline DistanceType operator()(const T* a, const size_t b_idx, size_t size, DistanceType worst_dist = -1) const 307 | { 308 | DistanceType result = DistanceType(); 309 | const T* last = a + size; 310 | const T* lastgroup = last - 3; 311 | size_t d = 0; 312 | 313 | /* Process 4 items with each loop for efficiency. */ 314 | while (a < lastgroup) { 315 | const DistanceType diff0 = a[0] - data_source.kdtree_get_pt(b_idx,d++); 316 | const DistanceType diff1 = a[1] - data_source.kdtree_get_pt(b_idx,d++); 317 | const DistanceType diff2 = a[2] - data_source.kdtree_get_pt(b_idx,d++); 318 | const DistanceType diff3 = a[3] - data_source.kdtree_get_pt(b_idx,d++); 319 | result += diff0 * diff0 + diff1 * diff1 + diff2 * diff2 + diff3 * diff3; 320 | a += 4; 321 | if ((worst_dist>0)&&(result>worst_dist)) { 322 | return result; 323 | } 324 | } 325 | /* Process last 0-3 components. Not needed for standard vector lengths. */ 326 | while (a < last) { 327 | const DistanceType diff0 = *a++ - data_source.kdtree_get_pt(b_idx,d++); 328 | result += diff0 * diff0; 329 | } 330 | return result; 331 | } 332 | 333 | template 334 | inline DistanceType accum_dist(const U a, const V b, int ) const 335 | { 336 | return (a-b)*(a-b); 337 | } 338 | }; 339 | 340 | /** Squared Euclidean distance functor (suitable for low-dimensionality datasets, like 2D or 3D point clouds) 341 | * Corresponding distance traits: nanoflann::metric_L2_Simple 342 | * \tparam T Type of the elements (e.g. double, float, uint8_t) 343 | * \tparam DistanceType Type of distance variables (must be signed) (e.g. float, double, int64_t) 344 | */ 345 | template 346 | struct L2_Simple_Adaptor 347 | { 348 | typedef T ElementType; 349 | typedef _DistanceType DistanceType; 350 | 351 | const DataSource &data_source; 352 | 353 | L2_Simple_Adaptor(const DataSource &_data_source) : data_source(_data_source) { } 354 | 355 | inline DistanceType operator()(const T* a, const size_t b_idx, size_t size) const { 356 | return data_source.kdtree_distance(a,b_idx,size); 357 | } 358 | 359 | template 360 | inline DistanceType accum_dist(const U a, const V b, int ) const 361 | { 362 | return (a-b)*(a-b); 363 | } 364 | }; 365 | 366 | /** Metaprogramming helper traits class for the L1 (Manhattan) metric */ 367 | struct metric_L1 { 368 | template 369 | struct traits { 370 | typedef L1_Adaptor distance_t; 371 | }; 372 | }; 373 | /** Metaprogramming helper traits class for the L2 (Euclidean) metric */ 374 | struct metric_L2 { 375 | template 376 | struct traits { 377 | typedef L2_Adaptor distance_t; 378 | }; 379 | }; 380 | /** Metaprogramming helper traits class for the L2_simple (Euclidean) metric */ 381 | struct metric_L2_Simple { 382 | template 383 | struct traits { 384 | typedef L2_Simple_Adaptor distance_t; 385 | }; 386 | }; 387 | 388 | /** @} */ 389 | 390 | 391 | 392 | /** @addtogroup param_grp Parameter structs 393 | * @{ */ 394 | 395 | /** Parameters (see http://code.google.com/p/nanoflann/ for help choosing the parameters) 396 | */ 397 | struct KDTreeSingleIndexAdaptorParams 398 | { 399 | KDTreeSingleIndexAdaptorParams(size_t _leaf_max_size = 10, int dim_ = -1) : 400 | leaf_max_size(_leaf_max_size), dim(dim_) 401 | {} 402 | 403 | size_t leaf_max_size; 404 | int dim; 405 | }; 406 | 407 | /** Search options for KDTreeSingleIndexAdaptor::findNeighbors() */ 408 | struct SearchParams 409 | { 410 | /** Note: The first argument (checks_IGNORED_) is ignored, but kept for compatibility with the FLANN interface */ 411 | SearchParams(int checks_IGNORED_ = 32, float eps_ = 0, bool sorted_ = true ) : 412 | checks(checks_IGNORED_), eps(eps_), sorted(sorted_) {} 413 | 414 | int checks; //!< Ignored parameter (Kept for compatibility with the FLANN interface). 415 | float eps; //!< search for eps-approximate neighbours (default: 0) 416 | bool sorted; //!< only for radius search, require neighbours sorted by distance (default: true) 417 | }; 418 | /** @} */ 419 | 420 | 421 | /** @addtogroup memalloc_grp Memory allocation 422 | * @{ */ 423 | 424 | /** 425 | * Allocates (using C's malloc) a generic type T. 426 | * 427 | * Params: 428 | * count = number of instances to allocate. 429 | * Returns: pointer (of type T*) to memory buffer 430 | */ 431 | template 432 | inline T* allocate(size_t count = 1) 433 | { 434 | T* mem = (T*) ::malloc(sizeof(T)*count); 435 | return mem; 436 | } 437 | 438 | 439 | /** 440 | * Pooled storage allocator 441 | * 442 | * The following routines allow for the efficient allocation of storage in 443 | * small chunks from a specified pool. Rather than allowing each structure 444 | * to be freed individually, an entire pool of storage is freed at once. 445 | * This method has two advantages over just using malloc() and free(). First, 446 | * it is far more efficient for allocating small objects, as there is 447 | * no overhead for remembering all the information needed to free each 448 | * object or consolidating fragmented memory. Second, the decision about 449 | * how long to keep an object is made at the time of allocation, and there 450 | * is no need to track down all the objects to free them. 451 | * 452 | */ 453 | 454 | const size_t WORDSIZE=16; 455 | const size_t BLOCKSIZE=8192; 456 | 457 | class PooledAllocator 458 | { 459 | /* We maintain memory alignment to word boundaries by requiring that all 460 | allocations be in multiples of the machine wordsize. */ 461 | /* Size of machine word in bytes. Must be power of 2. */ 462 | /* Minimum number of bytes requested at a time from the system. Must be multiple of WORDSIZE. */ 463 | 464 | 465 | size_t remaining; /* Number of bytes left in current block of storage. */ 466 | void* base; /* Pointer to base of current block of storage. */ 467 | void* loc; /* Current location in block to next allocate memory. */ 468 | size_t blocksize; 469 | 470 | void internal_init() 471 | { 472 | remaining = 0; 473 | base = NULL; 474 | usedMemory = 0; 475 | wastedMemory = 0; 476 | } 477 | 478 | public: 479 | size_t usedMemory; 480 | size_t wastedMemory; 481 | 482 | /** 483 | Default constructor. Initializes a new pool. 484 | */ 485 | PooledAllocator(const size_t blocksize_ = BLOCKSIZE) : blocksize(blocksize_) { 486 | internal_init(); 487 | } 488 | 489 | /** 490 | * Destructor. Frees all the memory allocated in this pool. 491 | */ 492 | ~PooledAllocator() { 493 | free_all(); 494 | } 495 | 496 | /** Frees all allocated memory chunks */ 497 | void free_all() 498 | { 499 | while (base != NULL) { 500 | void *prev = *((void**) base); /* Get pointer to prev block. */ 501 | ::free(base); 502 | base = prev; 503 | } 504 | internal_init(); 505 | } 506 | 507 | /** 508 | * Returns a pointer to a piece of new memory of the given size in bytes 509 | * allocated from the pool. 510 | */ 511 | void* malloc(const size_t req_size) 512 | { 513 | /* Round size up to a multiple of wordsize. The following expression 514 | only works for WORDSIZE that is a power of 2, by masking last bits of 515 | incremented size to zero. 516 | */ 517 | const size_t size = (req_size + (WORDSIZE - 1)) & ~(WORDSIZE - 1); 518 | 519 | /* Check whether a new block must be allocated. Note that the first word 520 | of a block is reserved for a pointer to the previous block. 521 | */ 522 | if (size > remaining) { 523 | 524 | wastedMemory += remaining; 525 | 526 | /* Allocate new storage. */ 527 | const size_t blocksize = (size + sizeof(void*) + (WORDSIZE-1) > BLOCKSIZE) ? 528 | size + sizeof(void*) + (WORDSIZE-1) : BLOCKSIZE; 529 | 530 | // use the standard C malloc to allocate memory 531 | void* m = ::malloc(blocksize); 532 | if (!m) { 533 | fprintf(stderr,"Failed to allocate memory.\n"); 534 | return NULL; 535 | } 536 | 537 | /* Fill first word of new block with pointer to previous block. */ 538 | ((void**) m)[0] = base; 539 | base = m; 540 | 541 | size_t shift = 0; 542 | //int size_t = (WORDSIZE - ( (((size_t)m) + sizeof(void*)) & (WORDSIZE-1))) & (WORDSIZE-1); 543 | 544 | remaining = blocksize - sizeof(void*) - shift; 545 | loc = ((char*)m + sizeof(void*) + shift); 546 | } 547 | void* rloc = loc; 548 | loc = (char*)loc + size; 549 | remaining -= size; 550 | 551 | usedMemory += size; 552 | 553 | return rloc; 554 | } 555 | 556 | /** 557 | * Allocates (using this pool) a generic type T. 558 | * 559 | * Params: 560 | * count = number of instances to allocate. 561 | * Returns: pointer (of type T*) to memory buffer 562 | */ 563 | template 564 | T* allocate(const size_t count = 1) 565 | { 566 | T* mem = (T*) this->malloc(sizeof(T)*count); 567 | return mem; 568 | } 569 | 570 | }; 571 | /** @} */ 572 | 573 | 574 | /** @addtogroup kdtrees_grp KD-tree classes and adaptors 575 | * @{ */ 576 | 577 | /** kd-tree index 578 | * 579 | * Contains the k-d trees and other information for indexing a set of points 580 | * for nearest-neighbor matching. 581 | * 582 | * The class "DatasetAdaptor" must provide the following interface (can be non-virtual, inlined methods): 583 | * 584 | * \code 585 | * // Must return the number of data points 586 | * inline size_t kdtree_get_point_count() const { ... } 587 | * 588 | * // Must return the Euclidean (L2) distance between the vector "p1[0:size-1]" and the data point with index "idx_p2" stored in the class: 589 | * inline DistanceType kdtree_distance(const T *p1, const size_t idx_p2,size_t size) const { ... } 590 | * 591 | * // Must return the dim'th component of the idx'th point in the class: 592 | * inline T kdtree_get_pt(const size_t idx, int dim) const { ... } 593 | * 594 | * // Optional bounding-box computation: return false to default to a standard bbox computation loop. 595 | * // Return true if the BBOX was already computed by the class and returned in "bb" so it can be avoided to redo it again. 596 | * // Look at bb.size() to find out the expected dimensionality (e.g. 2 or 3 for point clouds) 597 | * template 598 | * bool kdtree_get_bbox(BBOX &bb) const 599 | * { 600 | * bb[0].low = ...; bb[0].high = ...; // 0th dimension limits 601 | * bb[1].low = ...; bb[1].high = ...; // 1st dimension limits 602 | * ... 603 | * return true; 604 | * } 605 | * 606 | * \endcode 607 | * 608 | * \tparam IndexType Will be typically size_t or int 609 | */ 610 | template 611 | class KDTreeSingleIndexAdaptor 612 | { 613 | public: 614 | typedef typename Distance::ElementType ElementType; 615 | typedef typename Distance::DistanceType DistanceType; 616 | protected: 617 | 618 | /** 619 | * Array of indices to vectors in the dataset. 620 | */ 621 | std::vector vind; 622 | 623 | size_t m_leaf_max_size; 624 | 625 | 626 | /** 627 | * The dataset used by this index 628 | */ 629 | const DatasetAdaptor &dataset; //!< The source of our data 630 | 631 | const KDTreeSingleIndexAdaptorParams index_params; 632 | 633 | size_t m_size; 634 | int dim; //!< Dimensionality of each data point 635 | 636 | 637 | /*--------------------- Internal Data Structures --------------------------*/ 638 | struct Node 639 | { 640 | union { 641 | struct 642 | { 643 | /** 644 | * Indices of points in leaf node 645 | */ 646 | IndexType left, right; 647 | } lr; 648 | struct 649 | { 650 | /** 651 | * Dimension used for subdivision. 652 | */ 653 | int divfeat; 654 | /** 655 | * The values used for subdivision. 656 | */ 657 | DistanceType divlow, divhigh; 658 | } sub; 659 | }; 660 | /** 661 | * The child nodes. 662 | */ 663 | Node* child1, * child2; 664 | }; 665 | typedef Node* NodePtr; 666 | 667 | 668 | struct Interval 669 | { 670 | ElementType low, high; 671 | }; 672 | 673 | typedef std::vector BoundingBox; 674 | 675 | 676 | /** This record represents a branch point when finding neighbors in 677 | the tree. It contains a record of the minimum distance to the query 678 | point, as well as the node at which the search resumes. 679 | */ 680 | template 681 | struct BranchStruct 682 | { 683 | T node; /* Tree node at which search resumes */ 684 | DistanceType mindist; /* Minimum distance to query for all nodes below. */ 685 | 686 | BranchStruct() {} 687 | BranchStruct(const T& aNode, DistanceType dist) : node(aNode), mindist(dist) {} 688 | 689 | inline bool operator<(const BranchStruct& rhs) const 690 | { 691 | return mindist BranchSt; 700 | typedef BranchSt* Branch; 701 | 702 | BoundingBox root_bbox; 703 | 704 | /** 705 | * Pooled memory allocator. 706 | * 707 | * Using a pooled memory allocator is more efficient 708 | * than allocating memory directly when there is a large 709 | * number small of memory allocations. 710 | */ 711 | PooledAllocator pool; 712 | 713 | public: 714 | 715 | Distance distance; 716 | 717 | /** 718 | * KDTree constructor 719 | * 720 | * Params: 721 | * inputData = dataset with the input features 722 | * params = parameters passed to the kdtree algorithm (see http://code.google.com/p/nanoflann/ for help choosing the parameters) 723 | */ 724 | KDTreeSingleIndexAdaptor(const int dimensionality, const DatasetAdaptor& inputData, const KDTreeSingleIndexAdaptorParams& params = KDTreeSingleIndexAdaptorParams() ) : 725 | dataset(inputData), index_params(params), root_node(NULL), distance(inputData) 726 | { 727 | m_size = dataset.kdtree_get_point_count(); 728 | dim = dimensionality; 729 | if (DIM>0) dim=DIM; 730 | else { 731 | if (params.dim>0) dim = params.dim; 732 | } 733 | m_leaf_max_size = params.leaf_max_size; 734 | 735 | // Create a permutable array of indices to the input vectors. 736 | init_vind(); 737 | } 738 | 739 | /** 740 | * Standard destructor 741 | */ 742 | ~KDTreeSingleIndexAdaptor() 743 | { 744 | } 745 | 746 | /** Frees the previously-built index. Automatically called within buildIndex(). */ 747 | void freeIndex() 748 | { 749 | pool.free_all(); 750 | root_node=NULL; 751 | } 752 | 753 | /** 754 | * Builds the index 755 | */ 756 | void buildIndex() 757 | { 758 | init_vind(); 759 | computeBoundingBox(root_bbox); 760 | freeIndex(); 761 | root_node = divideTree(0, m_size, root_bbox ); // construct the tree 762 | } 763 | 764 | /** 765 | * Returns size of index. 766 | */ 767 | size_t size() const 768 | { 769 | return m_size; 770 | } 771 | 772 | /** 773 | * Returns the length of an index feature. 774 | */ 775 | size_t veclen() const 776 | { 777 | return static_cast(DIM>0 ? DIM : dim); 778 | } 779 | 780 | /** 781 | * Computes the inde memory usage 782 | * Returns: memory used by the index 783 | */ 784 | size_t usedMemory() const 785 | { 786 | return pool.usedMemory+pool.wastedMemory+dataset.kdtree_get_point_count()*sizeof(IndexType); // pool memory and vind array memory 787 | } 788 | 789 | /** \name Query methods 790 | * @{ */ 791 | 792 | /** 793 | * Find set of nearest neighbors to vec[0:dim-1]. Their indices are stored inside 794 | * the result object. 795 | * 796 | * Params: 797 | * result = the result object in which the indices of the nearest-neighbors are stored 798 | * vec = the vector for which to search the nearest neighbors 799 | * 800 | * \tparam RESULTSET Should be any ResultSet 801 | * \sa knnSearch, radiusSearch 802 | */ 803 | template 804 | void findNeighbors(RESULTSET& result, const ElementType* vec, const SearchParams& searchParams) const 805 | { 806 | assert(vec); 807 | if (!root_node) throw std::runtime_error("[nanoflann] findNeighbors() called before building the index."); 808 | float epsError = 1+searchParams.eps; 809 | 810 | std::vector dists( (DIM>0 ? DIM : dim) ,0); 811 | DistanceType distsq = computeInitialDistances(vec, dists); 812 | searchLevel(result, vec, root_node, distsq, dists, epsError); // "count_leaf" parameter removed since was neither used nor returned to the user. 813 | } 814 | 815 | /** 816 | * Find the "num_closest" nearest neighbors to the \a query_point[0:dim-1]. Their indices are stored inside 817 | * the result object. 818 | * \sa radiusSearch, findNeighbors 819 | * \note nChecks_IGNORED is ignored but kept for compatibility with the original FLANN interface. 820 | */ 821 | inline void knnSearch(const ElementType *query_point, const size_t num_closest, IndexType *out_indices, DistanceType *out_distances_sq, const int nChecks_IGNORED = 10) const 822 | { 823 | nanoflann::KNNResultSet resultSet(num_closest); 824 | resultSet.init(out_indices, out_distances_sq); 825 | this->findNeighbors(resultSet, query_point, nanoflann::SearchParams()); 826 | } 827 | 828 | /** 829 | * Find all the neighbors to \a query_point[0:dim-1] within a maximum radius. 830 | * The output is given as a vector of pairs, of which the first element is a point index and the second the corresponding distance. 831 | * Previous contents of \a IndicesDists are cleared. 832 | * 833 | * If searchParams.sorted==true, the output list is sorted by ascending distances. 834 | * 835 | * For a better performance, it is advisable to do a .reserve() on the vector if you have any wild guess about the number of expected matches. 836 | * 837 | * \sa knnSearch, findNeighbors 838 | * \return The number of points within the given radius (i.e. indices.size() or dists.size() ) 839 | */ 840 | size_t radiusSearch(const ElementType *query_point,const DistanceType radius, std::vector >& IndicesDists, const SearchParams& searchParams) const 841 | { 842 | RadiusResultSet resultSet(radius,IndicesDists); 843 | this->findNeighbors(resultSet, query_point, searchParams); 844 | 845 | if (searchParams.sorted) 846 | std::sort(IndicesDists.begin(),IndicesDists.end(), IndexDist_Sorter() ); 847 | 848 | return resultSet.size(); 849 | } 850 | 851 | /** @} */ 852 | 853 | private: 854 | /** Make sure the auxiliary list \a vind has the same size than the current dataset, and re-generate if size has changed. */ 855 | void init_vind() 856 | { 857 | // Create a permutable array of indices to the input vectors. 858 | m_size = dataset.kdtree_get_point_count(); 859 | if (vind.size()!=m_size) vind.resize(m_size); 860 | for (size_t i = 0; i < m_size; i++) vind[i] = i; 861 | } 862 | 863 | /// Helper accessor to the dataset points: 864 | inline ElementType dataset_get(size_t idx, int component) const { 865 | return dataset.kdtree_get_pt(idx,component); 866 | } 867 | 868 | 869 | void save_tree(FILE* stream, NodePtr tree) 870 | { 871 | save_value(stream, *tree); 872 | if (tree->child1!=NULL) { 873 | save_tree(stream, tree->child1); 874 | } 875 | if (tree->child2!=NULL) { 876 | save_tree(stream, tree->child2); 877 | } 878 | } 879 | 880 | 881 | void load_tree(FILE* stream, NodePtr& tree) 882 | { 883 | tree = pool.allocate(); 884 | load_value(stream, *tree); 885 | if (tree->child1!=NULL) { 886 | load_tree(stream, tree->child1); 887 | } 888 | if (tree->child2!=NULL) { 889 | load_tree(stream, tree->child2); 890 | } 891 | } 892 | 893 | 894 | void computeBoundingBox(BoundingBox& bbox) 895 | { 896 | bbox.resize((DIM>0 ? DIM : dim)); 897 | if (dataset.kdtree_get_bbox(bbox)) 898 | { 899 | // Done! It was implemented in derived class 900 | } 901 | else 902 | { 903 | for (int i=0; i<(DIM>0 ? DIM : dim); ++i) { 904 | bbox[i].low = 905 | bbox[i].high = dataset_get(0,i); 906 | } 907 | const size_t N = dataset.kdtree_get_point_count(); 908 | for (size_t k=1; k0 ? DIM : dim); ++i) { 910 | if (dataset_get(k,i)bbox[i].high) bbox[i].high = dataset_get(k,i); 912 | } 913 | } 914 | } 915 | } 916 | 917 | 918 | /** 919 | * Create a tree node that subdivides the list of vecs from vind[first] 920 | * to vind[last]. The routine is called recursively on each sublist. 921 | * Place a pointer to this new tree node in the location pTree. 922 | * 923 | * Params: pTree = the new node to create 924 | * first = index of the first vector 925 | * last = index of the last vector 926 | */ 927 | NodePtr divideTree(const IndexType left, const IndexType right, BoundingBox& bbox) 928 | { 929 | NodePtr node = pool.allocate(); // allocate memory 930 | 931 | /* If too few exemplars remain, then make this a leaf node. */ 932 | if ( (right-left) <= m_leaf_max_size) { 933 | node->child1 = node->child2 = NULL; /* Mark as leaf node. */ 934 | node->lr.left = left; 935 | node->lr.right = right; 936 | 937 | // compute bounding-box of leaf points 938 | for (int i=0; i<(DIM>0 ? DIM : dim); ++i) { 939 | bbox[i].low = dataset_get(vind[left],i); 940 | bbox[i].high = dataset_get(vind[left],i); 941 | } 942 | for (IndexType k=left+1; k0 ? DIM : dim); ++i) { 944 | if (bbox[i].low>dataset_get(vind[k],i)) bbox[i].low=dataset_get(vind[k],i); 945 | if (bbox[i].highsub.divfeat = cutfeat; 956 | 957 | BoundingBox left_bbox(bbox); 958 | left_bbox[cutfeat].high = cutval; 959 | node->child1 = divideTree(left, left+idx, left_bbox); 960 | 961 | BoundingBox right_bbox(bbox); 962 | right_bbox[cutfeat].low = cutval; 963 | node->child2 = divideTree(left+idx, right, right_bbox); 964 | 965 | node->sub.divlow = left_bbox[cutfeat].high; 966 | node->sub.divhigh = right_bbox[cutfeat].low; 967 | 968 | for (int i=0; i<(DIM>0 ? DIM : dim); ++i) { 969 | bbox[i].low = std::min(left_bbox[i].low, right_bbox[i].low); 970 | bbox[i].high = std::max(left_bbox[i].high, right_bbox[i].high); 971 | } 972 | } 973 | 974 | return node; 975 | } 976 | 977 | void computeMinMax(IndexType* ind, IndexType count, int element, ElementType& min_elem, ElementType& max_elem) 978 | { 979 | min_elem = dataset_get(ind[0],element); 980 | max_elem = dataset_get(ind[0],element); 981 | for (IndexType i=1; imax_elem) max_elem = val; 985 | } 986 | } 987 | 988 | void middleSplit(IndexType* ind, IndexType count, IndexType& index, int& cutfeat, DistanceType& cutval, const BoundingBox& bbox) 989 | { 990 | // find the largest span from the approximate bounding box 991 | ElementType max_span = bbox[0].high-bbox[0].low; 992 | cutfeat = 0; 993 | cutval = (bbox[0].high+bbox[0].low)/2; 994 | for (int i=1; i<(DIM>0 ? DIM : dim); ++i) { 995 | ElementType span = bbox[i].low-bbox[i].low; 996 | if (span>max_span) { 997 | max_span = span; 998 | cutfeat = i; 999 | cutval = (bbox[i].high+bbox[i].low)/2; 1000 | } 1001 | } 1002 | 1003 | // compute exact span on the found dimension 1004 | ElementType min_elem, max_elem; 1005 | computeMinMax(ind, count, cutfeat, min_elem, max_elem); 1006 | cutval = (min_elem+max_elem)/2; 1007 | max_span = max_elem - min_elem; 1008 | 1009 | // check if a dimension of a largest span exists 1010 | size_t k = cutfeat; 1011 | for (size_t i=0; i<(DIM>0 ? DIM : dim); ++i) { 1012 | if (i==k) continue; 1013 | ElementType span = bbox[i].high-bbox[i].low; 1014 | if (span>max_span) { 1015 | computeMinMax(ind, count, i, min_elem, max_elem); 1016 | span = max_elem - min_elem; 1017 | if (span>max_span) { 1018 | max_span = span; 1019 | cutfeat = i; 1020 | cutval = (min_elem+max_elem)/2; 1021 | } 1022 | } 1023 | } 1024 | IndexType lim1, lim2; 1025 | planeSplit(ind, count, cutfeat, cutval, lim1, lim2); 1026 | 1027 | if (lim1>count/2) index = lim1; 1028 | else if (lim2(0.00001); 1036 | ElementType max_span = bbox[0].high-bbox[0].low; 1037 | for (int i=1; i<(DIM>0 ? DIM : dim); ++i) { 1038 | ElementType span = bbox[i].high-bbox[i].low; 1039 | if (span>max_span) { 1040 | max_span = span; 1041 | } 1042 | } 1043 | ElementType max_spread = -1; 1044 | cutfeat = 0; 1045 | for (int i=0; i<(DIM>0 ? DIM : dim); ++i) { 1046 | ElementType span = bbox[i].high-bbox[i].low; 1047 | if (span>(1-EPS)*max_span) { 1048 | ElementType min_elem, max_elem; 1049 | computeMinMax(ind, count, cutfeat, min_elem, max_elem); 1050 | ElementType spread = max_elem-min_elem;; 1051 | if (spread>max_spread) { 1052 | cutfeat = i; 1053 | max_spread = spread; 1054 | } 1055 | } 1056 | } 1057 | // split in the middle 1058 | DistanceType split_val = (bbox[cutfeat].low+bbox[cutfeat].high)/2; 1059 | ElementType min_elem, max_elem; 1060 | computeMinMax(ind, count, cutfeat, min_elem, max_elem); 1061 | 1062 | if (split_valmax_elem) cutval = max_elem; 1064 | else cutval = split_val; 1065 | 1066 | IndexType lim1, lim2; 1067 | planeSplit(ind, count, cutfeat, cutval, lim1, lim2); 1068 | 1069 | if (lim1>count/2) index = lim1; 1070 | else if (lim2cutval 1083 | */ 1084 | void planeSplit(IndexType* ind, const IndexType count, int cutfeat, DistanceType cutval, IndexType& lim1, IndexType& lim2) 1085 | { 1086 | /* Move vector indices for left subtree to front of list. */ 1087 | IndexType left = 0; 1088 | IndexType right = count-1; 1089 | for (;; ) { 1090 | while (left<=right && dataset_get(ind[left],cutfeat)=cutval) --right; 1092 | if (left>right || !right) break; // "!right" was added to support unsigned Index types 1093 | std::swap(ind[left], ind[right]); 1094 | ++left; 1095 | --right; 1096 | } 1097 | /* If either list is empty, it means that all remaining features 1098 | * are identical. Split in the middle to maintain a balanced tree. 1099 | */ 1100 | lim1 = left; 1101 | right = count-1; 1102 | for (;; ) { 1103 | while (left<=right && dataset_get(ind[left],cutfeat)<=cutval) ++left; 1104 | while (right && left<=right && dataset_get(ind[right],cutfeat)>cutval) --right; 1105 | if (left>right || !right) break; // "!right" was added to support unsigned Index types 1106 | std::swap(ind[left], ind[right]); 1107 | ++left; 1108 | --right; 1109 | } 1110 | lim2 = left; 1111 | } 1112 | 1113 | DistanceType computeInitialDistances(const ElementType* vec, std::vector& dists) const 1114 | { 1115 | assert(vec); 1116 | DistanceType distsq = 0.0; 1117 | 1118 | for (int i = 0; i < (DIM>0 ? DIM : dim); ++i) { 1119 | if (vec[i] < root_bbox[i].low) { 1120 | dists[i] = distance.accum_dist(vec[i], root_bbox[i].low, i); 1121 | distsq += dists[i]; 1122 | } 1123 | if (vec[i] > root_bbox[i].high) { 1124 | dists[i] = distance.accum_dist(vec[i], root_bbox[i].high, i); 1125 | distsq += dists[i]; 1126 | } 1127 | } 1128 | 1129 | return distsq; 1130 | } 1131 | 1132 | /** 1133 | * Performs an exact search in the tree starting from a node. 1134 | * \tparam RESULTSET Should be any ResultSet 1135 | */ 1136 | template 1137 | void searchLevel(RESULTSET& result_set, const ElementType* vec, const NodePtr node, DistanceType mindistsq, 1138 | std::vector& dists, const float epsError) const 1139 | { 1140 | /* If this is a leaf node, then do check and return. */ 1141 | if ((node->child1 == NULL)&&(node->child2 == NULL)) { 1142 | //count_leaf += (node->lr.right-node->lr.left); // Removed since was neither used nor returned to the user. 1143 | DistanceType worst_dist = result_set.worstDist(); 1144 | for (IndexType i=node->lr.left; ilr.right; ++i) { 1145 | const IndexType index = vind[i];// reorder... : i; 1146 | DistanceType dist = distance(vec, index, (DIM>0 ? DIM : dim)); 1147 | if (distsub.divfeat; 1156 | ElementType val = vec[idx]; 1157 | DistanceType diff1 = val - node->sub.divlow; 1158 | DistanceType diff2 = val - node->sub.divhigh; 1159 | 1160 | NodePtr bestChild; 1161 | NodePtr otherChild; 1162 | DistanceType cut_dist; 1163 | if ((diff1+diff2)<0) { 1164 | bestChild = node->child1; 1165 | otherChild = node->child2; 1166 | cut_dist = distance.accum_dist(val, node->sub.divhigh, idx); 1167 | } 1168 | else { 1169 | bestChild = node->child2; 1170 | otherChild = node->child1; 1171 | cut_dist = distance.accum_dist( val, node->sub.divlow, idx); 1172 | } 1173 | 1174 | /* Call recursively to search next level down. */ 1175 | searchLevel(result_set, vec, bestChild, mindistsq, dists, epsError); 1176 | 1177 | DistanceType dst = dists[idx]; 1178 | mindistsq = mindistsq + cut_dist - dst; 1179 | dists[idx] = cut_dist; 1180 | if (mindistsq*epsError<=result_set.worstDist()) { 1181 | searchLevel(result_set, vec, otherChild, mindistsq, dists, epsError); 1182 | } 1183 | dists[idx] = dst; 1184 | } 1185 | 1186 | public: 1187 | /** Stores the index in a binary file. 1188 | * IMPORTANT NOTE: The set of data points is NOT stored in the file, so when loading the index object it must be constructed associated to the same source of data points used while building it. 1189 | * See the example: examples/saveload_example.cpp 1190 | * \sa loadIndex */ 1191 | void saveIndex(FILE* stream) 1192 | { 1193 | save_value(stream, m_size); 1194 | save_value(stream, dim); 1195 | save_value(stream, root_bbox); 1196 | save_value(stream, m_leaf_max_size); 1197 | save_value(stream, vind); 1198 | save_tree(stream, root_node); 1199 | } 1200 | 1201 | /** Loads a previous index from a binary file. 1202 | * IMPORTANT NOTE: The set of data points is NOT stored in the file, so the index object must be constructed associated to the same source of data points used while building the index. 1203 | * See the example: examples/saveload_example.cpp 1204 | * \sa loadIndex */ 1205 | void loadIndex(FILE* stream) 1206 | { 1207 | load_value(stream, m_size); 1208 | load_value(stream, dim); 1209 | load_value(stream, root_bbox); 1210 | load_value(stream, m_leaf_max_size); 1211 | load_value(stream, vind); 1212 | load_tree(stream, root_node); 1213 | } 1214 | 1215 | }; // class KDTree 1216 | 1217 | 1218 | /** A simple KD-tree adaptor for working with data directly stored in an Eigen Matrix, without duplicating the data storage. 1219 | * Each row in the matrix represents a point in the state space. 1220 | * 1221 | * Example of usage: 1222 | * \code 1223 | * Eigen::Matrix mat; 1224 | * // Fill out "mat"... 1225 | * 1226 | * typedef KDTreeEigenMatrixAdaptor< Eigen::Matrix > my_kd_tree_t; 1227 | * const int max_leaf = 10; 1228 | * my_kd_tree_t mat_index(dimdim, mat, max_leaf ); 1229 | * mat_index.index->buildIndex(); 1230 | * mat_index.index->... 1231 | * \endcode 1232 | * 1233 | * \tparam DIM If set to >0, it specifies a compile-time fixed dimensionality for the points in the data set, allowing more compiler optimizations. 1234 | * \tparam Distance The distance metric to use: nanoflann::metric_L1, nanoflann::metric_L2, nanoflann::metric_L2_Simple, etc. 1235 | * \tparam IndexType The type for indices in the KD-tree index (typically, size_t of int) 1236 | */ 1237 | template 1238 | struct KDTreeEigenMatrixAdaptor 1239 | { 1240 | typedef KDTreeEigenMatrixAdaptor self_t; 1241 | typedef typename MatrixType::Scalar num_t; 1242 | typedef typename Distance::template traits::distance_t metric_t; 1243 | typedef KDTreeSingleIndexAdaptor< metric_t,self_t,DIM,IndexType> index_t; 1244 | 1245 | index_t* index; //! The kd-tree index for the user to call its methods as usual with any other FLANN index. 1246 | 1247 | /// Constructor: takes a const ref to the matrix object with the data points 1248 | KDTreeEigenMatrixAdaptor(const int dimensionality, const MatrixType &mat, const int leaf_max_size = 10) : m_data_matrix(mat) 1249 | { 1250 | const size_t dims = mat.cols(); 1251 | if (DIM>0 && static_cast(dims)!=DIM) 1252 | throw std::runtime_error("Data set dimensionality does not match the 'DIM' template argument"); 1253 | index = new index_t( dims, *this /* adaptor */, nanoflann::KDTreeSingleIndexAdaptorParams(leaf_max_size, dims ) ); 1254 | index->buildIndex(); 1255 | } 1256 | 1257 | ~KDTreeEigenMatrixAdaptor() { 1258 | delete index; 1259 | } 1260 | 1261 | const MatrixType &m_data_matrix; 1262 | 1263 | /** Query for the \a num_closest closest points to a given point (entered as query_point[0:dim-1]). 1264 | * Note that this is a short-cut method for index->findNeighbors(). 1265 | * The user can also call index->... methods as desired. 1266 | * \note nChecks_IGNORED is ignored but kept for compatibility with the original FLANN interface. 1267 | */ 1268 | inline void query(const num_t *query_point, const size_t num_closest, IndexType *out_indices, num_t *out_distances_sq, const int nChecks_IGNORED = 10) const 1269 | { 1270 | nanoflann::KNNResultSet resultSet(num_closest); 1271 | resultSet.init(out_indices, out_distances_sq); 1272 | index->findNeighbors(resultSet, query_point, nanoflann::SearchParams()); 1273 | } 1274 | 1275 | /** @name Interface expected by KDTreeSingleIndexAdaptor 1276 | * @{ */ 1277 | 1278 | const self_t & derived() const { 1279 | return *this; 1280 | } 1281 | self_t & derived() { 1282 | return *this; 1283 | } 1284 | 1285 | // Must return the number of data points 1286 | inline size_t kdtree_get_point_count() const { 1287 | return m_data_matrix.rows(); 1288 | } 1289 | 1290 | // Returns the distance between the vector "p1[0:size-1]" and the data point with index "idx_p2" stored in the class: 1291 | inline num_t kdtree_distance(const num_t *p1, const size_t idx_p2,size_t size) const 1292 | { 1293 | num_t s=0; 1294 | for (size_t i=0; i 1310 | bool kdtree_get_bbox(BBOX &bb) const { 1311 | return false; 1312 | } 1313 | 1314 | /** @} */ 1315 | 1316 | }; // end of KDTreeEigenMatrixAdaptor 1317 | /** @} */ 1318 | 1319 | /** @} */ // end of grouping 1320 | } // end of NS 1321 | 1322 | 1323 | #endif /* NANOFLANN_HPP_ */ 1324 | --------------------------------------------------------------------------------