├── .gitignore ├── CMakeLists.txt ├── LICENSE ├── README.md ├── cmake ├── CMakeUninstall.cmake.in ├── FindEigen.cmake ├── NViewsTrianConfig.cmake.in ├── NViewsTrianConfigVersion.cmake.in └── setup_installation.cmake ├── examples ├── CMakeLists.txt └── example_base.cpp ├── include ├── NViewsCertifier.h ├── NViewsClass.h ├── NViewsTypes.h └── NViewsUtils.h ├── src ├── NViewsCertifier.cpp ├── NViewsClass.cpp └── NViewsUtils.cpp ├── tests ├── CMakeLists.txt ├── basic_params.txt ├── experimentsHelper.cpp ├── experimentsHelper.h └── generic_test.cpp └── utils ├── ceresSolver.cpp ├── ceresSolver.h ├── generatePointCloud.cpp └── generatePointCloud.h /.gitignore: -------------------------------------------------------------------------------- 1 | # Prerequisites 2 | *.d 3 | 4 | # Compiled Object files 5 | *.slo 6 | *.lo 7 | *.o 8 | *.obj 9 | 10 | # Precompiled Headers 11 | *.gch 12 | *.pch 13 | 14 | # Compiled Dynamic libraries 15 | *.so 16 | *.dylib 17 | *.dll 18 | 19 | # Fortran module files 20 | *.mod 21 | *.smod 22 | 23 | # Compiled Static libraries 24 | *.lai 25 | *.la 26 | *.a 27 | *.lib 28 | 29 | # Executables 30 | *.exe 31 | *.out 32 | *.app 33 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # PROJECT CONFIGURATION 2 | cmake_minimum_required(VERSION 3.1) 3 | 4 | project(NViewsTrian LANGUAGES C CXX VERSION 1.0.0) 5 | 6 | set(LIBRARY_TARGET_NAME "NViewsTrian") 7 | set(LIBRARY_TARGET_NAME_EXPORT "${LIBRARY_TARGET_NAME}Export") 8 | 9 | set(CMAKE_CXX_STANDARD 14) 10 | set(CMAKE_CXX_STANDARD_REQUIRED ON) # We require C++ 14 11 | 12 | 13 | if(NOT CMAKE_BUILD_TYPE) 14 | set(CMAKE_BUILD_TYPE Release) 15 | endif() 16 | 17 | # set(CMAKE_CXX_FLAGS "-Wall -Wextra") 18 | set(CMAKE_CXX_FLAGS_DEBUG "-g") 19 | set(CMAKE_CXX_FLAGS_RELEASE "-O3") 20 | set(CMAKE_CXX_FLAGS "-O3 -DNDEBUG") 21 | 22 | message(STATUS "Building in ${CMAKE_BUILD_TYPE} mode\n") 23 | 24 | 25 | 26 | # build the examples 27 | set(BUILD_${LIBRARY_TARGET_NAME}_EXAMPLE ON) 28 | # build the tests 29 | set(BUILD_${LIBRARY_TARGET_NAME}_TESTS ON) 30 | 31 | 32 | 33 | # Build type 34 | 35 | # Directory for built libraries 36 | set(LIBRARY_OUTPUT_PATH ${CMAKE_CURRENT_BINARY_DIR}/lib CACHE PATH "The directory in which to place libraries built by this project") 37 | # Directory for built executables 38 | set(EXECUTABLE_OUTPUT_PATH ${CMAKE_CURRENT_BINARY_DIR}/bin CACHE PATH "The directory in which to place executables built by this project") 39 | 40 | # BUILD CONFIGURATIONS 41 | option(CMAKE_VERBOSE_MAKEFILE "Generate verbose makefiles?" OFF) 42 | 43 | set(CODE_PROFILING OFF CACHE BOOL "Turn on code profiling?") 44 | 45 | 46 | # Add the .cmake files that ship with Eigen3 to the CMake module path (useful for finding other stuff) 47 | set(CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/cmake" CACHE STRING "The CMake module path used for this project") 48 | 49 | # FIND EIGEN3 50 | set( ENV{EIGEN3_ROOT_DIR} ${CMAKE_SOURCE_DIR}/eigen) 51 | find_package(Eigen3 3.3 REQUIRED) 52 | if(EIGEN3_FOUND) 53 | message(STATUS "Found Eigen3 library (version ${EIGEN3_VERSION_STRING})") 54 | message(STATUS "Eigen3 include directory: ${EIGEN3_INCLUDE_DIR}\n") 55 | else() 56 | message(STATUS "Eigen library not found!") 57 | endif() 58 | 59 | 60 | 61 | set(${LIBRARY_TARGET_NAME}_INCLUDE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include) 62 | set(${LIBRARY_TARGET_NAME}_SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src) 63 | set(${LIBRARY_TARGET_NAME}_EXAMPLES_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/examples) 64 | set(${LIBRARY_TARGET_NAME}_TESTS_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/tests) 65 | 66 | 67 | # Expose the include directories for this project 68 | set(${LIBRARY_TARGET_NAME}_ADD_INCLUDES ${EIGEN3_INCLUDE_DIR} ) 69 | set(${LIBRARY_TARGET_NAME}_CERT_INCLUDES ${${LIBRARY_TARGET_NAME}_INCLUDE_DIRS} ${${LIBRARY_TARGET_NAME}_ADD_INCLUDES}) 70 | 71 | 72 | # Get the set of Essential header and source files 73 | set(${LIBRARY_TARGET_NAME}_HDRS 74 | ${${LIBRARY_TARGET_NAME}_INCLUDE_DIRS}/NViewsTypes.h 75 | ${${LIBRARY_TARGET_NAME}_INCLUDE_DIRS}/NViewsUtils.h 76 | ${${LIBRARY_TARGET_NAME}_INCLUDE_DIRS}/NViewsClass.h 77 | ${${LIBRARY_TARGET_NAME}_INCLUDE_DIRS}/NViewsCertifier.h 78 | ) 79 | 80 | 81 | set(${LIBRARY_TARGET_NAME}_SRCS 82 | ${${LIBRARY_TARGET_NAME}_SOURCE_DIRS}/NViewsUtils.cpp 83 | ${${LIBRARY_TARGET_NAME}_SOURCE_DIRS}/NViewsClass.cpp 84 | ${${LIBRARY_TARGET_NAME}_SOURCE_DIRS}/NViewsCertifier.cpp 85 | ) 86 | 87 | 88 | 89 | 90 | 91 | # Build the Essential library 92 | add_library(${LIBRARY_TARGET_NAME} ${${LIBRARY_TARGET_NAME}_HDRS} ${${LIBRARY_TARGET_NAME}_SRCS} ) 93 | 94 | 95 | target_include_directories(${LIBRARY_TARGET_NAME} PUBLIC 96 | # only when building from the source tree 97 | $ 98 | $ 99 | ${${LIBRARY_TARGET_NAME}_ADD_INCLUDES} 100 | ) 101 | 102 | 103 | target_link_libraries(${LIBRARY_TARGET_NAME}) 104 | 105 | 106 | 107 | 108 | if(${CODE_PROFILING}) 109 | set_target_properties(${LIBRARY_TARGET_NAME} PROPERTIES COMPILE_FLAGS "-pg -g" LINK_FLAGS "-pg -g") 110 | endif() 111 | 112 | 113 | set_target_properties(${LIBRARY_TARGET_NAME} PROPERTIES POSITION_INDEPENDENT_CODE ON) 114 | 115 | install(TARGETS ${LIBRARY_TARGET_NAME} 116 | EXPORT ${LIBRARY_TARGET_NAME_EXPORT} 117 | RUNTIME DESTINATION bin 118 | LIBRARY DESTINATION lib${LIB_SUFFIX} 119 | ARCHIVE DESTINATION lib${LIB_SUFFIX} 120 | INCLUDES DESTINATION "include" 121 | PUBLIC_HEADER DESTINATION "include/${LIBRARY_TARGET_NAME}" 122 | ) 123 | 124 | 125 | 126 | # Build the example executable 127 | IF(BUILD_${LIBRARY_TARGET_NAME}_EXAMPLE) 128 | message(STATUS "Adding examples to build") 129 | add_subdirectory(examples) 130 | endif() 131 | 132 | 133 | # building the tests 134 | if(BUILD_${LIBRARY_TARGET_NAME}_TESTS) 135 | message(STATUS "Adding tests to build") 136 | add_subdirectory(tests) 137 | endif() 138 | 139 | 140 | # Install 141 | include(cmake/setup_installation.cmake) 142 | 143 | -------------------------------------------------------------------------------- /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 | # Certifiable solver for real-time N-view triangulation 2 | 3 | --- 4 | This repository contains the code 5 | for the fast N-view triangulation 6 | solver explained in [this paper](https://ieeexplore.ieee.org/document/10044919). 7 | 8 | 9 | **Authors:** 10 | [Mercedes Garcia-Salguero](https://mapir.isa.uma.es/mapirwebsite/?p=1718), 11 | [Javier Gonzalez-Jimenez](https://mapir.isa.uma.es/mapirwebsite/?p=1536) 12 | 13 | 14 | **License:** [GPLv3](https://github.com/mergarsal/FastNViewTriangulation/blob/main/LICENSE) 15 | 16 | 17 | If you use this code for your research, please cite: 18 | 19 | ``` 20 | @ARTICLE{, 21 | author = {Garcia-Salguero, Mercedes and Gonzalez-Jimenez, Javier}, 22 | month = {{{{feb}}}}, 23 | title = {Certifiable solver for real-time N-view triangulation}, 24 | journal = {IEEE Robotics and Automation Letters}, 25 | year = {2023}, 26 | issn = {2377-3766}, 27 | url = {http://mapir.isa.uma.es/papersrepo/2023/2023_mercedes_RAL_Nview_triangulation_paper.pdf}, 28 | doi = {10.1109/LRA.2023.3245408} 29 | } 30 | ``` 31 | 32 | ## Dependencies 33 | 34 | - Eigen 35 | - Ceres 36 | 37 | *Note*: Ceres is used in the example 38 | and test. You can remove that part 39 | and thus the dependency. 40 | 41 | 42 | 43 | ## Build 44 | 45 | ``` 46 | git clone https://github.com/mergarsal/FastNViewTriangulation.git 47 | cd FastNViewTriangulation 48 | 49 | mkdir build & cd build 50 | 51 | cmake .. 52 | 53 | make -jX 54 | 55 | ``` 56 | 57 | The compiled examples should be inside the `bin` directory. Run: 58 | 59 | ``` 60 | ./bin/example_base 61 | ``` 62 | 63 | 64 | 65 | ## Install 66 | In `build` folder: 67 | 68 | ``` 69 | sudo make install 70 | ``` 71 | 72 | We also provide the uninstall script: 73 | 74 | ``` 75 | sudo make uninstall 76 | ``` 77 | 78 | 79 | 80 | 81 | 82 | # Use in external project 83 | 1. Install our library with 84 | 85 | ``` 86 | sudo make install 87 | ``` 88 | 89 | 2. In your project, find the library. 90 | 91 | 92 | ``` 93 | find_package(NViewsTrian REQUIRED) 94 | ``` 95 | 96 | 3. Include the library as target, e.g., 97 | 98 | ``` 99 | add_executable(example_base ${CMAKE_CURRENT_SOURCE_DIR}/example_base.cpp) 100 | 101 | target_link_libraries(example_base 102 | NViewsTrian 103 | ) 104 | ``` 105 | 106 | -------------------------------------------------------------------------------- /cmake/CMakeUninstall.cmake.in: -------------------------------------------------------------------------------- 1 | if(NOT EXISTS "@CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt") 2 | message(FATAL_ERROR "Cannot find install manifest: @CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt") 3 | endif() 4 | 5 | file(READ "@CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt" files) 6 | string(REGEX REPLACE "\n" ";" files "${files}") 7 | 8 | foreach(file ${files}) 9 | message(STATUS "Uninstalling $ENV{DESTDIR}${file}") 10 | if(IS_SYMLINK "$ENV{DESTDIR}${file}" OR EXISTS "$ENV{DESTDIR}${file}") 11 | exec_program("@CMAKE_COMMAND@" ARGS "-E remove \"$ENV{DESTDIR}${file}\"" 12 | OUTPUT_VARIABLE rm_out 13 | RETURN_VALUE rm_retval) 14 | if(NOT "${rm_retval}" STREQUAL 0) 15 | message(FATAL_ERROR "Problem when removing $ENV{DESTDIR}${file}") 16 | endif() 17 | else() 18 | message(STATUS "File $ENV{DESTDIR}${file} does not exist.") 19 | endif() 20 | endforeach() 21 | -------------------------------------------------------------------------------- /cmake/FindEigen.cmake: -------------------------------------------------------------------------------- 1 | # Ceres Solver - A fast non-linear least squares minimizer 2 | # Copyright 2015 Google Inc. All rights reserved. 3 | # http://ceres-solver.org/ 4 | # 5 | # Redistribution and use in source and binary forms, with or without 6 | # modification, are permitted provided that the following conditions are met: 7 | # 8 | # * Redistributions of source code must retain the above copyright notice, 9 | # this list of conditions and the following disclaimer. 10 | # * Redistributions in binary form must reproduce the above copyright notice, 11 | # this list of conditions and the following disclaimer in the documentation 12 | # and/or other materials provided with the distribution. 13 | # * Neither the name of Google Inc. nor the names of its contributors may be 14 | # used to endorse or promote products derived from this software without 15 | # specific prior written permission. 16 | # 17 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | # POSSIBILITY OF SUCH DAMAGE. 28 | # 29 | # Author: alexs.mac@gmail.com (Alex Stewart) 30 | # 31 | 32 | # FindEigen.cmake - Find Eigen library, version >= 3. 33 | # 34 | # This module defines the following variables: 35 | # 36 | # EIGEN_FOUND: TRUE iff Eigen is found. 37 | # EIGEN_INCLUDE_DIRS: Include directories for Eigen. 38 | # EIGEN_VERSION: Extracted from Eigen/src/Core/util/Macros.h 39 | # EIGEN_WORLD_VERSION: Equal to 3 if EIGEN_VERSION = 3.2.0 40 | # EIGEN_MAJOR_VERSION: Equal to 2 if EIGEN_VERSION = 3.2.0 41 | # EIGEN_MINOR_VERSION: Equal to 0 if EIGEN_VERSION = 3.2.0 42 | # FOUND_INSTALLED_EIGEN_CMAKE_CONFIGURATION: True iff the version of Eigen 43 | # found was built & installed / 44 | # exported as a CMake package. 45 | # 46 | # The following variables control the behaviour of this module: 47 | # 48 | # EIGEN_PREFER_EXPORTED_EIGEN_CMAKE_CONFIGURATION: TRUE/FALSE, iff TRUE then 49 | # then prefer using an exported CMake configuration 50 | # generated by Eigen over searching for the 51 | # Eigen components manually. Otherwise (FALSE) 52 | # ignore any exported Eigen CMake configurations and 53 | # always perform a manual search for the components. 54 | # Default: TRUE iff user does not define this variable 55 | # before we are called, and does NOT specify 56 | # EIGEN_INCLUDE_DIR_HINTS, otherwise FALSE. 57 | # EIGEN_INCLUDE_DIR_HINTS: List of additional directories in which to 58 | # search for eigen includes, e.g: /timbuktu/eigen3. 59 | # 60 | # The following variables are also defined by this module, but in line with 61 | # CMake recommended FindPackage() module style should NOT be referenced directly 62 | # by callers (use the plural variables detailed above instead). These variables 63 | # do however affect the behaviour of the module via FIND_[PATH/LIBRARY]() which 64 | # are NOT re-called (i.e. search for library is not repeated) if these variables 65 | # are set with valid values _in the CMake cache_. This means that if these 66 | # variables are set directly in the cache, either by the user in the CMake GUI, 67 | # or by the user passing -DVAR=VALUE directives to CMake when called (which 68 | # explicitly defines a cache variable), then they will be used verbatim, 69 | # bypassing the HINTS variables and other hard-coded search locations. 70 | # 71 | # EIGEN_INCLUDE_DIR: Include directory for CXSparse, not including the 72 | # include directory of any dependencies. 73 | 74 | # Called if we failed to find Eigen or any of it's required dependencies, 75 | # unsets all public (designed to be used externally) variables and reports 76 | # error message at priority depending upon [REQUIRED/QUIET/] argument. 77 | macro(EIGEN_REPORT_NOT_FOUND REASON_MSG) 78 | unset(EIGEN_FOUND) 79 | unset(EIGEN_INCLUDE_DIRS) 80 | unset(FOUND_INSTALLED_EIGEN_CMAKE_CONFIGURATION) 81 | # Make results of search visible in the CMake GUI if Eigen has not 82 | # been found so that user does not have to toggle to advanced view. 83 | mark_as_advanced(CLEAR EIGEN_INCLUDE_DIR) 84 | # Note _FIND_[REQUIRED/QUIETLY] variables defined by FindPackage() 85 | # use the camelcase library name, not uppercase. 86 | if (Eigen_FIND_QUIETLY) 87 | message(STATUS "Failed to find Eigen - " ${REASON_MSG} ${ARGN}) 88 | elseif (Eigen_FIND_REQUIRED) 89 | message(FATAL_ERROR "Failed to find Eigen - " ${REASON_MSG} ${ARGN}) 90 | else() 91 | # Neither QUIETLY nor REQUIRED, use no priority which emits a message 92 | # but continues configuration and allows generation. 93 | message("-- Failed to find Eigen - " ${REASON_MSG} ${ARGN}) 94 | endif () 95 | return() 96 | endmacro(EIGEN_REPORT_NOT_FOUND) 97 | 98 | # Protect against any alternative find_package scripts for this library having 99 | # been called previously (in a client project) which set EIGEN_FOUND, but not 100 | # the other variables we require / set here which could cause the search logic 101 | # here to fail. 102 | unset(EIGEN_FOUND) 103 | 104 | # ----------------------------------------------------------------- 105 | # By default, if the user has expressed no preference for using an exported 106 | # Eigen CMake configuration over performing a search for the installed 107 | # components, and has not specified any hints for the search locations, then 108 | # prefer an exported configuration if available. 109 | if (NOT DEFINED EIGEN_PREFER_EXPORTED_EIGEN_CMAKE_CONFIGURATION 110 | AND NOT EIGEN_INCLUDE_DIR_HINTS) 111 | message(STATUS "No preference for use of exported Eigen CMake configuration " 112 | "set, and no hints for include directory provided. " 113 | "Defaulting to preferring an installed/exported Eigen CMake configuration " 114 | "if available.") 115 | set(EIGEN_PREFER_EXPORTED_EIGEN_CMAKE_CONFIGURATION TRUE) 116 | endif() 117 | 118 | if (EIGEN_PREFER_EXPORTED_EIGEN_CMAKE_CONFIGURATION) 119 | # Try to find an exported CMake configuration for Eigen. 120 | # 121 | # We search twice, s/t we can invert the ordering of precedence used by 122 | # find_package() for exported package build directories, and installed 123 | # packages (found via CMAKE_SYSTEM_PREFIX_PATH), listed as items 6) and 7) 124 | # respectively in [1]. 125 | # 126 | # By default, exported build directories are (in theory) detected first, and 127 | # this is usually the case on Windows. However, on OS X & Linux, the install 128 | # path (/usr/local) is typically present in the PATH environment variable 129 | # which is checked in item 4) in [1] (i.e. before both of the above, unless 130 | # NO_SYSTEM_ENVIRONMENT_PATH is passed). As such on those OSs installed 131 | # packages are usually detected in preference to exported package build 132 | # directories. 133 | # 134 | # To ensure a more consistent response across all OSs, and as users usually 135 | # want to prefer an installed version of a package over a locally built one 136 | # where both exist (esp. as the exported build directory might be removed 137 | # after installation), we first search with NO_CMAKE_PACKAGE_REGISTRY which 138 | # means any build directories exported by the user are ignored, and thus 139 | # installed directories are preferred. If this fails to find the package 140 | # we then research again, but without NO_CMAKE_PACKAGE_REGISTRY, so any 141 | # exported build directories will now be detected. 142 | # 143 | # To prevent confusion on Windows, we also pass NO_CMAKE_BUILDS_PATH (which 144 | # is item 5) in [1]), to not preferentially use projects that were built 145 | # recently with the CMake GUI to ensure that we always prefer an installed 146 | # version if available. 147 | # 148 | # [1] http://www.cmake.org/cmake/help/v2.8.11/cmake.html#command:find_package 149 | find_package(Eigen3 QUIET 150 | NO_MODULE 151 | NO_CMAKE_PACKAGE_REGISTRY 152 | NO_CMAKE_BUILDS_PATH) 153 | if (EIGEN3_FOUND) 154 | message(STATUS "Found installed version of Eigen: ${Eigen3_DIR}") 155 | else() 156 | # Failed to find an installed version of Eigen, repeat search allowing 157 | # exported build directories. 158 | message(STATUS "Failed to find installed Eigen CMake configuration, " 159 | "searching for Eigen build directories exported with CMake.") 160 | # Again pass NO_CMAKE_BUILDS_PATH, as we know that Eigen is exported and 161 | # do not want to treat projects built with the CMake GUI preferentially. 162 | find_package(Eigen3 QUIET 163 | NO_MODULE 164 | NO_CMAKE_BUILDS_PATH) 165 | if (EIGEN3_FOUND) 166 | message(STATUS "Found exported Eigen build directory: ${Eigen3_DIR}") 167 | endif() 168 | endif() 169 | if (EIGEN3_FOUND) 170 | set(FOUND_INSTALLED_EIGEN_CMAKE_CONFIGURATION TRUE) 171 | set(EIGEN_FOUND ${EIGEN3_FOUND}) 172 | set(EIGEN_INCLUDE_DIR "${EIGEN3_INCLUDE_DIR}" CACHE STRING 173 | "Eigen include directory" FORCE) 174 | else() 175 | message(STATUS "Failed to find an installed/exported CMake configuration " 176 | "for Eigen, will perform search for installed Eigen components.") 177 | endif() 178 | endif() 179 | 180 | if (NOT EIGEN_FOUND) 181 | # Search user-installed locations first, so that we prefer user installs 182 | # to system installs where both exist. 183 | list(APPEND EIGEN_CHECK_INCLUDE_DIRS 184 | /usr/local/include 185 | /usr/local/homebrew/include # Mac OS X 186 | /opt/local/var/macports/software # Mac OS X. 187 | /opt/local/include 188 | /usr/include) 189 | # Additional suffixes to try appending to each search path. 190 | list(APPEND EIGEN_CHECK_PATH_SUFFIXES 191 | eigen3 # Default root directory for Eigen. 192 | Eigen/include/eigen3 # Windows (for C:/Program Files prefix) < 3.3 193 | Eigen3/include/eigen3 ) # Windows (for C:/Program Files prefix) >= 3.3 194 | 195 | # Search supplied hint directories first if supplied. 196 | find_path(EIGEN_INCLUDE_DIR 197 | NAMES Eigen/Core 198 | HINTS ${EIGEN_INCLUDE_DIR_HINTS} 199 | PATHS ${EIGEN_CHECK_INCLUDE_DIRS} 200 | PATH_SUFFIXES ${EIGEN_CHECK_PATH_SUFFIXES}) 201 | 202 | if (NOT EIGEN_INCLUDE_DIR OR 203 | NOT EXISTS ${EIGEN_INCLUDE_DIR}) 204 | eigen_report_not_found( 205 | "Could not find eigen3 include directory, set EIGEN_INCLUDE_DIR to " 206 | "path to eigen3 include directory, e.g. /usr/local/include/eigen3.") 207 | endif (NOT EIGEN_INCLUDE_DIR OR 208 | NOT EXISTS ${EIGEN_INCLUDE_DIR}) 209 | 210 | # Mark internally as found, then verify. EIGEN_REPORT_NOT_FOUND() unsets 211 | # if called. 212 | set(EIGEN_FOUND TRUE) 213 | endif() 214 | 215 | # Extract Eigen version from Eigen/src/Core/util/Macros.h 216 | if (EIGEN_INCLUDE_DIR) 217 | set(EIGEN_VERSION_FILE ${EIGEN_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h) 218 | if (NOT EXISTS ${EIGEN_VERSION_FILE}) 219 | eigen_report_not_found( 220 | "Could not find file: ${EIGEN_VERSION_FILE} " 221 | "containing version information in Eigen install located at: " 222 | "${EIGEN_INCLUDE_DIR}.") 223 | else (NOT EXISTS ${EIGEN_VERSION_FILE}) 224 | file(READ ${EIGEN_VERSION_FILE} EIGEN_VERSION_FILE_CONTENTS) 225 | 226 | string(REGEX MATCH "#define EIGEN_WORLD_VERSION [0-9]+" 227 | EIGEN_WORLD_VERSION "${EIGEN_VERSION_FILE_CONTENTS}") 228 | string(REGEX REPLACE "#define EIGEN_WORLD_VERSION ([0-9]+)" "\\1" 229 | EIGEN_WORLD_VERSION "${EIGEN_WORLD_VERSION}") 230 | 231 | string(REGEX MATCH "#define EIGEN_MAJOR_VERSION [0-9]+" 232 | EIGEN_MAJOR_VERSION "${EIGEN_VERSION_FILE_CONTENTS}") 233 | string(REGEX REPLACE "#define EIGEN_MAJOR_VERSION ([0-9]+)" "\\1" 234 | EIGEN_MAJOR_VERSION "${EIGEN_MAJOR_VERSION}") 235 | 236 | string(REGEX MATCH "#define EIGEN_MINOR_VERSION [0-9]+" 237 | EIGEN_MINOR_VERSION "${EIGEN_VERSION_FILE_CONTENTS}") 238 | string(REGEX REPLACE "#define EIGEN_MINOR_VERSION ([0-9]+)" "\\1" 239 | EIGEN_MINOR_VERSION "${EIGEN_MINOR_VERSION}") 240 | 241 | # This is on a single line s/t CMake does not interpret it as a list of 242 | # elements and insert ';' separators which would result in 3.;2.;0 nonsense. 243 | set(EIGEN_VERSION "${EIGEN_WORLD_VERSION}.${EIGEN_MAJOR_VERSION}.${EIGEN_MINOR_VERSION}") 244 | endif (NOT EXISTS ${EIGEN_VERSION_FILE}) 245 | endif (EIGEN_INCLUDE_DIR) 246 | 247 | # Set standard CMake FindPackage variables if found. 248 | if (EIGEN_FOUND) 249 | set(EIGEN_INCLUDE_DIRS ${EIGEN_INCLUDE_DIR}) 250 | endif (EIGEN_FOUND) 251 | 252 | # Handle REQUIRED / QUIET optional arguments and version. 253 | include(FindPackageHandleStandardArgs) 254 | find_package_handle_standard_args(Eigen 255 | REQUIRED_VARS EIGEN_INCLUDE_DIRS 256 | VERSION_VAR EIGEN_VERSION) 257 | 258 | # Only mark internal variables as advanced if we found Eigen, otherwise 259 | # leave it visible in the standard GUI for the user to set manually. 260 | if (EIGEN_FOUND) 261 | mark_as_advanced(FORCE EIGEN_INCLUDE_DIR 262 | Eigen3_DIR) # Autogenerated by find_package(Eigen3) 263 | endif (EIGEN_FOUND) 264 | -------------------------------------------------------------------------------- /cmake/NViewsTrianConfig.cmake.in: -------------------------------------------------------------------------------- 1 | include(CMakeFindDependencyMacro) 2 | @NViewsTrian_FIND_DEPENDENCY_CALLS@ 3 | include("${CMAKE_CURRENT_LIST_DIR}/@exported_targets_filename@") 4 | 5 | -------------------------------------------------------------------------------- /cmake/NViewsTrianConfigVersion.cmake.in: -------------------------------------------------------------------------------- 1 | # Ceres Solver - A fast non-linear least squares minimizer 2 | # Copyright 2013 Google Inc. All rights reserved. 3 | # http://code.google.com/p/ceres-solver/ 4 | # 5 | # Redistribution and use in source and binary forms, with or without 6 | # modification, are permitted provided that the following conditions are met: 7 | # 8 | # * Redistributions of source code must retain the above copyright notice, 9 | # this list of conditions and the following disclaimer. 10 | # * Redistributions in binary form must reproduce the above copyright notice, 11 | # this list of conditions and the following disclaimer in the documentation 12 | # and/or other materials provided with the distribution. 13 | # * Neither the name of Google Inc. nor the names of its contributors may be 14 | # used to endorse or promote products derived from this software without 15 | # specific prior written permission. 16 | # 17 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | # POSSIBILITY OF SUCH DAMAGE. 28 | # 29 | # Author: pablo.speciale@gmail.com (Pablo Speciale) 30 | # 31 | # FIND_PACKAGE() searches for a Config.cmake file and an associated 32 | # Version.cmake file, which it loads to check the version number. 33 | # 34 | # This file can be used with CONFIGURE_FILE() to generate such a file for a 35 | # project with very basic logic. 36 | # 37 | # It sets PACKAGE_VERSION_EXACT if the current version string and the requested 38 | # version string are exactly the same and it sets PACKAGE_VERSION_COMPATIBLE 39 | # if the current version is >= requested version. 40 | 41 | set(PACKAGE_VERSION @NViewsTrian_VERSION@) 42 | 43 | if ("${PACKAGE_VERSION}" VERSION_LESS "${PACKAGE_FIND_VERSION}") 44 | set(PACKAGE_VERSION_COMPATIBLE FALSE) 45 | else ("${PACKAGE_VERSION}" VERSION_LESS "${PACKAGE_FIND_VERSION}") 46 | set(PACKAGE_VERSION_COMPATIBLE TRUE) 47 | if ("${PACKAGE_FIND_VERSION}" STREQUAL "${PACKAGE_VERSION}") 48 | set(PACKAGE_VERSION_EXACT TRUE) 49 | endif ("${PACKAGE_FIND_VERSION}" STREQUAL "${PACKAGE_VERSION}") 50 | endif ("${PACKAGE_VERSION}" VERSION_LESS "${PACKAGE_FIND_VERSION}") 51 | -------------------------------------------------------------------------------- /cmake/setup_installation.cmake: -------------------------------------------------------------------------------- 1 | include(CMakePackageConfigHelpers) 2 | # Include module with fuction 'write_basic_package_version_file' 3 | 4 | # Configuration 5 | 6 | # Set up install directories. INCLUDE_INSTALL_DIR, LIB_INSTALL_DIR and 7 | # CMAKECONFIG_INSTALL_DIR must not be absolute paths. 8 | set(include_install_dir "include/NViewsTrian") 9 | set(LIB_INSTALL_DIR "${LIBRARY_TARGET_NAME}") 10 | set(CMAKECONFIG_INSTALL_DIR "share/${LIBRARY_TARGET_NAME}") 11 | set(RELATIVE_CMAKECONFIG_INSTALL_DIR "share/${LIBRARY_TARGET_NAME}") 12 | set(config_install_dir "lib/cmake/${LIBRARY_TARGET_NAME}-${PROJECT_VERSION}") 13 | set(exported_targets_name "${LIBRARY_TARGET_NAME}Targets") 14 | set(exported_targets_filename "${exported_targets_name}.cmake") 15 | set(export_dirpath "lib/cmake/${LIBRARY_TARGET_NAME}") 16 | set(config_basename "${LIBRARY_TARGET_NAME}Config") 17 | set(config_filename "${config_basename}.cmake") 18 | set(version_filename "${config_basename}Version.cmake") 19 | 20 | #################################################### 21 | ## THEIA INSPIRED ## 22 | #################################################### 23 | 24 | # Install the .h files 25 | file(GLOB ${LIBRARY_TARGET_NAME}_HDRS_GLOB ${CMAKE_SOURCE_DIR}/include/*.h) 26 | install(FILES ${${LIBRARY_TARGET_NAME}_HDRS_GLOB} DESTINATION ${include_install_dir}) 27 | 28 | install(DIRECTORY /include DESTINATION include/${LIBRARY_TARGET_NAME} FILES_MATCHING PATTERN "*.h") 29 | 30 | 31 | # This "exports" all targets which have been put into the export set 32 | # "TheiaExport". This means that CMake generates a file with the given 33 | # filename, which can later on be loaded by projects using this package. 34 | # This file contains ADD_LIBRARY(bar IMPORTED) statements for each target 35 | # in the export set, so when loaded later on CMake will create "imported" 36 | # library targets from these, which can be used in many ways in the same way 37 | # as a normal library target created via a normal ADD_LIBRARY(). 38 | install(EXPORT ${LIBRARY_TARGET_NAME_EXPORT} 39 | DESTINATION ${RELATIVE_CMAKECONFIG_INSTALL_DIR} FILE ${exported_targets_filename}) 40 | 41 | # Figure out the relative path from the installed Config.cmake file to the 42 | # install prefix (which may be at runtime different from the chosen 43 | # CMAKE_INSTALL_PREFIX if under Windows the package was installed anywhere) 44 | # This relative path will be configured into the TheiaConfig.cmake. 45 | file(RELATIVE_PATH INSTALL_ROOT_REL_CONFIG_INSTALL_DIR 46 | ${CMAKE_INSTALL_PREFIX}/${CMAKECONFIG_INSTALL_DIR} ${CMAKE_INSTALL_PREFIX}) 47 | 48 | # Create a TheiaConfig.cmake file. Config.cmake files are searched by 49 | # FIND_PACKAGE() automatically. We configure that file so that we can put any 50 | # information we want in it, e.g. version numbers, include directories, etc. 51 | configure_package_config_file( 52 | "${CMAKE_SOURCE_DIR}/cmake/${config_filename}.in" 53 | "${CMAKE_CURRENT_BINARY_DIR}/${config_filename}" 54 | INSTALL_DESTINATION "${config_install_dir}") 55 | 56 | # Additionally, when CMake has found a TheiaConfig.cmake, it can check for a 57 | # TheiaConfigVersion.cmake in the same directory when figuring out the version 58 | # of the package when a version has been specified in the FIND_PACKAGE() call, 59 | # e.g. FIND_PACKAGE(Theia [0.5.2] REQUIRED). The version argument is optional. 60 | configure_file("${CMAKE_SOURCE_DIR}/cmake/${version_filename}.in" 61 | "${CMAKE_CURRENT_BINARY_DIR}/${version_filename}" @ONLY) 62 | 63 | # Install these files into the same directory as the generated exports-file, 64 | # we include the FindPackage scripts for libraries whose headers are included 65 | # in the public API of Theia and should thus be present in THEIA_INCLUDE_DIRS. 66 | install(FILES "${CMAKE_CURRENT_BINARY_DIR}/${config_filename}" 67 | "${CMAKE_CURRENT_BINARY_DIR}/${version_filename}" 68 | "${CMAKE_SOURCE_DIR}/cmake/FindEigen.cmake" 69 | DESTINATION ${CMAKECONFIG_INSTALL_DIR}) 70 | 71 | 72 | # uninstall target 73 | if(NOT TARGET uninstall) 74 | configure_file( 75 | "${CMAKE_CURRENT_SOURCE_DIR}/cmake/CMakeUninstall.cmake.in" 76 | "${CMAKE_CURRENT_BINARY_DIR}/cmake/CMakeUninstall.cmake" 77 | IMMEDIATE @ONLY) 78 | 79 | add_custom_target(uninstall 80 | COMMAND ${CMAKE_COMMAND} -P ${CMAKE_CURRENT_BINARY_DIR}/cmake/CMakeUninstall.cmake) 81 | endif() 82 | 83 | -------------------------------------------------------------------------------- /examples/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | project(NViewsTrian-Examples CXX) 2 | 3 | 4 | # Find Eigen library 5 | set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_CURRENT_SOURCE_DIR}/../cmake/") 6 | find_package(Eigen3 3.3.3 REQUIRED) 7 | if(EIGEN3_FOUND) 8 | message(STATUS "Found Eigen3 library (version ${EIGEN3_VERSION})") 9 | message(STATUS "Eigen3 include directory: ${EIGEN3_INCLUDE_DIR}\n") 10 | else() 11 | message(STATUS "Eigen library not found!") 12 | endif() 13 | 14 | 15 | # Add the Eigen include directories 16 | include_directories(${EIGEN3_INCLUDE_DIR}) 17 | 18 | 19 | find_package(Ceres REQUIRED) 20 | include_directories(${CERES_INCLUDE_DIRS}) 21 | 22 | 23 | add_library(exp_gen ../utils/generatePointCloud.h ../utils/generatePointCloud.cpp) 24 | 25 | 26 | add_library(exp_ceres ../utils/ceresSolver.h ../utils/ceresSolver.cpp) 27 | 28 | 29 | 30 | 31 | # Basic example 32 | add_executable(example_base ${CMAKE_CURRENT_SOURCE_DIR}/example_base.cpp) 33 | target_link_libraries(example_base 34 | NViewsTrian 35 | exp_gen 36 | exp_ceres 37 | ${CERES_LIBRARIES} 38 | ) 39 | 40 | 41 | -------------------------------------------------------------------------------- /examples/example_base.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | 8 | // include types 9 | #include "NViewsTypes.h" 10 | // include utils 11 | #include "NViewsUtils.h" 12 | // triangulation solver 13 | #include "NViewsClass.h" 14 | 15 | 16 | // data generation 17 | #include "../utils/generatePointCloud.h" 18 | // ceres solver 19 | #include "../utils/ceresSolver.h" 20 | 21 | 22 | // for eigendecomposition 23 | #include 24 | #include 25 | 26 | #include // timer 27 | 28 | using namespace std::chrono; 29 | 30 | 31 | using namespace std; 32 | using namespace NViewsTrian; 33 | 34 | 35 | 36 | int main(int argc, char** argv) 37 | { 38 | 39 | std::cout << "Example N view triangulation\n"; 40 | 41 | 42 | // parameters for estimation 43 | double noise = 2.0; 44 | size_t n_points = 1; 45 | double max_parallax = 1.0; // in meters 46 | double focal_length = 512; 47 | size_t size_img = 512; 48 | double max_side = 8.0; 49 | double dist_center = 3.0; 50 | double max_rot = 0.5; 51 | int M_cameras = 100; 52 | 53 | std::srand(std::time(nullptr)); 54 | 55 | 56 | // generate problem 57 | PCRes str_out(n_points); 58 | PCParams str_in; 59 | str_in.noise = noise; 60 | str_in.focal_length = focal_length; 61 | str_in.size_img = size_img; 62 | str_in.N_points = n_points; 63 | str_in.M_cameras = M_cameras; 64 | str_in.max_side = max_side; 65 | str_in.dist_center = dist_center; 66 | 67 | str_out = generatePointCloud(str_in); //, UtilsTwoView::generateTranslationStereo); 68 | 69 | Matrix3 K = Matrix3::Identity(); 70 | K(0,0) = focal_length; 71 | K(1,1) = focal_length; 72 | K(0,2) = (double) size_img; 73 | K(1,2) = (double) size_img; 74 | Matrix3 iK = K.inverse(); 75 | 76 | // generate full graph (M 2) combinations 77 | 78 | Eigen::MatrixXd idx_matrix; 79 | int n_comb = generateM2Comb(M_cameras, idx_matrix); 80 | 81 | // generate correspondences 82 | std::vector set_corr; 83 | set_corr.empty(); 84 | for (int jj=0; jj(0,0) = R1; 96 | P1.block<3,1>(0,3) = t1; 97 | P2.block<3,3>(0,0) = R2; 98 | P2.block<3,1>(0,3) = t2; 99 | 100 | Matrix4 Prel = P2 * P1.inverse(); 101 | Matrix3 Rrel = Prel.block<3,3>(0,0); 102 | Vector3 trel = Prel.block<3,1>(0,3); 103 | trel.normalize(); 104 | 105 | 106 | 107 | Matrix3 Ess = Matrix3::Identity(); 108 | Matrix3 Tx = Matrix3::Zero(); 109 | Tx << 0, -trel(2), trel(1), trel(2), 0, -trel(0), -trel(1), trel(0), 0; 110 | // fill T 111 | Ess = Tx * Rrel; 112 | Matrix3 F = iK.transpose() * Ess * iK; 113 | // normalize F 114 | Eigen::JacobiSVD svd(F); 115 | F /= svd.singularValues()(1); 116 | corr_i.id1 = id1; 117 | corr_i.id2 = id2; 118 | corr_i.F = Ess; 119 | 120 | corr_i.p1 = iK * str_out.obs[0].col(id1); 121 | corr_i.p2 = iK * str_out.obs[0].col(id2); 122 | set_corr.push_back(corr_i); 123 | 124 | 125 | } 126 | 127 | // run correction method 128 | NViewsClass corr_N_view; 129 | // 1. Create constraint matrices 130 | corr_N_view.createProblemMatrices(set_corr, M_cameras); 131 | 132 | 133 | // 2. Run correction 134 | NViewsOptions options_corr; 135 | options_corr.save_val_constr = false; 136 | options_corr.debug = true; 137 | NViewsResult res_corr = corr_N_view.correctObservations(options_corr); 138 | 139 | // Show results 140 | corr_N_view.printResult(res_corr); 141 | 142 | 143 | 144 | // Matrix projections 145 | std::vector proj_s; 146 | proj_s.empty(); 147 | std::vector obs_s, obs_init, obs_ref; 148 | obs_s.empty(); 149 | obs_init.empty(); 150 | obs_ref.empty(); 151 | 152 | for (int jc=0; jc(0,0) = R; 159 | P1.block<3,1>(0,3) = t; 160 | proj_s.push_back(P1); 161 | 162 | // observations 163 | 164 | Vector3 pt = iK * str_out.obs[0].col(jc); 165 | obs_s.push_back(pt); 166 | 167 | // update observation init 168 | Vector3 delta_init; 169 | delta_init << res_corr.sol_init( jc*2), res_corr.sol_init(jc*2 + 1), 0; 170 | obs_init.push_back(pt + delta_init); 171 | 172 | // update observation refinenement 173 | Vector3 delta_ref; 174 | delta_ref << res_corr.sol_final( jc*2), res_corr.sol_final(jc*2 + 1), 0; 175 | 176 | obs_ref.push_back(pt + delta_ref); 177 | } 178 | 179 | // triangulate point 180 | Vector3 P_lin; 181 | Eigen::VectorXd depths_lin; 182 | double error_lin = triangulateNPoint(proj_s, obs_s, P_lin, depths_lin); 183 | 184 | Vector3 P_init; 185 | Eigen::VectorXd depths_init; 186 | double error_init = triangulateNPoint(proj_s, obs_init, P_init, depths_init); 187 | 188 | Vector3 P_ref; 189 | Eigen::VectorXd depths_ref; 190 | double error_ref = triangulateNPoint(proj_s, obs_ref, P_ref, depths_ref); 191 | 192 | std::cout << "Number constraints: " << n_comb << std::endl; 193 | std::cout << "Error linear: " << error_lin << std::endl; 194 | std::cout << "error init: " << error_init << std::endl; 195 | std::cout << "Error final: " << error_ref << std::endl; 196 | 197 | 198 | std::cout << "P3d:\n" << str_out.points_3D.col(0) << std::endl; 199 | std::cout << "P linear:\n" << P_lin << std::endl; 200 | std::cout << "P init:\n" << P_init << std::endl; 201 | std::cout << "P ref:\n" << P_ref << std::endl; 202 | 203 | 204 | /** Run ceres **/ 205 | std::vector,Eigen::Vector2d>> vector_pair_ceres_data; 206 | vector_pair_ceres_data.reserve(M_cameras); 207 | 208 | for (int jc=0; jc(0,0) = R; 216 | P1.block<3,1>(0,3) = t; 217 | 218 | // observations 219 | Vector3 pt = iK * str_out.obs[0].col(jc); 220 | 221 | std::pair,Eigen::Vector2d> pair_i(P1, pt.topRows(2)); 222 | vector_pair_ceres_data.push_back(pair_i); 223 | } 224 | 225 | auto start_ceres = high_resolution_clock::now(); 226 | Eigen::Vector3d P_ceres = CeresSolver::Triangulate(vector_pair_ceres_data, P_lin); 227 | auto time_ceres = duration_cast(high_resolution_clock::now() - start_ceres); 228 | 229 | std::cout << "Point 3D from ceres:\n" << P_ceres << std::endl; 230 | std::cout << "Time ceres: " << (double) time_ceres.count() << std::endl; 231 | 232 | 233 | 234 | return 0; 235 | 236 | } // end of main fcn 237 | -------------------------------------------------------------------------------- /include/NViewsCertifier.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | // include types 9 | #include "NViewsTypes.h" 10 | 11 | 12 | namespace NViewsTrian 13 | { 14 | 15 | /* Struct for the result */ 16 | struct NCertRes 17 | { 18 | double min_eig = -10; 19 | double error_mult = -10; 20 | Eigen::VectorXd mult; 21 | Eigen::MatrixXd Hess; 22 | 23 | double time_mult = 10; 24 | double time_hess = 10; 25 | 26 | NCertRes(){}; 27 | }; // end of struct result 28 | 29 | 30 | class NViewCertClass 31 | { 32 | public: 33 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 34 | 35 | NViewCertClass(const int M, 36 | const std::vector& constr, 37 | bool debug = true): 38 | M_(2*M), constr_red_(constr), debug_(debug){}; 39 | ~NViewCertClass(){}; 40 | 41 | /* Compute multipliers */ 42 | 43 | double computeMult(const Eigen::VectorXd & sol, 44 | const Eigen::MatrixXd & C, 45 | Eigen::VectorXd & sol_mult); 46 | 47 | /* Form Hessian and compute minimum eigenvalue */ 48 | double computeHessian(const double cost_sol, 49 | const Eigen::VectorXd & mult, 50 | Eigen::MatrixXd & Hess); 51 | 52 | /* Check optimality of solution */ 53 | NCertRes checkOptimality(const Eigen::VectorXd & sol, 54 | const Eigen::MatrixXd & C); 55 | 56 | /* Print results */ 57 | void printResult(const NCertRes & res); 58 | 59 | private: 60 | std::vector constr_red_; // constraints 61 | int M_; // 2 * number cameras 62 | bool debug_; // debug flag 63 | 64 | }; // end of main class 65 | 66 | 67 | 68 | } // end of namespace 69 | -------------------------------------------------------------------------------- /include/NViewsClass.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | // include types 9 | #include "NViewsTypes.h" 10 | 11 | 12 | namespace NViewsTrian 13 | { 14 | 15 | 16 | /* Struct with options */ 17 | struct NViewsOptions 18 | { 19 | int max_iters = 5; 20 | double max_constr = 1e-10; 21 | double max_tot_constr = 1e-09; 22 | double max_diff_sol = 3e-10; 23 | 24 | bool save_val_constr = false; // record the final values 25 | bool record_constr = false; // record all the values 26 | 27 | bool debug = true; // debug flag 28 | bool debug_cert = false; // debug flag for certifier 29 | NViewsOptions(){}; 30 | }; // end of struct options 31 | 32 | 33 | /* Struct for the result */ 34 | struct NViewsResult 35 | { 36 | int n_iters = 0; 37 | Eigen::VectorXd sol_init; 38 | Eigen::VectorXd sol_final; 39 | double max_constr = 10; 40 | double tot_constr = 10; 41 | double sq_constr = 10; 42 | 43 | double error_lin = 10; 44 | 45 | double diff_sol = 10; 46 | Eigen::VectorXd all_constr; 47 | std::vector rec_constr = {}; 48 | 49 | double tot_constr_init = 10; 50 | double max_constr_init = 10; 51 | double sq_constr_init = 10; 52 | 53 | double time_init = 100; 54 | double time_ref = 100; 55 | double time_opt = 100; 56 | double time_cert_mult = 100; 57 | double time_cert_hess = 100; 58 | 59 | double min_eig = -1; 60 | double error_mult = -1; 61 | 62 | 63 | 64 | NViewsResult(){}; 65 | }; // end of struct result 66 | 67 | 68 | class NViewsClass 69 | { 70 | public: 71 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 72 | 73 | NViewsClass(){}; 74 | ~NViewsClass(){}; 75 | 76 | /* Create problem matrices */ 77 | void createProblemMatrices(const std::vector & obj, 78 | const int N_cams); 79 | 80 | 81 | /* Initialize the solution sol_init */ 82 | double initCorrection(Eigen::VectorXd & sol_init, Eigen::MatrixXd & A, Eigen::VectorXd & b); 83 | 84 | /* Single refinement of the solution sol_init */ 85 | double refineCorrection(const Eigen::MatrixXd & A, 86 | const Eigen::VectorXd & b, 87 | const Eigen::VectorXd & sol_init, 88 | Eigen::VectorXd & sol_ref, 89 | Eigen::MatrixXd & Ci); 90 | /* Main function */ 91 | NViewsResult correctObservations(NViewsOptions & options); 92 | 93 | /* Show results */ 94 | void printResult(NViewsResult & res_corr); 95 | 96 | void getConstrRed(std::vector &C){ C = constr_red_;}; 97 | 98 | int checkOpt(const Eigen::VectorXd & sol_init, double & time_opt, int &, int & ); 99 | 100 | private: 101 | int M_; // number constraints 102 | int N_cams_; // number cameras 103 | std::vector constr_red_; // reduced constraints 104 | bool constr_are_created_; // true if the constraints are done 105 | 106 | }; // end of main class 107 | 108 | 109 | 110 | } // end of namespace 111 | -------------------------------------------------------------------------------- /include/NViewsTypes.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace NViewsTrian 6 | { 7 | typedef Eigen::Matrix Vector2; 8 | typedef Eigen::Matrix Vector3; 9 | typedef Eigen::Matrix Vector4; 10 | typedef Eigen::Matrix Vector6; 11 | 12 | 13 | typedef Eigen::Matrix Matrix2; 14 | typedef Eigen::Matrix Matrix3; 15 | typedef Eigen::Matrix Matrix4; 16 | typedef Eigen::Matrix Matrix34; 17 | 18 | 19 | struct PairObj 20 | { 21 | 22 | int id1 = 0; 23 | int id2 = 0; 24 | Vector3 p1; 25 | Vector3 p2; 26 | Matrix3 F; 27 | 28 | PairObj(){}; 29 | 30 | PairObj(const Vector3 & p1, 31 | const Vector3 & p2, 32 | const Matrix3 & F, 33 | const int id1, 34 | const int id2) 35 | : p1(p1), p2(p2), id1(id1), id2(id2), F(F) {}; 36 | }; // end of PairObj struct 37 | 38 | /* Struct for constraints */ 39 | struct Constr2View 40 | { 41 | Eigen::Matrix2d F = Eigen::Matrix2d::Identity(); 42 | double b = 0; 43 | Eigen::Vector2d Fp2 = Eigen::Vector2d::Zero(); 44 | Eigen::Vector2d Fp1 = Eigen::Vector2d::Zero(); 45 | int id_1 = 0; 46 | int id_2 = 0; 47 | Constr2View(){}; 48 | }; // end of struct for constraints 49 | 50 | } // end of namespace NViewsTrian 51 | -------------------------------------------------------------------------------- /include/NViewsUtils.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | // include types 9 | #include "NViewsTypes.h" 10 | 11 | namespace NViewsTrian{ 12 | 13 | // check constraints 14 | double checkConstraints(const Eigen::VectorXd & sol, 15 | std::vector & constr, 16 | Eigen::VectorXd & val_constr, 17 | double & max_constr_val, 18 | double & sq_constr_val); 19 | 20 | // solve linear system minimum norm 21 | double solveLinearSystemMinNorm(const Eigen::MatrixXd & A, 22 | const Eigen::VectorXd & b, 23 | Eigen::VectorXd & sol); 24 | // triangulate point 25 | double triangulateNPoint(const std::vector> & proj_s, 26 | const std::vector & obs_s, 27 | Eigen::Vector3d & P_3d, 28 | Eigen::VectorXd & depths); 29 | 30 | 31 | double triangulateNPoint(const std::vector & proj_s, 32 | const std::vector & obs_s, 33 | Eigen::Vector3d & P_3d, 34 | Eigen::VectorXd & depths); 35 | 36 | double solveLinearSystemMinNormIt(const Eigen::MatrixXd & A, 37 | const Eigen::VectorXd & b, 38 | Eigen::VectorXd & sol); 39 | } // end of namespace NViewsTrian 40 | -------------------------------------------------------------------------------- /src/NViewsCertifier.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | // include types 8 | #include "NViewsTypes.h" 9 | 10 | // include utils 11 | #include "NViewsUtils.h" 12 | 13 | // include headers 14 | #include "NViewsCertifier.h" 15 | 16 | // for eigendecomposition 17 | #include 18 | #include 19 | 20 | #include // timer 21 | 22 | using namespace std::chrono; 23 | 24 | namespace NViewsTrian 25 | { 26 | 27 | 28 | void invertPosDiagMatrix(const Eigen::VectorXd & D, 29 | double thresh, 30 | Eigen::MatrixXd & D2) 31 | { 32 | 33 | Eigen::VectorXd D3 = D; 34 | for (int i = 0; i < D.rows(); i++) 35 | { 36 | D3(i) = (D3(i) < thresh) ? 0 : 1 / D3(i); 37 | 38 | } 39 | D2 = D3.asDiagonal(); 40 | 41 | } 42 | 43 | 44 | 45 | // second version: faster 46 | double NViewCertClass::computeMult(const Eigen::VectorXd & sol, 47 | const Eigen::MatrixXd & C, 48 | Eigen::VectorXd & sol_mult) 49 | { 50 | 51 | if (C.rows() == 1) 52 | sol_mult = 2 * C * sol / (C.squaredNorm()); 53 | 54 | else 55 | { 56 | Eigen::SelfAdjointEigenSolver svd(C.transpose() * C); 57 | Eigen::MatrixXd U = svd.eigenvectors(); 58 | Eigen::MatrixXd D; 59 | invertPosDiagMatrix(svd.eigenvalues().cwiseAbs().cwiseSqrt(), 1e-06, D); 60 | 61 | Eigen::VectorXd ss = D * U.transpose() * sol; 62 | 63 | 64 | sol_mult = 2 * C * U * D * ss; 65 | 66 | 67 | } 68 | 69 | double error_m = ( 0.5 * C.transpose() * sol_mult - sol).squaredNorm(); 70 | 71 | 72 | return error_m; 73 | 74 | } 75 | 76 | 77 | 78 | double NViewCertClass::computeHessian(const double cost_sol, 79 | const Eigen::VectorXd & mult, 80 | Eigen::MatrixXd & Hess) 81 | { 82 | auto start_t_mult = high_resolution_clock::now(); 83 | Hess = Eigen::MatrixXd::Identity(M_+1, M_+1); 84 | Hess(M_, M_) = - cost_sol; 85 | int N = constr_red_.size(); 86 | 87 | for (int i=0; i (id_1, id_2) -= 0.5 * mult(i) * constr_red_[i].F; 95 | // the symmetric part 96 | Hess.block<2,2>(id_2, id_1) -= 0.5 * mult(i) * constr_red_[i].F.transpose(); 97 | 98 | Hess.block<2,1>(id_1, M_) -= 0.5 * mult(i) * constr_red_[i].Fp2; 99 | // symmetry 100 | Hess.block<1,2>(M_, id_1) -= 0.5 * mult(i) * constr_red_[i].Fp2.transpose(); 101 | Hess.block<2,1>(id_2, M_) -= 0.5 * mult(i) * constr_red_[i].Fp1; 102 | // symmetry 103 | Hess.block<1,2>(M_, id_2) -= 0.5 * mult(i) * constr_red_[i].Fp1.transpose(); 104 | } 105 | 106 | auto time_mult = duration_cast(high_resolution_clock::now() - start_t_mult); 107 | 108 | auto start_t_mult2 = high_resolution_clock::now(); 109 | Eigen::SelfAdjointEigenSolver eigen_solver_M(Hess); 110 | 111 | Eigen::VectorXd eigen_hess = eigen_solver_M.eigenvalues().real(); 112 | auto time_mult2 = duration_cast(high_resolution_clock::now() - start_t_mult2); 113 | 114 | 115 | 116 | if (debug_) 117 | { 118 | std::cout << "[HESS] Time matrix: " << (double) time_mult.count() << std::endl; 119 | std::cout << "[HESS] Time EIG: " << (double) time_mult2.count() << std::endl; 120 | } 121 | 122 | 123 | 124 | return eigen_hess(0); 125 | } 126 | 127 | NCertRes NViewCertClass::checkOptimality(const Eigen::VectorXd & sol, 128 | const Eigen::MatrixXd & C) 129 | { 130 | /* Main function: 131 | 1. compute multipliers 132 | 2. compute hessian 133 | */ 134 | 135 | // 1. Compute multipliers 136 | Eigen::VectorXd sol_mult; 137 | 138 | auto start_t_mult = high_resolution_clock::now(); 139 | double error_lin = computeMult(sol, C, sol_mult); 140 | auto time_mult = duration_cast(high_resolution_clock::now() - start_t_mult); 141 | 142 | // 2. Form and check Hessian 143 | double cost_sol = sol.dot(sol); 144 | 145 | 146 | Eigen::MatrixXd Hess; 147 | auto start_t_hess = high_resolution_clock::now(); 148 | 149 | // reduced 150 | double min_eigen = computeHessian(cost_sol, sol_mult, Hess); 151 | 152 | auto time_hess = duration_cast(high_resolution_clock::now() - start_t_hess); 153 | 154 | 155 | if (debug_) 156 | { 157 | std::cout << "[OPT] Error linear system for multipliers: " << error_lin << std::endl; 158 | std::cout << "[OPT] Minimum eigenvalue Hessian: " << min_eigen << std::endl; 159 | } 160 | 161 | NCertRes res; 162 | res.min_eig = min_eigen; 163 | res.error_mult = error_lin; 164 | 165 | res.time_mult = (double) time_mult.count(); 166 | res.time_hess = (double) time_hess.count(); 167 | 168 | if (debug_) 169 | { 170 | res.mult = sol_mult; 171 | res.Hess = Hess; 172 | } 173 | return res; 174 | 175 | } 176 | 177 | 178 | void NViewCertClass::printResult(const NCertRes & res) 179 | { 180 | std::cout << "---------------------\n| SOLUTION |\n---------------------\n"; 181 | std::cout << "Minimum eigenvalue Hessian: " << res.min_eig << std::endl; 182 | std::cout << "Time for multipliers [nanosecs]: " << res.time_mult << std::endl; 183 | std::cout << "Time for Hessian [nanosecs]: " << res.time_hess << std::endl; 184 | 185 | } 186 | } // end of namespace NViewtrian 187 | -------------------------------------------------------------------------------- /src/NViewsClass.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | // include types 8 | #include "NViewsTypes.h" 9 | // utils 10 | #include "NViewsUtils.h" 11 | // header 12 | #include "NViewsClass.h" 13 | // certifier 14 | #include "NViewsCertifier.h" 15 | 16 | #include // timer 17 | 18 | using namespace std::chrono; 19 | 20 | namespace NViewsTrian 21 | { 22 | // create problem matrices 23 | void NViewsClass::createProblemMatrices(const std::vector & obj, 24 | const int N_cams) 25 | { 26 | 27 | constr_red_.empty(); 28 | 29 | M_ = obj.size(); 30 | N_cams_ = N_cams; 31 | int s_constr = 2 * N_cams_ + 1; 32 | for (int i=0; i(i, constr_red_[i].id_1) = constr_red_[i].Fp2.transpose(); 79 | A.block<1,2>(i, constr_red_[i].id_2) = constr_red_[i].Fp1.transpose(); 80 | 81 | b(i) = constr_red_[i].b; 82 | } 83 | 84 | sol_init.resize(2 * N_cams_); 85 | double error_sol = solveLinearSystemMinNorm(A.transpose() * A, - A.transpose() * b, sol_init); 86 | 87 | 88 | return error_sol; 89 | 90 | } 91 | 92 | 93 | 94 | 95 | // correction ref 96 | double NViewsClass::refineCorrection(const Eigen::MatrixXd & A0, 97 | const Eigen::VectorXd & b0, 98 | const Eigen::VectorXd & sol_init, 99 | Eigen::VectorXd & sol_ref, 100 | Eigen::MatrixXd & Ci) 101 | { 102 | 103 | 104 | Eigen::MatrixXd C(M_, 2 * N_cams_); 105 | Eigen::VectorXd e(M_); 106 | 107 | C = A0; 108 | e = b0; 109 | 110 | 111 | for (int i=0; i < M_; i++) 112 | { 113 | 114 | Vector2 p1 = sol_init.block<2,1>(constr_red_[i].id_1, 0); 115 | Vector2 p2 = sol_init.block<2,1>(constr_red_[i].id_2, 0); 116 | 117 | 118 | C.block<1,2>(i, constr_red_[i].id_1) += p2.transpose() * constr_red_[i].F.transpose(); 119 | C.block<1,2>(i, constr_red_[i].id_2) += p1.transpose() * constr_red_[i].F; 120 | e(i) -= p1.transpose() * constr_red_[i].F * p2; 121 | } 122 | 123 | Eigen::VectorXd sol_delta(2 * N_cams_); 124 | double error_sol_t; 125 | 126 | if (C.cols() <= C.rows()) 127 | error_sol_t = solveLinearSystemMinNorm(C.transpose() * C, - C.transpose() * e, sol_delta); 128 | else 129 | error_sol_t = solveLinearSystemMinNorm(C, - e, sol_delta); 130 | 131 | 132 | 133 | // Save output 134 | sol_ref.resize(2 * N_cams_); 135 | sol_ref = sol_delta; 136 | 137 | Ci.resize(C.rows(), C.cols()); 138 | Ci = C; 139 | return error_sol_t; 140 | 141 | } 142 | 143 | 144 | // correction: K-th refinement 145 | // main function 146 | NViewsResult NViewsClass::correctObservations(NViewsOptions & options) 147 | { 148 | 149 | if (!constr_are_created_) 150 | { 151 | std::cout << "[ERRROR] Constraints are not created!\n"; 152 | return NViewsResult(); 153 | } 154 | // else 155 | 156 | Eigen::VectorXd sol_i, sol_init; 157 | Eigen::MatrixXd A0; 158 | Eigen::VectorXd b0; 159 | 160 | if (options.debug) 161 | std::cout << "[CORR] Estimating initial guess\n"; 162 | 163 | auto start_t_init = high_resolution_clock::now(); 164 | double error_init = initCorrection(sol_init, A0, b0); 165 | auto time_init = duration_cast(high_resolution_clock::now() - start_t_init); 166 | 167 | 168 | Eigen::VectorXd val_constr_i; 169 | double max_constr_val_i = 10.0, sq_constr_i = 10.0; 170 | auto start_t_ref_i2 = high_resolution_clock::now(); 171 | 172 | 173 | double tot_constr_i = checkConstraints(sol_init, constr_red_, val_constr_i, max_constr_val_i, sq_constr_i); 174 | 175 | auto time_ref_i2 = duration_cast(high_resolution_clock::now() - start_t_ref_i2); 176 | int k_iter = 0; 177 | 178 | if (options.debug) 179 | { 180 | std::cout << "[CORR] Maximum value constraint for initial guess: " << max_constr_val_i << std::endl; 181 | std::cout << "[CORR] L-1 norm constraints for initial guess: " << tot_constr_i << std::endl; 182 | } 183 | 184 | 185 | 186 | 187 | 188 | NViewsResult res = NViewsResult(); 189 | res.tot_constr_init = tot_constr_i; 190 | res.max_constr_init = max_constr_val_i; 191 | res.sq_constr_init = sq_constr_i; 192 | if (options.record_constr) 193 | { 194 | res.rec_constr.empty(); 195 | res.rec_constr.push_back(val_constr_i); 196 | 197 | } 198 | 199 | if (options.debug) 200 | { 201 | std::cout << "[CORR] Refining solution\n--- --- --- --- --- --- --- --- ---\n"; 202 | } 203 | 204 | sol_i = sol_init; 205 | double time_ref = 0; 206 | double diff_sol = 10; 207 | double error_lin = 10; 208 | Eigen::MatrixXd Cnext = A0; 209 | // evaluate stopping condition 210 | // We sop if 211 | // 1. we reach the maximum number of iterations 212 | // 2. The diff between solutions is less than threshold 213 | 214 | while ( (k_iter < options.max_iters) && (diff_sol > options.max_diff_sol) ) 215 | { 216 | 217 | // keep refining 218 | Eigen::VectorXd sol_next = sol_i; 219 | 220 | auto start_t_ref_i = high_resolution_clock::now(); 221 | 222 | error_lin = refineCorrection(A0, b0, sol_i, sol_next, Cnext); 223 | 224 | auto time_ref_i = duration_cast(high_resolution_clock::now() - start_t_ref_i); 225 | 226 | diff_sol = (sol_next - sol_i).squaredNorm(); 227 | 228 | 229 | tot_constr_i = checkConstraints(sol_next, constr_red_, val_constr_i, max_constr_val_i, sq_constr_i); 230 | 231 | if (options.debug) 232 | { 233 | std::cout << "[CORR] Iteration #" << k_iter; 234 | std::cout << " Max constraint: " << max_constr_val_i; 235 | std::cout << " Total constraint L1: " << tot_constr_i; 236 | std::cout << " Total constraint L2: " << sq_constr_i; 237 | std::cout << " Norm prev solution: " << sol_i.squaredNorm(); 238 | std::cout << " Norm new solution: " << sol_next.squaredNorm(); 239 | std::cout << " Diff between solutions: " << diff_sol << std::endl; 240 | 241 | } 242 | 243 | if (options.record_constr) 244 | { 245 | res.rec_constr.push_back(val_constr_i); 246 | 247 | } 248 | time_ref += (double) time_ref_i.count(); 249 | 250 | // update iteration 251 | k_iter++; 252 | sol_i = sol_next; 253 | 254 | } 255 | 256 | 257 | if (options.debug) 258 | { 259 | std::cout << "\n--- --- --- --- --- --- --- --- ---\n"; 260 | std::cout << "[CORR] Nr. iterations: " << k_iter; 261 | std::cout << " Max constraint: " << max_constr_val_i; 262 | std::cout << " Total constraint L1: " << tot_constr_i; 263 | std::cout << " Total constraint L2: " << sq_constr_i; 264 | std::cout << " Norm intial solution: " << sol_init.squaredNorm(); 265 | std::cout << " Norm final solution: " << sol_i.squaredNorm() << std::endl; 266 | std::cout << " Diff between solutions: " << diff_sol << std::endl; 267 | } 268 | 269 | // Check optimality with sufficient condition 270 | 271 | // suff. condition 272 | NViewCertClass cert_obj(N_cams_, constr_red_, options.debug_cert); 273 | 274 | // call function 275 | auto start_t_cert = high_resolution_clock::now(); 276 | NCertRes res_cert = cert_obj.checkOptimality(sol_i, Cnext); 277 | auto time_cert = duration_cast(high_resolution_clock::now() - start_t_cert); 278 | 279 | 280 | 281 | /* Save results */ 282 | 283 | res.min_eig = res_cert.min_eig; 284 | res.error_mult = res_cert.error_mult; 285 | res.time_cert_mult = res_cert.time_mult; 286 | res.time_cert_hess = res_cert.time_hess; 287 | 288 | 289 | res.n_iters = k_iter; 290 | res.sol_init = sol_init; 291 | res.sol_final = sol_i; 292 | res.max_constr = max_constr_val_i; 293 | res.tot_constr = tot_constr_i; 294 | res.sq_constr = sq_constr_i; 295 | res.diff_sol = diff_sol; 296 | if (options.save_val_constr) 297 | res.all_constr = val_constr_i; 298 | res.time_init = (double) time_init.count(); 299 | res.time_ref = time_ref; 300 | res.time_opt = (double) time_cert.count(); 301 | res.error_lin = error_lin; 302 | return res; 303 | } 304 | 305 | 306 | // Print solution 307 | void NViewsClass::printResult(NViewsResult & res_corr) 308 | { 309 | 310 | std::cout << "---------------------\n| SOLUTION |\n---------------------\n"; 311 | std::cout << "Number iterations: " << res_corr.n_iters << std::endl; 312 | // std::cout << "Initial solution:\n" << res_corr.sol_init << std::endl; 313 | // std::cout << "Final solution:\n" << res_corr.sol_final << std::endl; 314 | std::cout << "Maximum value of constraint: " << res_corr.max_constr << std::endl; 315 | std::cout << "Norm of the constraints [L1]: " << res_corr.tot_constr << std::endl; 316 | std::cout << "Norm of the constraints L2: " << res_corr.sq_constr << std::endl; 317 | std::cout << "Difference between the last two solutions: " << res_corr.diff_sol << std::endl; 318 | std::cout << "Error linear system: " << res_corr.error_lin << std::endl; 319 | std::cout << "Minimum eigenvalue Hessian: " << res_corr.min_eig << std::endl; 320 | if (res_corr.all_constr.size() > 0) 321 | std::cout << "value of all the constraints:\n" << res_corr.all_constr << std::endl; 322 | if (res_corr.rec_constr.size() > 0) 323 | { 324 | std::cout << "The value of the constraints for the iterations are:\n"; 325 | for (int j = 0; j < res_corr.rec_constr.size(); j++) 326 | { 327 | std::cout << "Iteration #" << j << std::endl; 328 | std::cout << res_corr.rec_constr[j].transpose() << std::endl; 329 | 330 | } 331 | 332 | } 333 | std::cout << "Time init [nanosecs]: " << res_corr.time_init << std::endl; 334 | std::cout << "Time ref [nanosecs]: " << res_corr.time_ref << std::endl; 335 | std::cout << "Time cert [nanosecs]: " << res_corr.time_opt << std::endl; 336 | } 337 | 338 | 339 | 340 | 341 | 342 | 343 | 344 | } // end of namespace 345 | -------------------------------------------------------------------------------- /src/NViewsUtils.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | // include types 8 | #include "NViewsTypes.h" 9 | 10 | // include header 11 | #include "NViewsUtils.h" 12 | 13 | #define EIGEN_USE_LAPACKE_STRICT 14 | // for eigendecomposition 15 | #include 16 | #include 17 | 18 | #include // timer 19 | 20 | 21 | 22 | using namespace std::chrono; 23 | using namespace std; 24 | 25 | 26 | namespace NViewsTrian 27 | { 28 | 29 | 30 | // Check constraints with reduced constraints 31 | double checkConstraints(const Eigen::VectorXd & sol, 32 | std::vector & constr, 33 | Eigen::VectorXd & val_constr, 34 | double & max_constr_val, 35 | double & sq_constr_val) 36 | { 37 | // Evaluate the constraints at the given solution 38 | int M = constr.size(); 39 | val_constr.resize(M); 40 | val_constr.setZero(); 41 | 42 | double tot_constr = 0.0; 43 | max_constr_val = -10.0; 44 | sq_constr_val = 0.0; 45 | for (int i=0; i < M; i++) 46 | { 47 | 48 | Vector2 p1 = sol.block<2,1>(constr[i].id_1, 0); 49 | Vector2 p2 = sol.block<2,1>(constr[i].id_2, 0); 50 | double val_i = constr[i].b + p1.transpose() * constr[i].F * p2 + p1.dot(constr[i].Fp2) + p2.dot(constr[i].Fp1); 51 | 52 | 53 | val_constr(i) = val_i; 54 | double abs_val_i = (val_i > 0) ? val_i : -val_i; 55 | tot_constr += abs_val_i; 56 | sq_constr_val += abs_val_i * abs_val_i; 57 | if (abs_val_i > max_constr_val) 58 | max_constr_val = abs_val_i; 59 | } 60 | return tot_constr; 61 | } 62 | 63 | 64 | // solve linear system minimum norm 65 | double solveLinearSystemMinNorm(const Eigen::MatrixXd & A, 66 | const Eigen::VectorXd & b, 67 | Eigen::VectorXd & sol) 68 | { 69 | // tol_rank = 1e-03 * 1e-03 * 1e-03; 70 | double tol_rank = 1e-03 * 1e-03 * 1e-03; 71 | 72 | Eigen::VectorXd y_sol; 73 | auto start_t_init = high_resolution_clock::now(); 74 | Eigen::CompleteOrthogonalDecomposition cod(A.rows(), 75 | A.cols()); 76 | cod.setThreshold(tol_rank); 77 | cod.compute(A); 78 | auto time_init = duration_cast(high_resolution_clock::now() - start_t_init); 79 | 80 | 81 | 82 | auto start_t_init2 = high_resolution_clock::now(); 83 | y_sol = cod.solve(b); 84 | auto time_init2 = duration_cast(high_resolution_clock::now() - start_t_init2); 85 | 86 | 87 | 88 | Eigen::VectorXd e_lin = A * y_sol - b; 89 | double error_sol = e_lin.squaredNorm(); 90 | sol.resize(A.cols(), 1); 91 | sol = y_sol; 92 | return error_sol; 93 | } 94 | 95 | 96 | 97 | 98 | 99 | double triangulateNPoint(const std::vector> & proj_s, 100 | const std::vector & obs_s, 101 | Eigen::Vector3d & P_3d, 102 | Eigen::VectorXd & depths) 103 | { 104 | 105 | std::vector P4 = {}; 106 | int N_cams = proj_s.size(); 107 | P4.reserve(N_cams); 108 | 109 | for (int i=0; i < N_cams; i++) 110 | { 111 | Eigen::Matrix4d Pi = Eigen::Matrix4d::Identity(); 112 | 113 | Pi.block<3,4>(0, 0) = proj_s[i]; 114 | P4.push_back(Pi); 115 | } 116 | 117 | return (triangulateNPoint(P4, obs_s, P_3d, depths)); 118 | 119 | } 120 | // triangulate point 121 | double triangulateNPoint(const std::vector & proj_s, 122 | const std::vector & obs_s, 123 | Eigen::Vector3d & P_3d, 124 | Eigen::VectorXd & depths) 125 | { 126 | 127 | int N_cams = proj_s.size(); 128 | Eigen::MatrixXd P(N_cams * 3, N_cams + 4); 129 | P.setZero(); 130 | for (int i=0; i < N_cams; i++) 131 | { 132 | const int ri = (i)*3; 133 | // int rf = (i-1)*3 + 2; 134 | P.block<3,4>(ri, 0) = proj_s[i].block<3,4>(0,0); 135 | P.block<3,1>(ri, 4+i) = obs_s[i]; 136 | } 137 | 138 | Eigen::JacobiSVD svd(P, Eigen::ComputeFullV); 139 | P_3d.setZero(); 140 | Eigen::Vector4d P_hom = svd.matrixV().topRightCorner(4, 1); 141 | P_3d = P_hom.topRows(3) / P_hom(3); 142 | 143 | depths.resize(N_cams); 144 | depths.setZero(); 145 | depths = svd.matrixV().bottomRightCorner(N_cams, 1); 146 | 147 | int last_id = std::min(N_cams * 3, N_cams + 4) - 1; 148 | 149 | double error_lin = svd.singularValues()(last_id); 150 | 151 | return error_lin; 152 | } 153 | 154 | 155 | 156 | } // end of namespace NViewtrian 157 | 158 | -------------------------------------------------------------------------------- /tests/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | project(NViewsTrian-Tests CXX) 2 | 3 | # Find Eigen library 4 | set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_CURRENT_SOURCE_DIR}/../cmake/") 5 | find_package(Eigen3 3.3.3 REQUIRED) 6 | if(EIGEN3_FOUND) 7 | message(STATUS "Found Eigen3 library (version ${EIGEN3_VERSION})") 8 | message(STATUS "Eigen3 include directory: ${EIGEN3_INCLUDE_DIR}\n") 9 | else() 10 | message(STATUS "Eigen library not found!") 11 | endif() 12 | 13 | # Add the Eigen include directories 14 | include_directories(${EIGEN3_INCLUDE_DIR}) 15 | 16 | 17 | find_package(Ceres REQUIRED) 18 | include_directories(${CERES_INCLUDE_DIRS}) 19 | 20 | 21 | add_library(exp_help_test experimentsHelper.h ../utils/generatePointCloud.h experimentsHelper.cpp ../utils/generatePointCloud.cpp) 22 | 23 | 24 | add_library(exp_ceres_test ../utils/ceresSolver.h ../utils/ceresSolver.cpp) 25 | 26 | 27 | 28 | ## Generic test synthetic 29 | add_executable(generic_test ${CMAKE_CURRENT_SOURCE_DIR}/generic_test.cpp) 30 | target_link_libraries(generic_test 31 | NViewsTrian 32 | exp_help_test 33 | exp_ceres 34 | ${CERES_LIBRARIES} 35 | ) 36 | 37 | 38 | 39 | 40 | -------------------------------------------------------------------------------- /tests/basic_params.txt: -------------------------------------------------------------------------------- 1 | # Number iterations 2 | 5 3 | # Number points per problem instance 4 | 10 5 | # Number cameras 6 | 5, 5, 10, 50, 100, 200 7 | # Maximum parallax 8 | 1, 3.0 9 | # Focal length 10 | 1, 512 11 | # Noise 12 | 4, 0.5, 1.5, 3.0, 5.0 13 | # Distance to center 14 | 1, 3.0 15 | # Use custom t 16 | 0 17 | # Translation vector 18 | 3, 1, 1,1 19 | # Maximum angle rotation 20 | 0.5 21 | # Pose generation option 22 | 2 23 | # Noise rotation 24 | 0.01 25 | # Noise translation vector 26 | 0.01 27 | 28 | -------------------------------------------------------------------------------- /tests/experimentsHelper.cpp: -------------------------------------------------------------------------------- 1 | #include "experimentsHelper.h" 2 | 3 | 4 | 5 | template 6 | void readSetParams(ifstream & in_file, int & N, vector & arr) 7 | { 8 | 9 | string line_i, line2; 10 | 11 | // discard first line (comment) 12 | getline(in_file, line_i); 13 | // get param 14 | getline(in_file, line_i); 15 | // get nine components 16 | std::stringstream ss(line_i); 17 | 18 | getline(ss, line2, ','); 19 | N = (int)atof(line2.c_str()); 20 | 21 | // clear arr if it is not empty 22 | if (!arr.empty()) arr.clear(); 23 | 24 | for (size_t j_col = 0; j_col < N; j_col++) 25 | { 26 | getline(ss, line2, ','); 27 | arr.push_back((double)atof(line2.c_str())); 28 | } 29 | 30 | // show 31 | std::cout << "Set of params read:\n"; 32 | for (size_t j_col = 0; j_col < N; j_col++) 33 | { 34 | std::cout << arr[j_col] << " "; 35 | } 36 | std::cout << std::endl; 37 | 38 | } 39 | 40 | 41 | 42 | bool readOptionsFromFile(string name_file, 43 | SceneOptions & options) 44 | { 45 | 46 | // aux. strings 47 | string line_i, line2; 48 | 49 | 50 | // try to open the file 51 | // open file 52 | ifstream in_file(name_file); 53 | 54 | // check if file exists 55 | if (!in_file.good()) return false; 56 | 57 | 58 | /* READ PARAMETERS !!! */ 59 | 60 | /* MAXIMUM NUMBER OF ITERATIONS */ 61 | // discard first line (comment) 62 | getline(in_file, line_i); 63 | // get param 64 | getline(in_file, line_i); 65 | options.max_iter = (double)atof(line_i.c_str()); 66 | std::cout << "Maximum number of iterations: " << options.max_iter << std::endl; 67 | 68 | /* Number points per problem instance */ 69 | // discard first line (comment) 70 | getline(in_file, line_i); 71 | // get param 72 | getline(in_file, line_i); 73 | options.n_points = (int)atof(line_i.c_str()); 74 | std::cout << "Number points per problem instance: " << options.n_points << std::endl; 75 | 76 | /* Image size */ 77 | std::cout << "Number cameras\n"; 78 | readSetParams(in_file, options.n_arr_cams, options.arr_cams); 79 | 80 | 81 | /* MAXIMUM PARALLAX */ 82 | std::cout << "Maximum parallax:\n"; 83 | readSetParams(in_file, options.n_parallax, options.max_parallax); 84 | 85 | /* FOCAL LENGTH */ 86 | std::cout << "Focal length:\n"; 87 | readSetParams(in_file, options.n_focal, options.focal_length); 88 | 89 | /* NOISE */ 90 | std::cout << "Noise:\n"; 91 | readSetParams(in_file, options.n_noise, options.noise); 92 | 93 | /* NUMBER OF CORRESPONDENCES */ 94 | std::cout << "Distance to center:\n"; 95 | readSetParams(in_file, options.n_dist_centers, options.dist_centers); 96 | 97 | 98 | /* TRANSLATION */ 99 | // discard first line (comment) 100 | getline(in_file, line_i); 101 | // get param 102 | getline(in_file, line_i); 103 | options.use_custom_t = (bool)atof(line_i.c_str()); 104 | std::cout << "Use custom t:\n" << options.use_custom_t << std::endl; 105 | 106 | /* Read translation vector */ 107 | std::cout << "Translation vector:\n"; 108 | int dum_var; 109 | std::vector temp_t; 110 | readSetParams(in_file, dum_var, temp_t); 111 | 112 | for (int i = 0; i < dum_var; i++) 113 | options.custom_t(i) = temp_t[i]; 114 | 115 | 116 | /* ROTATION */ 117 | // discard first line (comment) 118 | getline(in_file, line_i); 119 | // get param 120 | getline(in_file, line_i); 121 | options.max_rotation = (double)atof(line_i.c_str()); 122 | std::cout << "Max rotation:\n" << options.max_rotation << std::endl; 123 | 124 | /* TRANSLATION SELECTION */ 125 | // discard first line (comment) 126 | getline(in_file, line_i); 127 | // get param 128 | getline(in_file, line_i); 129 | options.method_trans = (int)atof(line_i.c_str()); 130 | std::cout << "Pose generation:\n" << options.method_trans << std::endl; 131 | 132 | 133 | /* Perturbation angle for rotation */ 134 | // discard first line (comment) 135 | getline(in_file, line_i); 136 | // get param 137 | getline(in_file, line_i); 138 | options.noise_rot = (double)atof(line_i.c_str()); 139 | std::cout << "Noise rotation:\n" << options.noise_rot << std::endl; 140 | 141 | /* Perturbation angle for translation */ 142 | // discard first line (comment) 143 | getline(in_file, line_i); 144 | // get param 145 | getline(in_file, line_i); 146 | options.noise_trans = (double)atof(line_i.c_str()); 147 | std::cout << "Translation rotation:\n" << options.noise_trans << std::endl; 148 | 149 | 150 | // Close the file 151 | in_file.close(); 152 | 153 | 154 | return true; 155 | } 156 | 157 | 158 | -------------------------------------------------------------------------------- /tests/experimentsHelper.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include // for the file 10 | 11 | #include 12 | 13 | using namespace std; 14 | 15 | 16 | struct SceneOptions { 17 | double max_iter = 100; // # Number of iterations 18 | 19 | int n_points = 10; 20 | 21 | int n_arr_cams = 1; // number of elements in FoV 22 | vector arr_cams = {10}; // # FoV 23 | 24 | int n_parallax = 1; 25 | vector max_parallax = {2}; // # max parallax 26 | 27 | int n_dist_centers = 1; 28 | vector dist_centers = {4.0}; // # max parallax 29 | 30 | int n_focal = 1; 31 | vector focal_length = {1024}; 32 | 33 | int n_noise = 1; 34 | vector noise = {0.5}; 35 | 36 | bool use_custom_t = false; 37 | 38 | Eigen::Matrix custom_t; 39 | 40 | double max_rotation = 0.5; 41 | 42 | int method_trans = 1; 43 | 44 | double noise_rot = 0.01; 45 | 46 | double noise_trans = 0.01; 47 | }; 48 | 49 | 50 | template 51 | void readSetParams(ifstream & in_file, int & N, vector & arr); 52 | 53 | 54 | 55 | 56 | bool readOptionsFromFile(string name_file, 57 | SceneOptions & options); 58 | 59 | 60 | 61 | 62 | -------------------------------------------------------------------------------- /tests/generic_test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include // timer 6 | #include 7 | #include // for the file 8 | 9 | // include helper 10 | #include "experimentsHelper.h" 11 | 12 | // include types 13 | #include "NViewsTypes.h" 14 | // include utils 15 | #include "NViewsUtils.h" 16 | // triangulation solver 17 | #include "NViewsClass.h" 18 | 19 | // data generation 20 | #include "../utils/generatePointCloud.h" 21 | // ceres solver 22 | #include "../utils/ceresSolver.h" 23 | 24 | // for eigen decomposition 25 | #include 26 | #include 27 | 28 | #include // timer 29 | 30 | using namespace NViewsTrian; 31 | using namespace std::chrono; 32 | 33 | #define SAVEPROB TRUE 34 | 35 | enum class methodGenPose{ 36 | ORBITAL = 0, 37 | LATERAL, 38 | GENERAL 39 | }; // end of enum class 40 | 41 | Eigen::Vector3d returnThisTranslation( double max_parallax, const Eigen::Vector3d & dir_parallax) 42 | { 43 | return (dir_parallax); 44 | 45 | }; 46 | 47 | int main(int argc, char** argv) 48 | { 49 | std::cout << "Generic test:\nN views triangulation comparison\n"; 50 | 51 | 52 | 53 | /* Read params from input */ 54 | 55 | string name_in_params = "basic_params.txt"; 56 | 57 | /* Read the name of the file */ 58 | if (argc > 1) 59 | name_in_params = argv[1]; 60 | 61 | 62 | SceneOptions options; 63 | 64 | std::cout << "Generic test file !\n"; 65 | std::cout << "Input for the test: " << name_in_params << std::endl; 66 | 67 | 68 | // read params from file 69 | bool valid_options = readOptionsFromFile(name_in_params, options); 70 | 71 | 72 | std::srand(std::time(nullptr)); 73 | 74 | int n_points = options.n_points; 75 | 76 | int size_img_i = 512; 77 | 78 | for (size_t noise_id=0; noise_id < options.n_noise; noise_id++) 79 | { 80 | double noise_i = options.noise[noise_id]; 81 | 82 | for (size_t cam_id = 0; cam_id < options.n_arr_cams; cam_id++) 83 | { 84 | int M_cam_i = options.arr_cams[cam_id]; 85 | 86 | 87 | for (size_t par_id = 0; par_id < options.n_parallax; par_id++) 88 | { 89 | double par_i = options.max_parallax[par_id]; 90 | 91 | 92 | for (size_t focal_id = 0; focal_id < options.n_focal; focal_id++) 93 | { 94 | double focal_i = options.focal_length[focal_id]; 95 | 96 | for (size_t d_c_id = 0; d_c_id < options.n_dist_centers; d_c_id++) 97 | { 98 | double dist_center_i = options.dist_centers[d_c_id]; 99 | 100 | // This file saves all our resutls 101 | auto name_f_sol = "res/sol_noise_" + std::to_string(noise_i) 102 | + "_cams_" + std::to_string((int)M_cam_i) 103 | + "_par_" + std::to_string(par_i) 104 | + "_focal_" + std::to_string((int)focal_i) 105 | + "_center_" + std::to_string(dist_center_i) 106 | + ".txt"; 107 | std::ofstream fsol(name_f_sol); 108 | 109 | auto name_f_lin_3d = "res/lin_3D_noise_" + std::to_string(noise_i) 110 | + "_cams_" + std::to_string((int)M_cam_i) 111 | + "_par_" + std::to_string(par_i) 112 | + "_focal_" + std::to_string((int)focal_i) 113 | + "_center_" + std::to_string(dist_center_i) 114 | + ".txt"; 115 | std::ofstream flin3d(name_f_lin_3d); 116 | 117 | auto name_f_3d = "res/err_3D_noise_" + std::to_string(noise_i) 118 | + "_cams_" + std::to_string((int)M_cam_i) 119 | + "_par_" + std::to_string(par_i) 120 | + "_focal_" + std::to_string((int)focal_i) 121 | + "_center_" + std::to_string(dist_center_i) 122 | + ".txt"; 123 | std::ofstream f3d(name_f_3d); 124 | 125 | auto name_f_l2 = "res/err_l2_noise_" + std::to_string(noise_i) 126 | + "_cams_" + std::to_string((int)M_cam_i) 127 | + "_par_" + std::to_string(par_i) 128 | + "_focal_" + std::to_string((int)focal_i) 129 | + "_center_" + std::to_string(dist_center_i) 130 | + ".txt"; 131 | std::ofstream fl2(name_f_l2); 132 | 133 | 134 | 135 | auto name_f_l1 = "res/err_l1_noise_" + std::to_string(noise_i) 136 | + "_cams_" + std::to_string((int)M_cam_i) 137 | + "_par_" + std::to_string(par_i) 138 | + "_focal_" + std::to_string((int)focal_i) 139 | + "_center_" + std::to_string(dist_center_i) 140 | + ".txt"; 141 | std::ofstream fl1(name_f_l1); 142 | 143 | auto name_f_linfty = "res/err_linfty_noise_" + std::to_string(noise_i) 144 | + "_cams_" + std::to_string((int)M_cam_i) 145 | + "_par_" + std::to_string(par_i) 146 | + "_focal_" + std::to_string((int)focal_i) 147 | + "_center_" + std::to_string(dist_center_i) 148 | + ".txt"; 149 | std::ofstream flinfty(name_f_linfty); 150 | 151 | auto name_f_times = "res/times_noise_" + std::to_string(noise_i) 152 | + "_cams_" + std::to_string((int)M_cam_i) 153 | + "_par_" + std::to_string(par_i) 154 | + "_focal_" + std::to_string((int)focal_i) 155 | + "_center_" + std::to_string(dist_center_i) 156 | + ".txt"; 157 | std::ofstream ftime(name_f_times); 158 | 159 | auto name_f_exp = "res/times_exp_noise_" + std::to_string(noise_i) 160 | + "_cams_" + std::to_string((int)M_cam_i) 161 | + "_par_" + std::to_string(par_i) 162 | + "_focal_" + std::to_string((int)focal_i) 163 | + "_center_" + std::to_string(dist_center_i) 164 | + ".txt"; 165 | std::ofstream ft_exp(name_f_exp); 166 | 167 | auto name_diff = "res/diff_noise_" + std::to_string(noise_i) 168 | + "_cams_" + std::to_string((int)M_cam_i) 169 | + "_par_" + std::to_string(par_i) 170 | + "_focal_" + std::to_string((int)focal_i) 171 | + "_center_" + std::to_string(dist_center_i) 172 | + ".txt"; 173 | std::ofstream fdiff(name_diff); 174 | 175 | 176 | #if SAVEPROB 177 | { 178 | auto name_prob = "res/prob_" + std::to_string(noise_i) 179 | + "_cams_" + std::to_string((int)M_cam_i) 180 | + "_par_" + std::to_string(par_i) 181 | + "_focal_" + std::to_string((int)focal_i) 182 | + "_center_" + std::to_string(dist_center_i) 183 | + ".txt"; 184 | std::ofstream fprob(name_prob); 185 | } 186 | #endif 187 | auto name_constr = "res/constr_" + std::to_string(noise_i) 188 | + "_cams_" + std::to_string((int)M_cam_i) 189 | + "_par_" + std::to_string(par_i) 190 | + "_focal_" + std::to_string((int)focal_i) 191 | + "_center_" + std::to_string(dist_center_i) 192 | + ".txt"; 193 | std::ofstream fconstr(name_constr); 194 | 195 | 196 | 197 | for (size_t n_iter = 0; n_iter < options.max_iter; n_iter++) 198 | { 199 | 200 | // define struct with params 201 | PCParams str_in = PCParams(); 202 | 203 | str_in.focal_length = focal_i; 204 | str_in.N_points = n_points; 205 | str_in.noise = noise_i; 206 | str_in.size_img = size_img_i; 207 | str_in.max_side = 8.0; 208 | str_in.std_pc = (double) dist_center_i / 4.0; 209 | str_in.dist_center = dist_center_i; 210 | str_in.max_parallax = par_i; 211 | str_in.max_angle = options.max_rotation; 212 | str_in.M_cameras = M_cam_i; 213 | 214 | // Select pose generation 215 | // param: options.method_trans in {1, 2, 3} 216 | methodGenPose m_gen_pose = static_cast(options.method_trans); 217 | 218 | 219 | // generate problem 220 | PCRes str_out = PCRes(); 221 | std::cout << "Selection method for pose generation\n"; 222 | 223 | switch (m_gen_pose) 224 | { 225 | case methodGenPose::ORBITAL: 226 | { 227 | std::cout << "[ORBITAL CAMERA]\n"; 228 | str_in.max_parallax = 5; // radius circle 229 | 230 | Eigen::Vector3d dist_vector; 231 | dist_vector << dist_center_i, 0, 0; 232 | str_in.dir_parallax = dist_vector; 233 | 234 | str_in.max_angle = dist_center_i; 235 | // generateOrbitalRotation takes the distance to center and the translation vector 236 | 237 | // In this configuration, rotation and translation depends on dist_center_i 238 | // and radius = max_parallax = 5 units 239 | str_in.noise_rot = options.noise_rot; 240 | str_in.noise_trans = options.noise_trans; 241 | 242 | str_out = generatePointCloud(str_in, generateOrbitalTranslation, generateOrbitalRotation); 243 | } 244 | break; 245 | 246 | case methodGenPose::LATERAL: 247 | std::cout << "[LATERAL CAMERA]\n"; 248 | // In this configuration, 249 | // translation has form X, 0 0 250 | // and rotation is identity 251 | 252 | str_in.max_angle = 0.0; 253 | str_in.max_parallax = par_i; 254 | str_in.noise_trans = options.noise_trans; 255 | str_in.noise_rot = options.noise_rot; 256 | str_out = generatePointCloud(str_in, generateTranslationStereo); 257 | break; 258 | 259 | /* 260 | case methodGenPose::GENERAL: 261 | std::cout << "[GENERAL CAMERA]\n"; 262 | str_out = generatePointCloud(str_in); 263 | break; 264 | */ 265 | 266 | default: 267 | std::cout << "[GENERAL CAMERA]\n"; 268 | str_in.max_angle = options.max_rotation; 269 | str_in.max_parallax = par_i; 270 | str_in.noise_trans = options.noise_trans; 271 | str_in.noise_rot = options.noise_rot; 272 | str_out = generatePointCloud(str_in); 273 | break; 274 | 275 | } 276 | 277 | Matrix3 K = Matrix3::Identity(); 278 | K(0,0) = focal_i; 279 | K(1,1) = focal_i; 280 | K(0,2) = (double) size_img_i; 281 | K(1,2) = (double) size_img_i; 282 | Matrix3 iK = K.inverse(); 283 | 284 | 285 | // extract data 286 | // generate full graph (M 2) combinations 287 | 288 | Eigen::MatrixXd idx_matrix; 289 | int n_comb = generateM2Comb(M_cam_i, idx_matrix); 290 | 291 | 292 | // generate correspondences 293 | std::vector set_corr; 294 | set_corr.empty(); 295 | for (int jj=0; jj(0,0) = R1; 307 | P1.block<3,1>(0,3) = t1; 308 | P2.block<3,3>(0,0) = R2; 309 | P2.block<3,1>(0,3) = t2; 310 | 311 | Eigen::Matrix4d Prel = P2 * P1.inverse(); 312 | Eigen::Matrix3d Rrel = Prel.block<3,3>(0,0); 313 | Eigen::Vector3d trel = Prel.block<3,1>(0,3); 314 | trel.normalize(); 315 | 316 | 317 | 318 | Eigen::Matrix3d Ess = Eigen::Matrix3d::Identity(); 319 | Eigen::Matrix3d Tx = Eigen::Matrix3d::Zero(); 320 | Tx << 0, -trel(2), trel(1), trel(2), 0, -trel(0), -trel(1), trel(0), 0; 321 | // fill T 322 | Ess = Tx * Rrel; 323 | corr_i.id1 = id1; 324 | corr_i.id2 = id2; 325 | corr_i.F = Ess; 326 | set_corr.push_back(corr_i); 327 | } 328 | 329 | // Save pose 330 | #if SAVEPROB 331 | { 332 | for (int jj=0; jj < M_cam_i; jj++) 333 | { 334 | Eigen::Matrix3d R = str_out.set_rot[jj]; 335 | Eigen::Vector3d t = str_out.set_trans[jj]; 336 | fprob << t(0) << "," << t(1) << "," << t(2) << ","; 337 | 338 | for (int id_r = 0; id_r < 3; id_r++) 339 | { 340 | for (int id_c=0; id_c < 3; id_c++) 341 | fprob << R(id_r, id_c) << ","; 342 | } 343 | fprob << std::endl; 344 | } 345 | } 346 | #endif 347 | 348 | // for each point 349 | int idx = 0; 350 | for (idx = 0; idx < n_points; idx++) 351 | { 352 | 353 | // 1. Loop through cameras 354 | for (int jj=0; jj(high_resolution_clock::now() - start_t_ours); 378 | 379 | 380 | 381 | // c. Reconstruct 3D point by linear method 382 | 383 | // Matrix projections 384 | std::vector proj_s; 385 | proj_s.empty(); 386 | std::vector obs_s, obs_init, obs_ref; 387 | obs_s.empty(); 388 | obs_init.empty(); 389 | obs_ref.empty(); 390 | 391 | for (int jc=0; jc(0,0) = R; 398 | P1.block<3,1>(0,3) = t; 399 | proj_s.push_back(P1); 400 | 401 | // observations 402 | 403 | Eigen::Vector3d pt = iK * str_out.obs[idx].col(jc); 404 | obs_s.push_back(pt); 405 | 406 | // update observation init 407 | Eigen::Vector3d delta_init; 408 | delta_init << res_corr.sol_init( jc*2), res_corr.sol_init(jc*2 + 1), 0; 409 | obs_init.push_back(pt + delta_init); 410 | 411 | // update observation refinenement 412 | Eigen::Vector3d delta_ref; 413 | delta_ref << res_corr.sol_final( jc*2), res_corr.sol_final(jc*2 + 1), 0; 414 | obs_ref.push_back(pt + delta_ref); 415 | } 416 | 417 | 418 | // triangulate point 419 | Eigen::Vector3d P_lin; 420 | Eigen::VectorXd depths_lin; 421 | double error_lin = triangulateNPoint(proj_s, obs_s, P_lin, depths_lin); 422 | 423 | Eigen::Vector3d P_init; 424 | Eigen::VectorXd depths_init; 425 | double error_init = triangulateNPoint(proj_s, obs_init, P_init, depths_init); 426 | 427 | Eigen::Vector3d P_ref; 428 | Eigen::VectorXd depths_ref; 429 | double error_ref = triangulateNPoint(proj_s, obs_ref, P_ref, depths_ref); 430 | 431 | 432 | /** Run ceres **/ 433 | std::vector, Eigen::Vector2d>> vector_pair_ceres_data; 434 | vector_pair_ceres_data.reserve(M_cam_i); 435 | 436 | for (int jc=0; jc P1; 442 | P1.setZero(); 443 | P1.block<3,3>(0,0) = R; 444 | P1.block<3,1>(0,3) = t; 445 | 446 | // observations 447 | Eigen::Vector3d pt = obs_ref[jc]; 448 | 449 | std::pair,Eigen::Vector2d> pair_i(P1, pt.topRows(2)); 450 | vector_pair_ceres_data.push_back(pair_i); 451 | } 452 | 453 | /** Run ceres **/ 454 | Eigen::Vector3d P_ceres = CeresSolver::Triangulate(vector_pair_ceres_data, P_ref); 455 | 456 | Eigen::VectorXd sol_ceres = res_corr.sol_init; 457 | for (int jc=0; jc P1; 462 | P1.setZero(); 463 | P1.block<3,3>(0,0) = R; 464 | P1.block<3,1>(0,3) = t; 465 | 466 | Eigen::Vector4d P_ceres_h; 467 | P_ceres_h << P_ceres, 1; 468 | 469 | Eigen::Vector3d obs_ceres_i = P1 * P_ceres_h; 470 | obs_ceres_i /= obs_ceres_i(2); 471 | sol_ceres.block<2,1>(jc * 2,0) = (obs_ceres_i - iK * str_out.obs[idx].col(jc)).topRows(2); 472 | 473 | } 474 | 475 | /* Save results */ 476 | // solution of problem 477 | fsol << P_lin(0) << "," << P_lin(1) << "," << P_lin(2) << "," ; 478 | fsol << P_init(0) << "," << P_init(1) << "," << P_init(2) << "," ; 479 | fsol << P_ref(0) << "," << P_ref(1) << "," << P_ref(2) << "," ; 480 | fsol << P_ceres(0) << "," << P_ceres(1) << "," << P_ceres(2) << "," ; 481 | fsol << std::endl; 482 | 483 | // Euclidean distance 3D point 484 | f3d << (P_ceres-P_lin).norm() << ","; 485 | f3d << (P_ceres-P_init).norm() << ","; 486 | f3d << (P_ceres-P_ref).norm(); 487 | f3d << std::endl; 488 | 489 | // Error for the linear method 490 | flin3d << error_lin << ","; 491 | flin3d << error_init << ","; 492 | flin3d << error_ref << ","; 493 | flin3d << std::endl; 494 | 495 | // l2 norm for observations 496 | fl2 << res_corr.sol_init.squaredNorm() << ","; 497 | fl2 << res_corr.sol_final.squaredNorm() << ","; 498 | fl2 << sol_ceres.squaredNorm() << ","; 499 | fl2 << std::endl; 500 | 501 | // L1 norm for observations 502 | fl1 << res_corr.sol_init.lpNorm<1>() << ","; 503 | fl1 << res_corr.sol_final.lpNorm<1>()<< ","; 504 | fl1 << sol_ceres.lpNorm<1>()<< ","; 505 | fl1 << std::endl; 506 | 507 | // Linfty norm for observations 508 | flinfty << res_corr.sol_init.lpNorm() << ","; 509 | flinfty << res_corr.sol_final.lpNorm() << ","; 510 | flinfty << sol_ceres.lpNorm() << ","; 511 | flinfty << std::endl; 512 | 513 | 514 | ftime << res_corr.time_init << ","; 515 | ftime << (double) time_ours.count() << ","; 516 | ftime << M_cam_i << ","; 517 | ftime << std::endl; 518 | 519 | ft_exp << res_corr.time_init << ","; 520 | ft_exp << res_corr.time_ref << ","; 521 | ft_exp << res_corr.n_iters << ","; 522 | ft_exp << res_corr.time_opt << ","; 523 | ft_exp << res_corr.time_cert_mult << ","; 524 | ft_exp << res_corr.time_cert_hess << ","; 525 | ft_exp << res_corr.min_eig << ","; 526 | ft_exp << std::endl; 527 | 528 | 529 | /* Save diff wrt HS */ 530 | fdiff << (res_corr.sol_init - sol_ceres).norm()<< ","; 531 | fdiff << (res_corr.sol_final - sol_ceres).norm()<< ","; 532 | fdiff << std::endl; 533 | 534 | 535 | /* epipolar constraint */ 536 | 537 | // compute and save epipolar error 538 | fconstr << res_corr.max_constr_init << ","; 539 | fconstr << res_corr.max_constr << ","; 540 | fconstr << res_corr.tot_constr_init << ","; 541 | fconstr << res_corr.tot_constr << ","; 542 | fconstr << res_corr.sq_constr_init << ","; 543 | fconstr << res_corr.sq_constr << ","; 544 | fconstr << res_corr.error_lin << ","; 545 | fconstr << std::endl; 546 | 547 | 548 | 549 | } // end of each point 550 | } // end of each iteration 551 | fsol.close(); // solution 552 | flin3d.close(); // error linear method 553 | f3d.close(); // error 3d point 554 | fl2.close(); // error l2 obs 555 | fl1.close(); // error l1 obs 556 | flinfty.close(); // error linfty obs 557 | ftime.close(); // time 558 | ft_exp.close(); // time dual diagonal 559 | fdiff.close(); // diff wrt HS 560 | #if SAVEPROB 561 | fprob.close(); // close the problem file if opened 562 | #endif 563 | fconstr.close(); // epipolar constraint 564 | 565 | } // end for dist center 566 | 567 | } // enf for focal 568 | 569 | } // end for parallax 570 | 571 | } // end for M_cameras 572 | 573 | } // end for noise 574 | 575 | return 0; 576 | 577 | } // end of main fcn 578 | -------------------------------------------------------------------------------- /utils/ceresSolver.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | 9 | #include "ceresSolver.h" 10 | 11 | #include "ceres/ceres.h" 12 | 13 | 14 | using namespace std; 15 | 16 | 17 | namespace CeresSolver 18 | { 19 | 20 | 21 | class ReprojectionError { 22 | public: 23 | ReprojectionError( 24 | const Eigen::Matrix& projection_matrix, 25 | const Eigen::Vector2d& feature) 26 | : projection_matrix_(projection_matrix), feature_(feature) {} 27 | 28 | template 29 | bool operator()(const T* input_point, T* reprojection_error) const { 30 | Eigen::Map > point(input_point); 31 | 32 | // Multiply the point with the projection matrix, then perform homogeneous 33 | // normalization to obtain the 2D pixel location of the reprojection. 34 | 35 | const Eigen::Matrix reprojected_pixel = (projection_matrix_ * point).hnormalized(); 36 | // Reprojection error is the distance from the reprojection to the observed 37 | // feature location. 38 | reprojection_error[0] = feature_[0] - reprojected_pixel[0]; 39 | reprojection_error[1] = feature_[1] - reprojected_pixel[1]; 40 | 41 | return true; 42 | } 43 | static ceres::CostFunction * Create(const Eigen::Matrix& projection_matrix_, 44 | const Eigen::Vector2d& feature_) { 45 | return (new ceres::AutoDiffCostFunction 46 | (new ReprojectionError(projection_matrix_,feature_))); 47 | } 48 | 49 | private: 50 | const Eigen::Matrix& projection_matrix_; 51 | const Eigen::Vector2d& feature_; 52 | }; 53 | 54 | 55 | Eigen::Vector3d Triangulate(std::vector,Eigen::Vector2d>>& datas, 57 | Eigen::Vector3d & init_x) { 58 | Eigen::Vector4d x; 59 | x << init_x,1; 60 | ceres::Problem problem; 61 | ceres::Solver::Options options; 62 | ceres::LossFunction* loss= nullptr; 63 | ceres::Solver::Summary summary; 64 | 65 | for(auto & data:datas) { 66 | problem.AddResidualBlock(ReprojectionError::Create(data.first,data.second),loss,&x[0]); 67 | } 68 | ceres::Solve(options,&problem, &summary); 69 | return x.hnormalized(); 70 | } 71 | 72 | 73 | } // end of namespace ceres 74 | -------------------------------------------------------------------------------- /utils/ceresSolver.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | #include "ceres/ceres.h" 9 | 10 | 11 | namespace CeresSolver 12 | { 13 | 14 | Eigen::Vector3d Triangulate(std::vector,Eigen::Vector2d>>& datas, 16 | Eigen::Vector3d & init_x) ; 17 | 18 | 19 | }; // end of namespace ceresSolver 20 | 21 | -------------------------------------------------------------------------------- /utils/generatePointCloud.cpp: -------------------------------------------------------------------------------- 1 | #include "generatePointCloud.h" 2 | 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #define PI 3.1415927 13 | 14 | namespace NViewsTrian 15 | { 16 | 17 | 18 | Vector3 addNoise(double noise, double focal, size_t size_img, Vector3& obs); 19 | 20 | 21 | PCRes generatePointCloud(PCParams & options, 22 | const GenerateTranslation& generateTranslation, 23 | const GenerateRotation& generateRotation, 24 | const PerturbTranslation& perturbTranslation, 25 | const PerturbRotation& perturbRotation) 26 | { 27 | 28 | // std::cout << "[PC] Creating output struct\n"; 29 | PCRes res = PCRes(options.N_points); 30 | 31 | /** 1. GENERATE POINT CLOUD **/ 32 | 33 | res.points_3D.setZero(); 34 | 35 | for (int i = 0; i < options.N_points; i++) 36 | { 37 | bool coord_ok = false; 38 | double px = options.max_side, py = options.max_side, pz = options.max_side; 39 | int n_iter = 0; 40 | 41 | /* For X component */ 42 | while ((!coord_ok) && (n_iter < options.max_iter)) 43 | { 44 | px = ((((double) rand()) / ((double) RAND_MAX)) - 0.5) * options.std_pc * 2.0; 45 | 46 | if ((px <= options.max_side * 0.5) && (-options.max_side * 0.5 <= px)) 47 | { 48 | // save data 49 | coord_ok = true; 50 | } 51 | n_iter++; 52 | } 53 | // Check return 54 | if ((!coord_ok) && (n_iter >= options.max_iter)) 55 | { 56 | px = options.max_side; 57 | } 58 | 59 | /* For Y component */ 60 | // reset flags 61 | n_iter = 0; 62 | coord_ok = false; 63 | while ((!coord_ok) && (n_iter < options.max_iter)) 64 | { 65 | py = ((((double) rand()) / ((double) RAND_MAX)) - 0.5) * options.std_pc * 2.0; 66 | 67 | if ((py <= options.max_side * 0.5) && (-options.max_side * 0.5 <= py)) 68 | { 69 | // save data 70 | coord_ok = true; 71 | } 72 | n_iter++; 73 | } 74 | 75 | // Check return 76 | if ((!coord_ok) && (n_iter >= options.max_iter)) 77 | { 78 | py = options.max_side; 79 | } 80 | 81 | /* For Z component */ 82 | // reset flags 83 | n_iter = 0; 84 | coord_ok = false; 85 | while ((!coord_ok) && (n_iter < options.max_iter)) 86 | { 87 | pz = (((double) rand()) / ((double) RAND_MAX) - 0.5) * 2.0 * options.std_pc + options.dist_center; 88 | 89 | if (pz >= 0) 90 | { 91 | // save data 92 | coord_ok = true; 93 | } 94 | n_iter++; 95 | } 96 | // Check return 97 | if ((!coord_ok) && (n_iter >= options.max_iter)) 98 | { 99 | px = 0; 100 | } 101 | 102 | // save point 103 | res.points_3D.col(i) << px, py, pz; 104 | 105 | } // end: for each point 106 | 107 | 108 | // Previous pose 109 | res.set_rot.empty(); 110 | res.set_trans.empty(); 111 | 112 | for (int j=0; j < options.M_cameras; j++) 113 | { 114 | /** 2. GENERATE POSE **/ 115 | // a. Generate relative pose 116 | 117 | Vector3 translation = generateTranslation(options.max_parallax, options.dir_parallax); 118 | 119 | Matrix3 rotation = generateRotation(options.max_angle, options.dir_rotation, translation); 120 | 121 | // b. Perturb pose 122 | translation = perturbTranslation(options.noise_trans, translation); 123 | 124 | rotation = perturbRotation(options.noise_rot, rotation); 125 | 126 | 127 | res.set_rot.push_back(rotation); 128 | res.set_trans.push_back(translation); 129 | 130 | } 131 | 132 | 133 | 134 | /** 3. GENERATE CORRESPONDENCES **/ 135 | res.obs.empty(); 136 | 137 | 138 | for (int i=0; i < options.N_points; i++) 139 | { 140 | Vector3 p_3d = res.points_3D.col(i); 141 | Eigen::MatrixXd obs_i(3, options.M_cameras); 142 | obs_i.setZero(); 143 | for (int j=0; j < options.M_cameras; j++) 144 | { 145 | 146 | Matrix3 rot_j = res.set_rot[j]; 147 | Vector3 trans_j = res.set_trans[j]; 148 | Vector3 qj = rot_j * p_3d + trans_j; 149 | 150 | 151 | // add noise 152 | Vector3 obs_noisy = addNoise(options.noise, options.focal_length, options.size_img, qj); 153 | 154 | 155 | // save data 156 | obs_i.col(j) = obs_noisy; 157 | } 158 | // save matrix in array 159 | res.obs.push_back(obs_i); 160 | } 161 | 162 | 163 | return res; 164 | } // end of function 165 | 166 | 167 | 168 | Vector3 addNoise(double noise, double focal, size_t size_img, Vector3& obs) 169 | { 170 | Vector3 noisy_obs = obs; 171 | 172 | noisy_obs(0) /= noisy_obs(2); 173 | noisy_obs(1) /= noisy_obs(2); 174 | noisy_obs(2) = 1; 175 | 176 | Matrix3 K = Matrix3::Identity(); 177 | K(0,0) = focal; 178 | K(1,1) = focal; 179 | K(0,2) = (double)size_img; 180 | K(1,2) = (double)size_img; 181 | Vector3 obs_no_noise = K * noisy_obs; 182 | noisy_obs = obs_no_noise; 183 | 184 | noisy_obs(0) += (((double) rand()) / ((double) RAND_MAX)-0.5)*2.0 * noise; 185 | noisy_obs(1) += (((double) rand()) / ((double) RAND_MAX)-0.5)*2.0 * noise; 186 | 187 | 188 | return noisy_obs; 189 | } 190 | 191 | 192 | Vector3 generateRandomVector(double std_vec) 193 | { 194 | Vector3 v; 195 | v.setZero(); 196 | v(0) = (((double) rand()) / ((double) RAND_MAX)-0.5)*2.0 * std_vec; 197 | v(1) = (((double) rand()) / ((double) RAND_MAX)-0.5)*2.0 * std_vec; 198 | v(2) = (((double) rand()) / ((double) RAND_MAX)-0.5)*2.0 * std_vec; 199 | v.normalize(); 200 | 201 | return v; 202 | } 203 | 204 | Matrix3 generateRotationRodrigues(double max_angle, const Vector3 & dir_r) 205 | { 206 | double angle = max_angle * (((double) rand())/ ((double) RAND_MAX)-0.5)*2.0; 207 | 208 | Matrix3 rot; 209 | Vector3 dir_rot = dir_r.normalized(); 210 | Matrix3 T; 211 | // cross matrix 212 | T << 0, -dir_rot(2), dir_rot(1), 213 | dir_rot(2), 0, -dir_rot(0), 214 | -dir_rot(1), dir_rot(0), 0; 215 | 216 | rot = Matrix3::Identity() + sin(angle) * T + (1 - cos(angle)) * T * T; 217 | 218 | return rot; 219 | } 220 | 221 | 222 | // generate random translation with the specified norm 223 | Vector3 generateRandomTranslationDefault( double max_parallax, const Vector3 & dir_parallax) 224 | { 225 | double par_rnd = (((double) rand()) / ((double) RAND_MAX)-0.5)*2.0 * max_parallax; 226 | return (par_rnd * generateRandomVector(1.0)); 227 | 228 | } 229 | Matrix3 generateRandomRotation( double maxAngle ) 230 | { 231 | 232 | Vector3 rpy; 233 | rpy[0] = ((double) std::rand())/ ((double) RAND_MAX); 234 | rpy[1] = ((double) std::rand())/ ((double) RAND_MAX); 235 | rpy[2] = ((double) std::rand())/ ((double) RAND_MAX); 236 | 237 | rpy[0] = maxAngle*2.0*(rpy[0]-0.5); 238 | rpy[1] = maxAngle*2.0*(rpy[1]-0.5); 239 | rpy[2] = maxAngle*2.0*(rpy[2]-0.5); 240 | 241 | Matrix3 R1; 242 | R1(0,0) = 1.0; 243 | R1(0,1) = 0.0; 244 | R1(0,2) = 0.0; 245 | R1(1,0) = 0.0; 246 | R1(1,1) = cos(rpy[0]); 247 | R1(1,2) = -sin(rpy[0]); 248 | R1(2,0) = 0.0; 249 | R1(2,1) = -R1(1,2); 250 | R1(2,2) = R1(1,1); 251 | 252 | Matrix3 R2; 253 | R2(0,0) = cos(rpy[1]); 254 | R2(0,1) = 0.0; 255 | R2(0,2) = sin(rpy[1]); 256 | R2(1,0) = 0.0; 257 | R2(1,1) = 1.0; 258 | R2(1,2) = 0.0; 259 | R2(2,0) = -R2(0,2); 260 | R2(2,1) = 0.0; 261 | R2(2,2) = R2(0,0); 262 | 263 | Matrix3 R3; 264 | R3(0,0) = cos(rpy[2]); 265 | R3(0,1) = -sin(rpy[2]); 266 | R3(0,2) = 0.0; 267 | R3(1,0) =-R3(0,1); 268 | R3(1,1) = R3(0,0); 269 | R3(1,2) = 0.0; 270 | R3(2,0) = 0.0; 271 | R3(2,1) = 0.0; 272 | R3(2,2) = 1.0; 273 | 274 | Matrix3 rotation = R3 * R2 * R1; 275 | 276 | rotation.col(0) = rotation.col(0) / rotation.col(0).norm(); 277 | rotation.col(2) = rotation.col(0).cross(rotation.col(1)); 278 | rotation.col(2) = rotation.col(2) / rotation.col(2).norm(); 279 | rotation.col(1) = rotation.col(2).cross(rotation.col(0)); 280 | rotation.col(1) = rotation.col(1) / rotation.col(1).norm(); 281 | return rotation; 282 | } 283 | 284 | 285 | 286 | // generate random rotation with maxAngle 287 | Matrix3 generateRandomRotationDefault( double max_angle, const Vector3 & dir_rot, const Vector3 & trans) 288 | { 289 | 290 | 291 | return (generateRandomRotation( max_angle )); 292 | 293 | } 294 | 295 | // generate orbital rotation with maxAngle 296 | Matrix3 generateOrbitalRotation( double d, const Vector3 & dir_rot, const Vector3 & trans) 297 | { 298 | // here: 299 | // d: distance to center point cloud 300 | // trans: translation vector 301 | // compute angle 302 | Vector3 d_c; 303 | d_c << 0, 0, d; 304 | 305 | Vector3 r = trans - d_c ; 306 | 307 | double theta = atan2(r(2), r(0)) + PI * 0.5; 308 | 309 | // Construct matrix 310 | Matrix3 R = Matrix3::Identity(); 311 | R(0, 0) = cos(theta); 312 | R(0, 2) = sin(theta); 313 | R(2, 0) = - sin(theta); 314 | R(2, 2) = cos(theta); 315 | 316 | return (R); 317 | 318 | } 319 | 320 | Vector3 generateOrbitalTranslation( double max_parallax, const Vector3 & dir_parallax) 321 | { 322 | // here max parallax is the radius of the circle 323 | double angle_in_circle = (((double) rand()) / ((double) RAND_MAX)-0.5)* 2 * PI; 324 | 325 | double x_c = cos(angle_in_circle) * max_parallax; 326 | double z_c = sin(angle_in_circle) * max_parallax; 327 | Vector3 trans; 328 | trans << x_c, 0, dir_parallax(0) + z_c; 329 | 330 | return (trans); 331 | 332 | }; 333 | 334 | 335 | // generate random perturbation for translation 336 | Vector3 perturbRandomTranslationDefault( double noise, const Vector3 & trans) 337 | { 338 | double n_trans = trans.norm(); 339 | 340 | Vector3 res = trans; 341 | // perturbation 342 | res(0) += (((double) rand()) / ((double) RAND_MAX)-0.5)*2.0 * noise; 343 | res(1) += (((double) rand()) / ((double) RAND_MAX)-0.5)*2.0 * noise; 344 | res(2) += (((double) rand()) / ((double) RAND_MAX)-0.5)*2.0 * noise; 345 | 346 | return (n_trans * res.normalized()); 347 | } 348 | // generate random perturbation for translation 349 | Matrix3 perturbRandomRotationDefault( double noise, const Matrix3 & rot) 350 | { 351 | Matrix3 noise_rot = generateRandomRotation(noise); 352 | 353 | return (noise_rot * rot); 354 | } 355 | 356 | 357 | Vector3 generateTranslationForward( double max_parallax, const Vector3 & dir_parallax) 358 | { 359 | Vector3 t; 360 | t << 0, 0, 1; 361 | 362 | return (max_parallax * t); 363 | 364 | }; 365 | 366 | Vector3 generateTranslationStereo( double max_parallax, const Vector3 & dir_parallax) 367 | { 368 | Vector3 t; 369 | 370 | double par_rnd = (((double) rand()) / ((double) RAND_MAX)-0.5)*max_parallax; 371 | t << par_rnd, 0, 0; 372 | 373 | return (t); 374 | 375 | }; 376 | 377 | 378 | Vector3 generateTranslationSideways( double max_parallax, const Vector3 & dir_parallax) 379 | { 380 | Vector3 t; 381 | t(0) = (((double) rand()) / ((double) RAND_MAX)-0.5)*2.0; 382 | t(1) = (((double) rand()) / ((double) RAND_MAX)-0.5)*2.0; 383 | t(2) = 0; 384 | t.normalize(); 385 | 386 | return (max_parallax * t); 387 | 388 | }; 389 | 390 | Vector3 generateTranslationOblique( double max_parallax, const Vector3 & dir_parallax) 391 | { 392 | Vector3 t; 393 | t << 1, 1, 1; 394 | t.normalize(); 395 | 396 | return (max_parallax * t); 397 | 398 | }; 399 | 400 | int generateM2Comb(const int M, 401 | Eigen::MatrixXd & comb_idx) 402 | { 403 | // probably not the best way 404 | int M_comb = M * (M-1) / 2; 405 | comb_idx.resize(2, M_comb); 406 | comb_idx.setZero(); 407 | int col_id = 0; 408 | for (int i=0;i 4 | #include 5 | #include 6 | 7 | namespace NViewsTrian 8 | { 9 | 10 | typedef Eigen::Matrix Vector2; 11 | typedef Eigen::Matrix Vector3; 12 | typedef Eigen::Matrix Matrix3; 13 | typedef Eigen::Matrix Matrix4; 14 | 15 | /* Generate random translation */ 16 | using GenerateTranslation = std::function; 17 | /* Generate random rotation */ 18 | using GenerateRotation = std::function; 19 | /* Generate random perturbation for translation */ 20 | using PerturbTranslation = std::function; 21 | /* Generate random perturbation for rotation */ 22 | using PerturbRotation = std::function; 23 | 24 | // generate random translation with the specified norm 25 | Vector3 generateRandomTranslationDefault( double max_parallax, const Vector3 & dir_parallax); 26 | // generate random rotation with maxAngle 27 | Matrix3 generateRandomRotationDefault( double maxAngle, const Vector3 & dir_rot, const Vector3& dir_trans); 28 | 29 | 30 | // generate random perturbation for translation 31 | Vector3 perturbRandomTranslationDefault( double noise, const Vector3 & trans); 32 | // generate random perturbation for translation 33 | Matrix3 perturbRandomRotationDefault( double noise, const Matrix3 & rot); 34 | 35 | int generateM2Comb(const int M, 36 | Eigen::MatrixXd & comb_idx); 37 | 38 | /* Options for the point cloud generation */ 39 | struct PCParams 40 | { 41 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 42 | 43 | double focal_length = 512; // in pixels 44 | size_t size_img = 1024; 45 | size_t N_points = 1; 46 | double noise = 0.1; // in pixels 47 | int max_iter = 500; 48 | double max_side = 8; 49 | double std_pc = 4.0; 50 | double dist_center = 8.0; 51 | int M_cameras = 2; 52 | 53 | // params for relpose 54 | double max_parallax = 2.0; // in meters 55 | double max_angle = 0.5; // in degrees 56 | 57 | double noise_trans = 0.0; 58 | double noise_rot = 0.0; 59 | 60 | Vector3 dir_parallax; 61 | Vector3 dir_rotation; 62 | 63 | // constructor 64 | PCParams(){}; 65 | 66 | 67 | 68 | }; // end of struct PCParams 69 | 70 | 71 | /* Result of the point cloud generation */ 72 | struct PCRes 73 | { 74 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 75 | 76 | // poses 77 | std::vector set_rot; 78 | std::vector set_trans; 79 | 80 | // Observations 81 | std::vector obs; // noisy observations 82 | // 3D world points 83 | Eigen::MatrixXd points_3D; 84 | 85 | // constructor 86 | /// default 87 | PCRes() 88 | { 89 | points_3D = Eigen::MatrixXd(3, 1); 90 | obs = {}; 91 | set_rot = {}; 92 | set_trans = {}; 93 | }; 94 | 95 | PCRes(size_t n_points) { 96 | points_3D = Eigen::MatrixXd(3, n_points); 97 | obs = {}; 98 | set_rot = {}; 99 | set_trans = {}; 100 | }; 101 | 102 | }; // end of PCRes 103 | 104 | 105 | PCRes generatePointCloud(PCParams & options, 106 | const GenerateTranslation& generateTranslation = generateRandomTranslationDefault, 107 | const GenerateRotation& generateRotation = generateRandomRotationDefault, 108 | const PerturbTranslation& perturbTranslation = perturbRandomTranslationDefault, 109 | const PerturbRotation& perturbRotation = perturbRandomRotationDefault); 110 | 111 | 112 | 113 | 114 | 115 | 116 | /** Some special functions **/ 117 | Vector3 generateTranslationForward( double max_parallax, const Vector3 & dir_parallax); 118 | 119 | Vector3 generateTranslationStereo( double max_parallax, const Vector3 & dir_parallax); 120 | 121 | Vector3 generateTranslationSideways( double max_parallax, const Vector3 & dir_parallax); 122 | 123 | Vector3 generateTranslationOblique( double max_parallax, const Vector3 & dir_parallax); 124 | 125 | Vector3 generateOrbitalTranslation( double max_parallax, const Vector3 & dir_parallax); 126 | 127 | Matrix3 generateOrbitalRotation( double max_parallax, const Vector3 & dir_parallax, const Vector3& dir_trans); 128 | 129 | }; // end of namespace UtilsTwoView 130 | 131 | --------------------------------------------------------------------------------