├── .gitignore ├── CMakeLists.txt ├── LICENSE ├── README.md ├── config ├── rvio_euroc.yaml └── rvio_rviz.rviz ├── launch └── euroc.launch ├── package.xml ├── rvio.gif └── src ├── rvio ├── FeatureDetector.cc ├── FeatureDetector.h ├── InputBuffer.cc ├── InputBuffer.h ├── PreIntegrator.cc ├── PreIntegrator.h ├── Ransac.cc ├── Ransac.h ├── System.cc ├── System.h ├── Tracker.cc ├── Tracker.h ├── Updater.cc └── Updater.h ├── rvio_mono.cc └── util └── Numerics.h /.gitignore: -------------------------------------------------------------------------------- 1 | CMakeLists.txt.user 2 | *~ 3 | 4 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8) 2 | project(rvio) 3 | 4 | # Set build type 5 | IF(NOT CMAKE_BUILD_TYPE) 6 | SET(CMAKE_BUILD_TYPE Release) 7 | ENDIF() 8 | MESSAGE("Build type: " ${CMAKE_BUILD_TYPE}) 9 | 10 | # Check C++11 or C++0x support 11 | include(CheckCXXCompilerFlag) 12 | CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11) 13 | CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X) 14 | if(COMPILER_SUPPORTS_CXX11) 15 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") 16 | add_definitions(-DCOMPILEDWITHC11) 17 | message(STATUS "Using flag -std=c++11.") 18 | elseif(COMPILER_SUPPORTS_CXX0X) 19 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x") 20 | add_definitions(-DCOMPILEDWITHC0X) 21 | message(STATUS "Using flag -std=c++0x.") 22 | else() 23 | message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.") 24 | endif() 25 | 26 | # Enable debug flags (if you want to debug in gdb) 27 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3 -g3") 28 | 29 | # Find catkin (the ROS build system) 30 | # Add this for finding ros dependencies 31 | find_package(catkin REQUIRED COMPONENTS 32 | roscpp roslib tf sensor_msgs geometry_msgs nav_msgs cv_bridge eigen_conversions 33 | ) 34 | 35 | # Describe catkin project 36 | catkin_package(CATKIN_DEPENDS 37 | roscpp roslib tf std_msgs sensor_msgs geometry_msgs nav_msgs cv_bridge eigen_conversions 38 | INCLUDE_DIRS src 39 | LIBRARIES ${PROJECT_NAME} 40 | ) 41 | 42 | # Include libraries 43 | find_package(Eigen3 REQUIRED) 44 | find_package(OpenCV 3.0 QUIET) 45 | if(NOT OpenCV_FOUND) 46 | find_package(OpenCV 2.4.3 QUIET) 47 | if(NOT OpenCV_FOUND) 48 | message(FATAL_ERROR "OpenCV > 2.4.3 not found.") 49 | endif() 50 | endif() 51 | 52 | # Include directories of headers 53 | include_directories( 54 | ${PROJECT_SOURCE_DIR} 55 | ${EIGEN3_INCLUDE_DIR} 56 | ${catkin_INCLUDE_DIRS} 57 | ) 58 | 59 | ################################################## 60 | # Make binary for the main library 61 | ################################################## 62 | add_library(${PROJECT_NAME} SHARED 63 | src/rvio/System.cc 64 | src/rvio/Updater.cc 65 | src/rvio/Tracker.cc 66 | src/rvio/PreIntegrator.cc 67 | src/rvio/Ransac.cc 68 | src/rvio/InputBuffer.cc 69 | src/rvio/FeatureDetector.cc 70 | ) 71 | 72 | target_link_libraries(${PROJECT_NAME} 73 | ${OpenCV_LIBS} 74 | ${EIGEN3_LIBS} 75 | ) 76 | 77 | ################################################## 78 | # Make binary for main method ROS 79 | ################################################## 80 | add_executable(rvio_mono src/rvio_mono.cc) 81 | target_link_libraries(rvio_mono 82 | ${PROJECT_NAME} 83 | ${OpenCV_LIBS} 84 | ${catkin_LIBRARIES} 85 | ) 86 | -------------------------------------------------------------------------------- /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 | # R-VIO 2 | 3 | R-VIO is an efficient, lightweight, **robocentric** visual-inertial navigation algorithm for consistent 3D motion tracking using only a monocular camera and a single IMU. Different from the standard world-centric algorithms which directly estimate absolute motion of the mobile platform with respect to a fixed, gravity-aligned, global frame of reference, R-VIO i) estimates relative motion of higher accuracy with respect to a moving, local frame (the IMU frame here), and ii) incrementally updates global pose (orientation and position) through a composition step. This code implements our robocentric sliding-window filtering-based VIO formulation that was originally proposed in our *IROS2018* paper and presented in detail in our recent *IJRR* paper: 4 | 5 | - Zheng Huai and Guoquan Huang, **Robocentric visual-inertial odometry**, *The International Journal of Robotics Research (IJRR)*, 2022: [download](https://journals.sagepub.com/doi/10.1177/0278364919853361). 6 | ``` 7 | @article{huai2022robocentric, 8 | title={Robocentric visual-inertial odometry}, 9 | author={Huai, Zheng and Huang, Guoquan}, 10 | journal={The International Journal of Robotics Research}, 11 | volume={41}, 12 | number={7}, 13 | pages={667--689}, 14 | year={2022}, 15 | publisher={SAGE Publications Sage UK: London, England} 16 | } 17 | ``` 18 | 19 | - Zheng Huai and Guoquan Huang, **Robocentric visual-inertial odometry**, *IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)*, Madrid, Spain, Oct 1-5, 2018: [download](https://ieeexplore.ieee.org/document/8593643). 20 | ``` 21 | @inproceedings{huai2018robocentric, 22 | title = {Robocentric visual-inertial odometry}, 23 | author = {Huai, Zheng and Huang, Guoquan}, 24 | booktitle = {IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)}, 25 | pages = {6319--6326}, 26 | year = {2018}, 27 | address = {Madrid, Spain} 28 | } 29 | ``` 30 | 31 | This work has been further extended in our *IEEE RA-L* paper below, and the proposed [R-VIO2](https://github.com/rpng/R-VIO2) is also open sourced. 32 | - Zheng Huai and Guoquan Huang, **Square-Root Robocentric Visual-Inertial Odometry with Online Spatiotemporal Calibration**, *IEEE Robotics and Automation Letters (RA-L)*, 2022: [download](https://ieeexplore.ieee.org/document/9830847). 33 | ``` 34 | @article{huai2022square, 35 | title={Square-root robocentric visual-inertial odometry with online spatiotemporal calibration}, 36 | author={Huai, Zheng and Huang, Guoquan}, 37 | journal={IEEE Robotics and Automation Letters}, 38 | volume={7}, 39 | number={4}, 40 | pages={9961--9968}, 41 | year={2022}, 42 | publisher={IEEE} 43 | } 44 | ``` 45 | 46 | ![](https://media.giphy.com/media/RMecOYlfxEcy4T8JdS/giphy.gif) 47 | 48 | IROS video (**EuRoC MAV** dataset): [YouTube](https://www.youtube.com/watch?v=UtiZ0EKa55M). 49 | 50 | ![](rvio.gif) 51 | 52 | IJRR video (9.8km **Urban Driving** test): [YouTube](https://www.youtube.com/watch?v=l9IC2ddBEYQ). 53 | 54 | ## 1. Prerequisites 55 | 56 | We have tested this code under Ubuntu **16.04** and ROS **Kinetic**. 57 | 58 | ### ROS 59 | Download and install instructions can be found at: http://wiki.ros.org/kinetic/Installation/Ubuntu. 60 | 61 | Additional ROS packages needed: tf, sensor_msgs, geometry_msgs, nav_msgs, cv_bridge, eigen_conversions. 62 | 63 | ### Eigen 64 | Download and install instructions can be found at: http://eigen.tuxfamily.org. **Required at least 3.1.0**. 65 | 66 | ### OpenCV 67 | Download and install instructions can be found at: http://opencv.org. **Required at leat 2.4.3**. **Tested with 2.4.11 and 3.3.1**. 68 | 69 | ## 2. Build and Run 70 | First, `git clone` the repository and `catkin_make` it. Then, to run `rvio` with single camera/IMU inputs from the ROS topics `/camera/image_raw` and `/imu`, a config file in *config* folder and the corresponding launch file in *launch* folder (for example, `rvio_euroc.yaml` and `euroc.launch` are for [EuRoC](https://projects.asl.ethz.ch/datasets/doku.php?id=kmavvisualinertialdatasets) dataset) are needed, and to visualize the outputs of R-VIO please use `rviz` with the settings file `rvio_rviz.rviz` in *config* folder. 71 | ``` 72 | Terminal 1: roscore 73 | ``` 74 | ``` 75 | Terminal 2: rviz (AND OPEN rvio_rviz.rviz IN THE CONFIG FOLDER) 76 | ``` 77 | ``` 78 | Terminal 3: roslaunch rvio euroc.launch 79 | ``` 80 | ``` 81 | Terminal 4: rosbag play --pause V1_01_easy.bag /cam0/image_raw:=/camera/image_raw /imu0:=/imu 82 | ``` 83 | 84 | Note that when testing the `Machine Hall` sequences, you should skip the data in the first few seconds (e.g., 40s for `MH_01_easy`) which are used for initializing the map for SLAM-based algorithms. 85 | 86 | You can also run R-VIO with your own sensors (data) by creating a config file `rvio_NAME_OF_YOUR_DATA.yaml` in *config* folder and the corresponding launch file `NAME_OF_YOUR_DATA.launch` in *launch* folder, referring to our EuRoC example. 87 | 88 | ## 3. License 89 | 90 | This code is released under [GNU General Public License v3 (GPL-3.0)](https://www.gnu.org/licenses/gpl-3.0.en.html). 91 | -------------------------------------------------------------------------------- /config/rvio_euroc.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #-------------------------------------------------------------------------------------------- 4 | # IMU Parameters (fixed). 5 | #-------------------------------------------------------------------------------------------- 6 | 7 | # IMU data rate 8 | IMU.dps: 200 9 | 10 | # IMU sensor noise 11 | IMU.sigma_g: 1.6968e-04 12 | IMU.sigma_wg: 1.9393e-05 13 | IMU.sigma_a: 2.0000e-3 14 | IMU.sigma_wa: 3.0000e-3 15 | 16 | # Gravity 17 | IMU.nG: 9.8082 18 | 19 | # Threshold of small angle [rad] (<.1deg) 20 | IMU.nSmallAngle: 0.001745329 21 | 22 | #-------------------------------------------------------------------------------------------- 23 | # Camera Parameters (fixed). 24 | #-------------------------------------------------------------------------------------------- 25 | 26 | # Camera frame rate 27 | Camera.fps: 20 28 | 29 | # Is RGB or not 30 | Camera.RGB: 0 31 | 32 | # Is fisheye or not 33 | Camera.Fisheye: 0 34 | 35 | # Camera image resolution 36 | Camera.width: 752 37 | Camera.height: 480 38 | 39 | # Camera intrinsics 40 | Camera.fx: 458.654 41 | Camera.fy: 457.296 42 | Camera.cx: 367.215 43 | Camera.cy: 248.375 44 | 45 | Camera.k1: -0.28340811 46 | Camera.k2: 0.07395907 47 | Camera.p1: 0.00019359 48 | Camera.p2: 1.76187114e-05 49 | 50 | # Camera image noise (1/f) 51 | Camera.sigma_px: 0.002180293 52 | Camera.sigma_py: 0.002186767 53 | 54 | # Camera extrinsics [B:IMU,C0:cam0] 55 | Camera.T_BC0: !!opencv-matrix 56 | rows: 4 57 | cols: 4 58 | dt: d 59 | data: [ 0.0148655429818, -0.999880929698, 0.00414029679422, -0.0216401454975, 60 | 0.999557249008, 0.0149672133247, 0.025715529948, -0.064676986768, 61 | -0.0257744366974, 0.00375618835797, 0.999660727178, 0.00981073058949, 62 | 0.0, 0.0, 0.0, 1.0] 63 | 64 | # Timeshift of camera to IMU [s] (t_imu=t_cam+shift) 65 | Camera.nTimeOffset: 0 66 | 67 | #-------------------------------------------------------------------------------------------- 68 | # Tracker Parameters (tunable). 69 | #-------------------------------------------------------------------------------------------- 70 | 71 | # Number of features per image 72 | Tracker.nFeatures: 200 73 | 74 | # Max. tracking length 75 | Tracker.nMaxTrackingLength: 15 76 | 77 | # Min. tracking length 78 | Tracker.nMinTrackingLength: 3 79 | 80 | # Min. distance between features 81 | Tracker.nMinDist: 15 82 | 83 | # Quality level of features 84 | Tracker.nQualLvl: 0.01 85 | 86 | # Block size of image chess grid 87 | Tracker.nBlockSizeX: 150 88 | Tracker.nBlockSizeY: 120 89 | 90 | # Use histogram equalizer or not 91 | Tracker.EnableEqualizer: 1 92 | 93 | # Use Sampson error or not (RANSAC) 94 | Tracker.UseSampson: 1 95 | 96 | # Error threshold for inlier (RANSAC) 97 | Tracker.nInlierThrd: 1e-5 98 | 99 | #-------------------------------------------------------------------------------------------- 100 | # Initialization Parameters (tunable). 101 | #-------------------------------------------------------------------------------------------- 102 | 103 | # Thresholds for moving detection [rad,m] 104 | INI.nThresholdAngle: 0.005 # 0.01 (for MH_*) 105 | INI.nThresholdDispl: 0.01 106 | 107 | # Use gravity alignment or not 108 | INI.EnableAlignment: 1 109 | 110 | # Record the outputs or not 111 | INI.RecordOutputs: 0 112 | 113 | #-------------------------------------------------------------------------------------------- 114 | # Display Parameters (tunable). 115 | #-------------------------------------------------------------------------------------------- 116 | 117 | # Size of point 118 | Landmark.nScale: 0.03 119 | 120 | # Publishing rate 121 | Landmark.nPubRate: 5 122 | -------------------------------------------------------------------------------- /config/rvio_rviz.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 0 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: ~ 7 | Splitter Ratio: 0.600000024 8 | Tree Height: 502 9 | - Class: rviz/Selection 10 | Name: Selection 11 | - Class: rviz/Tool Properties 12 | Expanded: 13 | - /2D Pose Estimate1 14 | - /2D Nav Goal1 15 | - /Publish Point1 16 | Name: Tool Properties 17 | Splitter Ratio: 0.588679016 18 | - Class: rviz/Views 19 | Expanded: 20 | - /Current View1 21 | Name: Views 22 | Splitter Ratio: 0.5 23 | - Class: rviz/Time 24 | Experimental: false 25 | Name: Time 26 | SyncMode: 0 27 | SyncSource: Image 28 | Visualization Manager: 29 | Class: "" 30 | Displays: 31 | - Alpha: 0.5 32 | Cell Size: 1 33 | Class: rviz/Grid 34 | Color: 0; 0; 0 35 | Enabled: true 36 | Line Style: 37 | Line Width: 0.0299999993 38 | Value: Lines 39 | Name: Grid 40 | Normal Cell Count: 0 41 | Offset: 42 | X: 0 43 | Y: 0 44 | Z: 0 45 | Plane: XY 46 | Plane Cell Count: 10 47 | Reference Frame: 48 | Value: true 49 | - Class: rviz/TF 50 | Enabled: true 51 | Frame Timeout: 15 52 | Frames: 53 | All Enabled: true 54 | imu: 55 | Value: true 56 | world: 57 | Value: true 58 | Marker Scale: 2 59 | Name: TF 60 | Show Arrows: true 61 | Show Axes: true 62 | Show Names: true 63 | Tree: 64 | world: 65 | imu: 66 | {} 67 | Update Interval: 0 68 | Value: true 69 | - Class: rviz/Image 70 | Enabled: true 71 | Image Topic: /camera/image_raw 72 | Max Value: 1 73 | Median window: 5 74 | Min Value: 0 75 | Name: Image 76 | Normalize Range: true 77 | Queue Size: 2 78 | Transport Hint: raw 79 | Unreliable: false 80 | Value: true 81 | - Class: rviz/Image 82 | Enabled: true 83 | Image Topic: /rvio/track 84 | Max Value: 1 85 | Median window: 5 86 | Min Value: 0 87 | Name: Tracking [blue:inlier, red:outlier] 88 | Normalize Range: true 89 | Queue Size: 2 90 | Transport Hint: raw 91 | Unreliable: false 92 | Value: true 93 | - Class: rviz/Image 94 | Enabled: false 95 | Image Topic: /rvio/newer 96 | Max Value: 1 97 | Median window: 5 98 | Min Value: 0 99 | Name: Redetection [blue:old, green:new] 100 | Normalize Range: true 101 | Queue Size: 2 102 | Transport Hint: raw 103 | Unreliable: false 104 | Value: false 105 | - Class: rviz/Marker 106 | Enabled: true 107 | Marker Topic: /rvio/landmarks 108 | Name: Features 109 | Namespaces: 110 | points: true 111 | Queue Size: 100 112 | Value: true 113 | - Alpha: 1 114 | Buffer Length: 1 115 | Class: rviz/Path 116 | Color: 255; 65; 0 117 | Enabled: true 118 | Head Diameter: 0.300000012 119 | Head Length: 0.200000003 120 | Length: 0.300000012 121 | Line Style: Lines 122 | Line Width: 0.0299999993 123 | Name: Path 124 | Offset: 125 | X: 0 126 | Y: 0 127 | Z: 0 128 | Pose Color: 255; 85; 255 129 | Pose Style: None 130 | Radius: 0.0299999993 131 | Shaft Diameter: 0.100000001 132 | Shaft Length: 0.100000001 133 | Topic: /rvio/trajectory 134 | Unreliable: false 135 | Value: true 136 | - Angle Tolerance: 0.100000001 137 | Class: rviz/Odometry 138 | Covariance: 139 | Orientation: 140 | Alpha: 0.5 141 | Color: 255; 255; 127 142 | Color Style: Unique 143 | Frame: Local 144 | Offset: 1 145 | Scale: 1 146 | Value: true 147 | Position: 148 | Alpha: 0.300000012 149 | Color: 204; 51; 204 150 | Scale: 1 151 | Value: true 152 | Value: true 153 | Enabled: true 154 | Keep: 15 155 | Name: Odometry 156 | Position Tolerance: 0.100000001 157 | Shape: 158 | Alpha: 1 159 | Axes Length: 0.300000012 160 | Axes Radius: 0.0299999993 161 | Color: 0; 0; 255 162 | Head Length: 0.300000012 163 | Head Radius: 0.100000001 164 | Shaft Length: 1 165 | Shaft Radius: 0.0500000007 166 | Value: Axes 167 | Topic: /rvio/odometry 168 | Unreliable: false 169 | Value: true 170 | Enabled: true 171 | Global Options: 172 | Background Color: 220; 220; 220 173 | Default Light: true 174 | Fixed Frame: world 175 | Frame Rate: 30 176 | Name: root 177 | Tools: 178 | - Class: rviz/Interact 179 | Hide Inactive Objects: true 180 | - Class: rviz/MoveCamera 181 | - Class: rviz/Select 182 | - Class: rviz/FocusCamera 183 | - Class: rviz/Measure 184 | - Class: rviz/SetInitialPose 185 | Topic: /initialpose 186 | - Class: rviz/SetGoal 187 | Topic: /move_base_simple/goal 188 | - Class: rviz/PublishPoint 189 | Single click: true 190 | Topic: /clicked_point 191 | Value: true 192 | Views: 193 | Current: 194 | Class: rviz/Orbit 195 | Distance: 12.4289961 196 | Enable Stereo Rendering: 197 | Stereo Eye Separation: 0.0599999987 198 | Stereo Focal Distance: 1 199 | Swap Stereo Eyes: false 200 | Value: false 201 | Focal Point: 202 | X: -0.627679884 203 | Y: 0.381455839 204 | Z: -2.03634953 205 | Focal Shape Fixed Size: true 206 | Focal Shape Size: 0.0500000007 207 | Invert Z Axis: false 208 | Name: Current View 209 | Near Clip Distance: 0.00999999978 210 | Pitch: 0.594797254 211 | Target Frame: 212 | Value: Orbit (rviz) 213 | Yaw: 3.72088385 214 | Saved: ~ 215 | Window Geometry: 216 | Displays: 217 | collapsed: true 218 | Height: 1028 219 | Hide Left Dock: true 220 | Hide Right Dock: true 221 | Image: 222 | collapsed: false 223 | QMainWindow State: 000000ff00000000fd00000004000000000000016a00000237fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007300000001af00000237000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000016300000295fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000295000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b20000000000000000000000020000048900000181fc0100000008fb000000460054007200610063006b0069006e00670020005b0062006c00750065003a0069006e006c006900650072002c0020007200650064003a006f00750074006c006900650072005d0100000000000002420000010b00fffffffb0000000a0049006d0061006700650100000248000002410000005c00fffffffb0000000a0049006d00610067006500000002370000022a0000000000000000fb00000042005200650064006500740065006300740069006f006e0020005b0062006c00750065003a006f006c0064002c00200067007200650065006e003a006e00650077005d00000002340000022d0000011900fffffffb000000460054007200610063006b0069006e00670020005b0062006c00750065003a0069006e006c006900650072002c0020007200650064003a006f00750074006c006900650072005d0100000000000002330000000000000000fb0000000a0049006d0061006700650100000239000002280000000000000000fb0000003e005200650064006500740065006300740069006f006e0020005b0062006c00750065003a006f006c0064002c0020007200650064003a006e00650077005d00000002340000022d0000000000000000fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004610000005cfc0100000002fb0000000800540069006d00650000000000000004610000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000004890000023700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 224 | Redetection [blue:old, green:new]: 225 | collapsed: false 226 | Selection: 227 | collapsed: false 228 | Time: 229 | collapsed: false 230 | Tool Properties: 231 | collapsed: false 232 | Tracking [blue:inlier, red:outlier]: 233 | collapsed: false 234 | Views: 235 | collapsed: true 236 | Width: 1161 237 | X: 65 238 | Y: 24 239 | -------------------------------------------------------------------------------- /launch/euroc.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | rvio 4 | 0.0.0 5 | Robocentric visual-inertial odometry 6 | 7 | Zheng Huai 8 | 9 | Zheng Huai 10 | 11 | GPLv3 12 | 13 | catkin 14 | 15 | roscpp 16 | roslib 17 | tf 18 | std_msgs 19 | sensor_msgs 20 | geometry_msgs 21 | nav_msgs 22 | cv_bridge 23 | eigen_conversions 24 | 25 | 26 | -------------------------------------------------------------------------------- /rvio.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rpng/R-VIO/48757e5b4d0974dcde143ec71ac14a6451f5ab45/rvio.gif -------------------------------------------------------------------------------- /src/rvio/FeatureDetector.cc: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of R-VIO. 3 | * 4 | * Copyright (C) 2019 Zheng Huai and Guoquan Huang 5 | * For more information see 6 | * 7 | * R-VIO is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * R-VIO is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with R-VIO. If not, see . 19 | */ 20 | 21 | #include 22 | 23 | #include "FeatureDetector.h" 24 | 25 | 26 | namespace RVIO 27 | { 28 | 29 | FeatureDetector::FeatureDetector(const cv::FileStorage& fsSettings) 30 | { 31 | mnMinDistance = fsSettings["Tracker.nMinDist"]; 32 | mnQualityLevel = fsSettings["Tracker.nQualLvl"]; 33 | 34 | mnBlockSizeX = fsSettings["Tracker.nBlockSizeX"]; 35 | mnBlockSizeY = fsSettings["Tracker.nBlockSizeY"]; 36 | 37 | mnImageCols = fsSettings["Camera.width"]; 38 | mnImageRows = fsSettings["Camera.height"]; 39 | 40 | mnGridCols = std::floor(mnImageCols/mnBlockSizeX); 41 | mnGridRows = std::floor(mnImageRows/mnBlockSizeY); 42 | 43 | mnBlocks = mnGridCols*mnGridRows; 44 | 45 | mnOffsetX = .5*(mnImageCols-mnGridCols*mnBlockSizeX); 46 | mnOffsetY = .5*(mnImageRows-mnGridRows*mnBlockSizeY); 47 | 48 | int nMaxFeatsPerImage = fsSettings["Tracker.nFeatures"]; 49 | mnMaxFeatsPerBlock = (float)nMaxFeatsPerImage/mnBlocks; 50 | 51 | mvvGrid.resize(mnBlocks); 52 | } 53 | 54 | 55 | int FeatureDetector::DetectWithSubPix(const cv::Mat& im, 56 | const int nCorners, 57 | const int s, 58 | std::vector& vCorners) 59 | { 60 | vCorners.clear(); 61 | vCorners.reserve(nCorners); 62 | 63 | cv::goodFeaturesToTrack(im, vCorners, nCorners, mnQualityLevel, s*mnMinDistance); 64 | 65 | if (!vCorners.empty()) 66 | { 67 | // Refine the location 68 | cv::Size subPixWinSize(std::floor(.5*mnMinDistance), std::floor(.5*mnMinDistance)); 69 | cv::Size subPixZeroZone(-1,-1); 70 | cv::TermCriteria subPixCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 30, 1e-2); 71 | cv::cornerSubPix(im, vCorners, subPixWinSize, subPixZeroZone, subPixCriteria); 72 | } 73 | 74 | return (int)vCorners.size(); 75 | } 76 | 77 | 78 | void FeatureDetector::ChessGrid(const std::vector& vCorners) 79 | { 80 | mvvGrid.clear(); 81 | mvvGrid.resize(mnBlocks); 82 | 83 | for (const cv::Point2f &pt : vCorners) 84 | { 85 | if (pt.x<=mnOffsetX || pt.y<=mnOffsetY || pt.x>=(mnImageCols-mnOffsetX) || pt.y>=(mnImageRows-mnOffsetY)) 86 | continue; 87 | 88 | int col = std::floor((pt.x-mnOffsetX)/mnBlockSizeX); 89 | int row = std::floor((pt.y-mnOffsetY)/mnBlockSizeY); 90 | 91 | int nBlockIdx = row*mnGridCols+col; 92 | mvvGrid.at(nBlockIdx).emplace_back(pt); 93 | } 94 | } 95 | 96 | 97 | int FeatureDetector::FindNewer(const std::vector& vCorners, 98 | const std::vector& vRefCorners, 99 | std::deque& qNewCorners) 100 | { 101 | ChessGrid(vRefCorners); 102 | 103 | for (const cv::Point2f &pt : vCorners) 104 | { 105 | if (pt.x<=mnOffsetX || pt.y<=mnOffsetY || pt.x>=(mnImageCols-mnOffsetX) || pt.y>=(mnImageRows-mnOffsetY)) 106 | continue; 107 | 108 | int col = std::floor((pt.x-mnOffsetX)/mnBlockSizeX); 109 | int row = std::floor((pt.y-mnOffsetY)/mnBlockSizeY); 110 | 111 | float xl = col*mnBlockSizeX+mnOffsetX; 112 | float xr = xl+mnBlockSizeX; 113 | float yt = row*mnBlockSizeY+mnOffsetY; 114 | float yb = yt+mnBlockSizeY; 115 | 116 | if (fabs(pt.x-xl)1*mnMinDistance) 130 | cnt++; 131 | else 132 | break; 133 | } 134 | 135 | if (cnt==(int)mvvGrid.at(nBlockIdx).size()) 136 | { 137 | qNewCorners.push_back(pt); 138 | mvvGrid.at(nBlockIdx).push_back(pt); 139 | } 140 | } 141 | else 142 | { 143 | qNewCorners.push_back(pt); 144 | mvvGrid.at(nBlockIdx).push_back(pt); 145 | } 146 | } 147 | } 148 | 149 | return (int)qNewCorners.size(); 150 | } 151 | 152 | } // namespace RVIO 153 | -------------------------------------------------------------------------------- /src/rvio/FeatureDetector.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of R-VIO. 3 | * 4 | * Copyright (C) 2019 Zheng Huai and Guoquan Huang 5 | * For more information see 6 | * 7 | * R-VIO is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * R-VIO is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with R-VIO. If not, see . 19 | */ 20 | 21 | #ifndef FEATURE_DETECTOR_H 22 | #define FEATURE_DETECTOR_H 23 | 24 | #include 25 | #include 26 | 27 | #include 28 | 29 | 30 | namespace RVIO 31 | { 32 | 33 | class FeatureDetector 34 | { 35 | public: 36 | 37 | FeatureDetector(const cv::FileStorage& fsSettings); 38 | 39 | /** 40 | * Corner detector 41 | * 42 | * @detect corners on the input image @p im and return the number of corners. 43 | * @realized using the OpenCV functions: 44 | * cv::goodFeaturesToTrack(): determines strong corners in the image, and 45 | * cv::cornerSubPix(): refines the corners' locations. 46 | * @extract the number of corners needed @p nCorners, 47 | * @store the corners into @p vCorners for the visual tracking. 48 | */ 49 | int DetectWithSubPix(const cv::Mat& im, const int nCorners, const int s, std::vector& vCorners); 50 | 51 | int FindNewer(const std::vector& vCorners, const std::vector& vRefCorners, std::deque& qNewCorners); 52 | 53 | private: 54 | 55 | void ChessGrid(const std::vector& vCorners); 56 | 57 | private: 58 | 59 | int mnImageCols; 60 | int mnImageRows; 61 | 62 | float mnMinDistance; 63 | float mnQualityLevel; 64 | 65 | int mnGridCols; 66 | int mnGridRows; 67 | 68 | // Top-left corner of chess grid 69 | int mnOffsetX; 70 | int mnOffsetY; 71 | 72 | float mnBlockSizeX; 73 | float mnBlockSizeY; 74 | 75 | int mnBlocks; 76 | 77 | int mnMaxFeatsPerBlock; 78 | 79 | /** 80 | * Corner distribution 81 | * 82 | * @each vector contains all the corners within the block. 83 | * @used for finding new features for the visual tracking. 84 | */ 85 | std::vector > mvvGrid; 86 | }; 87 | 88 | } // namespace RVIO 89 | 90 | #endif 91 | -------------------------------------------------------------------------------- /src/rvio/InputBuffer.cc: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of R-VIO. 3 | * 4 | * Copyright (C) 2019 Zheng Huai and Guoquan Huang 5 | * For more information see 6 | * 7 | * R-VIO is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * R-VIO is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with R-VIO. If not, see . 19 | */ 20 | 21 | #include "InputBuffer.h" 22 | #include "../util/Numerics.h" 23 | 24 | 25 | namespace RVIO 26 | { 27 | 28 | InputBuffer::InputBuffer() {} 29 | 30 | 31 | void InputBuffer::PushImuData(ImuData* pData) 32 | { 33 | std::unique_lock lock(mMutex); 34 | 35 | mlImuFIFO.push_back(pData); 36 | 37 | if (!mlImuFIFO.empty()) 38 | mlImuFIFO.sort([](const ImuData* a, const ImuData* b){return a->TimestampTimestamp;}); 39 | } 40 | 41 | 42 | void InputBuffer::PushImageData(ImageData* pData) 43 | { 44 | std::unique_lock lock(mMutex); 45 | 46 | mlImageFIFO.push_back(pData); 47 | 48 | if (!mlImageFIFO.empty()) 49 | mlImageFIFO.sort([](const ImageData* a, const ImageData* b){return a->TimestampTimestamp;}); 50 | } 51 | 52 | 53 | bool InputBuffer::GetMeasurements(double nTimeOffset, std::pair >& pMeasurements) 54 | { 55 | if (mlImuFIFO.empty() || mlImageFIFO.empty()) 56 | return false; 57 | 58 | // Make sure we have enough IMU measurements for an image to process! 59 | if (mlImuFIFO.back()->TimestampTimestamp+nTimeOffset) 60 | return false; 61 | 62 | std::unique_lock lock(mMutex); 63 | 64 | ImageData* image = mlImageFIFO.front(); 65 | mlImageFIFO.pop_front(); 66 | 67 | std::list imus; 68 | 69 | while (!mlImuFIFO.empty() && mlImuFIFO.front()->Timestamp<=image->Timestamp+nTimeOffset) 70 | { 71 | imus.push_back(mlImuFIFO.front()); 72 | mlImuFIFO.pop_front(); 73 | } 74 | 75 | if (imus.size()<2) 76 | return false; 77 | 78 | pMeasurements = {image, imus}; 79 | 80 | return true; 81 | } 82 | 83 | } // namespace RVIO 84 | -------------------------------------------------------------------------------- /src/rvio/InputBuffer.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of R-VIO. 3 | * 4 | * Copyright (C) 2019 Zheng Huai and Guoquan Huang 5 | * For more information see 6 | * 7 | * R-VIO is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * R-VIO is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with R-VIO. If not, see . 19 | */ 20 | 21 | #ifndef INPUTBUFFER_H 22 | #define INPUTBUFFER_H 23 | 24 | #include 25 | #include 26 | 27 | #include 28 | 29 | #include 30 | 31 | 32 | namespace RVIO 33 | { 34 | 35 | struct ImuData 36 | { 37 | Eigen::Vector3d AngularVel; 38 | Eigen::Vector3d LinearAccel; 39 | double Timestamp; 40 | double TimeInterval; 41 | 42 | ImuData() 43 | { 44 | AngularVel.setZero(); 45 | LinearAccel.setZero(); 46 | Timestamp = 0; 47 | TimeInterval = 0; 48 | } 49 | 50 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 51 | }; 52 | 53 | struct ImageData 54 | { 55 | cv::Mat Image; 56 | double Timestamp; 57 | 58 | ImageData() 59 | { 60 | Image = cv::Mat(); 61 | Timestamp = 0; 62 | } 63 | }; 64 | 65 | 66 | class InputBuffer 67 | { 68 | public: 69 | 70 | InputBuffer(); 71 | 72 | void PushImuData(ImuData* pData); 73 | void PushImageData(ImageData* pData); 74 | 75 | bool GetMeasurements(double nTimeOffset, std::pair >& pMeasurements); 76 | 77 | protected: 78 | 79 | std::list mlImuFIFO; 80 | std::list mlImageFIFO; 81 | 82 | std::mutex mMutex; 83 | }; 84 | 85 | } // namespace RVIO 86 | 87 | #endif 88 | -------------------------------------------------------------------------------- /src/rvio/PreIntegrator.cc: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of R-VIO. 3 | * 4 | * Copyright (C) 2019 Zheng Huai and Guoquan Huang 5 | * For more information see 6 | * 7 | * R-VIO is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * R-VIO is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with R-VIO. If not, see . 19 | */ 20 | 21 | #include 22 | 23 | #include "PreIntegrator.h" 24 | #include "../util/Numerics.h" 25 | 26 | 27 | namespace RVIO 28 | { 29 | 30 | PreIntegrator::PreIntegrator(const cv::FileStorage& fsSettings) 31 | { 32 | mnGravity = fsSettings["IMU.nG"]; 33 | mnSmallAngle = fsSettings["IMU.nSmallAngle"]; 34 | 35 | mnGyroNoiseSigma = fsSettings["IMU.sigma_g"]; 36 | mnGyroRandomWalkSigma = fsSettings["IMU.sigma_wg"]; 37 | mnAccelNoiseSigma = fsSettings["IMU.sigma_a"]; 38 | mnAccelRandomWalkSigma = fsSettings["IMU.sigma_wa"]; 39 | 40 | ImuNoiseMatrix.setIdentity(); 41 | ImuNoiseMatrix.block<3,3>(0,0) *= pow(mnGyroNoiseSigma,2); 42 | ImuNoiseMatrix.block<3,3>(3,3) *= pow(mnGyroRandomWalkSigma,2); 43 | ImuNoiseMatrix.block<3,3>(6,6) *= pow(mnAccelNoiseSigma,2); 44 | ImuNoiseMatrix.block<3,3>(9,9) *= pow(mnAccelRandomWalkSigma,2); 45 | 46 | xk1k.setZero(26,1); 47 | Pk1k.setZero(24,24); 48 | } 49 | 50 | 51 | void PreIntegrator::propagate(Eigen::VectorXd& xkk, 52 | Eigen::MatrixXd& Pkk, 53 | std::list& lImuData) 54 | { 55 | Eigen::Vector3d gk = xkk.block(7,0,3,1); // unit vector 56 | Eigen::Vector4d qk = xkk.block(10,0,4,1); // [0,0,0,1] 57 | Eigen::Vector3d pk = xkk.block(14,0,3,1); // [0,0,0] 58 | Eigen::Vector3d vk = xkk.block(17,0,3,1); 59 | Eigen::Vector3d bg = xkk.block(20,0,3,1); 60 | Eigen::Vector3d ba = xkk.block(23,0,3,1); 61 | 62 | // Gravity vector in {R} 63 | Eigen::Vector3d gR = gk; 64 | 65 | // Local velocity in {R} 66 | Eigen::Vector3d vR = vk; 67 | 68 | // Relative rotation term 69 | Eigen::Matrix3d Rk = QuatToRot(qk); 70 | Eigen::Matrix3d Rk_T = Rk.transpose(); 71 | 72 | // Preintegrated terms 73 | Eigen::Vector3d dp; 74 | Eigen::Vector3d dv; 75 | dp.setZero(); 76 | dv.setZero(); 77 | 78 | // State transition matrix 79 | Eigen::Matrix F; 80 | Eigen::Matrix Phi; 81 | Eigen::Matrix Psi; 82 | F.setZero(); 83 | Phi.setZero(); 84 | Psi.setIdentity(); 85 | 86 | // Noise matrix 87 | Eigen::Matrix G; 88 | Eigen::Matrix Q; 89 | G.setZero(); 90 | Q.setZero(); 91 | 92 | Eigen::Matrix3d I; 93 | I.setIdentity(); 94 | 95 | double Dt = 0; 96 | 97 | for (std::list::const_iterator lit=lImuData.begin(); 98 | lit!=lImuData.end(); ++lit) 99 | { 100 | Eigen::Vector3d wm = (*lit)->AngularVel; 101 | Eigen::Vector3d am = (*lit)->LinearAccel; 102 | 103 | double dt = (*lit)->TimeInterval; 104 | Dt += dt; 105 | 106 | Eigen::Vector3d w = wm-bg; 107 | Eigen::Vector3d a = am-ba; 108 | 109 | bool bIsSmallAngle = false; 110 | if (w.norm()(9,9) = -wx; 124 | F.block<3,3>(9,18) = -I; 125 | F.block<3,3>(12,9) = -Rk_T*vx; 126 | F.block<3,3>(12,15) = Rk_T; 127 | F.block<3,3>(15,6) = -mnGravity*Rk; 128 | F.block<3,3>(15,9) = -mnGravity*SkewSymm(gk); 129 | F.block<3,3>(15,15) = -wx; 130 | F.block<3,3>(15,18) = -vx; 131 | F.block<3,3>(15,21) = -I; 132 | Phi = Eigen::Matrix::Identity()+dt*F; 133 | Psi = Phi*Psi; 134 | 135 | G.block<3,3>(9,0) = -I; 136 | G.block<3,3>(15,0) = -vx; 137 | G.block<3,3>(15,6) = -I; 138 | G.block<3,3>(18,3) = I; 139 | G.block<3,3>(21,9) = I; 140 | Q = dt*G*ImuNoiseMatrix*(G.transpose()); 141 | 142 | Pkk.block(0,0,24,24) = Phi*(Pkk.block(0,0,24,24))*(Phi.transpose())+Q; 143 | 144 | // State 145 | Eigen::Matrix3d deltaR; 146 | double f1, f2, f3, f4; 147 | if (bIsSmallAngle) 148 | { 149 | deltaR = I-dt*wx+(pow(dt,2)/2)*wx2; 150 | assert(std::isnan(deltaR.norm())!=true); 151 | 152 | f1 = -pow(dt,3)/3; 153 | f2 = pow(dt,4)/8; 154 | f3 = -pow(dt,2)/2; 155 | f4 = pow(dt,3)/6; 156 | } 157 | else 158 | { 159 | deltaR = I-(sinwdt/w1)*wx+((1-coswdt)/pow(w1,2))*wx2; 160 | assert(std::isnan(deltaR.norm())!=true); 161 | 162 | f1 = (wdt*coswdt-sinwdt)/pow(w1,3); 163 | f2 = .5*(wdt2-2*coswdt-2*wdt*sinwdt+2)/pow(w1,4); 164 | f3 = (coswdt-1)/pow(w1,2); 165 | f4 = (wdt-sinwdt)/pow(w1,3); 166 | } 167 | 168 | Rk = deltaR*Rk; 169 | Rk_T = Rk.transpose(); 170 | 171 | dp += dv*dt; 172 | dp += Rk_T*(.5*pow(dt,2)*I+f1*wx+f2*wx2)*a; 173 | dv += Rk_T*(dt*I+f3*wx+f4*wx2)*a; 174 | 175 | pk = vR*Dt-.5*mnGravity*gR*pow(Dt,2)+dp; 176 | vk = Rk*(vR-mnGravity*gR*Dt+dv); 177 | gk = Rk*gR; 178 | gk.normalize(); 179 | } 180 | 181 | xk1k = xkk; 182 | xk1k.block(10,0,4,1) = RotToQuat(Rk); 183 | xk1k.block(14,0,3,1) = pk; 184 | xk1k.block(17,0,3,1) = vk; 185 | 186 | int nCloneStates = (xkk.rows()-26)/7; 187 | if (nCloneStates>0) 188 | { 189 | Pkk.block(0,24,24,6*nCloneStates) = Psi*Pkk.block(0,24,24,6*nCloneStates); 190 | Pkk.block(24,0,6*nCloneStates,24) = Pkk.block(0,24,24,6*nCloneStates).transpose(); 191 | } 192 | Pkk = .5*(Pkk+Pkk.transpose()); 193 | Pk1k = Pkk; 194 | } 195 | 196 | } // namespace RVIO 197 | -------------------------------------------------------------------------------- /src/rvio/PreIntegrator.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of R-VIO. 3 | * 4 | * Copyright (C) 2019 Zheng Huai and Guoquan Huang 5 | * For more information see 6 | * 7 | * R-VIO is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * R-VIO is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with R-VIO. If not, see . 19 | */ 20 | 21 | #ifndef PREINTEGRATOR_H 22 | #define PREINTEGRATOR_H 23 | 24 | #include 25 | 26 | #include 27 | 28 | #include "InputBuffer.h" 29 | 30 | 31 | namespace RVIO 32 | { 33 | 34 | class PreIntegrator 35 | { 36 | public: 37 | 38 | PreIntegrator(const cv::FileStorage& fsSettings); 39 | 40 | void propagate(Eigen::VectorXd& xkk, Eigen::MatrixXd& Pkk, std::list& lImuData); 41 | 42 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 43 | 44 | public: 45 | 46 | // Outputs 47 | Eigen::VectorXd xk1k; 48 | Eigen::MatrixXd Pk1k; 49 | 50 | private: 51 | 52 | double mnGravity; 53 | double mnSmallAngle; 54 | 55 | // Sigma{g,wg,a,wa} 56 | double mnGyroNoiseSigma; 57 | double mnGyroRandomWalkSigma; 58 | double mnAccelNoiseSigma; 59 | double mnAccelRandomWalkSigma; 60 | 61 | Eigen::Matrix ImuNoiseMatrix; 62 | }; 63 | 64 | } // namespace RVIO 65 | 66 | #endif 67 | -------------------------------------------------------------------------------- /src/rvio/Ransac.cc: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of R-VIO. 3 | * 4 | * Copyright (C) 2019 Zheng Huai and Guoquan Huang 5 | * For more information see 6 | * 7 | * R-VIO is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * R-VIO is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with R-VIO. If not, see . 19 | */ 20 | 21 | #include 22 | 23 | #include 24 | 25 | #include "Ransac.h" 26 | #include "../util/Numerics.h" 27 | 28 | 29 | namespace RVIO 30 | { 31 | 32 | Ransac::Ransac(const cv::FileStorage& fsSettings) 33 | { 34 | const int bUseSampson = fsSettings["Tracker.UseSampson"]; 35 | mbUseSampson = bUseSampson; 36 | 37 | mnInlierThreshold = fsSettings["Tracker.nInlierThrd"]; 38 | 39 | mnSmallAngle = fsSettings["IMU.nSmallAngle"]; 40 | 41 | cv::Mat T(4,4,CV_32F); 42 | fsSettings["Camera.T_BC0"] >> T; 43 | Eigen::Matrix4d Tic; 44 | cv::cv2eigen(T,Tic); 45 | mRic = Tic.block<3,3>(0,0); 46 | mRci = mRic.transpose(); 47 | } 48 | 49 | 50 | void Ransac::SetPointPair(const int nInlierCandidates, 51 | const int nIterations) 52 | { 53 | std::vector vIndices(nInlierCandidates); 54 | for (int i=0; i(3*nIterNum,0) = SkewSymm(t)*R; 117 | } 118 | 119 | 120 | void Ransac::GetRotation(std::list& lImuData, 121 | Eigen::Matrix3d& R) 122 | { 123 | Eigen::Matrix3d tempR; 124 | tempR.setIdentity(); 125 | 126 | Eigen::Matrix3d I; 127 | I.setIdentity(); 128 | 129 | for (std::list::const_iterator lit=lImuData.begin(); 130 | lit!=lImuData.end(); ++lit) 131 | { 132 | Eigen::Vector3d wm = (*lit)->AngularVel; 133 | double dt = (*lit)->TimeInterval; 134 | 135 | bool bIsSmallAngle = false; 136 | if (wm.norm()::const_iterator vit=mvInlierCandidateIndices.begin(); 164 | vit!=mvInlierCandidateIndices.end(); ++vit) 165 | { 166 | int idx = *vit; 167 | 168 | double nDistance; 169 | if (mbUseSampson) 170 | nDistance = SampsonError(Points1.col(idx), Points2.col(idx), mRansacModel.hypotheses.block<3,3>(3*nIterNum,0)); 171 | else 172 | nDistance = AlgebraicError(Points1.col(idx), Points2.col(idx), mRansacModel.hypotheses.block<3,3>(3*nIterNum,0)); 173 | 174 | if (nDistance& lImuData, 183 | std::vector& vInlierFlag) 184 | { 185 | mRansacModel.hypotheses.setZero(); 186 | mRansacModel.nInliers.setZero(); 187 | mRansacModel.twoPoints.setZero(); 188 | 189 | mvInlierCandidateIndices.clear(); 190 | 191 | int nInlierCandidates = 0; 192 | for (int i=0; i<(int)vInlierFlag.size(); ++i) 193 | { 194 | if (vInlierFlag.at(i)) 195 | { 196 | mvInlierCandidateIndices.push_back(i); 197 | nInlierCandidates++; 198 | } 199 | } 200 | 201 | if (nInlierCandidates>mRansacModel.nIterations) 202 | SetPointPair(nInlierCandidates, mRansacModel.nIterations); 203 | else 204 | // Too few inliers 205 | return 0; 206 | 207 | Eigen::Matrix3d R; 208 | GetRotation(lImuData, R); 209 | 210 | int nWinnerInliersNumber = 0; 211 | int nWinnerHypothesisIdx = 0; 212 | for (int i=0; inWinnerInliersNumber) 219 | { 220 | nWinnerInliersNumber = mRansacModel.nInliers(i); 221 | nWinnerHypothesisIdx = i; 222 | } 223 | } 224 | 225 | Eigen::Matrix3d WinnerE = mRansacModel.hypotheses.block<3,3>(3*nWinnerHypothesisIdx,0); 226 | 227 | int nNewOutliers = 0; 228 | for (int i=0; imnInlierThreshold || std::isnan(nDistance)) 239 | { 240 | // Mark as outlier 241 | vInlierFlag.at(idx) = 0; 242 | nNewOutliers++; 243 | } 244 | } 245 | 246 | return nInlierCandidates-nNewOutliers; 247 | } 248 | 249 | 250 | double Ransac::SampsonError(const Eigen::Vector3d& pt1, 251 | const Eigen::Vector3d& pt2, 252 | const Eigen::Matrix3d& E) const 253 | { 254 | Eigen::Vector3d Fx1 = E*pt1; 255 | Eigen::Vector3d Fx2 = E.transpose()*pt2; 256 | 257 | return (pow(pt2.transpose()*E*pt1,2))/(pow(Fx1(0),2)+pow(Fx1(1),2)+pow(Fx2(0),2)+pow(Fx2(1),2)); 258 | } 259 | 260 | 261 | double Ransac::AlgebraicError(const Eigen::Vector3d& pt1, 262 | const Eigen::Vector3d& pt2, 263 | const Eigen::Matrix3d& E) const 264 | { 265 | return fabs(pt2.transpose()*E*pt1); 266 | } 267 | 268 | } // namespace RVIO 269 | -------------------------------------------------------------------------------- /src/rvio/Ransac.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of R-VIO. 3 | * 4 | * Copyright (C) 2019 Zheng Huai and Guoquan Huang 5 | * For more information see 6 | * 7 | * R-VIO is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * R-VIO is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with R-VIO. If not, see . 19 | */ 20 | 21 | #ifndef RANSAC_H 22 | #define RANSAC_H 23 | 24 | #include 25 | #include 26 | 27 | #include 28 | 29 | #include "InputBuffer.h" 30 | 31 | 32 | namespace RVIO 33 | { 34 | 35 | /** 36 | * 2-Point RANSAC model 37 | * 38 | * @do @p nIterations (min. 16) trials to find the best hypothesis (of essential matrix). 39 | * @all the hypotheses (of essential matrix E) are stored in @p hypotheses. 40 | * @the number of inliers for each hypothesis is stored in @p nInliers. 41 | * @the indices of two correspondences are stored in @p twoPoints. 42 | */ 43 | struct RansacModel 44 | { 45 | Eigen::MatrixXd hypotheses; 46 | Eigen::MatrixXi nInliers; 47 | Eigen::MatrixXi twoPoints; 48 | int nIterations; 49 | 50 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 51 | 52 | RansacModel() 53 | { 54 | nIterations = 16; 55 | hypotheses.resize(nIterations*3,3); 56 | nInliers.resize(nIterations,1); 57 | twoPoints.resize(nIterations,2); 58 | } 59 | }; 60 | 61 | 62 | class Ransac 63 | { 64 | public: 65 | 66 | Ransac(const cv::FileStorage& fsSettings); 67 | 68 | /** 69 | * @create a set of random 2-point indices for RANSAC. 70 | * @the number of inliers is @p nInlierCandidates. 71 | * @the number of iterations needed is @p nIterations. 72 | */ 73 | void SetPointPair(const int nInlierCandidates, const int nIterations); 74 | 75 | /** 76 | * @use two (normalized) correspondences point(i)(j), where seq i=(A,B) and 77 | * reference frame j = (1,2). 78 | * @use @p R to rotate the points in frame 1 to frame 0 having the same orientation as frame 2. 79 | * @compute essential matrix by solving a problem with unknow translation only, (p2^T)[tx]p0=0. 80 | */ 81 | void SetRansacModel(const Eigen::MatrixXd& Points1, const Eigen::MatrixXd& Points2, 82 | const Eigen::Matrix3d& R, const int nIterNum); 83 | 84 | /** 85 | * @get rotation matrix @p R by integrating the IMU (gyro) measurements. 86 | */ 87 | void GetRotation(std::list& lImuData, Eigen::Matrix3d& R); 88 | 89 | /** 90 | * @count the number of inliers in the @nIterNum-th trial. 91 | */ 92 | void CountInliers(const Eigen::MatrixXd& Points1, const Eigen::MatrixXd& Points2, 93 | const int nIterNum); 94 | 95 | /** 96 | * 2-Point RANSAC 97 | * 98 | * @the two sets used are @p Points1 and @p Points2. 99 | * @the original inlier flag vector @p vInlierFlag is obtained from the tracking, 100 | * i.e. @p status of function cv::calcOpticalFlowPyrLK(). 101 | * @the output is the refined flag vector @p vInlierFlag. 102 | */ 103 | int FindInliers(const Eigen::MatrixXd& Points1, const Eigen::MatrixXd& Points2, 104 | std::list& lImuData, std::vector& vInlierFlag); 105 | 106 | /** 107 | * Metric for evaluation 108 | */ 109 | double SampsonError(const Eigen::Vector3d& pt1, const Eigen::Vector3d& pt2, 110 | const Eigen::Matrix3d& E) const; 111 | 112 | double AlgebraicError(const Eigen::Vector3d& pt1, const Eigen::Vector3d& pt2, 113 | const Eigen::Matrix3d& E) const; 114 | 115 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 116 | 117 | private: 118 | 119 | bool mbUseSampson; 120 | 121 | double mnInlierThreshold; 122 | 123 | double mnSmallAngle; 124 | 125 | // Extrinsics 126 | Eigen::Matrix3d mRci; 127 | Eigen::Matrix3d mRic; 128 | 129 | RansacModel mRansacModel; 130 | 131 | std::vector mvInlierCandidateIndices; 132 | }; 133 | 134 | } // namespace RVIO 135 | 136 | #endif 137 | -------------------------------------------------------------------------------- /src/rvio/System.cc: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of R-VIO. 3 | * 4 | * Copyright (C) 2019 Zheng Huai and Guoquan Huang 5 | * For more information see 6 | * 7 | * R-VIO is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * R-VIO is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with R-VIO. If not, see . 19 | */ 20 | 21 | #include 22 | 23 | #include 24 | 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | 31 | #include "System.h" 32 | #include "../util/Numerics.h" 33 | 34 | 35 | namespace RVIO 36 | { 37 | 38 | nav_msgs::Path path; 39 | 40 | std::ofstream fPoseResults; 41 | std::ofstream fTimeResults; 42 | 43 | 44 | System::System(const std::string& strSettingsFile) 45 | { 46 | // Output welcome message 47 | std::cout << "\n" << 48 | "R-VIO: Robocentric visual-inertial odometry" << "\n" << "\n" 49 | "Copyright (C) 2019 Zheng Huai and Guoquan Huang" << "\n" << 50 | "Robot Perception and Navigation Group, University of Delaware." << "\n" << "\n"; 51 | 52 | // Read settings file 53 | cv::FileStorage fsSettings(strSettingsFile, cv::FileStorage::READ); 54 | if (!fsSettings.isOpened()) 55 | { 56 | ROS_ERROR("Failed to open settings file at: %s", strSettingsFile.c_str()); 57 | exit(-1); 58 | } 59 | 60 | mnImuRate = fsSettings["IMU.dps"]; 61 | 62 | msigmaGyroNoise = fsSettings["IMU.sigma_g"]; 63 | msigmaGyroBias = fsSettings["IMU.sigma_wg"]; 64 | msigmaAccelNoise = fsSettings["IMU.sigma_a"]; 65 | msigmaAccelBias = fsSettings["IMU.sigma_wa"]; 66 | 67 | mnGravity = fsSettings["IMU.nG"]; 68 | 69 | mnCamTimeOffset = fsSettings["Camera.nTimeOffset"]; 70 | 71 | const int nMaxTrackingLength = fsSettings["Tracker.nMaxTrackingLength"]; 72 | mnSlidingWindowSize = nMaxTrackingLength-1; 73 | 74 | const int nMinTrackingLength = fsSettings["Tracker.nMinTrackingLength"]; 75 | mnMinCloneStates = nMinTrackingLength-1; 76 | 77 | const int bEnableAlignment = fsSettings["INI.EnableAlignment"]; 78 | mbEnableAlignment = bEnableAlignment; 79 | 80 | const int bRecordOutputs = fsSettings["INI.RecordOutputs"]; 81 | mbRecordOutputs = bRecordOutputs; 82 | 83 | if (mbRecordOutputs) 84 | { 85 | std::string pkg_path = ros::package::getPath("rvio"); 86 | fPoseResults.open(pkg_path+"/stamped_pose_ests.dat", std::ofstream::out); 87 | fTimeResults.open(pkg_path+"/time_cost.dat", std::ofstream::out); 88 | } 89 | 90 | mnThresholdAngle = fsSettings["INI.nThresholdAngle"]; 91 | mnThresholdDispl = fsSettings["INI.nThresholdDispl"]; 92 | 93 | mbIsMoving = false; 94 | mbIsReady = false; 95 | 96 | mpInputBuffer = new InputBuffer(); 97 | mpTracker = new Tracker(fsSettings); 98 | mpUpdater = new Updater(fsSettings); 99 | mpPreIntegrator = new PreIntegrator(fsSettings); 100 | 101 | mPathPub = mSystemNode.advertise("/rvio/trajectory", 1); 102 | mOdomPub = mSystemNode.advertise("/rvio/odometry", 1); 103 | } 104 | 105 | 106 | System::~System() 107 | { 108 | delete mpTracker; 109 | delete mpUpdater; 110 | delete mpInputBuffer; 111 | delete mpPreIntegrator; 112 | } 113 | 114 | 115 | void System::initialize(const Eigen::Vector3d& w, const Eigen::Vector3d& a, 116 | const int nImuData, const bool bEnableAlignment) 117 | { 118 | Eigen::Vector3d g = a; 119 | g.normalize(); 120 | 121 | Eigen::Matrix3d R = Eigen::Matrix3d::Identity(); 122 | if (bEnableAlignment) 123 | { 124 | // i. Align z-axis with gravity 125 | Eigen::Vector3d zv = g; 126 | 127 | // ii. Make x-axis perpendicular to z-axis 128 | Eigen::Vector3d ex = Eigen::Vector3d(1,0,0); 129 | Eigen::Vector3d xv = ex-zv*zv.transpose()*ex; 130 | xv.normalize(); 131 | 132 | // iii. Get y-axis 133 | Eigen::Vector3d yv = SkewSymm(zv)*xv; 134 | yv.normalize(); 135 | 136 | // iv. The orientation of {G} in {R0} 137 | Eigen::Matrix3d tempR; 138 | tempR << xv, yv, zv; 139 | R = tempR; 140 | } 141 | 142 | xkk.setZero(26,1); 143 | xkk.block(0,0,4,1) = RotToQuat(R); 144 | xkk.block(7,0,3,1) = g; 145 | 146 | if (nImuData>1) 147 | { 148 | xkk.block(20,0,3,1) = w; // bg 149 | xkk.block(23,0,3,1) = a-mnGravity*g; // ba 150 | } 151 | 152 | double dt = 1./mnImuRate; 153 | 154 | Pkk.setZero(24,24); 155 | Pkk(0,0) = pow(1e-3,2); // qG 156 | Pkk(1,1) = pow(1e-3,2); 157 | Pkk(2,2) = pow(1e-3,2); 158 | Pkk(3,3) = pow(1e-3,2); // pG 159 | Pkk(4,4) = pow(1e-3,2); 160 | Pkk(5,5) = pow(1e-3,2); 161 | Pkk(6,6) = nImuData*dt*pow(msigmaAccelNoise,2); // g 162 | Pkk(7,7) = nImuData*dt*pow(msigmaAccelNoise,2); 163 | Pkk(8,8) = nImuData*dt*pow(msigmaAccelNoise,2); 164 | Pkk(18,18) = nImuData*dt*pow(msigmaGyroBias,2); // bg 165 | Pkk(19,19) = nImuData*dt*pow(msigmaGyroBias,2); 166 | Pkk(20,20) = nImuData*dt*pow(msigmaGyroBias,2); 167 | Pkk(21,21) = nImuData*dt*pow(msigmaAccelBias,2); // ba 168 | Pkk(22,22) = nImuData*dt*pow(msigmaAccelBias,2); 169 | Pkk(23,23) = nImuData*dt*pow(msigmaAccelBias,2); 170 | } 171 | 172 | 173 | void System::MonoVIO() 174 | { 175 | static int nCloneStates = 0; 176 | static int nImageCountAfterInit = 0; 177 | 178 | std::pair > pMeasurements; 179 | if (!mpInputBuffer->GetMeasurements(mnCamTimeOffset, pMeasurements)) 180 | return; 181 | 182 | /* Initialization */ 183 | if (!mbIsReady) 184 | { 185 | static Eigen::Vector3d wm = Eigen::Vector3d::Zero(); 186 | static Eigen::Vector3d am = Eigen::Vector3d::Zero(); 187 | static int nImuDataCount = 0; 188 | 189 | if (!mbIsMoving) 190 | { 191 | Eigen::Vector3d ang; 192 | Eigen::Vector3d vel; 193 | Eigen::Vector3d displ; 194 | ang.setZero(); 195 | vel.setZero(); 196 | displ.setZero(); 197 | 198 | for (std::list::const_iterator lit=pMeasurements.second.begin(); 199 | lit!=pMeasurements.second.end(); ++lit) 200 | { 201 | Eigen::Vector3d w = (*lit)->AngularVel; 202 | Eigen::Vector3d a = (*lit)->LinearAccel; 203 | double dt = (*lit)->TimeInterval; 204 | 205 | a -= mnGravity*a/a.norm(); 206 | 207 | ang += dt*w; 208 | vel += dt*a; 209 | displ += dt*vel+.5*pow(dt,2)*a; 210 | } 211 | 212 | // If the change is larger than the threshold 213 | if (ang.norm()>mnThresholdAngle || displ.norm()>mnThresholdDispl) 214 | mbIsMoving = true; 215 | } 216 | 217 | while (!pMeasurements.second.empty()) 218 | { 219 | if (!mbIsMoving) 220 | { 221 | wm += (pMeasurements.second.front())->AngularVel; 222 | am += (pMeasurements.second.front())->LinearAccel; 223 | pMeasurements.second.pop_front(); 224 | nImuDataCount++; 225 | } 226 | else 227 | { 228 | if (nImuDataCount==0) 229 | { 230 | wm = (pMeasurements.second.front())->AngularVel; 231 | am = (pMeasurements.second.front())->LinearAccel; 232 | nImuDataCount = 1; 233 | } 234 | else 235 | { 236 | wm = wm/nImuDataCount; 237 | am = am/nImuDataCount; 238 | } 239 | 240 | initialize(wm, am, nImuDataCount, mbEnableAlignment); 241 | 242 | mbIsReady = true; 243 | break; 244 | } 245 | } 246 | 247 | if (!mbIsReady) 248 | return; 249 | } 250 | 251 | nImageCountAfterInit++; 252 | 253 | ros::WallTime t1, t2, t3; 254 | 255 | t1 = ros::WallTime::now(); 256 | 257 | /* Visual tracking */ 258 | mpTracker->track(pMeasurements.first->Image, pMeasurements.second); 259 | 260 | t2 = ros::WallTime::now(); 261 | 262 | /* Propagation */ 263 | mpPreIntegrator->propagate(xkk, Pkk, pMeasurements.second); 264 | 265 | /* Update */ 266 | if (nCloneStates>mnMinCloneStates) 267 | { 268 | mpUpdater->update(mpPreIntegrator->xk1k, mpPreIntegrator->Pk1k, mpTracker->mvFeatTypesForUpdate, mpTracker->mvlFeatMeasForUpdate); 269 | 270 | xkk = mpUpdater->xk1k1; 271 | Pkk = mpUpdater->Pk1k1; 272 | } 273 | else 274 | { 275 | xkk = mpPreIntegrator->xk1k; 276 | Pkk = mpPreIntegrator->Pk1k; 277 | } 278 | 279 | /* State augmentation */ 280 | if (nImageCountAfterInit>1) 281 | { 282 | if (nCloneStates Vk; 345 | Vk.setZero(); 346 | Vk.block(0,0,3,3) = Rk; 347 | Vk.block(0,9,3,3).setIdentity(); 348 | Vk.block(3,3,3,3) = Rk; 349 | Vk.block(3,9,3,3) = SkewSymm(pkG); 350 | Vk.block(3,12,3,3) = -Rk; 351 | Vk.block(6,6,3,3) = Rk; 352 | Vk.block(6,9,3,3) = SkewSymm(gk); 353 | Vk.block(15,15,9,9).setIdentity(); 354 | 355 | Pkk.block(0,0,24,24) = Vk*Pkk.block(0,0,24,24)*(Vk.transpose()); 356 | Pkk.block(0,24,24,6*nCloneStates) = Vk*Pkk.block(0,24,24,6*nCloneStates); 357 | Pkk.block(24,0,6*nCloneStates,24) = Pkk.block(0,24,24,6*nCloneStates).transpose(); 358 | Pkk = .5*(Pkk+Pkk.transpose()); 359 | 360 | // xk 361 | xkk.block(0,0,4,1) = qkG; 362 | xkk.block(4,0,3,1) = pkG; 363 | xkk.block(7,0,3,1) = gk; 364 | xkk.block(10,0,4,1) = Eigen::Vector4d(0,0,0,1); 365 | xkk.block(14,0,3,1) = Eigen::Vector3d(0,0,0); 366 | 367 | t3 = ros::WallTime::now(); 368 | 369 | if (mbRecordOutputs) 370 | { 371 | fPoseResults << std::setprecision(19) << pMeasurements.first->Timestamp << " " 372 | << pGk(0) << " " << pGk(1) << " " << pGk(2) << " " 373 | << qkG(0) << " " << qkG(1) << " " << qkG(2) << " " << qkG(3) << "\n"; 374 | fPoseResults.flush(); 375 | 376 | fTimeResults << nImageCountAfterInit << std::setprecision(19) << " " 377 | << 1e3*(t2.toSec()-t1.toSec()) << " " 378 | << 1e3*(t3.toSec()-t2.toSec()) << "\n"; 379 | fTimeResults.flush(); 380 | } 381 | 382 | ROS_INFO("qkG: %5f, %5f, %5f, %5f", qkG(0), qkG(1), qkG(2), qkG(3)); 383 | ROS_INFO("pGk: %5f, %5f, %5f\n", pGk(0), pGk(1), pGk(2)); 384 | 385 | 386 | /* Interact with ROS rviz */ 387 | // Broadcast tf message 388 | geometry_msgs::TransformStamped transformStamped; 389 | transformStamped.header.stamp = ros::Time::now(); 390 | transformStamped.header.frame_id = "world"; 391 | transformStamped.child_frame_id = "imu"; 392 | transformStamped.transform.translation.x = pGk(0); 393 | transformStamped.transform.translation.y = pGk(1); 394 | transformStamped.transform.translation.z = pGk(2); 395 | transformStamped.transform.rotation.x = qkG(0); 396 | transformStamped.transform.rotation.y = qkG(1); 397 | transformStamped.transform.rotation.z = qkG(2); 398 | transformStamped.transform.rotation.w = qkG(3); 399 | 400 | mTfPub.sendTransform(transformStamped); 401 | 402 | // Broadcast odometry message 403 | nav_msgs::Odometry odom; 404 | odom.header.stamp = ros::Time::now(); 405 | odom.header.frame_id = "world"; 406 | odom.pose.pose.position.x = pGk(0); 407 | odom.pose.pose.position.y = pGk(1); 408 | odom.pose.pose.position.z = pGk(2); 409 | odom.pose.pose.orientation.x = qkG(0); 410 | odom.pose.pose.orientation.y = qkG(1); 411 | odom.pose.pose.orientation.z = qkG(2); 412 | odom.pose.pose.orientation.w = qkG(3); 413 | odom.child_frame_id = "imu"; 414 | odom.twist.twist.linear.x = vk(0); 415 | odom.twist.twist.linear.y = vk(1); 416 | odom.twist.twist.linear.z = vk(2); 417 | 418 | mOdomPub.publish(odom); 419 | 420 | // Visualize the trajectory 421 | geometry_msgs::PoseStamped pose; 422 | pose.header.frame_id = "world"; 423 | pose.pose.position.x = pGk(0); 424 | pose.pose.position.y = pGk(1); 425 | pose.pose.position.z = pGk(2); 426 | pose.pose.orientation.x = qkG(0); 427 | pose.pose.orientation.y = qkG(1); 428 | pose.pose.orientation.z = qkG(2); 429 | pose.pose.orientation.w = qkG(3); 430 | 431 | path.header.frame_id = "world"; 432 | path.poses.push_back(pose); 433 | 434 | mPathPub.publish(path); 435 | 436 | usleep(1000); 437 | } 438 | 439 | } //namespace RVIO 440 | -------------------------------------------------------------------------------- /src/rvio/System.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of R-VIO. 3 | * 4 | * Copyright (C) 2019 Zheng Huai and Guoquan Huang 5 | * For more information see 6 | * 7 | * R-VIO is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * R-VIO is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with R-VIO. If not, see . 19 | */ 20 | 21 | #ifndef SYSTEM_H 22 | #define SYSTEM_H 23 | 24 | #include 25 | 26 | #include 27 | 28 | #include 29 | 30 | #include 31 | 32 | #include "Tracker.h" 33 | #include "Updater.h" 34 | #include "InputBuffer.h" 35 | #include "PreIntegrator.h" 36 | 37 | 38 | namespace RVIO 39 | { 40 | 41 | class System 42 | { 43 | public: 44 | 45 | System(const std::string& strSettingsFile); 46 | 47 | ~System(); 48 | 49 | void PushImuData(ImuData* data) {mpInputBuffer->PushImuData(data);} 50 | void PushImageData(ImageData* pData) {mpInputBuffer->PushImageData(pData);} 51 | 52 | void initialize(const Eigen::Vector3d& w, const Eigen::Vector3d& a, const int nImuData, const bool bEnableAlignment); 53 | 54 | void MonoVIO(); 55 | 56 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 57 | 58 | private: 59 | 60 | double mnImuRate; 61 | double mnCamTimeOffset; 62 | 63 | int mnSlidingWindowSize; 64 | int mnMinCloneStates; 65 | 66 | bool mbEnableAlignment; 67 | bool mbRecordOutputs; 68 | 69 | bool mbIsMoving; 70 | bool mbIsReady; 71 | 72 | double mnThresholdAngle; 73 | double mnThresholdDispl; 74 | 75 | // Gravity 76 | double mnGravity; 77 | 78 | // Sigma{g,wg,a,wa} 79 | double msigmaGyroNoise; 80 | double msigmaGyroBias; 81 | double msigmaAccelNoise; 82 | double msigmaAccelBias; 83 | 84 | // State and covariance 85 | Eigen::VectorXd xkk; 86 | Eigen::MatrixXd Pkk; 87 | 88 | // Handlers 89 | Tracker* mpTracker; 90 | Updater* mpUpdater; 91 | InputBuffer* mpInputBuffer; 92 | PreIntegrator* mpPreIntegrator; 93 | 94 | // Interact with rviz 95 | ros::NodeHandle mSystemNode; 96 | ros::Publisher mPathPub; 97 | ros::Publisher mOdomPub; 98 | tf::TransformBroadcaster mTfPub; 99 | }; 100 | 101 | } // namespace RVIO 102 | 103 | #endif 104 | -------------------------------------------------------------------------------- /src/rvio/Tracker.cc: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of R-VIO. 3 | * 4 | * Copyright (C) 2019 Zheng Huai and Guoquan Huang 5 | * For more information see 6 | * 7 | * R-VIO is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * R-VIO is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with R-VIO. If not, see . 19 | */ 20 | 21 | #include 22 | #include 23 | #include 24 | 25 | #include 26 | 27 | #include "Tracker.h" 28 | 29 | 30 | namespace RVIO 31 | { 32 | 33 | CvScalar red = CV_RGB(255,64,64); 34 | CvScalar green = CV_RGB(64,255,64); 35 | CvScalar blue = CV_RGB(64,64,255); 36 | 37 | Tracker::Tracker(const cv::FileStorage& fsSettings) 38 | { 39 | const float fx = fsSettings["Camera.fx"]; 40 | const float fy = fsSettings["Camera.fy"]; 41 | const float cx = fsSettings["Camera.cx"]; 42 | const float cy = fsSettings["Camera.cy"]; 43 | 44 | cv::Mat K = cv::Mat::eye(3,3,CV_32F); 45 | K.at(0,0) = fx; 46 | K.at(1,1) = fy; 47 | K.at(0,2) = cx; 48 | K.at(1,2) = cy; 49 | K.copyTo(mK); 50 | 51 | cv::Mat DistCoef(4,1,CV_32F); 52 | DistCoef.at(0) = fsSettings["Camera.k1"]; 53 | DistCoef.at(1) = fsSettings["Camera.k2"]; 54 | DistCoef.at(2) = fsSettings["Camera.p1"]; 55 | DistCoef.at(3) = fsSettings["Camera.p2"]; 56 | const float k3 = fsSettings["Camera.k3"]; 57 | if(k3!=0) 58 | { 59 | DistCoef.resize(5); 60 | DistCoef.at(4) = k3; 61 | } 62 | DistCoef.copyTo(mDistCoef); 63 | 64 | const int bIsRGB = fsSettings["Camera.RGB"]; 65 | mbIsRGB = bIsRGB; 66 | 67 | const int bIsFisheye = fsSettings["Camera.Fisheye"]; 68 | mbIsFisheye = bIsFisheye; 69 | 70 | const int bEnableEqualizer = fsSettings["Tracker.EnableEqualizer"]; 71 | mbEnableEqualizer = bEnableEqualizer; 72 | 73 | mnMaxFeatsPerImage = fsSettings["Tracker.nFeatures"]; 74 | mnMaxFeatsForUpdate = std::ceil(.5*mnMaxFeatsPerImage); 75 | 76 | mvlTrackingHistory.resize(mnMaxFeatsPerImage); 77 | 78 | mnMaxTrackingLength = fsSettings["Tracker.nMaxTrackingLength"]; 79 | mnMinTrackingLength = fsSettings["Tracker.nMinTrackingLength"]; 80 | 81 | mbIsTheFirstImage = true; 82 | 83 | mLastImage = cv::Mat(); 84 | 85 | mpFeatureDetector = new FeatureDetector(fsSettings); 86 | mpRansac = new Ransac(fsSettings); 87 | 88 | mTrackPub = mTrackerNode.advertise("/rvio/track", 1); 89 | mNewerPub = mTrackerNode.advertise("/rvio/newer", 1); 90 | } 91 | 92 | 93 | Tracker::~Tracker() 94 | { 95 | delete mpFeatureDetector; 96 | delete mpRansac; 97 | } 98 | 99 | 100 | template 101 | void Tracker::UndistortAndNormalize(const int N, 102 | T1& src, 103 | T2& dst) 104 | { 105 | cv::Mat mat(N,2,CV_32F); 106 | 107 | // Get (u,v) from src 108 | for(int i=0; i(i,0) = src.at(i).x; 111 | mat.at(i,1) = src.at(i).y; 112 | } 113 | 114 | // Undistort and normalize (u,v) 115 | mat = mat.reshape(2); 116 | if (!mbIsFisheye) 117 | cv::undistortPoints(mat, mat, mK, mDistCoef); 118 | else 119 | cv::fisheye::undistortPoints(mat, mat, mK, mDistCoef); 120 | mat = mat.reshape(1); 121 | 122 | // Fill (u'/f,v'/f) to dst 123 | dst.clear(); 124 | for(int i=0; i(i,0); 128 | ptUN.y = mat.at(i,1); 129 | 130 | dst.push_back(ptUN); 131 | } 132 | } 133 | 134 | 135 | void Tracker::DisplayTrack(const cv::Mat& imIn, 136 | std::vector& vPoints1, 137 | std::vector& vPoints2, 138 | std::vector& vInlierFlag, 139 | cv_bridge::CvImage& imOut) 140 | { 141 | imOut.header = std_msgs::Header(); 142 | imOut.encoding = "bgr8"; 143 | 144 | cvtColor(imIn, imOut.image, CV_GRAY2BGR); 145 | 146 | for (int i=0; i<(int)vPoints1.size(); ++i) 147 | { 148 | if (vInlierFlag.at(i)!=0) 149 | { 150 | cv::circle(imOut.image, vPoints1.at(i), 3, blue, -1); 151 | cv::line(imOut.image, vPoints1.at(i), vPoints2.at(i), blue); 152 | } 153 | else 154 | { 155 | cv::circle(imOut.image, vPoints1.at(i), 3, red, 0); 156 | } 157 | } 158 | } 159 | 160 | 161 | void Tracker::DisplayNewer(const cv::Mat& imIn, 162 | std::vector& vFeats, 163 | std::deque& qNewFeats, 164 | cv_bridge::CvImage& imOut) 165 | { 166 | imOut.header = std_msgs::Header(); 167 | imOut.encoding = "bgr8"; 168 | 169 | cvtColor(imIn, imOut.image, CV_GRAY2BGR); 170 | 171 | for (int i=0; i<(int)vFeats.size(); ++i) 172 | cv::circle(imOut.image, vFeats.at(i), 3, blue, 0); 173 | 174 | for (int i=0; i<(int)qNewFeats.size(); ++i) 175 | cv::circle(imOut.image, qNewFeats.at(i), 3, green, -1); 176 | } 177 | 178 | 179 | void Tracker::track(const cv::Mat& im, 180 | std::list& lImuData) 181 | { 182 | // Convert to gray scale 183 | if (im.channels()==3) 184 | { 185 | if (mbIsRGB) 186 | cvtColor(im, im, CV_RGB2GRAY); 187 | else 188 | cvtColor(im, im, CV_BGR2GRAY); 189 | } 190 | else if (im.channels()==4) 191 | { 192 | if (mbIsRGB) 193 | cvtColor(im, im, CV_RGBA2GRAY); 194 | else 195 | cvtColor(im, im, CV_BGRA2GRAY); 196 | } 197 | 198 | if (mbEnableEqualizer) 199 | { 200 | cv::Ptr clahe = cv::createCLAHE(3.0, cv::Size(5,5)); 201 | clahe->apply(im, im); 202 | } 203 | 204 | if (mbIsTheFirstImage) 205 | { 206 | // Detect features 207 | mnFeatsToTrack = mpFeatureDetector->DetectWithSubPix(im, mnMaxFeatsPerImage, 1, mvFeatsToTrack); 208 | 209 | if (mnFeatsToTrack==0) 210 | { 211 | ROS_DEBUG("No features available to track."); 212 | return; 213 | } 214 | 215 | std::vector vFeatsUndistNorm; 216 | UndistortAndNormalize(mnFeatsToTrack, mvFeatsToTrack, vFeatsUndistNorm); 217 | 218 | mPoints1ForRansac.setZero(3,mnFeatsToTrack); 219 | for (int i=0; i vFeatsTracked; 240 | std::vector vInlierFlag; 241 | std::vector vTrackingError; 242 | 243 | // Lucas-Kanade method 244 | cv::calcOpticalFlowPyrLK(mLastImage, im, mvFeatsToTrack, vFeatsTracked, vInlierFlag, vTrackingError, winSize, 3, termCriteria, 0, 1e-3); 245 | 246 | if (vFeatsTracked.empty()) 247 | { 248 | ROS_DEBUG("No features tracked in current image."); 249 | return; 250 | } 251 | 252 | std::vector vFeatsUndistNorm; 253 | UndistortAndNormalize(mnFeatsToTrack, vFeatsTracked, vFeatsUndistNorm); 254 | 255 | mPoints2ForRansac.setZero(3,mnFeatsToTrack); 256 | for (int i=0; iFindInliers(mPoints1ForRansac, mPoints2ForRansac, lImuData, vInlierFlag); 265 | 266 | // Show the result in rviz 267 | cv_bridge::CvImage imTrack; 268 | DisplayTrack(im, mvFeatsToTrack, vFeatsTracked, vInlierFlag, imTrack); 269 | mTrackPub.publish(imTrack.toImageMsg()); 270 | 271 | // Prepare data for update 272 | mvFeatTypesForUpdate.clear(); 273 | mvlFeatMeasForUpdate.clear(); 274 | mvlFeatMeasForUpdate.resize(mnMaxFeatsForUpdate); 275 | 276 | mvFeatsToTrack.clear(); 277 | std::vector vInlierIndicesToTrack; 278 | Eigen::MatrixXd tempPointsForRansac(3,mnMaxFeatsPerImage); 279 | 280 | int nMeasCount = 0; 281 | int nInlierCount = 0; 282 | 283 | for (int i=0; i=mnMinTrackingLength) 292 | { 293 | if (nMeasCountmnMaxTrackingLength-(std::ceil(.5*mnMaxTrackingLength)-1)) 328 | mvlTrackingHistory.at(idx).pop_front(); 329 | 330 | nMeasCount++; 331 | } 332 | else 333 | mvlTrackingHistory.at(idx).pop_front(); 334 | } 335 | mvlTrackingHistory.at(idx).push_back(ptUN); 336 | 337 | Eigen::Vector3d ptUNe = Eigen::Vector3d(ptUN.x,ptUN.y,1); 338 | tempPointsForRansac.block(0,nInlierCount,3,1) = ptUNe; 339 | 340 | nInlierCount++; 341 | } 342 | } 343 | 344 | if (!mlFreeIndices.empty()) 345 | { 346 | // Feature refill 347 | std::vector vTempFeats; 348 | std::deque qNewFeats; 349 | 350 | mpFeatureDetector->DetectWithSubPix(im, mnMaxFeatsPerImage, 2, vTempFeats); 351 | int nNewFeats = mpFeatureDetector->FindNewer(vTempFeats, mvFeatsToTrack, qNewFeats); 352 | 353 | // Show the result in rviz 354 | cv_bridge::CvImage imNewer; 355 | DisplayNewer(im, vTempFeats, qNewFeats, imNewer); 356 | mNewerPub.publish(imNewer.toImageMsg()); 357 | 358 | if (nNewFeats!=0) 359 | { 360 | std::deque qNewFeatsUndistNorm; 361 | UndistortAndNormalize(nNewFeats, qNewFeats, qNewFeatsUndistNorm); 362 | 363 | for (;;) 364 | { 365 | int idx = mlFreeIndices.front(); 366 | vInlierIndicesToTrack.push_back(idx); 367 | 368 | cv::Point2f pt = qNewFeats.front(); 369 | mvFeatsToTrack.push_back(pt); 370 | 371 | cv::Point2f ptUN = qNewFeatsUndistNorm.front(); 372 | mvlTrackingHistory.at(idx).push_back(ptUN); 373 | 374 | Eigen::Vector3d ptUNe = Eigen::Vector3d(ptUN.x,ptUN.y,1); 375 | tempPointsForRansac.block(0,nInlierCount,3,1) = ptUNe; 376 | 377 | nInlierCount++; 378 | 379 | mlFreeIndices.pop_front(); 380 | qNewFeats.pop_front(); 381 | qNewFeatsUndistNorm.pop_front(); 382 | 383 | if (mlFreeIndices.empty() || qNewFeats.empty() || qNewFeatsUndistNorm.empty() || nInlierCount==mnMaxFeatsPerImage) 384 | break; 385 | } 386 | } 387 | } 388 | 389 | // Update tracker 390 | mnFeatsToTrack = nInlierCount; 391 | mvInlierIndices = vInlierIndicesToTrack; 392 | mPoints1ForRansac = tempPointsForRansac.block(0,0,3,nInlierCount); 393 | } 394 | 395 | im.copyTo(mLastImage); 396 | } 397 | 398 | } // namespace RVIO 399 | -------------------------------------------------------------------------------- /src/rvio/Tracker.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of R-VIO. 3 | * 4 | * Copyright (C) 2019 Zheng Huai and Guoquan Huang 5 | * For more information see 6 | * 7 | * R-VIO is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * R-VIO is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with R-VIO. If not, see . 19 | */ 20 | 21 | #ifndef TRACKER_H 22 | #define TRACKER_H 23 | 24 | #include 25 | #include 26 | #include 27 | 28 | #include 29 | 30 | #include 31 | 32 | #include 33 | #include 34 | 35 | #include "FeatureDetector.h" 36 | #include "Ransac.h" 37 | 38 | 39 | namespace RVIO 40 | { 41 | 42 | class Tracker 43 | { 44 | public: 45 | 46 | Tracker(const cv::FileStorage& fsSettings); 47 | 48 | ~Tracker(); 49 | 50 | void track(const cv::Mat& im, std::list& lImuData); 51 | 52 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 53 | 54 | private: 55 | 56 | template 57 | void UndistortAndNormalize(const int N, T1& src, T2& dst); 58 | 59 | void DisplayTrack(const cv::Mat& imIn, std::vector& vPoints1, std::vector& vPoints2, 60 | std::vector& vInlierFlag, cv_bridge::CvImage& imOut); 61 | 62 | void DisplayNewer(const cv::Mat& imIn, std::vector& vFeats, std::deque& qNewFeats, 63 | cv_bridge::CvImage& imOut); 64 | 65 | public: 66 | 67 | // Feature types for update 68 | // '1': lose track 69 | // '2': reach the max. tracking length 70 | std::vector mvFeatTypesForUpdate; 71 | 72 | // Feature measurements for update 73 | // Each list corresponds to a feature with its size the tracking length. 74 | std::vector > mvlFeatMeasForUpdate; 75 | 76 | private: 77 | 78 | int mnMaxFeatsPerImage; 79 | int mnMaxFeatsForUpdate; 80 | 81 | int mnMaxTrackingLength; 82 | int mnMinTrackingLength; 83 | 84 | bool mbIsRGB; 85 | bool mbIsFisheye; 86 | bool mbIsTheFirstImage; 87 | 88 | bool mbEnableEqualizer; 89 | 90 | // Intrinsics 91 | cv::Mat mK; 92 | cv::Mat mDistCoef; 93 | 94 | // Last image 95 | cv::Mat mLastImage; 96 | 97 | /** 98 | * Feature tracking history 99 | * 100 | * @each list number is a reusable feature index. 101 | * @each list corresponds to a feature with its size the tracking length. 102 | * @only store the undistorted and normalized coordinates, (u'/f,v'/f). 103 | */ 104 | std::vector > mvlTrackingHistory; 105 | 106 | // Indices of inliers 107 | std::vector mvInlierIndices; 108 | 109 | // Indices available 110 | std::list mlFreeIndices; 111 | 112 | // Features to track 113 | std::vector mvFeatsToTrack; 114 | int mnFeatsToTrack; 115 | 116 | // For RANSAC 117 | Eigen::MatrixXd mPoints1ForRansac; 118 | Eigen::MatrixXd mPoints2ForRansac; 119 | 120 | // Handlers 121 | FeatureDetector* mpFeatureDetector; 122 | Ransac* mpRansac; 123 | 124 | // Interact with rviz 125 | ros::NodeHandle mTrackerNode; 126 | ros::Publisher mTrackPub; 127 | ros::Publisher mNewerPub; 128 | }; 129 | 130 | } // namespace RVIO 131 | 132 | #endif 133 | -------------------------------------------------------------------------------- /src/rvio/Updater.cc: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of R-VIO. 3 | * 4 | * Copyright (C) 2019 Zheng Huai and Guoquan Huang 5 | * For more information see 6 | * 7 | * R-VIO is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * R-VIO is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with R-VIO. If not, see . 19 | */ 20 | 21 | #include 22 | 23 | #include 24 | 25 | #include 26 | 27 | #include "Updater.h" 28 | #include "../util/Numerics.h" 29 | 30 | 31 | namespace RVIO 32 | { 33 | 34 | static int cloud_id = 0; 35 | std_msgs::ColorRGBA colorLandmark; 36 | geometry_msgs::Vector3 scaleLandmark; 37 | 38 | Updater::Updater(const cv::FileStorage& fsSettings) 39 | { 40 | mnCamRate = fsSettings["Camera.fps"]; 41 | 42 | const float nImageNoiseSigmaX = fsSettings["Camera.sigma_px"]; 43 | const float nImageNoiseSigmaY = fsSettings["Camera.sigma_py"]; 44 | mnImageNoiseSigma = std::max(nImageNoiseSigmaX, nImageNoiseSigmaY); 45 | 46 | cv::Mat T(4,4,CV_32F); 47 | fsSettings["Camera.T_BC0"] >> T; 48 | Eigen::Matrix4d Tic; 49 | cv::cv2eigen(T,Tic); 50 | mRic = Tic.block<3,3>(0,0); 51 | mtic = Tic.block<3,1>(0,3); 52 | mRci = mRic.transpose(); 53 | mtci = -mRci*mtic; 54 | 55 | xk1k1.setZero(26,1); 56 | Pk1k1.setZero(24,24); 57 | 58 | mFeatPub = mUpdaterNode.advertise("/rvio/landmarks", 1); 59 | mnPubRate = fsSettings["Landmark.nPubRate"]; 60 | 61 | scaleLandmark.x = fsSettings["Landmark.nScale"]; 62 | scaleLandmark.y = fsSettings["Landmark.nScale"]; 63 | scaleLandmark.z = fsSettings["Landmark.nScale"]; 64 | 65 | colorLandmark.a = 1; 66 | colorLandmark.r = 0; 67 | colorLandmark.b = 1; 68 | colorLandmark.g = 0; 69 | } 70 | 71 | 72 | void Updater::update(Eigen::VectorXd& xk1k, 73 | Eigen::MatrixXd& Pk1k, 74 | std::vector& vFeatTypesForUpdate, 75 | std::vector >& vlFeatMeasForUpdate) 76 | { 77 | // Interact with ROS rviz 78 | visualization_msgs::Marker cloud; 79 | cloud.header.frame_id = "imu"; 80 | cloud.ns = "points"; 81 | cloud.id = ++cloud_id; 82 | cloud.color = colorLandmark; 83 | cloud.scale = scaleLandmark; 84 | cloud.pose.orientation.w = 1.0; 85 | cloud.lifetime = ros::Duration(1/mnPubRate); 86 | cloud.action = visualization_msgs::Marker::ADD; 87 | cloud.type = visualization_msgs::Marker::POINTS; 88 | 89 | // Number of features 90 | int nFeat = (int)vFeatTypesForUpdate.size(); 91 | 92 | // Number of (max) rows of matrices 93 | int nRows = 0; 94 | for (int i=0; i lFeatMeas = vlFeatMeasForUpdate.at(featIdx); 113 | 114 | int nTrackLength = (int)lFeatMeas.size(); 115 | int nTrackPhases = nTrackLength-1; 116 | int nRelPosesDim = 7*nTrackPhases; 117 | 118 | Eigen::VectorXd mRelPoses; 119 | if (featType=='1') 120 | mRelPoses = xk1k.tail(nRelPosesDim); 121 | else 122 | mRelPoses = xk1k.block(26,0,nRelPosesDim,1); 123 | 124 | // [qIi1,tIi1] 125 | Eigen::VectorXd mRelPosesToFirst(nRelPosesDim,1); 126 | mRelPosesToFirst.block(0,0,7,1) << mRelPoses.block(0,0,4,1), -QuatToRot(mRelPoses.block(0,0,4,1))*mRelPoses.block(4,0,3,1); 127 | for (int i=1; i.5*3.14 || fabs(psi)>.5*3.14) 155 | { 156 | ROS_DEBUG("Invalid inverse-depth feature estimate (0)!"); 157 | continue; 158 | } 159 | 160 | //====================================================================== 161 | // Feature estimate refinement 162 | // Use Levenberg–Marquardt (LM) algorithm with inverse-depth parameters. 163 | // Unit vector of pfinv 164 | Eigen::Vector3d epfinv; 165 | epfinv << cos(phi)*sin(psi), sin(phi), cos(phi)*cos(psi); 166 | 167 | // Jacobian of epfinv wrt. [phi,psi]' 168 | Eigen::Matrix Jang; 169 | Jang << -sin(phi)*sin(psi), cos(phi)*cos(psi), 170 | cos(phi), 0, 171 | -sin(phi)*cos(psi), -cos(phi)*sin(psi); 172 | 173 | Eigen::Matrix2d Rinv; 174 | Rinv << 1./pow(mnImageNoiseSigma,2), 0, 175 | 0, 1./pow(mnImageNoiseSigma,2); 176 | 177 | int maxIter = 10; 178 | double lambda = 0.01; 179 | double lastCost = std::numeric_limits::infinity(); 180 | 181 | for (int nIter=0; nIter Hproj1; 191 | Hproj1 << 1/h1(2), 0, -h1(0)/pow(h1(2),2), 192 | 0, 1/h1(2), -h1(1)/pow(h1(2),2); 193 | 194 | Eigen::Matrix H1; 195 | H1 << Hproj1*Jang, Eigen::Vector2d::Zero(); 196 | 197 | cv::Point2f pt1; 198 | pt1.x = h1(0)/h1(2); 199 | pt1.y = h1(1)/h1(2); 200 | 201 | Eigen::Vector2d e1; 202 | e1 << (ptFirst-pt1).x, (ptFirst-pt1).y; 203 | 204 | cost += e1.transpose()*Rinv*e1; 205 | HTRinvH.noalias() += H1.transpose()*Rinv*H1; 206 | HTRinve.noalias() += H1.transpose()*Rinv*e1; 207 | 208 | // The following measurements 209 | std::list::const_iterator lit = lFeatMeas.begin(); 210 | for (int i=0; i Hproj; 217 | Hproj << 1/h(2), 0, -h(0)/pow(h(2),2), 218 | 0, 1/h(2), -h(1)/pow(h(2),2); 219 | 220 | Eigen::Matrix H; 221 | H << Hproj*Rc*Jang, Hproj*tc; 222 | 223 | cv::Point2f pt; 224 | pt.x = h(0)/h(2); 225 | pt.y = h(1)/h(2); 226 | 227 | Eigen::Vector2d e; 228 | e << ((*lit)-pt).x, ((*lit)-pt).y; 229 | 230 | cost += e.transpose()*Rinv*e; 231 | HTRinvH.noalias() += H.transpose()*Rinv*H; 232 | HTRinve.noalias() += H.transpose()*Rinv*e; 233 | } 234 | 235 | if (cost<=lastCost) 236 | { 237 | // Down 238 | HTRinvH.diagonal() += lambda*HTRinvH.diagonal(); 239 | Eigen::Vector3d dpfinv = HTRinvH.colPivHouseholderQr().solve(HTRinve); 240 | 241 | phi += dpfinv(0); 242 | psi += dpfinv(1); 243 | rho += dpfinv(2); 244 | 245 | epfinv << cos(phi)*sin(psi), sin(phi), cos(phi)*cos(psi); 246 | 247 | Jang << -sin(phi)*sin(psi), cos(phi)*cos(psi), 248 | cos(phi), 0, 249 | -sin(phi)*cos(psi), -cos(phi)*sin(psi); 250 | 251 | if (fabs(lastCost-cost)<1e-6 && dpfinv(2)<1e-6) 252 | break; 253 | 254 | lambda *= .1; 255 | lastCost = cost; 256 | } 257 | else 258 | { 259 | // Up 260 | lambda *= 10; 261 | lastCost = cost; 262 | } 263 | } 264 | 265 | if (fabs(phi)>.5*3.14 || fabs(psi)>.5*3.14 || std::isinf(rho) || rho<0) 266 | { 267 | ROS_DEBUG("Invalid inverse-depth feature estimate (1)!"); 268 | continue; 269 | } 270 | 271 | if (featType=='2') 272 | { 273 | nTrackLength = std::ceil(.5*nTrackLength); 274 | nTrackPhases = nTrackLength-1; 275 | } 276 | 277 | 278 | //========================================== 279 | // Construct inverse-depth measurement model 280 | // i. Construct residual and Jacobians (the nonzero subblocks) 281 | Eigen::VectorXd tempr(2*nTrackLength,1); 282 | Eigen::MatrixXd tempHx(2*nTrackLength,6*nCloneStates); 283 | Eigen::MatrixXd tempHf(2*nTrackLength,3); 284 | tempr.setZero(); 285 | tempHx.setZero(); 286 | tempHf.setZero(); 287 | 288 | int nStartRow = 0; 289 | int nStartCol; 290 | if (featType=='1') 291 | nStartCol = 6*(nCloneStates-nTrackPhases); 292 | else 293 | nStartCol = 0; 294 | 295 | // For the 1st measurement 296 | Eigen::Vector3d h1 = epfinv; 297 | 298 | cv::Point2f pt1; 299 | pt1.x = h1(0)/h1(2); 300 | pt1.y = h1(1)/h1(2); 301 | 302 | Eigen::Matrix Hproj1; 303 | Hproj1 << 1/h1(2), 0, -h1(0)/pow(h1(2),2), 304 | 0, 1/h1(2), -h1(1)/pow(h1(2),2); 305 | 306 | // r 307 | cv::Point2f e1 = ptFirst-pt1; 308 | tempr.block(0,0,2,1) << e1.x, e1.y; 309 | 310 | // Hx: zero matrix 311 | 312 | // Hf 313 | Eigen::Matrix3d tempm = Eigen::Matrix3d::Zero(); 314 | tempm.block<3,2>(0,0) = Jang; 315 | tempHf.block<2,3>(0,0) = Hproj1*tempm; 316 | 317 | nStartRow += 2; 318 | 319 | // For the following measurements 320 | std::list::const_iterator lit = lFeatMeas.begin(); 321 | for (int i=1; i Hproj; 334 | Hproj << 1/h(2), 0, -h(0)/pow(h(2),2), 335 | 0, 1/h(2), -h(1)/pow(h(2),2); 336 | 337 | // r 338 | cv::Point2f e = (*lit)-pt; 339 | tempr.block(2*i,0,2,1) << e.x, e.y; 340 | 341 | // Hx: the 1st clone's subblock 342 | Eigen::Matrix3d R0T = QuatToRot(mRelPosesToFirst.block(0,0,4,1)).transpose(); 343 | Eigen::Vector3d t0 = mRelPosesToFirst.block(4,0,3,1); 344 | 345 | Eigen::Matrix3d dpx0 = SkewSymm(mRic*epfinv+rho*mtic+rho*R0T*t0); 346 | Eigen::Matrix subH; 347 | subH << dpx0*R0T, -rho*Eigen::Matrix3d::Identity(); 348 | 349 | tempHx.block(nStartRow,nStartCol,2,6) = Hproj*mRci*R*subH; 350 | 351 | // Hx: the following clones' subblocks 352 | for (int j=1; j tempHf_GR; 382 | 383 | for (int n=0; nn; m--) 386 | { 387 | // Givens matrix G 388 | tempHf_GR.makeGivens(tempHf(m-1,n), tempHf(m,n)); 389 | 390 | // Multiply G' to the corresponding lines (m-1,m) in each matrix 391 | // Note: we only apply G' to the nonzero cols [n:N-1], which is 392 | // equivalent to applying G' to the entire row [0:N-1]. 393 | // G'*Hf 394 | (tempHf.block(m-1,n,2,N-n)).applyOnTheLeft(0,1,tempHf_GR.adjoint()); 395 | 396 | // G'*Hx 397 | (tempHx.block(m-1,0,2,tempHx.cols())).applyOnTheLeft(0,1,tempHf_GR.adjoint()); 398 | 399 | // G'*r 400 | (tempr.block(m-1,0,2,1)).applyOnTheLeft(0,1,tempHf_GR.adjoint()); 401 | } 402 | } 403 | 404 | // iii. Mahalanobis distance test 405 | // Note: this tests the dissimilarity between the measurement and the estimate, 406 | // D=r'*Sinv*r, where Sinv*r is the solution of Sx=r, and S=H*P*H'+R. 407 | int nDOF = M-N; 408 | Eigen::VectorXd tempr_ = tempr.block(N,0,nDOF,1); 409 | Eigen::MatrixXd tempHx_ = tempHx.block(N,0,nDOF,tempHx.cols()); 410 | 411 | Eigen::VectorXd tempR; 412 | tempR.setOnes(nDOF,1); 413 | tempR *= pow(mnImageNoiseSigma, 2); 414 | 415 | Eigen::MatrixXd tempS; 416 | tempS = tempHx_*Pk1k.block(24,24,6*nCloneStates,6*nCloneStates)*(tempHx_.transpose()); 417 | tempS.diagonal() += tempR; 418 | tempS = .5*(tempS+tempS.transpose()); 419 | 420 | double nMahalanobisDist = (tempr_.transpose()*(tempS.colPivHouseholderQr().solve(tempr_))).norm(); 421 | 422 | if (nMahalanobisDist0) 431 | { 432 | // Feature visualization (rviz) 433 | // Note: pf is in the current reference frame {Rk}. 434 | Eigen::VectorXd posek = mRelPosesToFirst.tail(7); 435 | Eigen::Matrix3d Rk = QuatToRot(posek.head(4)); 436 | Eigen::Vector3d tk = posek.tail(3); 437 | 438 | // pf in {C1}->{R1}->{Rk} 439 | Eigen::Vector3d pfc = 1/rho*epfinv; 440 | Eigen::Vector3d pf1 = mRic*pfc+mtic; 441 | Eigen::Vector3d pfk = Rk*pf1+tk; 442 | 443 | geometry_msgs::Point feat; 444 | feat.x = pfk(0); 445 | feat.y = pfk(1); 446 | feat.z = pfk(2); 447 | cloud.points.push_back(feat); 448 | } 449 | } 450 | else 451 | { 452 | ROS_DEBUG("Failed in Mahalanobis distance test!"); 453 | continue; 454 | } 455 | } 456 | 457 | // Visualize features in rviz 458 | mFeatPub.publish(cloud); 459 | 460 | if (nGoodFeatCount>2) 461 | { 462 | Eigen::VectorXd ro = r.block(0,0,nRowCount,1); 463 | Eigen::MatrixXd Ho = Hx.block(0,0,nRowCount,Hx.cols()); 464 | 465 | Eigen::VectorXd Ro; 466 | Ro.setOnes(nRowCount,1); 467 | Ro *= pow(mnImageNoiseSigma, 2); 468 | 469 | // Model compression 470 | Eigen::VectorXd rn; 471 | Eigen::MatrixXd Hn; 472 | Eigen::VectorXd Rn; 473 | 474 | if (Ho.rows()>Ho.cols()-24) 475 | { 476 | // If Hw is a tall matrix 477 | int M = Ho.rows(); 478 | int N = Ho.cols()-24; 479 | 480 | Eigen::MatrixXd tempHw = Ho.block(0,24,M,N); 481 | 482 | for (int i=N; i>0; i--) 483 | { 484 | if (tempHw.col(i-1).norm()==0) 485 | { 486 | ROS_DEBUG("Hw is rank deficient!"); 487 | N--; 488 | } 489 | else 490 | break; 491 | } 492 | 493 | // Use Givens rotations (QR) 494 | Eigen::JacobiRotation tempHw_GR; 495 | 496 | for (int n=0; nn; m--) 499 | { 500 | // Givens matrix G 501 | tempHw_GR.makeGivens(tempHw(m-1,n), tempHw(m,n)); 502 | 503 | // Multiply G' to the corresponding lines (m-1,m) in each matrix 504 | // Note: we only apply G' to the nonzero cols [n:N-1], which is 505 | // equivalent to applying G' to the entire row [0:N-1]. 506 | // G'*H 507 | (tempHw.block(m-1,n,2,N-n)).applyOnTheLeft(0,1,tempHw_GR.adjoint()); 508 | 509 | // G'*r 510 | (ro.block(m-1,0,2,1)).applyOnTheLeft(0,1,tempHw_GR.adjoint()); 511 | } 512 | } 513 | 514 | Ho.block(0,24,M,N) = tempHw.block(0,0,M,N); 515 | 516 | int nRank = 0; 517 | for (int i=0; i and Guoquan Huang 5 | * For more information see 6 | * 7 | * R-VIO is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * R-VIO is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with R-VIO. If not, see . 19 | */ 20 | 21 | #ifndef UPDATER_H 22 | #define UPDATER_H 23 | 24 | #include 25 | #include 26 | 27 | #include 28 | 29 | #include 30 | 31 | #include 32 | 33 | 34 | namespace RVIO 35 | { 36 | 37 | class Updater 38 | { 39 | public: 40 | 41 | Updater(const cv::FileStorage& fsSettings); 42 | 43 | void update(Eigen::VectorXd& xk1k, Eigen::MatrixXd& Pk1k, std::vector& vFeatTypesForUpdate, 44 | std::vector >& vlFeatMeasForUpdate); 45 | 46 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 47 | 48 | public: 49 | 50 | // Outputs 51 | Eigen::VectorXd xk1k1; 52 | Eigen::MatrixXd Pk1k1; 53 | 54 | private: 55 | 56 | double mnCamRate; 57 | 58 | // Sigma{pixel} 59 | double mnImageNoiseSigma; 60 | 61 | // Extrinsics 62 | Eigen::Matrix3d mRic; 63 | Eigen::Vector3d mtic; 64 | Eigen::Matrix3d mRci; 65 | Eigen::Vector3d mtci; 66 | 67 | // Interact with rviz 68 | ros::NodeHandle mUpdaterNode; 69 | ros::Publisher mFeatPub; 70 | double mnPubRate; 71 | }; 72 | 73 | } // namespace RVIO 74 | 75 | #endif 76 | -------------------------------------------------------------------------------- /src/rvio_mono.cc: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of R-VIO. 3 | * 4 | * Copyright (C) 2019 Zheng Huai and Guoquan Huang 5 | * For more information see 6 | * 7 | * R-VIO is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * R-VIO is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with R-VIO. If not, see . 19 | */ 20 | 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | #include 28 | 29 | #include "rvio/System.h" 30 | 31 | 32 | class ImageGrabber 33 | { 34 | public: 35 | ImageGrabber(RVIO::System* pSys) : mpSys(pSys) {} 36 | 37 | void GrabImage(const sensor_msgs::ImageConstPtr& msg); 38 | 39 | RVIO::System* mpSys; 40 | }; 41 | 42 | 43 | class ImuGrabber 44 | { 45 | public: 46 | ImuGrabber(RVIO::System* pSys) : mpSys(pSys) {} 47 | 48 | void GrabImu(const sensor_msgs::ImuConstPtr& msg); 49 | 50 | RVIO::System* mpSys; 51 | }; 52 | 53 | 54 | void ImageGrabber::GrabImage(const sensor_msgs::ImageConstPtr& msg) 55 | { 56 | static int lastseq = -1; 57 | if ((int)msg->header.seq!=lastseq+1 && lastseq!=-1) 58 | ROS_DEBUG("Image message drop! curr seq: %d expected seq: %d.", msg->header.seq, lastseq+1); 59 | lastseq = msg->header.seq; 60 | 61 | cv_bridge::CvImageConstPtr cv_ptr; 62 | try 63 | { 64 | cv_ptr = cv_bridge::toCvShare(msg, sensor_msgs::image_encodings::MONO8); 65 | } 66 | catch (cv_bridge::Exception& e) 67 | { 68 | ROS_ERROR("cv_bridge exception: %s", e.what()); 69 | return; 70 | } 71 | 72 | RVIO::ImageData* pData = new RVIO::ImageData(); 73 | pData->Image = cv_ptr->image.clone(); 74 | pData->Timestamp = cv_ptr->header.stamp.toSec(); 75 | 76 | mpSys->PushImageData(pData); 77 | 78 | mpSys->MonoVIO(); 79 | } 80 | 81 | 82 | void ImuGrabber::GrabImu(const sensor_msgs::ImuConstPtr& msg) 83 | { 84 | static int lastseq = -1; 85 | if ((int) msg->header.seq!=lastseq+1 && lastseq!=-1) 86 | ROS_DEBUG("IMU message drop! curr seq: %d expected seq: %d.", msg->header.seq, lastseq+1); 87 | lastseq = msg->header.seq; 88 | 89 | Eigen::Vector3d angular_velocity; 90 | tf::vectorMsgToEigen(msg->angular_velocity, angular_velocity); 91 | 92 | Eigen::Vector3d linear_acceleration; 93 | tf::vectorMsgToEigen(msg->linear_acceleration, linear_acceleration); 94 | 95 | double currtime = msg->header.stamp.toSec(); 96 | 97 | RVIO::ImuData* pData = new RVIO::ImuData(); 98 | pData->AngularVel = angular_velocity; 99 | pData->LinearAccel = linear_acceleration; 100 | pData->Timestamp = currtime; 101 | 102 | static double lasttime = -1; 103 | if (lasttime!=-1) 104 | pData->TimeInterval = currtime-lasttime; 105 | else 106 | pData->TimeInterval = 0; 107 | lasttime = currtime; 108 | 109 | mpSys->PushImuData(pData); 110 | } 111 | 112 | 113 | int main(int argc, char **argv) 114 | { 115 | ros::init(argc, argv, "rvio"); 116 | ros::start(); 117 | 118 | RVIO::System Sys(argv[1]); 119 | 120 | ImageGrabber igb1(&Sys); 121 | ImuGrabber igb2(&Sys); 122 | 123 | ros::NodeHandle nodeHandler; 124 | ros::Subscriber image_sub = nodeHandler.subscribe("/camera/image_raw", 10, &ImageGrabber::GrabImage, &igb1); 125 | ros::Subscriber imu_sub = nodeHandler.subscribe("/imu", 100, &ImuGrabber::GrabImu, &igb2); 126 | 127 | ros::spin(); 128 | 129 | ros::shutdown(); 130 | 131 | return 0; 132 | } 133 | -------------------------------------------------------------------------------- /src/util/Numerics.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of R-VIO. 3 | * 4 | * Copyright (C) 2019 Zheng Huai and Guoquan Huang 5 | * For more information see 6 | * 7 | * R-VIO is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * R-VIO is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with R-VIO. If not, see . 19 | */ 20 | 21 | #ifndef NUMERICS_H 22 | #define NUMERICS_H 23 | 24 | #include 25 | 26 | 27 | /** 28 | * Quaternion multiplication 29 | */ 30 | inline Eigen::Vector4d QuatMul(const Eigen::Vector4d& q1, 31 | const Eigen::Vector4d& q2) 32 | { 33 | Eigen::Matrix4d mat; 34 | 35 | mat(0,0) = q1(3,0); 36 | mat(0,1) = q1(2,0); 37 | mat(0,2) = -q1(1,0); 38 | mat(0,3) = q1(0,0); 39 | 40 | mat(1,0) = -q1(2,0); 41 | mat(1,1) = q1(3,0); 42 | mat(1,2) = q1(0,0); 43 | mat(1,3) = q1(1,0); 44 | 45 | mat(2,0) = q1(1,0); 46 | mat(2,1) = -q1(0,0); 47 | mat(2,2) = q1(3,0); 48 | mat(2,3) = q1(2,0); 49 | 50 | mat(3,0) = -q1(0,0); 51 | mat(3,1) = -q1(1,0); 52 | mat(3,2) = -q1(2,0); 53 | mat(3,3) = q1(3,0); 54 | 55 | Eigen::Vector4d q = mat*q2; 56 | 57 | q.normalize(); 58 | 59 | if (q(3)<0) 60 | q = -q; 61 | 62 | return q; 63 | } 64 | 65 | 66 | /** 67 | * Quaternion inverse 68 | */ 69 | inline Eigen::Vector4d QuatInv(const Eigen::Vector4d& q) 70 | { 71 | Eigen::Vector4d q_inv; 72 | 73 | if (q(3)>0) 74 | { 75 | q_inv(0) = -q(0); 76 | q_inv(1) = -q(1); 77 | q_inv(2) = -q(2); 78 | q_inv(3) = q(3); 79 | } 80 | else 81 | { 82 | q_inv(0) = q(0); 83 | q_inv(1) = q(1); 84 | q_inv(2) = q(2); 85 | q_inv(3) = -q(3); 86 | } 87 | 88 | q_inv.normalize(); 89 | 90 | return q_inv; 91 | } 92 | 93 | 94 | /** 95 | * Construct 3x3 Skew-symmetric matrix 96 | */ 97 | inline Eigen::Matrix3d SkewSymm(const Eigen::Vector3d& w) 98 | { 99 | Eigen::Matrix3d mat; 100 | mat << 0, -w(2), w(1), 101 | w(2), 0, -w(0), 102 | -w(1), w(0), 0; 103 | 104 | return mat; 105 | } 106 | 107 | 108 | /** 109 | * Convert quaternion to rotation matrix (orthonormal) 110 | */ 111 | inline Eigen::Matrix3d QuatToRot(const Eigen::Vector4d& q) 112 | { 113 | Eigen::Matrix3d qx = SkewSymm(q.block<3, 1>(0,0)); 114 | Eigen::Matrix3d I = Eigen::Matrix3d::Identity(); 115 | 116 | Eigen::Matrix3d mat; 117 | mat = I-2*q(3)*qx+2*qx*qx; 118 | 119 | return mat; 120 | } 121 | 122 | 123 | /** 124 | * Convert rotation matrix to quaternion according to the JPL procedure (Breckenridge Memo) 125 | */ 126 | inline Eigen::Vector4d RotToQuat(const Eigen::MatrixXd& R) 127 | { 128 | Eigen::Vector4d q; 129 | 130 | double T = R.trace(); 131 | 132 | if ((R(0,0)>T)&&(R(0,0)>R(1,1))&&(R(0,0)>R(2,2))) 133 | { 134 | q(0) = sqrt((1+(2*R(0,0))-T)/4); 135 | q(1) = (1/(4*q(0)))*(R(0,1)+R(1,0)); 136 | q(2) = (1/(4*q(0)))*(R(0,2)+R(2,0)); 137 | q(3) = (1/(4*q(0)))*(R(1,2)-R(2,1)); 138 | } 139 | else if ((R(1,1)>T)&&(R(1,1)>R(0,0))&&(R(1,1)>R(2,2))) 140 | { 141 | q(1) = sqrt((1+(2*R(1,1))-T)/4); 142 | q(0) = (1/(4*q(1)))*(R(0,1)+R(1,0)); 143 | q(2) = (1/(4*q(1)))*(R(1,2)+R(2,1)); 144 | q(3) = (1/(4*q(1)))*(R(2,0)-R(0,2)); 145 | } 146 | else if ((R(2,2)>T)&&(R(2,2)>R(0,0))&&(R(2,2)>R(1,1))) 147 | { 148 | q(2) = sqrt((1+(2*R(2,2))-T)/4); 149 | q(0) = (1/(4*q(2)))*(R(0,2)+R(2,0)); 150 | q(1) = (1/(4*q(2)))*(R(1,2)+R(2,1)); 151 | q(3) = (1/(4*q(2)))*(R(0,1)-R(1,0)); 152 | } 153 | else 154 | { 155 | q(3) = sqrt((1+T)/4); 156 | q(0) = (1/(4*q(3)))*(R(1,2)-R(2,1)); 157 | q(1) = (1/(4*q(3)))*(R(2,0)-R(0,2)); 158 | q(2) = (1/(4*q(3)))*(R(0,1)-R(1,0)); 159 | } 160 | 161 | q.normalize(); 162 | 163 | if (q(3)<0) 164 | q = -q; 165 | 166 | return q; 167 | } 168 | 169 | 170 | /** 171 | * Chi-square thresholds based on the DOF of state (chi2(0.95,DOF)) 172 | */ 173 | const double CHI_THRESHOLD[500] = { 174 | 3.841459, 5.991465, 7.814728, 9.487729, 11.070498, 12.591587, 14.067140, 15.507313, 16.918978, 18.307038, 175 | 19.675138, 21.026070, 22.362032, 23.684791, 24.995790, 26.296228, 27.587112, 28.869299, 30.143527, 31.410433, 176 | 32.670573, 33.924438, 35.172462, 36.415029, 37.652484, 38.885139, 40.113272, 41.337138, 42.556968, 43.772972, 177 | 44.985343, 46.194260, 47.399884, 48.602367, 49.801850, 50.998460, 52.192320, 53.383541, 54.572228, 55.758479, 178 | 56.942387, 58.124038, 59.303512, 60.480887, 61.656233, 62.829620, 64.001112, 65.170769, 66.338649, 67.504807, 179 | 68.669294, 69.832160, 70.993453, 72.153216, 73.311493, 74.468324, 75.623748, 76.777803, 77.930524, 79.081944, 180 | 80.232098, 81.381015, 82.528727, 83.675261, 84.820645, 85.964907, 87.108072, 88.250164, 89.391208, 90.531225, 181 | 91.670239, 92.808270, 93.945340, 95.081467, 96.216671, 97.350970, 98.484383, 99.616927, 100.748619, 101.879474, 182 | 103.009509, 104.138738, 105.267177, 106.394840, 107.521741, 108.647893, 109.773309, 110.898003, 112.021986, 113.145270, 183 | 114.267868, 115.389790, 116.511047, 117.631651, 118.751612, 119.870939, 120.989644, 122.107735, 123.225221, 124.342113, 184 | 125.458419, 126.574148, 127.689308, 128.803908, 129.917955, 131.031458, 132.144425, 133.256862, 134.368777, 135.480178, 185 | 136.591071, 137.701464, 138.811363, 139.920774, 141.029704, 142.138160, 143.246147, 144.353672, 145.460740, 146.567358, 186 | 147.673530, 148.779262, 149.884561, 150.989430, 152.093876, 153.197903, 154.301516, 155.404721, 156.507522, 157.609923, 187 | 158.711930, 159.813547, 160.914778, 162.015628, 163.116101, 164.216201, 165.315932, 166.415299, 167.514305, 168.612954, 188 | 169.711251, 170.809198, 171.906799, 173.004059, 174.100981, 175.197567, 176.293823, 177.389750, 178.485353, 179.580634, 189 | 180.675597, 181.770246, 182.864582, 183.958610, 185.052332, 186.145751, 187.238870, 188.331692, 189.424220, 190.516457, 190 | 191.608404, 192.700066, 193.791445, 194.882542, 195.973362, 197.063906, 198.154177, 199.244177, 200.333909, 201.423375, 191 | 202.512577, 203.601519, 204.690201, 205.778627, 206.866798, 207.954717, 209.042386, 210.129807, 211.216982, 212.303913, 192 | 213.390602, 214.477052, 215.563263, 216.649239, 217.734981, 218.820491, 219.905770, 220.990822, 222.075646, 223.160247, 193 | 224.244624, 225.328780, 226.412716, 227.496435, 228.579938, 229.663226, 230.746302, 231.829167, 232.911822, 233.994269, 194 | 235.076510, 236.158546, 237.240378, 238.322009, 239.403439, 240.484671, 241.565705, 242.646544, 243.727187, 244.807638, 195 | 245.887897, 246.967965, 248.047844, 249.127536, 250.207041, 251.286361, 252.365498, 253.444451, 254.523224, 255.601816, 196 | 256.680230, 257.758465, 258.836525, 259.914409, 260.992120, 262.069657, 263.147023, 264.224218, 265.301243, 266.378101, 197 | 267.454791, 268.531314, 269.607673, 270.683868, 271.759900, 272.835769, 273.911478, 274.987027, 276.062417, 277.137650, 198 | 278.212725, 279.287644, 280.362409, 281.437019, 282.511477, 283.585782, 284.659936, 285.733940, 286.807794, 287.881501, 199 | 288.955059, 290.028471, 291.101737, 292.174858, 293.247835, 294.320669, 295.393360, 296.465910, 297.538319, 298.610588, 200 | 299.682719, 300.754710, 301.826565, 302.898282, 303.969864, 305.041310, 306.112622, 307.183800, 308.254846, 309.325759, 201 | 310.396541, 311.467192, 312.537713, 313.608105, 314.678368, 315.748503, 316.818512, 317.888393, 318.958149, 320.027780, 202 | 321.097286, 322.166669, 323.235928, 324.305065, 325.374080, 326.442974, 327.511748, 328.580401, 329.648936, 330.717351, 203 | 331.785649, 332.853829, 333.921892, 334.989839, 336.057670, 337.125386, 338.192988, 339.260476, 340.327850, 341.395112, 204 | 342.462262, 343.529300, 344.596226, 345.663043, 346.729749, 347.796346, 348.862834, 349.929214, 350.995485, 352.061650, 205 | 353.127708, 354.193659, 355.259504, 356.325245, 357.390880, 358.456412, 359.521839, 360.587163, 361.652385, 362.717504, 206 | 363.782521, 364.847437, 365.912253, 366.976967, 368.041582, 369.106097, 370.170513, 371.234831, 372.299051, 373.363173, 207 | 374.427197, 375.491125, 376.554957, 377.618692, 378.682332, 379.745878, 380.809328, 381.872684, 382.935947, 383.999116, 208 | 385.062192, 386.125175, 387.188067, 388.250867, 389.313575, 390.376192, 391.438719, 392.501156, 393.563503, 394.625760, 209 | 395.687929, 396.750009, 397.812000, 398.873904, 399.935720, 400.997450, 402.059092, 403.120648, 404.182118, 405.243502, 210 | 406.304801, 407.366015, 408.427145, 409.488190, 410.549151, 411.610029, 412.670823, 413.731535, 414.792164, 415.852711, 211 | 416.913176, 417.973559, 419.033862, 420.094083, 421.154224, 422.214284, 423.274265, 424.334166, 425.393988, 426.453731, 212 | 427.513395, 428.572980, 429.632488, 430.691918, 431.751271, 432.810546, 433.869745, 434.928867, 435.987913, 437.046882, 213 | 438.105777, 439.164596, 440.223339, 441.282008, 442.340603, 443.399123, 444.457570, 445.515942, 446.574242, 447.632468, 214 | 448.690621, 449.748702, 450.806711, 451.864647, 452.922512, 453.980305, 455.038027, 456.095679, 457.153259, 458.210769, 215 | 459.268209, 460.325579, 461.382879, 462.440110, 463.497272, 464.554365, 465.611389, 466.668344, 467.725232, 468.782052, 216 | 469.838804, 470.895488, 471.952105, 473.008656, 474.065139, 475.121556, 476.177907, 477.234192, 478.290411, 479.346565, 217 | 480.402653, 481.458676, 482.514634, 483.570528, 484.626357, 485.682122, 486.737823, 487.793460, 488.849033, 489.904544, 218 | 490.959991, 492.015375, 493.070697, 494.125956, 495.181153, 496.236287, 497.291360, 498.346372, 499.401322, 500.456210, 219 | 501.511038, 502.565805, 503.620511, 504.675157, 505.729742, 506.784268, 507.838733, 508.893140, 509.947486, 511.001774, 220 | 512.056002, 513.110172, 514.164283, 515.218335, 516.272329, 517.326265, 518.380143, 519.433964, 520.487727, 521.541432, 221 | 522.595081, 523.648672, 524.702207, 525.755685, 526.809107, 527.862472, 528.915781, 529.969035, 531.022232, 532.075374, 222 | 533.128461, 534.181492, 535.234469, 536.287390, 537.340257, 538.393069, 539.445827, 540.498531, 541.551181, 542.603777, 223 | 543.656319, 544.708807, 545.761243, 546.813625, 547.865954, 548.918230, 549.970453, 551.022624, 552.074743, 553.126809 224 | }; 225 | 226 | #endif 227 | --------------------------------------------------------------------------------