Repository: ChangwuLiu/InGVIO Branch: master_github Commit: eca739e894c6 Files: 144 Total size: 1019.3 KB Directory structure: gitextract_o7z2p0ui/ ├── .gitignore ├── README.md ├── camera_model/ │ ├── CMakeLists.txt │ ├── LICENSE │ ├── include/ │ │ ├── MeiCamera.h │ │ ├── PinholeCamera.h │ │ ├── camera.h │ │ └── camera_factory.h │ ├── package.xml │ ├── src/ │ │ ├── MeiCamera.cpp │ │ ├── PinholeCamera.cpp │ │ ├── camera.cpp │ │ └── camera_factory.cpp │ └── test/ │ └── CMakeLists.txt ├── config/ │ ├── fw_zed2i_f9p/ │ │ ├── ingvio_mono.yaml │ │ ├── ingvio_stereo.yaml │ │ ├── mono_config.yaml │ │ ├── stereo_left_config.yaml │ │ └── stereo_right_config.yaml │ └── sportsfield/ │ ├── ingvio_mono.yaml │ ├── ingvio_stereo.yaml │ ├── mono_config.yaml │ ├── stereo_left_config.yaml │ └── stereo_right_config.yaml ├── feature_tracker/ │ ├── CMakeLists.txt │ ├── LICENSE │ ├── include/ │ │ ├── Color.h │ │ ├── mono_parameters.h │ │ ├── mono_tracker.h │ │ ├── random_pair.hpp │ │ ├── stereo_parameters.h │ │ ├── stereo_tracker.h │ │ └── tic_toc.hpp │ ├── launch/ │ │ └── sportsfield/ │ │ ├── mono_tracker_sf.launch │ │ └── stereo_tracker_sf.launch │ ├── msg/ │ │ ├── MonoFrame.msg │ │ ├── MonoMeas.msg │ │ ├── StereoFrame.msg │ │ └── StereoMeas.msg │ ├── package.xml │ └── src/ │ ├── mono_parameters.cpp │ ├── mono_tracker.cpp │ ├── mono_tracker_node.cpp │ ├── stereo_parameters.cpp │ ├── stereo_tracker.cpp │ └── stereo_tracker_node.cpp ├── gnss_comm/ │ ├── .gitignore │ ├── CMakeLists.txt │ ├── LICENSE │ ├── README.md │ ├── cmake/ │ │ ├── FindEigen.cmake │ │ └── FindGlog.cmake │ ├── docker/ │ │ ├── Dockerfile │ │ └── Makefile │ ├── include/ │ │ └── gnss_comm/ │ │ ├── gnss_constant.hpp │ │ ├── gnss_ros.hpp │ │ ├── gnss_spp.hpp │ │ ├── gnss_utility.hpp │ │ └── rinex_helper.hpp │ ├── msg/ │ │ ├── GnssBestXYZMsg.msg │ │ ├── GnssEphemMsg.msg │ │ ├── GnssGloEphemMsg.msg │ │ ├── GnssMeasMsg.msg │ │ ├── GnssObsMsg.msg │ │ ├── GnssPVTSolnMsg.msg │ │ ├── GnssSvsMsg.msg │ │ ├── GnssTimeMsg.msg │ │ ├── GnssTimePulseInfoMsg.msg │ │ └── StampedFloat64Array.msg │ ├── package.xml │ └── src/ │ ├── gnss_ros.cpp │ ├── gnss_spp.cpp │ ├── gnss_utility.cpp │ └── rinex_helper.cpp └── ingvio_estimator/ ├── CMakeLists.txt ├── LICENSE ├── README.md ├── cmake/ │ ├── FindSuiteSparse.cmake │ └── ROS1.cmake ├── launch/ │ ├── ingvio_mono_fw.launch │ ├── ingvio_mono_fw_record.launch │ ├── ingvio_mono_sf.launch │ ├── ingvio_mono_sf_record.launch │ ├── ingvio_stereo_fw.launch │ ├── ingvio_stereo_fw_record.launch │ ├── ingvio_stereo_sf.launch │ └── ingvio_stereo_sf_record.launch ├── package.xml ├── rviz/ │ ├── ingvio_mono.rviz │ └── ingvio_stereo.rviz ├── src/ │ ├── AnchoredLandmark.cpp │ ├── AnchoredLandmark.h │ ├── AuxGammaFunc.cpp │ ├── AuxGammaFunc.h │ ├── Color.h │ ├── GnssData.cpp │ ├── GnssData.h │ ├── GnssManager.cpp │ ├── GnssManager.h │ ├── GnssProcessor.cpp │ ├── GnssSync.cpp │ ├── GnssSync.h │ ├── GnssUpdate.cpp │ ├── GnssUpdate.h │ ├── GvioAligner.cpp │ ├── GvioAligner.h │ ├── ImuPropagator.cpp │ ├── ImuPropagator.h │ ├── IngvioFilter.cpp │ ├── IngvioFilter.h │ ├── IngvioNode.cpp │ ├── IngvioParams.cpp │ ├── IngvioParams.h │ ├── KeyframeUpdate.cpp │ ├── KeyframeUpdate.h │ ├── LandmarkUpdate.cpp │ ├── LandmarkUpdate.h │ ├── MapServer.cpp │ ├── MapServer.h │ ├── MapServerManager.cpp │ ├── MapServerManager.h │ ├── PoseState.cpp │ ├── PoseState.h │ ├── RemoveLostUpdate.cpp │ ├── RemoveLostUpdate.h │ ├── State.cpp │ ├── State.h │ ├── StateManager.cpp │ ├── StateManager.h │ ├── SwMargUpdate.cpp │ ├── SwMargUpdate.h │ ├── TicToc.h │ ├── Triangulator.cpp │ ├── Triangulator.h │ ├── Update.cpp │ ├── Update.h │ ├── VecState.cpp │ └── VecState.h └── test/ ├── GenerateNoise.cpp ├── GenerateNoise.h ├── TestMapServer.cpp ├── TestPropagator.cpp ├── TestStateManager.cpp └── TestTriangulator.cpp ================================================ FILE CONTENTS ================================================ ================================================ FILE: .gitignore ================================================ *.kdev4 ================================================ FILE: README.md ================================================ # InGVIO An invariant filter for visual-inertial-raw GNSS fusion. **Paper:** InGVIO: A Consistent Invariant Filter For Fast and High-Accuracy GNSS-Visual-Inertial Odometry **Paper Author:** Changwu Liu, Chen Jiang and Haowen Wang **Paper Status:** Manuscript submitted to RA-L for possible publication. Preprint version available on ArXiv. **Current Paper Link:** https://arxiv.org/abs/2210.15145 InGVIO is an invariant filter approach for fusion of monocular/stereo camera, IMU and raw GNSS measurements including pseudo ranges and Doppler shifts. InGVIO is intrinsically consistent under conditional infinitesimal invariance of the GNSS-Visual-Inertial system. InGVIO has the following key features: (a) fast due to decoupled IMU propagation, key-frame marginalization strategy and no SLAM-features; (b) accurate due to intrinsic consistency maintenance; (c) better convergence properties than 'naive' EKF-based filters. Moreover, we offer our fixed-wing datasets in the form of ROS Bags including stereo-visual, IMU and raw-GNSS measurements. **Fixed-Wing Dataset Link:** https://cloud.tsinghua.edu.cn/d/65ecf6768563421faaed/ password to extract: LCW@uav_18 **Fixed-Wing Dataset Videos:** The videos are uploaded to the link that stores the fixed-wing datasets. The links to the datasets will be continuously updated. The config files for this dataset are contained in the InGVIO code configuration in path 'config/fw_zed2i_f9p'. ## 1. System Requirements ### 1.1 Support of C++ 14 Features The compiler should at least support c++14 standards. ### 1.2 ROS-Noetic System InGVIO is developed under [ROS-Noetic](http://wiki.ros.org/noetic/Installation/Ubuntu) with its default OpenCV4 library. However, InGVIO should be working on ROS-Melodic with OpenCV3. In the future, we may add support to ROS 2. ### 1.3 Eigen Library Eigen is a fantastic matrix computation library. InGVIO is developed under [Eigen3.3.7](https://eigen.tuxfamily.org/index.php?title=Main_Page). Other Eigen 3 versions should be OK for InGVIO. ### 1.4 SuiteSparse Library We use [SuiteSparse](https://github.com/DrTimothyAldenDavis/SuiteSparse/releases) Library for sparse QR-decomposition in visual updates. ### 1.5 gnss_comm Library A wrapper for GNSS messages in ROS. See [gnss_comm](https://github.com/HKUST-Aerial-Robotics/gnss_comm). The fantastic optimization-based work [GVINS](https://github.com/HKUST-Aerial-Robotics/GVINS) also relies on this library. We reserve a copy of gnss_comm in this repo. ## 2. Build InGVIO Download or clone this repo to your ROS workspace. ``` cd ~/ws_catkin catkin_make ``` Source the setup file to let ROS recognize the related launch files. ``` source devel/setup.bash ``` ## 3. Run with Our Fixed-Wing Datasets First download and unzip our fixed-wing dataset to your environment. The 'fw_zed2i_f9p' folder contains the configuration files for fixed-wing datasets. To run InGVIO in your environment, the following should be conducted first. Please adjust and modify the paths in 'config/fw_zed2i_f9p/ingvio_mono.yaml' and 'config/fw_zed2i_f9p/ingvio_stereo.yaml' to the case in your environment. If you want to record the output topics, please set your output dir in 'ingvio/launch/ingvio_mono, stereo_fw_record.launch' files. To run with monocular camera, please enter: ``` roslaunch ingvio_estimator ingvio_mono_fw.launch ``` For stereo-case, please enter: ``` roslaunch ingvio_estimator ingvio_stereo_fw.launch ``` To record the output topics, you can directly use our script file by: ``` roslaunch ingvio_estimator ingvio_mono_fw_record.launch roslaunch ingvio_estimator ingvio_stereo_fw_record.launch ``` Rviz will be automatically launched. Run the ROS Bag to see the results: ``` rosbag play fw_easy.bag --pause ``` ## 4. Run with GVINS Public Datasets ### 4.1 Configuration Settings The GVINS datasets can be acquired from [GitHub - HKUST-Aerial-Robotics/GVINS-Dataset](https://github.com/HKUST-Aerial-Robotics/GVINS-Dataset). The config files for GVINS Datasets are already integrated in 'config/sportsfield'. It's valid for all 3 datasets provided by GVINS. To run InGVIO in your environment, the following should be conducted first. Please adjust and modify the paths in 'config/sports_field/ingvio_mono.yaml' and 'config/sports_field/ingvio_stereo.yaml' to the case in your environment. If you want to record the output topics, please set your output dir in 'ingvio/launch/ingvio_mono, stereo_sf_record.launch' files. To run with monocular camera, please enter: ``` roslaunch ingvio_estimator ingvio_mono_sf.launch ``` For stereo-case, please enter: ``` roslaunch ingvio_estimator ingvio_stereo_sf.launch ``` To record the output topics, you can directly use our script file by: ``` roslaunch ingvio_estimator ingvio_mono_sf_record.launch roslaunch ingvio_estimator ingvio_stereo_sf_record.launch ``` Rviz will be automatically launched. Run the ROS Bag to see the results: ``` rosbag play sports_field.bag --pause rosbag play complex_environment.bag --pause rosbag play urban_driving.bag --pause ``` ### 4.2 Parameter Tuning Hints **For sports_field.bag:** Monocular/Stereo Case: max_pts_frame = 150/110, visual_noise = 0.12, gnss_chi2_test = false, imu_buffer_size = 3000. **For urban_driving.bag:** Monocular/Stereo Case: max_pts_frame = 150/110, visual_noise = 0.12, gnss_chi2_test = true, imu_buffer_size = 3000. To avoid polluted GNSS measurements, a stronger version could be gnss_chi2_test = false, gnss_strong_reject = true. **For indoors_outdoors.bag:** Monocular/Stereo Case: max_pts_frame = 150/110, visual_noise = 0.18, gnss_chi2_test = false, gnss_strong_reject = true. Please modify or try other parameters if the above behave not well. Good parameters may be different under different environment settings. ## 5. Acknowledgements The realization of the type-based index system in filter framework is inspired by [OpenVINS](https://github.com/rpng/open_vins). The author himself has learned lots of programming skills in SLAM by reading the codes of OpenVINS. The [gnss_comm](https://github.com/HKUST-Aerial-Robotics/gnss_comm) provided by GVINS is a great wrapper in developing codes involving raw GNSS measurements in ROS. ## 6. License This software is open-sourced under GPLv3 license. See [GNU GPL v3.0 - GNU](https://www.gnu.org/licenses/gpl-3.0.html). Please cite our paper if you use either our code or our fixed-wing datasets. ================================================ FILE: camera_model/CMakeLists.txt ================================================ cmake_minimum_required(VERSION 2.8.3) project(camera_model) set(CMAKE_BUILD_TYPE "Release") set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") find_package(catkin REQUIRED COMPONENTS roscpp std_msgs ) if(EXISTS "/usr/include/eigen3") include_directories("/usr/include/eigen3") else() find_package(Eigen3 REQUIRED) include_directories(${EIGEN3_INCLUDE_DIR}) endif() find_package(Boost REQUIRED COMPONENTS filesystem program_options system) find_package(OpenCV REQUIRED) catkin_package( INCLUDE_DIRS include LIBRARIES camera_model CATKIN_DEPENDS roscpp std_msgs ) include_directories( ${PROJECT_SOURCE_DIR}/include ${catkin_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS} ) # if(EXISTS "${PROJECT_SOURCE_DIR}/test/") # enable_testing() # add_subdirectory(${PROJECT_SOURCE_DIR}/test) # endif() add_library(camera_model src/camera.cpp src/MeiCamera.cpp src/PinholeCamera.cpp src/camera_factory.cpp ) target_link_libraries(camera_model ${Boost_LIBRARIES} ${OpenCV_LIBS}) ================================================ FILE: camera_model/LICENSE ================================================ GNU GENERAL PUBLIC LICENSE Version 3, 29 June 2007 Copyright (C) 2007 Free Software Foundation, Inc. Everyone is permitted to copy and distribute verbatim copies of this license document, but changing it is not allowed. Preamble The GNU General Public License is a free, copyleft license for software and other kinds of works. The licenses for most software and other practical works are designed to take away your freedom to share and change the works. By contrast, the GNU General Public License is intended to guarantee your freedom to share and change all versions of a program--to make sure it remains free software for all its users. We, the Free Software Foundation, use the GNU General Public License for most of our software; it applies also to any other work released this way by its authors. You can apply it to your programs, too. When we speak of free software, we are referring to freedom, not price. Our General Public Licenses are designed to make sure that you have the freedom to distribute copies of free software (and charge for them if you wish), that you receive source code or can get it if you want it, that you can change the software or use pieces of it in new free programs, and that you know you can do these things. To protect your rights, we need to prevent others from denying you these rights or asking you to surrender the rights. Therefore, you have certain responsibilities if you distribute copies of the software, or if you modify it: responsibilities to respect the freedom of others. For example, if you distribute copies of such a program, whether gratis or for a fee, you must pass on to the recipients the same freedoms that you received. You must make sure that they, too, receive or can get the source code. And you must show them these terms so they know their rights. Developers that use the GNU GPL protect your rights with two steps: (1) assert copyright on the software, and (2) offer you this License giving you legal permission to copy, distribute and/or modify it. For the developers' and authors' protection, the GPL clearly explains that there is no warranty for this free software. For both users' and authors' sake, the GPL requires that modified versions be marked as changed, so that their problems will not be attributed erroneously to authors of previous versions. Some devices are designed to deny users access to install or run modified versions of the software inside them, although the manufacturer can do so. This is fundamentally incompatible with the aim of protecting users' freedom to change the software. The systematic pattern of such abuse occurs in the area of products for individuals to use, which is precisely where it is most unacceptable. Therefore, we have designed this version of the GPL to prohibit the practice for those products. If such problems arise substantially in other domains, we stand ready to extend this provision to those domains in future versions of the GPL, as needed to protect the freedom of users. Finally, every program is threatened constantly by software patents. States should not allow patents to restrict development and use of software on general-purpose computers, but in those that do, we wish to avoid the special danger that patents applied to a free program could make it effectively proprietary. To prevent this, the GPL assures that patents cannot be used to render the program non-free. The precise terms and conditions for copying, distribution and modification follow. TERMS AND CONDITIONS 0. Definitions. "This License" refers to version 3 of the GNU General Public License. "Copyright" also means copyright-like laws that apply to other kinds of works, such as semiconductor masks. "The Program" refers to any copyrightable work licensed under this License. Each licensee is addressed as "you". "Licensees" and "recipients" may be individuals or organizations. To "modify" a work means to copy from or adapt all or part of the work in a fashion requiring copyright permission, other than the making of an exact copy. The resulting work is called a "modified version" of the earlier work or a work "based on" the earlier work. A "covered work" means either the unmodified Program or a work based on the Program. To "propagate" a work means to do anything with it that, without permission, would make you directly or secondarily liable for infringement under applicable copyright law, except executing it on a computer or modifying a private copy. Propagation includes copying, distribution (with or without modification), making available to the public, and in some countries other activities as well. To "convey" a work means any kind of propagation that enables other parties to make or receive copies. Mere interaction with a user through a computer network, with no transfer of a copy, is not conveying. An interactive user interface displays "Appropriate Legal Notices" to the extent that it includes a convenient and prominently visible feature that (1) displays an appropriate copyright notice, and (2) tells the user that there is no warranty for the work (except to the extent that warranties are provided), that licensees may convey the work under this License, and how to view a copy of this License. If the interface presents a list of user commands or options, such as a menu, a prominent item in the list meets this criterion. 1. Source Code. The "source code" for a work means the preferred form of the work for making modifications to it. "Object code" means any non-source form of a work. A "Standard Interface" means an interface that either is an official standard defined by a recognized standards body, or, in the case of interfaces specified for a particular programming language, one that is widely used among developers working in that language. The "System Libraries" of an executable work include anything, other than the work as a whole, that (a) is included in the normal form of packaging a Major Component, but which is not part of that Major Component, and (b) serves only to enable use of the work with that Major Component, or to implement a Standard Interface for which an implementation is available to the public in source code form. A "Major Component", in this context, means a major essential component (kernel, window system, and so on) of the specific operating system (if any) on which the executable work runs, or a compiler used to produce the work, or an object code interpreter used to run it. The "Corresponding Source" for a work in object code form means all the source code needed to generate, install, and (for an executable work) run the object code and to modify the work, including scripts to control those activities. However, it does not include the work's System Libraries, or general-purpose tools or generally available free programs which are used unmodified in performing those activities but which are not part of the work. For example, Corresponding Source includes interface definition files associated with source files for the work, and the source code for shared libraries and dynamically linked subprograms that the work is specifically designed to require, such as by intimate data communication or control flow between those subprograms and other parts of the work. The Corresponding Source need not include anything that users can regenerate automatically from other parts of the Corresponding Source. The Corresponding Source for a work in source code form is that same work. 2. Basic Permissions. All rights granted under this License are granted for the term of copyright on the Program, and are irrevocable provided the stated conditions are met. This License explicitly affirms your unlimited permission to run the unmodified Program. The output from running a covered work is covered by this License only if the output, given its content, constitutes a covered work. This License acknowledges your rights of fair use or other equivalent, as provided by copyright law. You may make, run and propagate covered works that you do not convey, without conditions so long as your license otherwise remains in force. You may convey covered works to others for the sole purpose of having them make modifications exclusively for you, or provide you with facilities for running those works, provided that you comply with the terms of this License in conveying all material for which you do not control copyright. Those thus making or running the covered works for you must do so exclusively on your behalf, under your direction and control, on terms that prohibit them from making any copies of your copyrighted material outside their relationship with you. Conveying under any other circumstances is permitted solely under the conditions stated below. Sublicensing is not allowed; section 10 makes it unnecessary. 3. Protecting Users' Legal Rights From Anti-Circumvention Law. No covered work shall be deemed part of an effective technological measure under any applicable law fulfilling obligations under article 11 of the WIPO copyright treaty adopted on 20 December 1996, or similar laws prohibiting or restricting circumvention of such measures. When you convey a covered work, you waive any legal power to forbid circumvention of technological measures to the extent such circumvention is effected by exercising rights under this License with respect to the covered work, and you disclaim any intention to limit operation or modification of the work as a means of enforcing, against the work's users, your or third parties' legal rights to forbid circumvention of technological measures. 4. Conveying Verbatim Copies. You may convey verbatim copies of the Program's source code as you receive it, in any medium, provided that you conspicuously and appropriately publish on each copy an appropriate copyright notice; keep intact all notices stating that this License and any non-permissive terms added in accord with section 7 apply to the code; keep intact all notices of the absence of any warranty; and give all recipients a copy of this License along with the Program. You may charge any price or no price for each copy that you convey, and you may offer support or warranty protection for a fee. 5. Conveying Modified Source Versions. You may convey a work based on the Program, or the modifications to produce it from the Program, in the form of source code under the terms of section 4, provided that you also meet all of these conditions: a) The work must carry prominent notices stating that you modified it, and giving a relevant date. b) The work must carry prominent notices stating that it is released under this License and any conditions added under section 7. This requirement modifies the requirement in section 4 to "keep intact all notices". c) You must license the entire work, as a whole, under this License to anyone who comes into possession of a copy. This License will therefore apply, along with any applicable section 7 additional terms, to the whole of the work, and all its parts, regardless of how they are packaged. This License gives no permission to license the work in any other way, but it does not invalidate such permission if you have separately received it. d) If the work has interactive user interfaces, each must display Appropriate Legal Notices; however, if the Program has interactive interfaces that do not display Appropriate Legal Notices, your work need not make them do so. A compilation of a covered work with other separate and independent works, which are not by their nature extensions of the covered work, and which are not combined with it such as to form a larger program, in or on a volume of a storage or distribution medium, is called an "aggregate" if the compilation and its resulting copyright are not used to limit the access or legal rights of the compilation's users beyond what the individual works permit. Inclusion of a covered work in an aggregate does not cause this License to apply to the other parts of the aggregate. 6. Conveying Non-Source Forms. You may convey a covered work in object code form under the terms of sections 4 and 5, provided that you also convey the machine-readable Corresponding Source under the terms of this License, in one of these ways: a) Convey the object code in, or embodied in, a physical product (including a physical distribution medium), accompanied by the Corresponding Source fixed on a durable physical medium customarily used for software interchange. b) Convey the object code in, or embodied in, a physical product (including a physical distribution medium), accompanied by a written offer, valid for at least three years and valid for as long as you offer spare parts or customer support for that product model, to give anyone who possesses the object code either (1) a copy of the Corresponding Source for all the software in the product that is covered by this License, on a durable physical medium customarily used for software interchange, for a price no more than your reasonable cost of physically performing this conveying of source, or (2) access to copy the Corresponding Source from a network server at no charge. c) Convey individual copies of the object code with a copy of the written offer to provide the Corresponding Source. This alternative is allowed only occasionally and noncommercially, and only if you received the object code with such an offer, in accord with subsection 6b. d) Convey the object code by offering access from a designated place (gratis or for a charge), and offer equivalent access to the Corresponding Source in the same way through the same place at no further charge. You need not require recipients to copy the Corresponding Source along with the object code. If the place to copy the object code is a network server, the Corresponding Source may be on a different server (operated by you or a third party) that supports equivalent copying facilities, provided you maintain clear directions next to the object code saying where to find the Corresponding Source. Regardless of what server hosts the Corresponding Source, you remain obligated to ensure that it is available for as long as needed to satisfy these requirements. e) Convey the object code using peer-to-peer transmission, provided you inform other peers where the object code and Corresponding Source of the work are being offered to the general public at no charge under subsection 6d. A separable portion of the object code, whose source code is excluded from the Corresponding Source as a System Library, need not be included in conveying the object code work. A "User Product" is either (1) a "consumer product", which means any tangible personal property which is normally used for personal, family, or household purposes, or (2) anything designed or sold for incorporation into a dwelling. In determining whether a product is a consumer product, doubtful cases shall be resolved in favor of coverage. For a particular product received by a particular user, "normally used" refers to a typical or common use of that class of product, regardless of the status of the particular user or of the way in which the particular user actually uses, or expects or is expected to use, the product. A product is a consumer product regardless of whether the product has substantial commercial, industrial or non-consumer uses, unless such uses represent the only significant mode of use of the product. "Installation Information" for a User Product means any methods, procedures, authorization keys, or other information required to install and execute modified versions of a covered work in that User Product from a modified version of its Corresponding Source. The information must suffice to ensure that the continued functioning of the modified object code is in no case prevented or interfered with solely because modification has been made. If you convey an object code work under this section in, or with, or specifically for use in, a User Product, and the conveying occurs as part of a transaction in which the right of possession and use of the User Product is transferred to the recipient in perpetuity or for a fixed term (regardless of how the transaction is characterized), the Corresponding Source conveyed under this section must be accompanied by the Installation Information. But this requirement does not apply if neither you nor any third party retains the ability to install modified object code on the User Product (for example, the work has been installed in ROM). The requirement to provide Installation Information does not include a requirement to continue to provide support service, warranty, or updates for a work that has been modified or installed by the recipient, or for the User Product in which it has been modified or installed. Access to a network may be denied when the modification itself materially and adversely affects the operation of the network or violates the rules and protocols for communication across the network. Corresponding Source conveyed, and Installation Information provided, in accord with this section must be in a format that is publicly documented (and with an implementation available to the public in source code form), and must require no special password or key for unpacking, reading or copying. 7. Additional Terms. "Additional permissions" are terms that supplement the terms of this License by making exceptions from one or more of its conditions. Additional permissions that are applicable to the entire Program shall be treated as though they were included in this License, to the extent that they are valid under applicable law. If additional permissions apply only to part of the Program, that part may be used separately under those permissions, but the entire Program remains governed by this License without regard to the additional permissions. When you convey a copy of a covered work, you may at your option remove any additional permissions from that copy, or from any part of it. (Additional permissions may be written to require their own removal in certain cases when you modify the work.) You may place additional permissions on material, added by you to a covered work, for which you have or can give appropriate copyright permission. Notwithstanding any other provision of this License, for material you add to a covered work, you may (if authorized by the copyright holders of that material) supplement the terms of this License with terms: a) Disclaiming warranty or limiting liability differently from the terms of sections 15 and 16 of this License; or b) Requiring preservation of specified reasonable legal notices or author attributions in that material or in the Appropriate Legal Notices displayed by works containing it; or c) Prohibiting misrepresentation of the origin of that material, or requiring that modified versions of such material be marked in reasonable ways as different from the original version; or d) Limiting the use for publicity purposes of names of licensors or authors of the material; or e) Declining to grant rights under trademark law for use of some trade names, trademarks, or service marks; or f) Requiring indemnification of licensors and authors of that material by anyone who conveys the material (or modified versions of it) with contractual assumptions of liability to the recipient, for any liability that these contractual assumptions directly impose on those licensors and authors. All other non-permissive additional terms are considered "further restrictions" within the meaning of section 10. If the Program as you received it, or any part of it, contains a notice stating that it is governed by this License along with a term that is a further restriction, you may remove that term. If a license document contains a further restriction but permits relicensing or conveying under this License, you may add to a covered work material governed by the terms of that license document, provided that the further restriction does not survive such relicensing or conveying. If you add terms to a covered work in accord with this section, you must place, in the relevant source files, a statement of the additional terms that apply to those files, or a notice indicating where to find the applicable terms. Additional terms, permissive or non-permissive, may be stated in the form of a separately written license, or stated as exceptions; the above requirements apply either way. 8. Termination. You may not propagate or modify a covered work except as expressly provided under this License. Any attempt otherwise to propagate or modify it is void, and will automatically terminate your rights under this License (including any patent licenses granted under the third paragraph of section 11). However, if you cease all violation of this License, then your license from a particular copyright holder is reinstated (a) provisionally, unless and until the copyright holder explicitly and finally terminates your license, and (b) permanently, if the copyright holder fails to notify you of the violation by some reasonable means prior to 60 days after the cessation. Moreover, your license from a particular copyright holder is reinstated permanently if the copyright holder notifies you of the violation by some reasonable means, this is the first time you have received notice of violation of this License (for any work) from that copyright holder, and you cure the violation prior to 30 days after your receipt of the notice. Termination of your rights under this section does not terminate the licenses of parties who have received copies or rights from you under this License. If your rights have been terminated and not permanently reinstated, you do not qualify to receive new licenses for the same material under section 10. 9. Acceptance Not Required for Having Copies. You are not required to accept this License in order to receive or run a copy of the Program. Ancillary propagation of a covered work occurring solely as a consequence of using peer-to-peer transmission to receive a copy likewise does not require acceptance. However, nothing other than this License grants you permission to propagate or modify any covered work. These actions infringe copyright if you do not accept this License. Therefore, by modifying or propagating a covered work, you indicate your acceptance of this License to do so. 10. Automatic Licensing of Downstream Recipients. Each time you convey a covered work, the recipient automatically receives a license from the original licensors, to run, modify and propagate that work, subject to this License. You are not responsible for enforcing compliance by third parties with this License. An "entity transaction" is a transaction transferring control of an organization, or substantially all assets of one, or subdividing an organization, or merging organizations. If propagation of a covered work results from an entity transaction, each party to that transaction who receives a copy of the work also receives whatever licenses to the work the party's predecessor in interest had or could give under the previous paragraph, plus a right to possession of the Corresponding Source of the work from the predecessor in interest, if the predecessor has it or can get it with reasonable efforts. You may not impose any further restrictions on the exercise of the rights granted or affirmed under this License. For example, you may not impose a license fee, royalty, or other charge for exercise of rights granted under this License, and you may not initiate litigation (including a cross-claim or counterclaim in a lawsuit) alleging that any patent claim is infringed by making, using, selling, offering for sale, or importing the Program or any portion of it. 11. Patents. A "contributor" is a copyright holder who authorizes use under this License of the Program or a work on which the Program is based. The work thus licensed is called the contributor's "contributor version". A contributor's "essential patent claims" are all patent claims owned or controlled by the contributor, whether already acquired or hereafter acquired, that would be infringed by some manner, permitted by this License, of making, using, or selling its contributor version, but do not include claims that would be infringed only as a consequence of further modification of the contributor version. For purposes of this definition, "control" includes the right to grant patent sublicenses in a manner consistent with the requirements of this License. Each contributor grants you a non-exclusive, worldwide, royalty-free patent license under the contributor's essential patent claims, to make, use, sell, offer for sale, import and otherwise run, modify and propagate the contents of its contributor version. In the following three paragraphs, a "patent license" is any express agreement or commitment, however denominated, not to enforce a patent (such as an express permission to practice a patent or covenant not to sue for patent infringement). To "grant" such a patent license to a party means to make such an agreement or commitment not to enforce a patent against the party. If you convey a covered work, knowingly relying on a patent license, and the Corresponding Source of the work is not available for anyone to copy, free of charge and under the terms of this License, through a publicly available network server or other readily accessible means, then you must either (1) cause the Corresponding Source to be so available, or (2) arrange to deprive yourself of the benefit of the patent license for this particular work, or (3) arrange, in a manner consistent with the requirements of this License, to extend the patent license to downstream recipients. "Knowingly relying" means you have actual knowledge that, but for the patent license, your conveying the covered work in a country, or your recipient's use of the covered work in a country, would infringe one or more identifiable patents in that country that you have reason to believe are valid. If, pursuant to or in connection with a single transaction or arrangement, you convey, or propagate by procuring conveyance of, a covered work, and grant a patent license to some of the parties receiving the covered work authorizing them to use, propagate, modify or convey a specific copy of the covered work, then the patent license you grant is automatically extended to all recipients of the covered work and works based on it. A patent license is "discriminatory" if it does not include within the scope of its coverage, prohibits the exercise of, or is conditioned on the non-exercise of one or more of the rights that are specifically granted under this License. You may not convey a covered work if you are a party to an arrangement with a third party that is in the business of distributing software, under which you make payment to the third party based on the extent of your activity of conveying the work, and under which the third party grants, to any of the parties who would receive the covered work from you, a discriminatory patent license (a) in connection with copies of the covered work conveyed by you (or copies made from those copies), or (b) primarily for and in connection with specific products or compilations that contain the covered work, unless you entered into that arrangement, or that patent license was granted, prior to 28 March 2007. Nothing in this License shall be construed as excluding or limiting any implied license or other defenses to infringement that may otherwise be available to you under applicable patent law. 12. No Surrender of Others' Freedom. If conditions are imposed on you (whether by court order, agreement or otherwise) that contradict the conditions of this License, they do not excuse you from the conditions of this License. If you cannot convey a covered work so as to satisfy simultaneously your obligations under this License and any other pertinent obligations, then as a consequence you may not convey it at all. For example, if you agree to terms that obligate you to collect a royalty for further conveying from those to whom you convey the Program, the only way you could satisfy both those terms and this License would be to refrain entirely from conveying the Program. 13. Use with the GNU Affero General Public License. Notwithstanding any other provision of this License, you have permission to link or combine any covered work with a work licensed under version 3 of the GNU Affero General Public License into a single combined work, and to convey the resulting work. The terms of this License will continue to apply to the part which is the covered work, but the special requirements of the GNU Affero General Public License, section 13, concerning interaction through a network will apply to the combination as such. 14. Revised Versions of this License. The Free Software Foundation may publish revised and/or new versions of the GNU General Public License from time to time. Such new versions will be similar in spirit to the present version, but may differ in detail to address new problems or concerns. Each version is given a distinguishing version number. If the Program specifies that a certain numbered version of the GNU General Public License "or any later version" applies to it, you have the option of following the terms and conditions either of that numbered version or of any later version published by the Free Software Foundation. If the Program does not specify a version number of the GNU General Public License, you may choose any version ever published by the Free Software Foundation. If the Program specifies that a proxy can decide which future versions of the GNU General Public License can be used, that proxy's public statement of acceptance of a version permanently authorizes you to choose that version for the Program. Later license versions may give you additional or different permissions. However, no additional obligations are imposed on any author or copyright holder as a result of your choosing to follow a later version. 15. Disclaimer of Warranty. THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM IS WITH YOU. SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF ALL NECESSARY SERVICING, REPAIR OR CORRECTION. 16. Limitation of Liability. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS), EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH DAMAGES. 17. Interpretation of Sections 15 and 16. If the disclaimer of warranty and limitation of liability provided above cannot be given local legal effect according to their terms, reviewing courts shall apply local law that most closely approximates an absolute waiver of all civil liability in connection with the Program, unless a warranty or assumption of liability accompanies a copy of the Program in return for a fee. END OF TERMS AND CONDITIONS How to Apply These Terms to Your New Programs If you develop a new program, and you want it to be of the greatest possible use to the public, the best way to achieve this is to make it free software which everyone can redistribute and change under these terms. To do so, attach the following notices to the program. It is safest to attach them to the start of each source file to most effectively state the exclusion of warranty; and each file should have at least the "copyright" line and a pointer to where the full notice is found. Copyright (C) This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version. This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with this program. If not, see . Also add information on how to contact you by electronic and paper mail. If the program does terminal interaction, make it output a short notice like this when it starts in an interactive mode: Copyright (C) This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'. This is free software, and you are welcome to redistribute it under certain conditions; type `show c' for details. The hypothetical commands `show w' and `show c' should show the appropriate parts of the General Public License. Of course, your program's commands might be different; for a GUI interface, you would use an "about box". You should also get your employer (if you work as a programmer) or school, if any, to sign a "copyright disclaimer" for the program, if necessary. For more information on this, and how to apply and follow the GNU GPL, see . The GNU General Public License does not permit incorporating your program into proprietary programs. If your program is a subroutine library, you may consider it more useful to permit linking proprietary applications with the library. If this is what you want to do, use the GNU Lesser General Public License instead of this License. But first, please read . ================================================ FILE: camera_model/include/MeiCamera.h ================================================ /** This File is part of Camera Model * * Copyright (C) 2022 Changwu Liu (cwliu529@163.com, * lcw18@mails.tsinghua.edu.cn (valid until 2023)) * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program. If not, see . * * Since some functions are adapted from GVINS * GVINS is under GPLv3 License. */ #pragma once #include "camera.h" #include namespace camera_model { class MeiCamera: public Camera { public: class Parameters: public Camera::Parameters { public: Parameters(); Parameters(const std::string& cameraName, int w, int h, double xi, double k1, double k2, double p1, double p2, double gamma1, double gamma2, double u0, double v0); double& xi(); double& k1(); double& k2(); double& p1(); double& p2(); double& gamma1(); double& gamma2(); double& u0(); double& v0(); double xi() const; double k1() const; double k2() const; double p1() const; double p2() const; double gamma1() const; double gamma2() const; double u0() const; double v0() const; bool readFromYamlFile(const std::string& filename) override; void writeToYamlFile(const std::string& filename) const override; Parameters& operator= (const Parameters& other); friend std::ostream& operator<< (std::ostream& out, const Parameters& params); private: double m_xi; double m_k1; double m_k2; double m_p1; double m_p2; double m_gamma1; double m_gamma2; double m_u0; double m_v0; }; MeiCamera(); MeiCamera(const std::string& cameraName, int imageWidth, int imageHeight, double xi, double k1, double k2, double p1, double p2, double gamma1, double gamma2, double u0, double v0); MeiCamera(const Parameters& params); Camera::ModelType modelType(void) const override; const std::string& cameraName(void) const override; int imageWidth(void) const override; int imageHeight(void) const override; double getNormalPixel() const override; void liftSphere(const Eigen::Vector2d& p, Eigen::Vector3d& P) const override; void liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const override; void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p) const override; void undistToPlane(const Eigen::Vector2d& p_u, Eigen::Vector2d& p) const override; template static void spaceToPlane(const T* const params, const T* const q, const T* const t, const Eigen::Matrix& P, Eigen::Matrix& p); void distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u) const; void distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u, Eigen::Matrix2d& J) const; int parameterCount() const override; const Parameters& getParameters(void) const; void setParameters(const Parameters& parameters); void readParameters(const std::vector& parameterVec) override; void writeParameters(std::vector& parameterVec) const override; void writeParametersToYamlFile(const std::string& filename) const override; std::string parametersToString(void) const override; private: Parameters mParameters; double m_inv_K11, m_inv_K13, m_inv_K22, m_inv_K23; bool m_noDistortion; }; typedef boost::shared_ptr MeiCameraPtr; typedef boost::shared_ptr MeiCameraConstPtr; template void MeiCamera::spaceToPlane(const T* const params, const T* const q, const T* const t, const Eigen::Matrix& P, Eigen::Matrix& p) { Eigen::Vector3d Pw((double)P(0), (double)P(1), (double)P(2)); Eigen::Vector3d tw((double)t(0), (double)t(1), (double)t(2)); Eigen::Quaterniond qw; qw.w() = (double) q[3]; qw.x() = (double) q[0]; qw.y() = (double) q[1]; qw.z() = (double) q[2]; qw.normalized(); Eigen::Vector3d transP = qw.toRotationMatrix()*Pw + tw; T P_c[3]; P_c[0] = T(transP(0)); P_c[1] = T(transP(1)); P_c[2] = T(transP(2)); T xi = params[0]; T k1 = params[1]; T k2 = params[2]; T p1 = params[3]; T p2 = params[4]; T gamma1 = params[5]; T gamma2 = params[6]; T alpha = T(0); T u0 = params[7]; T v0 = params[8]; T len = std::sqrt(P_c[0]*P_c[0] + P_c[1]*P_c[1] + P_c[2]*P_c[2]); P_c[0] /= len; P_c[1] /= len; P_c[2] /= len; T u = P_c[0]/(P_c[2] + xi); T v = P_c[1]/(P_c[2] + xi); T rho_sqr = u*u + v*v; T L = T(1.0) + k1*rho_sqr + k2*rho_sqr*rho_sqr; T du = T(2.0)*p1*u*v + p2*(rho_sqr + T(2.0)*u*u); T dv = p1*(rho_sqr + T(2.0)*v*v) + T(2.0)*p2*u*v; u = L*u + du; v = L*v + dv; p(0) = gamma1*(u + alpha*v) + u0; p(1) = gamma2*v + v0; } } ================================================ FILE: camera_model/include/PinholeCamera.h ================================================ /** This File is part of Camera Model * * Copyright (C) 2022 Changwu Liu (cwliu529@163.com, * lcw18@mails.tsinghua.edu.cn (valid until 2023)) * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program. If not, see . * * Since some functions are adapted from GVINS * GVINS is under GPLv3 License. */ #pragma once #include "camera.h" #include namespace camera_model { class PinholeCamera: public Camera { public: class Parameters: public Camera::Parameters { public: Parameters(); Parameters(const std::string& cameraName, int w, int h, double k1, double k2, double p1, double p2, double fx, double fy, double cx, double cy); double& k1(void); double& k2(void); double& p1(void); double& p2(void); double& fx(void); double& fy(void); double& cx(void); double& cy(void); double xi(void) const; double k1(void) const; double k2(void) const; double p1(void) const; double p2(void) const; double fx(void) const; double fy(void) const; double cx(void) const; double cy(void) const; bool readFromYamlFile(const std::string& filename) override; void writeToYamlFile(const std::string& filename) const override; Parameters& operator=(const Parameters& other); friend std::ostream& operator<< (std::ostream& out, const Parameters& params); private: double m_k1; double m_k2; double m_p1; double m_p2; double m_fx; double m_fy; double m_cx; double m_cy; }; PinholeCamera(); PinholeCamera(const std::string& cameraName, int imageWidth, int imageHeight, double k1, double k2, double p1, double p2, double fx, double fy, double cx, double cy); PinholeCamera(const Parameters& params); Camera::ModelType modelType(void) const override; const std::string& cameraName(void) const override; int imageWidth(void) const override; int imageHeight(void) const override; double getNormalPixel() const override; void liftSphere(const Eigen::Vector2d& p, Eigen::Vector3d& P) const override; void liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const override; void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p) const override; void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p, Eigen::Matrix& J) const; void undistToPlane(const Eigen::Vector2d& p_u, Eigen::Vector2d& p) const override; template static void spaceToPlane(const T* const params, const T* const q, const T* const t, const Eigen::Matrix& P, Eigen::Matrix& p); void distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u) const; void distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u, Eigen::Matrix2d& J) const; int parameterCount() const override; const Parameters& getParameters(void) const; void setParameters(const Parameters& parameters); void readParameters(const std::vector& parameterVec) override; void writeParameters(std::vector& parameterVec) const override; void writeParametersToYamlFile(const std::string& filename) const override; std::string parametersToString(void) const override; private: Parameters mParameters; double m_inv_K11, m_inv_K13, m_inv_K22, m_inv_K23; bool m_noDistortion; }; typedef boost::shared_ptr PinholeCameraPtr; typedef boost::shared_ptr PinholeCameraConstPtr; template void PinholeCamera::spaceToPlane(const T* const params, const T* const q, const T* const t, const Eigen::Matrix& P, Eigen::Matrix& p) { Eigen::Vector3d Pw((double)P(0), (double)P(1), (double)P(2)); Eigen::Vector3d tw((double)t(0), (double)t(1), (double)t(2)); Eigen::Quaterniond qw; qw.w() = (double) q[3]; qw.x() = (double) q[0]; qw.y() = (double) q[1]; qw.z() = (double) q[2]; qw.normalized(); Eigen::Vector3d transP = qw.toRotationMatrix()*Pw + tw; T P_c[3]; P_c[0] = T(transP(0)); P_c[1] = T(transP(1)); P_c[2] = T(transP(2)); T k1 = params[0]; T k2 = params[1]; T p1 = params[2]; T p2 = params[3]; T fx = params[4]; T fy = params[5]; T alpha = T(0); T cx = params[6]; T cy = params[7]; T u = P_c[0]/P_c[2]; T v = P_c[1]/P_c[2]; T rho_sqr = u*u + v*v; T L = T(1.0) + k1*rho_sqr + k2*rho_sqr*rho_sqr; T du = T(2.0)*p1*u*v + p2*(rho_sqr + T(2.0)*u*u); T dv = p1*(rho_sqr + T(2.0)*v*v) + T(2.0)*p2*u*v; u = L*u + du; v = L*v + dv; p(0) = fx*(u + alpha*v) + cx; p(1) = fy*v + cy; } } ================================================ FILE: camera_model/include/camera.h ================================================ /** This File is part Camera Model * * * Copyright (C) 2022 Changwu Liu (cwliu529@163.com, * lcw18@mails.tsinghua.edu.cn (valid until 2023)) * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program. If not, see . * * Since some functions are adapted from GVINS * GVINS is under GPLv3 License. */ #pragma once #include #include #include #include #include #include #include namespace camera_model { class Camera { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW enum ModelType { MEI, PINHOLE }; class Parameters { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW Parameters(ModelType modelType); Parameters(ModelType modelType, const std::string& cameraName, int w, int h); ModelType& modelType(); ModelType modelType() const; std::string& cameraName(); const std::string& cameraName() const; int& imageWidth(); int imageWidth() const; int& imageHeight(); int imageHeight() const; int nIntrinsics() const; virtual bool readFromYamlFile(const std::string& filename) = 0; virtual void writeToYamlFile(const std::string& filename) const = 0; protected: ModelType m_modelType; int m_nIntrinsics; std::string m_cameraName; int m_imageWidth; int m_imageHeight; }; virtual ModelType modelType() const = 0; virtual const std::string& cameraName() const = 0; virtual int imageWidth() const = 0; virtual int imageHeight() const = 0; virtual cv::Mat& mask(); virtual const cv::Mat& mask() const; virtual double getNormalPixel() const = 0; // from the image plane to the sphere virtual void liftSphere(const Eigen::Vector2d& p, Eigen::Vector3d& P) const = 0; // from the image plane to the projective space virtual void liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const = 0; // from 3D points to the image plane virtual void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p) const = 0; virtual void undistToPlane(const Eigen::Vector2d& p_u, Eigen::Vector2d& p) const = 0; virtual int parameterCount(void) const = 0; virtual void readParameters(const std::vector& parameters) = 0; virtual void writeParameters(std::vector& parameters) const = 0; virtual void writeParametersToYamlFile(const std::string& filename) const = 0; virtual std::string parametersToString() const = 0; double reprojectionDist(const Eigen::Vector3d& P1, const Eigen::Vector3d& P2) const; double reprojectionError(const Eigen::Vector3d& P, const Eigen::Quaterniond& camera_q, const Eigen::Vector3d& camera_t, const Eigen::Vector2d& observed_p) const; protected: cv::Mat m_mask; }; typedef boost::shared_ptr CameraPtr; typedef boost::shared_ptr CameraConstPtr; } ================================================ FILE: camera_model/include/camera_factory.h ================================================ /** This File is part of Camera Model * * Copyright (C) 2022 Changwu Liu (cwliu529@163.com, * lcw18@mails.tsinghua.edu.cn (valid until 2023)) * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program. If not, see . * * Since some functions are adapted from GVINS * GVINS is under GPLv3 License. */ #pragma once #include "camera.h" namespace camera_model { class CameraFactory { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW CameraFactory(); static boost::shared_ptr instance(void); CameraPtr generateCamera(Camera::ModelType modelType, const std::string& cameraName, cv::Size imageSize) const; CameraPtr generateCameraFromYamlFile(const std::string& filename); private: static boost::shared_ptr m_instance; }; } ================================================ FILE: camera_model/package.xml ================================================ camera_model 0.0.0 The camera_model package lcw18 GPLv3 catkin roscpp std_msgs roscpp std_msgs ================================================ FILE: camera_model/src/MeiCamera.cpp ================================================ /** This File is part of Camera Model * * * Copyright (C) 2022 Changwu Liu (cwliu529@163.com, * lcw18@mails.tsinghua.edu.cn (valid until 2023)) * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program. If not, see . * * Since some functions are adapted from GVINS * GVINS is under GPLv3 License. */ #include "MeiCamera.h" #include namespace camera_model { MeiCamera::Parameters::Parameters(): Camera::Parameters(MEI), m_xi(0.0), m_k1(0.0), m_k2(0.0), m_p1(0.0), m_p2(0.0), m_gamma1(0.0), m_gamma2(0.0), m_u0(0.0), m_v0(0.0) {} MeiCamera::Parameters::Parameters(const std::string& cameraName, int w, int h, double xi, double k1, double k2, double p1, double p2, double gamma1, double gamma2, double u0, double v0) : Camera::Parameters(MEI, cameraName, w, h), m_xi(xi), m_k1(k1), m_k2(k2), m_p1(p1), m_p2(p2), m_gamma1(gamma1), m_gamma2(gamma2), m_u0(u0), m_v0(v0) {} double& MeiCamera::Parameters::xi() { return m_xi;} double& MeiCamera::Parameters::k1() { return m_k1;} double& MeiCamera::Parameters::k2() { return m_k2;} double& MeiCamera::Parameters::p1() { return m_p1;} double& MeiCamera::Parameters::p2() { return m_p2;} double& MeiCamera::Parameters::gamma1() { return m_gamma1;} double& MeiCamera::Parameters::gamma2() { return m_gamma2;} double& MeiCamera::Parameters::u0() { return m_u0;} double& MeiCamera::Parameters::v0() { return m_v0;} double MeiCamera::Parameters::xi() const { return m_xi;} double MeiCamera::Parameters::k1() const { return m_k1;} double MeiCamera::Parameters::k2() const { return m_k2;} double MeiCamera::Parameters::p1() const { return m_p1;} double MeiCamera::Parameters::p2() const { return m_p2;} double MeiCamera::Parameters::gamma1() const { return m_gamma1;} double MeiCamera::Parameters::gamma2() const { return m_gamma2;} double MeiCamera::Parameters::u0() const { return m_u0;} double MeiCamera::Parameters::v0() const { return m_v0;} bool MeiCamera::Parameters::readFromYamlFile(const std::string& filename) { cv::FileStorage fs(filename, cv::FileStorage::READ); if (!fs.isOpened()) return false; if (!fs["model_type"].isNone()) { std::string sModelType; fs["model_type"] >> sModelType; if (sModelType.compare("MEI") != 0) return false; } m_modelType = MEI; fs["camera_name"] >> m_cameraName; m_imageWidth = static_cast(fs["image_width"]); m_imageHeight = static_cast(fs["image_height"]); cv::FileNode n = fs["mirror_parameters"]; m_xi = static_cast(n["xi"]); n = fs["distortion_parameters"]; m_k1 = static_cast(n["k1"]); m_k2 = static_cast(n["k2"]); m_p1 = static_cast(n["p1"]); m_p2 = static_cast(n["p2"]); n = fs["projection_parameters"]; m_gamma1 = static_cast(n["gamma1"]); m_gamma2 = static_cast(n["gamma2"]); m_u0 = static_cast(n["u0"]); m_v0 = static_cast(n["v0"]); return true; } void MeiCamera::Parameters::writeToYamlFile(const std::string& filename) const { cv::FileStorage fs(filename, cv::FileStorage::WRITE); fs << "model_type" << "MEI"; fs << "camera_name" << m_cameraName; fs << "image_width" << m_imageWidth; fs << "image_height" << m_imageHeight; fs << "mirror_parameters"; fs << "{" << "xi" << m_xi << "}"; fs << "distortion_parameters"; fs << "{" << "k1" << m_k1 << "k2" << m_k2 << "p1" << m_p1 << "p2" << m_p2 << "}"; fs << "projection_parameters"; fs << "{" << "gamma1" << m_gamma1 << "gamma2" << m_gamma2 << "u0" << m_u0 << "v0" << m_v0 << "}"; fs.release(); } MeiCamera::Parameters& MeiCamera::Parameters::operator=(const MeiCamera::Parameters& other) { if (this != &other) { m_modelType = other.m_modelType; m_cameraName = other.m_cameraName; m_imageWidth = other.m_imageWidth; m_imageHeight = other.m_imageHeight; m_xi = other.m_xi; m_k1 = other.m_k1; m_k2 = other.m_k2; m_p1 = other.m_p1; m_p2 = other.m_p2; m_gamma1 = other.m_gamma1; m_gamma2 = other.m_gamma2; m_u0 = other.m_u0; m_v0 = other.m_v0; } return *this; } std::ostream& operator<< (std::ostream& out, const MeiCamera::Parameters& params) { out << "Camera Parameters:" << std::endl; out << " model_type " << "MEI" << std::endl; out << " camera_name " << params.m_cameraName << std::endl; out << " image_width " << params.m_imageWidth << std::endl; out << " image_height " << params.m_imageHeight << std::endl; out << "Mirror Parameters" << std::endl; out << std::fixed << std::setprecision(10); out << " xi " << params.m_xi << std::endl; out << "Distortion Parameters" << std::endl; out << " k1 " << params.m_k1 << std::endl << " k2 " << params.m_k2 << std::endl << " p1 " << params.m_p1 << std::endl << " p2 " << params.m_p2 << std::endl; out << "Projection Parameters" << std::endl; out << " gamma1 " << params.m_gamma1 << std::endl << " gamma2 " << params.m_gamma2 << std::endl << " u0 " << params.m_u0 << std::endl << " v0 " << params.m_v0 << std::endl; return out; } MeiCamera::MeiCamera(): m_inv_K11(1.0), m_inv_K13(0.0), m_inv_K22(1.0), m_inv_K23(0.0), m_noDistortion(true) {} MeiCamera::MeiCamera(const std::string& cameraName, int imageWidth, int imageHeight, double xi, double k1, double k2, double p1, double p2, double gamma1, double gamma2, double u0, double v0) : mParameters(cameraName, imageWidth, imageHeight, xi, k1, k2, p1, p2, gamma1, gamma2, u0, v0) { if ((mParameters.k1() == 0.0) && (mParameters.k2() == 0.0) && (mParameters.p1() == 0.0) && (mParameters.p2() == 0.0)) { m_noDistortion = true; } else { m_noDistortion = false; } m_inv_K11 = 1.0 / mParameters.gamma1(); m_inv_K13 = -mParameters.u0() / mParameters.gamma1(); m_inv_K22 = 1.0 / mParameters.gamma2(); m_inv_K23 = -mParameters.v0() / mParameters.gamma2(); } MeiCamera::MeiCamera(const MeiCamera::Parameters& params) : mParameters(params) { if ((mParameters.k1() == 0.0) && (mParameters.k2() == 0.0) && (mParameters.p1() == 0.0) && (mParameters.p2() == 0.0)) { m_noDistortion = true; } else { m_noDistortion = false; } m_inv_K11 = 1.0 / mParameters.gamma1(); m_inv_K13 = -mParameters.u0() / mParameters.gamma1(); m_inv_K22 = 1.0 / mParameters.gamma2(); m_inv_K23 = -mParameters.v0() / mParameters.gamma2(); } Camera::ModelType MeiCamera::modelType() const { return mParameters.modelType(); } const std::string& MeiCamera::cameraName() const { return mParameters.cameraName(); } int MeiCamera::imageWidth() const { return mParameters.imageWidth(); } int MeiCamera::imageHeight() const { return mParameters.imageHeight(); } double MeiCamera::getNormalPixel() const { return 2.0/(mParameters.gamma1() + mParameters.gamma2()); } void MeiCamera::liftSphere(const Eigen::Vector2d& p, Eigen::Vector3d& P) const { double mx_d, my_d,mx2_d, mxy_d, my2_d, mx_u, my_u; double rho2_d, rho4_d, radDist_d, Dx_d, Dy_d, inv_denom_d; double lambda; mx_d = m_inv_K11 * p(0) + m_inv_K13; my_d = m_inv_K22 * p(1) + m_inv_K23; if (m_noDistortion) { mx_u = mx_d; my_u = my_d; } else { double k1 = mParameters.k1(); double k2 = mParameters.k2(); double p1 = mParameters.p1(); double p2 = mParameters.p2(); mx2_d = mx_d*mx_d; my2_d = my_d*my_d; mxy_d = mx_d*my_d; rho2_d = mx2_d+my2_d; rho4_d = rho2_d*rho2_d; radDist_d = k1*rho2_d+k2*rho4_d; Dx_d = mx_d*radDist_d + p2*(rho2_d+2*mx2_d) + 2*p1*mxy_d; Dy_d = my_d*radDist_d + p1*(rho2_d+2*my2_d) + 2*p2*mxy_d; inv_denom_d = 1/(1+4*k1*rho2_d+6*k2*rho4_d+8*p1*my_d+8*p2*mx_d); mx_u = mx_d - inv_denom_d*Dx_d; my_u = my_d - inv_denom_d*Dy_d; } double xi = mParameters.xi(); if (xi == 1.0) { lambda = 2.0/(mx_u*mx_u + my_u*my_u + 1.0); P << lambda*mx_u, lambda*my_u, lambda - 1.0; } else { lambda = (xi + std::sqrt(1.0 + (1.0 - xi*xi)*(mx_u*mx_u + my_u*my_u))) / (1.0 + mx_u*mx_u + my_u*my_u); P << lambda*mx_u, lambda*my_u, lambda - xi; } } void MeiCamera::liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const { double mx_d, my_d,mx2_d, mxy_d, my2_d, mx_u, my_u; double rho2_d, rho4_d, radDist_d, Dx_d, Dy_d, inv_denom_d; mx_d = m_inv_K11 * p(0) + m_inv_K13; my_d = m_inv_K22 * p(1) + m_inv_K23; if (m_noDistortion) { mx_u = mx_d; my_u = my_d; } else { double k1 = mParameters.k1(); double k2 = mParameters.k2(); double p1 = mParameters.p1(); double p2 = mParameters.p2(); mx2_d = mx_d*mx_d; my2_d = my_d*my_d; mxy_d = mx_d*my_d; rho2_d = mx2_d+my2_d; rho4_d = rho2_d*rho2_d; radDist_d = k1*rho2_d+k2*rho4_d; Dx_d = mx_d*radDist_d + p2*(rho2_d+2*mx2_d) + 2*p1*mxy_d; Dy_d = my_d*radDist_d + p1*(rho2_d+2*my2_d) + 2*p2*mxy_d; inv_denom_d = 1/(1+4*k1*rho2_d+6*k2*rho4_d+8*p1*my_d+8*p2*mx_d); mx_u = mx_d - inv_denom_d*Dx_d; my_u = my_d - inv_denom_d*Dy_d; } double xi = mParameters.xi(); if (xi == 1.0) { P << mx_u, my_u, (1.0 - mx_u*mx_u - my_u*my_u)/2.0; } else { rho2_d = mx_u*mx_u + my_u*my_u; P << mx_u, my_u, 1.0 - xi*(rho2_d + 1.0)/(xi + std::sqrt(1.0 + (1.0 - xi*xi)*rho2_d)); } P(0) = P(0)/P(2); P(1) = P(1)/P(2); P(2) = 1.0; } void MeiCamera::spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p) const { Eigen::Vector2d p_u, p_d; double z = P(2) + mParameters.xi() * P.norm(); p_u << P(0) / z, P(1) / z; if (m_noDistortion) { p_d = p_u; } else { Eigen::Vector2d d_u; distortion(p_u, d_u); p_d = p_u + d_u; } p << mParameters.gamma1()*p_d(0) + mParameters.u0(), mParameters.gamma2()*p_d(1) + mParameters.v0(); } void MeiCamera::undistToPlane(const Eigen::Vector2d& p_u, Eigen::Vector2d& p) const { Eigen::Vector2d p_d; if (m_noDistortion) { p_d = p_u; } else { Eigen::Vector2d d_u; distortion(p_u, d_u); p_d = p_u + d_u; } p << mParameters.gamma1()*p_d(0) + mParameters.u0(), mParameters.gamma2()*p_d(1) + mParameters.v0(); } void MeiCamera::distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u) const { double k1 = mParameters.k1(); double k2 = mParameters.k2(); double p1 = mParameters.p1(); double p2 = mParameters.p2(); double mx2_u, my2_u, mxy_u, rho2_u, rad_dist_u; mx2_u = p_u(0)*p_u(0); my2_u = p_u(1)*p_u(1); mxy_u = p_u(0)*p_u(1); rho2_u = mx2_u + my2_u; rad_dist_u = k1*rho2_u + k2*rho2_u*rho2_u; d_u(0) = p_u(0)*rad_dist_u + 2.0*p1*mxy_u + p2*(rho2_u + 2.0*mx2_u); d_u(1) = p_u(1)*rad_dist_u + 2.0*p2*mxy_u + p1*(rho2_u + 2.0*my2_u); } void MeiCamera::distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u, Eigen::Matrix2d& J) const { double k1 = mParameters.k1(); double k2 = mParameters.k2(); double p1 = mParameters.p1(); double p2 = mParameters.p2(); double mx2_u, my2_u, mxy_u, rho2_u, rad_dist_u; mx2_u = p_u(0)*p_u(0); my2_u = p_u(1)*p_u(1); mxy_u = p_u(0)*p_u(1); rho2_u = mx2_u + my2_u; rad_dist_u = k1*rho2_u + k2*rho2_u*rho2_u; d_u(0) = p_u(0)*rad_dist_u + 2.0*p1*mxy_u + p2*(rho2_u + 2.0*mx2_u); d_u(1) = p_u(1)*rad_dist_u + 2.0*p2*mxy_u + p1*(rho2_u + 2.0*my2_u); J(0, 0) = 1.0 + rad_dist_u + k1*2.0*mx2_u + k2*rho2_u*4.0*mx2_u + 2.0*p1*p_u(1) + 6.0*p2*p_u(0); J(0, 1) = k1*2.0*p_u(0)*p_u(1) + k2*4.0*rho2_u*p_u(0)*p_u(1) + p1*2.0*p_u(0) + 2.0*p2*p_u(1); J(1, 0) = J(0, 1); J(1, 1) = 1.0 + rad_dist_u + k1*2.0*my2_u + k2*rho2_u*4.0*my2_u + 6.0*p1*p_u(1) + 2.0*p2*p_u(0); } int MeiCamera::parameterCount() const { return 9; } const MeiCamera::Parameters& MeiCamera::getParameters() const { return mParameters; } void MeiCamera::setParameters(const MeiCamera::Parameters& parameters) { mParameters = parameters; if ((mParameters.k1() == 0.0) && (mParameters.k2() == 0.0) && (mParameters.p1() == 0.0) && (mParameters.p2() == 0.0)) { m_noDistortion = true; } else { m_noDistortion = false; } m_inv_K11 = 1.0 / mParameters.gamma1(); m_inv_K13 = -mParameters.u0() / mParameters.gamma1(); m_inv_K22 = 1.0 / mParameters.gamma2(); m_inv_K23 = -mParameters.v0() / mParameters.gamma2(); } void MeiCamera::readParameters(const std::vector& parameterVec) { if ((int)parameterVec.size() != parameterCount()) return; Parameters params = getParameters(); params.xi() = parameterVec.at(0); params.k1() = parameterVec.at(1); params.k2() = parameterVec.at(2); params.p1() = parameterVec.at(3); params.p2() = parameterVec.at(4); params.gamma1() = parameterVec.at(5); params.gamma2() = parameterVec.at(6); params.u0() = parameterVec.at(7); params.v0() = parameterVec.at(8); setParameters(params); } void MeiCamera::writeParameters(std::vector& parameterVec) const { parameterVec.resize(parameterCount()); parameterVec.at(0) = mParameters.xi(); parameterVec.at(1) = mParameters.k1(); parameterVec.at(2) = mParameters.k2(); parameterVec.at(3) = mParameters.p1(); parameterVec.at(4) = mParameters.p2(); parameterVec.at(5) = mParameters.gamma1(); parameterVec.at(6) = mParameters.gamma2(); parameterVec.at(7) = mParameters.u0(); parameterVec.at(8) = mParameters.v0(); } void MeiCamera::writeParametersToYamlFile(const std::string& filename) const { mParameters.writeToYamlFile(filename); } std::string MeiCamera::parametersToString() const { std::ostringstream oss; oss << mParameters; return oss.str(); } } ================================================ FILE: camera_model/src/PinholeCamera.cpp ================================================ /** This File is part of Camera Model * * * Copyright (C) 2022 Changwu Liu (cwliu529@163.com, * lcw18@mails.tsinghua.edu.cn (valid until 2023)) * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program. If not, see . * * Since some functions are adapted from GVINS * GVINS is under GPLv3 License. */ #include "PinholeCamera.h" #include namespace camera_model { PinholeCamera::Parameters::Parameters() : Camera::Parameters(PINHOLE), m_k1(0.0), m_k2(0.0), m_p1(0.0), m_p2(0.0), m_fx(0.0), m_fy(0.0), m_cx(0.0), m_cy(0.0) {} PinholeCamera::Parameters::Parameters(const std::string& cameraName, int w, int h, double k1, double k2, double p1, double p2, double fx, double fy, double cx, double cy) : Camera::Parameters(PINHOLE, cameraName, w, h), m_k1(k1), m_k2(k2), m_p1(p1), m_p2(p2), m_fx(fx), m_fy(fy), m_cx(cx), m_cy(cy) {} double& PinholeCamera::Parameters::k1(void) { return m_k1;} double& PinholeCamera::Parameters::k2(void) { return m_k2;} double& PinholeCamera::Parameters::p1(void) { return m_p1;} double& PinholeCamera::Parameters::p2(void) { return m_p2;} double& PinholeCamera::Parameters::fx(void) { return m_fx;} double& PinholeCamera::Parameters::fy(void) { return m_fy;} double& PinholeCamera::Parameters::cx(void) { return m_cx;} double& PinholeCamera::Parameters::cy(void) { return m_cy;} double PinholeCamera::Parameters::k1(void) const { return m_k1;} double PinholeCamera::Parameters::k2(void) const { return m_k2;} double PinholeCamera::Parameters::p1(void) const { return m_p1;} double PinholeCamera::Parameters::p2(void) const { return m_p2;} double PinholeCamera::Parameters::fx(void) const { return m_fx;} double PinholeCamera::Parameters::fy(void) const { return m_fy;} double PinholeCamera::Parameters::cx(void) const { return m_cx;} double PinholeCamera::Parameters::cy(void) const { return m_cy;} bool PinholeCamera::Parameters::readFromYamlFile(const std::string& filename) { cv::FileStorage fs(filename, cv::FileStorage::READ); if (!fs.isOpened()) { return false; } if (!fs["model_type"].isNone()) { std::string sModelType; fs["model_type"] >> sModelType; if (sModelType.compare("PINHOLE") != 0) { return false; } } m_modelType = PINHOLE; fs["camera_name"] >> m_cameraName; m_imageWidth = static_cast(fs["image_width"]); m_imageHeight = static_cast(fs["image_height"]); cv::FileNode n = fs["distortion_parameters"]; m_k1 = static_cast(n["k1"]); m_k2 = static_cast(n["k2"]); m_p1 = static_cast(n["p1"]); m_p2 = static_cast(n["p2"]); n = fs["projection_parameters"]; m_fx = static_cast(n["fx"]); m_fy = static_cast(n["fy"]); m_cx = static_cast(n["cx"]); m_cy = static_cast(n["cy"]); return true; } void PinholeCamera::Parameters::writeToYamlFile(const std::string& filename) const { cv::FileStorage fs(filename, cv::FileStorage::WRITE); fs << "model_type" << "PINHOLE"; fs << "camera_name" << m_cameraName; fs << "image_width" << m_imageWidth; fs << "image_height" << m_imageHeight; // radial distortion: k1, k2 // tangential distortion: p1, p2 fs << "distortion_parameters"; fs << "{" << "k1" << m_k1 << "k2" << m_k2 << "p1" << m_p1 << "p2" << m_p2 << "}"; // projection: fx, fy, cx, cy fs << "projection_parameters"; fs << "{" << "fx" << m_fx << "fy" << m_fy << "cx" << m_cx << "cy" << m_cy << "}"; fs.release(); } PinholeCamera::Parameters& PinholeCamera::Parameters::operator=(const PinholeCamera::Parameters& other) { if (this != &other) { m_modelType = other.m_modelType; m_cameraName = other.m_cameraName; m_imageWidth = other.m_imageWidth; m_imageHeight = other.m_imageHeight; m_k1 = other.m_k1; m_k2 = other.m_k2; m_p1 = other.m_p1; m_p2 = other.m_p2; m_fx = other.m_fx; m_fy = other.m_fy; m_cx = other.m_cx; m_cy = other.m_cy; } return *this; } std::ostream& operator<< (std::ostream& out, const PinholeCamera::Parameters& params) { out << "Camera Parameters:" << std::endl; out << " model_type " << "PINHOLE" << std::endl; out << " camera_name " << params.m_cameraName << std::endl; out << " image_width " << params.m_imageWidth << std::endl; out << " image_height " << params.m_imageHeight << std::endl; // radial distortion: k1, k2 // tangential distortion: p1, p2 out << "Distortion Parameters" << std::endl; out << " k1 " << params.m_k1 << std::endl << " k2 " << params.m_k2 << std::endl << " p1 " << params.m_p1 << std::endl << " p2 " << params.m_p2 << std::endl; // projection: fx, fy, cx, cy out << "Projection Parameters" << std::endl; out << " fx " << params.m_fx << std::endl << " fy " << params.m_fy << std::endl << " cx " << params.m_cx << std::endl << " cy " << params.m_cy << std::endl; return out; } PinholeCamera::PinholeCamera() : m_inv_K11(1.0), m_inv_K13(0.0), m_inv_K22(1.0), m_inv_K23(0.0), m_noDistortion(true) {} PinholeCamera::PinholeCamera(const std::string& cameraName, int imageWidth, int imageHeight, double k1, double k2, double p1, double p2, double fx, double fy, double cx, double cy) : mParameters(cameraName, imageWidth, imageHeight, k1, k2, p1, p2, fx, fy, cx, cy) { if ((mParameters.k1() == 0.0) && (mParameters.k2() == 0.0) && (mParameters.p1() == 0.0) && (mParameters.p2() == 0.0)) { m_noDistortion = true; } else { m_noDistortion = false; } m_inv_K11 = 1.0 / mParameters.fx(); m_inv_K13 = -mParameters.cx() / mParameters.fx(); m_inv_K22 = 1.0 / mParameters.fy(); m_inv_K23 = -mParameters.cy() / mParameters.fy(); } PinholeCamera::PinholeCamera(const PinholeCamera::Parameters& params) : mParameters(params) { if ((mParameters.k1() == 0.0) && (mParameters.k2() == 0.0) && (mParameters.p1() == 0.0) && (mParameters.p2() == 0.0)) { m_noDistortion = true; } else { m_noDistortion = false; } m_inv_K11 = 1.0 / mParameters.fx(); m_inv_K13 = -mParameters.cx() / mParameters.fx(); m_inv_K22 = 1.0 / mParameters.fy(); m_inv_K23 = -mParameters.cy() / mParameters.fy(); } Camera::ModelType PinholeCamera::modelType(void) const { return mParameters.modelType(); } const std::string& PinholeCamera::cameraName(void) const { return mParameters.cameraName(); } int PinholeCamera::imageWidth(void) const { return mParameters.imageWidth(); } int PinholeCamera::imageHeight(void) const { return mParameters.imageHeight(); } double PinholeCamera::getNormalPixel() const { return 2.0/(mParameters.fx() + mParameters.fy()); } void PinholeCamera::liftSphere(const Eigen::Vector2d& p, Eigen::Vector3d& P) const { liftProjective(p, P); P.normalize(); } void PinholeCamera::liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const { double mx_d, my_d,mx2_d, mxy_d, my2_d, mx_u, my_u; double rho2_d, rho4_d, radDist_d, Dx_d, Dy_d, inv_denom_d; mx_d = m_inv_K11 * p(0) + m_inv_K13; my_d = m_inv_K22 * p(1) + m_inv_K23; if (m_noDistortion) { mx_u = mx_d; my_u = my_d; } else { double k1 = mParameters.k1(); double k2 = mParameters.k2(); double p1 = mParameters.p1(); double p2 = mParameters.p2(); mx2_d = mx_d*mx_d; my2_d = my_d*my_d; mxy_d = mx_d*my_d; rho2_d = mx2_d+my2_d; rho4_d = rho2_d*rho2_d; radDist_d = k1*rho2_d+k2*rho4_d; Dx_d = mx_d*radDist_d + p2*(rho2_d+2*mx2_d) + 2*p1*mxy_d; Dy_d = my_d*radDist_d + p1*(rho2_d+2*my2_d) + 2*p2*mxy_d; inv_denom_d = 1/(1+4*k1*rho2_d+6*k2*rho4_d+8*p1*my_d+8*p2*mx_d); mx_u = mx_d - inv_denom_d*Dx_d; my_u = my_d - inv_denom_d*Dy_d; } P << mx_u, my_u, 1.0; } void PinholeCamera::spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p) const { Eigen::Vector2d p_u, p_d; p_u << P(0) / P(2), P(1) / P(2); if (m_noDistortion) { p_d = p_u; } else { Eigen::Vector2d d_u; distortion(p_u, d_u); p_d = p_u + d_u; } p << mParameters.fx() * p_d(0) + mParameters.cx(), mParameters.fy() * p_d(1) + mParameters.cy(); } void PinholeCamera::spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p, Eigen::Matrix& J) const { Eigen::Vector2d p_u, p_d; double norm, inv_denom; double dxdmx, dydmx, dxdmy, dydmy; norm = P.norm(); inv_denom = 1.0 / P(2); p_u << inv_denom * P(0), inv_denom * P(1); double dudx = inv_denom; double dvdx = 0.0; double dudy = 0.0; double dvdy = inv_denom; inv_denom = - inv_denom * inv_denom; double dudz = P(0) * inv_denom; double dvdz = P(1) * inv_denom; if (m_noDistortion) { p_d = p_u; } else { Eigen::Vector2d d_u; distortion(p_u, d_u); p_d = p_u + d_u; } double fx = mParameters.fx(); double fy = mParameters.fy(); // Make the product of the jacobians // and add projection matrix jacobian inv_denom = fx * (dudx * dxdmx + dvdx * dxdmy); // reuse dvdx = fy * (dudx * dydmx + dvdx * dydmy); dudx = inv_denom; inv_denom = fx * (dudy * dxdmx + dvdy * dxdmy); // reuse dvdy = fy * (dudy * dydmx + dvdy * dydmy); dudy = inv_denom; inv_denom = fx * (dudz * dxdmx + dvdz * dxdmy); // reuse dvdz = fy * (dudz * dydmx + dvdz * dydmy); dudz = inv_denom; // Apply generalised projection matrix p << fx * p_d(0) + mParameters.cx(), fy * p_d(1) + mParameters.cy(); J << dudx, dudy, dudz, dvdx, dvdy, dvdz; } void PinholeCamera::undistToPlane(const Eigen::Vector2d& p_u, Eigen::Vector2d& p) const { Eigen::Vector2d p_d; if (m_noDistortion) { p_d = p_u; } else { // Apply distortion Eigen::Vector2d d_u; distortion(p_u, d_u); p_d = p_u + d_u; } // Apply generalised projection matrix p << mParameters.fx() * p_d(0) + mParameters.cx(), mParameters.fy() * p_d(1) + mParameters.cy(); } void PinholeCamera::distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u) const { double k1 = mParameters.k1(); double k2 = mParameters.k2(); double p1 = mParameters.p1(); double p2 = mParameters.p2(); double mx2_u, my2_u, mxy_u, rho2_u, rad_dist_u; mx2_u = p_u(0) * p_u(0); my2_u = p_u(1) * p_u(1); mxy_u = p_u(0) * p_u(1); rho2_u = mx2_u + my2_u; rad_dist_u = k1 * rho2_u + k2 * rho2_u * rho2_u; d_u << p_u(0) * rad_dist_u + 2.0 * p1 * mxy_u + p2 * (rho2_u + 2.0 * mx2_u), p_u(1) * rad_dist_u + 2.0 * p2 * mxy_u + p1 * (rho2_u + 2.0 * my2_u); } void PinholeCamera::distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u, Eigen::Matrix2d& J) const { double k1 = mParameters.k1(); double k2 = mParameters.k2(); double p1 = mParameters.p1(); double p2 = mParameters.p2(); double mx2_u, my2_u, mxy_u, rho2_u, rad_dist_u; mx2_u = p_u(0) * p_u(0); my2_u = p_u(1) * p_u(1); mxy_u = p_u(0) * p_u(1); rho2_u = mx2_u + my2_u; rad_dist_u = k1 * rho2_u + k2 * rho2_u * rho2_u; d_u << p_u(0) * rad_dist_u + 2.0 * p1 * mxy_u + p2 * (rho2_u + 2.0 * mx2_u), p_u(1) * rad_dist_u + 2.0 * p2 * mxy_u + p1 * (rho2_u + 2.0 * my2_u); double dxdmx = 1.0 + rad_dist_u + k1 * 2.0 * mx2_u + k2 * rho2_u * 4.0 * mx2_u + 2.0 * p1 * p_u(1) + 6.0 * p2 * p_u(0); double dydmx = k1 * 2.0 * p_u(0) * p_u(1) + k2 * 4.0 * rho2_u * p_u(0) * p_u(1) + p1 * 2.0 * p_u(0) + 2.0 * p2 * p_u(1); double dxdmy = dydmx; double dydmy = 1.0 + rad_dist_u + k1 * 2.0 * my2_u + k2 * rho2_u * 4.0 * my2_u + 6.0 * p1 * p_u(1) + 2.0 * p2 * p_u(0); J << dxdmx, dxdmy, dydmx, dydmy; } int PinholeCamera::parameterCount(void) const { return 8; } const PinholeCamera::Parameters& PinholeCamera::getParameters(void) const { return mParameters; } void PinholeCamera::setParameters(const PinholeCamera::Parameters& parameters) { mParameters = parameters; if ((mParameters.k1() == 0.0) && (mParameters.k2() == 0.0) && (mParameters.p1() == 0.0) && (mParameters.p2() == 0.0)) { m_noDistortion = true; } else { m_noDistortion = false; } m_inv_K11 = 1.0 / mParameters.fx(); m_inv_K13 = -mParameters.cx() / mParameters.fx(); m_inv_K22 = 1.0 / mParameters.fy(); m_inv_K23 = -mParameters.cy() / mParameters.fy(); } void PinholeCamera::readParameters(const std::vector& parameterVec) { if ((int)parameterVec.size() != parameterCount()) { return; } Parameters params = getParameters(); params.k1() = parameterVec.at(0); params.k2() = parameterVec.at(1); params.p1() = parameterVec.at(2); params.p2() = parameterVec.at(3); params.fx() = parameterVec.at(4); params.fy() = parameterVec.at(5); params.cx() = parameterVec.at(6); params.cy() = parameterVec.at(7); setParameters(params); } void PinholeCamera::writeParameters(std::vector& parameterVec) const { parameterVec.resize(parameterCount()); parameterVec.at(0) = mParameters.k1(); parameterVec.at(1) = mParameters.k2(); parameterVec.at(2) = mParameters.p1(); parameterVec.at(3) = mParameters.p2(); parameterVec.at(4) = mParameters.fx(); parameterVec.at(5) = mParameters.fy(); parameterVec.at(6) = mParameters.cx(); parameterVec.at(7) = mParameters.cy(); } void PinholeCamera::writeParametersToYamlFile(const std::string& filename) const { mParameters.writeToYamlFile(filename); } std::string PinholeCamera::parametersToString(void) const { std::ostringstream oss; oss << mParameters; return oss.str(); } } ================================================ FILE: camera_model/src/camera.cpp ================================================ /** This File is part of Camera Model * * * Copyright (C) 2022 Changwu Liu (cwliu529@163.com, * lcw18@mails.tsinghua.edu.cn (valid until 2023)) * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program. If not, see . * * Since some functions are adapted from GVINS * GVINS is under GPLv3 License. */ #include "camera.h" namespace camera_model { Camera::Parameters::Parameters(ModelType modelType): m_modelType(modelType), m_imageWidth(0), m_imageHeight(0) { switch (modelType) { case PINHOLE: m_nIntrinsics = 8; break; case MEI: default: m_nIntrinsics = 9; } } Camera::Parameters::Parameters(ModelType modelType, const std::string& cameraName, int w, int h): m_modelType(modelType), m_cameraName(cameraName), m_imageWidth(w), m_imageHeight(h) { switch (modelType) { case PINHOLE: m_nIntrinsics = 8; break; case MEI: default: m_nIntrinsics = 9; } } Camera::ModelType& Camera::Parameters::modelType() { return m_modelType; } std::string& Camera::Parameters::cameraName() { return m_cameraName; } int& Camera::Parameters::imageWidth() { return m_imageWidth; } int& Camera::Parameters::imageHeight() { return m_imageHeight; } Camera::ModelType Camera::Parameters::modelType() const { return m_modelType; } const std::string& Camera::Parameters::cameraName() const { return m_cameraName; } int Camera::Parameters::imageWidth() const { return m_imageWidth; } int Camera::Parameters::imageHeight() const { return m_imageHeight; } int Camera::Parameters::nIntrinsics() const { return m_nIntrinsics; } cv::Mat& Camera::mask() { return m_mask; } const cv::Mat& Camera::mask() const { return m_mask; } double Camera::reprojectionDist(const Eigen::Vector3d& P1, const Eigen::Vector3d& P2) const { Eigen::Vector2d p1, p2; spaceToPlane(P1, p1); spaceToPlane(P2, p2); return (p1-p2).norm(); } double Camera::reprojectionError(const Eigen::Vector3d& P, const Eigen::Quaterniond& camera_q, const Eigen::Vector3d& camera_t, const Eigen::Vector2d& observed_p) const { Eigen::Vector3d P_cam = camera_q.toRotationMatrix() * P + camera_t; Eigen::Vector2d p; spaceToPlane(P_cam, p); return (p - observed_p).norm(); } } ================================================ FILE: camera_model/src/camera_factory.cpp ================================================ /** This File is part of Camera Model * * * Copyright (C) 2022 Changwu Liu (cwliu529@163.com, * lcw18@mails.tsinghua.edu.cn (valid until 2023)) * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program. If not, see . * * Since some functions are adapted from GVINS * GVINS is under GPLv3 License. */ #include "camera_factory.h" #include "MeiCamera.h" #include "PinholeCamera.h" #include namespace camera_model { boost::shared_ptr CameraFactory::m_instance; CameraFactory::CameraFactory() {} boost::shared_ptr CameraFactory::instance(void) { if (m_instance.get() == 0) m_instance.reset(new CameraFactory); return m_instance; } CameraPtr CameraFactory::generateCamera(Camera::ModelType modelType, const std::string& cameraName, cv::Size imageSize) const { switch (modelType) { case Camera::PINHOLE: { PinholeCameraPtr camera(new PinholeCamera); PinholeCamera::Parameters params = camera->getParameters(); params.cameraName() = cameraName; params.imageWidth() = imageSize.width; params.imageHeight() = imageSize.height; camera->setParameters(params); return camera; } case Camera::MEI: default: { MeiCameraPtr camera(new MeiCamera); MeiCamera::Parameters params = camera->getParameters(); params.cameraName() = cameraName; params.imageWidth() = imageSize.width; params.imageHeight() = imageSize.height; camera->setParameters(params); return camera; } } } CameraPtr CameraFactory::generateCameraFromYamlFile(const std::string& filename) { cv::FileStorage fs(filename, cv::FileStorage::READ); if (!fs.isOpened()) return CameraPtr(); Camera::ModelType modelType = Camera::MEI; if (!fs["model_type"].isNone()) { std::string sModelType; fs["model_type"] >> sModelType; if (boost::iequals(sModelType, "MEI")) { modelType = Camera::MEI; } else if (boost::iequals(sModelType, "PINHOLE")) { modelType = Camera::PINHOLE; } else { std::cerr << "# ERROR: Unknown camera model type: " << sModelType << std::endl; return CameraPtr(); } } switch (modelType) { case Camera::PINHOLE: { PinholeCameraPtr camera(new PinholeCamera); PinholeCamera::Parameters params = camera->getParameters(); params.readFromYamlFile(filename); camera->setParameters(params); return camera; } case Camera::MEI: default: { MeiCameraPtr camera(new MeiCamera); MeiCamera::Parameters params = camera->getParameters(); params.readFromYamlFile(filename); camera->setParameters(params); return camera; } } return CameraPtr(); } } ================================================ FILE: camera_model/test/CMakeLists.txt ================================================ # find_package(GTest REQUIRED) # include_directories(${GTEST_INCLUDE_DIRS}) # add each test source file and executable items # add_executable(test_common test_common.cpp) # target_link_libraries(test_common ${GTEST_BOTH_LIBRARIES} pthread) # gtest_discover_tests(test_common) ================================================ FILE: config/fw_zed2i_f9p/ingvio_mono.yaml ================================================ %YAML:1.0 # config for fw_zed2i_f9p imu_topic: "/imu0" feature_topic: "/mono_tracker/mono_feature" cam_nums: 1 cam_left_file_path: "/home/lcw/VIO/ws_ingvio/src/config/fw_zed2i_f9p/mono_config.yaml" max_sliding_window_poses: 25 is_key_frame: 1 max_landmark_features: 0 enable_gnss: 1 noise_gyro: 0.005 noise_accel: 0.09 noise_bias_gyro: 0.0005 noise_bias_accel: 0.009 noise_rcv_clockbias: 2.0 noise_rcv_clockbias_randomwalk: 0.2 init_cov_rot: 0.0 init_cov_pos: 0.0 init_cov_vel: 0.25 init_cov_bg: 0.01 init_cov_ba: 0.01 init_cov_ext_rot: 1.8e-02 init_cov_ext_pos: 2e-03 init_cov_rcv_clockbias: 2.0 init_cov_rcv_clockbias_randomwalk: 1.0 init_cov_yof: 0.015 gravity_norm: 9.8 max_imu_buffer_size: 10000 init_imu_buffer_sp: 100 trans_thres: 0.25 huber_epsilon: 0.01 conv_precision: 5e-08 init_damping: 1e-03 outer_loop_max_iter: 10 inner_loop_max_iter: 10 max_depth: 80.0 min_depth: 0.2 max_baseline_ratio: 80.0 chi2_max_dof: 150 chi2_thres: 0.95 visual_noise: 0.08 frame_select_interval: 23 gnss_ephem_topic: "/ublox_driver/ephem" gnss_glo_ephem_topic: "/ublox_driver/glo_ephem" gnss_meas_topic: "/ublox_driver/range_meas" gnss_iono_params_topic: "/ublox_driver/iono_params" rtk_gt_topic: "/ublox_driver/receiver_lla" gnss_elevation_thres: 20.0 gnss_psr_std_thres: 8.0 gnss_dopp_std_thres: 8.0 gnss_track_num_thres: 20 use_fix_time_offset: 1 gnss_local_offset: -970047.18 gnss_chi2_test: 0 gnss_strong_reject: 0 gv_align_batch_size: 25 gv_align_max_iter: 10 gv_align_conv_epsilon: 1e-05 gv_align_vel_thres: 0.4 psr_noise_amp: 1.0 dopp_noise_amp: 1.0 is_adjust_yof: 0 ================================================ FILE: config/fw_zed2i_f9p/ingvio_stereo.yaml ================================================ %YAML:1.0 # config for fw_zed2i_f9p imu_topic: "/imu0" feature_topic: "/stereo_tracker/stereo_feature" cam_nums: 2 cam_left_file_path: "/home/lcw/VIO/ws_ingvio/src/config/fw_zed2i_f9p/stereo_left_config.yaml" cam_right_file_path: "/home/lcw/VIO/ws_ingvio/src/config/fw_zed2i_f9p/stereo_right_config.yaml" max_sliding_window_poses: 21 is_key_frame: 1 max_landmark_features: 0 enable_gnss: 1 noise_gyro: 0.005 noise_accel: 0.09 noise_bias_gyro: 0.0005 noise_bias_accel: 0.009 noise_rcv_clockbias: 2.0 noise_rcv_clockbias_randomwalk: 0.2 init_cov_rot: 0.0 init_cov_pos: 0.0 init_cov_vel: 0.25 init_cov_bg: 0.01 init_cov_ba: 0.01 init_cov_ext_rot: 1.8e-02 init_cov_ext_pos: 2e-03 init_cov_rcv_clockbias: 2.0 init_cov_rcv_clockbias_randomwalk: 1.0 init_cov_yof: 0.015 gravity_norm: 9.8 max_imu_buffer_size: 10000 init_imu_buffer_sp: 100 trans_thres: 0.25 huber_epsilon: 0.01 conv_precision: 5e-08 init_damping: 1e-03 outer_loop_max_iter: 10 inner_loop_max_iter: 10 max_depth: 80.0 min_depth: 0.2 max_baseline_ratio: 80.0 chi2_max_dof: 150 chi2_thres: 0.95 visual_noise: 0.08 frame_select_interval: 18 gnss_ephem_topic: "/ublox_driver/ephem" gnss_glo_ephem_topic: "/ublox_driver/glo_ephem" gnss_meas_topic: "/ublox_driver/range_meas" gnss_iono_params_topic: "/ublox_driver/iono_params" rtk_gt_topic: "/ublox_driver/receiver_lla" gnss_elevation_thres: 20.0 gnss_psr_std_thres: 8.0 gnss_dopp_std_thres: 8.0 gnss_track_num_thres: 20 use_fix_time_offset: 1 gnss_local_offset: -970047.18 gnss_chi2_test: 0 gnss_strong_reject: 0 gv_align_batch_size: 25 gv_align_max_iter: 10 gv_align_conv_epsilon: 1e-05 gv_align_vel_thres: 0.4 psr_noise_amp: 1.0 dopp_noise_amp: 1.0 is_adjust_yof: 0 ================================================ FILE: config/fw_zed2i_f9p/mono_config.yaml ================================================ %YAML:1.0 # config for fw_zed2i_f9p # common parameters imu_topic: "/imu0" image_topic: "/cam0/image_raw" # intrinsic model_type: PINHOLE camera_name: cam0 image_width: 1280 image_height: 720 distortion_parameters: k1: -0.013323572637177162 k2: 0.012297484726727028 p1: -0.0008173313551158254 p2: -0.0036129297585792528 projection_parameters: fx: 528.0821562999166 fy: 528.1818555071027 cx: 646.6309796410819 cy: 351.87255392593374 # extrinsic extrinsicRotation: !!opencv-matrix # R^{imu}_{cam} rows: 3 cols: 3 dt: d data: [0.01301095634388352, -0.005070623738001712, 0.9999024971415602, -0.9999097972900367, 0.003267771869320563, 0.01302762259241452, -0.003333511424589375, -0.9999818050554322, -0.005027649516865795] extrinsicTranslation: !!opencv-matrix # t^{imu}_{cam} rows: 3 cols: 1 dt: d data: [0.03671615355111148, 0.02169026717979497, -0.0000578135929000942] # feature tracker parameters max_cnt: 100 # max feature number in feature tracking min_dist: 35 # min distance between two features freq: 0 # freq (Hz) of publishing tracking result. At least 10Hz for good # estimation. If set 0, the frequence will be the same as raw image F_threshold: 1.0 # ransac threshold (pixel) show_track: 1 # publish tracking image as topic show_timer: 0 # show time counting timer_warning_thres: 50.0 # in (ms) equalize: 1 # if image is too dark or light, # turn on equalize to find enough features ================================================ FILE: config/fw_zed2i_f9p/stereo_left_config.yaml ================================================ %YAML:1.0 # config for fw_zed2i_f9p #topic parameters imu_topic: "/imu0" cam_left_topic: "/cam0/image_raw" cam_right_topic: "/cam1/image_raw" # intrinsic model_type: PINHOLE camera_name: cam0 image_width: 1280 image_height: 720 distortion_parameters: k1: -0.013323572637177162 k2: 0.012297484726727028 p1: -0.0008173313551158254 p2: -0.0036129297585792528 projection_parameters: fx: 528.0821562999166 fy: 528.1818555071027 cx: 646.6309796410819 cy: 351.87255392593374 # extrinsic extrinsicRotation: !!opencv-matrix # R^{imu}_{cam} rows: 3 cols: 3 dt: d data: [0.01301095634388352, -0.005070623738001712, 0.9999024971415602, -0.9999097972900367, 0.003267771869320563, 0.01302762259241452, -0.003333511424589375, -0.9999818050554322, -0.005027649516865795] extrinsicTranslation: !!opencv-matrix # t^{imu}_{cam} rows: 3 cols: 1 dt: d data: [0.03671615355111148, 0.02169026717979497, -0.0000578135929000942] # tracker config max_cnt: 60 # max feature number in feature tracking min_dist: 45 # min distance between two features freq: 0 # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be the same as raw image show_track: 1 # publish tracking image as topic show_timer: 0 # show time counting timer_warning_thres: 50.0 # in (ms) equalize: 1 # if image is too dark or light, turn on equalize to find enough features window_size: 20 epipolar_thres: 15.0 F_threshold: 1.0 print_track_info: 0 ================================================ FILE: config/fw_zed2i_f9p/stereo_right_config.yaml ================================================ %YAML:1.0 # config for fw_zed2i_f9p # intrinsic model_type: PINHOLE camera_name: cam1 image_width: 1280 image_height: 720 distortion_parameters: k1: -0.013705400292675644 k2: 0.01351956571839721 p1: -0.000806913038206586 p2: -0.0028638404748635034 projection_parameters: fx: 527.4644311562542 fy: 527.9383388808839 cx: 647.6983568199339 cy: 351.1977685749355 # extrinsic extrinsicRotation: !!opencv-matrix # R^{imu}_{cam} rows: 3 cols: 3 dt: d data: [ 0.01129080808837021, -0.004471320310557636, 0.9999262597548844, -0.9999301632412926, 0.00344058703135805, 0.01130623726631103, -0.003490887129951881,-0.9999840847011319, -0.004432161040157589] extrinsicTranslation: !!opencv-matrix # t^{imu}_{cam} rows: 3 cols: 1 dt: d data: [0.03767186874423049, -0.0980688527108189, -0.0002632245838794373] ================================================ FILE: config/sportsfield/ingvio_mono.yaml ================================================ %YAML:1.0 # config for sportsfield imu_topic: "/imu0" feature_topic: "/mono_tracker/mono_feature" cam_nums: 1 cam_left_file_path: "/home/lcw/VIO/ws_ingvio/src/config/sportsfield/mono_config.yaml" max_sliding_window_poses: 35 is_key_frame: 1 max_landmark_features: 0 enable_gnss: 1 noise_gyro: 0.004 noise_accel: 0.08 noise_bias_gyro: 0.0002 noise_bias_accel: 0.008 noise_rcv_clockbias: 2.0 noise_rcv_clockbias_randomwalk: 0.2 init_cov_rot: 0.0 init_cov_pos: 0.0 init_cov_vel: 0.25 init_cov_bg: 0.01 init_cov_ba: 0.01 init_cov_ext_rot: 1.8e-02 init_cov_ext_pos: 2e-03 init_cov_rcv_clockbias: 2.0 init_cov_rcv_clockbias_randomwalk: 1.0 init_cov_yof: 0.015 gravity_norm: 9.8 max_imu_buffer_size: 3000 init_imu_buffer_sp: 300 trans_thres: 0.25 huber_epsilon: 0.01 conv_precision: 5e-08 init_damping: 1e-03 outer_loop_max_iter: 10 inner_loop_max_iter: 10 max_depth: 60.0 min_depth: 0.2 max_baseline_ratio: 80.0 chi2_max_dof: 150 chi2_thres: 0.95 visual_noise: 0.18 frame_select_interval: 28 gnss_ephem_topic: "/ublox_driver/ephem" gnss_glo_ephem_topic: "/ublox_driver/glo_ephem" gnss_meas_topic: "/ublox_driver/range_meas" gnss_iono_params_topic: "/ublox_driver/iono_params" rtk_gt_topic: "/ublox_driver/receiver_lla" gnss_elevation_thres: 20.0 gnss_psr_std_thres: 8.0 gnss_dopp_std_thres: 8.0 gnss_track_num_thres: 20 use_fix_time_offset: 1 gnss_local_offset: -18.0 gnss_chi2_test: 0 gnss_strong_reject: 1 gv_align_batch_size: 45 gv_align_max_iter: 25 gv_align_conv_epsilon: 1e-05 gv_align_vel_thres: 0.4 psr_noise_amp: 1.0 dopp_noise_amp: 1.0 is_adjust_yof: 0 ================================================ FILE: config/sportsfield/ingvio_stereo.yaml ================================================ %YAML:1.0 # config for sportsfield imu_topic: "/imu0" feature_topic: "/stereo_tracker/stereo_feature" cam_nums: 2 cam_left_file_path: "/home/lcw/VIO/ws_ingvio/src/config/sportsfield/stereo_left_config.yaml" cam_right_file_path: "/home/lcw/VIO/ws_ingvio/src/config/sportsfield/stereo_right_config.yaml" max_sliding_window_poses: 27 is_key_frame: 1 max_landmark_features: 0 enable_gnss: 1 noise_gyro: 0.004 noise_accel: 0.08 noise_bias_gyro: 0.0002 noise_bias_accel: 0.008 noise_rcv_clockbias: 2.0 noise_rcv_clockbias_randomwalk: 0.2 init_cov_rot: 0.0 init_cov_pos: 0.0 init_cov_vel: 0.25 init_cov_bg: 0.01 init_cov_ba: 0.01 init_cov_ext_rot: 1.8e-02 init_cov_ext_pos: 2e-03 init_cov_rcv_clockbias: 2.0 init_cov_rcv_clockbias_randomwalk: 1.0 init_cov_yof: 0.015 gravity_norm: 9.8 max_imu_buffer_size: 3000 init_imu_buffer_sp: 300 trans_thres: 0.25 huber_epsilon: 0.01 conv_precision: 5e-07 init_damping: 1e-03 outer_loop_max_iter: 10 inner_loop_max_iter: 10 max_depth: 40.0 min_depth: 0.2 max_baseline_ratio: 80.0 chi2_max_dof: 150 chi2_thres: 0.95 visual_noise: 0.18 frame_select_interval: 18 gnss_ephem_topic: "/ublox_driver/ephem" gnss_glo_ephem_topic: "/ublox_driver/glo_ephem" gnss_meas_topic: "/ublox_driver/range_meas" gnss_iono_params_topic: "/ublox_driver/iono_params" rtk_gt_topic: "/ublox_driver/receiver_lla" gnss_elevation_thres: 20.0 gnss_psr_std_thres: 8.0 gnss_dopp_std_thres: 8.0 gnss_track_num_thres: 20 use_fix_time_offset: 1 gnss_local_offset: -18.0 gnss_chi2_test: 0 gnss_strong_reject: 1 gv_align_batch_size: 25 gv_align_max_iter: 10 gv_align_conv_epsilon: 1e-05 gv_align_vel_thres: 0.4 psr_noise_amp: 1.0 dopp_noise_amp: 1.0 is_adjust_yof: 0 ================================================ FILE: config/sportsfield/mono_config.yaml ================================================ %YAML:1.0 # config for sportsfield # common parameters imu_topic: "/imu0" image_topic: "/cam1/image_raw" # camera calibration model_type: MEI camera_name: camera image_width: 752 image_height: 480 mirror_parameters: xi: 1.8476540167437447 distortion_parameters: k1: -0.06597811223735722 k2: 0.8559479340704287 p1: -0.0006445829733139821 p2: 0.0015137487236065916 projection_parameters: gamma1: 1338.1845333957547 gamma2: 1340.1190112672946 u0: 378.7909740462579 v0: 217.69105287172025 # extrinsic extrinsicRotation: !!opencv-matrix # R^{imu}_{cam} rows: 3 cols: 3 dt: d data: [ 0.9999014076382304, -0.0133731297219721, 0.0042818692791948, 0.0133731003056063, 0.9999105754655292, 0.0000355022536769, -0.0042819611512717, 0.0000217631139403, 0.9999908321255077 ] extrinsicTranslation: !!opencv-matrix # t^{imu}_{cam} rows: 3 cols: 1 dt: d data: [0.0341738532732442, -0.0032623030537933, -0.0017782029037505] # feature tracker parameters max_cnt: 150 # max feature number in feature tracking min_dist: 30 # min distance between two features freq: 0 # freq (Hz) of publishing tracking result. At least 10Hz for good # estimation. If set 0, the frequence will be the same as raw image F_threshold: 1.0 # ransac threshold (pixel) show_track: 1 # publish tracking image as topic show_timer: 0 # show time counting timer_warning_thres: 50.0 # in (ms) equalize: 1 # if image is too dark or light, # turn on equalize to find enough features ================================================ FILE: config/sportsfield/stereo_left_config.yaml ================================================ %YAML:1.0 # config for sportsfield #topic parameters imu_topic: "/imu0" cam_left_topic: "/cam1/image_raw" cam_right_topic: "/cam0/image_raw" # intrinsic model_type: MEI camera_name: cam1 image_width: 752 image_height: 480 mirror_parameters: xi: 1.8476540167437447 distortion_parameters: k1: -0.06597811223735722 k2: 0.8559479340704287 p1: -0.0006445829733139821 p2: 0.0015137487236065916 projection_parameters: gamma1: 1338.1845333957547 gamma2: 1340.1190112672946 u0: 378.7909740462579 v0: 217.69105287172025 # extrinsic extrinsicRotation: !!opencv-matrix # R^{imu}_{cam} rows: 3 cols: 3 dt: d data: [0.9999890386957373, -0.0043227774403168, 0.0017989117755288, 0.0043276579084841, 0.9999869417854389, -0.0027180205355500, -0.0017871388870994, 0.0027257758172719, 0.9999946881262878] extrinsicTranslation: !!opencv-matrix # t^{imu}_{cam} rows: 3 cols: 1 dt: d data: [-0.0759472920952561, -0.0039320527565750, -0.0016395029500217] # tracker config max_cnt: 110 # max feature number in feature tracking min_dist: 30 # min distance between two features freq: 0 # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be the same as raw image show_track: 1 # publish tracking image as topic show_timer: 0 # show time counting timer_warning_thres: 50.0 # in (ms) equalize: 1 # if image is too dark or light, turn on equalize to find enough features window_size: 20 epipolar_thres: 25.0 F_threshold: 2.0 print_track_info: 0 ================================================ FILE: config/sportsfield/stereo_right_config.yaml ================================================ %YAML:1.0 # config for sportsfield # intrinsic model_type: MEI camera_name: cam0 image_width: 752 image_height: 480 mirror_parameters: xi: 1.8482000080983196 distortion_parameters: k1: -0.05322412328104923 k2: 0.7676625579468673 p1: -0.00019942420435829978 p2: -0.0006936085281739436 projection_parameters: gamma1: 1330.3922281659666 gamma2: 1332.49270401596 u0: 377.39301029010045 v0: 233.04955276185672 # extrinsic extrinsicRotation: !!opencv-matrix # R^{imu}_{cam} rows: 3 cols: 3 dt: d data: [ 0.9999014076382304, -0.0133731297219721, 0.0042818692791948, 0.0133731003056063, 0.9999105754655292, 0.0000355022536769, -0.0042819611512717, 0.0000217631139403, 0.9999908321255077 ] extrinsicTranslation: !!opencv-matrix # t^{imu}_{cam} rows: 3 cols: 1 dt: d data: [0.0341738532732442, -0.0032623030537933, -0.0017782029037505] ================================================ FILE: feature_tracker/CMakeLists.txt ================================================ cmake_minimum_required(VERSION 2.8.3) project(feature_tracker) set(CMAKE_BUILD_TYPE "Release") set(CMAKE_CXX_FLAGS "-std=c++11") find_package(catkin REQUIRED COMPONENTS roscpp std_msgs message_filters sensor_msgs cv_bridge camera_model message_generation ) find_package(Boost REQUIRED COMPONENTS filesystem program_options system) find_package(OpenCV REQUIRED) add_message_files( FILES MonoMeas.msg MonoFrame.msg StereoMeas.msg StereoFrame.msg ) generate_messages( DEPENDENCIES std_msgs geometry_msgs ) catkin_package() include_directories( ${PROJECT_SOURCE_DIR}/include "/usr/include/eigen3" ${catkin_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS} ) add_executable(mono_tracker src/mono_tracker.cpp src/mono_tracker_node.cpp src/mono_parameters.cpp ) add_executable(stereo_tracker src/stereo_tracker.cpp src/stereo_tracker_node.cpp src/stereo_parameters.cpp ) target_link_libraries(mono_tracker ${catkin_LIBRARIES} ${OpenCV_LIBS}) target_link_libraries(stereo_tracker ${catkin_LIBRARIES} ${OpenCV_LIBS}) ================================================ FILE: feature_tracker/LICENSE ================================================ GNU GENERAL PUBLIC LICENSE Version 3, 29 June 2007 Copyright (C) 2007 Free Software Foundation, Inc. Everyone is permitted to copy and distribute verbatim copies of this license document, but changing it is not allowed. Preamble The GNU General Public License is a free, copyleft license for software and other kinds of works. The licenses for most software and other practical works are designed to take away your freedom to share and change the works. By contrast, the GNU General Public License is intended to guarantee your freedom to share and change all versions of a program--to make sure it remains free software for all its users. We, the Free Software Foundation, use the GNU General Public License for most of our software; it applies also to any other work released this way by its authors. You can apply it to your programs, too. When we speak of free software, we are referring to freedom, not price. Our General Public Licenses are designed to make sure that you have the freedom to distribute copies of free software (and charge for them if you wish), that you receive source code or can get it if you want it, that you can change the software or use pieces of it in new free programs, and that you know you can do these things. To protect your rights, we need to prevent others from denying you these rights or asking you to surrender the rights. Therefore, you have certain responsibilities if you distribute copies of the software, or if you modify it: responsibilities to respect the freedom of others. For example, if you distribute copies of such a program, whether gratis or for a fee, you must pass on to the recipients the same freedoms that you received. You must make sure that they, too, receive or can get the source code. And you must show them these terms so they know their rights. Developers that use the GNU GPL protect your rights with two steps: (1) assert copyright on the software, and (2) offer you this License giving you legal permission to copy, distribute and/or modify it. For the developers' and authors' protection, the GPL clearly explains that there is no warranty for this free software. For both users' and authors' sake, the GPL requires that modified versions be marked as changed, so that their problems will not be attributed erroneously to authors of previous versions. Some devices are designed to deny users access to install or run modified versions of the software inside them, although the manufacturer can do so. This is fundamentally incompatible with the aim of protecting users' freedom to change the software. The systematic pattern of such abuse occurs in the area of products for individuals to use, which is precisely where it is most unacceptable. Therefore, we have designed this version of the GPL to prohibit the practice for those products. If such problems arise substantially in other domains, we stand ready to extend this provision to those domains in future versions of the GPL, as needed to protect the freedom of users. Finally, every program is threatened constantly by software patents. States should not allow patents to restrict development and use of software on general-purpose computers, but in those that do, we wish to avoid the special danger that patents applied to a free program could make it effectively proprietary. To prevent this, the GPL assures that patents cannot be used to render the program non-free. The precise terms and conditions for copying, distribution and modification follow. TERMS AND CONDITIONS 0. Definitions. "This License" refers to version 3 of the GNU General Public License. "Copyright" also means copyright-like laws that apply to other kinds of works, such as semiconductor masks. "The Program" refers to any copyrightable work licensed under this License. Each licensee is addressed as "you". "Licensees" and "recipients" may be individuals or organizations. To "modify" a work means to copy from or adapt all or part of the work in a fashion requiring copyright permission, other than the making of an exact copy. The resulting work is called a "modified version" of the earlier work or a work "based on" the earlier work. A "covered work" means either the unmodified Program or a work based on the Program. To "propagate" a work means to do anything with it that, without permission, would make you directly or secondarily liable for infringement under applicable copyright law, except executing it on a computer or modifying a private copy. Propagation includes copying, distribution (with or without modification), making available to the public, and in some countries other activities as well. To "convey" a work means any kind of propagation that enables other parties to make or receive copies. Mere interaction with a user through a computer network, with no transfer of a copy, is not conveying. An interactive user interface displays "Appropriate Legal Notices" to the extent that it includes a convenient and prominently visible feature that (1) displays an appropriate copyright notice, and (2) tells the user that there is no warranty for the work (except to the extent that warranties are provided), that licensees may convey the work under this License, and how to view a copy of this License. If the interface presents a list of user commands or options, such as a menu, a prominent item in the list meets this criterion. 1. Source Code. The "source code" for a work means the preferred form of the work for making modifications to it. "Object code" means any non-source form of a work. A "Standard Interface" means an interface that either is an official standard defined by a recognized standards body, or, in the case of interfaces specified for a particular programming language, one that is widely used among developers working in that language. The "System Libraries" of an executable work include anything, other than the work as a whole, that (a) is included in the normal form of packaging a Major Component, but which is not part of that Major Component, and (b) serves only to enable use of the work with that Major Component, or to implement a Standard Interface for which an implementation is available to the public in source code form. A "Major Component", in this context, means a major essential component (kernel, window system, and so on) of the specific operating system (if any) on which the executable work runs, or a compiler used to produce the work, or an object code interpreter used to run it. The "Corresponding Source" for a work in object code form means all the source code needed to generate, install, and (for an executable work) run the object code and to modify the work, including scripts to control those activities. However, it does not include the work's System Libraries, or general-purpose tools or generally available free programs which are used unmodified in performing those activities but which are not part of the work. For example, Corresponding Source includes interface definition files associated with source files for the work, and the source code for shared libraries and dynamically linked subprograms that the work is specifically designed to require, such as by intimate data communication or control flow between those subprograms and other parts of the work. The Corresponding Source need not include anything that users can regenerate automatically from other parts of the Corresponding Source. The Corresponding Source for a work in source code form is that same work. 2. Basic Permissions. All rights granted under this License are granted for the term of copyright on the Program, and are irrevocable provided the stated conditions are met. This License explicitly affirms your unlimited permission to run the unmodified Program. The output from running a covered work is covered by this License only if the output, given its content, constitutes a covered work. This License acknowledges your rights of fair use or other equivalent, as provided by copyright law. You may make, run and propagate covered works that you do not convey, without conditions so long as your license otherwise remains in force. You may convey covered works to others for the sole purpose of having them make modifications exclusively for you, or provide you with facilities for running those works, provided that you comply with the terms of this License in conveying all material for which you do not control copyright. Those thus making or running the covered works for you must do so exclusively on your behalf, under your direction and control, on terms that prohibit them from making any copies of your copyrighted material outside their relationship with you. Conveying under any other circumstances is permitted solely under the conditions stated below. Sublicensing is not allowed; section 10 makes it unnecessary. 3. Protecting Users' Legal Rights From Anti-Circumvention Law. No covered work shall be deemed part of an effective technological measure under any applicable law fulfilling obligations under article 11 of the WIPO copyright treaty adopted on 20 December 1996, or similar laws prohibiting or restricting circumvention of such measures. When you convey a covered work, you waive any legal power to forbid circumvention of technological measures to the extent such circumvention is effected by exercising rights under this License with respect to the covered work, and you disclaim any intention to limit operation or modification of the work as a means of enforcing, against the work's users, your or third parties' legal rights to forbid circumvention of technological measures. 4. Conveying Verbatim Copies. You may convey verbatim copies of the Program's source code as you receive it, in any medium, provided that you conspicuously and appropriately publish on each copy an appropriate copyright notice; keep intact all notices stating that this License and any non-permissive terms added in accord with section 7 apply to the code; keep intact all notices of the absence of any warranty; and give all recipients a copy of this License along with the Program. You may charge any price or no price for each copy that you convey, and you may offer support or warranty protection for a fee. 5. Conveying Modified Source Versions. You may convey a work based on the Program, or the modifications to produce it from the Program, in the form of source code under the terms of section 4, provided that you also meet all of these conditions: a) The work must carry prominent notices stating that you modified it, and giving a relevant date. b) The work must carry prominent notices stating that it is released under this License and any conditions added under section 7. This requirement modifies the requirement in section 4 to "keep intact all notices". c) You must license the entire work, as a whole, under this License to anyone who comes into possession of a copy. This License will therefore apply, along with any applicable section 7 additional terms, to the whole of the work, and all its parts, regardless of how they are packaged. This License gives no permission to license the work in any other way, but it does not invalidate such permission if you have separately received it. d) If the work has interactive user interfaces, each must display Appropriate Legal Notices; however, if the Program has interactive interfaces that do not display Appropriate Legal Notices, your work need not make them do so. A compilation of a covered work with other separate and independent works, which are not by their nature extensions of the covered work, and which are not combined with it such as to form a larger program, in or on a volume of a storage or distribution medium, is called an "aggregate" if the compilation and its resulting copyright are not used to limit the access or legal rights of the compilation's users beyond what the individual works permit. Inclusion of a covered work in an aggregate does not cause this License to apply to the other parts of the aggregate. 6. Conveying Non-Source Forms. You may convey a covered work in object code form under the terms of sections 4 and 5, provided that you also convey the machine-readable Corresponding Source under the terms of this License, in one of these ways: a) Convey the object code in, or embodied in, a physical product (including a physical distribution medium), accompanied by the Corresponding Source fixed on a durable physical medium customarily used for software interchange. b) Convey the object code in, or embodied in, a physical product (including a physical distribution medium), accompanied by a written offer, valid for at least three years and valid for as long as you offer spare parts or customer support for that product model, to give anyone who possesses the object code either (1) a copy of the Corresponding Source for all the software in the product that is covered by this License, on a durable physical medium customarily used for software interchange, for a price no more than your reasonable cost of physically performing this conveying of source, or (2) access to copy the Corresponding Source from a network server at no charge. c) Convey individual copies of the object code with a copy of the written offer to provide the Corresponding Source. This alternative is allowed only occasionally and noncommercially, and only if you received the object code with such an offer, in accord with subsection 6b. d) Convey the object code by offering access from a designated place (gratis or for a charge), and offer equivalent access to the Corresponding Source in the same way through the same place at no further charge. You need not require recipients to copy the Corresponding Source along with the object code. If the place to copy the object code is a network server, the Corresponding Source may be on a different server (operated by you or a third party) that supports equivalent copying facilities, provided you maintain clear directions next to the object code saying where to find the Corresponding Source. Regardless of what server hosts the Corresponding Source, you remain obligated to ensure that it is available for as long as needed to satisfy these requirements. e) Convey the object code using peer-to-peer transmission, provided you inform other peers where the object code and Corresponding Source of the work are being offered to the general public at no charge under subsection 6d. A separable portion of the object code, whose source code is excluded from the Corresponding Source as a System Library, need not be included in conveying the object code work. A "User Product" is either (1) a "consumer product", which means any tangible personal property which is normally used for personal, family, or household purposes, or (2) anything designed or sold for incorporation into a dwelling. In determining whether a product is a consumer product, doubtful cases shall be resolved in favor of coverage. For a particular product received by a particular user, "normally used" refers to a typical or common use of that class of product, regardless of the status of the particular user or of the way in which the particular user actually uses, or expects or is expected to use, the product. A product is a consumer product regardless of whether the product has substantial commercial, industrial or non-consumer uses, unless such uses represent the only significant mode of use of the product. "Installation Information" for a User Product means any methods, procedures, authorization keys, or other information required to install and execute modified versions of a covered work in that User Product from a modified version of its Corresponding Source. The information must suffice to ensure that the continued functioning of the modified object code is in no case prevented or interfered with solely because modification has been made. If you convey an object code work under this section in, or with, or specifically for use in, a User Product, and the conveying occurs as part of a transaction in which the right of possession and use of the User Product is transferred to the recipient in perpetuity or for a fixed term (regardless of how the transaction is characterized), the Corresponding Source conveyed under this section must be accompanied by the Installation Information. But this requirement does not apply if neither you nor any third party retains the ability to install modified object code on the User Product (for example, the work has been installed in ROM). The requirement to provide Installation Information does not include a requirement to continue to provide support service, warranty, or updates for a work that has been modified or installed by the recipient, or for the User Product in which it has been modified or installed. Access to a network may be denied when the modification itself materially and adversely affects the operation of the network or violates the rules and protocols for communication across the network. Corresponding Source conveyed, and Installation Information provided, in accord with this section must be in a format that is publicly documented (and with an implementation available to the public in source code form), and must require no special password or key for unpacking, reading or copying. 7. Additional Terms. "Additional permissions" are terms that supplement the terms of this License by making exceptions from one or more of its conditions. Additional permissions that are applicable to the entire Program shall be treated as though they were included in this License, to the extent that they are valid under applicable law. If additional permissions apply only to part of the Program, that part may be used separately under those permissions, but the entire Program remains governed by this License without regard to the additional permissions. When you convey a copy of a covered work, you may at your option remove any additional permissions from that copy, or from any part of it. (Additional permissions may be written to require their own removal in certain cases when you modify the work.) You may place additional permissions on material, added by you to a covered work, for which you have or can give appropriate copyright permission. Notwithstanding any other provision of this License, for material you add to a covered work, you may (if authorized by the copyright holders of that material) supplement the terms of this License with terms: a) Disclaiming warranty or limiting liability differently from the terms of sections 15 and 16 of this License; or b) Requiring preservation of specified reasonable legal notices or author attributions in that material or in the Appropriate Legal Notices displayed by works containing it; or c) Prohibiting misrepresentation of the origin of that material, or requiring that modified versions of such material be marked in reasonable ways as different from the original version; or d) Limiting the use for publicity purposes of names of licensors or authors of the material; or e) Declining to grant rights under trademark law for use of some trade names, trademarks, or service marks; or f) Requiring indemnification of licensors and authors of that material by anyone who conveys the material (or modified versions of it) with contractual assumptions of liability to the recipient, for any liability that these contractual assumptions directly impose on those licensors and authors. All other non-permissive additional terms are considered "further restrictions" within the meaning of section 10. If the Program as you received it, or any part of it, contains a notice stating that it is governed by this License along with a term that is a further restriction, you may remove that term. If a license document contains a further restriction but permits relicensing or conveying under this License, you may add to a covered work material governed by the terms of that license document, provided that the further restriction does not survive such relicensing or conveying. If you add terms to a covered work in accord with this section, you must place, in the relevant source files, a statement of the additional terms that apply to those files, or a notice indicating where to find the applicable terms. Additional terms, permissive or non-permissive, may be stated in the form of a separately written license, or stated as exceptions; the above requirements apply either way. 8. Termination. You may not propagate or modify a covered work except as expressly provided under this License. Any attempt otherwise to propagate or modify it is void, and will automatically terminate your rights under this License (including any patent licenses granted under the third paragraph of section 11). However, if you cease all violation of this License, then your license from a particular copyright holder is reinstated (a) provisionally, unless and until the copyright holder explicitly and finally terminates your license, and (b) permanently, if the copyright holder fails to notify you of the violation by some reasonable means prior to 60 days after the cessation. Moreover, your license from a particular copyright holder is reinstated permanently if the copyright holder notifies you of the violation by some reasonable means, this is the first time you have received notice of violation of this License (for any work) from that copyright holder, and you cure the violation prior to 30 days after your receipt of the notice. Termination of your rights under this section does not terminate the licenses of parties who have received copies or rights from you under this License. If your rights have been terminated and not permanently reinstated, you do not qualify to receive new licenses for the same material under section 10. 9. Acceptance Not Required for Having Copies. You are not required to accept this License in order to receive or run a copy of the Program. Ancillary propagation of a covered work occurring solely as a consequence of using peer-to-peer transmission to receive a copy likewise does not require acceptance. However, nothing other than this License grants you permission to propagate or modify any covered work. These actions infringe copyright if you do not accept this License. Therefore, by modifying or propagating a covered work, you indicate your acceptance of this License to do so. 10. Automatic Licensing of Downstream Recipients. Each time you convey a covered work, the recipient automatically receives a license from the original licensors, to run, modify and propagate that work, subject to this License. You are not responsible for enforcing compliance by third parties with this License. An "entity transaction" is a transaction transferring control of an organization, or substantially all assets of one, or subdividing an organization, or merging organizations. If propagation of a covered work results from an entity transaction, each party to that transaction who receives a copy of the work also receives whatever licenses to the work the party's predecessor in interest had or could give under the previous paragraph, plus a right to possession of the Corresponding Source of the work from the predecessor in interest, if the predecessor has it or can get it with reasonable efforts. You may not impose any further restrictions on the exercise of the rights granted or affirmed under this License. For example, you may not impose a license fee, royalty, or other charge for exercise of rights granted under this License, and you may not initiate litigation (including a cross-claim or counterclaim in a lawsuit) alleging that any patent claim is infringed by making, using, selling, offering for sale, or importing the Program or any portion of it. 11. Patents. A "contributor" is a copyright holder who authorizes use under this License of the Program or a work on which the Program is based. The work thus licensed is called the contributor's "contributor version". A contributor's "essential patent claims" are all patent claims owned or controlled by the contributor, whether already acquired or hereafter acquired, that would be infringed by some manner, permitted by this License, of making, using, or selling its contributor version, but do not include claims that would be infringed only as a consequence of further modification of the contributor version. For purposes of this definition, "control" includes the right to grant patent sublicenses in a manner consistent with the requirements of this License. Each contributor grants you a non-exclusive, worldwide, royalty-free patent license under the contributor's essential patent claims, to make, use, sell, offer for sale, import and otherwise run, modify and propagate the contents of its contributor version. In the following three paragraphs, a "patent license" is any express agreement or commitment, however denominated, not to enforce a patent (such as an express permission to practice a patent or covenant not to sue for patent infringement). To "grant" such a patent license to a party means to make such an agreement or commitment not to enforce a patent against the party. If you convey a covered work, knowingly relying on a patent license, and the Corresponding Source of the work is not available for anyone to copy, free of charge and under the terms of this License, through a publicly available network server or other readily accessible means, then you must either (1) cause the Corresponding Source to be so available, or (2) arrange to deprive yourself of the benefit of the patent license for this particular work, or (3) arrange, in a manner consistent with the requirements of this License, to extend the patent license to downstream recipients. "Knowingly relying" means you have actual knowledge that, but for the patent license, your conveying the covered work in a country, or your recipient's use of the covered work in a country, would infringe one or more identifiable patents in that country that you have reason to believe are valid. If, pursuant to or in connection with a single transaction or arrangement, you convey, or propagate by procuring conveyance of, a covered work, and grant a patent license to some of the parties receiving the covered work authorizing them to use, propagate, modify or convey a specific copy of the covered work, then the patent license you grant is automatically extended to all recipients of the covered work and works based on it. A patent license is "discriminatory" if it does not include within the scope of its coverage, prohibits the exercise of, or is conditioned on the non-exercise of one or more of the rights that are specifically granted under this License. You may not convey a covered work if you are a party to an arrangement with a third party that is in the business of distributing software, under which you make payment to the third party based on the extent of your activity of conveying the work, and under which the third party grants, to any of the parties who would receive the covered work from you, a discriminatory patent license (a) in connection with copies of the covered work conveyed by you (or copies made from those copies), or (b) primarily for and in connection with specific products or compilations that contain the covered work, unless you entered into that arrangement, or that patent license was granted, prior to 28 March 2007. Nothing in this License shall be construed as excluding or limiting any implied license or other defenses to infringement that may otherwise be available to you under applicable patent law. 12. No Surrender of Others' Freedom. If conditions are imposed on you (whether by court order, agreement or otherwise) that contradict the conditions of this License, they do not excuse you from the conditions of this License. If you cannot convey a covered work so as to satisfy simultaneously your obligations under this License and any other pertinent obligations, then as a consequence you may not convey it at all. For example, if you agree to terms that obligate you to collect a royalty for further conveying from those to whom you convey the Program, the only way you could satisfy both those terms and this License would be to refrain entirely from conveying the Program. 13. Use with the GNU Affero General Public License. Notwithstanding any other provision of this License, you have permission to link or combine any covered work with a work licensed under version 3 of the GNU Affero General Public License into a single combined work, and to convey the resulting work. The terms of this License will continue to apply to the part which is the covered work, but the special requirements of the GNU Affero General Public License, section 13, concerning interaction through a network will apply to the combination as such. 14. Revised Versions of this License. The Free Software Foundation may publish revised and/or new versions of the GNU General Public License from time to time. Such new versions will be similar in spirit to the present version, but may differ in detail to address new problems or concerns. Each version is given a distinguishing version number. If the Program specifies that a certain numbered version of the GNU General Public License "or any later version" applies to it, you have the option of following the terms and conditions either of that numbered version or of any later version published by the Free Software Foundation. If the Program does not specify a version number of the GNU General Public License, you may choose any version ever published by the Free Software Foundation. If the Program specifies that a proxy can decide which future versions of the GNU General Public License can be used, that proxy's public statement of acceptance of a version permanently authorizes you to choose that version for the Program. Later license versions may give you additional or different permissions. However, no additional obligations are imposed on any author or copyright holder as a result of your choosing to follow a later version. 15. Disclaimer of Warranty. THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM IS WITH YOU. SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF ALL NECESSARY SERVICING, REPAIR OR CORRECTION. 16. Limitation of Liability. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS), EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH DAMAGES. 17. Interpretation of Sections 15 and 16. If the disclaimer of warranty and limitation of liability provided above cannot be given local legal effect according to their terms, reviewing courts shall apply local law that most closely approximates an absolute waiver of all civil liability in connection with the Program, unless a warranty or assumption of liability accompanies a copy of the Program in return for a fee. END OF TERMS AND CONDITIONS How to Apply These Terms to Your New Programs If you develop a new program, and you want it to be of the greatest possible use to the public, the best way to achieve this is to make it free software which everyone can redistribute and change under these terms. To do so, attach the following notices to the program. It is safest to attach them to the start of each source file to most effectively state the exclusion of warranty; and each file should have at least the "copyright" line and a pointer to where the full notice is found. Copyright (C) This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version. This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with this program. If not, see . Also add information on how to contact you by electronic and paper mail. If the program does terminal interaction, make it output a short notice like this when it starts in an interactive mode: Copyright (C) This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'. This is free software, and you are welcome to redistribute it under certain conditions; type `show c' for details. The hypothetical commands `show w' and `show c' should show the appropriate parts of the General Public License. Of course, your program's commands might be different; for a GUI interface, you would use an "about box". You should also get your employer (if you work as a programmer) or school, if any, to sign a "copyright disclaimer" for the program, if necessary. For more information on this, and how to apply and follow the GNU GPL, see . The GNU General Public License does not permit incorporating your program into proprietary programs. If your program is a subroutine library, you may consider it more useful to permit linking proprietary applications with the library. If this is what you want to do, use the GNU Lesser General Public License instead of this License. But first, please read . ================================================ FILE: feature_tracker/include/Color.h ================================================ /** This File is part of Feature Tracker * * Copyright (C) 2022 Changwu Liu (cwliu529@163.com, * lcw18@mails.tsinghua.edu.cn (valid until 2023)) * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program. If not, see . */ #pragma once #include namespace color { inline std::ostream& setBlue(std::ostream& s) { s << "\033[0;1;34m"; return s; } inline std::ostream& setRed(std::ostream& s) { s << "\033[0;1;31m"; return s; } inline std::ostream& setGreen(std::ostream& s) { s << "\033[0;1;32m"; return s; } inline std::ostream& setYellow(std::ostream& s) { s << "\033[0;1;33m"; return s; } inline std::ostream& setWhite(std::ostream& s) { s << "\033[0;1;37m"; return s; } inline std::ostream& resetColor(std::ostream& s) { s << "\033[0m"; return s; } } ================================================ FILE: feature_tracker/include/mono_parameters.h ================================================ /** This File is part of Feature Tracker * * Copyright (C) 2022 Changwu Liu (cwliu529@163.com, * lcw18@mails.tsinghua.edu.cn (valid until 2023)) * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program. If not, see . * * A few functions of the mono tracker mimic the realization from GVINS * * GVINS is under GPLv3 License. */ #pragma once #include #include namespace feature_tracker { class MonoParameters { public: MonoParameters(); int row, col; int focal_length; std::string image_topic; std::string imu_topic; std::string cam_name; std::string cam_path; int max_cnt; int min_dist; int window_size; int freq; double f_threshold; int show_track; int equalize; double timer_warning_thres; int show_timer; void readParameters(ros::NodeHandle& n); template static T readParam(ros::NodeHandle& n, std::string name); }; template T MonoParameters::readParam(ros::NodeHandle& n, std::string name) { T ans; if (n.getParam(name, ans)) { ROS_INFO_STREAM("Loaded " << name << ": " << ans); } else { ROS_ERROR_STREAM("Failed to load: " << name); n.shutdown(); } return ans; } } ================================================ FILE: feature_tracker/include/mono_tracker.h ================================================ /** This File is part of Feature Tracker * * Copyright (C) 2022 Changwu Liu (cwliu529@163.com, * lcw18@mails.tsinghua.edu.cn (valid until 2023)) * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program. If not, see . * * A few functions of the mono tracker mimic the realization from GVINS * * GVINS is under GPLv3 License. */ #pragma once #include #include #include #include #include #include #include #include #include #include #include #include #include "tic_toc.hpp" #include "camera_factory.h" #include "MeiCamera.h" #include "PinholeCamera.h" #include "mono_parameters.h" #include #include namespace feature_tracker { class MonoTracker { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW MonoTracker(ros::NodeHandle& n) : nh(n) {} ~MonoTracker() {} MonoTracker(const MonoTracker&) = delete; MonoTracker operator=(const MonoTracker&) = delete; void init(); private: ros::NodeHandle nh; ros::Publisher pub_mono_features, pub_match; ros::Subscriber sub_img; void img_callback(const sensor_msgs::ImageConstPtr& img_msg); MonoParameters param; camera_model::CameraPtr m_camera; bool pub_this_frame; bool first_image_flag; double first_image_time, last_image_time; int pub_count; static int n_id; double cur_time, prev_time; cv::Mat mask; cv::Mat prev_img, cur_img, forw_img; std::vector prev_pts, cur_pts, forw_pts; std::vector prev_un_pts, cur_un_pts; std::vector pts_velocity; std::vector n_pts; std::vector ids; std::vector track_cnt; std::map cur_un_pts_map; std::map prev_un_pts_map; void readImage(const cv::Mat& _img, double _cur_time); void rejectWithF(); void setMask(); void addPoints(); void undistortedPoints(); void updateID(); bool inBorder(const cv::Point2f& pt); template void reduceVector(std::vector& v, std::vector status); void readIntrinsicParameter(const std::string& calib_file); }; typedef boost::shared_ptr MonoTrackerPtr; template void MonoTracker::reduceVector(std::vector& v, std::vector status) { int j = 0; for (int i = 0; i < int(v.size()); ++i) if (status[i]) v[j++] = v[i]; v.resize(j); } } ================================================ FILE: feature_tracker/include/random_pair.hpp ================================================ /** This File is part of Feature Tracker * * Copyright (C) 2022 Changwu Liu (cwliu529@163.com, * lcw18@mails.tsinghua.edu.cn (valid until 2023)) * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program. If not, see . */ #pragma once #include #include #include #include namespace feature_tracker { class RandomPairGenerator { public: RandomPairGenerator(unsigned int n = 50) : xa(static_cast(n*(n-1)/2)), count(0), N(n) { for (int i = 0; i < static_cast(n*(n-1)/2); ++i) xa[i] = i; std::random_shuffle(xa.begin(), xa.end()); } RandomPairGenerator(RandomPairGenerator& obj) = delete; ~RandomPairGenerator(){} static void setRandomPairSeed() { srand((unsigned)time(NULL)); } void generatePair(std::vector& pair) { pair.clear(); unsigned int i = xa[count]; float k_max = (1.0+sqrt(i*8+9))/2.0; float k_min = (-1.0+sqrt(i*8+9))/2.0; float k_guess = floor(k_max); unsigned int k; if (k_guess < k_max && k_guess >= k_min) k = static_cast(k_guess); else k = static_cast(k_guess)-1; pair.push_back(N-k-1); pair.push_back(N-k+i-static_cast(k*(k-1)/2)); ++count; if (count >= static_cast(N*(N-1)/2)) count = 0; } private: std::vector xa; unsigned int count; unsigned int N; }; } ================================================ FILE: feature_tracker/include/stereo_parameters.h ================================================ /** This File is part of Feature Tracker * * Copyright (C) 2022 Changwu Liu (cwliu529@163.com, * lcw18@mails.tsinghua.edu.cn (valid until 2023)) * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program. If not, see . */ #pragma once #include #include #include #include namespace feature_tracker { class StereoParameters { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW StereoParameters(); int row, col; std::string image_left_topic; std::string image_right_topic; std::string cam_left_path; std::string cam_right_path; std::string cam_left_name; std::string cam_right_name; int max_cnt; int min_dist; int window_size; int freq; int show_track; int show_timer; double timer_warning_thres; int equalize; double epipolar_thres; double F_threshold; int print_track_info; Eigen::Isometry3d T_cl2i, T_cr2i; void readParameters(ros::NodeHandle& n); template static T readParam(ros::NodeHandle& n, std::string name); }; template T StereoParameters::readParam(ros::NodeHandle& n, std::string name) { T ans; if (n.getParam(name, ans)) { ROS_INFO_STREAM("Loaded " << name << ": " << ans); } else { ROS_ERROR_STREAM("Failed to load: " << name); n.shutdown(); } return ans; } } ================================================ FILE: feature_tracker/include/stereo_tracker.h ================================================ /** This File is part of Feature Tracker * * Copyright (C) 2022 Changwu Liu (cwliu529@163.com, * lcw18@mails.tsinghua.edu.cn (valid until 2023)) * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program. If not, see . */ #pragma once #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include "tic_toc.hpp" #include "camera_factory.h" #include "MeiCamera.h" #include "PinholeCamera.h" #include "random_pair.hpp" #include "stereo_parameters.h" #include "feature_tracker/StereoMeas.h" #include "feature_tracker/StereoFrame.h" namespace feature_tracker { class StereoTracker { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW StereoTracker(ros::NodeHandle& n) : nh(n) {} ~StereoTracker() {} StereoTracker(const StereoTracker&) = delete; StereoTracker operator=(const StereoTracker&) = delete; void init(); private: ros::NodeHandle nh; ros::Publisher pub_stereo_feature, pub_match; typedef message_filters::sync_policies::ApproximateTime stereoSyncPolicy; boost::shared_ptr> cam_left_sub_; boost::shared_ptr> cam_right_sub_; boost::shared_ptr> stereo_sub_; StereoParameters param; camera_model::CameraPtr m_camera_left, m_camera_right; cv::Mat mask; bool first_image_flag; double first_image_time, last_image_time; bool pub_this_frame; unsigned long long int pub_count; double cur_time, prev_time; cv::Mat prev_left_img, cur_left_img, forw_left_img; cv::Mat prev_right_img, cur_right_img, forw_right_img; std::vector prev_left_pts, cur_left_pts, forw_left_pts; std::vector prev_right_pts, cur_right_pts, forw_right_pts; std::vector prev_left_un_pts, cur_left_un_pts; std::vector prev_right_un_pts, cur_right_un_pts; std::vector ids; std::vector track_cnt; std::vector n_left_pts, n_right_pts; static unsigned long long int n_id; void stereo_callback(const sensor_msgs::Image::ConstPtr& cam_left_msg, const sensor_msgs::Image::ConstPtr& cam_right_msg); void readIntrinsicParameters(); void processStereoImage(const cv::Mat& _left_img, const cv::Mat& _right_img, double _cur_time); bool inBorder(const cv::Point2f& pt); void markStereoMatchOutlier(const std::vector& left_pts, const std::vector& right_pts, std::vector& status); void rejectWithTwinRANSAC(); void setMask(); void addPoints(); void undistortedPoints(); void updateID(); static void publishStereoImg(const cv::Mat& left_img, const cv::Mat& right_img, const std::vector& left_pts, const std::vector& right_pts, const int row, const int col, bool pair_line = true); void predictHomography(const std::vector& left_pts, std::vector& right_pts); template void reduceVector(std::vector& v, std::vector status); template void reduceTwinVector(std::vector& v1, std::vector& v2, std::vector status); template void reduceQuadVector(std::vector& v1, std::vector& v2, std::vector& v3, std::vector& v4, std::vector status); }; typedef boost::shared_ptr StereoTrackerPtr; template void StereoTracker::reduceVector(std::vector& v, std::vector status) { int j = 0; for (int i = 0; i < int(v.size()); ++i) if (status[i]) v[j++] = v[i]; v.resize(j); } template void StereoTracker::reduceTwinVector(std::vector& v1, std::vector& v2, std::vector status) { if (v1.size() != v2.size()) ROS_WARN("[Stereo Tracker]: Reduce twin vectors, size: %i and %i are not equal!", int(v1.size()), int(v2.size())); int j = 0; for (int i = 0; i < int(v1.size()); ++i) if (status[i]) { v1[j] = v1[i]; v2[j] = v2[i]; ++j; } v1.resize(j); v2.resize(j); } template void StereoTracker::reduceQuadVector(std::vector& v1, std::vector& v2, std::vector& v3, std::vector& v4, std::vector status) { if (!(v1.size() == v2.size() && v2.size() == v3.size() && v3.size() == v4.size())) ROS_WARN("[Stereo Tracker]: Reduce quad vectors, size: %i, %i, %i and %i are not equal!", int(v1.size()), int(v2.size()), int(v3.size()), int(v4.size())); int j = 0; for (int i = 0; i < int(v1.size()); ++i) if (status[i]) { v1[j] = v1[i]; v2[j] = v2[i]; v3[j] = v3[i]; v4[j] = v4[i]; ++j; } v1.resize(j); v2.resize(j); v3.resize(j); v4.resize(j); } } ================================================ FILE: feature_tracker/include/tic_toc.hpp ================================================ /** This File is part of Feature Tracker * * Copyright (C) 2022 Changwu Liu (cwliu529@163.com, * lcw18@mails.tsinghua.edu.cn (valid until 2023)) * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program. If not, see . * * A few functions of the mono tracker mimic the realization from GVINS * * GVINS is under GPLv3 License. */ #pragma once #include #include #include namespace feature_tracker { class TicToc { public: TicToc() { tic(); } void tic() { start = std::chrono::system_clock::now(); } double toc() { end = std::chrono::system_clock::now(); std::chrono::duration elapsed_seconds = end - start; return elapsed_seconds.count() * 1000.0; } private: std::chrono::time_point start, end; }; } ================================================ FILE: feature_tracker/launch/sportsfield/mono_tracker_sf.launch ================================================ ================================================ FILE: feature_tracker/launch/sportsfield/stereo_tracker_sf.launch ================================================ ================================================ FILE: feature_tracker/msg/MonoFrame.msg ================================================ std_msgs/Header header MonoMeas[] mono_features ================================================ FILE: feature_tracker/msg/MonoMeas.msg ================================================ uint64 id float64 u0 float64 v0 ================================================ FILE: feature_tracker/msg/StereoFrame.msg ================================================ std_msgs/Header header StereoMeas[] stereo_features ================================================ FILE: feature_tracker/msg/StereoMeas.msg ================================================ uint64 id float64 u0 float64 v0 float64 u1 float64 v1 ================================================ FILE: feature_tracker/package.xml ================================================ feature_tracker 0.0.0 The mono and stereo feature_tracker package lcw18 GPLv3 catkin roscpp std_msgs roscpp std_msgs nodelet pluginlib roscpp std_msgs roslib rosbag message_filters sensor_msgs eigen_conversions cv_bridge image_transport message_generation message_runtime ================================================ FILE: feature_tracker/src/mono_parameters.cpp ================================================ /** This File is part of Feature Tracker * * Copyright (C) 2022 Changwu Liu (cwliu529@163.com, * lcw18@mails.tsinghua.edu.cn (valid until 2023)) * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program. If not, see . * * A few functions of the mono tracker mimic the realization from GVINS * * GVINS is under GPLv3 License. */ #include "mono_parameters.h" namespace feature_tracker { MonoParameters::MonoParameters() : row(480), col(752), focal_length(460), image_topic("/cam1/image_raw"), imu_topic("/imu0"), cam_name("camera"), cam_path("~"), max_cnt(150), min_dist(30), window_size(20), freq(0), f_threshold(1.0), show_track(1), equalize(1) {} void MonoParameters::readParameters(ros::NodeHandle& n) { std::string config_file; config_file = readParam(n, "config_file"); cv::FileStorage fs(config_file, cv::FileStorage::READ); if (!fs.isOpened()) { ROS_ERROR("ERROR: Wrong path to mono config yaml files!"); n.shutdown(); } cam_path = config_file; row = fs["image_height"]; col = fs["image_width"]; focal_length = 460; fs["image_topic"] >> image_topic; fs["imu_topic"] >> imu_topic; fs["camera_name"] >> cam_name; max_cnt = fs["max_cnt"]; min_dist = fs["min_dist"]; window_size = 20; freq = fs["freq"]; if (freq == 0) freq = 100; f_threshold = fs["F_threshold"]; show_track = fs["show_track"]; show_timer = fs["show_timer"]; timer_warning_thres = fs["timer_warning_thres"]; equalize = fs["equalize"]; fs.release(); } } ================================================ FILE: feature_tracker/src/mono_tracker.cpp ================================================ /** This File is part of Feature Tracker * * Copyright (C) 2022 Changwu Liu (cwliu529@163.com, * lcw18@mails.tsinghua.edu.cn (valid until 2023)) * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program. If not, see . * * A few functions of the mono tracker mimic the realization from GVINS * * GVINS is under GPLv3 License. */ #include "mono_tracker.h" #include "Color.h" namespace feature_tracker { int MonoTracker::n_id = 0; void MonoTracker::init() { param.readParameters(nh); readIntrinsicParameter(param.cam_path); sub_img = nh.subscribe(param.image_topic, 100, &MonoTracker::img_callback, this); pub_mono_features = nh.advertise("mono_feature", 100); pub_match = nh.advertise("mono_track", 100); first_image_flag = true; first_image_time = 0.0; last_image_time = 0.0; pub_this_frame = false; pub_count = 1; } void MonoTracker::img_callback(const sensor_msgs::ImageConstPtr& img_msg) { TicToc mono_timer; if (first_image_flag) { first_image_flag = false; first_image_time = img_msg->header.stamp.toSec(); last_image_time = img_msg->header.stamp.toSec(); return; } if (img_msg->header.stamp.toSec() - last_image_time > 1.0 || img_msg->header.stamp.toSec() < last_image_time) { ROS_WARN("[Mono Tracker]: Image discontinue! Mono tracker reset!"); first_image_flag = true; first_image_time = 0.0; last_image_time = 0.0; pub_count = 1; pub_this_frame = false; return; } last_image_time = img_msg->header.stamp.toSec(); if (std::round(1.0*pub_count/(last_image_time-first_image_time)) <= param.freq) { pub_this_frame = true; if (std::fabs(1.0*pub_count/(last_image_time-first_image_time)-param.freq) < 0.01*param.freq) { first_image_time = last_image_time; pub_count = 0; } } else { pub_this_frame = false; } cv_bridge::CvImageConstPtr ptr; if (img_msg->encoding == "8UC1") { sensor_msgs::Image img; img.header = img_msg->header; img.height = img_msg->height; img.width = img_msg->width; img.is_bigendian = img_msg->is_bigendian; img.step = img_msg->step; img.data = img_msg->data; img.encoding = "mono8"; ptr = cv_bridge::toCvCopy(img, sensor_msgs::image_encodings::MONO8); } else ptr = cv_bridge::toCvCopy(img_msg, sensor_msgs::image_encodings::MONO8); readImage(ptr->image, img_msg->header.stamp.toSec()); updateID(); if (pub_this_frame) { ++pub_count; feature_tracker::MonoFrame::Ptr mono_frame_ptr(new feature_tracker::MonoFrame); mono_frame_ptr->header = img_msg->header; for (int i = 0; i < ids.size(); ++i) { if (track_cnt[i] > 1) { feature_tracker::MonoMeas::Ptr mono_feature_ptr(new feature_tracker::MonoMeas); mono_feature_ptr->id = ids[i]; mono_feature_ptr->u0 = cur_un_pts[i].x; mono_feature_ptr->v0 = cur_un_pts[i].y; mono_frame_ptr->mono_features.push_back(*mono_feature_ptr); } } if (mono_frame_ptr->mono_features.size() > 0) pub_mono_features.publish(mono_frame_ptr); if (param.show_track) { ptr = cv_bridge::cvtColor(ptr, sensor_msgs::image_encodings::BGR8); cv::Mat tmp_img = ptr->image; for (unsigned int j = 0; j < cur_pts.size(); ++j) { double len = std::min(1.0, 1.0*track_cnt[j]/param.window_size); cv::circle(tmp_img, cur_pts[j], 2, cv::Scalar(255*(1-len), 0, 255*len), 2); } pub_match.publish(ptr->toImageMsg()); } } if (param.show_timer) { double mono_time = mono_timer.toc(); if (mono_time <= param.timer_warning_thres) std::cout << color::setBlue << "[MonoTracker]: Mono tracking processing time = " << mono_time << " (ms) " << color::resetColor << std::endl; else std::cout << color::setRed << "[MonoTracker]: Mono tracking processing time = " << mono_time << " (ms) " << color::resetColor << std::endl; } } void MonoTracker::updateID() { for (unsigned int i = 0; i < ids.size(); ++i) if (ids[i] == -1) ids[i] = n_id++; } void MonoTracker::readImage(const cv::Mat& _img, double _cur_time) { cv::Mat img; cur_time = _cur_time; if (param.equalize) { cv::Ptr clahe = cv::createCLAHE(3.0, cv::Size(8, 8)); clahe->apply(_img, img); } else img = _img; if (forw_img.empty()) prev_img = cur_img = forw_img = img; else forw_img = img; forw_pts.clear(); if (cur_pts.size() > 0) { std::vector status; std::vector err; cv::calcOpticalFlowPyrLK(cur_img, forw_img, cur_pts, forw_pts, status, err, cv::Size(21, 21), 3); for (int i = 0; i < int(forw_pts.size()); ++i) if (status[i] && !inBorder(forw_pts[i])) status[i] = 0; reduceVector(prev_pts, status); reduceVector(cur_pts, status); reduceVector(forw_pts, status); reduceVector(ids, status); reduceVector(cur_un_pts, status); reduceVector(track_cnt, status); } for (auto& n: track_cnt) ++n; if (pub_this_frame) { rejectWithF(); setMask(); int n_max_cnt = param.max_cnt - static_cast(forw_pts.size()); if (n_max_cnt > 0) { if (mask.empty()) ROS_WARN("[Mono Tracker]: Mask is empty!"); if (mask.type() != CV_8UC1) ROS_WARN("[Mono Tracker]: Mask type wrong!"); if (mask.size() != forw_img.size()) ROS_WARN("[Mono Tracker]: Mask size wrong!"); cv::goodFeaturesToTrack(forw_img, n_pts, n_max_cnt, 0.01, param.min_dist, mask); } else n_pts.clear(); addPoints(); } prev_img = cur_img; prev_pts = cur_pts; prev_un_pts = cur_un_pts; cur_img = forw_img; cur_pts = forw_pts; undistortedPoints(); prev_time = cur_time; } void MonoTracker::addPoints() { for (auto& p: n_pts) { forw_pts.push_back(p); ids.push_back(-1); track_cnt.push_back(1); } } void MonoTracker::undistortedPoints() { cur_un_pts.clear(); cur_un_pts_map.clear(); for (unsigned int i = 0; i < cur_pts.size(); ++i) { Eigen::Vector2d a(cur_pts[i].x, cur_pts[i].y); Eigen::Vector3d b; m_camera->liftProjective(a, b); cur_un_pts.push_back(cv::Point2f(b.x()/b.z(), b.y()/b.z())); cur_un_pts_map.insert(std::make_pair(ids[i], cv::Point2f(b.x()/b.z(), b.y()/b.z()))); } if (!prev_un_pts_map.empty()) { double dt = cur_time - prev_time; pts_velocity.clear(); for (unsigned int i = 0; i < cur_un_pts.size(); ++i) { if (ids[i] != -1) { auto it = prev_un_pts_map.find(ids[i]); if (it != prev_un_pts_map.end()) { double v_x = (cur_un_pts[i].x - it->second.x) / dt; double v_y = (cur_un_pts[i].y - it->second.y) / dt; pts_velocity.push_back(cv::Point2f(v_x, v_y)); } else pts_velocity.push_back(cv::Point2f(0.0, 0.0)); } else pts_velocity.push_back(cv::Point2f(0.0, 0.0)); } } else { for (unsigned int i = 0; i < cur_pts.size(); ++i) pts_velocity.push_back(cv::Point2f(0.0, 0.0)); } prev_un_pts_map = cur_un_pts_map; } void MonoTracker::setMask() { mask = cv::Mat(param.row, param.col, CV_8UC1, cv::Scalar(255)); std::vector>> cnt_pts_id; for (unsigned int i = 0; i < forw_pts.size(); ++i) cnt_pts_id.push_back(std::make_pair(track_cnt[i], std::make_pair(forw_pts[i], ids[i]))); std::sort(cnt_pts_id.begin(), cnt_pts_id.end(), [](const std::pair>& a, const std::pair>& b){ return a.first > b.first; }); forw_pts.clear(); ids.clear(); track_cnt.clear(); for (auto& it: cnt_pts_id) { if (mask.at(it.second.first) == 255) { forw_pts.push_back(it.second.first); ids.push_back(it.second.second); track_cnt.push_back(it.first); cv::circle(mask, it.second.first, param.min_dist, 0, -1); } } } void MonoTracker::rejectWithF() { if (forw_pts.size() >= 8) { std::vector un_cur_pts(cur_pts.size()), un_forw_pts(forw_pts.size()); for (unsigned int i = 0; i < cur_pts.size(); ++i) { Eigen::Vector3d tmp_p; m_camera->liftProjective(Eigen::Vector2d(cur_pts[i].x, cur_pts[i].y), tmp_p); tmp_p.x() = param.focal_length*tmp_p.x()/tmp_p.z() + param.col/2.0; tmp_p.y() = param.focal_length*tmp_p.y()/tmp_p.z() + param.row/2.0; un_cur_pts[i] = cv::Point2f(tmp_p.x(), tmp_p.y()); m_camera->liftProjective(Eigen::Vector2d(forw_pts[i].x, forw_pts[i].y), tmp_p); tmp_p.x() = param.focal_length*tmp_p.x()/tmp_p.z() + param.col/2.0; tmp_p.y() = param.focal_length*tmp_p.y()/tmp_p.z() + param.row/2.0; un_forw_pts[i] = cv::Point2f(tmp_p.x(), tmp_p.y()); } std::vector status; cv::findFundamentalMat(un_cur_pts, un_forw_pts, cv::FM_RANSAC, param.f_threshold, 0.99, status); // int size_a = cur_pts.size(); reduceVector(prev_pts, status); reduceVector(cur_pts, status); reduceVector(forw_pts, status); reduceVector(cur_un_pts, status); reduceVector(ids, status); reduceVector(track_cnt, status); // ROS_INFO("FM RANSAC: %d points reduced to -> %lu: %f", size_a, forw_pts.size(), 1.0*forw_pts.size()/size_a); } } bool MonoTracker::inBorder(const cv::Point2f& pt) { const int BORDER_SIZE = 1; int img_x = cvRound(pt.x); int img_y = cvRound(pt.y); return BORDER_SIZE <= img_x && img_x < param.col-BORDER_SIZE && BORDER_SIZE <= img_y && img_y < param.row-BORDER_SIZE; } void MonoTracker::readIntrinsicParameter(const std::string& calib_file) { ROS_INFO("[Mono Tracker]: Reading parameter of camera from: %s", calib_file.c_str()); m_camera = camera_model::CameraFactory::instance()->generateCameraFromYamlFile(calib_file); } } ================================================ FILE: feature_tracker/src/mono_tracker_node.cpp ================================================ /** This File is part of Feature Tracker * * Copyright (C) 2022 Changwu Liu (cwliu529@163.com, * lcw18@mails.tsinghua.edu.cn (valid until 2023)) * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program. If not, see . * * A few functions of the mono tracker mimic the realization from GVINS * * GVINS is under GPLv3 License. */ #include "mono_tracker.h" int main(int argc, char** argv) { ros::init(argc, argv, "mono_tracker"); ros::NodeHandle n("~"); ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info); feature_tracker::MonoTrackerPtr mono_tracker_ptr(new feature_tracker::MonoTracker(n)); mono_tracker_ptr->init(); ros::spin(); return 0; } ================================================ FILE: feature_tracker/src/stereo_parameters.cpp ================================================ /** This File is part of Feature Tracker * * Copyright (C) 2022 Changwu Liu (cwliu529@163.com, * lcw18@mails.tsinghua.edu.cn (valid until 2023)) * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program. If not, see . */ #include "stereo_parameters.h" #include namespace feature_tracker { StereoParameters::StereoParameters() : row(480), col(752), image_left_topic("/cam0/image_raw"), image_right_topic("/cam1/image_raw"), cam_left_path("~"), cam_right_path("~"), cam_left_name("camera_left"), cam_right_name("camera_right"), max_cnt(150), min_dist(30), window_size(20), freq(0), show_track(1), equalize(1), epipolar_thres(25.0), F_threshold(1.5), print_track_info(0), T_cl2i(Eigen::Isometry3d::Identity()), T_cr2i(Eigen::Isometry3d::Identity()) {} void StereoParameters::readParameters(ros::NodeHandle& n) { std::string config_file; config_file = readParam(n, "config_file_left"); cam_left_path = config_file; config_file = readParam(n, "config_file_right"); cam_right_path = config_file; cv::FileStorage fs_left(cam_left_path, cv::FileStorage::READ); if (!fs_left.isOpened()) { ROS_ERROR("ERROR: Wrong path to stereo left config yaml files!"); n.shutdown(); } row = fs_left["image_height"]; col = fs_left["image_width"]; fs_left["cam_left_topic"] >> image_left_topic; fs_left["cam_right_topic"] >> image_right_topic; fs_left["camera_name"] >> cam_left_name; max_cnt = fs_left["max_cnt"]; min_dist = fs_left["min_dist"]; window_size = fs_left["window_size"]; freq = fs_left["freq"]; if (freq == 0) freq = 100; show_track = fs_left["show_track"]; show_timer = fs_left["show_timer"]; timer_warning_thres = fs_left["timer_warning_thres"]; equalize = fs_left["equalize"]; epipolar_thres = static_cast(fs_left["epipolar_thres"]); F_threshold = static_cast(fs_left["F_threshold"]); print_track_info = fs_left["print_track_info"]; cv::Mat cv_R1, cv_T1; fs_left["extrinsicRotation"] >> cv_R1; fs_left["extrinsicTranslation"] >> cv_T1; Eigen::Matrix3d eigen_R1; Eigen::Vector3d eigen_T1; cv::cv2eigen(cv_R1, eigen_R1); cv::cv2eigen(cv_T1, eigen_T1); T_cl2i.linear() = eigen_R1; T_cl2i.translation() = eigen_T1; fs_left.release(); cv::FileStorage fs_right(cam_right_path, cv::FileStorage::READ); if (!fs_right.isOpened()) { ROS_ERROR("ERROR: Wrong path to stereo right config yaml files!"); n.shutdown(); } fs_right["camera_name"] >> cam_right_name; cv::Mat cv_R2, cv_T2; fs_right["extrinsicRotation"] >> cv_R2; fs_right["extrinsicTranslation"] >> cv_T2; Eigen::Matrix3d eigen_R2; Eigen::Vector3d eigen_T2; cv::cv2eigen(cv_R2, eigen_R2); cv::cv2eigen(cv_T2, eigen_T2); T_cr2i.linear() = eigen_R2; T_cr2i.translation() = eigen_T2; fs_right.release(); } } ================================================ FILE: feature_tracker/src/stereo_tracker.cpp ================================================ /** This File is part of Feature Tracker * * Copyright (C) 2022 Changwu Liu (cwliu529@163.com, * lcw18@mails.tsinghua.edu.cn (valid until 2023)) * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program. If not, see . */ #include "stereo_tracker.h" #include "Color.h" namespace feature_tracker { unsigned long long int StereoTracker::n_id = 0; void StereoTracker::init() { param.readParameters(nh); readIntrinsicParameters(); cam_left_sub_.reset(new message_filters::Subscriber(nh, param.image_left_topic, 50)); cam_right_sub_.reset(new message_filters::Subscriber(nh, param.image_right_topic, 50)); stereo_sub_.reset(new message_filters::Synchronizer(stereoSyncPolicy(50), *cam_left_sub_, *cam_right_sub_)); stereo_sub_->registerCallback(boost::bind(&StereoTracker::stereo_callback, this, _1, _2)); pub_stereo_feature = nh.advertise("stereo_feature", 50); pub_match = nh.advertise("stereo_track", 50); first_image_flag = true; first_image_time = 0.0; last_image_time = 0.0; pub_this_frame = false; pub_count = 1; } void StereoTracker::readIntrinsicParameters() { ROS_INFO("[Stereo Tracker]: Reading parameter of left camera from: %s", param.cam_left_path.c_str()); m_camera_left = camera_model::CameraFactory::instance()->generateCameraFromYamlFile(param.cam_left_path); ROS_INFO("[Stereo Tracker]: Reading parameter of right camera from: %s", param.cam_right_path.c_str()); m_camera_right = camera_model::CameraFactory::instance()->generateCameraFromYamlFile(param.cam_right_path); } void StereoTracker::stereo_callback(const sensor_msgs::Image::ConstPtr& cam_left_msg, const sensor_msgs::Image::ConstPtr& cam_right_msg) { TicToc stereo_timer; if (first_image_flag) { first_image_flag = false; first_image_time = cam_left_msg->header.stamp.toSec(); last_image_time = cam_left_msg->header.stamp.toSec(); return; } if (cam_left_msg->header.stamp.toSec() - last_image_time > 1.0 || cam_left_msg->header.stamp.toSec() < last_image_time) { ROS_WARN("[Stereo Tracker]: Image discontinue! Stereo tracker reset!"); first_image_flag = true; first_image_time = 0.0; last_image_time = 0.0; pub_count = 1; pub_this_frame = false; return; } last_image_time = cam_left_msg->header.stamp.toSec(); if (std::round(1.0*pub_count/(last_image_time-first_image_time)) <= param.freq) { pub_this_frame = true; if (std::fabs(1.0*pub_count/(last_image_time-first_image_time)-param.freq) < 0.01*param.freq) { first_image_time = last_image_time; pub_count = 0; } } else { pub_this_frame = false; } cv_bridge::CvImageConstPtr left_ptr, right_ptr; if (cam_left_msg->encoding == "8UC1") { sensor_msgs::Image img; img.header = cam_left_msg->header; img.height = cam_left_msg->height; img.width = cam_left_msg->width; img.is_bigendian = cam_left_msg->is_bigendian; img.step = cam_left_msg->step; img.data = cam_left_msg->data; img.encoding = "mono8"; left_ptr = cv_bridge::toCvCopy(img, sensor_msgs::image_encodings::MONO8); } else left_ptr = cv_bridge::toCvCopy(cam_left_msg, sensor_msgs::image_encodings::MONO8); if (cam_right_msg->encoding == "8UC1") { sensor_msgs::Image img; img.header = cam_right_msg->header; img.height = cam_right_msg->height; img.width = cam_right_msg->width; img.is_bigendian = cam_right_msg->is_bigendian; img.step = cam_right_msg->step; img.data = cam_right_msg->data; img.encoding = "mono8"; right_ptr = cv_bridge::toCvCopy(img, sensor_msgs::image_encodings::MONO8); } else right_ptr = cv_bridge::toCvCopy(cam_right_msg, sensor_msgs::image_encodings::MONO8); processStereoImage(left_ptr->image, right_ptr->image, cam_left_msg->header.stamp.toSec()); if (pub_this_frame) { ++pub_count; feature_tracker::StereoFrame::Ptr stereo_frame_ptr(new feature_tracker::StereoFrame); stereo_frame_ptr->header = cam_left_msg->header; for (unsigned int i = 0; i < ids.size(); ++i) { if (track_cnt[i] > 1) { feature_tracker::StereoMeas::Ptr stereo_feature_ptr(new feature_tracker::StereoMeas); stereo_feature_ptr->id = ids[i]; stereo_feature_ptr->u0 = cur_left_un_pts[i].x; stereo_feature_ptr->v0 = cur_left_un_pts[i].y; stereo_feature_ptr->u1 = cur_right_un_pts[i].x; stereo_feature_ptr->v1 = cur_right_un_pts[i].y; stereo_frame_ptr->stereo_features.push_back(*stereo_feature_ptr); } } if (stereo_frame_ptr->stereo_features.size() > 0) pub_stereo_feature.publish(stereo_frame_ptr); if (param.show_track) { cv::Mat track_img(param.row, 2*param.col, CV_8UC3); cv::cvtColor(left_ptr->image, track_img.colRange(0, param.col), CV_GRAY2BGR); cv::cvtColor(right_ptr->image, track_img.colRange(param.col, 2*param.col), CV_GRAY2BGR); for (unsigned int j = 0; j < cur_left_pts.size(); ++j) { double len = std::min(1.0, 1.0*track_cnt[j]/param.window_size); cv::circle(track_img, cur_left_pts[j], 3, cv::Scalar(255*(1-len), 0, 255*len), 2); cv::circle(track_img, cv::Point2f(cur_right_pts[j].x + param.col, cur_right_pts[j].y), 3, cv::Scalar(255*(1-len), 0, 255*len), 2); cv::line(track_img, cur_left_pts[j], cv::Point2f(cur_right_pts[j].x + param.col, cur_right_pts[j].y), cv::Scalar(255*(1-len), 0, 255*len), 2, 4); } cv_bridge::CvImage track_img_ros(cam_left_msg->header, "bgr8", track_img); pub_match.publish(track_img_ros.toImageMsg()); } } if (param.show_timer) { double stereo_time = stereo_timer.toc(); if (stereo_time <= param.timer_warning_thres) std::cout << color::setBlue << "[StereoTracker]: Stereo tracking processing time = " << stereo_time << " (ms) " << color::resetColor << std::endl; else std::cout << color::setRed << "[StereoTracker]: Stereo tracking processing time = " << stereo_time << " (ms) " << color::resetColor << std::endl; } } void StereoTracker::processStereoImage(const cv::Mat& _left_img, const cv::Mat& _right_img, double _cur_time) { cv::Mat left_img, right_img; cur_time = _cur_time; if (param.equalize) { cv::Ptr clahe = cv::createCLAHE(3.0, cv::Size(8, 8)); clahe->apply(_left_img, left_img); clahe->apply(_right_img, right_img); } else { left_img = _left_img; right_img = _right_img; } if (forw_left_img.empty()) { prev_left_img = cur_left_img = forw_left_img = left_img; prev_right_img = cur_right_img = forw_right_img = right_img; } else { forw_left_img = left_img; forw_right_img = right_img; } forw_left_pts.clear(); forw_right_pts.clear(); if (cur_left_pts.size() > 0) { std::vector status; std::vector err; cv::calcOpticalFlowPyrLK(cur_left_img, forw_left_img, cur_left_pts, forw_left_pts, status, err, cv::Size(21, 21), 3); for (unsigned int i = 0; i < forw_left_pts.size(); ++i) if (status[i] && !inBorder(forw_left_pts[i])) status[i] = 0; reduceTwinVector(prev_left_pts, prev_right_pts, status); reduceTwinVector(cur_left_pts, cur_right_pts, status); reduceVector(forw_left_pts, status); reduceVector(ids, status); // reduceTwinVector(prev_left_un_pts, prev_right_un_pts, status); reduceTwinVector(cur_left_un_pts, cur_right_un_pts, status); reduceVector(track_cnt, status); if (forw_left_pts.size() > 0) { status.clear(); err.clear(); predictHomography(forw_left_pts, forw_right_pts); cv::calcOpticalFlowPyrLK(forw_left_img, forw_right_img, forw_left_pts, forw_right_pts, status, err, cv::Size(15, 15), 3, cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 30, 0.01), cv::OPTFLOW_USE_INITIAL_FLOW); for (unsigned int i = 0; i < forw_right_pts.size(); ++i) if (status[i] && !inBorder(forw_right_pts[i])) status[i] = 0; markStereoMatchOutlier(forw_left_pts, forw_right_pts, status); reduceTwinVector(prev_left_pts, prev_right_pts, status); reduceTwinVector(cur_left_pts, cur_right_pts, status); reduceTwinVector(forw_left_pts, forw_right_pts, status); reduceVector(ids, status); // reduceTwinVector(prev_left_un_pts, prev_right_un_pts, status); reduceTwinVector(cur_left_un_pts, cur_right_un_pts, status); reduceVector(track_cnt, status); } } for (auto& n: track_cnt) ++n; if (pub_this_frame) { rejectWithTwinRANSAC(); setMask(); int n_max_cnt = param.max_cnt - static_cast(forw_left_pts.size()); if (n_max_cnt > 0) { if (mask.empty()) ROS_WARN("[Stereo Tracker]: Mask is empty!"); if (mask.type() != CV_8UC1) ROS_WARN("[Stereo Tracker]: Mask type wrong!"); if (mask.size() != forw_left_img.size()) ROS_WARN("[Stereo Tracker]: Mask size wrong!"); n_left_pts.clear(); n_right_pts.clear(); cv::goodFeaturesToTrack(forw_left_img, n_left_pts, n_max_cnt, 0.01, param.min_dist, mask); if (n_left_pts.size() > 0) { std::vector status; std::vector err; predictHomography(n_left_pts, n_right_pts); cv::calcOpticalFlowPyrLK(forw_left_img, forw_right_img, n_left_pts, n_right_pts, status, err, cv::Size(15, 15), 3, cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 30, 0.01), cv::OPTFLOW_USE_INITIAL_FLOW); for (unsigned int i = 0; i < n_right_pts.size(); ++i) if (status[i] && !inBorder(n_right_pts[i])) status[i] = 0; markStereoMatchOutlier(n_left_pts, n_right_pts, status); reduceTwinVector(n_left_pts, n_right_pts, status); } } else { n_left_pts.clear(); n_right_pts.clear(); } addPoints(); } prev_left_img = cur_left_img; prev_right_img = cur_right_img; prev_left_pts = cur_left_pts; prev_right_pts = cur_right_pts; prev_left_un_pts = cur_left_un_pts; prev_right_un_pts = cur_right_un_pts; cur_left_img = forw_left_img; cur_right_img = forw_right_img; cur_left_pts = forw_left_pts; cur_right_pts = forw_right_pts; undistortedPoints(); prev_time = cur_time; updateID(); if (param.print_track_info) ROS_INFO("[Stereo Tracker]: Current feature pairs: %i, including newly detected pairs: %i", int(cur_left_pts.size()), int(n_left_pts.size())); } void StereoTracker::predictHomography(const std::vector& left_pts, std::vector& right_pts) { right_pts.clear(); const Eigen::Matrix3d C_cl2cr = param.T_cr2i.linear().transpose()*param.T_cl2i.linear(); for (unsigned int i = 0; i < left_pts.size(); ++i) { Eigen::Vector3d tmp_lpt; m_camera_left->liftProjective(Eigen::Vector2d(left_pts[i].x, left_pts[i].y), tmp_lpt); Eigen::Vector3d tmp_rpt = C_cl2cr*tmp_lpt; Eigen::Vector2d rpt; m_camera_right->spaceToPlane(tmp_rpt, rpt); right_pts.push_back(cv::Point2f(rpt.x(), rpt.y())); } } void StereoTracker::publishStereoImg(const cv::Mat& left_img, const cv::Mat& right_img, const std::vector& left_pts, const std::vector& right_pts, const int row, const int col, bool pair_line) { cv::Mat track_img(row, 2*col, CV_8UC3); cv::cvtColor(left_img, track_img.colRange(0, col), CV_GRAY2BGR); cv::cvtColor(right_img, track_img.colRange(col, 2*col), CV_GRAY2BGR); for (unsigned int j = 0; j < left_pts.size(); ++j) cv::circle(track_img, left_pts[j], 3, cv::Scalar(255, 0, 0), 2); for (unsigned int j = 0; j < right_pts.size(); ++j) cv::circle(track_img, cv::Point2f(right_pts[j].x + col, right_pts[j].y), 3, cv::Scalar(255, 0, 0), 2); if (pair_line && left_pts.size() == right_pts.size()) for (unsigned int j = 0; j < left_pts.size(); ++j) cv::line(track_img, left_pts[j], cv::Point2f(right_pts[j].x + col, right_pts[j].y), cv::Scalar(255, 0, 0), 2, 4); cv::imshow("stereo_match", track_img); cv::waitKey(0); } void StereoTracker::updateID() { for (unsigned int i = 0; i < ids.size(); ++i) if (ids[i] == -1) ids[i] = n_id++; } void StereoTracker::undistortedPoints() { cur_left_un_pts.clear(); cur_right_un_pts.clear(); for (unsigned int i = 0; i < cur_left_pts.size(); ++i) { Eigen::Vector2d al(cur_left_pts[i].x, cur_left_pts[i].y); Eigen::Vector2d ar(cur_right_pts[i].x, cur_right_pts[i].y); Eigen::Vector3d bl, br; m_camera_left->liftProjective(al, bl); m_camera_right->liftProjective(ar, br); cur_left_un_pts.push_back(cv::Point2f(bl.x()/bl.z(), bl.y()/bl.z())); cur_right_un_pts.push_back(cv::Point2f(br.x()/br.z(), br.y()/br.z())); } } void StereoTracker::addPoints() { for (unsigned int i = 0; i < n_left_pts.size(); ++i) { forw_left_pts.push_back(n_left_pts[i]); forw_right_pts.push_back(n_right_pts[i]); ids.push_back(-1); track_cnt.push_back(1); } } void StereoTracker::setMask() { using namespace std; mask = cv::Mat(param.row, param.col, CV_8UC1, cv::Scalar(255)); typedef pair>> pts_pair; typedef vector pts_pair_vector; pts_pair_vector cnt_lpts_rpts_id; for (unsigned int i = 0; i < forw_left_pts.size(); ++i) cnt_lpts_rpts_id.push_back(make_pair(track_cnt[i], make_pair(forw_left_pts[i], make_pair(forw_right_pts[i], ids[i])))); sort(cnt_lpts_rpts_id.begin(), cnt_lpts_rpts_id.end(), [](const pts_pair& a, const pts_pair& b){ return a.first > b.first; }); forw_left_pts.clear(); forw_right_pts.clear(); ids.clear(); track_cnt.clear(); for (auto& it: cnt_lpts_rpts_id) { if (mask.at(it.second.first) == 255) { forw_left_pts.push_back(it.second.first); forw_right_pts.push_back(it.second.second.first); track_cnt.push_back(it.first); ids.push_back(it.second.second.second); cv::circle(mask, it.second.first, param.min_dist, 0, -1); } } } void StereoTracker::rejectWithTwinRANSAC() { int origin_pts_num = forw_left_pts.size(); if (forw_left_pts.size() >= 8) { std::vector un_cur_left_pts(cur_left_pts.size()), un_forw_left_pts(forw_left_pts.size()); double left_focal = 1.0/m_camera_left->getNormalPixel(); for (unsigned int i = 0; i < cur_left_pts.size(); ++i) { Eigen::Vector3d tmp_p; m_camera_left->liftProjective(Eigen::Vector2d(cur_left_pts[i].x, cur_left_pts[i].y), tmp_p); tmp_p.x() = left_focal*tmp_p.x()/tmp_p.z() + param.col/2.0; tmp_p.y() = left_focal*tmp_p.y()/tmp_p.z() + param.row/2.0; un_cur_left_pts[i] = cv::Point2f(tmp_p.x(), tmp_p.y()); m_camera_left->liftProjective(Eigen::Vector2d(forw_left_pts[i].x, forw_left_pts[i].y), tmp_p); tmp_p.x() = left_focal*tmp_p.x()/tmp_p.z() + param.col/2.0; tmp_p.y() = left_focal*tmp_p.y()/tmp_p.z() + param.row/2.0; un_forw_left_pts[i] = cv::Point2f(tmp_p.x(), tmp_p.y()); } std::vector status; cv::findFundamentalMat(un_cur_left_pts, un_forw_left_pts, cv::FM_RANSAC, param.F_threshold, 0.99, status); reduceTwinVector(prev_left_pts, prev_right_pts, status); reduceTwinVector(cur_left_pts, cur_right_pts, status); reduceTwinVector(forw_left_pts, forw_right_pts, status); reduceTwinVector(cur_left_un_pts, cur_right_un_pts, status); // reduceTwinVector(prev_left_un_pts, prev_right_un_pts, status); reduceVector(ids, status); reduceVector(track_cnt, status); } if (forw_right_pts.size() > 8) { std::vector un_cur_right_pts(cur_right_pts.size()), un_forw_right_pts(forw_right_pts.size()); double right_focal = 1.0/m_camera_right->getNormalPixel(); for (unsigned int i = 0; i < cur_right_pts.size(); ++i) { Eigen::Vector3d tmp_p; m_camera_right->liftProjective(Eigen::Vector2d(cur_right_pts[i].x, cur_right_pts[i].y), tmp_p); tmp_p.x() = right_focal*tmp_p.x()/tmp_p.z() + param.col/2.0; tmp_p.y() = right_focal*tmp_p.y()/tmp_p.z() + param.row/2.0; un_cur_right_pts[i] = cv::Point2f(tmp_p.x(), tmp_p.y()); m_camera_right->liftProjective(Eigen::Vector2d(forw_right_pts[i].x, forw_right_pts[i].y), tmp_p); tmp_p.x() = right_focal*tmp_p.x()/tmp_p.z() + param.col/2.0; tmp_p.y() = right_focal*tmp_p.y()/tmp_p.z() + param.row/2.0; un_forw_right_pts[i] = cv::Point2f(tmp_p.x(), tmp_p.y()); } std::vector status; cv::findFundamentalMat(un_cur_right_pts, un_forw_right_pts, cv::FM_RANSAC, param.F_threshold, 0.99, status); reduceTwinVector(prev_left_pts, prev_right_pts, status); reduceTwinVector(cur_left_pts, cur_right_pts, status); reduceTwinVector(forw_left_pts, forw_right_pts, status); reduceTwinVector(cur_left_un_pts, cur_right_un_pts, status); // reduceTwinVector(prev_left_un_pts, prev_right_un_pts, status); reduceVector(ids, status); reduceVector(track_cnt, status); } int final_pts_num = forw_left_pts.size(); if (origin_pts_num > 0 && final_pts_num != origin_pts_num && param.print_track_info) ROS_INFO("[Stereo Tracker]: Twin side RANSAC remove %f %% of points.", 100.0-100.0*float(final_pts_num)/float(origin_pts_num)); } void StereoTracker::markStereoMatchOutlier(const std::vector& left_pts, const std::vector& right_pts, std::vector& status) { Eigen::Isometry3d T_cl2cr = param.T_cr2i.inverse()*param.T_cl2i; Eigen::Matrix3d t_hat; const Eigen::Vector3d& t = T_cl2cr.translation(); t_hat << 0.0, -t.z(), t.y(), t.z(), 0.0, -t.x(), -t.y(), t.x(), 0.0; const Eigen::Matrix3d essen_mat = t_hat*T_cl2cr.linear(); double unmatch_stereo_thres = 2.0*param.epipolar_thres/(1.0/m_camera_left->getNormalPixel() + 1.0/m_camera_right->getNormalPixel()); int remove_num = 0; int total_num = 0; for (unsigned int i = 0; i < left_pts.size(); ++i) { if (status[i] == 0) continue; ++total_num; Eigen::Vector3d left_proj, right_proj; m_camera_left->liftProjective(Eigen::Vector2d(left_pts[i].x, left_pts[i].y), left_proj); m_camera_right->liftProjective(Eigen::Vector2d(right_pts[i].x, right_pts[i].y), right_proj); Eigen::Vector3d epipolar_line = essen_mat*left_proj; double norm_err = std::fabs((right_proj.transpose()*epipolar_line)[0]/std::sqrt(epipolar_line[0]*epipolar_line[0]+epipolar_line[1]*epipolar_line[1])); if (norm_err > unmatch_stereo_thres) { status[i] = 0; ++remove_num; } } if (total_num > 0 && param.print_track_info) ROS_INFO("[Stereo Tracker]: Remove %f %% of stereo matches through epipolar constrains!", float(remove_num)/float(total_num)*100.0); } bool StereoTracker::inBorder(const cv::Point2f& pt) { const int BORDER_SIZE = 1; int img_x = cvRound(pt.x); int img_y = cvRound(pt.y); return BORDER_SIZE <= img_x && img_x < param.col-BORDER_SIZE && BORDER_SIZE <= img_y && img_y < param.row-BORDER_SIZE; } } ================================================ FILE: feature_tracker/src/stereo_tracker_node.cpp ================================================ /** This File is part of Feature Tracker * * Copyright (C) 2022 Changwu Liu (cwliu529@163.com, * lcw18@mails.tsinghua.edu.cn (valid until 2023)) * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program. If not, see . */ #include "stereo_tracker.h" int main(int argc, char** argv) { ros::init(argc, argv, "stereo_tracker"); ros::NodeHandle n("~"); ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info); feature_tracker::StereoTrackerPtr stereo_tracker_ptr(new feature_tracker::StereoTracker(n)); stereo_tracker_ptr->init(); ros::spin(); return 0; } ================================================ FILE: gnss_comm/.gitignore ================================================ .vscode/ ================================================ FILE: gnss_comm/CMakeLists.txt ================================================ cmake_minimum_required(VERSION 2.8.3) project(gnss_comm) set(CMAKE_BUILD_TYPE "release") set(CMAKE_CXX_FLAGS "-std=c++11 ") set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -fPIC") find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs message_generation ) file(GLOB_RECURSE message_files ${PROJECT_SOURCE_DIR}/msg/*.msg) add_message_files(FILES GnssTimeMsg.msg GnssEphemMsg.msg GnssGloEphemMsg.msg GnssMeasMsg.msg GnssObsMsg.msg GnssBestXYZMsg.msg GnssPVTSolnMsg.msg GnssSvsMsg.msg GnssTimePulseInfoMsg.msg StampedFloat64Array.msg ) generate_messages( DEPENDENCIES std_msgs ) catkin_package( CATKIN_DEPENDS message_runtime ) catkin_package( INCLUDE_DIRS include LIBRARIES ${PROJECT_NAME} CATKIN_DEPENDS roscpp std_msgs message_generation DEPENDS ) set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake) find_package(Eigen3 REQUIRED) find_package(Glog REQUIRED) include_directories( ${EIGEN3_INCLUDE_DIR} ${GLOG_INCLUDE_DIR} ${catkin_INCLUDE_DIRS} ${PROJECT_SOURCE_DIR}/include/ ) file(GLOB_RECURSE source_files ${PROJECT_SOURCE_DIR}/src/*.cpp) add_library(${PROJECT_NAME} ${source_files}) target_include_directories(${PROJECT_NAME} PUBLIC ${EIGEN3_INCLUDE_DIR} ${catkin_INCLUDE_DIRS} ${PROJECT_SOURCE_DIR}/include/${PROJECT_NAME}/ ) target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${GLOG_LIBRARIES}) add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_generate_messages_cpp) ================================================ FILE: gnss_comm/LICENSE ================================================ GNU GENERAL PUBLIC LICENSE Version 3, 29 June 2007 Copyright (C) 2007 Free Software Foundation, Inc. Everyone is permitted to copy and distribute verbatim copies of this license document, but changing it is not allowed. Preamble The GNU General Public License is a free, copyleft license for software and other kinds of works. The licenses for most software and other practical works are designed to take away your freedom to share and change the works. By contrast, the GNU General Public License is intended to guarantee your freedom to share and change all versions of a program--to make sure it remains free software for all its users. We, the Free Software Foundation, use the GNU General Public License for most of our software; it applies also to any other work released this way by its authors. You can apply it to your programs, too. When we speak of free software, we are referring to freedom, not price. Our General Public Licenses are designed to make sure that you have the freedom to distribute copies of free software (and charge for them if you wish), that you receive source code or can get it if you want it, that you can change the software or use pieces of it in new free programs, and that you know you can do these things. To protect your rights, we need to prevent others from denying you these rights or asking you to surrender the rights. Therefore, you have certain responsibilities if you distribute copies of the software, or if you modify it: responsibilities to respect the freedom of others. For example, if you distribute copies of such a program, whether gratis or for a fee, you must pass on to the recipients the same freedoms that you received. You must make sure that they, too, receive or can get the source code. And you must show them these terms so they know their rights. Developers that use the GNU GPL protect your rights with two steps: (1) assert copyright on the software, and (2) offer you this License giving you legal permission to copy, distribute and/or modify it. For the developers' and authors' protection, the GPL clearly explains that there is no warranty for this free software. For both users' and authors' sake, the GPL requires that modified versions be marked as changed, so that their problems will not be attributed erroneously to authors of previous versions. Some devices are designed to deny users access to install or run modified versions of the software inside them, although the manufacturer can do so. This is fundamentally incompatible with the aim of protecting users' freedom to change the software. The systematic pattern of such abuse occurs in the area of products for individuals to use, which is precisely where it is most unacceptable. Therefore, we have designed this version of the GPL to prohibit the practice for those products. If such problems arise substantially in other domains, we stand ready to extend this provision to those domains in future versions of the GPL, as needed to protect the freedom of users. Finally, every program is threatened constantly by software patents. States should not allow patents to restrict development and use of software on general-purpose computers, but in those that do, we wish to avoid the special danger that patents applied to a free program could make it effectively proprietary. To prevent this, the GPL assures that patents cannot be used to render the program non-free. The precise terms and conditions for copying, distribution and modification follow. TERMS AND CONDITIONS 0. Definitions. "This License" refers to version 3 of the GNU General Public License. "Copyright" also means copyright-like laws that apply to other kinds of works, such as semiconductor masks. "The Program" refers to any copyrightable work licensed under this License. Each licensee is addressed as "you". "Licensees" and "recipients" may be individuals or organizations. To "modify" a work means to copy from or adapt all or part of the work in a fashion requiring copyright permission, other than the making of an exact copy. The resulting work is called a "modified version" of the earlier work or a work "based on" the earlier work. A "covered work" means either the unmodified Program or a work based on the Program. To "propagate" a work means to do anything with it that, without permission, would make you directly or secondarily liable for infringement under applicable copyright law, except executing it on a computer or modifying a private copy. Propagation includes copying, distribution (with or without modification), making available to the public, and in some countries other activities as well. To "convey" a work means any kind of propagation that enables other parties to make or receive copies. Mere interaction with a user through a computer network, with no transfer of a copy, is not conveying. An interactive user interface displays "Appropriate Legal Notices" to the extent that it includes a convenient and prominently visible feature that (1) displays an appropriate copyright notice, and (2) tells the user that there is no warranty for the work (except to the extent that warranties are provided), that licensees may convey the work under this License, and how to view a copy of this License. If the interface presents a list of user commands or options, such as a menu, a prominent item in the list meets this criterion. 1. Source Code. The "source code" for a work means the preferred form of the work for making modifications to it. "Object code" means any non-source form of a work. A "Standard Interface" means an interface that either is an official standard defined by a recognized standards body, or, in the case of interfaces specified for a particular programming language, one that is widely used among developers working in that language. The "System Libraries" of an executable work include anything, other than the work as a whole, that (a) is included in the normal form of packaging a Major Component, but which is not part of that Major Component, and (b) serves only to enable use of the work with that Major Component, or to implement a Standard Interface for which an implementation is available to the public in source code form. A "Major Component", in this context, means a major essential component (kernel, window system, and so on) of the specific operating system (if any) on which the executable work runs, or a compiler used to produce the work, or an object code interpreter used to run it. The "Corresponding Source" for a work in object code form means all the source code needed to generate, install, and (for an executable work) run the object code and to modify the work, including scripts to control those activities. However, it does not include the work's System Libraries, or general-purpose tools or generally available free programs which are used unmodified in performing those activities but which are not part of the work. For example, Corresponding Source includes interface definition files associated with source files for the work, and the source code for shared libraries and dynamically linked subprograms that the work is specifically designed to require, such as by intimate data communication or control flow between those subprograms and other parts of the work. The Corresponding Source need not include anything that users can regenerate automatically from other parts of the Corresponding Source. The Corresponding Source for a work in source code form is that same work. 2. Basic Permissions. All rights granted under this License are granted for the term of copyright on the Program, and are irrevocable provided the stated conditions are met. This License explicitly affirms your unlimited permission to run the unmodified Program. The output from running a covered work is covered by this License only if the output, given its content, constitutes a covered work. This License acknowledges your rights of fair use or other equivalent, as provided by copyright law. You may make, run and propagate covered works that you do not convey, without conditions so long as your license otherwise remains in force. You may convey covered works to others for the sole purpose of having them make modifications exclusively for you, or provide you with facilities for running those works, provided that you comply with the terms of this License in conveying all material for which you do not control copyright. Those thus making or running the covered works for you must do so exclusively on your behalf, under your direction and control, on terms that prohibit them from making any copies of your copyrighted material outside their relationship with you. Conveying under any other circumstances is permitted solely under the conditions stated below. Sublicensing is not allowed; section 10 makes it unnecessary. 3. Protecting Users' Legal Rights From Anti-Circumvention Law. No covered work shall be deemed part of an effective technological measure under any applicable law fulfilling obligations under article 11 of the WIPO copyright treaty adopted on 20 December 1996, or similar laws prohibiting or restricting circumvention of such measures. When you convey a covered work, you waive any legal power to forbid circumvention of technological measures to the extent such circumvention is effected by exercising rights under this License with respect to the covered work, and you disclaim any intention to limit operation or modification of the work as a means of enforcing, against the work's users, your or third parties' legal rights to forbid circumvention of technological measures. 4. Conveying Verbatim Copies. You may convey verbatim copies of the Program's source code as you receive it, in any medium, provided that you conspicuously and appropriately publish on each copy an appropriate copyright notice; keep intact all notices stating that this License and any non-permissive terms added in accord with section 7 apply to the code; keep intact all notices of the absence of any warranty; and give all recipients a copy of this License along with the Program. You may charge any price or no price for each copy that you convey, and you may offer support or warranty protection for a fee. 5. Conveying Modified Source Versions. You may convey a work based on the Program, or the modifications to produce it from the Program, in the form of source code under the terms of section 4, provided that you also meet all of these conditions: a) The work must carry prominent notices stating that you modified it, and giving a relevant date. b) The work must carry prominent notices stating that it is released under this License and any conditions added under section 7. This requirement modifies the requirement in section 4 to "keep intact all notices". c) You must license the entire work, as a whole, under this License to anyone who comes into possession of a copy. This License will therefore apply, along with any applicable section 7 additional terms, to the whole of the work, and all its parts, regardless of how they are packaged. This License gives no permission to license the work in any other way, but it does not invalidate such permission if you have separately received it. d) If the work has interactive user interfaces, each must display Appropriate Legal Notices; however, if the Program has interactive interfaces that do not display Appropriate Legal Notices, your work need not make them do so. A compilation of a covered work with other separate and independent works, which are not by their nature extensions of the covered work, and which are not combined with it such as to form a larger program, in or on a volume of a storage or distribution medium, is called an "aggregate" if the compilation and its resulting copyright are not used to limit the access or legal rights of the compilation's users beyond what the individual works permit. Inclusion of a covered work in an aggregate does not cause this License to apply to the other parts of the aggregate. 6. Conveying Non-Source Forms. You may convey a covered work in object code form under the terms of sections 4 and 5, provided that you also convey the machine-readable Corresponding Source under the terms of this License, in one of these ways: a) Convey the object code in, or embodied in, a physical product (including a physical distribution medium), accompanied by the Corresponding Source fixed on a durable physical medium customarily used for software interchange. b) Convey the object code in, or embodied in, a physical product (including a physical distribution medium), accompanied by a written offer, valid for at least three years and valid for as long as you offer spare parts or customer support for that product model, to give anyone who possesses the object code either (1) a copy of the Corresponding Source for all the software in the product that is covered by this License, on a durable physical medium customarily used for software interchange, for a price no more than your reasonable cost of physically performing this conveying of source, or (2) access to copy the Corresponding Source from a network server at no charge. c) Convey individual copies of the object code with a copy of the written offer to provide the Corresponding Source. This alternative is allowed only occasionally and noncommercially, and only if you received the object code with such an offer, in accord with subsection 6b. d) Convey the object code by offering access from a designated place (gratis or for a charge), and offer equivalent access to the Corresponding Source in the same way through the same place at no further charge. You need not require recipients to copy the Corresponding Source along with the object code. If the place to copy the object code is a network server, the Corresponding Source may be on a different server (operated by you or a third party) that supports equivalent copying facilities, provided you maintain clear directions next to the object code saying where to find the Corresponding Source. Regardless of what server hosts the Corresponding Source, you remain obligated to ensure that it is available for as long as needed to satisfy these requirements. e) Convey the object code using peer-to-peer transmission, provided you inform other peers where the object code and Corresponding Source of the work are being offered to the general public at no charge under subsection 6d. A separable portion of the object code, whose source code is excluded from the Corresponding Source as a System Library, need not be included in conveying the object code work. A "User Product" is either (1) a "consumer product", which means any tangible personal property which is normally used for personal, family, or household purposes, or (2) anything designed or sold for incorporation into a dwelling. In determining whether a product is a consumer product, doubtful cases shall be resolved in favor of coverage. For a particular product received by a particular user, "normally used" refers to a typical or common use of that class of product, regardless of the status of the particular user or of the way in which the particular user actually uses, or expects or is expected to use, the product. A product is a consumer product regardless of whether the product has substantial commercial, industrial or non-consumer uses, unless such uses represent the only significant mode of use of the product. "Installation Information" for a User Product means any methods, procedures, authorization keys, or other information required to install and execute modified versions of a covered work in that User Product from a modified version of its Corresponding Source. The information must suffice to ensure that the continued functioning of the modified object code is in no case prevented or interfered with solely because modification has been made. If you convey an object code work under this section in, or with, or specifically for use in, a User Product, and the conveying occurs as part of a transaction in which the right of possession and use of the User Product is transferred to the recipient in perpetuity or for a fixed term (regardless of how the transaction is characterized), the Corresponding Source conveyed under this section must be accompanied by the Installation Information. But this requirement does not apply if neither you nor any third party retains the ability to install modified object code on the User Product (for example, the work has been installed in ROM). The requirement to provide Installation Information does not include a requirement to continue to provide support service, warranty, or updates for a work that has been modified or installed by the recipient, or for the User Product in which it has been modified or installed. Access to a network may be denied when the modification itself materially and adversely affects the operation of the network or violates the rules and protocols for communication across the network. Corresponding Source conveyed, and Installation Information provided, in accord with this section must be in a format that is publicly documented (and with an implementation available to the public in source code form), and must require no special password or key for unpacking, reading or copying. 7. Additional Terms. "Additional permissions" are terms that supplement the terms of this License by making exceptions from one or more of its conditions. Additional permissions that are applicable to the entire Program shall be treated as though they were included in this License, to the extent that they are valid under applicable law. If additional permissions apply only to part of the Program, that part may be used separately under those permissions, but the entire Program remains governed by this License without regard to the additional permissions. When you convey a copy of a covered work, you may at your option remove any additional permissions from that copy, or from any part of it. (Additional permissions may be written to require their own removal in certain cases when you modify the work.) You may place additional permissions on material, added by you to a covered work, for which you have or can give appropriate copyright permission. Notwithstanding any other provision of this License, for material you add to a covered work, you may (if authorized by the copyright holders of that material) supplement the terms of this License with terms: a) Disclaiming warranty or limiting liability differently from the terms of sections 15 and 16 of this License; or b) Requiring preservation of specified reasonable legal notices or author attributions in that material or in the Appropriate Legal Notices displayed by works containing it; or c) Prohibiting misrepresentation of the origin of that material, or requiring that modified versions of such material be marked in reasonable ways as different from the original version; or d) Limiting the use for publicity purposes of names of licensors or authors of the material; or e) Declining to grant rights under trademark law for use of some trade names, trademarks, or service marks; or f) Requiring indemnification of licensors and authors of that material by anyone who conveys the material (or modified versions of it) with contractual assumptions of liability to the recipient, for any liability that these contractual assumptions directly impose on those licensors and authors. All other non-permissive additional terms are considered "further restrictions" within the meaning of section 10. If the Program as you received it, or any part of it, contains a notice stating that it is governed by this License along with a term that is a further restriction, you may remove that term. If a license document contains a further restriction but permits relicensing or conveying under this License, you may add to a covered work material governed by the terms of that license document, provided that the further restriction does not survive such relicensing or conveying. If you add terms to a covered work in accord with this section, you must place, in the relevant source files, a statement of the additional terms that apply to those files, or a notice indicating where to find the applicable terms. Additional terms, permissive or non-permissive, may be stated in the form of a separately written license, or stated as exceptions; the above requirements apply either way. 8. Termination. You may not propagate or modify a covered work except as expressly provided under this License. Any attempt otherwise to propagate or modify it is void, and will automatically terminate your rights under this License (including any patent licenses granted under the third paragraph of section 11). However, if you cease all violation of this License, then your license from a particular copyright holder is reinstated (a) provisionally, unless and until the copyright holder explicitly and finally terminates your license, and (b) permanently, if the copyright holder fails to notify you of the violation by some reasonable means prior to 60 days after the cessation. Moreover, your license from a particular copyright holder is reinstated permanently if the copyright holder notifies you of the violation by some reasonable means, this is the first time you have received notice of violation of this License (for any work) from that copyright holder, and you cure the violation prior to 30 days after your receipt of the notice. Termination of your rights under this section does not terminate the licenses of parties who have received copies or rights from you under this License. If your rights have been terminated and not permanently reinstated, you do not qualify to receive new licenses for the same material under section 10. 9. Acceptance Not Required for Having Copies. You are not required to accept this License in order to receive or run a copy of the Program. Ancillary propagation of a covered work occurring solely as a consequence of using peer-to-peer transmission to receive a copy likewise does not require acceptance. However, nothing other than this License grants you permission to propagate or modify any covered work. These actions infringe copyright if you do not accept this License. Therefore, by modifying or propagating a covered work, you indicate your acceptance of this License to do so. 10. Automatic Licensing of Downstream Recipients. Each time you convey a covered work, the recipient automatically receives a license from the original licensors, to run, modify and propagate that work, subject to this License. You are not responsible for enforcing compliance by third parties with this License. An "entity transaction" is a transaction transferring control of an organization, or substantially all assets of one, or subdividing an organization, or merging organizations. If propagation of a covered work results from an entity transaction, each party to that transaction who receives a copy of the work also receives whatever licenses to the work the party's predecessor in interest had or could give under the previous paragraph, plus a right to possession of the Corresponding Source of the work from the predecessor in interest, if the predecessor has it or can get it with reasonable efforts. You may not impose any further restrictions on the exercise of the rights granted or affirmed under this License. For example, you may not impose a license fee, royalty, or other charge for exercise of rights granted under this License, and you may not initiate litigation (including a cross-claim or counterclaim in a lawsuit) alleging that any patent claim is infringed by making, using, selling, offering for sale, or importing the Program or any portion of it. 11. Patents. A "contributor" is a copyright holder who authorizes use under this License of the Program or a work on which the Program is based. The work thus licensed is called the contributor's "contributor version". A contributor's "essential patent claims" are all patent claims owned or controlled by the contributor, whether already acquired or hereafter acquired, that would be infringed by some manner, permitted by this License, of making, using, or selling its contributor version, but do not include claims that would be infringed only as a consequence of further modification of the contributor version. For purposes of this definition, "control" includes the right to grant patent sublicenses in a manner consistent with the requirements of this License. Each contributor grants you a non-exclusive, worldwide, royalty-free patent license under the contributor's essential patent claims, to make, use, sell, offer for sale, import and otherwise run, modify and propagate the contents of its contributor version. In the following three paragraphs, a "patent license" is any express agreement or commitment, however denominated, not to enforce a patent (such as an express permission to practice a patent or covenant not to sue for patent infringement). To "grant" such a patent license to a party means to make such an agreement or commitment not to enforce a patent against the party. If you convey a covered work, knowingly relying on a patent license, and the Corresponding Source of the work is not available for anyone to copy, free of charge and under the terms of this License, through a publicly available network server or other readily accessible means, then you must either (1) cause the Corresponding Source to be so available, or (2) arrange to deprive yourself of the benefit of the patent license for this particular work, or (3) arrange, in a manner consistent with the requirements of this License, to extend the patent license to downstream recipients. "Knowingly relying" means you have actual knowledge that, but for the patent license, your conveying the covered work in a country, or your recipient's use of the covered work in a country, would infringe one or more identifiable patents in that country that you have reason to believe are valid. If, pursuant to or in connection with a single transaction or arrangement, you convey, or propagate by procuring conveyance of, a covered work, and grant a patent license to some of the parties receiving the covered work authorizing them to use, propagate, modify or convey a specific copy of the covered work, then the patent license you grant is automatically extended to all recipients of the covered work and works based on it. A patent license is "discriminatory" if it does not include within the scope of its coverage, prohibits the exercise of, or is conditioned on the non-exercise of one or more of the rights that are specifically granted under this License. You may not convey a covered work if you are a party to an arrangement with a third party that is in the business of distributing software, under which you make payment to the third party based on the extent of your activity of conveying the work, and under which the third party grants, to any of the parties who would receive the covered work from you, a discriminatory patent license (a) in connection with copies of the covered work conveyed by you (or copies made from those copies), or (b) primarily for and in connection with specific products or compilations that contain the covered work, unless you entered into that arrangement, or that patent license was granted, prior to 28 March 2007. Nothing in this License shall be construed as excluding or limiting any implied license or other defenses to infringement that may otherwise be available to you under applicable patent law. 12. No Surrender of Others' Freedom. If conditions are imposed on you (whether by court order, agreement or otherwise) that contradict the conditions of this License, they do not excuse you from the conditions of this License. If you cannot convey a covered work so as to satisfy simultaneously your obligations under this License and any other pertinent obligations, then as a consequence you may not convey it at all. For example, if you agree to terms that obligate you to collect a royalty for further conveying from those to whom you convey the Program, the only way you could satisfy both those terms and this License would be to refrain entirely from conveying the Program. 13. Use with the GNU Affero General Public License. Notwithstanding any other provision of this License, you have permission to link or combine any covered work with a work licensed under version 3 of the GNU Affero General Public License into a single combined work, and to convey the resulting work. The terms of this License will continue to apply to the part which is the covered work, but the special requirements of the GNU Affero General Public License, section 13, concerning interaction through a network will apply to the combination as such. 14. Revised Versions of this License. The Free Software Foundation may publish revised and/or new versions of the GNU General Public License from time to time. Such new versions will be similar in spirit to the present version, but may differ in detail to address new problems or concerns. Each version is given a distinguishing version number. If the Program specifies that a certain numbered version of the GNU General Public License "or any later version" applies to it, you have the option of following the terms and conditions either of that numbered version or of any later version published by the Free Software Foundation. If the Program does not specify a version number of the GNU General Public License, you may choose any version ever published by the Free Software Foundation. If the Program specifies that a proxy can decide which future versions of the GNU General Public License can be used, that proxy's public statement of acceptance of a version permanently authorizes you to choose that version for the Program. Later license versions may give you additional or different permissions. However, no additional obligations are imposed on any author or copyright holder as a result of your choosing to follow a later version. 15. Disclaimer of Warranty. THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM IS WITH YOU. SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF ALL NECESSARY SERVICING, REPAIR OR CORRECTION. 16. Limitation of Liability. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS), EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH DAMAGES. 17. Interpretation of Sections 15 and 16. If the disclaimer of warranty and limitation of liability provided above cannot be given local legal effect according to their terms, reviewing courts shall apply local law that most closely approximates an absolute waiver of all civil liability in connection with the Program, unless a warranty or assumption of liability accompanies a copy of the Program in return for a fee. END OF TERMS AND CONDITIONS How to Apply These Terms to Your New Programs If you develop a new program, and you want it to be of the greatest possible use to the public, the best way to achieve this is to make it free software which everyone can redistribute and change under these terms. To do so, attach the following notices to the program. It is safest to attach them to the start of each source file to most effectively state the exclusion of warranty; and each file should have at least the "copyright" line and a pointer to where the full notice is found. Copyright (C) This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version. This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with this program. If not, see . Also add information on how to contact you by electronic and paper mail. If the program does terminal interaction, make it output a short notice like this when it starts in an interactive mode: Copyright (C) This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'. This is free software, and you are welcome to redistribute it under certain conditions; type `show c' for details. The hypothetical commands `show w' and `show c' should show the appropriate parts of the General Public License. Of course, your program's commands might be different; for a GUI interface, you would use an "about box". You should also get your employer (if you work as a programmer) or school, if any, to sign a "copyright disclaimer" for the program, if necessary. For more information on this, and how to apply and follow the GNU GPL, see . The GNU General Public License does not permit incorporating your program into proprietary programs. If your program is a subroutine library, you may consider it more useful to permit linking proprietary applications with the library. If this is what you want to do, use the GNU Lesser General Public License instead of this License. But first, please read . ================================================ FILE: gnss_comm/README.md ================================================ # gnss_comm **Authors/Maintainers:** CAO Shaozu (shaozu.cao AT gmail.com) The *gnss_comm* package contains basic definitions and utility functions for GNSS raw measurement processing. ## 1. Prerequisites ### 1.1 C++11 Compiler This package requires some features of C++11. ### 1.2 ROS This package is developed under [ROS Kinetic](http://wiki.ros.org/kinetic) environment. ### 1.3 Eigen Our code uses [Eigen 3.3.3](https://gitlab.com/libeigen/eigen/-/archive/3.3.3/eigen-3.3.3.zip) for matrix manipulation. After downloading and unzipping the Eigen source code package, you may install it with the following commands: ``` cd eigen-3.3.3/ mkdir build cd build cmake .. sudo make install ``` ### 1.4 Glog We use google's glog library for message output. If you are using Ubuntu, install it by: ``` sudo apt-get install libgoogle-glog-dev ``` If you are on other OS or just want to build it from source, please follow [these instructions](https://github.com/google/glog#building-glog-with-cmake) to install it. ## 2. Build gnss_comm library Clone the repository to your catkin workspace (for example `~/catkin_ws/`): ``` cd ~/catkin_ws/src/ git clone https://github.com/HKUST-Aerial-Robotics/gnss_comm.git ``` Then build the package with: ``` cd ~/catkin_ws/ catkin_make source ~/catkin_ws/devel/setup.bash ``` If you encounter any problem during the building of *gnss_comm*, try with docker in [the next section](#docker_section). ## 3. Docker Support To simplify the building process, we add docker in our code. Docker is like a sandbox so it can isolate our code from your local environment. To run with docker, first make sure [ros](http://wiki.ros.org/ROS/Installation) and [docker](https://docs.docker.com/get-docker/) are installed on your machine. Then add your account to `docker` group by `sudo usermod -aG docker $USER`. **Relaunch the terminal or logout and re-login if you get `Permission denied` error**, type: ``` cd ~/catkin_ws/src/gnss_comm/docker make build ``` The docker image `gnss_comm:latest` should be successfully built after a while. Then you can check all available docker images in your local machine by `docker image ls` command. ## 4. Acknowledgements Many of the definitions and utility functions in this package are adapted from [RTKLIB](http://www.rtklib.com/). ## 5. License The source code is released under [GPLv3](https://www.gnu.org/licenses/gpl-3.0.html) license. ================================================ FILE: gnss_comm/cmake/FindEigen.cmake ================================================ # Ceres Solver - A fast non-linear least squares minimizer # Copyright 2015 Google Inc. All rights reserved. # http://ceres-solver.org/ # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: # # * Redistributions of source code must retain the above copyright notice, # this list of conditions and the following disclaimer. # * Redistributions in binary form must reproduce the above copyright notice, # this list of conditions and the following disclaimer in the documentation # and/or other materials provided with the distribution. # * Neither the name of Google Inc. nor the names of its contributors may be # used to endorse or promote products derived from this software without # specific prior written permission. # # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. # # Author: alexs.mac@gmail.com (Alex Stewart) # # FindEigen.cmake - Find Eigen library, version >= 3. # # This module defines the following variables: # # EIGEN_FOUND: TRUE iff Eigen is found. # EIGEN_INCLUDE_DIRS: Include directories for Eigen. # # EIGEN_VERSION: Extracted from Eigen/src/Core/util/Macros.h # EIGEN_WORLD_VERSION: Equal to 3 if EIGEN_VERSION = 3.2.0 # EIGEN_MAJOR_VERSION: Equal to 2 if EIGEN_VERSION = 3.2.0 # EIGEN_MINOR_VERSION: Equal to 0 if EIGEN_VERSION = 3.2.0 # # The following variables control the behaviour of this module: # # EIGEN_INCLUDE_DIR_HINTS: List of additional directories in which to # search for eigen includes, e.g: /timbuktu/eigen3. # # The following variables are also defined by this module, but in line with # CMake recommended FindPackage() module style should NOT be referenced directly # by callers (use the plural variables detailed above instead). These variables # do however affect the behaviour of the module via FIND_[PATH/LIBRARY]() which # are NOT re-called (i.e. search for library is not repeated) if these variables # are set with valid values _in the CMake cache_. This means that if these # variables are set directly in the cache, either by the user in the CMake GUI, # or by the user passing -DVAR=VALUE directives to CMake when called (which # explicitly defines a cache variable), then they will be used verbatim, # bypassing the HINTS variables and other hard-coded search locations. # # EIGEN_INCLUDE_DIR: Include directory for CXSparse, not including the # include directory of any dependencies. # Called if we failed to find Eigen or any of it's required dependencies, # unsets all public (designed to be used externally) variables and reports # error message at priority depending upon [REQUIRED/QUIET/] argument. macro(EIGEN_REPORT_NOT_FOUND REASON_MSG) unset(EIGEN_FOUND) unset(EIGEN_INCLUDE_DIRS) # Make results of search visible in the CMake GUI if Eigen has not # been found so that user does not have to toggle to advanced view. mark_as_advanced(CLEAR EIGEN_INCLUDE_DIR) # Note _FIND_[REQUIRED/QUIETLY] variables defined by FindPackage() # use the camelcase library name, not uppercase. if (Eigen_FIND_QUIETLY) message(STATUS "Failed to find Eigen - " ${REASON_MSG} ${ARGN}) elseif (Eigen_FIND_REQUIRED) message(FATAL_ERROR "Failed to find Eigen - " ${REASON_MSG} ${ARGN}) else() # Neither QUIETLY nor REQUIRED, use no priority which emits a message # but continues configuration and allows generation. message("-- Failed to find Eigen - " ${REASON_MSG} ${ARGN}) endif () return() endmacro(EIGEN_REPORT_NOT_FOUND) # Protect against any alternative find_package scripts for this library having # been called previously (in a client project) which set EIGEN_FOUND, but not # the other variables we require / set here which could cause the search logic # here to fail. unset(EIGEN_FOUND) # Search user-installed locations first, so that we prefer user installs # to system installs where both exist. list(APPEND EIGEN_CHECK_INCLUDE_DIRS /usr/local/include /usr/local/homebrew/include # Mac OS X /opt/local/var/macports/software # Mac OS X. /opt/local/include /usr/include) # Additional suffixes to try appending to each search path. list(APPEND EIGEN_CHECK_PATH_SUFFIXES eigen3 # Default root directory for Eigen. Eigen/include/eigen3 # Windows (for C:/Program Files prefix) < 3.3 Eigen3/include/eigen3 ) # Windows (for C:/Program Files prefix) >= 3.3 # Search supplied hint directories first if supplied. find_path(EIGEN_INCLUDE_DIR NAMES Eigen/Core PATHS ${EIGEN_INCLUDE_DIR_HINTS} ${EIGEN_CHECK_INCLUDE_DIRS} PATH_SUFFIXES ${EIGEN_CHECK_PATH_SUFFIXES}) if (NOT EIGEN_INCLUDE_DIR OR NOT EXISTS ${EIGEN_INCLUDE_DIR}) eigen_report_not_found( "Could not find eigen3 include directory, set EIGEN_INCLUDE_DIR to " "path to eigen3 include directory, e.g. /usr/local/include/eigen3.") endif (NOT EIGEN_INCLUDE_DIR OR NOT EXISTS ${EIGEN_INCLUDE_DIR}) # Mark internally as found, then verify. EIGEN_REPORT_NOT_FOUND() unsets # if called. set(EIGEN_FOUND TRUE) # Extract Eigen version from Eigen/src/Core/util/Macros.h if (EIGEN_INCLUDE_DIR) set(EIGEN_VERSION_FILE ${EIGEN_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h) if (NOT EXISTS ${EIGEN_VERSION_FILE}) eigen_report_not_found( "Could not find file: ${EIGEN_VERSION_FILE} " "containing version information in Eigen install located at: " "${EIGEN_INCLUDE_DIR}.") else (NOT EXISTS ${EIGEN_VERSION_FILE}) file(READ ${EIGEN_VERSION_FILE} EIGEN_VERSION_FILE_CONTENTS) string(REGEX MATCH "#define EIGEN_WORLD_VERSION [0-9]+" EIGEN_WORLD_VERSION "${EIGEN_VERSION_FILE_CONTENTS}") string(REGEX REPLACE "#define EIGEN_WORLD_VERSION ([0-9]+)" "\\1" EIGEN_WORLD_VERSION "${EIGEN_WORLD_VERSION}") string(REGEX MATCH "#define EIGEN_MAJOR_VERSION [0-9]+" EIGEN_MAJOR_VERSION "${EIGEN_VERSION_FILE_CONTENTS}") string(REGEX REPLACE "#define EIGEN_MAJOR_VERSION ([0-9]+)" "\\1" EIGEN_MAJOR_VERSION "${EIGEN_MAJOR_VERSION}") string(REGEX MATCH "#define EIGEN_MINOR_VERSION [0-9]+" EIGEN_MINOR_VERSION "${EIGEN_VERSION_FILE_CONTENTS}") string(REGEX REPLACE "#define EIGEN_MINOR_VERSION ([0-9]+)" "\\1" EIGEN_MINOR_VERSION "${EIGEN_MINOR_VERSION}") # This is on a single line s/t CMake does not interpret it as a list of # elements and insert ';' separators which would result in 3.;2.;0 nonsense. set(EIGEN_VERSION "${EIGEN_WORLD_VERSION}.${EIGEN_MAJOR_VERSION}.${EIGEN_MINOR_VERSION}") endif (NOT EXISTS ${EIGEN_VERSION_FILE}) endif (EIGEN_INCLUDE_DIR) # Set standard CMake FindPackage variables if found. if (EIGEN_FOUND) set(EIGEN_INCLUDE_DIRS ${EIGEN_INCLUDE_DIR}) endif (EIGEN_FOUND) # Handle REQUIRED / QUIET optional arguments and version. include(FindPackageHandleStandardArgs) find_package_handle_standard_args(Eigen REQUIRED_VARS EIGEN_INCLUDE_DIRS VERSION_VAR EIGEN_VERSION) # Only mark internal variables as advanced if we found Eigen, otherwise # leave it visible in the standard GUI for the user to set manually. if (EIGEN_FOUND) mark_as_advanced(FORCE EIGEN_INCLUDE_DIR) endif (EIGEN_FOUND) ================================================ FILE: gnss_comm/cmake/FindGlog.cmake ================================================ # Ceres Solver - A fast non-linear least squares minimizer # Copyright 2015 Google Inc. All rights reserved. # http://ceres-solver.org/ # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: # # * Redistributions of source code must retain the above copyright notice, # this list of conditions and the following disclaimer. # * Redistributions in binary form must reproduce the above copyright notice, # this list of conditions and the following disclaimer in the documentation # and/or other materials provided with the distribution. # * Neither the name of Google Inc. nor the names of its contributors may be # used to endorse or promote products derived from this software without # specific prior written permission. # # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. # # Author: alexs.mac@gmail.com (Alex Stewart) # # FindGlog.cmake - Find Google glog logging library. # # This module defines the following variables: # # GLOG_FOUND: TRUE iff glog is found. # GLOG_INCLUDE_DIRS: Include directories for glog. # GLOG_LIBRARIES: Libraries required to link glog. # FOUND_INSTALLED_GLOG_CMAKE_CONFIGURATION: True iff the version of glog found # was built & installed / exported # as a CMake package. # # The following variables control the behaviour of this module: # # GLOG_PREFER_EXPORTED_GLOG_CMAKE_CONFIGURATION: TRUE/FALSE, iff TRUE then # then prefer using an exported CMake configuration # generated by glog > 0.3.4 over searching for the # glog components manually. Otherwise (FALSE) # ignore any exported glog CMake configurations and # always perform a manual search for the components. # Default: TRUE iff user does not define this variable # before we are called, and does NOT specify either # GLOG_INCLUDE_DIR_HINTS or GLOG_LIBRARY_DIR_HINTS # otherwise FALSE. # GLOG_INCLUDE_DIR_HINTS: List of additional directories in which to # search for glog includes, e.g: /timbuktu/include. # GLOG_LIBRARY_DIR_HINTS: List of additional directories in which to # search for glog libraries, e.g: /timbuktu/lib. # # The following variables are also defined by this module, but in line with # CMake recommended FindPackage() module style should NOT be referenced directly # by callers (use the plural variables detailed above instead). These variables # do however affect the behaviour of the module via FIND_[PATH/LIBRARY]() which # are NOT re-called (i.e. search for library is not repeated) if these variables # are set with valid values _in the CMake cache_. This means that if these # variables are set directly in the cache, either by the user in the CMake GUI, # or by the user passing -DVAR=VALUE directives to CMake when called (which # explicitly defines a cache variable), then they will be used verbatim, # bypassing the HINTS variables and other hard-coded search locations. # # GLOG_INCLUDE_DIR: Include directory for glog, not including the # include directory of any dependencies. # GLOG_LIBRARY: glog library, not including the libraries of any # dependencies. # Reset CALLERS_CMAKE_FIND_LIBRARY_PREFIXES to its value when # FindGlog was invoked. macro(GLOG_RESET_FIND_LIBRARY_PREFIX) if (MSVC AND CALLERS_CMAKE_FIND_LIBRARY_PREFIXES) set(CMAKE_FIND_LIBRARY_PREFIXES "${CALLERS_CMAKE_FIND_LIBRARY_PREFIXES}") endif() endmacro(GLOG_RESET_FIND_LIBRARY_PREFIX) # Called if we failed to find glog or any of it's required dependencies, # unsets all public (designed to be used externally) variables and reports # error message at priority depending upon [REQUIRED/QUIET/] argument. macro(GLOG_REPORT_NOT_FOUND REASON_MSG) unset(GLOG_FOUND) unset(GLOG_INCLUDE_DIRS) unset(GLOG_LIBRARIES) # Make results of search visible in the CMake GUI if glog has not # been found so that user does not have to toggle to advanced view. mark_as_advanced(CLEAR GLOG_INCLUDE_DIR GLOG_LIBRARY) glog_reset_find_library_prefix() # Note _FIND_[REQUIRED/QUIETLY] variables defined by FindPackage() # use the camelcase library name, not uppercase. if (Glog_FIND_QUIETLY) message(STATUS "Failed to find glog - " ${REASON_MSG} ${ARGN}) elseif (Glog_FIND_REQUIRED) message(FATAL_ERROR "Failed to find glog - " ${REASON_MSG} ${ARGN}) else() # Neither QUIETLY nor REQUIRED, use no priority which emits a message # but continues configuration and allows generation. message("-- Failed to find glog - " ${REASON_MSG} ${ARGN}) endif () return() endmacro(GLOG_REPORT_NOT_FOUND) # glog_message([mode] "message text") # # Wraps the standard cmake 'message' command, but suppresses output # if the QUIET flag was passed to the find_package(Glog ...) call. function(GLOG_MESSAGE) if (NOT Glog_FIND_QUIETLY) message(${ARGN}) endif() endfunction() # Protect against any alternative find_package scripts for this library having # been called previously (in a client project) which set GLOG_FOUND, but not # the other variables we require / set here which could cause the search logic # here to fail. unset(GLOG_FOUND) # ----------------------------------------------------------------- # By default, if the user has expressed no preference for using an exported # glog CMake configuration over performing a search for the installed # components, and has not specified any hints for the search locations, then # prefer a glog exported configuration if available. if (NOT DEFINED GLOG_PREFER_EXPORTED_GLOG_CMAKE_CONFIGURATION AND NOT GLOG_INCLUDE_DIR_HINTS AND NOT GLOG_LIBRARY_DIR_HINTS) glog_message(STATUS "No preference for use of exported glog CMake " "configuration set, and no hints for include/library directories provided. " "Defaulting to preferring an installed/exported glog CMake configuration " "if available.") set(GLOG_PREFER_EXPORTED_GLOG_CMAKE_CONFIGURATION TRUE) endif() # On macOS, add the Homebrew prefix (with appropriate suffixes) to the # respective HINTS directories (after any user-specified locations). This # handles Homebrew installations into non-standard locations (not /usr/local). # We do not use CMAKE_PREFIX_PATH for this as given the search ordering of # find_xxx(), doing so would override any user-specified HINTS locations with # the Homebrew version if it exists. if (CMAKE_SYSTEM_NAME MATCHES "Darwin") find_program(HOMEBREW_EXECUTABLE brew) mark_as_advanced(FORCE HOMEBREW_EXECUTABLE) if (HOMEBREW_EXECUTABLE) # Detected a Homebrew install, query for its install prefix. execute_process(COMMAND ${HOMEBREW_EXECUTABLE} --prefix OUTPUT_VARIABLE HOMEBREW_INSTALL_PREFIX OUTPUT_STRIP_TRAILING_WHITESPACE) glog_message(STATUS "Detected Homebrew with install prefix: " "${HOMEBREW_INSTALL_PREFIX}, adding to CMake search paths.") list(APPEND GLOG_INCLUDE_DIR_HINTS "${HOMEBREW_INSTALL_PREFIX}/include") list(APPEND GLOG_LIBRARY_DIR_HINTS "${HOMEBREW_INSTALL_PREFIX}/lib") endif() endif() if (GLOG_PREFER_EXPORTED_GLOG_CMAKE_CONFIGURATION) # Try to find an exported CMake configuration for glog, as generated by # glog versions > 0.3.4 # # We search twice, s/t we can invert the ordering of precedence used by # find_package() for exported package build directories, and installed # packages (found via CMAKE_SYSTEM_PREFIX_PATH), listed as items 6) and 7) # respectively in [1]. # # By default, exported build directories are (in theory) detected first, and # this is usually the case on Windows. However, on OS X & Linux, the install # path (/usr/local) is typically present in the PATH environment variable # which is checked in item 4) in [1] (i.e. before both of the above, unless # NO_SYSTEM_ENVIRONMENT_PATH is passed). As such on those OSs installed # packages are usually detected in preference to exported package build # directories. # # To ensure a more consistent response across all OSs, and as users usually # want to prefer an installed version of a package over a locally built one # where both exist (esp. as the exported build directory might be removed # after installation), we first search with NO_CMAKE_PACKAGE_REGISTRY which # means any build directories exported by the user are ignored, and thus # installed directories are preferred. If this fails to find the package # we then research again, but without NO_CMAKE_PACKAGE_REGISTRY, so any # exported build directories will now be detected. # # To prevent confusion on Windows, we also pass NO_CMAKE_BUILDS_PATH (which # is item 5) in [1]), to not preferentially use projects that were built # recently with the CMake GUI to ensure that we always prefer an installed # version if available. # # NOTE: We use the NAMES option as glog erroneously uses 'google-glog' as its # project name when built with CMake, but exports itself as just 'glog'. # On Linux/OS X this does not break detection as the project name is # not used as part of the install path for the CMake package files, # e.g. /usr/local/lib/cmake/glog, where the suffix is hardcoded # in glog's CMakeLists. However, on Windows the project name *is* # part of the install prefix: C:/Program Files/google-glog/[include,lib]. # However, by default CMake checks: # C:/Program Files/ which does not # exist and thus detection fails. Thus we use the NAMES to force the # search to use both google-glog & glog. # # [1] http://www.cmake.org/cmake/help/v2.8.11/cmake.html#command:find_package find_package(glog QUIET NAMES google-glog glog HINTS ${glog_DIR} ${HOMEBREW_INSTALL_PREFIX} NO_MODULE NO_CMAKE_PACKAGE_REGISTRY NO_CMAKE_BUILDS_PATH) if (glog_FOUND) glog_message(STATUS "Found installed version of glog: ${glog_DIR}") else() # Failed to find an installed version of glog, repeat search allowing # exported build directories. glog_message(STATUS "Failed to find installed glog CMake configuration, " "searching for glog build directories exported with CMake.") # Again pass NO_CMAKE_BUILDS_PATH, as we know that glog is exported and # do not want to treat projects built with the CMake GUI preferentially. find_package(glog QUIET NAMES google-glog glog NO_MODULE NO_CMAKE_BUILDS_PATH) if (glog_FOUND) glog_message(STATUS "Found exported glog build directory: ${glog_DIR}") endif(glog_FOUND) endif(glog_FOUND) set(FOUND_INSTALLED_GLOG_CMAKE_CONFIGURATION ${glog_FOUND}) if (FOUND_INSTALLED_GLOG_CMAKE_CONFIGURATION) glog_message(STATUS "Detected glog version: ${glog_VERSION}") set(GLOG_FOUND ${glog_FOUND}) # glog wraps the include directories into the exported glog::glog target. set(GLOG_INCLUDE_DIR "") set(GLOG_LIBRARY glog::glog) else (FOUND_INSTALLED_GLOG_CMAKE_CONFIGURATION) glog_message(STATUS "Failed to find an installed/exported CMake " "configuration for glog, will perform search for installed glog " "components.") endif (FOUND_INSTALLED_GLOG_CMAKE_CONFIGURATION) endif(GLOG_PREFER_EXPORTED_GLOG_CMAKE_CONFIGURATION) if (NOT GLOG_FOUND) # Either failed to find an exported glog CMake configuration, or user # told us not to use one. Perform a manual search for all glog components. # Handle possible presence of lib prefix for libraries on MSVC, see # also GLOG_RESET_FIND_LIBRARY_PREFIX(). if (MSVC) # Preserve the caller's original values for CMAKE_FIND_LIBRARY_PREFIXES # s/t we can set it back before returning. set(CALLERS_CMAKE_FIND_LIBRARY_PREFIXES "${CMAKE_FIND_LIBRARY_PREFIXES}") # The empty string in this list is important, it represents the case when # the libraries have no prefix (shared libraries / DLLs). set(CMAKE_FIND_LIBRARY_PREFIXES "lib" "" "${CMAKE_FIND_LIBRARY_PREFIXES}") endif (MSVC) # Search user-installed locations first, so that we prefer user installs # to system installs where both exist. list(APPEND GLOG_CHECK_INCLUDE_DIRS /usr/local/include /usr/local/homebrew/include # Mac OS X /opt/local/var/macports/software # Mac OS X. /opt/local/include /usr/include) # Windows (for C:/Program Files prefix). list(APPEND GLOG_CHECK_PATH_SUFFIXES glog/include glog/Include Glog/include Glog/Include google-glog/include # CMake installs with project name prefix. google-glog/Include) list(APPEND GLOG_CHECK_LIBRARY_DIRS /usr/local/lib /usr/local/homebrew/lib # Mac OS X. /opt/local/lib /usr/lib) # Windows (for C:/Program Files prefix). list(APPEND GLOG_CHECK_LIBRARY_SUFFIXES glog/lib glog/Lib Glog/lib Glog/Lib google-glog/lib # CMake installs with project name prefix. google-glog/Lib) # Search supplied hint directories first if supplied. find_path(GLOG_INCLUDE_DIR NAMES glog/logging.h HINTS ${GLOG_INCLUDE_DIR_HINTS} PATHS ${GLOG_CHECK_INCLUDE_DIRS} PATH_SUFFIXES ${GLOG_CHECK_PATH_SUFFIXES}) if (NOT GLOG_INCLUDE_DIR OR NOT EXISTS ${GLOG_INCLUDE_DIR}) glog_report_not_found( "Could not find glog include directory, set GLOG_INCLUDE_DIR " "to directory containing glog/logging.h") endif (NOT GLOG_INCLUDE_DIR OR NOT EXISTS ${GLOG_INCLUDE_DIR}) find_library(GLOG_LIBRARY NAMES glog HINTS ${GLOG_LIBRARY_DIR_HINTS} PATHS ${GLOG_CHECK_LIBRARY_DIRS} PATH_SUFFIXES ${GLOG_CHECK_LIBRARY_SUFFIXES}) if (NOT GLOG_LIBRARY OR NOT EXISTS ${GLOG_LIBRARY}) glog_report_not_found( "Could not find glog library, set GLOG_LIBRARY " "to full path to libglog.") endif (NOT GLOG_LIBRARY OR NOT EXISTS ${GLOG_LIBRARY}) # Mark internally as found, then verify. GLOG_REPORT_NOT_FOUND() unsets # if called. set(GLOG_FOUND TRUE) # Glog does not seem to provide any record of the version in its # source tree, thus cannot extract version. # Catch case when caller has set GLOG_INCLUDE_DIR in the cache / GUI and # thus FIND_[PATH/LIBRARY] are not called, but specified locations are # invalid, otherwise we would report the library as found. if (GLOG_INCLUDE_DIR AND NOT EXISTS ${GLOG_INCLUDE_DIR}/glog/logging.h) glog_report_not_found( "Caller defined GLOG_INCLUDE_DIR:" " ${GLOG_INCLUDE_DIR} does not contain glog/logging.h header.") endif (GLOG_INCLUDE_DIR AND NOT EXISTS ${GLOG_INCLUDE_DIR}/glog/logging.h) # TODO: This regex for glog library is pretty primitive, we use lowercase # for comparison to handle Windows using CamelCase library names, could # this check be better? string(TOLOWER "${GLOG_LIBRARY}" LOWERCASE_GLOG_LIBRARY) if (GLOG_LIBRARY AND NOT "${LOWERCASE_GLOG_LIBRARY}" MATCHES ".*glog[^/]*") glog_report_not_found( "Caller defined GLOG_LIBRARY: " "${GLOG_LIBRARY} does not match glog.") endif (GLOG_LIBRARY AND NOT "${LOWERCASE_GLOG_LIBRARY}" MATCHES ".*glog[^/]*") glog_reset_find_library_prefix() endif(NOT GLOG_FOUND) # Set standard CMake FindPackage variables if found. if (GLOG_FOUND) set(GLOG_INCLUDE_DIRS ${GLOG_INCLUDE_DIR}) set(GLOG_LIBRARIES ${GLOG_LIBRARY}) endif (GLOG_FOUND) # If we are using an exported CMake glog target, the include directories are # wrapped into the target itself, and do not have to be (and are not) # separately specified. In which case, we should not add GLOG_INCLUDE_DIRS # to the list of required variables in order that glog be reported as found. if (FOUND_INSTALLED_GLOG_CMAKE_CONFIGURATION) set(GLOG_REQUIRED_VARIABLES GLOG_LIBRARIES) else() set(GLOG_REQUIRED_VARIABLES GLOG_INCLUDE_DIRS GLOG_LIBRARIES) endif() # Handle REQUIRED / QUIET optional arguments. include(FindPackageHandleStandardArgs) find_package_handle_standard_args(Glog DEFAULT_MSG ${GLOG_REQUIRED_VARIABLES}) # Only mark internal variables as advanced if we found glog, otherwise # leave them visible in the standard GUI for the user to set manually. if (GLOG_FOUND) mark_as_advanced(FORCE GLOG_INCLUDE_DIR GLOG_LIBRARY glog_DIR) # Autogenerated by find_package(glog) endif (GLOG_FOUND) ================================================ FILE: gnss_comm/docker/Dockerfile ================================================ FROM ros:kinetic-perception ENV EIGEN_VERSION="3.3.3" ENV CATKIN_WS=/root/catkin_ws # set up thread number for building RUN apt-get update && apt-get install -y \ git \ cmake \ python-catkin-tools \ libgoogle-glog-dev && \ rm -rf /var/lib/apt/lists/* && \ # install eigen git clone https://gitlab.com/libeigen/eigen.git && \ cd eigen && \ git checkout tags/${EIGEN_VERSION} && \ mkdir build && cd build && \ cmake .. && make install && \ cd ../.. && rm -rf eigen && \ # create gnss_comm directory mkdir -p $CATKIN_WS/src/gnss_comm/ # Copy the local replica of gnss_comm COPY ./ $CATKIN_WS/src/gnss_comm/ # use the following line if you only have this dockerfile # RUN git clone https://github.com/HKUST-Aerial-Robotics/gnss_comm.git # Build gnss_comm WORKDIR $CATKIN_WS ENV TERM xterm ENV PYTHONIOENCODING UTF-8 RUN catkin config \ --extend /opt/ros/$ROS_DISTRO \ --cmake-args \ -DCMAKE_BUILD_TYPE=Release && \ catkin build && \ sed -i '/exec "$@"/i \ source "/root/catkin_ws/devel/setup.bash"' /ros_entrypoint.sh ================================================ FILE: gnss_comm/docker/Makefile ================================================ all: help help: @echo "" @echo "-- Help Menu" @echo "" @echo " 1. make build - build all images" @echo " 1. make clean - remove all images" @echo "" build: @docker build --tag gnss_comm -f ./Dockerfile .. clean: @docker rmi -f gnss_comm ================================================ FILE: gnss_comm/include/gnss_comm/gnss_constant.hpp ================================================ /** * This file is part of gnss_comm. * * Copyright (C) 2021 Aerial Robotics Group, Hong Kong University of Science and Technology * Author: CAO Shaozu (shaozu.cao@gmail.com) * * gnss_comm is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * gnss_comm is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with gnss_comm. If not, see . * * As most of the GNSS-related constants are adapted from RTKLIB, * the license for those part of code is claimed as follows: * * The RTKLIB software package is distributed under the following BSD 2-clause * license (http://opensource.org/licenses/BSD-2-Clause) and additional two * exclusive clauses. Users are permitted to develop, produce or sell their own * non-commercial or commercial products utilizing, linking or including RTKLIB as * long as they comply with the license. * * Copyright (c) 2007-2020, T. Takasu, All rights reserved. */ #ifndef GNSS_CONSTANT_HPP_ #define GNSS_CONSTANT_HPP_ #include #include #include #include #include #include #include #include namespace gnss_comm { // some parameters are adapted from RTKLIB #define MAXFREQ 7 /* max N_FREQ */ #define FREQ1 1.57542E9 /* L1/E1 frequency (Hz) */ #define FREQ2 1.22760E9 /* L2 frequency (Hz) */ #define FREQ5 1.17645E9 /* L5/E5a frequency (Hz) */ #define FREQ6 1.27875E9 /* E6/LEX frequency (Hz) */ #define FREQ7 1.20714E9 /* E5b frequency (Hz) */ #define FREQ8 1.191795E9 /* E5a+b frequency (Hz) */ #define FREQ9 2.492028E9 /* S frequency (Hz) */ #define FREQ1_GLO 1.60200E9 /* GLONASS G1 base frequency (Hz) */ #define DFRQ1_GLO 0.56250E6 /* GLONASS G1 bias frequency (Hz/n) */ #define FREQ2_GLO 1.24600E9 /* GLONASS G2 base frequency (Hz) */ #define DFRQ2_GLO 0.43750E6 /* GLONASS G2 bias frequency (Hz/n) */ #define FREQ3_GLO 1.202025E9 /* GLONASS G3 frequency (Hz) */ #define FREQ1_BDS 1.561098E9 /* BeiDou B1 frequency (Hz) */ #define FREQ2_BDS 1.20714E9 /* BeiDou B2 frequency (Hz) */ #define FREQ3_BDS 1.26852E9 /* BeiDou B3 frequency (Hz) */ #define SYS_NONE 0x00 /* navigation system: none */ #define SYS_GPS 0x01 /* navigation system: GPS */ #define SYS_SBS 0x02 /* navigation system: SBAS */ #define SYS_GLO 0x04 /* navigation system: GLONASS */ #define SYS_GAL 0x08 /* navigation system: Galileo */ #define SYS_QZS 0x10 /* navigation system: QZSS */ #define SYS_BDS 0x20 /* navigation system: BeiDou */ #define SYS_IRN 0x40 /* navigation system: IRNSS */ #define SYS_LEO 0x80 /* navigation system: LEO */ #define SYS_ALL 0xFF /* navigation system: all */ #define T_SYS_GPS 0 /* time system: GPS time */ #define T_SYS_UTC 1 /* time system: UTC */ #define T_SYS_GLO 2 /* time system: GLONASS time */ #define T_SYS_GAL 3 /* time system: Galileo time */ #define T_SYS_QZS 4 /* time system: QZSS time */ #define T_SYS_BDS 5 /* time system: BeiDou time */ #ifndef N_FREQ #define N_FREQ 3 /* number of carrier frequencies */ #endif #define N_FREQ_GLO 2 /* number of carrier frequencies of GLONASS */ #define MIN_PRN_GPS 1 /* min satellite PRN number of GPS */ #define MAX_PRN_GPS 32 /* max satellite PRN number of GPS */ #define N_SAT_GPS (MAX_PRN_GPS-MIN_PRN_GPS+1) /* number of GPS satellites */ #define N_SYS_GPS 1 // #define MIN_PRN_GLO 38 /* min satellite slot number of GLONASS */ For UM4B0 // #define MAX_PRN_GLO 61 /* max satellite slot number of GLONASS */ For UM4B0 #define MIN_PRN_GLO 1 /* min satellite slot number of GLONASS */ #define MAX_PRN_GLO 27 /* max satellite slot number of GLONASS */ #define N_SAT_GLO (MAX_PRN_GLO-MIN_PRN_GLO+1) /* number of GLONASS satellites */ #define N_SYS_GLO 1 #define MIN_PRN_GAL 1 /* min satellite PRN number of Galileo */ #define MAX_PRN_GAL 38 /* max satellite PRN number of Galileo */ #define N_SAT_GAL (MAX_PRN_GAL-MIN_PRN_GAL+1) /* number of Galileo satellites */ #define N_SYS_GAL 1 #define MIN_PRN_BDS 1 /* min satellite sat number of BeiDou */ #define MAX_PRN_BDS 63 /* max satellite sat number of BeiDou */ #define N_SAT_BDS (MAX_PRN_BDS-MIN_PRN_BDS+1) /* number of BeiDou satellites */ #define N_SYS_BDS 1 #define N_SYS (N_SYS_GPS+N_SYS_GLO+N_SYS_GAL++N_SYS_BDS) /* number of systems */ #define MAX_SAT (N_SAT_GPS+N_SAT_GLO+N_SAT_GAL+N_SAT_BDS) #ifndef MAXOBS #define MAXOBS 64 /* max number of obs in an epoch */ #endif // solution type // #define SOL_COMPUTED 0 // #define SOL_INSUFFICIENT_OBS 1 // #define NO_CONVERGENCE 2 // #define COV_TRACE 4 // extend solution type // #define EXT_NONE 0x00 // #define EXT_RTK 0x01 // #define EXT_IONO_BRDC 0x02 // #define EXT_IONO_SBAS 0x04 // #define EXT_IONO_IFLC 0x06 // #define EXT_IONO_DIFF 0x08 // position(velocity) type #define PV_NONE 0 /* no solution */ #define PV_FIXED_POS 1 #define PV_FIXED_HEIGHT 2 #define PV_DOPPLER_VELOCITY 8 #define PV_SINGLE 16 #define PV_PSR_DIFF 17 #define PV_WASS 18 #define PV_L1_FLOAT 32 #define PV_IONO_FREE_FLOAT 33 #define PV_NARROW_FLOW 34 #define PV_L1_INT 48 #define PV_WIDE_INT 49 #define PV_NARROW_INT 50 #define PV_INS 52 #define PV_INS_PSR_SP 53 #define PV_INS_PSR_DIFF 54 #define PV_INS_RTK_FLOAT 55 #define PV_INS_RTK_FIXED 56 #define CODE_NONE 0 /* obs code: none or unknown */ #define CODE_L1C 1 /* obs code: L1C/A,G1C/A,E1C (GPS,GLO,GAL,QZS,SBS) */ #define CODE_L1P 2 /* obs code: L1P,G1P (GPS,GLO) */ #define CODE_L1W 3 /* obs code: L1 Z-track (GPS) */ #define CODE_L1Y 4 /* obs code: L1Y (GPS) */ #define CODE_L1M 5 /* obs code: L1M (GPS) */ #define CODE_L1N 6 /* obs code: L1codeless (GPS) */ #define CODE_L1S 7 /* obs code: L1C(D) (GPS,QZS) */ #define CODE_L1L 8 /* obs code: L1C(P) (GPS,QZS) */ #define CODE_L1E 9 /* obs code: L1-SAIF (QZS) */ #define CODE_L1A 10 /* obs code: E1A (GAL) */ #define CODE_L1B 11 /* obs code: E1B (GAL) */ #define CODE_L1X 12 /* obs code: E1B+C,L1C(D+P) (GAL,QZS) */ #define CODE_L1Z 13 /* obs code: E1A+B+C,L1SAIF (GAL,QZS) */ #define CODE_L2C 14 /* obs code: L2C/A,G1C/A (GPS,GLO) */ #define CODE_L2D 15 /* obs code: L2 L1C/A-(P2-P1) (GPS) */ #define CODE_L2S 16 /* obs code: L2C(M) (GPS,QZS) */ #define CODE_L2L 17 /* obs code: L2C(L) (GPS,QZS) */ #define CODE_L2X 18 /* obs code: L2C(M+L),B1I+Q (GPS,QZS,CMP) */ #define CODE_L2P 19 /* obs code: L2P,G2P (GPS,GLO) */ #define CODE_L2W 20 /* obs code: L2 Z-track (GPS) */ #define CODE_L2Y 21 /* obs code: L2Y (GPS) */ #define CODE_L2M 22 /* obs code: L2M (GPS) */ #define CODE_L2N 23 /* obs code: L2codeless (GPS) */ #define CODE_L5I 24 /* obs code: L5/E5aI (GPS,GAL,QZS,SBS) */ #define CODE_L5Q 25 /* obs code: L5/E5aQ (GPS,GAL,QZS,SBS) */ #define CODE_L5X 26 /* obs code: L5/E5aI+Q (GPS,GAL,QZS,SBS) */ #define CODE_L7I 27 /* obs code: E5bI,B2I (GAL,CMP) */ #define CODE_L7Q 28 /* obs code: E5bQ,B2Q (GAL,CMP) */ #define CODE_L7X 29 /* obs code: E5bI+Q,B2I+Q (GAL,CMP) */ #define CODE_L6A 30 /* obs code: E6A (GAL) */ #define CODE_L6B 31 /* obs code: E6B (GAL) */ #define CODE_L6C 32 /* obs code: E6C (GAL) */ #define CODE_L6X 33 /* obs code: E6B+C,LEXS+L,B3I+Q (GAL,QZS,CMP) */ #define CODE_L6Z 34 /* obs code: E6A+B+C (GAL) */ #define CODE_L6S 35 /* obs code: LEXS (QZS) */ #define CODE_L6L 36 /* obs code: LEXL (QZS) */ #define CODE_L8I 37 /* obs code: E5(a+b)I (GAL) */ #define CODE_L8Q 38 /* obs code: E5(a+b)Q (GAL) */ #define CODE_L8X 39 /* obs code: E5(a+b)I+Q (GAL) */ #define CODE_L2I 40 /* obs code: B1I (CMP) */ #define CODE_L2Q 41 /* obs code: B1Q (CMP) */ #define CODE_L6I 42 /* obs code: B3I (CMP) */ #define CODE_L6Q 43 /* obs code: B3Q (CMP) */ #define CODE_L3I 44 /* obs code: G3I (GLO) */ #define CODE_L3Q 45 /* obs code: G3Q (GLO) */ #define CODE_L3X 46 /* obs code: G3I+Q (GLO) */ #define CODE_L1I 47 /* obs code: B1I (BDS) */ #define CODE_L1Q 48 /* obs code: B1Q (BDS) */ #define MAXCODE 48 /* max number of obs code */ #define EARTH_ECCE_2 6.69437999014e-3 // WGS 84 (Earth eccentricity)^2 (m^2) #define EARTH_MEAN_RADIUS 6371009 // Mean R of ellipsoid(m) IU Gedosey& Geophysics #define EARTH_SEMI_MAJOR 6378137 // WGS 84 Earth semi-major axis (m) #define EARTH_SEMI_MAJOR_GLO 6378136.0 // radius of earth (m) #define EARTH_OMG_GLO 7.2921150000e-5 // GLO value of earth's rotation rate (rad/s) #define EARTH_OMG_GPS 7.2921151467e-5 // GPS/GAL value of earth's rotation rate (rad/s) #define EARTH_OMG_BDS 7.2921150000e-5 // BDS value of earth's rotation rate (rad/s) #define MU_GPS 3.9860050000e14 // gravitational constant (GPS) #define MU 3.9860044180e14 // gravitational constant (GAL, BDS, GLO) #define TSTEP 60.0 // integration step glonass ephemeris (s) #define J2_GLO 1.0826257E-3 // 2nd zonal harmonic of geopot #define LIGHT_SPEED 2.99792458e8 // WGS-84 Speed of light in a vacuum (m/s) #define GPS_EPHCH_JD 2444244.5 // GPS Epoch in Julian Days #define EPH_VALID_SECONDS 7200 // 2 hours ephemeris validity #define WEEK_SECONDS 604800 // Seconds within one week #define EPSILON_KEPLER 1e-14 // Kepler equation terminate condition #define MAX_ITER_KEPLER 30 // Kepler equation maximum iteration number #define EPSILON_PVT 1e-8 // PVT terminate condition #define MAX_ITER_PVT 30 // PVT maximum iteration number #define RANGE_FREQ 1 // Range measurement frequency #define R2D (180.0/M_PI) // radius to degree #define D2R (M_PI/180.0) // degree to radius #define SC2RAD 3.1415926535898 /* semi-circle to radian (IS-GPS) */ #define SIN_N5 -0.0871557427476582 // sin(-5.0 deg) #define COS_N5 0.9961946980917456 // cos(-5.0 deg) #define INVALID_CLK 999999.999999 #define MAXLEAPS 64 const double leaps[MAXLEAPS+1][7]={ /* leap seconds (y,m,d,h,m,s,utc-gpst) */ {2017,1,1,0,0,0,-18}, {2015,7,1,0,0,0,-17}, {2012,7,1,0,0,0,-16}, {2009,1,1,0,0,0,-15}, {2006,1,1,0,0,0,-14}, {1999,1,1,0,0,0,-13}, {1997,7,1,0,0,0,-12}, {1996,1,1,0,0,0,-11}, {1994,7,1,0,0,0,-10}, {1993,7,1,0,0,0, -9}, {1992,7,1,0,0,0, -8}, {1991,1,1,0,0,0, -7}, {1990,1,1,0,0,0, -6}, {1988,1,1,0,0,0, -5}, {1985,7,1,0,0,0, -4}, {1983,7,1,0,0,0, -3}, {1982,7,1,0,0,0, -2}, {1981,7,1,0,0,0, -1}, {0} }; /* System identifier to system code */ const std::map char2sys = { {'G', SYS_GPS}, {'C', SYS_BDS}, {'R', SYS_GLO}, {'E', SYS_GAL} }; /* System map to index */ const std::map sys2idx = { {SYS_GPS, 0}, {SYS_GLO, 1}, {SYS_GAL, 2}, {SYS_BDS, 3} }; /* RINEX frequency encoding to frequency value */ const std::map type2freq = { {"G1", FREQ1}, {"G2", FREQ2}, {"G5", FREQ5}, {"R1", FREQ1_GLO}, {"R2", FREQ2_GLO}, {"R3", 1.202025E9}, {"R4", 1.600995E9}, {"R6", 1.248060E9}, {"E1", FREQ1}, {"E5", FREQ5}, {"E6", FREQ6}, {"E7", FREQ7}, {"E8", FREQ8}, {"C1", FREQ1}, {"C2", FREQ1_BDS}, {"C5", FREQ5}, {"C6", FREQ3_BDS}, {"C7", FREQ2_BDS}, {"C8", FREQ8} }; struct gtime_t { time_t time; /* time (s) expressed by standard time_t */ double sec; /* fraction of second under 1 s */ }; struct EphemBase { virtual ~EphemBase() = default; uint32_t sat; /* satellite number */ gtime_t ttr; /* transmission time in GPST */ gtime_t toe; /* ephemeris reference time in GPST */ uint32_t health; /* satellite health */ double ura; /* satellite signal accuracy */ uint32_t iode; }; typedef std::shared_ptr EphemBasePtr; struct GloEphem : EphemBase { int freqo; /* satellite frequency number */ uint32_t age; /* satellite age */ double pos[3]; /* satellite position (ecef) (m) */ double vel[3]; /* satellite velocity (ecef) (m/s) */ double acc[3]; /* satellite acceleration (ecef) (m/s^2) */ double tau_n, gamma; /* SV clock bias (s)/relative freq bias */ double delta_tau_n; /* delay between L1 and L2 (s) */ }; typedef std::shared_ptr GloEphemPtr; struct Ephem : EphemBase { gtime_t toc; /* clock correction reference time in GPST */ double toe_tow; /* toe seconds within the week */ uint32_t week; uint32_t iodc; uint32_t code; double A, e, i0, omg, OMG0, M0, delta_n, OMG_dot, i_dot; /* SV orbit parameters */ double cuc, cus, crc, crs, cic, cis; double af0, af1, af2; /* SV clock parameters */ double tgd[2]; /* group delay parameters */ /* GPS :tgd[0]=TGD */ /* GAL :tgd[0]=BGD E5a/E1,tgd[1]=BGD E5b/E1 */ /* BDS :tgd[0]=BGD1,tgd[1]=BGD2 */ double A_dot, n_dot; /* Adot,ndot for CNAV */ }; typedef std::shared_ptr EphemPtr; struct Obs /* observation data record */ { gtime_t time; /* receiver sampling time (GPST) */ uint32_t sat; /* satellite number */ std::vector freqs; /* received satellite frequencies */ std::vector CN0; /* signal strength */ std::vector LLI; /* signal lost-lock indictor */ std::vector code; /* code indicator (CODE_???) */ std::vector psr; /* observation data pseudorange (m) */ std::vector psr_std; /* pseudorange std (m) */ std::vector cp; /* observation data carrier-phase (cycle) */ std::vector cp_std; /* carrier-phase std (cycle) */ std::vector dopp; /* observation data doppler frequency (Hz) */ std::vector dopp_std; /* doppler std (Hz) */ std::vector status; /* cycle slip valid flag. bit_0 (psr valid), bit_1(cp valid), bit_2(half cp valid, *ublox), bit_3(half cp subtracted, *ublox) */ }; typedef std::shared_ptr ObsPtr; struct TimePulseInfo /* time pulse(PPS) information */ { gtime_t time; bool utc_based; uint32_t time_sys; }; typedef std::shared_ptr TimePulseInfoPtr; struct BestSat { gtime_t time; uint32_t sat; uint8_t status; std::set freq_used; /* frequency used in solution */ }; typedef std::shared_ptr BestSatPtr; struct BestPos { gtime_t time; uint32_t sol_status; uint32_t pos_type; double lat, lon, hgt; double lat_sigma, lon_sigma, hgt_sigma; double undulation; double diff_age; double sol_age; uint8_t num_svs; uint8_t num_soln_svs; uint8_t ext_sol_stat; uint8_t sig_mask; }; typedef std::shared_ptr BestPosPtr; struct BestVel { gtime_t time; uint32_t sol_status; uint32_t vel_type; double latency; double age; double hor_speed, vel_speed; double direction2north_degree; }; typedef std::shared_ptr BestVelPtr; struct BestXYZ { gtime_t time; uint32_t pos_sol_status; uint32_t pos_type; double pos[3], pos_sigma[3]; uint32_t vel_sol_status; uint32_t vel_type; double vel[3], vel_sigma[3]; double vel_latency; double diff_age; double sol_age; uint8_t num_svs; uint8_t num_soln_svs; uint8_t ext_sol_stat; uint8_t sig_mask; }; typedef std::shared_ptr BestXYZPtr; struct PVTSolution { gtime_t time; uint8_t fix_type; // 0-no fix, 1-dead reckoning only, 2-2D, 3-3D, 4-GNSS+(1), 5-time only fix bool valid_fix; bool diff_soln; // whether differential applied uint8_t carr_soln; // 0-no cp solution, 1-float, 2-fixed uint8_t num_sv; double lat, lon, hgt; double hgt_msl; // height above mean sea level double h_acc, v_acc; // horizontal and vertical accuracy double p_dop; // position DOP double vel_n, vel_e, vel_d; // velocity in NED frame double vel_acc; // velocity accuracy }; typedef std::shared_ptr PVTSolutionPtr; struct PsrPos { gtime_t time; uint32_t sol_status; uint32_t pos_type; double lat, lon, hgt; double lat_sigma, lon_sigma, hgt_sigma; double undulation; double diff_age; double sol_age; uint8_t num_svs; uint8_t num_soln_svs; uint8_t ext_sol_stat; uint8_t sig_mask; }; typedef std::shared_ptr PsrPosPtr; struct PsrVel { gtime_t time; uint32_t sol_status; uint32_t vel_type; double latency; double age; /* differential age */ double hor_spd; double trk_gnd; double vert_spd; }; typedef std::shared_ptr PsrVelPtr; struct SvInfo { gtime_t time; uint32_t sat; uint32_t freqo; bool health; double elev_degree; double az_degree; uint32_t sig_mask; }; typedef std::shared_ptr SvInfoPtr; struct TECEpoch { gtime_t time; // epoch time double rb; // earth radius (km) double lla_start[3]; double lla_end[3]; double lla_interval[3]; std::vector data; // num_hgt * num_lat * num_lon std::vector rms; // num_hgt * num_lat * num_lon }; typedef std::shared_ptr TECEpochPtr; struct EphemerideEpoch { gtime_t time; std::map sat2pos; std::map sat2clk; }; typedef std::shared_ptr EphemerideEpochPtr; struct SatState { uint32_t sat_id; gtime_t ttx; Eigen::Vector3d pos; Eigen::Vector3d vel; double dt; double ddt; double tgd; }; typedef std::shared_ptr SatStatePtr; } // namespace gnss_comm #endif ================================================ FILE: gnss_comm/include/gnss_comm/gnss_ros.hpp ================================================ /** * This file is part of gnss_comm. * * Copyright (C) 2021 Aerial Robotics Group, Hong Kong University of Science and Technology * Author: CAO Shaozu (shaozu.cao@gmail.com) * * gnss_comm is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * gnss_comm is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with gnss_comm. If not, see . */ #ifndef GNSS_ROS_HPP_ #define GNSS_ROS_HPP_ #include #include #include #include #include #include #include #include #include #include #include #include "gnss_constant.hpp" #include "gnss_utility.hpp" namespace gnss_comm { /* convert Ephem struct to ros message --------------------------------------- * args : Ephem & ephem I Ephemeris * return : cooresponding ephemeris message *-----------------------------------------------------------------------------*/ GnssEphemMsg ephem2msg(const EphemPtr &ephem_ptr); /* parse Ephem struct from ros message ---------------------------------------------- * args : GnssEphemConstPtr & gnss_ephem_msg I Ephemeris ros message * return : cooresponding ephemeris struct *-----------------------------------------------------------------------------------*/ EphemPtr msg2ephem(const GnssEphemMsgConstPtr &gnss_ephem_msg); /* convert GloEphem struct to ros message --------------------------------------- * args : GloEphem & glo_ephem I GLONASS Ephemeris * return : cooresponding ephemeris message *-----------------------------------------------------------------------------*/ GnssGloEphemMsg glo_ephem2msg(const GloEphemPtr &glo_ephem_ptr); /* parse GloEphem struct from ros message ---------------------------------------------- * args : GnssGloEphemConstPtr & gnss_glo_ephem_msg I GLONASS Ephemeris ros message * return : cooresponding GLONASS ephemeris struct *--------------------------------------------------------------------------------------*/ GloEphemPtr msg2glo_ephem(const GnssGloEphemMsgConstPtr &gnss_glo_ephem_msg); /* convert GNSS measurements to ros message ---------------------------------- * args : std::vector & meas I GNSS measurements * return : cooresponding GNSS measurement message *-----------------------------------------------------------------------------*/ GnssMeasMsg meas2msg(const std::vector &meas); /* parse GNSS measurements from ros message ---------------------------------- * args : GnssMeasConstPtr & gnss_meas_msg I GNSS measurement message * return : cooresponding GNSS measurements *-----------------------------------------------------------------------------*/ std::vector msg2meas(const GnssMeasMsgConstPtr &gnss_meas_msg); /* convert GNSS time pulse information to ros message ---------------------------------- * args : TimePulseInfoPtr tp_info I GNSS time pulse information * return : cooresponding GNSS time pulse information message *-----------------------------------------------------------------------------*/ GnssTimePulseInfoMsg tp_info2msg(const TimePulseInfoPtr &tp_info); /* parse GNSS time pulse information from ros message --------------------------------- * args : GnssTimePulseInfoMsgConstPtr gnss_tp_info_msg I GNSS time pulse message * return : cooresponding GNSS time pulse information *-----------------------------------------------------------------------------*/ TimePulseInfoPtr msg2tp_info(const GnssTimePulseInfoMsgConstPtr &gnss_tp_info_msg); /* convert GNSS best estimated xyz to ros message ---------------------------- * args : BestXYZ & best_xyz I GNSS estimated xyz in ECEF * return : cooresponding GNSS best xyz message *-----------------------------------------------------------------------------*/ GnssBestXYZMsg best_xyz2msg(const BestXYZPtr &best_xyz); /* convert GNSS PVT solution to ros message ----------------------------------- * args : PVTSolutionPtr & pvt_soln I GNSS PVT solution * return : cooresponding GNSS PVT solution message *-----------------------------------------------------------------------------*/ GnssPVTSolnMsg pvt2msg(const PVTSolutionPtr &pvt_soln); /* parse GNSS PVT solution from ros message --------------------------------- * args : GnssPVTSolnMsgConstPtr pvt_msg I GNSS PVT solution message * return : cooresponding GNSS PVT solution *-----------------------------------------------------------------------------*/ PVTSolutionPtr msg2pvt(const GnssPVTSolnMsgConstPtr &pvt_msg); /* convert GNSS space vehicles' info to ros message ---------------------------- * args : std::vector & svs I GNSS space vehicles' info * return : cooresponding GNSS space vehicle info message *-----------------------------------------------------------------------------*/ GnssSvsMsg svs2msg(const std::vector &svs); } // namespace gnss_comm #endif ================================================ FILE: gnss_comm/include/gnss_comm/gnss_spp.hpp ================================================ /** * This file is part of gnss_comm. * * Copyright (C) 2021 Aerial Robotics Group, Hong Kong University of Science and Technology * Author: CAO Shaozu (shaozu.cao@gmail.com) * * gnss_comm is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * gnss_comm is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with gnss_comm. If not, see . */ #ifndef GNSS_SPS_HPP_ #define GNSS_SPS_HPP_ #include #include "gnss_constant.hpp" namespace gnss_comm { /* filter observation, only keep L1-observed Obs ----------------------------- * args : std::vector& obs I GNSS observation data * std::vector& ephems I GNSS ephemeris data * args : std::vector& L1_obs I GNSS L1 observation data * std::vector& L1_ephems I GNSS L1 ephemeris data * return : void *-----------------------------------------------------------------------------*/ void filter_L1(const std::vector &obs, const std::vector &ephems, std::vector &L1_obs, std::vector &L1_ephems); /* calculate satellite states ----------------------------------------------- * args : std::vector& obs I GNSS observation data * std::vector& ephems I GNSS ephemeris data * return : std::vector satellite states *-----------------------------------------------------------------------------*/ std::vector sat_states(const std::vector &obs, const std::vector &ephems); /* calculate pseudo-range residual and Jacobian ------------------------------- * args : Eigen::Matrix& rcv_state I receiver state * std::vector& obs I GNSS observations * std::vector& all_sv_states I satellite states * std::vector& iono_params I ionospheric parameters * Eigen::VectorXd& res O pseudo-range residual * Eigen::MatrixXd& J O Jacobian * std::vector& atmos_delay O ion and tro delay * std::vector& all_sv_azel O satellite azimuth and elevation (radius) * return : void *-----------------------------------------------------------------------------*/ void psr_res(const Eigen::Matrix &rcv_state, const std::vector &obs, const std::vector &all_sv_states, const std::vector &iono_params, Eigen::VectorXd &res, Eigen::MatrixXd &J, std::vector &atmos_delay, std::vector &all_sv_azel); /* positioning by pseudo-range localization ---------------------------------- * args : std::vector& obs I GNSS observation data * std::vector& ephems I GNSS ephemeris data * std::vector& iono_params I ionosphere parameters * return : receiver position in ECEF and four clock bias for 4 constellations *-----------------------------------------------------------------------------*/ Eigen::Matrix psr_pos(const std::vector &obs, const std::vector &ephems, const std::vector &iono_params); /* calculate doppler residual and Jacobian ------------------------------- * args : Eigen::Matrix& rcv_state I receiver state * Eigen::Vector3d& rcv_ecef I receiver ECEF position * std::vector& obs I GNSS observations * std::vector& all_sv_states I satellite states * Eigen::VectorXd& res O pseudo-range residual * Eigen::MatrixXd& J O Jacobian * return : void *-----------------------------------------------------------------------------*/ void dopp_res(const Eigen::Matrix &rcv_state, const Eigen::Vector3d &rcv_ecef, const std::vector &obs, const std::vector &all_sv_states, Eigen::VectorXd &res, Eigen::MatrixXd &J); /* calculate velocity by using Doppler measurement ------------------------------------------------- * args : std::vector& obs I GNSS observation data * std::vector& ephems I GNSS ephemeris data * Eigen::Vector3d& ref_ecef IO reference ECEF position, (0,0,0) if unknown * return : receiver velocity in ECEF and clock bias changing rate *----------------------------------------------------------------------------------------------------*/ Eigen::Matrix dopp_vel(const std::vector &obs, const std::vector &ephems, Eigen::Vector3d &ref_ecef); } #endif ================================================ FILE: gnss_comm/include/gnss_comm/gnss_utility.hpp ================================================ /** * This file is part of gnss_comm. * * Copyright (C) 2021 Aerial Robotics Group, Hong Kong University of Science and Technology * Author: CAO Shaozu (shaozu.cao@gmail.com) * * gnss_comm is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * gnss_comm is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with gnss_comm. If not, see . * * As many of the utility functions are adapted from RTKLIB, * the license for those part of code is claimed as follows: * * The RTKLIB software package is distributed under the following BSD 2-clause * license (http://opensource.org/licenses/BSD-2-Clause) and additional two * exclusive clauses. Users are permitted to develop, produce or sell their own * non-commercial or commercial products utilizing, linking or including RTKLIB as * long as they comply with the license. * * Copyright (c) 2007-2020, T. Takasu, All rights reserved. */ #ifndef GNSS_UTILITY_HPP_ #define GNSS_UTILITY_HPP_ #include #include #include #include #include #include #include #include #include #include #include #include "gnss_constant.hpp" namespace gnss_comm { // some of the following functions are adapted from RTKLIB /* satellite system+prn/slot number to satellite number ------------------------ * convert satellite system+prn/slot number to satellite number * args : uint32_t sys I satellite system (SYS_GPS,SYS_GLO,...) * uint32_t prn I satellite prn/slot number * return : satellite number (0:error) *-----------------------------------------------------------------------------*/ uint32_t sat_no(uint32_t sys, uint32_t prn); /* satellite number to satellite system ---------------------------------------- * convert satellite number to satellite system * args : uint32_t sat I satellite number (1-MAXSAT) * uint32_t *prn IO satellite prn/slot number (NULL: no output) * return : satellite system (SYS_GPS,SYS_GLO,...) *-----------------------------------------------------------------------------*/ uint32_t satsys(uint32_t sat, uint32_t *prn); /* convert calendar day/time to time ------------------------------------------- * convert calendar day/time to gtime_t struct * args : double *ep I day/time {year,month,day,hour,min,sec} * return : gtime_t struct * notes : proper in 1970-2037 or 1970-2099 (64bit time_t) *-----------------------------------------------------------------------------*/ gtime_t epoch2time(const double *ep); /* time to calendar day/time --------------------------------------------------- * convert gtime_t struct to calendar day/time * args : gtime_t t I gtime_t struct * double *ep O day/time {year,month,day,hour,min,sec} * return : none * notes : proper in 1970-2037 or 1970-2099 (64bit time_t) *-----------------------------------------------------------------------------*/ void time2epoch(gtime_t t, double *ep); /* gps time to time ------------------------------------------------------------ * convert week and tow in gps time to gtime_t struct * args : uint32_t week I week number in gps time * double tow I time of week in gps time (s) * return : gtime_t struct *-----------------------------------------------------------------------------*/ gtime_t gpst2time(uint32_t week, double tow); /* time to gps time ------------------------------------------------------------ * convert gtime_t struct to week and tow in gps time * args : gtime_t t I gtime_t struct * uint32_t *week IO week number in gps time (NULL: no output) * return : time of week in gps time (s) *-----------------------------------------------------------------------------*/ double time2gpst(gtime_t t, uint32_t *week); /* galileo system time to time ------------------------------------------------- * convert week and tow in galileo system time (gst) to gtime_t struct * args : int week I week number in gst * double tow I time of week in gst (s) * return : gtime_t struct *-----------------------------------------------------------------------------*/ gtime_t gst2time(int week, double tow); /* time to galileo system time ------------------------------------------------- * convert gtime_t struct to week and tow in galileo system time (gst) * args : gtime_t t I gtime_t struct * int *week IO week number in gst (NULL: no output) * return : time of week in gst (s) *-----------------------------------------------------------------------------*/ double time2gst(gtime_t t, int *week); /* beidou time (bdt) to time --------------------------------------------------- * convert week and tow in beidou time (bdt) to gtime_t struct * args : int week I week number in bdt * double tow I time of week in bdt (s) * return : gtime_t struct *-----------------------------------------------------------------------------*/ gtime_t bdt2time(int week, double tow); /* time to beidouo time (bdt) -------------------------------------------------- * convert gtime_t struct to week and tow in beidou time (bdt) * args : gtime_t t I gtime_t struct * int *week IO week number in bdt (NULL: no output) * return : time of week in bdt (s) *-----------------------------------------------------------------------------*/ double time2bdt(gtime_t t, int *week); /* gpstime to utc -------------------------------------------------------------- * convert gpstime to utc considering leap seconds * args : gtime_t t I time expressed in gpstime * return : time expressed in utc * notes : ignore slight time offset under 100 ns *-----------------------------------------------------------------------------*/ gtime_t gpst2utc(gtime_t t); /* utc to gpstime -------------------------------------------------------------- * convert utc to gpstime considering leap seconds * args : gtime_t t I time expressed in utc * return : time expressed in gpstime * notes : ignore slight time offset under 100 ns *-----------------------------------------------------------------------------*/ gtime_t utc2gpst(gtime_t t); /* datetime to Julian day ----------------------------------------------------- * convert datetime to Julian day * args : std::vector datetime I datetime (year, month, day, hour, minute, second) * return : Julian day corresponding to the datetime * notes : vaild range: 1900 ~ 2100 *-----------------------------------------------------------------------------*/ double julian_day(std::vector datetime); /* find the number of leap seconds at the given datetime since GPS start epoch * args : std::vector datetime I datetime (year, month, day, hour, minute, second) * return : leap seconds at the given datetime *-----------------------------------------------------------------------------*/ uint32_t leap_seconds_from_GPS_epoch(std::vector datetime); /* time to day of year --------------------------------------------------------- * convert time to day of year * args : gtime_t t I gtime_t struct * return : day of year (days) *-----------------------------------------------------------------------------*/ double time2doy(gtime_t time); /* time difference between two timestamps ------------------------------------- * args : gtime_t t1 I time 1 * gtime_t t2 I time 2 * return : time difference between t1 and t2 in seconds, e.g. (t1 - t2) *-----------------------------------------------------------------------------*/ double time_diff(gtime_t t1, gtime_t t2); /* add specific seconds to a timestamp ------------------------------------- * args : gtime_t t I base timestamp * double sec I duration to add * return : timestamp which equals to `sec` seconds past t *-----------------------------------------------------------------------------*/ gtime_t time_add(gtime_t t, double sec); /* time to timestamp in seconds ----------------------------------------------- * convert gtime_t struct to timestamp in seconds * args : gtime_t t I gtime_t struct * return : timestamp (s) *-----------------------------------------------------------------------------*/ double time2sec(gtime_t time); /* timestamp to time in seconds ----------------------------------------------- * convert timestamp to gtime_t struct in seconds * args : timestamp sec I timestamp in seconds * return : gtime_t struct *-----------------------------------------------------------------------------*/ gtime_t sec2time(const double sec); /* Geodetic coordinate to ECEF coordinate ------------------------------------- * convert a point in geodetic coordinate to ECEF coordinate * args : lla I Eigen::Vector3d Latitude, Longitude, Altitude (degree) * return : Corresponding ECEF coordinate *-----------------------------------------------------------------------------*/ Eigen::Vector3d geo2ecef(const Eigen::Vector3d &lla); /* ECEF coordinate to Geodetic coordinate ---------------------------------------------- * convert a point in ECEF coordinate to geodetic coordinate * args : xyz I Eigen::Vector3d * return : Corresponding geodetic coordinate, Longitude, Latitude, Altitude in degree *-------------------------------------------------------------------------------------*/ Eigen::Vector3d ecef2geo(const Eigen::Vector3d &xyz); /* solve Kepler equation ------------------------------------------------------*/ double Kepler(const double mk, const double es); /* calculate satellite clock drift ------------------------------------------- * calculate satellite position in ECEF (GPS, GAL, BDS) * args : gtime_t curr_time I current time * Ephem ephem I satellite ephemeris (GPS, BDS, GAL) * return : satellite clock drift in seconds *-----------------------------------------------------------------------------*/ double eph2svdt(const gtime_t &curr_time, const EphemPtr ephem_ptr); /* calculate satellite position in ECEF ---------------------------------------- * calculate satellite position in ECEF (GPS, GAL, BDS) * args : gtime_t curr_time I current time * Ephem ephem I satellite ephemeris (GPS, BDS, GAL) * double* svdt IO satellite clock drift (NULL: no output) * return : satellite position in ECEF frame *-----------------------------------------------------------------------------*/ Eigen::Vector3d eph2pos(const gtime_t &curr_time, const EphemPtr ephem_ptr, double *svdt); /* calculate satellite velocity in ECEF ---------------------------------------- * calculate satellite velocity in ECEF (GPS, GAL, BDS) * args : gtime_t curr_time I current time * Ephem ephem I satellite ephemeris (GPS, BDS, GAL) * double* svddt IO satellite clock drift rate (NULL: no output) * return : satellite velocity in ECEF frame *-----------------------------------------------------------------------------*/ Eigen::Vector3d eph2vel(const gtime_t &curr_time, const EphemPtr ephem, double *svddt); /* glonass orbit differential equations --------------------------------------*/ void deq(const Eigen::Vector3d &pos, const Eigen::Vector3d &vel, const Eigen::Vector3d &acc, Eigen::Vector3d &pos_dot, Eigen::Vector3d &vel_dot); /* glonass position and velocity by numerical integration (RK4) --------------------*/ void glo_orbit(double dt, Eigen::Vector3d &pos, Eigen::Vector3d &vel, const Eigen::Vector3d &acc); /* glonass ephemeris to satellite clock bias ----------------------------------- * compute satellite clock bias with glonass ephemeris * args : gtime_t curr_time I time by satellite clock (gpst) * geph_t geph I glonass ephemeris * return : satellite clock bias (s) *-----------------------------------------------------------------------------*/ double geph2svdt(const gtime_t &curr_time, const GloEphemPtr geph_ptr); /* glonass ephemeris to satellite position and clock bias ---------------------- * compute satellite position and clock bias with glonass ephemeris * args : gtime_t curr_time I time (gpst) * geph_t geph I glonass ephemeris * double *svdt IO output satellite clock bias in second (NULL: no output) * return : satellite position in ECEF Eigen::Vector3d *-----------------------------------------------------------------------------*/ Eigen::Vector3d geph2pos(const gtime_t &curr_time, const GloEphemPtr geph_ptr, double *svdt); /* glonass ephemeris to satellite position and clock bias ---------------------- * compute satellite velocity and clock bias change rate with glonass ephemeris * args : gtime_t curr_time I time (gpst) * geph_t geph I glonass ephemeris * double *svddt IO output satellite clock bias change rate(s/s) (NULL: no output) * return : satellite velocity in ECEF Eigen::Vector3d *-----------------------------------------------------------------------------*/ Eigen::Vector3d geph2vel(const gtime_t &curr_time, const GloEphemPtr geph_ptr, double *svddt); /* satellite azimuth/elevation angle ------------------------------------------- * compute satellite azimuth/elevation angle * args : Eigen::Vector3d rev_pos I receiver position in ECEF * Eigen::Vector3d sat_pos I satellilte position in ECEF * double* azel O azimuth/elevation {az,el} (rad) (NULL: no output) * (0.0<=azel[0]<2*pi,-pi/2<=azel[1]<=pi/2) *-----------------------------------------------------------------------------*/ void sat_azel(const Eigen::Vector3d &rev_pos, const Eigen::Vector3d &sat_pos, double *azel); /* transform ecef vector to local tangental coordinate ------------------------- * transform ecef vector to local tangental coordinate * args : Eigen::Vector3d pos_lla I geodetic position {lat,lon} (degree) * Eigen::Vector3d v_ecef I vector in ecef coordinate {x,y,z} * return : vector in local tangental coordinate {e,n,u} *-----------------------------------------------------------------------------*/ Eigen::Vector3d ecef2enu(const Eigen::Vector3d &pos_lla, const Eigen::Vector3d &v_ecef); /* calculate rotation from ENU frame to ECEF via reference geodetic coordinate --- * args : Eigen::Vector3d ref_geo I reference position in geodetic frame * return : rotation matrix from ENU frame to ECEF frame *-----------------------------------------------------------------------------*/ Eigen::Matrix3d geo2rotation(const Eigen::Vector3d &ref_geo); /* calculate rotation from ENU frame to ECEF via reference ECEF coordinate --- * args : Eigen::Vector3d ref_ecef I reference position in ECEF * return : rotation matrix from ENU frame to ECEF frame *-----------------------------------------------------------------------------*/ Eigen::Matrix3d ecef2rotation(const Eigen::Vector3d &ref_ecef); /* calculate signal delay caused by troposphere ------------------------------ * calculate signal delay caused by troposphere * args : gtime_t time I time (gpst) * args : Eigen::Vector3d rev_lla I receiver position in geodetic coordinate * double* azel I satellite azimuth/elevation angle in radius * return : delay expressed by light travel distance (m) *-----------------------------------------------------------------------------*/ double calculate_trop_delay(gtime_t time, const Eigen::Vector3d &rev_lla, const double *azel); /* calculate signal delay caused by ionosphere ------------------------------ * calculate signal delay caused by ionosphere * args : gtime_t time I time (gpst) * std::vector ion_parameters I ionosphere parameters {a0,a1,a2,a3,b0,b1,b2,b3} * Eigen::Vector3d rev_lla I receiver position in LLA (lat, lon, hgt) (deg, m) * double* azel I satellite azimuth/elevation angle (rad) * return : ionospheric delay (L1) (m) *-----------------------------------------------------------------------------*/ double calculate_ion_delay(gtime_t t, const std::vector &ion_parameters, const Eigen::Vector3d &rev_lla, const double *azel); /* satellite RINEX identifier from RTKLIB satellite id ------------------------------ * args : uint32_t sat_no I satellite id in RTKLIB * return : satellite RINEX identifier *-----------------------------------------------------------------------------*/ std::string sat2str(uint32_t sat_no); /* satellite RINEX identifier to RTKLIB satellite id ------------------------------ * args : std::string &sat_no I satellite identifier in RINEX * return : satellite id in RTKLIB *-----------------------------------------------------------------------------*/ uint32_t str2sat(const std::string &sat_str); /* Get L1 frequency --------------------------------------------------------- * args : ObsPtr& obs I GNSS observation * int* l1_idx I/O L1 frequency index (NULL if not required) * return : L1 frequency (negative if L1 frequency not exists) *-----------------------------------------------------------------------------*/ double L1_freq(const ObsPtr &obs, int *l1_idx); /* execute the given command --------------------------------------------------------- * args : std::string cmd I command to execute * return : execute result *-------------------------------------------------------------------------------------*/ std::string exec(const std::string &cmd); } // namespace gnss_comm #endif ================================================ FILE: gnss_comm/include/gnss_comm/rinex_helper.hpp ================================================ /** * This file is part of gnss_comm. * * Copyright (C) 2021 Aerial Robotics Group, Hong Kong University of Science and Technology * Author: CAO Shaozu (shaozu.cao@gmail.com) * * gnss_comm is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * gnss_comm is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with gnss_comm. If not, see . */ #ifndef RINEX_HELPER_HPP_ #define RINEX_HELPER_HPP_ #include #include #include #include #include #include #include #include "gnss_constant.hpp" #include "gnss_utility.hpp" namespace gnss_comm { /* parse ephemeris from RINEX navigation file ---------------------------------------------------------- * args : std::string rinex_filepath I RINEX file path * std::map>& sat2ephem IO satellite number to ephemeris * return : None *---------------------------------------------------------------------------------------------*/ void rinex2ephems(const std::string &rinex_filepath, std::map> &sat2ephem); /* parse GNSS measurement from RINEX observation file --------------------------------------- * args : std::string rinex_filepath I RINEX file path * std::vector>& rinex_meas IO GNSS measurement in time order * return : None *---------------------------------------------------------------------------------------------*/ void rinex2obs(const std::string &rinex_filepath, std::vector> &rinex_meas); /* output GNSS measurement to RINEX file ----------------------------------------------------- * args : std::string rinex_filepath I RINEX file path * std::vector>& rinex_meas I GNSS measurement in time order * return : None * This function is still under experiment, many hacks here. *---------------------------------------------------------------------------------------------*/ void obs2rinex(const std::string &rinex_filepath, const std::vector> &gnss_meas); /* parse GPS ionosphere from RINEX navigation file ------------------------------------------ * args : std::string rinex_filepath I RINEX file path * std::vector iono_params IO 8 ionosphere parameters (alpha 1~4, beta 1~4) * return : None *---------------------------------------------------------------------------------------------*/ void rinex2iono_params(const std::string &rinex_filepath, std::vector &iono_params); } // namespace gnss_comm #endif ================================================ FILE: gnss_comm/msg/GnssBestXYZMsg.msg ================================================ Header header float64[] pos float64[] pos_sigma float64[] vel float64[] vel_sigma uint32 num_svs ================================================ FILE: gnss_comm/msg/GnssEphemMsg.msg ================================================ # This message contains GPS, Galileo and BeiDou ephemeris data uint32 sat GnssTimeMsg ttr GnssTimeMsg toe GnssTimeMsg toc float64 toe_tow uint32 week uint32 iode uint32 iodc uint32 health uint32 code float64 ura float64 A float64 e float64 i0 float64 omg float64 OMG0 float64 M0 float64 delta_n float64 OMG_dot float64 i_dot float64 cuc float64 cus float64 crc float64 crs float64 cic float64 cis float64 af0 float64 af1 float64 af2 float64 tgd0 float64 tgd1 float64 A_dot float64 n_dot ================================================ FILE: gnss_comm/msg/GnssGloEphemMsg.msg ================================================ # This message contains GLONASS ephemeris data uint32 sat GnssTimeMsg ttr GnssTimeMsg toe int32 freqo uint32 iode uint32 health uint32 age float64 ura float64 pos_x float64 pos_y float64 pos_z float64 vel_x float64 vel_y float64 vel_z float64 acc_x float64 acc_y float64 acc_z float64 tau_n float64 gamma float64 delta_tau_n ================================================ FILE: gnss_comm/msg/GnssMeasMsg.msg ================================================ # This message contains one-epoch measurements from multiple satellites GnssObsMsg[] meas ================================================ FILE: gnss_comm/msg/GnssObsMsg.msg ================================================ # This message contains one-epoch measurements from one single satellite GnssTimeMsg time # measurement time uint32 sat # satellite ID (define by `sat_no` function in `gnss_utility.hpp`) float64[] freqs # observed frequencies [Hz] float64[] CN0 # carrier-to-noise density ratio (signal strength) [dB-Hz] uint8[] LLI # lost-lock indicator (1=lost) uint8[] code # channel code float64[] psr # pseudorange measurement [m] float64[] psr_std # pseudorange standard deviation [m] float64[] cp # carrier phase measurement [cycle] float64[] cp_std # carrier phase standard deviation [cycle] float64[] dopp # Doppler measurement [Hz] float64[] dopp_std # Doppler standard deviation [Hz] # tracking status. bit_0:psr valid, bit_1:cp valid, # bit_2:half cp valid, bit_3:half cp subtracted uint8[] status ================================================ FILE: gnss_comm/msg/GnssPVTSolnMsg.msg ================================================ # This message contains information of UBX-NAV-PVT message. # reference: [1]. UBX-18010854-R08, page 132 GnssTimeMsg time # GNSS time of the navigation epoch # GNSS fix type (0=no fix, 1=dead reckoning only, 2=2D-fix, 3=3D-fix, # 4=GNSS+dead reckoning combined, 5=time only fix) uint8 fix_type bool valid_fix # if fix valid (1=valid fix) bool diff_soln # if differential correction were applied (1=applied) uint8 carr_soln # carrier phase range solution status (0=no carrier phase, 1=float, 2=fix) uint8 num_sv # number of satellites used in the solution float64 latitude # latitude [degree] float64 longitude # longitude [degree] float64 altitude # height above ellipsoid [m] float64 height_msl # height above mean sea level [m] float64 h_acc # horizontal accuracy estimate [m] float64 v_acc # vertical accuracy estimate [m] float64 p_dop # Position DOP float64 vel_n # NED north velocity [m/s] float64 vel_e # NED east velocity [m/s] float64 vel_d # NED down velocity [m/s] float64 vel_acc # speed accuracy estimate [m/s] ================================================ FILE: gnss_comm/msg/GnssSvsMsg.msg ================================================ Header header uint32[] sat uint32[] freqo bool[] health float64[] elev_degree float64[] az_degree uint32[] sig_mask ================================================ FILE: gnss_comm/msg/GnssTimeMsg.msg ================================================ # This message contains GNSS time expressed in the form of # GNSS week number and time of week(in seconds) uint32 week float64 tow ================================================ FILE: gnss_comm/msg/GnssTimePulseInfoMsg.msg ================================================ # This message contains information of UBX-TIM-TP message. # reference: [1]. UBX-18010854-R08, page 156 GnssTimeMsg time # GNSS time of the next time pulse bool utc_based # if the time is UTC (1=UTC, 0=GNSS) uint32 time_sys # GNSS time reference system, if the time base is GNSS (utc_base=0) ================================================ FILE: gnss_comm/msg/StampedFloat64Array.msg ================================================ # A list of float values with timestamp Header header float64[] data ================================================ FILE: gnss_comm/package.xml ================================================ gnss_comm 1.0.0 Definitions and utility functions for GNSS CAO Shaozu GPLv3 catkin roscpp rospy std_msgs message_generation roscpp rospy std_msgs message_runtime message_generation ================================================ FILE: gnss_comm/src/gnss_ros.cpp ================================================ /** * This file is part of gnss_comm. * * Copyright (C) 2021 Aerial Robotics Group, Hong Kong University of Science and Technology * Author: CAO Shaozu (shaozu.cao@gmail.com) * * gnss_comm is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * gnss_comm is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with gnss_comm. If not, see . */ #include namespace gnss_comm { GnssEphemMsg ephem2msg(const EphemPtr &ephem_ptr) { GnssEphemMsg ephem_msg; uint32_t week = 0; double tow = 0.0; ephem_msg.sat = ephem_ptr->sat; tow = time2gpst(ephem_ptr->ttr, &week); ephem_msg.ttr.week = week; ephem_msg.ttr.tow = tow; tow = time2gpst(ephem_ptr->toe, &week); ephem_msg.toe.week = week; ephem_msg.toe.tow = tow; tow = time2gpst(ephem_ptr->toc, &week); ephem_msg.toc.week = week; ephem_msg.toc.tow = tow; ephem_msg.toe_tow = ephem_ptr->toe_tow; ephem_msg.week = ephem_ptr->week; ephem_msg.iode = ephem_ptr->iode; ephem_msg.iodc = ephem_ptr->iodc; ephem_msg.health = ephem_ptr->health; ephem_msg.code = ephem_ptr->code; ephem_msg.ura = ephem_ptr->ura; ephem_msg.A = ephem_ptr->A; ephem_msg.e = ephem_ptr->e; ephem_msg.i0 = ephem_ptr->i0; ephem_msg.omg = ephem_ptr->omg; ephem_msg.OMG0 = ephem_ptr->OMG0; ephem_msg.M0 = ephem_ptr->M0; ephem_msg.delta_n = ephem_ptr->delta_n; ephem_msg.OMG_dot = ephem_ptr->OMG_dot; ephem_msg.i_dot = ephem_ptr->i_dot; ephem_msg.cuc = ephem_ptr->cuc; ephem_msg.cus = ephem_ptr->cus; ephem_msg.crc = ephem_ptr->crc; ephem_msg.crs = ephem_ptr->crs; ephem_msg.cic = ephem_ptr->cic; ephem_msg.cis = ephem_ptr->cis; ephem_msg.af0 = ephem_ptr->af0; ephem_msg.af1 = ephem_ptr->af1; ephem_msg.af2 = ephem_ptr->af2; ephem_msg.tgd0 = ephem_ptr->tgd[0]; ephem_msg.tgd1 = ephem_ptr->tgd[1]; ephem_msg.A_dot = ephem_ptr->A_dot; ephem_msg.n_dot = ephem_ptr->n_dot; return ephem_msg; } EphemPtr msg2ephem(const GnssEphemMsgConstPtr &gnss_ephem_msg) { EphemPtr ephem(new Ephem()); ephem->sat = gnss_ephem_msg->sat; ephem->ttr = gpst2time(gnss_ephem_msg->ttr.week, gnss_ephem_msg->ttr.tow); ephem->toe = gpst2time(gnss_ephem_msg->toe.week, gnss_ephem_msg->toe.tow); ephem->toc = gpst2time(gnss_ephem_msg->toc.week, gnss_ephem_msg->toc.tow); ephem->toe_tow = gnss_ephem_msg->toe_tow; ephem->week = gnss_ephem_msg->week; ephem->iode = gnss_ephem_msg->iode; ephem->iodc = gnss_ephem_msg->iodc; ephem->health = gnss_ephem_msg->health; ephem->code = gnss_ephem_msg->code; ephem->ura = gnss_ephem_msg->ura; ephem->A = gnss_ephem_msg->A; ephem->e = gnss_ephem_msg->e; ephem->i0 = gnss_ephem_msg->i0; ephem->omg = gnss_ephem_msg->omg; ephem->OMG0 = gnss_ephem_msg->OMG0; ephem->M0 = gnss_ephem_msg->M0; ephem->delta_n = gnss_ephem_msg->delta_n; ephem->OMG_dot = gnss_ephem_msg->OMG_dot; ephem->i_dot = gnss_ephem_msg->i_dot; ephem->cuc = gnss_ephem_msg->cuc; ephem->cus = gnss_ephem_msg->cus; ephem->crc = gnss_ephem_msg->crc; ephem->crs = gnss_ephem_msg->crs; ephem->cic = gnss_ephem_msg->cic; ephem->cis = gnss_ephem_msg->cis; ephem->af0 = gnss_ephem_msg->af0; ephem->af1 = gnss_ephem_msg->af1; ephem->af2 = gnss_ephem_msg->af2; ephem->tgd[0] = gnss_ephem_msg->tgd0; ephem->tgd[1] = gnss_ephem_msg->tgd1; ephem->A_dot = gnss_ephem_msg->A_dot; ephem->n_dot = gnss_ephem_msg->n_dot; return ephem; } GnssGloEphemMsg glo_ephem2msg(const GloEphemPtr &glo_ephem_ptr) { GnssGloEphemMsg glo_ephem_msg; uint32_t week = 0; double tow = 0.0; glo_ephem_msg.sat = glo_ephem_ptr->sat; tow = time2gpst(glo_ephem_ptr->ttr, &week); glo_ephem_msg.ttr.week = week; glo_ephem_msg.ttr.tow = tow; tow = time2gpst(glo_ephem_ptr->toe, &week); glo_ephem_msg.toe.week = week; glo_ephem_msg.toe.tow = tow; glo_ephem_msg.freqo = glo_ephem_ptr->freqo; glo_ephem_msg.iode = glo_ephem_ptr->iode; glo_ephem_msg.health = glo_ephem_ptr->health; glo_ephem_msg.age = glo_ephem_ptr->age; glo_ephem_msg.ura = glo_ephem_ptr->ura; glo_ephem_msg.pos_x = glo_ephem_ptr->pos[0]; glo_ephem_msg.pos_y = glo_ephem_ptr->pos[1]; glo_ephem_msg.pos_z = glo_ephem_ptr->pos[2]; glo_ephem_msg.vel_x = glo_ephem_ptr->vel[0]; glo_ephem_msg.vel_y = glo_ephem_ptr->vel[1]; glo_ephem_msg.vel_z = glo_ephem_ptr->vel[2]; glo_ephem_msg.acc_x = glo_ephem_ptr->acc[0]; glo_ephem_msg.acc_y = glo_ephem_ptr->acc[1]; glo_ephem_msg.acc_z = glo_ephem_ptr->acc[2]; glo_ephem_msg.tau_n = glo_ephem_ptr->tau_n; glo_ephem_msg.gamma = glo_ephem_ptr->gamma; glo_ephem_msg.delta_tau_n = glo_ephem_ptr->delta_tau_n; return glo_ephem_msg; } GloEphemPtr msg2glo_ephem(const GnssGloEphemMsgConstPtr &gnss_glo_ephem_msg) { GloEphemPtr glo_ephem(new GloEphem()); glo_ephem->sat = gnss_glo_ephem_msg->sat; glo_ephem->ttr = gpst2time(gnss_glo_ephem_msg->ttr.week, gnss_glo_ephem_msg->ttr.tow); glo_ephem->toe = gpst2time(gnss_glo_ephem_msg->toe.week, gnss_glo_ephem_msg->toe.tow); glo_ephem->freqo = gnss_glo_ephem_msg->freqo; glo_ephem->iode = gnss_glo_ephem_msg->iode; glo_ephem->health = gnss_glo_ephem_msg->health; glo_ephem->age = gnss_glo_ephem_msg->age; glo_ephem->ura = gnss_glo_ephem_msg->ura; glo_ephem->pos[0] = gnss_glo_ephem_msg->pos_x; glo_ephem->pos[1] = gnss_glo_ephem_msg->pos_y; glo_ephem->pos[2] = gnss_glo_ephem_msg->pos_z; glo_ephem->vel[0] = gnss_glo_ephem_msg->vel_x; glo_ephem->vel[1] = gnss_glo_ephem_msg->vel_y; glo_ephem->vel[2] = gnss_glo_ephem_msg->vel_z; glo_ephem->acc[0] = gnss_glo_ephem_msg->acc_x; glo_ephem->acc[1] = gnss_glo_ephem_msg->acc_y; glo_ephem->acc[2] = gnss_glo_ephem_msg->acc_z; glo_ephem->tau_n = gnss_glo_ephem_msg->tau_n; glo_ephem->gamma = gnss_glo_ephem_msg->gamma; glo_ephem->delta_tau_n = gnss_glo_ephem_msg->delta_tau_n; return glo_ephem; } GnssMeasMsg meas2msg(const std::vector &meas) { GnssMeasMsg gnss_meas_msg; for (ObsPtr obs : meas) { GnssObsMsg obs_msg; uint32_t week = 0; double tow = time2gpst(obs->time, &week); obs_msg.time.week = week; obs_msg.time.tow = tow; obs_msg.sat = obs->sat; obs_msg.freqs = obs->freqs; obs_msg.CN0 = obs->CN0; obs_msg.LLI = obs->LLI; obs_msg.code = obs->code; obs_msg.psr = obs->psr; obs_msg.psr_std = obs->psr_std; obs_msg.cp = obs->cp; obs_msg.cp_std = obs->cp_std; obs_msg.dopp = obs->dopp; obs_msg.dopp_std = obs->dopp_std; obs_msg.status = obs->status; gnss_meas_msg.meas.push_back(obs_msg); } return gnss_meas_msg; } std::vector msg2meas(const GnssMeasMsgConstPtr &gnss_meas_msg) { std::vector meas; for (size_t i = 0; i < gnss_meas_msg->meas.size(); ++i) { GnssObsMsg obs_msg = gnss_meas_msg->meas[i]; ObsPtr obs(new Obs()); obs->time = gpst2time(obs_msg.time.week, obs_msg.time.tow); obs->sat = obs_msg.sat; obs->freqs = obs_msg.freqs; obs->CN0 = obs_msg.CN0; obs->LLI = obs_msg.LLI; obs->code = obs_msg.code; obs->psr = obs_msg.psr; obs->psr_std = obs_msg.psr_std; obs->cp = obs_msg.cp; obs->cp_std = obs_msg.cp_std; obs->dopp = obs_msg.dopp; obs->dopp_std = obs_msg.dopp_std; obs->status = obs_msg.status; meas.push_back(obs); } return meas; } GnssTimePulseInfoMsg tp_info2msg(const TimePulseInfoPtr &tp_info) { GnssTimePulseInfoMsg tp_info_msg; tp_info_msg.time.tow = time2gpst(tp_info->time, &(tp_info_msg.time.week)); tp_info_msg.utc_based = tp_info->utc_based; tp_info_msg.time_sys = tp_info->time_sys; return tp_info_msg; } TimePulseInfoPtr msg2tp_info(const GnssTimePulseInfoMsgConstPtr &gnss_tp_info_msg) { TimePulseInfoPtr tp_info(new TimePulseInfo()); tp_info->time = gpst2time(gnss_tp_info_msg->time.week, gnss_tp_info_msg->time.tow); tp_info->utc_based = gnss_tp_info_msg->utc_based; tp_info->time_sys = gnss_tp_info_msg->time_sys; return tp_info; } GnssBestXYZMsg best_xyz2msg(const BestXYZPtr &best_xyz) { GnssBestXYZMsg gnss_best_xyz_msg; gnss_best_xyz_msg.header.stamp = ros::Time(time2sec(best_xyz->time)); gnss_best_xyz_msg.header.frame_id = "ECEF"; for (size_t i = 0; i < 3; ++i) { gnss_best_xyz_msg.pos.push_back(best_xyz->pos[i]); gnss_best_xyz_msg.pos_sigma.push_back(best_xyz->pos_sigma[i]); gnss_best_xyz_msg.vel.push_back(best_xyz->vel[i]); gnss_best_xyz_msg.vel_sigma.push_back(best_xyz->vel_sigma[i]); } gnss_best_xyz_msg.num_svs = best_xyz->num_soln_svs; return gnss_best_xyz_msg; } GnssPVTSolnMsg pvt2msg(const PVTSolutionPtr &pvt_soln) { GnssPVTSolnMsg pvt_msg; pvt_msg.time.tow = time2gpst(pvt_soln->time, &(pvt_msg.time.week)); pvt_msg.fix_type = pvt_soln->fix_type; pvt_msg.valid_fix = pvt_soln->valid_fix; pvt_msg.diff_soln = pvt_soln->diff_soln; pvt_msg.carr_soln = pvt_soln->carr_soln; pvt_msg.num_sv = pvt_soln->num_sv; pvt_msg.latitude = pvt_soln->lat; pvt_msg.longitude = pvt_soln->lon; pvt_msg.altitude = pvt_soln->hgt; pvt_msg.height_msl = pvt_soln->hgt_msl; pvt_msg.h_acc = pvt_soln->h_acc; pvt_msg.v_acc = pvt_soln->v_acc; pvt_msg.p_dop = pvt_soln->p_dop; pvt_msg.vel_n = pvt_soln->vel_n; pvt_msg.vel_e = pvt_soln->vel_e; pvt_msg.vel_d = pvt_soln->vel_d; pvt_msg.vel_acc = pvt_soln->vel_acc; return pvt_msg; } PVTSolutionPtr msg2pvt(const GnssPVTSolnMsgConstPtr &pvt_msg) { PVTSolutionPtr pvt_soln(new PVTSolution()); pvt_soln->time = gpst2time(pvt_msg->time.week, pvt_msg->time.tow); pvt_soln->fix_type = pvt_msg->fix_type; pvt_soln->valid_fix = pvt_msg->valid_fix; pvt_soln->diff_soln = pvt_msg->diff_soln; pvt_soln->carr_soln = pvt_msg->carr_soln; pvt_soln->num_sv = pvt_msg->num_sv; pvt_soln->lat = pvt_msg->latitude; pvt_soln->lon = pvt_msg->longitude; pvt_soln->hgt = pvt_msg->altitude; pvt_soln->hgt_msl = pvt_msg->height_msl; pvt_soln->h_acc = pvt_msg->h_acc; pvt_soln->v_acc = pvt_msg->v_acc; pvt_soln->p_dop = pvt_msg->p_dop; pvt_soln->vel_n = pvt_msg->vel_n; pvt_soln->vel_e = pvt_msg->vel_e; pvt_soln->vel_d = pvt_msg->vel_d; pvt_soln->vel_acc = pvt_msg->vel_acc; return pvt_soln; } GnssSvsMsg svs2msg(const std::vector &svs) { GnssSvsMsg svs_msg; if (svs.empty()) return svs_msg; svs_msg.header.stamp = ros::Time(time2sec(svs[0].time)); for (auto & sv : svs) { svs_msg.sat.push_back(sv.sat); svs_msg.freqo.push_back(sv.freqo); svs_msg.health.push_back(sv.health); svs_msg.elev_degree.push_back(sv.elev_degree); svs_msg.az_degree.push_back(sv.az_degree); svs_msg.sig_mask.push_back(sv.sig_mask); } return svs_msg; } } // namespace gnss_comm ================================================ FILE: gnss_comm/src/gnss_spp.cpp ================================================ /** * This file is part of gnss_comm. * * Copyright (C) 2021 Aerial Robotics Group, Hong Kong University of Science and Technology * Author: CAO Shaozu (shaozu.cao@gmail.com) * * gnss_comm is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * gnss_comm is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with gnss_comm. If not, see . */ #include #include #include #define CUT_OFF_DEGREE 15.0 namespace gnss_comm { void filter_L1(const std::vector &obs, const std::vector &ephems, std::vector &L1_obs, std::vector &L1_ephems) { L1_obs.clear(); L1_ephems.clear(); for (size_t i = 0; i < obs.size(); ++i) { const ObsPtr &this_obs = obs[i]; // check system const uint32_t sys = satsys(this_obs->sat, NULL); if (sys != SYS_GPS && sys != SYS_GLO && sys != SYS_BDS && sys != SYS_GAL) continue; // check signal frequency const double obs_freq = L1_freq(this_obs, NULL); if (obs_freq < 0) continue; L1_obs.push_back(this_obs); L1_ephems.push_back(ephems[i]); } } std::vector sat_states(const std::vector &obs, const std::vector &ephems) { std::vector all_sv_states; const uint32_t num_obs = obs.size(); for (size_t i = 0; i < num_obs; ++i) { SatStatePtr sat_state(new SatState()); all_sv_states.push_back(sat_state); ObsPtr this_obs = obs[i]; const uint32_t sat = this_obs->sat; const uint32_t sys = satsys(sat, NULL); int l1_idx = -1; L1_freq(this_obs, &l1_idx); if (l1_idx < 0) continue; double tof = this_obs->psr[l1_idx] / LIGHT_SPEED; gtime_t sv_tx = time_add(this_obs->time, -tof); double svdt = 0, svddt = 0; Eigen::Vector3d sv_pos = Eigen::Vector3d::Zero(); Eigen::Vector3d sv_vel = Eigen::Vector3d::Zero(); if (sys == SYS_GLO) { GloEphemPtr glo_ephem = std::dynamic_pointer_cast(ephems[i]); svdt = geph2svdt(sv_tx, glo_ephem); sv_tx = time_add(sv_tx, -svdt); sv_pos = geph2pos(sv_tx, glo_ephem, &svdt); sv_vel = geph2vel(sv_tx, glo_ephem, &svddt); } else { EphemPtr ephem = std::dynamic_pointer_cast(ephems[i]); svdt = eph2svdt(sv_tx, ephem); sv_tx = time_add(sv_tx, -svdt); sv_pos = eph2pos(sv_tx, ephem, &svdt); sv_vel = eph2vel(sv_tx, ephem, &svddt); sat_state->tgd = ephem->tgd[0]; } sat_state->sat_id = sat; sat_state->ttx = sv_tx; sat_state->pos = sv_pos; sat_state->vel = sv_vel; sat_state->dt = svdt; sat_state->ddt = svddt; } return all_sv_states; } void psr_res(const Eigen::Matrix &rcv_state, const std::vector &obs, const std::vector &all_sv_states, const std::vector &iono_params, Eigen::VectorXd &res, Eigen::MatrixXd &J, std::vector &atmos_delay, std::vector &all_sv_azel) { const uint32_t num_sv = all_sv_states.size(); // clear output res = Eigen::VectorXd::Zero(num_sv); J = Eigen::MatrixXd::Zero(num_sv, 7); atmos_delay.resize(num_sv); all_sv_azel.resize(num_sv); for (uint32_t i = 0; i < num_sv; ++i) { int l1_idx = -1; L1_freq(obs[i], &l1_idx); if (l1_idx < 0) continue; const SatStatePtr &sat_state = all_sv_states[i]; uint32_t this_sys = satsys(sat_state->sat_id, NULL); Eigen::Vector3d sv_pos = sat_state->pos; double ion_delay=0, tro_delay=0; double azel[2] = {0, M_PI/2.0}; if (rcv_state.topLeftCorner<3,1>().norm() > 0) { sat_azel(rcv_state.topLeftCorner<3,1>(), sv_pos, azel); Eigen::Vector3d rcv_lla = ecef2geo(rcv_state.head<3>()); // use satellite signal transmit time instead tro_delay = calculate_trop_delay(sat_state->ttx, rcv_lla, azel); ion_delay = calculate_ion_delay(sat_state->ttx, iono_params, rcv_lla, azel); } Eigen::Vector3d rv2sv = sv_pos - rcv_state.topLeftCorner<3,1>(); Eigen::Vector3d unit_rv2sv = rv2sv.normalized(); double sagnac_term = EARTH_OMG_GPS*(sv_pos(0)*rcv_state(1,0)- sv_pos(1)*rcv_state(0,0))/LIGHT_SPEED; double psr_estimated = rv2sv.norm() + sagnac_term + rcv_state(3+sys2idx.at(this_sys)) - sat_state->dt*LIGHT_SPEED + tro_delay + ion_delay + sat_state->tgd*LIGHT_SPEED; J.block(i, 0, 1, 3) = -unit_rv2sv.transpose(); J(i, 3+sys2idx.at(this_sys)) = 1.0; res(i) = psr_estimated - obs[i]->psr[l1_idx]; atmos_delay[i] = Eigen::Vector2d(ion_delay, tro_delay); all_sv_azel[i] = Eigen::Vector2d(azel[0], azel[1]); } } Eigen::Matrix psr_pos(const std::vector &obs, const std::vector &ephems, const std::vector &iono_params) { Eigen::Matrix result; result.setZero(); std::vector valid_obs; std::vector valid_ephems; filter_L1(obs, ephems, valid_obs, valid_ephems); if (valid_obs.size() < 4) { LOG(ERROR) << "[gnss_comm::psr_pos] GNSS observation not enough.\n"; return result; } std::vector all_sat_states = sat_states(valid_obs, valid_ephems); Eigen::Matrix xyzt; xyzt.setZero(); double dx_norm = 1.0; uint32_t num_iter = 0; while(num_iter < MAX_ITER_PVT && dx_norm > EPSILON_PVT) { Eigen::VectorXd b; Eigen::MatrixXd G; std::vector atmos_delay; std::vector all_sv_azel; psr_res(xyzt, valid_obs, all_sat_states, iono_params, b, G, atmos_delay, all_sv_azel); std::vector good_idx; for (uint32_t i = 0; i < valid_obs.size(); ++i) { if (G.row(i).norm() <= 0) continue; // res not computed if (all_sv_azel[i].y() > CUT_OFF_DEGREE/180.0*M_PI) good_idx.push_back(i); } int sys_mask[4] = {0, 0, 0, 0}; for (uint32_t i = 0; i < valid_obs.size(); ++i) { const uint32_t obs_sys = satsys(valid_obs[i]->sat, NULL); sys_mask[sys2idx.at(obs_sys)] = 1; } uint32_t num_extra_constraint = 4; for (uint32_t k = 0; k < 4; ++k) num_extra_constraint -= sys_mask[k]; LOG_IF(FATAL, num_extra_constraint >= 4) << "[gnss_comm::psr_pos] too many extra-clock constraints.\n"; const uint32_t good_num = good_idx.size(); LOG_IF(ERROR, good_num < 4) << "too few good obs: " << good_num; Eigen::MatrixXd good_G(good_num+num_extra_constraint, 7); Eigen::VectorXd good_b(good_num+num_extra_constraint); Eigen::MatrixXd good_W(good_num+num_extra_constraint, good_num+num_extra_constraint); good_W.setZero(); for (uint32_t i = 0; i < good_num; ++i) { const uint32_t origin_idx = good_idx[i]; const ObsPtr &this_obs = valid_obs[origin_idx]; good_G.row(i) = G.row(origin_idx); good_b(i) = b(origin_idx); // compose weight const double sin_el = sin(all_sv_azel[origin_idx].y()); double weight = sin_el*sin_el; int l1_idx = -1; L1_freq(this_obs, &l1_idx); LOG_IF(FATAL, l1_idx < 0) << "[gnss_comm::psr_pos] no L1 observation found.\n"; if (this_obs->psr_std[l1_idx] > 0) weight /= (this_obs->psr_std[l1_idx]/0.16); const uint32_t obs_sys = satsys(this_obs->sat, NULL); if (obs_sys == SYS_GPS || obs_sys == SYS_BDS) weight /= valid_ephems[origin_idx]->ura-1; else if (obs_sys == SYS_GAL) weight /= valid_ephems[origin_idx]->ura-2; else if (obs_sys == SYS_GLO) weight /= 4; good_W(i, i) = weight; } uint32_t tmp_count = good_num; // add extra pseudo measurement to contraint unobservable clock bias for (size_t k = 0; k < 4; ++k) { if (!sys_mask[k]) { good_G.row(tmp_count).setZero(); good_G(tmp_count, k+3) = 1.0; good_b(tmp_count) = 0; good_W(tmp_count, tmp_count) = 1000; // large weight ++tmp_count; } } // ready for solving Eigen::VectorXd dx = -(good_G.transpose()*good_W*good_G).inverse() * good_G.transpose() * good_W * good_b; dx_norm = dx.norm(); xyzt += dx; // LOG(INFO) << "cov is \n" << (G.transpose()*W*G).inverse(); ++num_iter; } if (num_iter == MAX_ITER_PVT) { LOG(WARNING) << "[gnss_comm::psr_pos] XYZT solver reached maximum iterations.\n"; return result; } result = xyzt; return result; } void dopp_res(const Eigen::Matrix &rcv_state, const Eigen::Vector3d &rcv_ecef, const std::vector &obs, const std::vector &all_sv_states, Eigen::VectorXd &res, Eigen::MatrixXd &J) { const uint32_t num_sv = all_sv_states.size(); //clear output res = Eigen::VectorXd::Zero(num_sv); J = Eigen::MatrixXd::Zero(num_sv, 4); for (uint32_t i = 0; i < num_sv; ++i) { const SatStatePtr &sat_state = all_sv_states[i]; Eigen::Vector3d unit_rv2sv = (sat_state->pos - rcv_ecef).normalized(); double sagnac_term = EARTH_OMG_GPS/LIGHT_SPEED*( sat_state->vel(0)*rcv_ecef(1)+ sat_state->pos(0)*rcv_state(1,0) - sat_state->vel(1)*rcv_ecef(0) - sat_state->pos(1)*rcv_state(0,0)); double dopp_estimated = (sat_state->vel - rcv_state.topLeftCorner<3, 1>()).dot(unit_rv2sv) + rcv_state(3, 0) + sagnac_term - sat_state->ddt*LIGHT_SPEED; int l1_idx = -1; const double obs_freq = L1_freq(obs[i], &l1_idx); if (obs_freq < 0) continue; const double wavelength = LIGHT_SPEED / obs_freq; res(i) = dopp_estimated + obs[i]->dopp[l1_idx]*wavelength; J.block(i, 0, 1, 3) = -1.0 * unit_rv2sv.transpose(); J(i, 3) = 1.0; } } Eigen::Matrix dopp_vel(const std::vector &obs, const std::vector &ephems, Eigen::Vector3d &ref_ecef) { // LOG(INFO) << "try to solve Doppler velocity."; Eigen::Matrix result; result.setZero(); if (ref_ecef.norm() == 0) { // reference point not given, try calculate using pseudorange std::vector zero_iono_params(8, 0.0); Eigen::Matrix psr_result = psr_pos(obs, ephems, zero_iono_params); if (psr_result.head<3>().norm() != 0) { ref_ecef = psr_result.head<3>(); } else { LOG(ERROR) << "[gnss_comm::dopp_vel] Unable to initialize reference position for Doppler calculation.\n"; return result; } } std::vector valid_obs; std::vector valid_ephems; filter_L1(obs, ephems, valid_obs, valid_ephems); if (valid_obs.size() < 4) { LOG(ERROR) << "[gnss_comm::dopp_vel] GNSS observation not enough for velocity calculation.\n"; return result; } std::vector all_sat_states = sat_states(valid_obs, valid_ephems); // compute azel for all satellite std::vector all_sv_azel; for (uint32_t i = 0; i < all_sat_states.size(); ++i) { double azel[2] = {0, 0}; sat_azel(ref_ecef, all_sat_states[i]->pos, azel); all_sv_azel.emplace_back(azel[0], azel[1]); } Eigen::Matrix xyzt_dot; xyzt_dot.setZero(); double dx_norm = 1.0; uint32_t num_iter = 0; while(num_iter < MAX_ITER_PVT && dx_norm > EPSILON_PVT) { Eigen::MatrixXd G; Eigen::VectorXd b; dopp_res(xyzt_dot, ref_ecef, valid_obs, all_sat_states, b, G); std::vector good_idx; for (uint32_t i = 0; i < valid_obs.size(); ++i) { if (G.row(i).norm() <= 0) continue; // res not computed if (all_sv_azel[i].y() > CUT_OFF_DEGREE/180.0*M_PI) good_idx.push_back(i); } const uint32_t good_num = good_idx.size(); Eigen::MatrixXd good_G = Eigen::MatrixXd::Zero(good_num, 4); Eigen::VectorXd good_b = Eigen::VectorXd::Zero(good_num); Eigen::MatrixXd good_W = Eigen::MatrixXd::Zero(good_num, good_num); for (uint32_t i = 0; i < good_num; ++i) { const uint32_t origin_idx = good_idx[i]; good_G.row(i) = G.row(origin_idx); good_b(i) = b(origin_idx); const double sin_el = sin(all_sv_azel[origin_idx].y()); double weight = sin_el*sin_el; const ObsPtr &this_obs = valid_obs[origin_idx]; int l1_idx = -1; L1_freq(this_obs, &l1_idx); LOG_IF(FATAL, l1_idx < 0) << "[gnss_comm::dopp_vel] no L1 observation found.\n"; if (this_obs->dopp_std[l1_idx] > 0) weight /= (this_obs->dopp_std[l1_idx]/0.256); const uint32_t obs_sys = satsys(this_obs->sat, NULL); if (obs_sys == SYS_GPS || obs_sys == SYS_BDS) weight /= valid_ephems[origin_idx]->ura-1; else if (obs_sys == SYS_GAL) weight /= valid_ephems[origin_idx]->ura-2; else if (obs_sys == SYS_GLO) weight /= 2; good_W(i, i) = weight; } // solve Eigen::VectorXd dx = -(good_G.transpose()*good_W*good_G).inverse() * good_G.transpose() * good_W * good_b; dx_norm = dx.norm(); xyzt_dot += dx; // dx_norm = 0; // LOG(INFO) << "cov is \n" << (G.transpose()*W*G).inverse(); ++num_iter; } if (num_iter == MAX_ITER_PVT) LOG(WARNING) << "[gnss_comm::dopp_vel] XYZT solver reached maximum iterations.\n"; result = xyzt_dot; return result; } } ================================================ FILE: gnss_comm/src/gnss_utility.cpp ================================================ /** * This file is part of gnss_comm. * * Copyright (C) 2021 Aerial Robotics Group, Hong Kong University of Science and Technology * Author: CAO Shaozu (shaozu.cao@gmail.com) * * gnss_comm is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * gnss_comm is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with gnss_comm. If not, see . * * As many of the utility functions are adapted from RTKLIB, * the license for those part of code is claimed as follows: * * The RTKLIB software package is distributed under the following BSD 2-clause * license (http://opensource.org/licenses/BSD-2-Clause) and additional two * exclusive clauses. Users are permitted to develop, produce or sell their own * non-commercial or commercial products utilizing, linking or including RTKLIB as * long as they comply with the license. * * Copyright (c) 2007-2020, T. Takasu, All rights reserved. */ #include namespace gnss_comm { // some of the following functions are adapted from RTKLIB const static double gpst0[] = {1980,1,6,0,0,0}; /* gps time reference */ const static double gst0 [] = {1999,8,22,0,0,0}; /* galileo system time reference */ const static double bdt0 [] = {2006,1,1,0,0,0}; /* beidou time reference */ /* satellite system+prn/slot number to satellite number ------------------------ * convert satellite system+prn/slot number to satellite number * args : uint32_t sys I satellite system (SYS_GPS,SYS_GLO,...) * uint32_t prn I satellite prn/slot number * return : satellite number (0:error) *-----------------------------------------------------------------------------*/ uint32_t sat_no(uint32_t sys, uint32_t prn) { if (prn == 0) return 0; switch (sys) { case SYS_GPS: if (prn < MIN_PRN_GPS || prn > MAX_PRN_GPS) return 0; return prn-MIN_PRN_GPS+1; case SYS_GLO: if (prn < MIN_PRN_GLO || prn > MAX_PRN_GLO) return 0; return N_SAT_GPS+prn-MIN_PRN_GLO+1; case SYS_GAL: if (prn < MIN_PRN_GAL || prn > MAX_PRN_GAL) return 0; return N_SAT_GPS+N_SAT_GLO+prn-MIN_PRN_GAL+1; case SYS_BDS: if (prn < MIN_PRN_BDS || prn > MAX_PRN_BDS) return 0; return N_SAT_GPS+N_SAT_GLO+N_SAT_GAL+prn-MIN_PRN_BDS+1; } return 0; } /* satellite number to satellite system ---------------------------------------- * convert satellite number to satellite system * args : uint32_t sat I satellite number (1-MAXSAT) * uint32_t *prn IO satellite prn/slot number (NULL: no output) * return : satellite system (SYS_GPS,SYS_GLO,...) *-----------------------------------------------------------------------------*/ uint32_t satsys(uint32_t sat, uint32_t *prn) { uint32_t sys = SYS_NONE; if (sat <= 0 || sat > MAX_SAT) sat = 0; else if (sat <= N_SAT_GPS) { sys = SYS_GPS; sat += MIN_PRN_GPS-1; } else if ((sat-=N_SAT_GPS) <= N_SAT_GLO) { sys = SYS_GLO; sat += MIN_PRN_GLO-1; } else if ((sat-=N_SAT_GLO) <= N_SAT_GAL) { sys = SYS_GAL; sat += MIN_PRN_GAL-1; } else if ((sat-=N_SAT_GAL) <= N_SAT_BDS) { sys = SYS_BDS; sat += MIN_PRN_BDS-1; } else sat = 0; if (prn) *prn = sat; return sys; } /* convert calendar day/time to time ------------------------------------------- * convert calendar day/time to gtime_t struct * args : double *ep I day/time {year,month,day,hour,min,sec} * return : gtime_t struct * notes : proper in 1970-2037 or 1970-2099 (64bit time_t) *-----------------------------------------------------------------------------*/ gtime_t epoch2time(const double *ep) { const int doy[] = {1,32,60,91,121,152,182,213,244,274,305,335}; gtime_t time = {0}; int days, sec, year=(int)ep[0], mon=(int)ep[1], day=(int)ep[2]; if (year < 1970 || year > 2099 || mon < 1 || mon > 12) return time; /* leap year if year%4==0 in 1901-2099 */ days = (year-1970)*365 + (year-1969)/4 + doy[mon-1] + day-2 + (year%4==0&&mon>=3?1:0); sec = (int)floor(ep[5]); time.time = (time_t)days*86400 + (int)ep[3]*3600 + (int)ep[4]*60 + sec; time.sec = ep[5] - sec; return time; } /* time to calendar day/time --------------------------------------------------- * convert gtime_t struct to calendar day/time * args : gtime_t t I gtime_t struct * double *ep O day/time {year,month,day,hour,min,sec} * return : none * notes : proper in 1970-2037 or 1970-2099 (64bit time_t) *-----------------------------------------------------------------------------*/ void time2epoch(gtime_t t, double *ep) { const int mday[] = { /* # of days in a month */ 31,28,31,30,31,30,31,31,30,31,30,31,31,28,31,30,31,30,31,31,30,31,30,31, 31,29,31,30,31,30,31,31,30,31,30,31,31,28,31,30,31,30,31,31,30,31,30,31 }; int days, sec, mon, day; /* leap year if year%4==0 in 1901-2099 */ days = (int)(t.time / 86400); sec = (int)(t.time - (time_t)days*86400); for (day=days%1461, mon=0; mon<48; mon++) { if (day >= mday[mon]) day -= mday[mon]; else break; } ep[0] = 1970 + days/1461*4 + mon/12; ep[1] = mon%12 + 1; ep[2] = day + 1; ep[3] = sec / 3600; ep[4] = sec%3600 / 60; ep[5] = sec%60 + t.sec; } /* gps time to time ------------------------------------------------------------ * convert week and tow in gps time to gtime_t struct * args : uint32_t week I week number in gps time * double tow I time of week in gps time (s) * return : gtime_t struct *-----------------------------------------------------------------------------*/ gtime_t gpst2time(uint32_t week, double tow) { gtime_t t = epoch2time(gpst0); if (tow < -1E9 || tow > 1E9) tow = 0.0; t.time += 86400*7*week + (int)tow; t.sec = tow - (int)tow; return t; } /* time to gps time ------------------------------------------------------------ * convert gtime_t struct to week and tow in gps time * args : gtime_t t I gtime_t struct * uint32_t *week IO week number in gps time (NULL: no output) * return : time of week in gps time (s) *-----------------------------------------------------------------------------*/ double time2gpst(gtime_t t, uint32_t *week) { gtime_t t0 = epoch2time(gpst0); time_t sec = t.time - t0.time; uint32_t w = (uint32_t)(sec / (86400*7)); if (week) *week = w; return (double)(sec - w*86400*7) + t.sec; } /* galileo system time to time ------------------------------------------------- * convert week and tow in galileo system time (gst) to gtime_t struct * args : int week I week number in gst * double tow I time of week in gst (s) * return : gtime_t struct *-----------------------------------------------------------------------------*/ gtime_t gst2time(int week, double tow) { gtime_t t = epoch2time(gst0); if (tow < -1E9 || tow > 1E9) tow = 0.0; t.time += 86400*7*week + (int)tow; t.sec = tow - (int)tow; return t; } /* time to galileo system time ------------------------------------------------- * convert gtime_t struct to week and tow in galileo system time (gst) * args : gtime_t t I gtime_t struct * int *week IO week number in gst (NULL: no output) * return : time of week in gst (s) *-----------------------------------------------------------------------------*/ double time2gst(gtime_t t, int *week) { gtime_t t0 = epoch2time(gst0); time_t sec = t.time-t0.time; int w = (int)(sec / (86400*7)); if (week) *week = w; return (double)(sec - w*86400*7) + t.sec; } /* beidou time (bdt) to time --------------------------------------------------- * convert week and tow in beidou time (bdt) to gtime_t struct * args : int week I week number in bdt * double tow I time of week in bdt (s) * return : gtime_t struct *-----------------------------------------------------------------------------*/ gtime_t bdt2time(int week, double tow) { gtime_t t = epoch2time(bdt0); if (tow < -1E9 || tow > 1E9) tow = 0.0; t.time += 86400*7*week + (int)tow; t.sec = tow - (int)tow; return t; } /* time to beidouo time (bdt) -------------------------------------------------- * convert gtime_t struct to week and tow in beidou time (bdt) * args : gtime_t t I gtime_t struct * int *week IO week number in bdt (NULL: no output) * return : time of week in bdt (s) *-----------------------------------------------------------------------------*/ double time2bdt(gtime_t t, int *week) { gtime_t t0 = epoch2time(bdt0); time_t sec = t.time - t0.time; int w = (int)(sec / (86400*7)); if (week) *week = w; return (double)(sec - w*86400*7) + t.sec; } /* gpstime to utc -------------------------------------------------------------- * convert gpstime to utc considering leap seconds * args : gtime_t t I time expressed in gpstime * return : time expressed in utc * notes : ignore slight time offset under 100 ns *-----------------------------------------------------------------------------*/ gtime_t gpst2utc(gtime_t t) { gtime_t tu; int i; for (i=0;leaps[i][0]>0;i++) { tu=time_add(t,leaps[i][6]); if (time_diff(tu,epoch2time(leaps[i]))>=0.0) return tu; } return t; } /* utc to gpstime -------------------------------------------------------------- * convert utc to gpstime considering leap seconds * args : gtime_t t I time expressed in utc * return : time expressed in gpstime * notes : ignore slight time offset under 100 ns *-----------------------------------------------------------------------------*/ gtime_t utc2gpst(gtime_t t) { int i; for (i=0;leaps[i][0]>0;i++) { if (time_diff(t,epoch2time(leaps[i]))>=0.0) return time_add(t,-leaps[i][6]); } return t; } double julian_day(std::vector datetime) { LOG_IF(FATAL, datetime.size() != 6) << "datetime should contain 6 fields"; LOG_IF(FATAL, datetime[0] < 1901 || datetime[0] > 2099) << datetime[0] << " should within range 1900 ~ 2100"; if (datetime[1] <= 2) { datetime[1] += 12; datetime[0] -= 1; } double julian_day = floor(365.25*datetime[0]) + floor(30.6001*(datetime[1]+1)) - 15 + 1720996.5 + datetime[2] + datetime[3] / 24 + datetime[4] / 60 / 24 + datetime[5] / 3600 / 24; return julian_day; } uint32_t leap_seconds_from_GPS_epoch(std::vector datetime) { gtime_t given_t = epoch2time(&(datetime[0])); for (size_t i = 0; leaps[i][0] > 0; ++i) { gtime_t tu = time_add(given_t, leaps[i][6]); if (time_diff(tu, epoch2time(leaps[i])) >= 0.0) return static_cast(-leaps[i][6]); } return static_cast(-1); } double time2doy(gtime_t time) { double ep[6]; time2epoch(time, ep); ep[1] = ep[2] = 1.0; ep[3] = ep[4] = ep[5] = 0.0; return time_diff(time, epoch2time(ep))/86400.0 + 1.0; } double time_diff(gtime_t t1, gtime_t t2) { return difftime(t1.time,t2.time)+t1.sec-t2.sec; } gtime_t time_add(gtime_t t, double sec) { t.sec += sec; double tt = floor(t.sec); t.time += static_cast(tt); t.sec -= tt; return t; } double time2sec(gtime_t time) { return static_cast(time.time) + time.sec; } gtime_t sec2time(const double sec) { gtime_t time; time.time = floor(sec); time.sec = sec - time.time; return time; } Eigen::Vector3d geo2ecef(const Eigen::Vector3d &lla) { Eigen::Vector3d xyz; const double cos_lat = std::cos(lla(0)*D2R); const double sin_lat = std::sin(lla(0)*D2R); const double N = EARTH_SEMI_MAJOR / std::sqrt(1 - EARTH_ECCE_2*sin_lat*sin_lat); xyz.x() = (N + lla(2)) * cos_lat * std::cos(lla(1)*D2R); xyz.y() = (N + lla(2)) * cos_lat * std::sin(lla(1)*D2R); xyz.z() = (N*(1-EARTH_ECCE_2) + lla(2)) * sin_lat; return xyz; } Eigen::Vector3d ecef2geo(const Eigen::Vector3d &xyz) { Eigen::Vector3d lla = Eigen::Vector3d::Zero(); if (xyz.x() == 0 && xyz.y() == 0) { LOG(ERROR) << "LLA coordinate is not defined if x = 0 and y = 0"; return lla; } double e2 = EARTH_ECCE_2; double a = EARTH_SEMI_MAJOR; double a2 = a * a; double b2 = a2 * (1 - e2); double b = sqrt(b2); double ep2 = (a2 - b2) / b2; double p = xyz.head<2>().norm(); // two sides and hypotenuse of right angle triangle with one angle = theta: double s1 = xyz.z() * a; double s2 = p * b; double h = sqrt(s1 * s1 + s2 * s2); double sin_theta = s1 / h; double cos_theta = s2 / h; // two sides and hypotenuse of right angle triangle with one angle = lat: s1 = xyz.z() + ep2 * b * pow(sin_theta, 3); s2 = p - a * e2 * pow(cos_theta, 3); h = sqrt(s1 * s1 + s2 * s2); double tan_lat = s1 / s2; double sin_lat = s1 / h; double cos_lat = s2 / h; double lat = atan(tan_lat); double lat_deg = lat * R2D; double N = a2 * pow((a2 * cos_lat * cos_lat + b2 * sin_lat * sin_lat), -0.5); double altM = p / cos_lat - N; double lon = atan2(xyz.y(), xyz.x()); double lon_deg = lon * R2D; lla << lat_deg, lon_deg, altM; return lla; } double Kepler(const double mk, const double es) { double e = mk; double ek = 1e6; size_t num_iter = 0; while (num_iter < MAX_ITER_KEPLER && fabs(e-ek) > EPSILON_KEPLER) { ek = e; e -= (e - es * sin(e) - mk) / (1.0 - es * cos(e)); ++ num_iter; } if (num_iter == MAX_ITER_KEPLER) LOG(WARNING) << "Kepler equation reached max number iteration"; return ek; } /* double eph2svdt(const gtime_t &curr_time, const EphemPtr ephem_ptr) { double tk = time_diff(curr_time, ephem_ptr->toe); if (tk > WEEK_SECONDS/2) tk -= WEEK_SECONDS; else if (tk < -WEEK_SECONDS/2) tk += WEEK_SECONDS; double n0 = sqrt(MU / pow(ephem_ptr->A, 3)); // mean motion angular velocity (rad/sec) double n = n0 + ephem_ptr->delta_n; // corrected mean motion double Mk = ephem_ptr->M0 + n * tk; double Ek = Kepler(Mk, ephem_ptr->e); // solve Kepler equation for eccentric anomaly double dt = time_diff(curr_time, ephem_ptr->toc); if (dt > WEEK_SECONDS/2) dt -= WEEK_SECONDS; else if (dt < -WEEK_SECONDS/2) dt += WEEK_SECONDS; for (size_t i = 0; i < 2; ++i) dt -= ephem_ptr->af0 + ephem_ptr->af1*dt + ephem_ptr->af2*dt*dt; double dtsv = ephem_ptr->af0 + ephem_ptr->af1 * dt + ephem_ptr->af2 * dt * dt; double rel_dt = -2.0 * sqrt(MU)/LIGHT_SPEED/LIGHT_SPEED * ephem_ptr->e * sqrt(ephem_ptr->A) * sin(Ek); rel_dt = 0; dtsv += rel_dt; // LOG(INFO) << "dtsv is " << std::setprecision(10) << dtsv; return dtsv; } */ double eph2svdt(const gtime_t &curr_time, const EphemPtr ephem_ptr) { double dt = time_diff(curr_time, ephem_ptr->toc); for (size_t i = 0; i < 2; ++i) dt -= ephem_ptr->af0 + ephem_ptr->af1*dt + ephem_ptr->af2*dt*dt; double dtsv = ephem_ptr->af0 + ephem_ptr->af1 * dt + ephem_ptr->af2 * dt * dt; return dtsv; } Eigen::Vector3d eph2pos(const gtime_t &curr_time, const EphemPtr ephem_ptr, double *svdt) { Eigen::Vector3d sv_pos; double tk = time_diff(curr_time, ephem_ptr->toe); if (tk > WEEK_SECONDS/2) tk -= WEEK_SECONDS; else if (tk < -WEEK_SECONDS/2) tk += WEEK_SECONDS; // LOG(INFO) << "tk is " << std::setprecision(10) << tk; if (std::abs(tk) > EPH_VALID_SECONDS) LOG(WARNING) << "Ephemeris is not valid anymore"; uint32_t prn; uint32_t sys = satsys(ephem_ptr->sat, &prn); double mu = MU, earth_omg = EARTH_OMG_GPS; switch (sys) { case SYS_GPS: mu = MU_GPS; earth_omg = EARTH_OMG_GPS; break; case SYS_GAL: mu = MU; earth_omg = EARTH_OMG_GPS; break; case SYS_GLO: mu = MU; earth_omg = EARTH_OMG_GLO; break; case SYS_BDS: mu = MU; earth_omg = EARTH_OMG_BDS; break; } // LOG(INFO) << "SYS: " << sys << ", prn: " << prn << ", curr_time is " // << std::setprecision(10) << curr_time.time // << ", and toe is " << ephem_ptr->toe.time; double n0 = sqrt(mu / pow(ephem_ptr->A, 3)); // mean motion angular velocity (rad/sec) double n = n0 + ephem_ptr->delta_n; // corrected mean motion double Mk = ephem_ptr->M0 + n * tk; double Ek = Kepler(Mk, ephem_ptr->e); // solve Kepler equation for eccentric anomaly double sin_Ek = sin(Ek), cos_Ek = cos(Ek); double vk = atan2(sqrt(1 - ephem_ptr->e*ephem_ptr->e) * sin_Ek, cos_Ek - ephem_ptr->e); double phi = vk + ephem_ptr->omg; double cos_2phi = cos(2 * phi); double sin_2phi = sin(2 * phi); double delta_uk = ephem_ptr->cus * sin_2phi + ephem_ptr->cuc * cos_2phi; // latitude correction double delta_rk = ephem_ptr->crs * sin_2phi + ephem_ptr->crc * cos_2phi; // radius correction double delta_ik = ephem_ptr->cis * sin_2phi + ephem_ptr->cic * cos_2phi; // correction to inclination double uk = phi + delta_uk; // corrected latitude double rk = ephem_ptr->A * (1 - ephem_ptr->e * cos_Ek) + delta_rk; // corrected radius double ik = ephem_ptr->i0 + ephem_ptr->i_dot * tk + delta_ik; // corrected inclination double sin_ik = sin(ik), cos_ik = cos(ik); double xk_prime = rk * cos(uk); // x position in orbital plane double yk_prime = rk * sin(uk); // y position in orbital plane double toe_tow = (sys == SYS_BDS ? time2bdt(time_add(ephem_ptr->toe, -14), NULL) : ephem_ptr->toe_tow); if (sys == SYS_BDS && prn <= 5) // BDS GEO satellite { double OMG_k = ephem_ptr->OMG0 + ephem_ptr->OMG_dot * tk - earth_omg * toe_tow; double sin_OMG_k = sin(OMG_k), cos_OMG_k = cos(OMG_k); double xg = xk_prime * cos_OMG_k - yk_prime * cos_ik * sin_OMG_k; double yg = xk_prime * sin_OMG_k + yk_prime * cos_ik * cos_OMG_k; double zg = yk_prime * sin_ik; double sin_o = sin(earth_omg * tk), cos_o = cos(earth_omg * tk); sv_pos.x() = xg * cos_o + yg * sin_o * COS_N5 + zg * sin_o * SIN_N5; sv_pos.y() = -xg * sin_o + yg * cos_o * COS_N5 + zg * cos_o * SIN_N5; sv_pos.z() = -yg * SIN_N5 + zg * COS_N5; } else { double OMG_k = ephem_ptr->OMG0 + (ephem_ptr->OMG_dot - earth_omg) * tk - earth_omg * toe_tow; double sin_OMG_k = sin(OMG_k), cos_OMG_k = cos(OMG_k); sv_pos.x() = xk_prime * cos_OMG_k - yk_prime * cos_ik * sin_OMG_k; sv_pos.y() = xk_prime * sin_OMG_k + yk_prime * cos_ik * cos_OMG_k; sv_pos.z() = yk_prime * sin_ik; } double dt = time_diff(curr_time, ephem_ptr->toc); double dts = ephem_ptr->af0 + ephem_ptr->af1 * dt + ephem_ptr->af2 * dt * dt; // relativity correction dts -= 2.0 * sqrt(mu * ephem_ptr->A) * ephem_ptr->e * sin_Ek / LIGHT_SPEED / LIGHT_SPEED; if (svdt) *svdt = dts; return sv_pos; } Eigen::Vector3d eph2vel(const gtime_t &curr_time, const EphemPtr ephem_ptr, double *svddt) { Eigen::Vector3d sv_vel; sv_vel.setZero(); double tk = time_diff(curr_time, ephem_ptr->toe); if (tk > WEEK_SECONDS/2) tk -= WEEK_SECONDS; else if (tk < -WEEK_SECONDS/2) tk += WEEK_SECONDS; // LOG(INFO) << "tk is " << std::setprecision(10) << tk; if (std::abs(tk) > EPH_VALID_SECONDS) LOG(WARNING) << "Ephemeris is not valid anymore"; uint32_t prn; uint32_t sys = satsys(ephem_ptr->sat, &prn); double mu = MU, earth_omg = EARTH_OMG_GPS; switch (sys) { case SYS_GPS: mu = MU_GPS; earth_omg = EARTH_OMG_GPS; break; case SYS_GAL: mu = MU; earth_omg = EARTH_OMG_GPS; break; case SYS_GLO: mu = MU; earth_omg = EARTH_OMG_GLO; break; case SYS_BDS: mu = MU; earth_omg = EARTH_OMG_BDS; break; } double n0 = sqrt(mu / pow(ephem_ptr->A, 3)); // mean motion angular velocity (rad/sec) double n = n0 + ephem_ptr->delta_n; // corrected mean motion double Mk = ephem_ptr->M0 + n * tk; double Ek = Kepler(Mk, ephem_ptr->e); // solve Kepler equation for eccentric anomaly double sin_Ek = sin(Ek), cos_Ek = cos(Ek); double Ek_dot = n / (1 - ephem_ptr->e*cos_Ek); double vk_dot = sqrt(1-ephem_ptr->e*ephem_ptr->e) * Ek_dot / (1 - ephem_ptr->e*cos_Ek); double vk = atan2(sqrt(1 - ephem_ptr->e*ephem_ptr->e) * sin_Ek, cos_Ek - ephem_ptr->e); double phi = vk + ephem_ptr->omg; double cos_2phi = cos(2 * phi); double sin_2phi = sin(2 * phi); double delta_uk_dot = 2 * vk_dot * (ephem_ptr->cus*cos_2phi - ephem_ptr->cuc*sin_2phi); double delta_rk_dot = 2 * vk_dot * (ephem_ptr->crs*cos_2phi - ephem_ptr->crc*sin_2phi); double delta_ik_dot = 2 * vk_dot * (ephem_ptr->cis*cos_2phi - ephem_ptr->cic*sin_2phi); double uk_dot = vk_dot + delta_uk_dot; double rk_dot = ephem_ptr->A * ephem_ptr->e * Ek_dot * sin_Ek + delta_rk_dot; double ik_dot = ephem_ptr->i_dot + delta_ik_dot; double delta_uk = ephem_ptr->cus * sin_2phi + ephem_ptr->cuc * cos_2phi; // latitude correction double delta_rk = ephem_ptr->crs * sin_2phi + ephem_ptr->crc * cos_2phi; // radius correction double delta_ik = ephem_ptr->cis * sin_2phi + ephem_ptr->cic * cos_2phi; // correction to inclination double uk = phi + delta_uk; // corrected latitude double rk = ephem_ptr->A * (1 - ephem_ptr->e * cos_Ek) + delta_rk; // corrected radius double ik = ephem_ptr->i0 + ephem_ptr->i_dot * tk + delta_ik; // corrected inclination double sin_ik = sin(ik), cos_ik = cos(ik); double sin_uk = sin(uk), cos_uk = cos(uk); double xk_prime = rk * cos(uk); // x position in orbital plane double yk_prime = rk * sin(uk); // y position in orbital plane double xk_prime_dot = rk_dot * cos_uk - rk * uk_dot * sin_uk; double yk_prime_dot = rk_dot * sin_uk + rk * uk_dot * cos_uk; double toe_tow = (sys == SYS_BDS ? time2bdt(time_add(ephem_ptr->toe, -14), NULL) : ephem_ptr->toe_tow); if (sys == SYS_BDS && prn <= 5) // BDS GEO satellite { double OMG_k = ephem_ptr->OMG0 + ephem_ptr->OMG_dot * tk - earth_omg * toe_tow; double sin_OMG_k = sin(OMG_k), cos_OMG_k = cos(OMG_k); double OMGk_dot = ephem_ptr->OMG_dot; double term1 = xk_prime_dot - yk_prime*OMGk_dot*cos_ik; double term2 = xk_prime*OMGk_dot + yk_prime_dot*cos_ik-yk_prime*ik_dot*sin_ik; double xg = xk_prime * cos_OMG_k - yk_prime * cos_ik * sin_OMG_k; double yg = xk_prime * sin_OMG_k + yk_prime * cos_ik * cos_OMG_k; double zg = yk_prime * sin_ik; double xg_dot = term1 * cos_OMG_k - term2 * sin_OMG_k; double yg_dot = term1 * sin_OMG_k + term2 * cos_OMG_k; double zg_dot = yk_prime_dot * sin_ik + yk_prime_dot * ik_dot * cos_ik; double sin_o = sin(earth_omg * tk), cos_o = cos(earth_omg * tk); double sin_o_dot = earth_omg * cos_o, cos_o_dot = -earth_omg * sin_o; sv_vel.x() = xg_dot*cos_o + xg*cos_o_dot + yg_dot*sin_o*COS_N5 + yg*sin_o_dot*COS_N5 + zg_dot*sin_o*SIN_N5 + zg*sin_o_dot*SIN_N5; sv_vel.y() = -xg_dot*sin_o - xg*sin_o_dot + yg_dot*cos_o*COS_N5 + yg*cos_o_dot*COS_N5 + zg_dot*cos_o*SIN_N5 + zg*cos_o_dot*SIN_N5; sv_vel.z() = -yg_dot*SIN_N5 + zg_dot*COS_N5; } else { double OMG_k = ephem_ptr->OMG0 + (ephem_ptr->OMG_dot - earth_omg) * tk - earth_omg * toe_tow; double sin_OMG_k = sin(OMG_k), cos_OMG_k = cos(OMG_k); double OMGk_dot = ephem_ptr->OMG_dot - earth_omg; double term1 = xk_prime_dot - yk_prime*OMGk_dot*cos_ik; double term2 = xk_prime*OMGk_dot + yk_prime_dot*cos_ik-yk_prime*ik_dot*sin_ik; sv_vel.x() = term1 * cos_OMG_k - term2 * sin_OMG_k; sv_vel.y() = term1 * sin_OMG_k + term2 * cos_OMG_k; sv_vel.z() = yk_prime_dot * sin_ik + yk_prime_dot * ik_dot * cos_ik; } double dt = time_diff(curr_time, ephem_ptr->toc); double ddts = ephem_ptr->af1 + 2.0 * ephem_ptr->af2 * dt; // relativity correction ddts -= 2.0 * sqrt(mu * ephem_ptr->A) * ephem_ptr->e * cos_Ek * Ek_dot / LIGHT_SPEED / LIGHT_SPEED; if (svddt) *svddt = ddts; return sv_vel; } void deq(const Eigen::Vector3d &pos, const Eigen::Vector3d &vel, const Eigen::Vector3d &acc, Eigen::Vector3d &pos_dot, Eigen::Vector3d &vel_dot) { double r2 = pos.squaredNorm(), r3 = r2 * sqrt(r2); double omg2 = EARTH_OMG_GLO * EARTH_OMG_GLO; if (r2 <= 0.0) { pos_dot.setZero(); vel_dot.setZero(); return; } /* ref [2] A.3.1.2 with bug fix for xdot[4],xdot[5] */ double a = 1.5 * J2_GLO * MU * EARTH_SEMI_MAJOR_GLO * EARTH_SEMI_MAJOR_GLO / r2 / r3; /* 3/2*J2*mu*Ae^2/r^5 */ double b = 5.0 * pos.z()*pos.z() / r2; /* 5*z^2/r^2 */ double c = -MU / r3 - a * (1.0 - b); /* -mu/r^3-a(1-b) */ pos_dot.x() = vel.x(); pos_dot.y() = vel.y(); pos_dot.z() = vel.z(); vel_dot.x() = (c + omg2) * pos.x() + 2.0 * EARTH_OMG_GLO * vel.y() + acc.x(); vel_dot.y() = (c + omg2) * pos.y() - 2.0 * EARTH_OMG_GLO * vel.x() + acc.y(); vel_dot.z() = (c - 2.0 * a) * pos.z() + acc.z(); } void glo_orbit(double dt, Eigen::Vector3d &pos, Eigen::Vector3d &vel, const Eigen::Vector3d &acc) { Eigen::Vector3d pos_dot1, pos_dot2, pos_dot3, pos_dot4; Eigen::Vector3d vel_dot1, vel_dot2, vel_dot3, vel_dot4; Eigen::Vector3d next_pos, next_vel; deq(pos, vel, acc, pos_dot1, vel_dot1); next_pos = pos + 0.5 * pos_dot1 * dt; next_vel = vel + 0.5 * vel_dot1 * dt; deq(next_pos, next_vel, acc, pos_dot2, vel_dot2); next_pos = pos + 0.5 * pos_dot2 * dt; next_vel = vel + 0.5 * vel_dot2 * dt; deq(next_pos, next_vel, acc, pos_dot3, vel_dot3); next_pos = pos + pos_dot3 * dt; next_vel = vel + vel_dot3 * dt; deq(next_pos, next_vel, acc, pos_dot4, vel_dot4); pos += (pos_dot1 + 2.0 * pos_dot2 + 2.0 * pos_dot3 + pos_dot4) * dt / 6.0; vel += (vel_dot1 + 2.0 * vel_dot2 + 2.0 * vel_dot3 + vel_dot4) * dt / 6.0; } double geph2svdt(const gtime_t &curr_time, const GloEphemPtr geph_ptr) { double dt = time_diff(curr_time, geph_ptr->toe); if (dt > WEEK_SECONDS/2) dt -= WEEK_SECONDS; else if (dt < -WEEK_SECONDS/2) dt += WEEK_SECONDS; for (size_t i = 0; i < 2; ++i) dt -= -geph_ptr->tau_n + geph_ptr->gamma * dt; double svdt = -geph_ptr->tau_n + geph_ptr->gamma * dt; return svdt; } Eigen::Vector3d geph2pos(const gtime_t &curr_time, const GloEphemPtr geph_ptr, double *svdt) { Eigen::Vector3d sv_pos, sv_vel, sv_acc; sv_pos << geph_ptr->pos[0], geph_ptr->pos[1], geph_ptr->pos[2]; sv_vel << geph_ptr->vel[0], geph_ptr->vel[1], geph_ptr->vel[2]; sv_acc << geph_ptr->acc[0], geph_ptr->acc[1], geph_ptr->acc[2]; double dt = time_diff(curr_time, geph_ptr->toe); double dts = -geph_ptr->tau_n + geph_ptr->gamma * dt; if (svdt) *svdt = dts; for (double tt = dt<0.0?-TSTEP:TSTEP; fabs(dt) > 1e-9; dt -= tt) { if (fabs(dt) < TSTEP) tt = dt; glo_orbit(tt, sv_pos, sv_vel, sv_acc); } return sv_pos; } Eigen::Vector3d geph2vel(const gtime_t &curr_time, const GloEphemPtr geph_ptr, double *svddt) { Eigen::Vector3d sv_pos, sv_vel, sv_acc; sv_pos << geph_ptr->pos[0], geph_ptr->pos[1], geph_ptr->pos[2]; sv_vel << geph_ptr->vel[0], geph_ptr->vel[1], geph_ptr->vel[2]; sv_acc << geph_ptr->acc[0], geph_ptr->acc[1], geph_ptr->acc[2]; double dt = time_diff(curr_time, geph_ptr->toe); if (svddt) *svddt = geph_ptr->gamma; for (double tt = dt<0.0?-TSTEP:TSTEP; fabs(dt) > 1e-9; dt -= tt) { if (fabs(dt) < TSTEP) tt = dt; glo_orbit(tt, sv_pos, sv_vel, sv_acc); } return sv_vel; } Eigen::Vector3d ecef2enu(const Eigen::Vector3d &ref_lla, const Eigen::Vector3d &v_ecef) { double lat = ref_lla.x() * D2R, lon = ref_lla.y() * D2R; double sin_lat = sin(lat), cos_lat = cos(lat); double sin_lon = sin(lon), cos_lon = cos(lon); Eigen::Matrix3d R_enu_ecef; R_enu_ecef << -sin_lon, cos_lon, 0, -sin_lat*cos_lon, -sin_lat*sin_lon, cos_lat, cos_lat*cos_lon, cos_lat*sin_lon, sin_lat; return (R_enu_ecef * v_ecef); } Eigen::Matrix3d geo2rotation(const Eigen::Vector3d &ref_geo) { double lat = ref_geo.x() * D2R, lon = ref_geo.y() * D2R; double sin_lat = sin(lat), cos_lat = cos(lat); double sin_lon = sin(lon), cos_lon = cos(lon); Eigen::Matrix3d R_ecef_enu; R_ecef_enu << -sin_lon, -sin_lat*cos_lon, cos_lat*cos_lon, cos_lon, -sin_lat*sin_lon, cos_lat*sin_lon, 0 , cos_lat , sin_lat; return R_ecef_enu; } Eigen::Matrix3d ecef2rotation(const Eigen::Vector3d &ref_ecef) { return geo2rotation(ecef2geo(ref_ecef)); } void sat_azel(const Eigen::Vector3d &rev_pos, const Eigen::Vector3d &sat_pos, double *azel) { if (!azel) return; Eigen::Vector3d rev_lla = ecef2geo(rev_pos); Eigen::Vector3d rev2sat_ecef = (sat_pos - rev_pos).normalized(); Eigen::Vector3d rev2sat_enu = ecef2enu(rev_lla, rev2sat_ecef); azel[0] = rev2sat_ecef.head<2>().norm() < 1e-12 ? 0.0 : atan2(rev2sat_enu.x(), rev2sat_enu.y()); azel[0] += (azel[0] < 0 ? 2*M_PI : 0); azel[1] = asin(rev2sat_enu.z()); } /* latitude in degree */ static double interpc(const double coef[], double lat) { int i=(int)(lat/15.0); if (i<1) return coef[0]; else if (i>4) return coef[4]; return coef[i-1]*(1.0-lat/15.0+i)+coef[i]*(lat/15.0-i); } /* elevation in radius */ static double mapf(double el, double a, double b, double c) { double sinel=sin(el); return (1.0+a/(1.0+b/(1.0+c)))/(sinel+(a/(sinel+b/(sinel+c)))); } /* calculate troposphere hydrostatic/wet mapping functions ------------------------------ * calculate troposphere hydrostatic/wet mapping functions by Niell model * ref: Global mapping functions for the atmosphere delay at radio wavelengths * args : gtime_t time I time (gpst) * args : Eigen::Vector3d rev_lla I receiver position in geodetic coordinate * double* azel I satellite azimuth/elevation angle in radius * double* mapfh IO hydrostatic mapping coefficient * return : delay expressed by light travel distance (m) *-----------------------------------------------------------------------------*/ static void nmf(gtime_t time, const Eigen::Vector3d rev_lla, const double azel[], double *mapfh, double *mapfw) { /* hydro-ave-a,b,c, hydro-amp-a,b,c, wet-a,b,c at latitude 15,30,45,60,75 */ const double coef[][5]={ { 1.2769934E-3, 1.2683230E-3, 1.2465397E-3, 1.2196049E-3, 1.2045996E-3}, { 2.9153695E-3, 2.9152299E-3, 2.9288445E-3, 2.9022565E-3, 2.9024912E-3}, { 62.610505E-3, 62.837393E-3, 63.721774E-3, 63.824265E-3, 64.258455E-3}, { 0.0000000E-0, 1.2709626E-5, 2.6523662E-5, 3.4000452E-5, 4.1202191E-5}, { 0.0000000E-0, 2.1414979E-5, 3.0160779E-5, 7.2562722E-5, 11.723375E-5}, { 0.0000000E-0, 9.0128400E-5, 4.3497037E-5, 84.795348E-5, 170.37206E-5}, { 5.8021897E-4, 5.6794847E-4, 5.8118019E-4, 5.9727542E-4, 6.1641693E-4}, { 1.4275268E-3, 1.5138625E-3, 1.4572752E-3, 1.5007428E-3, 1.7599082E-3}, { 4.3472961E-2, 4.6729510E-2, 4.3908931E-2, 4.4626982E-2, 5.4736038E-2} }; const double aht[]={ 2.53E-5, 5.49E-3, 1.14E-3}; /* height correction */ double ah[3],aw[3],dm,el=azel[1],lat=rev_lla.x(),hgt=rev_lla.z(); int i; if (el<=0.0) { if (mapfw) *mapfw=0.0; if (mapfh) *mapfh=0.0; return; } /* year from doy 28, added half a year for southern latitudes */ double y=(time2doy(time)-28.0)/365.25+(lat<0.0?0.5:0.0); double cosy=cos(2.0*M_PI*y); lat=fabs(lat); for (i=0;i<3;i++) { ah[i]=interpc(coef[i ],lat)-interpc(coef[i+3],lat)*cosy; aw[i]=interpc(coef[i+6],lat); } /* ellipsoidal height is used instead of height above sea level */ dm=(1.0/sin(el)-mapf(el,aht[0],aht[1],aht[2]))*hgt/1E3; if (mapfw) *mapfw=mapf(el,aw[0],aw[1],aw[2]); if (mapfh) *mapfh=mapf(el,ah[0],ah[1],ah[2])+dm; } double calculate_trop_delay(gtime_t time, const Eigen::Vector3d &rev_lla, const double *azel) { // compute tropospheric delay by standard atmosphere and saastamoinen model const double temp0=15.0; /* temparature at sea level */ const double humi =0.7; /* relative humidity */ if (rev_lla.z()<-100.0||1E4 &ion_parameters, const Eigen::Vector3d &rev_lla, const double *azel) { if (ion_parameters.empty()) return 0.0; if (rev_lla.z()<-1E3||azel[1]<=0) return 0.0; /* earth centered angle (semi-circle) */ double psi=0.0137/(azel[1]/M_PI+0.11)-0.022; /* subionospheric latitude/longitude (semi-circle) */ double phi=rev_lla.x()/180.0+psi*cos(azel[0]); if (phi> 0.416) phi= 0.416; else if (phi<-0.416) phi=-0.416; double lam=rev_lla.y()/180.0+psi*sin(azel[0])/cos(phi*M_PI); /* geomagnetic latitude (semi-circle) */ phi+=0.064*cos((lam-1.617)*M_PI); /* local time (s) */ uint32_t week = 0; double tt=43200.0*lam+time2gpst(t,&week); tt-=floor(tt/86400.0)*86400.0; /* 0<=tt<86400 */ /* slant factor */ double f=1.0+16.0*pow(0.53-azel[1]/M_PI,3.0); /* ionospheric delay */ double amp=ion_parameters[0]+phi*(ion_parameters[1]+phi*(ion_parameters[2]+phi*ion_parameters[3])); double per=ion_parameters[4]+phi*(ion_parameters[5]+phi*(ion_parameters[6]+phi*ion_parameters[7])); amp=amp< 0.0? 0.0:amp; per=per<72000.0?72000.0:per; double x=2.0*M_PI*(tt-50400.0)/per; return LIGHT_SPEED*f*(fabs(x)<1.57?5E-9+amp*(1.0+x*x*(-0.5+x*x/24.0)):5E-9); } std::string sat2str(uint32_t sat_no) { std::stringstream ss; uint32_t prn = 0; uint32_t sys = satsys(sat_no, &prn); switch (sys) { case SYS_GPS: ss << "G" << std::setw(2) << std::setfill('0') << prn; break; case SYS_GLO: ss << "R" << std::setw(2) << std::setfill('0') << prn; break; case SYS_BDS: ss << "C" << std::setw(2) << std::setfill('0') << prn; break; case SYS_GAL: ss << "E" << std::setw(2) << std::setfill('0') << prn; break; case SYS_SBS: ss << "S" << std::setw(2) << std::setfill('0') << prn; break; case SYS_QZS: ss << "J" << std::setw(2) << std::setfill('0') << prn; break; default: // current not support other system LOG(WARNING) << "currently not support satelite system id " << sys; break; } return ss.str(); } double L1_freq(const ObsPtr &obs, int *l1_idx) { const uint32_t sys = satsys(obs->sat, NULL); double freq_min = -1.0; double freq_max = -1.0; if (sys == SYS_GPS || sys == SYS_GAL) freq_min = freq_max = FREQ1; else if (sys == SYS_BDS) freq_min = freq_max = FREQ1_BDS; else if (sys == SYS_GLO) { freq_min = FREQ1_GLO - 7 * DFRQ1_GLO; freq_max = FREQ1_GLO + 6 * DFRQ1_GLO; } if (l1_idx != NULL) *l1_idx = -1; for (uint32_t i = 0; i < obs->freqs.size(); ++i) { if (obs->freqs[i] >= freq_min && obs->freqs[i] <= freq_max) { if (l1_idx != NULL) *l1_idx = static_cast(i); return obs->freqs[i]; } } return -1.0; } uint32_t str2sat(const std::string &sat_str) { if (sat_str.size() < 3) { LOG(ERROR) << "Invalid RINEX satellite identifier " << sat_str; return 0; } const uint32_t prn = std::stoul(sat_str.substr(1, 2)); switch (sat_str.at(0)) { case 'G': return sat_no(SYS_GPS, prn); case 'R': return sat_no(SYS_GLO, prn); case 'C': return sat_no(SYS_BDS, prn); case 'E': return sat_no(SYS_GAL, prn); case 'S': return sat_no(SYS_SBS, prn); case 'J': return sat_no(SYS_QZS, prn); } return 0; } std::string exec(const std::string &cmd) { char buffer[128]; std::string result = ""; FILE* pipe = popen(cmd.c_str(), "r"); if (!pipe) throw std::runtime_error("popen() failed!"); try { while (fgets(buffer, sizeof buffer, pipe) != NULL) { result += buffer; } } catch (...) { pclose(pipe); throw; } pclose(pipe); return result; } } // namespace gnss_comm ================================================ FILE: gnss_comm/src/rinex_helper.cpp ================================================ /** * This file is part of gnss_comm. * * Copyright (C) 2021 Aerial Robotics Group, Hong Kong University of Science and Technology * Author: CAO Shaozu (shaozu.cao@gmail.com) * * gnss_comm is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * gnss_comm is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with gnss_comm. If not, see . */ #include namespace gnss_comm { /* convert string to double ---------------------------------------------------------- * args : std::string num_str I input string * return : double value inside the string *-------------------------------------------------------------------------------------*/ static double str2double(const std::string &num_str) { // replace character 'D' with 'e' size_t D_pos = num_str.find("D"); std::string tmp_str = num_str; if (D_pos != std::string::npos) tmp_str = tmp_str.replace(D_pos, 1, "e"); return std::stod(tmp_str); } static GloEphemPtr rinex_line2glo_ephem(const std::vector &ephem_lines, const uint32_t gpst_leap_seconds) { LOG_IF(FATAL, ephem_lines.size() != 4) << "GLO ephemeris record should contain 8 lines"; LOG_IF(FATAL, ephem_lines[0].at(0) != 'R') << "Not a valid GLO ephemeris record"; GloEphemPtr glo_ephem(new GloEphem()); uint32_t prn = static_cast(std::stoi(ephem_lines[0].substr(1, 2))); glo_ephem->sat = sat_no(SYS_GLO, prn); double epoch[6]; epoch[0] = static_cast(std::stoi(ephem_lines[0].substr(4, 4))); epoch[1] = static_cast(std::stoi(ephem_lines[0].substr(9, 2))); epoch[2] = static_cast(std::stoi(ephem_lines[0].substr(12, 2))); epoch[3] = static_cast(std::stoi(ephem_lines[0].substr(15, 2))); epoch[4] = static_cast(std::stoi(ephem_lines[0].substr(18, 2))); epoch[5] = static_cast(std::stoi(ephem_lines[0].substr(21, 2))); glo_ephem->toe = epoch2time(epoch); glo_ephem->toe.time += gpst_leap_seconds; glo_ephem->tau_n = -1.0 * str2double(ephem_lines[0].substr(23, 19)); glo_ephem->gamma = str2double(ephem_lines[0].substr(42, 19)); // the second line glo_ephem->pos[0] = str2double(ephem_lines[1].substr(4, 19)) * 1e3; glo_ephem->vel[0] = str2double(ephem_lines[1].substr(23, 19)) * 1e3; glo_ephem->acc[0] = str2double(ephem_lines[1].substr(42, 19)) * 1e3; glo_ephem->health = static_cast(str2double(ephem_lines[1].substr(61, 19))); // the third line glo_ephem->pos[1] = str2double(ephem_lines[2].substr(4, 19)) * 1e3; glo_ephem->vel[1] = str2double(ephem_lines[2].substr(23, 19)) * 1e3; glo_ephem->acc[1] = str2double(ephem_lines[2].substr(42, 19)) * 1e3; glo_ephem->freqo = static_cast(str2double(ephem_lines[2].substr(61, 19))); // the forth line glo_ephem->pos[2] = str2double(ephem_lines[3].substr(4, 19)) * 1e3; glo_ephem->vel[2] = str2double(ephem_lines[3].substr(23, 19)) * 1e3; glo_ephem->acc[2] = str2double(ephem_lines[3].substr(42, 19)) * 1e3; glo_ephem->age = static_cast(str2double(ephem_lines[3].substr(61, 19))); return glo_ephem; } static EphemPtr rinex_line2ephem(const std::vector &ephem_lines) { LOG_IF(FATAL, ephem_lines.size() != 8) << "Ephemeris record should contain 8 lines"; uint32_t sat_sys = SYS_NONE; if (ephem_lines[0].at(0) == 'G') sat_sys = SYS_GPS; else if (ephem_lines[0].at(0) == 'C') sat_sys = SYS_BDS; else if (ephem_lines[0].at(0) == 'E') sat_sys = SYS_GAL; LOG_IF(FATAL, sat_sys == SYS_NONE) << "Satellite system is not supported: " << ephem_lines[0].at(0); EphemPtr ephem(new Ephem()); uint32_t prn = static_cast(std::stoi(ephem_lines[0].substr(1, 2))); ephem->sat = sat_no(sat_sys, prn); double epoch[6]; epoch[0] = static_cast(std::stoi(ephem_lines[0].substr(4, 4))); epoch[1] = static_cast(std::stoi(ephem_lines[0].substr(9, 2))); epoch[2] = static_cast(std::stoi(ephem_lines[0].substr(12, 2))); epoch[3] = static_cast(std::stoi(ephem_lines[0].substr(15, 2))); epoch[4] = static_cast(std::stoi(ephem_lines[0].substr(18, 2))); epoch[5] = static_cast(std::stoi(ephem_lines[0].substr(21, 2))); ephem->toc = epoch2time(epoch); if (sat_sys == SYS_BDS) ephem->toc.time += 14; // BDS-GPS time correction ephem->af0 = str2double(ephem_lines[0].substr(23, 19)); ephem->af1 = str2double(ephem_lines[0].substr(42, 19)); ephem->af2 = str2double(ephem_lines[0].substr(61, 19)); // the second line if (sat_sys == SYS_GPS) ephem->iode = str2double(ephem_lines[1].substr(4, 19)); ephem->crs = str2double(ephem_lines[1].substr(23, 19)); ephem->delta_n = str2double(ephem_lines[1].substr(42, 19)); ephem->M0 = str2double(ephem_lines[1].substr(61, 19)); // the third line ephem->cuc = str2double(ephem_lines[2].substr(4, 19)); ephem->e = str2double(ephem_lines[2].substr(23, 19)); ephem->cus = str2double(ephem_lines[2].substr(42, 19)); double sqrt_A = str2double(ephem_lines[2].substr(61, 19)); ephem->A = sqrt_A * sqrt_A; // the forth line ephem->toe_tow = str2double(ephem_lines[3].substr(4, 19)); ephem->cic = str2double(ephem_lines[3].substr(23, 19)); ephem->OMG0 = str2double(ephem_lines[3].substr(42, 19)); ephem->cis = str2double(ephem_lines[3].substr(61, 19)); // the fifth line ephem->i0 = str2double(ephem_lines[4].substr(4, 19)); ephem->crc = str2double(ephem_lines[4].substr(23, 19)); ephem->omg = str2double(ephem_lines[4].substr(42, 19)); ephem->OMG_dot = str2double(ephem_lines[4].substr(61, 19)); // the sixth line ephem->i_dot = str2double(ephem_lines[5].substr(4, 19)); if (sat_sys == SYS_GAL) { uint32_t ephe_source = static_cast(str2double(ephem_lines[5].substr(23, 19))); if (!(ephe_source & 0x01)) { // LOG(ERROR) << "not contain I/NAV E1-b info, skip this ephemeris"; return ephem; // only parse I/NAV E1-b ephemeris } } ephem->week = static_cast(str2double(ephem_lines[5].substr(42, 19))); if (sat_sys == SYS_GPS || sat_sys == SYS_GAL) ephem->toe = gpst2time(ephem->week, ephem->toe_tow); else if (sat_sys == SYS_BDS) ephem->toe = bdt2time(ephem->week, ephem->toe_tow+14); // if (sat_sys == SYS_GAL) ephem->toe = gst2time(ephem->week, ephem->toe_tow); // the seventh line ephem->ura = str2double(ephem_lines[6].substr(4, 19)); ephem->health = static_cast(str2double(ephem_lines[6].substr(23, 19))); ephem->tgd[0] = str2double(ephem_lines[6].substr(42, 19)); if (sat_sys == SYS_BDS || sat_sys == SYS_GAL) ephem->tgd[1] = str2double(ephem_lines[6].substr(61, 19)); if (sat_sys == SYS_GPS) ephem->iodc = str2double(ephem_lines[6].substr(61, 19)); // the eighth line double ttr_tow = str2double(ephem_lines[7].substr(4, 19)); // GAL week = GST week + 1024 + rollover, already align with GPS week!!! if (sat_sys == SYS_GPS || sat_sys == SYS_GAL) ephem->ttr = gpst2time(ephem->week, ttr_tow); else if (sat_sys == SYS_BDS) ephem->ttr = bdt2time(ephem->week, ttr_tow); // convert time system to parameter GPST if (sat_sys == SYS_BDS) { uint32_t week = 0; ephem->toe_tow = time2gpst(ephem->toe, &week); ephem->week = week; } return ephem; } void rinex2ephems(const std::string &rinex_filepath, std::map> &sat2ephem) { uint32_t gpst_leap_seconds = static_cast(-1); std::ifstream ephem_file(rinex_filepath); std::string line; while(std::getline(ephem_file, line)) { if (line.find("RINEX VERSION / TYPE") != std::string::npos && line.find("3.04") == std::string::npos) { LOG(ERROR) << "Only RINEX 3.04 is supported for observation file"; return; } else if (line.find("LEAP SECONDS") != std::string::npos && line.find("BDS") == std::string::npos) gpst_leap_seconds = static_cast(std::stoi(line.substr(4, 6))); else if (line.find("END OF HEADER") != std::string::npos) break; } LOG_IF(FATAL, gpst_leap_seconds == static_cast(-1)) << "No leap second record found"; while(std::getline(ephem_file, line)) { if (line.at(0) == 'G' || line.at(0) == 'C' || line.at(0) == 'E') { std::vector ephem_lines; ephem_lines.push_back(line); for (size_t i = 0; i < 7; ++i) { std::getline(ephem_file, line); ephem_lines.push_back(line); } EphemPtr ephem = rinex_line2ephem(ephem_lines); if (!ephem || ephem->ttr.time == 0) continue; if (sat2ephem.count(ephem->sat) == 0) sat2ephem.emplace(ephem->sat, std::vector()); sat2ephem.at(ephem->sat).push_back(ephem); } else if (line.at(0) == 'R') { std::vector ephem_lines; ephem_lines.push_back(line); for (size_t i = 0; i < 3; ++i) { std::getline(ephem_file, line); ephem_lines.push_back(line); } GloEphemPtr glo_ephem = rinex_line2glo_ephem(ephem_lines, gpst_leap_seconds); if (sat2ephem.count(glo_ephem->sat) == 0) sat2ephem.emplace(glo_ephem->sat, std::vector()); sat2ephem.at(glo_ephem->sat).push_back(glo_ephem); } } } static ObsPtr rinex_line2obs(const std::string rinex_str, const std::map> &sys2type) { ObsPtr obs; uint8_t sys_char = rinex_str.at(0); if (char2sys.count(sys_char) == 0) return obs; obs.reset(new Obs()); uint32_t sys = char2sys.at(sys_char); uint32_t prn = static_cast(std::stoi(rinex_str.substr(1, 2))); obs->sat = sat_no(sys, prn); std::map freq2idx; std::vector date_types = sys2type.at(sys_char); uint32_t line_offset = 3; for (auto type : date_types) { std::string field_str = rinex_str.substr(line_offset, 14); line_offset += 14 + 2; if (field_str.find_first_not_of(' ') == std::string::npos) continue; const double field_value = stod(field_str); std::stringstream ss; ss << sys_char << type.at(1); const double freq = type2freq.at(ss.str()); uint32_t freq_idx = static_cast(-1); if (freq2idx.count(freq) == 0) { obs->freqs.push_back(freq); freq_idx = obs->freqs.size()-1; freq2idx.emplace(freq, freq_idx); } else { freq_idx = freq2idx.at(freq); } if (type.at(0) == 'L') obs->cp[freq_idx] = field_value; else if (type.at(0) == 'C') obs->psr[freq_idx] = field_value; else if (type.at(0) == 'D') obs->dopp[freq_idx] = field_value; else if (type.at(0) == 'S') obs->CN0[freq_idx] = field_value; else LOG(FATAL) << "Unrecognized measurement type " << type.at(0); } // fill in other fields uint32_t num_freqs = obs->freqs.size(); LOG_IF(FATAL, num_freqs < obs->CN0.size()) << "Suspicious observation field.\n"; std::fill_n(std::back_inserter(obs->CN0), num_freqs-obs->CN0.size(), 0); LOG_IF(FATAL, num_freqs < obs->LLI.size()) << "Suspicious observation field.\n"; std::fill_n(std::back_inserter(obs->LLI), num_freqs-obs->LLI.size(), 0); LOG_IF(FATAL, num_freqs < obs->code.size()) << "Suspicious observation field.\n"; std::fill_n(std::back_inserter(obs->code), num_freqs-obs->code.size(), 0); LOG_IF(FATAL, num_freqs < obs->psr.size()) << "Suspicious observation field.\n"; std::fill_n(std::back_inserter(obs->psr), num_freqs-obs->psr.size(), 0); LOG_IF(FATAL, num_freqs < obs->psr_std.size()) << "Suspicious observation field.\n"; std::fill_n(std::back_inserter(obs->psr_std), num_freqs-obs->psr_std.size(), 0); LOG_IF(FATAL, num_freqs < obs->cp.size()) << "Suspicious observation field.\n"; std::fill_n(std::back_inserter(obs->cp), num_freqs-obs->cp.size(), 0); LOG_IF(FATAL, num_freqs < obs->cp_std.size()) << "Suspicious observation field.\n"; std::fill_n(std::back_inserter(obs->cp_std), num_freqs-obs->cp_std.size(), 0); LOG_IF(FATAL, num_freqs < obs->dopp.size()) << "Suspicious observation field.\n"; std::fill_n(std::back_inserter(obs->dopp), num_freqs-obs->dopp.size(), 0); LOG_IF(FATAL, num_freqs < obs->dopp_std.size()) << "Suspicious observation field.\n"; std::fill_n(std::back_inserter(obs->dopp_std), num_freqs-obs->dopp_std.size(), 0); LOG_IF(FATAL, num_freqs < obs->status.size()) << "Suspicious observation field.\n"; std::fill_n(std::back_inserter(obs->status), num_freqs-obs->status.size(), 0x0F); return obs; } // TODO: GLONASS slot number void rinex2obs(const std::string &rinex_filepath, std::vector> &rinex_meas) { std::ifstream obs_file(rinex_filepath); std::string rinex_line; // parse header std::map> sys2type; uint8_t sys_char = 0; while (std::getline(obs_file, rinex_line)) { if (rinex_line.find("RINEX VERSION / TYPE") != std::string::npos && rinex_line.find("3.04") == std::string::npos) { LOG(ERROR) << "Only RINEX 3.04 is supported for observation file"; return; } else if (rinex_line.find("SYS / # / OBS TYPES") != std::string::npos) { if (rinex_line.at(0) != ' ') { sys_char = rinex_line.at(0); sys2type.emplace(sys_char, std::vector()); } for (size_t i = 0; i < 13; ++i) if (rinex_line.substr(7+4*i, 3) != " ") sys2type.at(sys_char).emplace_back(rinex_line.substr(7+4*i, 3)); } else if (rinex_line.find("END OF HEADER") != std::string::npos) break; } while (std::getline(obs_file, rinex_line)) { LOG_IF(FATAL, rinex_line.at(0) != '>') << "Invalid Observation record " << rinex_line; LOG_IF(FATAL, rinex_line.at(31) != '0') << "Invalid Epoch data " << rinex_line.at(31)-48; std::vector epoch_time; epoch_time.emplace_back(std::stod(rinex_line.substr(2, 4))); epoch_time.emplace_back(std::stod(rinex_line.substr(7, 2))); epoch_time.emplace_back(std::stod(rinex_line.substr(10, 2))); epoch_time.emplace_back(std::stod(rinex_line.substr(13, 2))); epoch_time.emplace_back(std::stod(rinex_line.substr(16, 2))); epoch_time.emplace_back(std::stod(rinex_line.substr(18, 11))); gtime_t obs_time = epoch2time(&(epoch_time[0])); const int num_obs = std::stoi(rinex_line.substr(32, 3)); std::vector meas; for (int i = 0; i < num_obs; ++i) { LOG_IF(FATAL, !std::getline(obs_file, rinex_line)) << "Incomplete RINEX file"; ObsPtr obs = rinex_line2obs(rinex_line, sys2type); if (!obs || obs->freqs.empty()) continue; obs->time = obs_time; meas.emplace_back(obs); } rinex_meas.emplace_back(meas); } } void rinex2iono_params(const std::string &rinex_filepath, std::vector &iono_params) { iono_params.resize(8); std::ifstream file(rinex_filepath); std::string line; // check first line, mainly RINEX version if (!(std::getline(file, line) && line.find("RINEX VERSION") != std::string::npos && line.find("3.04") != std::string::npos)) { LOG(ERROR) << "Only RINEX 3.04 is supported"; return; } bool find_alpha = false, find_beta = false; while(std::getline(file, line)) { if (line.find("IONOSPHERIC CORR") != std::string::npos && line.find("GPSA") != std::string::npos) { // parse ion alpha value for (size_t i = 0; i < 4; ++i) { std::string value_str = line.substr(i*12+5, 12); iono_params[i] = str2double(value_str); } find_alpha = true; } else if (line.find("IONOSPHERIC CORR") != std::string::npos && line.find("GPSB") != std::string::npos) { // parse ion beta value for (size_t i = 0; i < 4; ++i) { std::string value_str = line.substr(i*12+5, 12); iono_params[i+4] = str2double(value_str); } find_beta = true; } if(find_alpha && find_beta) break; } file.close(); } int obs_index(const uint32_t sys, const double freq) { if (sys == SYS_GPS) { if (freq == FREQ1) return 0; else if (freq == FREQ2) return 1; } else if (sys == SYS_GLO) { if (freq <= (FREQ1_GLO+6*DFRQ1_GLO)) return 0; else if (freq <= (FREQ2_GLO+6*DFRQ2_GLO)) return 1; } else if (sys == SYS_GAL) { if (freq == FREQ1) return 0; else if (freq == FREQ7) return 1; } else if (sys == SYS_BDS) { if (freq == FREQ1_BDS) return 0; else if (freq == FREQ2_BDS) return 1; } return -1; } double index_freq(const ObsPtr &obs, const int band_idx, int &freq_idx) { freq_idx = -1; // set frequency index of `obs->freqs` invalid const uint32_t sys = satsys(obs->sat, NULL); // calculate target band frequency double freq_min = 0; double freq_max = 0; if (sys == SYS_GPS && band_idx == 0) { freq_min = freq_max = FREQ1; } else if (sys == SYS_GPS && band_idx == 1) { freq_min = freq_max = FREQ2; } else if (sys == SYS_GLO && band_idx == 0) { freq_min = FREQ1_GLO-7*DFRQ1_GLO; freq_max = FREQ1_GLO+6*DFRQ1_GLO; } else if (sys == SYS_GLO && band_idx == 1) { freq_min = FREQ2_GLO-7*DFRQ2_GLO; freq_max = FREQ2_GLO+6*DFRQ2_GLO; } else if (sys == SYS_GAL && band_idx == 0) { freq_min = freq_max = FREQ1; } else if (sys == SYS_GAL && band_idx == 1) { freq_min = freq_max = FREQ7; } else if (sys == SYS_BDS && band_idx == 0) { freq_min = freq_max = FREQ1_BDS; } else if (sys == SYS_BDS && band_idx == 1) { freq_min = freq_max = FREQ2_BDS; } // check if target frequency exists or not for (uint32_t i = 0; i < obs->freqs.size(); ++i) { if (obs->freqs[i] >= freq_min && obs->freqs[i] <= freq_max) { freq_idx = static_cast(i); return obs->freqs[i]; } } return -1.0; } std::map get_glo_freqo(const std::vector> &gnss_meas) { std::map sat2freqo; for (auto &meas : gnss_meas) { for (auto &obs : meas) { const uint32_t sys = satsys(obs->sat, NULL); if (sys != SYS_GLO) continue; for (double freq : obs->freqs) { if (freq > FREQ1_GLO-8*DFRQ1_GLO && freq < FREQ1_GLO+7*DFRQ1_GLO) { const int freqo = round((freq-FREQ1_GLO)/DFRQ1_GLO); sat2freqo.emplace(obs->sat, freqo); } else if (freq > FREQ2_GLO-8*DFRQ2_GLO && freq < FREQ2_GLO+7*DFRQ2_GLO) { const int freqo = round((freq-FREQ2_GLO)/DFRQ2_GLO); sat2freqo.emplace(obs->sat, freqo); } } } } return sat2freqo; } void obs2rinex(const std::string &rinex_filepath, const std::vector> &gnss_meas) { FILE *fp = fopen(rinex_filepath.c_str(), "w"); fprintf(fp, "%9.2f%-11s%-20s%-20s%-20s\n", 3.04, "", "OBSERVATION DATA", "M: Mixed", "RINEX VERSION / TYPE"); std::time_t time_ptr; time_ptr = time(NULL); tm *tm_utc = gmtime(&time_ptr); char date_str[256]; sprintf(date_str, "%4d%02d%02d %02d%02d%02d UTC", tm_utc->tm_year+1900, tm_utc->tm_mon+1, tm_utc->tm_mday, tm_utc->tm_hour, tm_utc->tm_min, tm_utc->tm_sec); fprintf(fp, "%-20.20s%-20.20s%-20.20s%-20s\n","gnss_comm obs2rinex", "", date_str, "PGM / RUN BY / DATE"); fprintf(fp, "%-60.60s%-20s\n", "", "MARKER NAME"); fprintf(fp, "%-20.20s%-40.40s%-20s\n", "", "", "MARKER NUMBER"); fprintf(fp, "%-20.20s%-40.40s%-20s\n", "", "", "MARKER TYPE"); fprintf(fp, "%-20.20s%-40.40s%-20s\n", "", "", "OBSERVER / AGENCY"); fprintf(fp, "%-20.20s%-20.20s%-20.20s%-20s\n", "", "", "", "REC # / TYPE / VERS"); fprintf(fp, "%-20.20s%-20.20s%-20.20s%-20s\n", "", "", "", "ANT # / TYPE"); // ignore approximate position fprintf(fp, "%-14.14s%-14.14s%-14.14s%-18s%-20s\n", "", "", "", "", "APPROX POSITION XYZ"); fprintf(fp, "%-14.14s%-14.14s%-14.14s%-18s%-20s\n", "", "", "", "", "ANTENNA: DELTA H/E/N"); // observation type, hard code here // only support dual-frequency record without the knowledge of signal type fprintf(fp, "%c %3d %3s %3s %3s %3s %3s %3s %3s %3s%20s %-20s\n", 'G', 8, "C1C", "L1C", "D1C", "S1C", "C2S", "L2S", "D2S", "S2S", "", "SYS / # / OBS TYPES "); fprintf(fp, "%c %3d %3s %3s %3s %3s %3s %3s %3s %3s%20s %-20s\n", 'R', 8, "C1C", "L1C", "D1C", "S1C", "C2C", "L2C", "D2C", "S2C", "", "SYS / # / OBS TYPES "); fprintf(fp, "%c %3d %3s %3s %3s %3s %3s %3s %3s %3s%20s %-20s\n", 'E', 8, "C1C", "L1C", "D1C", "S1C", "C7Q", "L7Q", "D7Q", "S7Q", "", "SYS / # / OBS TYPES "); fprintf(fp, "%c %3d %3s %3s %3s %3s %3s %3s %3s %3s%20s %-20s\n", 'C', 8, "C2I", "L2I", "D2I", "S2I", "C7I", "L7I", "D7I", "S7I", "", "SYS / # / OBS TYPES "); if (!gnss_meas.empty()) { int front_idx = -1; while(gnss_meas[++front_idx].empty()); const gtime_t start_time = gnss_meas[front_idx].front()->time; double ep[6]; time2epoch(start_time, ep); fprintf(fp, " %04.0f%6.0f%6.0f%6.0f%6.0f%13.7f %-12s%-20s\n", ep[0], ep[1], ep[2], ep[3], ep[4], ep[5], "GPS", "TIME OF FIRST OBS"); int back_idx = static_cast(gnss_meas.size()); while(gnss_meas[--back_idx].empty()); const gtime_t end_time = gnss_meas[back_idx].front()->time; time2epoch(end_time, ep); fprintf(fp, " %04.0f%6.0f%6.0f%6.0f%6.0f%13.7f %-12s%-20s\n", ep[0], ep[1], ep[2], ep[3], ep[4], ep[5], "GPS", "TIME OF LAST OBS"); } fprintf(fp,"%c %-58s%-20s\n",'G',"","SYS / PHASE SHIFT"); fprintf(fp,"%c %-58s%-20s\n",'R',"","SYS / PHASE SHIFT"); fprintf(fp,"%c %-58s%-20s\n",'E',"","SYS / PHASE SHIFT"); fprintf(fp,"%c %-58s%-20s\n",'C',"","SYS / PHASE SHIFT"); // add GLONASS SLOT / FRQ # std::map glo_sat2freqo = get_glo_freqo(gnss_meas); auto glo_sat2freqo_it = glo_sat2freqo.begin(); const int num_glo_sats = static_cast(glo_sat2freqo.size()); for (int i = 0; i < (num_glo_sats<=0?1:(num_glo_sats-1)/8+1); ++i) { if (i == 0) fprintf(fp, "%3d", num_glo_sats); else fprintf(fp, "%3s", ""); for (int j = 0; j < 8; ++j) { if (i*8+j < num_glo_sats) { fprintf(fp, " %3s %2d", sat2str(glo_sat2freqo_it->first).c_str(), glo_sat2freqo_it->second); ++ glo_sat2freqo_it; } else { fprintf(fp, " %6s", ""); } } fprintf(fp, " %-20s\n", "GLONASS SLOT / FRQ #"); } fprintf(fp," C1C 0.000 C1P 0.000 C2C 0.000 C2P 0.000 GLONASS COD/PHS/BIS \n"); fprintf(fp,"%-60.60s%-20s\n","","END OF HEADER"); // write data for (auto &meas : gnss_meas) { if (meas.empty()) continue; double ep[6]; time2epoch(meas[0]->time, ep); fprintf(fp, "> %04.0f %2.0f %2.0f %2.0f %2.0f%11.7f %d%3d%21s\n", ep[0], ep[1], ep[2], ep[3], ep[4], ep[5], 0, static_cast(meas.size()), ""); std::vector sorted_meas(meas); std::sort(sorted_meas.begin(), sorted_meas.end(), [](ObsPtr o1, ObsPtr o2) { return o1->sat < o2->sat; }); for (auto &obs : sorted_meas) { std::string sat_str = sat2str(obs->sat); fprintf(fp, "%3s", sat_str.c_str()); for (int k = 0; k < 2; ++k) { int freq_idx = -1; const double freq = index_freq(obs, k, freq_idx); if (freq < 0) { fprintf(fp, "%16s%16s%16s%16s", "", "", "", ""); } else { fprintf(fp, "%14.3f%2s", obs->psr[freq_idx], ""); fprintf(fp, "%14.3f%2s", obs->cp[freq_idx], ""); fprintf(fp, "%14.3f%2s", obs->dopp[freq_idx], ""); fprintf(fp, "%14.3f%2s", obs->CN0[freq_idx], ""); } } fprintf(fp, "\n"); } } fclose(fp); } } // namespace gnss_comm ================================================ FILE: ingvio_estimator/CMakeLists.txt ================================================ cmake_minimum_required(VERSION 3.16) project(ingvio_estimator) set(CMAKE_CXX_STANDARD 14) set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_CXX_EXTENSIONS OFF) set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_CURRENT_LIST_DIR}/cmake") if (NOT CMAKE_BUILD_TYPE) set(CMAKE_BUILD_TYPE "Release") endif () # set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3 -fsee -fomit-frame-pointer -fno-signed-zeros -fno-math-errno -funroll-loops") find_package(Eigen3 QUIET) if (NOT Eigen3_FOUND) set(EIGEN3_INCLUDE_DIR "/usr/include/eigen3") endif () find_package(OpenCV 3 QUIET) if (NOT OpenCV_FOUND) find_package(OpenCV 4 REQUIRED) endif () find_package(SuiteSparse REQUIRED) find_package(Boost REQUIRED COMPONENTS system filesystem thread date_time) find_package(catkin QUIET COMPONENTS roscpp) find_package(ament_cmake QUIET) if (catkin_FOUND) message(STATUS "ROS *1* version found!") include(${CMAKE_CURRENT_SOURCE_DIR}/cmake/ROS1.cmake) elseif (ament_cmake_FOUND) message(STATUS "ROS *2* version found! Not supported!") include(${CMAKE_CURRENT_SOURCE_DIR}/cmake/ROS2.cmake) else () message(STATUS "No ROS version found! Cannot build!") endif () ================================================ FILE: ingvio_estimator/LICENSE ================================================ GNU GENERAL PUBLIC LICENSE Version 3, 29 June 2007 Copyright (C) 2007 Free Software Foundation, Inc. Everyone is permitted to copy and distribute verbatim copies of this license document, but changing it is not allowed. Preamble The GNU General Public License is a free, copyleft license for software and other kinds of works. The licenses for most software and other practical works are designed to take away your freedom to share and change the works. By contrast, the GNU General Public License is intended to guarantee your freedom to share and change all versions of a program--to make sure it remains free software for all its users. We, the Free Software Foundation, use the GNU General Public License for most of our software; it applies also to any other work released this way by its authors. You can apply it to your programs, too. When we speak of free software, we are referring to freedom, not price. Our General Public Licenses are designed to make sure that you have the freedom to distribute copies of free software (and charge for them if you wish), that you receive source code or can get it if you want it, that you can change the software or use pieces of it in new free programs, and that you know you can do these things. To protect your rights, we need to prevent others from denying you these rights or asking you to surrender the rights. Therefore, you have certain responsibilities if you distribute copies of the software, or if you modify it: responsibilities to respect the freedom of others. For example, if you distribute copies of such a program, whether gratis or for a fee, you must pass on to the recipients the same freedoms that you received. You must make sure that they, too, receive or can get the source code. And you must show them these terms so they know their rights. Developers that use the GNU GPL protect your rights with two steps: (1) assert copyright on the software, and (2) offer you this License giving you legal permission to copy, distribute and/or modify it. For the developers' and authors' protection, the GPL clearly explains that there is no warranty for this free software. For both users' and authors' sake, the GPL requires that modified versions be marked as changed, so that their problems will not be attributed erroneously to authors of previous versions. Some devices are designed to deny users access to install or run modified versions of the software inside them, although the manufacturer can do so. This is fundamentally incompatible with the aim of protecting users' freedom to change the software. The systematic pattern of such abuse occurs in the area of products for individuals to use, which is precisely where it is most unacceptable. Therefore, we have designed this version of the GPL to prohibit the practice for those products. If such problems arise substantially in other domains, we stand ready to extend this provision to those domains in future versions of the GPL, as needed to protect the freedom of users. Finally, every program is threatened constantly by software patents. States should not allow patents to restrict development and use of software on general-purpose computers, but in those that do, we wish to avoid the special danger that patents applied to a free program could make it effectively proprietary. To prevent this, the GPL assures that patents cannot be used to render the program non-free. The precise terms and conditions for copying, distribution and modification follow. TERMS AND CONDITIONS 0. Definitions. "This License" refers to version 3 of the GNU General Public License. "Copyright" also means copyright-like laws that apply to other kinds of works, such as semiconductor masks. "The Program" refers to any copyrightable work licensed under this License. Each licensee is addressed as "you". "Licensees" and "recipients" may be individuals or organizations. To "modify" a work means to copy from or adapt all or part of the work in a fashion requiring copyright permission, other than the making of an exact copy. The resulting work is called a "modified version" of the earlier work or a work "based on" the earlier work. A "covered work" means either the unmodified Program or a work based on the Program. To "propagate" a work means to do anything with it that, without permission, would make you directly or secondarily liable for infringement under applicable copyright law, except executing it on a computer or modifying a private copy. Propagation includes copying, distribution (with or without modification), making available to the public, and in some countries other activities as well. To "convey" a work means any kind of propagation that enables other parties to make or receive copies. Mere interaction with a user through a computer network, with no transfer of a copy, is not conveying. An interactive user interface displays "Appropriate Legal Notices" to the extent that it includes a convenient and prominently visible feature that (1) displays an appropriate copyright notice, and (2) tells the user that there is no warranty for the work (except to the extent that warranties are provided), that licensees may convey the work under this License, and how to view a copy of this License. If the interface presents a list of user commands or options, such as a menu, a prominent item in the list meets this criterion. 1. Source Code. The "source code" for a work means the preferred form of the work for making modifications to it. "Object code" means any non-source form of a work. A "Standard Interface" means an interface that either is an official standard defined by a recognized standards body, or, in the case of interfaces specified for a particular programming language, one that is widely used among developers working in that language. The "System Libraries" of an executable work include anything, other than the work as a whole, that (a) is included in the normal form of packaging a Major Component, but which is not part of that Major Component, and (b) serves only to enable use of the work with that Major Component, or to implement a Standard Interface for which an implementation is available to the public in source code form. A "Major Component", in this context, means a major essential component (kernel, window system, and so on) of the specific operating system (if any) on which the executable work runs, or a compiler used to produce the work, or an object code interpreter used to run it. The "Corresponding Source" for a work in object code form means all the source code needed to generate, install, and (for an executable work) run the object code and to modify the work, including scripts to control those activities. However, it does not include the work's System Libraries, or general-purpose tools or generally available free programs which are used unmodified in performing those activities but which are not part of the work. For example, Corresponding Source includes interface definition files associated with source files for the work, and the source code for shared libraries and dynamically linked subprograms that the work is specifically designed to require, such as by intimate data communication or control flow between those subprograms and other parts of the work. The Corresponding Source need not include anything that users can regenerate automatically from other parts of the Corresponding Source. The Corresponding Source for a work in source code form is that same work. 2. Basic Permissions. All rights granted under this License are granted for the term of copyright on the Program, and are irrevocable provided the stated conditions are met. This License explicitly affirms your unlimited permission to run the unmodified Program. The output from running a covered work is covered by this License only if the output, given its content, constitutes a covered work. This License acknowledges your rights of fair use or other equivalent, as provided by copyright law. You may make, run and propagate covered works that you do not convey, without conditions so long as your license otherwise remains in force. You may convey covered works to others for the sole purpose of having them make modifications exclusively for you, or provide you with facilities for running those works, provided that you comply with the terms of this License in conveying all material for which you do not control copyright. Those thus making or running the covered works for you must do so exclusively on your behalf, under your direction and control, on terms that prohibit them from making any copies of your copyrighted material outside their relationship with you. Conveying under any other circumstances is permitted solely under the conditions stated below. Sublicensing is not allowed; section 10 makes it unnecessary. 3. Protecting Users' Legal Rights From Anti-Circumvention Law. No covered work shall be deemed part of an effective technological measure under any applicable law fulfilling obligations under article 11 of the WIPO copyright treaty adopted on 20 December 1996, or similar laws prohibiting or restricting circumvention of such measures. When you convey a covered work, you waive any legal power to forbid circumvention of technological measures to the extent such circumvention is effected by exercising rights under this License with respect to the covered work, and you disclaim any intention to limit operation or modification of the work as a means of enforcing, against the work's users, your or third parties' legal rights to forbid circumvention of technological measures. 4. Conveying Verbatim Copies. You may convey verbatim copies of the Program's source code as you receive it, in any medium, provided that you conspicuously and appropriately publish on each copy an appropriate copyright notice; keep intact all notices stating that this License and any non-permissive terms added in accord with section 7 apply to the code; keep intact all notices of the absence of any warranty; and give all recipients a copy of this License along with the Program. You may charge any price or no price for each copy that you convey, and you may offer support or warranty protection for a fee. 5. Conveying Modified Source Versions. You may convey a work based on the Program, or the modifications to produce it from the Program, in the form of source code under the terms of section 4, provided that you also meet all of these conditions: a) The work must carry prominent notices stating that you modified it, and giving a relevant date. b) The work must carry prominent notices stating that it is released under this License and any conditions added under section 7. This requirement modifies the requirement in section 4 to "keep intact all notices". c) You must license the entire work, as a whole, under this License to anyone who comes into possession of a copy. This License will therefore apply, along with any applicable section 7 additional terms, to the whole of the work, and all its parts, regardless of how they are packaged. This License gives no permission to license the work in any other way, but it does not invalidate such permission if you have separately received it. d) If the work has interactive user interfaces, each must display Appropriate Legal Notices; however, if the Program has interactive interfaces that do not display Appropriate Legal Notices, your work need not make them do so. A compilation of a covered work with other separate and independent works, which are not by their nature extensions of the covered work, and which are not combined with it such as to form a larger program, in or on a volume of a storage or distribution medium, is called an "aggregate" if the compilation and its resulting copyright are not used to limit the access or legal rights of the compilation's users beyond what the individual works permit. Inclusion of a covered work in an aggregate does not cause this License to apply to the other parts of the aggregate. 6. Conveying Non-Source Forms. You may convey a covered work in object code form under the terms of sections 4 and 5, provided that you also convey the machine-readable Corresponding Source under the terms of this License, in one of these ways: a) Convey the object code in, or embodied in, a physical product (including a physical distribution medium), accompanied by the Corresponding Source fixed on a durable physical medium customarily used for software interchange. b) Convey the object code in, or embodied in, a physical product (including a physical distribution medium), accompanied by a written offer, valid for at least three years and valid for as long as you offer spare parts or customer support for that product model, to give anyone who possesses the object code either (1) a copy of the Corresponding Source for all the software in the product that is covered by this License, on a durable physical medium customarily used for software interchange, for a price no more than your reasonable cost of physically performing this conveying of source, or (2) access to copy the Corresponding Source from a network server at no charge. c) Convey individual copies of the object code with a copy of the written offer to provide the Corresponding Source. This alternative is allowed only occasionally and noncommercially, and only if you received the object code with such an offer, in accord with subsection 6b. d) Convey the object code by offering access from a designated place (gratis or for a charge), and offer equivalent access to the Corresponding Source in the same way through the same place at no further charge. You need not require recipients to copy the Corresponding Source along with the object code. If the place to copy the object code is a network server, the Corresponding Source may be on a different server (operated by you or a third party) that supports equivalent copying facilities, provided you maintain clear directions next to the object code saying where to find the Corresponding Source. Regardless of what server hosts the Corresponding Source, you remain obligated to ensure that it is available for as long as needed to satisfy these requirements. e) Convey the object code using peer-to-peer transmission, provided you inform other peers where the object code and Corresponding Source of the work are being offered to the general public at no charge under subsection 6d. A separable portion of the object code, whose source code is excluded from the Corresponding Source as a System Library, need not be included in conveying the object code work. A "User Product" is either (1) a "consumer product", which means any tangible personal property which is normally used for personal, family, or household purposes, or (2) anything designed or sold for incorporation into a dwelling. In determining whether a product is a consumer product, doubtful cases shall be resolved in favor of coverage. For a particular product received by a particular user, "normally used" refers to a typical or common use of that class of product, regardless of the status of the particular user or of the way in which the particular user actually uses, or expects or is expected to use, the product. A product is a consumer product regardless of whether the product has substantial commercial, industrial or non-consumer uses, unless such uses represent the only significant mode of use of the product. "Installation Information" for a User Product means any methods, procedures, authorization keys, or other information required to install and execute modified versions of a covered work in that User Product from a modified version of its Corresponding Source. The information must suffice to ensure that the continued functioning of the modified object code is in no case prevented or interfered with solely because modification has been made. If you convey an object code work under this section in, or with, or specifically for use in, a User Product, and the conveying occurs as part of a transaction in which the right of possession and use of the User Product is transferred to the recipient in perpetuity or for a fixed term (regardless of how the transaction is characterized), the Corresponding Source conveyed under this section must be accompanied by the Installation Information. But this requirement does not apply if neither you nor any third party retains the ability to install modified object code on the User Product (for example, the work has been installed in ROM). The requirement to provide Installation Information does not include a requirement to continue to provide support service, warranty, or updates for a work that has been modified or installed by the recipient, or for the User Product in which it has been modified or installed. Access to a network may be denied when the modification itself materially and adversely affects the operation of the network or violates the rules and protocols for communication across the network. Corresponding Source conveyed, and Installation Information provided, in accord with this section must be in a format that is publicly documented (and with an implementation available to the public in source code form), and must require no special password or key for unpacking, reading or copying. 7. Additional Terms. "Additional permissions" are terms that supplement the terms of this License by making exceptions from one or more of its conditions. Additional permissions that are applicable to the entire Program shall be treated as though they were included in this License, to the extent that they are valid under applicable law. If additional permissions apply only to part of the Program, that part may be used separately under those permissions, but the entire Program remains governed by this License without regard to the additional permissions. When you convey a copy of a covered work, you may at your option remove any additional permissions from that copy, or from any part of it. (Additional permissions may be written to require their own removal in certain cases when you modify the work.) You may place additional permissions on material, added by you to a covered work, for which you have or can give appropriate copyright permission. Notwithstanding any other provision of this License, for material you add to a covered work, you may (if authorized by the copyright holders of that material) supplement the terms of this License with terms: a) Disclaiming warranty or limiting liability differently from the terms of sections 15 and 16 of this License; or b) Requiring preservation of specified reasonable legal notices or author attributions in that material or in the Appropriate Legal Notices displayed by works containing it; or c) Prohibiting misrepresentation of the origin of that material, or requiring that modified versions of such material be marked in reasonable ways as different from the original version; or d) Limiting the use for publicity purposes of names of licensors or authors of the material; or e) Declining to grant rights under trademark law for use of some trade names, trademarks, or service marks; or f) Requiring indemnification of licensors and authors of that material by anyone who conveys the material (or modified versions of it) with contractual assumptions of liability to the recipient, for any liability that these contractual assumptions directly impose on those licensors and authors. All other non-permissive additional terms are considered "further restrictions" within the meaning of section 10. If the Program as you received it, or any part of it, contains a notice stating that it is governed by this License along with a term that is a further restriction, you may remove that term. If a license document contains a further restriction but permits relicensing or conveying under this License, you may add to a covered work material governed by the terms of that license document, provided that the further restriction does not survive such relicensing or conveying. If you add terms to a covered work in accord with this section, you must place, in the relevant source files, a statement of the additional terms that apply to those files, or a notice indicating where to find the applicable terms. Additional terms, permissive or non-permissive, may be stated in the form of a separately written license, or stated as exceptions; the above requirements apply either way. 8. Termination. You may not propagate or modify a covered work except as expressly provided under this License. Any attempt otherwise to propagate or modify it is void, and will automatically terminate your rights under this License (including any patent licenses granted under the third paragraph of section 11). However, if you cease all violation of this License, then your license from a particular copyright holder is reinstated (a) provisionally, unless and until the copyright holder explicitly and finally terminates your license, and (b) permanently, if the copyright holder fails to notify you of the violation by some reasonable means prior to 60 days after the cessation. Moreover, your license from a particular copyright holder is reinstated permanently if the copyright holder notifies you of the violation by some reasonable means, this is the first time you have received notice of violation of this License (for any work) from that copyright holder, and you cure the violation prior to 30 days after your receipt of the notice. Termination of your rights under this section does not terminate the licenses of parties who have received copies or rights from you under this License. If your rights have been terminated and not permanently reinstated, you do not qualify to receive new licenses for the same material under section 10. 9. Acceptance Not Required for Having Copies. You are not required to accept this License in order to receive or run a copy of the Program. Ancillary propagation of a covered work occurring solely as a consequence of using peer-to-peer transmission to receive a copy likewise does not require acceptance. However, nothing other than this License grants you permission to propagate or modify any covered work. These actions infringe copyright if you do not accept this License. Therefore, by modifying or propagating a covered work, you indicate your acceptance of this License to do so. 10. Automatic Licensing of Downstream Recipients. Each time you convey a covered work, the recipient automatically receives a license from the original licensors, to run, modify and propagate that work, subject to this License. You are not responsible for enforcing compliance by third parties with this License. An "entity transaction" is a transaction transferring control of an organization, or substantially all assets of one, or subdividing an organization, or merging organizations. If propagation of a covered work results from an entity transaction, each party to that transaction who receives a copy of the work also receives whatever licenses to the work the party's predecessor in interest had or could give under the previous paragraph, plus a right to possession of the Corresponding Source of the work from the predecessor in interest, if the predecessor has it or can get it with reasonable efforts. You may not impose any further restrictions on the exercise of the rights granted or affirmed under this License. For example, you may not impose a license fee, royalty, or other charge for exercise of rights granted under this License, and you may not initiate litigation (including a cross-claim or counterclaim in a lawsuit) alleging that any patent claim is infringed by making, using, selling, offering for sale, or importing the Program or any portion of it. 11. Patents. A "contributor" is a copyright holder who authorizes use under this License of the Program or a work on which the Program is based. The work thus licensed is called the contributor's "contributor version". A contributor's "essential patent claims" are all patent claims owned or controlled by the contributor, whether already acquired or hereafter acquired, that would be infringed by some manner, permitted by this License, of making, using, or selling its contributor version, but do not include claims that would be infringed only as a consequence of further modification of the contributor version. For purposes of this definition, "control" includes the right to grant patent sublicenses in a manner consistent with the requirements of this License. Each contributor grants you a non-exclusive, worldwide, royalty-free patent license under the contributor's essential patent claims, to make, use, sell, offer for sale, import and otherwise run, modify and propagate the contents of its contributor version. In the following three paragraphs, a "patent license" is any express agreement or commitment, however denominated, not to enforce a patent (such as an express permission to practice a patent or covenant not to sue for patent infringement). To "grant" such a patent license to a party means to make such an agreement or commitment not to enforce a patent against the party. If you convey a covered work, knowingly relying on a patent license, and the Corresponding Source of the work is not available for anyone to copy, free of charge and under the terms of this License, through a publicly available network server or other readily accessible means, then you must either (1) cause the Corresponding Source to be so available, or (2) arrange to deprive yourself of the benefit of the patent license for this particular work, or (3) arrange, in a manner consistent with the requirements of this License, to extend the patent license to downstream recipients. "Knowingly relying" means you have actual knowledge that, but for the patent license, your conveying the covered work in a country, or your recipient's use of the covered work in a country, would infringe one or more identifiable patents in that country that you have reason to believe are valid. If, pursuant to or in connection with a single transaction or arrangement, you convey, or propagate by procuring conveyance of, a covered work, and grant a patent license to some of the parties receiving the covered work authorizing them to use, propagate, modify or convey a specific copy of the covered work, then the patent license you grant is automatically extended to all recipients of the covered work and works based on it. A patent license is "discriminatory" if it does not include within the scope of its coverage, prohibits the exercise of, or is conditioned on the non-exercise of one or more of the rights that are specifically granted under this License. You may not convey a covered work if you are a party to an arrangement with a third party that is in the business of distributing software, under which you make payment to the third party based on the extent of your activity of conveying the work, and under which the third party grants, to any of the parties who would receive the covered work from you, a discriminatory patent license (a) in connection with copies of the covered work conveyed by you (or copies made from those copies), or (b) primarily for and in connection with specific products or compilations that contain the covered work, unless you entered into that arrangement, or that patent license was granted, prior to 28 March 2007. Nothing in this License shall be construed as excluding or limiting any implied license or other defenses to infringement that may otherwise be available to you under applicable patent law. 12. No Surrender of Others' Freedom. If conditions are imposed on you (whether by court order, agreement or otherwise) that contradict the conditions of this License, they do not excuse you from the conditions of this License. If you cannot convey a covered work so as to satisfy simultaneously your obligations under this License and any other pertinent obligations, then as a consequence you may not convey it at all. For example, if you agree to terms that obligate you to collect a royalty for further conveying from those to whom you convey the Program, the only way you could satisfy both those terms and this License would be to refrain entirely from conveying the Program. 13. Use with the GNU Affero General Public License. Notwithstanding any other provision of this License, you have permission to link or combine any covered work with a work licensed under version 3 of the GNU Affero General Public License into a single combined work, and to convey the resulting work. The terms of this License will continue to apply to the part which is the covered work, but the special requirements of the GNU Affero General Public License, section 13, concerning interaction through a network will apply to the combination as such. 14. Revised Versions of this License. The Free Software Foundation may publish revised and/or new versions of the GNU General Public License from time to time. Such new versions will be similar in spirit to the present version, but may differ in detail to address new problems or concerns. Each version is given a distinguishing version number. If the Program specifies that a certain numbered version of the GNU General Public License "or any later version" applies to it, you have the option of following the terms and conditions either of that numbered version or of any later version published by the Free Software Foundation. If the Program does not specify a version number of the GNU General Public License, you may choose any version ever published by the Free Software Foundation. If the Program specifies that a proxy can decide which future versions of the GNU General Public License can be used, that proxy's public statement of acceptance of a version permanently authorizes you to choose that version for the Program. Later license versions may give you additional or different permissions. However, no additional obligations are imposed on any author or copyright holder as a result of your choosing to follow a later version. 15. Disclaimer of Warranty. THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM IS WITH YOU. SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF ALL NECESSARY SERVICING, REPAIR OR CORRECTION. 16. Limitation of Liability. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS), EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH DAMAGES. 17. Interpretation of Sections 15 and 16. If the disclaimer of warranty and limitation of liability provided above cannot be given local legal effect according to their terms, reviewing courts shall apply local law that most closely approximates an absolute waiver of all civil liability in connection with the Program, unless a warranty or assumption of liability accompanies a copy of the Program in return for a fee. END OF TERMS AND CONDITIONS How to Apply These Terms to Your New Programs If you develop a new program, and you want it to be of the greatest possible use to the public, the best way to achieve this is to make it free software which everyone can redistribute and change under these terms. To do so, attach the following notices to the program. It is safest to attach them to the start of each source file to most effectively state the exclusion of warranty; and each file should have at least the "copyright" line and a pointer to where the full notice is found. Copyright (C) This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version. This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with this program. If not, see . Also add information on how to contact you by electronic and paper mail. If the program does terminal interaction, make it output a short notice like this when it starts in an interactive mode: Copyright (C) This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'. This is free software, and you are welcome to redistribute it under certain conditions; type `show c' for details. The hypothetical commands `show w' and `show c' should show the appropriate parts of the General Public License. Of course, your program's commands might be different; for a GUI interface, you would use an "about box". You should also get your employer (if you work as a programmer) or school, if any, to sign a "copyright disclaimer" for the program, if necessary. For more information on this, and how to apply and follow the GNU GPL, see . The GNU General Public License does not permit incorporating your program into proprietary programs. If your program is a subroutine library, you may consider it more useful to permit linking proprietary applications with the library. If this is what you want to do, use the GNU Lesser General Public License instead of this License. But first, please read . ================================================ FILE: ingvio_estimator/README.md ================================================ # InGVIO An invariant filter for visual-inertial-raw GNSS fusion. **Paper:** InGVIO: A Consistent Invariant Filter For Fast and High-Accuracy GNSS-Visual-Inertial Odometry **Paper Author:** Changwu Liu, Chen Jiang and Haowen Wang **Paper Status:** Manuscript submitted to IEEE RA-L for possible publication. Preprint version available on ArXiv. **Current Paper Link:** https://arxiv.org/abs/2210.15145 InGVIO is an invariant filter approach for fusion of monocular/stereo camera, IMU and raw GNSS measurements including pseudo ranges and Doppler shifts. InGVIO is intrinsically consistent under conditional infinitesimal invariance of the GNSS-Visual-Inertial system. InGVIO has the following key features: (a) fast due to decoupled IMU propagation, key-frame marginalization strategy and no SLAM-features; (b) accurate due to intrinsic consistency maintenance; (c) better convergence properties than 'naive' EKF-based filters. Moreover, we offer our fixed-wing datasets in the form of ROS Bags including stereo-visual, IMU and raw-GNSS measurements. **Fixed-Wing Dataset Link:** https://cloud.tsinghua.edu.cn/d/4fd9b8a81e0f4186a722 password to extract: lcw18_thu_uav The links to the datasets will be continuously updated. The config files for this dataset are contained in the InGVIO code configuration in path 'config/fw_zed2i_f9p'. ## 1. System Requirements ### 1.1 Support of C++ 14 Features The compiler should at least support c++14 standards. ### 1.2 ROS-Noetic System InGVIO is developed under [ROS-Noetic](http://wiki.ros.org/noetic/Installation/Ubuntu) with its default OpenCV4 library. However, InGVIO should be working on ROS-Melodic with OpenCV3. In the future, we may add support to ROS 2. ### 1.3 Eigen Library Eigen is a fantastic matrix computation library. InGVIO is developed under [Eigen3.3.7](https://eigen.tuxfamily.org/index.php?title=Main_Page). Other Eigen 3 versions should be OK for InGVIO. ### 1.4 SuiteSparse Library We use [SuiteSparse](https://github.com/DrTimothyAldenDavis/SuiteSparse/releases) Library for sparse QR-decomposition in visual updates. ### 1.5 gnss_comm Library A wrapper for GNSS messages in ROS. See [gnss_comm](https://github.com/HKUST-Aerial-Robotics/gnss_comm). The fantastic optimization-based work [GVINS](https://github.com/HKUST-Aerial-Robotics/GVINS) also relies on this library. We reserve a copy of gnss_comm in this repo. ## 2. Build InGVIO Download or clone this repo to your ROS workspace. ``` cd ~/ws_catkin catkin_make ``` Source the setup file to let ROS recognize the related launch files. ``` source devel/setup.bash ``` ## 3. Run with Our Fixed-Wing Datasets First download and unzip our fixed-wing dataset to your environment. The 'fw_zed2i_f9p' folder contains the configuration files for fixed-wing datasets. To run InGVIO in your environment, the following should be conducted first. Please adjust and modify the paths in 'config/fw_zed2i_f9p/ingvio_mono.yaml' and 'config/fw_zed2i_f9p/ingvio_stereo.yaml' to the case in your environment. If you want to record the output topics, please set your output dir in 'ingvio/launch/ingvio_mono, stereo_fw_record.launch' files. To run with monocular camera, please enter: ``` roslaunch ingvio_estimator ingvio_mono_fw.launch ``` For stereo-case, please enter: ``` roslaunch ingvio_estimator ingvio_stereo_fw.launch ``` To record the output topics, you can directly use our script file by: ``` roslaunch ingvio_estimator ingvio_mono_fw_record.launch roslaunch ingvio_estimator ingvio_stereo_fw_record.launch ``` Rviz will be automatically launched. Run the ROS Bag to see the results: ``` rosbag play fw_easy.bag --pause ``` ## 4. Run with GVINS Public Datasets ### 4.1 Configuration Settings The GVINS datasets can be acquired from [GitHub - HKUST-Aerial-Robotics/GVINS-Dataset](https://github.com/HKUST-Aerial-Robotics/GVINS-Dataset). The config files for GVINS Datasets are already integrated in 'config/sportsfield'. It's valid for all 3 datasets provided by GVINS. To run InGVIO in your environment, the following should be conducted first. Please adjust and modify the paths in 'config/sports_field/ingvio_mono.yaml' and 'config/sports_field/ingvio_stereo.yaml' to the case in your environment. If you want to record the output topics, please set your output dir in 'ingvio/launch/ingvio_mono, stereo_sf_record.launch' files. To run with monocular camera, please enter: ``` roslaunch ingvio_estimator ingvio_mono_sf.launch ``` For stereo-case, please enter: ``` roslaunch ingvio_estimator ingvio_stereo_sf.launch ``` To record the output topics, you can directly use our script file by: ``` roslaunch ingvio_estimator ingvio_mono_fw_record.launch roslaunch ingvio_estimator ingvio_stereo_fw_record.launch ``` Rviz will be automatically launched. Run the ROS Bag to see the results: ``` rosbag play sports_field.bag --pause rosbag play indoors_outdoors.bag --pause rosbag play urban_driving.bag --pause ``` ### 4.2 Parameter Tuning Hints **For sports_field.bag:** Monocular/Stereo Case: max_pts_frame = 150/110, visual_noise = 0.12, gnss_chi2_test = false, imu_buffer_size = 3000. **For urban_driving.bag:** Monocular/Stereo Case: max_pts_frame = 150/110, visual_noise = 0.12, gnss_chi2_test = true, imu_buffer_size = 3000. To avoid polluted GNSS measurements, a stronger version could be gnss_chi2_test = false, gnss_strong_reject = true. **For indoors_outdoors.bag:** Monocular/Stereo Case: max_pts_frame = 150/110, visual_noise = 0.18, gnss_chi2_test = false, gnss_strong_reject = true. Please modify or try other parameters if the above behave not well. Good parameters may be different under different environment settings. ## 5. Acknowledgements The realization of type-based index system in filter framework is inspired by [OpenVINS](https://github.com/rpng/open_vins). The author himself has learned lots of programming skills in SLAM by reading the codes of OpenVINS. The [gnss_comm](https://github.com/HKUST-Aerial-Robotics/gnss_comm) provided by GVINS is a great wrapper in developing codes involving raw GNSS measurements in ROS. ## 6. License This software is open-sourced under GPLv3 license. See [GNU GPL v3.0 - GNU](https://www.gnu.org/licenses/gpl-3.0.html). Please cite our paper if you use either our code or our fixed-wing datasets. ================================================ FILE: ingvio_estimator/cmake/FindSuiteSparse.cmake ================================================ # Ceres Solver - A fast non-linear least squares minimizer # Copyright 2015 Google Inc. All rights reserved. # http://ceres-solver.org/ # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: # # * Redistributions of source code must retain the above copyright notice, # this list of conditions and the following disclaimer. # * Redistributions in binary form must reproduce the above copyright notice, # this list of conditions and the following disclaimer in the documentation # and/or other materials provided with the distribution. # * Neither the name of Google Inc. nor the names of its contributors may be # used to endorse or promote products derived from this software without # specific prior written permission. # # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. # # Author: alexs.mac@gmail.com (Alex Stewart) # # FindSuiteSparse.cmake - Find SuiteSparse libraries & dependencies. # # This module defines the following variables: # # SUITESPARSE_FOUND: TRUE iff SuiteSparse and all dependencies have been found. # SUITESPARSE_INCLUDE_DIRS: Include directories for all SuiteSparse components. # SUITESPARSE_LIBRARIES: Libraries for all SuiteSparse component libraries and # dependencies. # SUITESPARSE_VERSION: Extracted from UFconfig.h (<= v3) or # SuiteSparse_config.h (>= v4). # SUITESPARSE_MAIN_VERSION: Equal to 4 if SUITESPARSE_VERSION = 4.2.1 # SUITESPARSE_SUB_VERSION: Equal to 2 if SUITESPARSE_VERSION = 4.2.1 # SUITESPARSE_SUBSUB_VERSION: Equal to 1 if SUITESPARSE_VERSION = 4.2.1 # # SUITESPARSE_IS_BROKEN_SHARED_LINKING_UBUNTU_SYSTEM_VERSION: TRUE iff running # on Ubuntu, SUITESPARSE_VERSION is 3.4.0 and found SuiteSparse is a system # install, in which case found version of SuiteSparse cannot be used to link # a shared library due to a bug (static linking is unaffected). # # The following variables control the behaviour of this module: # # SUITESPARSE_INCLUDE_DIR_HINTS: List of additional directories in which to # search for SuiteSparse includes, # e.g: /timbuktu/include. # SUITESPARSE_LIBRARY_DIR_HINTS: List of additional directories in which to # search for SuiteSparse libraries, # e.g: /timbuktu/lib. # # The following variables define the presence / includes & libraries for the # SuiteSparse components searched for, the SUITESPARSE_XX variables are the # union of the variables for all components. # # == Symmetric Approximate Minimum Degree (AMD) # AMD_FOUND # AMD_INCLUDE_DIR # AMD_LIBRARY # # == Constrained Approximate Minimum Degree (CAMD) # CAMD_FOUND # CAMD_INCLUDE_DIR # CAMD_LIBRARY # # == Column Approximate Minimum Degree (COLAMD) # COLAMD_FOUND # COLAMD_INCLUDE_DIR # COLAMD_LIBRARY # # Constrained Column Approximate Minimum Degree (CCOLAMD) # CCOLAMD_FOUND # CCOLAMD_INCLUDE_DIR # CCOLAMD_LIBRARY # # == Sparse Supernodal Cholesky Factorization and Update/Downdate (CHOLMOD) # CHOLMOD_FOUND # CHOLMOD_INCLUDE_DIR # CHOLMOD_LIBRARY # # == Multifrontal Sparse QR (SuiteSparseQR) # SUITESPARSEQR_FOUND # SUITESPARSEQR_INCLUDE_DIR # SUITESPARSEQR_LIBRARY # # == Common configuration for all but CSparse (SuiteSparse version >= 4). # SUITESPARSE_CONFIG_FOUND # SUITESPARSE_CONFIG_INCLUDE_DIR # SUITESPARSE_CONFIG_LIBRARY # # == Common configuration for all but CSparse (SuiteSparse version < 4). # UFCONFIG_FOUND # UFCONFIG_INCLUDE_DIR # # Optional SuiteSparse Dependencies: # # == Serial Graph Partitioning and Fill-reducing Matrix Ordering (METIS) # METIS_FOUND # METIS_LIBRARY # # == Intel Thread Building Blocks (TBB) # TBB_FOUND # TBB_LIBRARY # TBB_MALLOC_FOUND # TBB_MALLOC_LIBRARY # Reset CALLERS_CMAKE_FIND_LIBRARY_PREFIXES to its value when # FindSuiteSparse was invoked. macro(SUITESPARSE_RESET_FIND_LIBRARY_PREFIX) if (MSVC) set(CMAKE_FIND_LIBRARY_PREFIXES "${CALLERS_CMAKE_FIND_LIBRARY_PREFIXES}") endif (MSVC) endmacro(SUITESPARSE_RESET_FIND_LIBRARY_PREFIX) # Called if we failed to find SuiteSparse or any of it's required dependencies, # unsets all public (designed to be used externally) variables and reports # error message at priority depending upon [REQUIRED/QUIET/] argument. macro(SUITESPARSE_REPORT_NOT_FOUND REASON_MSG) unset(SUITESPARSE_FOUND) unset(SUITESPARSE_INCLUDE_DIRS) unset(SUITESPARSE_LIBRARIES) unset(SUITESPARSE_VERSION) unset(SUITESPARSE_MAIN_VERSION) unset(SUITESPARSE_SUB_VERSION) unset(SUITESPARSE_SUBSUB_VERSION) # Do NOT unset SUITESPARSE_FOUND_REQUIRED_VARS here, as it is used by # FindPackageHandleStandardArgs() to generate the automatic error message on # failure which highlights which components are missing. suitesparse_reset_find_library_prefix() # Note _FIND_[REQUIRED/QUIETLY] variables defined by FindPackage() # use the camelcase library name, not uppercase. if (SuiteSparse_FIND_QUIETLY) message(STATUS "Failed to find SuiteSparse - " ${REASON_MSG} ${ARGN}) elseif (SuiteSparse_FIND_REQUIRED) message(FATAL_ERROR "Failed to find SuiteSparse - " ${REASON_MSG} ${ARGN}) else() # Neither QUIETLY nor REQUIRED, use no priority which emits a message # but continues configuration and allows generation. message("-- Failed to find SuiteSparse - " ${REASON_MSG} ${ARGN}) endif (SuiteSparse_FIND_QUIETLY) # Do not call return(), s/t we keep processing if not called with REQUIRED # and report all missing components, rather than bailing after failing to find # the first. endmacro(SUITESPARSE_REPORT_NOT_FOUND) # Protect against any alternative find_package scripts for this library having # been called previously (in a client project) which set SUITESPARSE_FOUND, but # not the other variables we require / set here which could cause the search # logic here to fail. unset(SUITESPARSE_FOUND) # Handle possible presence of lib prefix for libraries on MSVC, see # also SUITESPARSE_RESET_FIND_LIBRARY_PREFIX(). if (MSVC) # Preserve the caller's original values for CMAKE_FIND_LIBRARY_PREFIXES # s/t we can set it back before returning. set(CALLERS_CMAKE_FIND_LIBRARY_PREFIXES "${CMAKE_FIND_LIBRARY_PREFIXES}") # The empty string in this list is important, it represents the case when # the libraries have no prefix (shared libraries / DLLs). set(CMAKE_FIND_LIBRARY_PREFIXES "lib" "" "${CMAKE_FIND_LIBRARY_PREFIXES}") endif (MSVC) # Specify search directories for include files and libraries (this is the union # of the search directories for all OSs). Search user-specified hint # directories first if supplied, and search user-installed locations first # so that we prefer user installs to system installs where both exist. list(APPEND SUITESPARSE_CHECK_INCLUDE_DIRS /opt/local/include /opt/local/include/ufsparse # Mac OS X /usr/local/homebrew/include # Mac OS X /usr/local/include /usr/include) list(APPEND SUITESPARSE_CHECK_LIBRARY_DIRS /opt/local/lib /opt/local/lib/ufsparse # Mac OS X /usr/local/homebrew/lib # Mac OS X /usr/local/lib /usr/lib) # Additional suffixes to try appending to each search path. list(APPEND SUITESPARSE_CHECK_PATH_SUFFIXES suitesparse) # Windows/Ubuntu # Wrappers to find_path/library that pass the SuiteSparse search hints/paths. # # suitesparse_find_component( [FILES name1 [name2 ...]] # [LIBRARIES name1 [name2 ...]] # [REQUIRED]) macro(suitesparse_find_component COMPONENT) include(CMakeParseArguments) set(OPTIONS REQUIRED) set(MULTI_VALUE_ARGS FILES LIBRARIES) cmake_parse_arguments(SUITESPARSE_FIND_${COMPONENT} "${OPTIONS}" "" "${MULTI_VALUE_ARGS}" ${ARGN}) if (SUITESPARSE_FIND_${COMPONENT}_REQUIRED) list(APPEND SUITESPARSE_FOUND_REQUIRED_VARS ${COMPONENT}_FOUND) endif() set(${COMPONENT}_FOUND TRUE) if (SUITESPARSE_FIND_${COMPONENT}_FILES) find_path(${COMPONENT}_INCLUDE_DIR NAMES ${SUITESPARSE_FIND_${COMPONENT}_FILES} HINTS ${SUITESPARSE_INCLUDE_DIR_HINTS} PATHS ${SUITESPARSE_CHECK_INCLUDE_DIRS} PATH_SUFFIXES ${SUITESPARSE_CHECK_PATH_SUFFIXES}) if (${COMPONENT}_INCLUDE_DIR) message(STATUS "Found ${COMPONENT} headers in: " "${${COMPONENT}_INCLUDE_DIR}") mark_as_advanced(${COMPONENT}_INCLUDE_DIR) else() # Specified headers not found. set(${COMPONENT}_FOUND FALSE) if (SUITESPARSE_FIND_${COMPONENT}_REQUIRED) suitesparse_report_not_found( "Did not find ${COMPONENT} header (required SuiteSparse component).") else() message(STATUS "Did not find ${COMPONENT} header (optional " "SuiteSparse component).") # Hide optional vars from CMake GUI even if not found. mark_as_advanced(${COMPONENT}_INCLUDE_DIR) endif() endif() endif() if (SUITESPARSE_FIND_${COMPONENT}_LIBRARIES) find_library(${COMPONENT}_LIBRARY NAMES ${SUITESPARSE_FIND_${COMPONENT}_LIBRARIES} HINTS ${SUITESPARSE_LIBRARY_DIR_HINTS} PATHS ${SUITESPARSE_CHECK_LIBRARY_DIRS} PATH_SUFFIXES ${SUITESPARSE_CHECK_PATH_SUFFIXES}) if (${COMPONENT}_LIBRARY) message(STATUS "Found ${COMPONENT} library: ${${COMPONENT}_LIBRARY}") mark_as_advanced(${COMPONENT}_LIBRARY) else () # Specified libraries not found. set(${COMPONENT}_FOUND FALSE) if (SUITESPARSE_FIND_${COMPONENT}_REQUIRED) suitesparse_report_not_found( "Did not find ${COMPONENT} library (required SuiteSparse component).") else() message(STATUS "Did not find ${COMPONENT} library (optional SuiteSparse " "dependency)") # Hide optional vars from CMake GUI even if not found. mark_as_advanced(${COMPONENT}_LIBRARY) endif() endif() endif() endmacro() # Given the number of components of SuiteSparse, and to ensure that the # automatic failure message generated by FindPackageHandleStandardArgs() # when not all required components are found is helpful, we maintain a list # of all variables that must be defined for SuiteSparse to be considered found. unset(SUITESPARSE_FOUND_REQUIRED_VARS) # BLAS. find_package(BLAS QUIET) if (NOT BLAS_FOUND) suitesparse_report_not_found( "Did not find BLAS library (required for SuiteSparse).") endif (NOT BLAS_FOUND) list(APPEND SUITESPARSE_FOUND_REQUIRED_VARS BLAS_FOUND) # LAPACK. find_package(LAPACK QUIET) if (NOT LAPACK_FOUND) suitesparse_report_not_found( "Did not find LAPACK library (required for SuiteSparse).") endif (NOT LAPACK_FOUND) list(APPEND SUITESPARSE_FOUND_REQUIRED_VARS LAPACK_FOUND) suitesparse_find_component(AMD REQUIRED FILES amd.h LIBRARIES amd) suitesparse_find_component(CAMD REQUIRED FILES camd.h LIBRARIES camd) suitesparse_find_component(COLAMD REQUIRED FILES colamd.h LIBRARIES colamd) suitesparse_find_component(CCOLAMD REQUIRED FILES ccolamd.h LIBRARIES ccolamd) suitesparse_find_component(CHOLMOD REQUIRED FILES cholmod.h LIBRARIES cholmod) suitesparse_find_component( SUITESPARSEQR REQUIRED FILES SuiteSparseQR.hpp LIBRARIES spqr) if (SUITESPARSEQR_FOUND) # SuiteSparseQR may be compiled with Intel Threading Building Blocks, # we assume that if TBB is installed, SuiteSparseQR was compiled with # support for it, this will do no harm if it wasn't. find_package(TBB QUIET) if (TBB_FOUND) message(STATUS "Found Intel Thread Building Blocks (TBB) library " "(${TBB_VERSION}) assuming SuiteSparseQR was compiled " "with TBB.") # Add the TBB libraries to the SuiteSparseQR libraries (the only # libraries to optionally depend on TBB). list(APPEND SUITESPARSEQR_LIBRARY ${TBB_LIBRARIES}) else() message(STATUS "Did not find Intel TBB library, assuming SuiteSparseQR was " "not compiled with TBB.") endif() endif(SUITESPARSEQR_FOUND) # UFconfig / SuiteSparse_config. # # If SuiteSparse version is >= 4 then SuiteSparse_config is required. # For SuiteSparse 3, UFconfig.h is required. suitesparse_find_component( SUITESPARSE_CONFIG FILES SuiteSparse_config.h LIBRARIES suitesparseconfig) if (SUITESPARSE_CONFIG_FOUND) # SuiteSparse_config (SuiteSparse version >= 4) requires librt library for # timing by default when compiled on Linux or Unix, but not on OSX (which # does not have librt). if (CMAKE_SYSTEM_NAME MATCHES "Linux" OR UNIX AND NOT APPLE) suitesparse_find_component(LIBRT LIBRARIES rt) if (LIBRT_FOUND) message(STATUS "Adding librt: ${LIBRT_LIBRARY} to " "SuiteSparse_config libraries (required on Linux & Unix [not OSX] if " "SuiteSparse is compiled with timing).") list(APPEND SUITESPARSE_CONFIG_LIBRARY ${LIBRT_LIBRARY}) else() message(STATUS "Could not find librt, but found SuiteSparse_config, " "assuming that SuiteSparse was compiled without timing.") endif () endif (CMAKE_SYSTEM_NAME MATCHES "Linux" OR UNIX AND NOT APPLE) else() # Failed to find SuiteSparse_config (>= v4 installs), instead look for # UFconfig header which should be present in < v4 installs. suitesparse_find_component(UFCONFIG FILES UFconfig.h) endif () if (NOT SUITESPARSE_CONFIG_FOUND AND NOT UFCONFIG_FOUND) suitesparse_report_not_found( "Failed to find either: SuiteSparse_config header & library (should be " "present in all SuiteSparse >= v4 installs), or UFconfig header (should " "be present in all SuiteSparse < v4 installs).") endif() # Extract the SuiteSparse version from the appropriate header (UFconfig.h for # <= v3, SuiteSparse_config.h for >= v4). list(APPEND SUITESPARSE_FOUND_REQUIRED_VARS SUITESPARSE_VERSION) if (UFCONFIG_FOUND) # SuiteSparse version <= 3. set(SUITESPARSE_VERSION_FILE ${UFCONFIG_INCLUDE_DIR}/UFconfig.h) if (NOT EXISTS ${SUITESPARSE_VERSION_FILE}) suitesparse_report_not_found( "Could not find file: ${SUITESPARSE_VERSION_FILE} containing version " "information for <= v3 SuiteSparse installs, but UFconfig was found " "(only present in <= v3 installs).") else (NOT EXISTS ${SUITESPARSE_VERSION_FILE}) file(READ ${SUITESPARSE_VERSION_FILE} UFCONFIG_CONTENTS) string(REGEX MATCH "#define SUITESPARSE_MAIN_VERSION [0-9]+" SUITESPARSE_MAIN_VERSION "${UFCONFIG_CONTENTS}") string(REGEX REPLACE "#define SUITESPARSE_MAIN_VERSION ([0-9]+)" "\\1" SUITESPARSE_MAIN_VERSION "${SUITESPARSE_MAIN_VERSION}") string(REGEX MATCH "#define SUITESPARSE_SUB_VERSION [0-9]+" SUITESPARSE_SUB_VERSION "${UFCONFIG_CONTENTS}") string(REGEX REPLACE "#define SUITESPARSE_SUB_VERSION ([0-9]+)" "\\1" SUITESPARSE_SUB_VERSION "${SUITESPARSE_SUB_VERSION}") string(REGEX MATCH "#define SUITESPARSE_SUBSUB_VERSION [0-9]+" SUITESPARSE_SUBSUB_VERSION "${UFCONFIG_CONTENTS}") string(REGEX REPLACE "#define SUITESPARSE_SUBSUB_VERSION ([0-9]+)" "\\1" SUITESPARSE_SUBSUB_VERSION "${SUITESPARSE_SUBSUB_VERSION}") # This is on a single line s/t CMake does not interpret it as a list of # elements and insert ';' separators which would result in 4.;2.;1 nonsense. set(SUITESPARSE_VERSION "${SUITESPARSE_MAIN_VERSION}.${SUITESPARSE_SUB_VERSION}.${SUITESPARSE_SUBSUB_VERSION}") endif (NOT EXISTS ${SUITESPARSE_VERSION_FILE}) endif (UFCONFIG_FOUND) if (SUITESPARSE_CONFIG_FOUND) # SuiteSparse version >= 4. set(SUITESPARSE_VERSION_FILE ${SUITESPARSE_CONFIG_INCLUDE_DIR}/SuiteSparse_config.h) if (NOT EXISTS ${SUITESPARSE_VERSION_FILE}) suitesparse_report_not_found( "Could not find file: ${SUITESPARSE_VERSION_FILE} containing version " "information for >= v4 SuiteSparse installs, but SuiteSparse_config was " "found (only present in >= v4 installs).") else (NOT EXISTS ${SUITESPARSE_VERSION_FILE}) file(READ ${SUITESPARSE_VERSION_FILE} SUITESPARSE_CONFIG_CONTENTS) string(REGEX MATCH "#define SUITESPARSE_MAIN_VERSION [0-9]+" SUITESPARSE_MAIN_VERSION "${SUITESPARSE_CONFIG_CONTENTS}") string(REGEX REPLACE "#define SUITESPARSE_MAIN_VERSION ([0-9]+)" "\\1" SUITESPARSE_MAIN_VERSION "${SUITESPARSE_MAIN_VERSION}") string(REGEX MATCH "#define SUITESPARSE_SUB_VERSION [0-9]+" SUITESPARSE_SUB_VERSION "${SUITESPARSE_CONFIG_CONTENTS}") string(REGEX REPLACE "#define SUITESPARSE_SUB_VERSION ([0-9]+)" "\\1" SUITESPARSE_SUB_VERSION "${SUITESPARSE_SUB_VERSION}") string(REGEX MATCH "#define SUITESPARSE_SUBSUB_VERSION [0-9]+" SUITESPARSE_SUBSUB_VERSION "${SUITESPARSE_CONFIG_CONTENTS}") string(REGEX REPLACE "#define SUITESPARSE_SUBSUB_VERSION ([0-9]+)" "\\1" SUITESPARSE_SUBSUB_VERSION "${SUITESPARSE_SUBSUB_VERSION}") # This is on a single line s/t CMake does not interpret it as a list of # elements and insert ';' separators which would result in 4.;2.;1 nonsense. set(SUITESPARSE_VERSION "${SUITESPARSE_MAIN_VERSION}.${SUITESPARSE_SUB_VERSION}.${SUITESPARSE_SUBSUB_VERSION}") endif (NOT EXISTS ${SUITESPARSE_VERSION_FILE}) endif (SUITESPARSE_CONFIG_FOUND) # METIS (Optional dependency). suitesparse_find_component(METIS LIBRARIES metis) # Only mark SuiteSparse as found if all required components and dependencies # have been found. set(SUITESPARSE_FOUND TRUE) foreach(REQUIRED_VAR ${SUITESPARSE_FOUND_REQUIRED_VARS}) if (NOT ${REQUIRED_VAR}) set(SUITESPARSE_FOUND FALSE) endif (NOT ${REQUIRED_VAR}) endforeach(REQUIRED_VAR ${SUITESPARSE_FOUND_REQUIRED_VARS}) if (SUITESPARSE_FOUND) list(APPEND SUITESPARSE_INCLUDE_DIRS ${AMD_INCLUDE_DIR} ${CAMD_INCLUDE_DIR} ${COLAMD_INCLUDE_DIR} ${CCOLAMD_INCLUDE_DIR} ${CHOLMOD_INCLUDE_DIR} ${SUITESPARSEQR_INCLUDE_DIR}) # Handle config separately, as otherwise at least one of them will be set # to NOTFOUND which would cause any check on SUITESPARSE_INCLUDE_DIRS to fail. if (SUITESPARSE_CONFIG_FOUND) list(APPEND SUITESPARSE_INCLUDE_DIRS ${SUITESPARSE_CONFIG_INCLUDE_DIR}) endif (SUITESPARSE_CONFIG_FOUND) if (UFCONFIG_FOUND) list(APPEND SUITESPARSE_INCLUDE_DIRS ${UFCONFIG_INCLUDE_DIR}) endif (UFCONFIG_FOUND) # As SuiteSparse includes are often all in the same directory, remove any # repetitions. list(REMOVE_DUPLICATES SUITESPARSE_INCLUDE_DIRS) # Important: The ordering of these libraries is *NOT* arbitrary, as these # could potentially be static libraries their link ordering is important. list(APPEND SUITESPARSE_LIBRARIES ${SUITESPARSEQR_LIBRARY} ${CHOLMOD_LIBRARY} ${CCOLAMD_LIBRARY} ${CAMD_LIBRARY} ${COLAMD_LIBRARY} ${AMD_LIBRARY} ${LAPACK_LIBRARIES} ${BLAS_LIBRARIES}) if (SUITESPARSE_CONFIG_FOUND) list(APPEND SUITESPARSE_LIBRARIES ${SUITESPARSE_CONFIG_LIBRARY}) endif (SUITESPARSE_CONFIG_FOUND) if (METIS_FOUND) list(APPEND SUITESPARSE_LIBRARIES ${METIS_LIBRARY}) endif (METIS_FOUND) endif() # Determine if we are running on Ubuntu with the package install of SuiteSparse # which is broken and does not support linking a shared library. set(SUITESPARSE_IS_BROKEN_SHARED_LINKING_UBUNTU_SYSTEM_VERSION FALSE) if (CMAKE_SYSTEM_NAME MATCHES "Linux" AND SUITESPARSE_VERSION VERSION_EQUAL 3.4.0) find_program(LSB_RELEASE_EXECUTABLE lsb_release) if (LSB_RELEASE_EXECUTABLE) # Any even moderately recent Ubuntu release (likely to be affected by # this bug) should have lsb_release, if it isn't present we are likely # on a different Linux distribution (should be fine). execute_process(COMMAND ${LSB_RELEASE_EXECUTABLE} -si OUTPUT_VARIABLE LSB_DISTRIBUTOR_ID OUTPUT_STRIP_TRAILING_WHITESPACE) if (LSB_DISTRIBUTOR_ID MATCHES "Ubuntu" AND SUITESPARSE_LIBRARIES MATCHES "/usr/lib/libamd") # We are on Ubuntu, and the SuiteSparse version matches the broken # system install version and is a system install. set(SUITESPARSE_IS_BROKEN_SHARED_LINKING_UBUNTU_SYSTEM_VERSION TRUE) message(STATUS "Found system install of SuiteSparse " "${SUITESPARSE_VERSION} running on Ubuntu, which has a known bug " "preventing linking of shared libraries (static linking unaffected).") endif (LSB_DISTRIBUTOR_ID MATCHES "Ubuntu" AND SUITESPARSE_LIBRARIES MATCHES "/usr/lib/libamd") endif (LSB_RELEASE_EXECUTABLE) endif (CMAKE_SYSTEM_NAME MATCHES "Linux" AND SUITESPARSE_VERSION VERSION_EQUAL 3.4.0) suitesparse_reset_find_library_prefix() # Handle REQUIRED and QUIET arguments to FIND_PACKAGE include(FindPackageHandleStandardArgs) if (SUITESPARSE_FOUND) find_package_handle_standard_args(SuiteSparse REQUIRED_VARS ${SUITESPARSE_FOUND_REQUIRED_VARS} VERSION_VAR SUITESPARSE_VERSION FAIL_MESSAGE "Failed to find some/all required components of SuiteSparse.") else (SUITESPARSE_FOUND) # Do not pass VERSION_VAR to FindPackageHandleStandardArgs() if we failed to # find SuiteSparse to avoid a confusing autogenerated failure message # that states 'not found (missing: FOO) (found version: x.y.z)'. find_package_handle_standard_args(SuiteSparse REQUIRED_VARS ${SUITESPARSE_FOUND_REQUIRED_VARS} FAIL_MESSAGE "Failed to find some/all required components of SuiteSparse.") endif (SUITESPARSE_FOUND) ================================================ FILE: ingvio_estimator/cmake/ROS1.cmake ================================================ cmake_minimum_required(VERSION 3.16) find_package(catkin QUIET COMPONENTS roscpp roslib std_msgs message_filters sensor_msgs eigen_conversions geometry_msgs tf tf_conversions nav_msgs gnss_comm ) catkin_package( CATKIN_DEPENDS roscpp roslib std_msgs message_filters sensor_msgs eigen_conversions geometry_msgs tf tf_conversions nav_msgs gnss_comm INCLUDE_DIRS src/ DEPENDS OpenCV SUITESPARSE Boost ) include_directories( src test ${EIGEN3_INCLUDE_DIR} ${Boost_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS} ${SUITESPARSE_INCLUDE_DIRS} ) add_executable(ingvio ${PROJECT_SOURCE_DIR}/src/AuxGammaFunc.cpp ${PROJECT_SOURCE_DIR}/src/VecState.cpp ${PROJECT_SOURCE_DIR}/src/PoseState.cpp ${PROJECT_SOURCE_DIR}/src/AnchoredLandmark.cpp ${PROJECT_SOURCE_DIR}/src/ImuPropagator.cpp ${PROJECT_SOURCE_DIR}/src/IngvioFilter.cpp ${PROJECT_SOURCE_DIR}/src/IngvioNode.cpp ${PROJECT_SOURCE_DIR}/src/IngvioParams.cpp ${PROJECT_SOURCE_DIR}/src/MapServer.cpp ${PROJECT_SOURCE_DIR}/src/MapServerManager.cpp ${PROJECT_SOURCE_DIR}/src/RemoveLostUpdate.cpp ${PROJECT_SOURCE_DIR}/src/State.cpp ${PROJECT_SOURCE_DIR}/src/StateManager.cpp ${PROJECT_SOURCE_DIR}/src/LandmarkUpdate.cpp ${PROJECT_SOURCE_DIR}/src/SwMargUpdate.cpp ${PROJECT_SOURCE_DIR}/src/KeyframeUpdate.cpp ${PROJECT_SOURCE_DIR}/src/Triangulator.cpp ${PROJECT_SOURCE_DIR}/src/Update.cpp ${PROJECT_SOURCE_DIR}/src/GnssProcessor.cpp ${PROJECT_SOURCE_DIR}/src/GnssSync.cpp ${PROJECT_SOURCE_DIR}/src/GvioAligner.cpp ${PROJECT_SOURCE_DIR}/src/GnssUpdate.cpp ${PROJECT_SOURCE_DIR}/src/GnssManager.cpp ${PROJECT_SOURCE_DIR}/src/GnssData.cpp ) list(APPEND thirdparty_libs ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${OpenCV_LIBRARIES} ${SUITESPARSE_LIBRARIES} ) target_link_libraries(ingvio ${thirdparty_libs} ) message(STATUS "Tests enabled! Build in-project tests!") find_package(GTest REQUIRED) include_directories(${GTEST_INCLUDE_DIRS}) add_executable(test_state_manager ${PROJECT_SOURCE_DIR}/src/VecState.cpp ${PROJECT_SOURCE_DIR}/src/PoseState.cpp ${PROJECT_SOURCE_DIR}/test/TestStateManager.cpp ${PROJECT_SOURCE_DIR}/src/AnchoredLandmark.cpp ${PROJECT_SOURCE_DIR}/src/AuxGammaFunc.cpp ${PROJECT_SOURCE_DIR}/src/State.cpp ${PROJECT_SOURCE_DIR}/src/StateManager.cpp ${PROJECT_SOURCE_DIR}/src/IngvioParams.cpp ) target_link_libraries(test_state_manager ${GTEST_BOTH_LIBRARIES} pthread ${thirdparty_libs}) gtest_discover_tests(test_state_manager) add_executable(test_propagator ${PROJECT_SOURCE_DIR}/test/TestPropagator.cpp ${PROJECT_SOURCE_DIR}/src/VecState.cpp ${PROJECT_SOURCE_DIR}/src/PoseState.cpp ${PROJECT_SOURCE_DIR}/src/AnchoredLandmark.cpp ${PROJECT_SOURCE_DIR}/src/AuxGammaFunc.cpp ${PROJECT_SOURCE_DIR}/src/State.cpp ${PROJECT_SOURCE_DIR}/src/StateManager.cpp ${PROJECT_SOURCE_DIR}/src/IngvioParams.cpp ${PROJECT_SOURCE_DIR}/src/ImuPropagator.cpp ) target_link_libraries(test_propagator ${GTEST_BOTH_LIBRARIES} pthread ${thirdparty_libs}) gtest_discover_tests(test_propagator) add_executable(test_map_server ${PROJECT_SOURCE_DIR}/test/TestMapServer.cpp ${PROJECT_SOURCE_DIR}/src/VecState.cpp ${PROJECT_SOURCE_DIR}/src/PoseState.cpp ${PROJECT_SOURCE_DIR}/test/GenerateNoise.cpp ${PROJECT_SOURCE_DIR}/src/AnchoredLandmark.cpp ${PROJECT_SOURCE_DIR}/src/AuxGammaFunc.cpp ${PROJECT_SOURCE_DIR}/src/State.cpp ${PROJECT_SOURCE_DIR}/src/StateManager.cpp ${PROJECT_SOURCE_DIR}/src/IngvioParams.cpp ${PROJECT_SOURCE_DIR}/src/ImuPropagator.cpp ${PROJECT_SOURCE_DIR}/src/MapServer.cpp ${PROJECT_SOURCE_DIR}/src/MapServerManager.cpp ${PROJECT_SOURCE_DIR}/src/Triangulator.cpp ) target_link_libraries(test_map_server ${GTEST_BOTH_LIBRARIES} pthread ${thirdparty_libs}) gtest_discover_tests(test_map_server) add_executable(test_triangulator ${PROJECT_SOURCE_DIR}/test/TestTriangulator.cpp ${PROJECT_SOURCE_DIR}/src/VecState.cpp ${PROJECT_SOURCE_DIR}/src/PoseState.cpp ${PROJECT_SOURCE_DIR}/src/AnchoredLandmark.cpp ${PROJECT_SOURCE_DIR}/test/GenerateNoise.cpp ${PROJECT_SOURCE_DIR}/src/Triangulator.cpp ${PROJECT_SOURCE_DIR}/src/IngvioParams.cpp ${PROJECT_SOURCE_DIR}/src/AuxGammaFunc.cpp ${PROJECT_SOURCE_DIR}/src/MapServer.cpp ) target_link_libraries(test_triangulator ${GTEST_BOTH_LIBRARIES} pthread ${thirdparty_libs}) gtest_discover_tests(test_triangulator) ================================================ FILE: ingvio_estimator/launch/ingvio_mono_fw.launch ================================================ ================================================ FILE: ingvio_estimator/launch/ingvio_mono_fw_record.launch ================================================ ================================================ FILE: ingvio_estimator/launch/ingvio_mono_sf.launch ================================================ ================================================ FILE: ingvio_estimator/launch/ingvio_mono_sf_record.launch ================================================ ================================================ FILE: ingvio_estimator/launch/ingvio_stereo_fw.launch ================================================ ================================================ FILE: ingvio_estimator/launch/ingvio_stereo_fw_record.launch ================================================ ================================================ FILE: ingvio_estimator/launch/ingvio_stereo_sf.launch ================================================ ================================================ FILE: ingvio_estimator/launch/ingvio_stereo_sf_record.launch ================================================ ================================================ FILE: ingvio_estimator/package.xml ================================================ ingvio_estimator 1.0.0 Invariant filter for visual-inertial-GNSS fusion navigation. Changwu Liu Changwu Liu GPLv3 catkin cmake_modules roscpp roslib std_msgs message_filters sensor_msgs eigen_conversions geometry_msgs tf tf_conversions nav_msgs gnss_comm gtest catkin ================================================ FILE: ingvio_estimator/rviz/ingvio_mono.rviz ================================================ Panels: - Class: rviz/Displays Help Height: 0 Name: Displays Property Tree Widget: Expanded: - /Global Options1 - /Status1 Splitter Ratio: 0.5 Tree Height: 301 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties Expanded: - /2D Pose Estimate1 - /2D Nav Goal1 - /Publish Point1 Name: Tool Properties Splitter Ratio: 0.5886790156364441 - Class: rviz/Views Expanded: - /Current View1 Name: Views Splitter Ratio: 0.5 - Class: rviz/Time Name: Time SyncMode: 0 SyncSource: "" Preferences: PromptSaveOnExit: true Toolbars: toolButtonStyle: 2 Visualization Manager: Class: "" Displays: - Alpha: 0.5 Cell Size: 1 Class: rviz/Grid Color: 160; 160; 164 Enabled: true Line Style: Line Width: 0.029999999329447746 Value: Lines Name: Grid Normal Cell Count: 0 Offset: X: 0 Y: 0 Z: 0 Plane: XY Plane Cell Count: 10 Reference Frame: Value: true - Class: rviz/Image Enabled: true Image Topic: /mono_tracker/mono_track Max Value: 1 Median window: 5 Min Value: 0 Name: MonoTrack Normalize Range: true Queue Size: 2 Transport Hint: raw Unreliable: false Value: true - Alpha: 1 Buffer Length: 1 Class: rviz/Path Color: 25; 255; 0 Enabled: true Head Diameter: 0.30000001192092896 Head Length: 0.20000000298023224 Length: 0.30000001192092896 Line Style: Lines Line Width: 0.029999999329447746 Name: Path_world Offset: X: 0 Y: 0 Z: 0 Pose Color: 255; 85; 255 Pose Style: None Queue Size: 2 Radius: 0.029999999329447746 Shaft Diameter: 0.10000000149011612 Shaft Length: 0.10000000149011612 Topic: /ingvio_estimator/path_w Unreliable: false Value: true - Angle Tolerance: 0.10000000149011612 Class: rviz/Odometry Covariance: Orientation: Alpha: 0.5 Color: 255; 255; 127 Color Style: Unique Frame: Local Offset: 1 Scale: 1 Value: true Position: Alpha: 0.30000001192092896 Color: 204; 51; 204 Scale: 1 Value: true Value: true Enabled: true Keep: 1 Name: Pose_world Position Tolerance: 0.10000000149011612 Queue Size: 10 Shape: Alpha: 1 Axes Length: 1 Axes Radius: 0.10000000149011612 Color: 255; 25; 0 Head Length: 0.30000001192092896 Head Radius: 0.10000000149011612 Shaft Length: 1 Shaft Radius: 0.05000000074505806 Value: Axes Topic: /ingvio_estimator/pose_w Unreliable: false Value: true - Alpha: 1 Buffer Length: 1 Class: rviz/Path Color: 173; 127; 168 Enabled: true Head Diameter: 0.30000001192092896 Head Length: 0.20000000298023224 Length: 0.30000001192092896 Line Style: Lines Line Width: 0.029999999329447746 Name: Path_spp Offset: X: 0 Y: 0 Z: 0 Pose Color: 255; 85; 255 Pose Style: None Queue Size: 1 Radius: 0.029999999329447746 Shaft Diameter: 0.10000000149011612 Shaft Length: 0.10000000149011612 Topic: /ingvio_estimator/path_spp Unreliable: false Value: true - Alpha: 1 Buffer Length: 1 Class: rviz/Path Color: 239; 41; 41 Enabled: true Head Diameter: 0.30000001192092896 Head Length: 0.20000000298023224 Length: 0.30000001192092896 Line Style: Lines Line Width: 0.029999999329447746 Name: Path_rtk_gt Offset: X: 0 Y: 0 Z: 0 Pose Color: 255; 85; 255 Pose Style: None Queue Size: 1 Radius: 0.029999999329447746 Shaft Diameter: 0.10000000149011612 Shaft Length: 0.10000000149011612 Topic: /ingvio_estimator/path_gt Unreliable: false Value: true Enabled: true Global Options: Background Color: 48; 48; 48 Default Light: true Fixed Frame: world Frame Rate: 30 Name: root Tools: - Class: rviz/Interact Hide Inactive Objects: true - Class: rviz/MoveCamera - Class: rviz/Select - Class: rviz/FocusCamera - Class: rviz/Measure - Class: rviz/SetInitialPose Theta std deviation: 0.2617993950843811 Topic: /initialpose X std deviation: 0.5 Y std deviation: 0.5 - Class: rviz/SetGoal Topic: /move_base_simple/goal - Class: rviz/PublishPoint Single click: true Topic: /clicked_point Value: true Views: Current: Class: rviz/Orbit Distance: 24.759632110595703 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Field of View: 0.7853981852531433 Focal Point: X: 0 Y: 0 Z: 0 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 Pitch: 0.4253981113433838 Target Frame: Yaw: 0.6503981351852417 Saved: ~ Window Geometry: Displays: collapsed: false Height: 1014 Hide Left Dock: false Hide Right Dock: false MonoTrack: collapsed: false QMainWindow State: 000000ff00000000fd00000004000000000000025b00000358fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000016a000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000012004d006f006e006f0054007200610063006b01000001ad000001e80000001600ffffff000000010000010f00000358fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d00000358000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005ac0000003efc0100000002fb0000000800540069006d00650100000000000005ac000003bc00fffffffb0000000800540069006d00650100000000000004500000000000000000000002360000035800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: collapsed: false Tool Properties: collapsed: false Views: collapsed: false Width: 1452 X: 246 Y: 27 ================================================ FILE: ingvio_estimator/rviz/ingvio_stereo.rviz ================================================ Panels: - Class: rviz/Displays Help Height: 78 Name: Displays Property Tree Widget: Expanded: - /Global Options1 - /Status1 Splitter Ratio: 0.5 Tree Height: 268 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties Expanded: - /2D Pose Estimate1 - /2D Nav Goal1 - /Publish Point1 Name: Tool Properties Splitter Ratio: 0.5886790156364441 - Class: rviz/Views Expanded: - /Current View1 Name: Views Splitter Ratio: 0.5 - Class: rviz/Time Name: Time SyncMode: 0 SyncSource: "" Preferences: PromptSaveOnExit: true Toolbars: toolButtonStyle: 2 Visualization Manager: Class: "" Displays: - Alpha: 0.5 Cell Size: 1 Class: rviz/Grid Color: 160; 160; 164 Enabled: true Line Style: Line Width: 0.029999999329447746 Value: Lines Name: Grid Normal Cell Count: 0 Offset: X: 0 Y: 0 Z: 0 Plane: XY Plane Cell Count: 10 Reference Frame: Value: true - Class: rviz/Image Enabled: true Image Topic: /stereo_tracker/stereo_track Max Value: 1 Median window: 5 Min Value: 0 Name: StereoTrack Normalize Range: true Queue Size: 2 Transport Hint: raw Unreliable: false Value: true - Alpha: 1 Buffer Length: 1 Class: rviz/Path Color: 25; 255; 0 Enabled: true Head Diameter: 0.30000001192092896 Head Length: 0.20000000298023224 Length: 0.30000001192092896 Line Style: Lines Line Width: 0.029999999329447746 Name: Path_world Offset: X: 0 Y: 0 Z: 0 Pose Color: 255; 85; 255 Pose Style: None Queue Size: 2 Radius: 0.029999999329447746 Shaft Diameter: 0.10000000149011612 Shaft Length: 0.10000000149011612 Topic: /ingvio_estimator/path_w Unreliable: false Value: true - Angle Tolerance: 0.10000000149011612 Class: rviz/Odometry Covariance: Orientation: Alpha: 0.5 Color: 255; 255; 127 Color Style: Unique Frame: Local Offset: 1 Scale: 1 Value: true Position: Alpha: 0.30000001192092896 Color: 204; 51; 204 Scale: 1 Value: true Value: true Enabled: true Keep: 1 Name: Pose_world Position Tolerance: 0.10000000149011612 Queue Size: 10 Shape: Alpha: 1 Axes Length: 1 Axes Radius: 0.10000000149011612 Color: 255; 25; 0 Head Length: 0.30000001192092896 Head Radius: 0.10000000149011612 Shaft Length: 1 Shaft Radius: 0.05000000074505806 Value: Axes Topic: /ingvio_estimator/pose_w Unreliable: false Value: true - Alpha: 1 Buffer Length: 1 Class: rviz/Path Color: 239; 41; 41 Enabled: true Head Diameter: 0.30000001192092896 Head Length: 0.20000000298023224 Length: 0.30000001192092896 Line Style: Lines Line Width: 0.029999999329447746 Name: Path_rtk_gt Offset: X: 0 Y: 0 Z: 0 Pose Color: 255; 85; 255 Pose Style: None Queue Size: 1 Radius: 0.029999999329447746 Shaft Diameter: 0.10000000149011612 Shaft Length: 0.10000000149011612 Topic: /ingvio_estimator/path_gt Unreliable: false Value: true - Alpha: 1 Buffer Length: 1 Class: rviz/Path Color: 173; 127; 168 Enabled: true Head Diameter: 0.30000001192092896 Head Length: 0.20000000298023224 Length: 0.30000001192092896 Line Style: Lines Line Width: 0.029999999329447746 Name: Path_spp Offset: X: 0 Y: 0 Z: 0 Pose Color: 255; 85; 255 Pose Style: None Queue Size: 1 Radius: 0.029999999329447746 Shaft Diameter: 0.10000000149011612 Shaft Length: 0.10000000149011612 Topic: /ingvio_estimator/path_spp Unreliable: false Value: true Enabled: true Global Options: Background Color: 48; 48; 48 Default Light: true Fixed Frame: world Frame Rate: 30 Name: root Tools: - Class: rviz/Interact Hide Inactive Objects: true - Class: rviz/MoveCamera - Class: rviz/Select - Class: rviz/FocusCamera - Class: rviz/Measure - Class: rviz/SetInitialPose Theta std deviation: 0.2617993950843811 Topic: /initialpose X std deviation: 0.5 Y std deviation: 0.5 - Class: rviz/SetGoal Topic: /move_base_simple/goal - Class: rviz/PublishPoint Single click: true Topic: /clicked_point Value: true Views: Current: Class: rviz/Orbit Distance: 24.759632110595703 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Field of View: 0.7853981852531433 Focal Point: X: 0 Y: 0 Z: 0 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 Pitch: 0.6003981828689575 Target Frame: Yaw: 0.84539794921875 Saved: ~ Window Geometry: Displays: collapsed: false Height: 875 Hide Left Dock: false Hide Right Dock: false QMainWindow State: 000000ff00000000fd0000000400000000000001e8000002cdfc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000197000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000001600530074006500720065006f0054007200610063006b01000001da000001300000001600ffffff000000010000010f000002cdfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002cd000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005930000003efc0100000002fb0000000800540069006d0065010000000000000593000003bc00fffffffb0000000800540069006d0065010000000000000450000000000000000000000290000002cd00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false StereoTrack: collapsed: false Time: collapsed: false Tool Properties: collapsed: false Views: collapsed: false Width: 1427 X: 209 Y: 54 ================================================ FILE: ingvio_estimator/src/AnchoredLandmark.cpp ================================================ /** This File is part of InGVIO, an invariant filter for mono/stereo visual- * inertial-raw GNSS navigation. * * Copyright (C) 2022 Changwu Liu (cwliu529@163.com, * lcw18@mails.tsinghua.edu.cn (valid until 2023)) * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program. If not, see . */ #include "AnchoredLandmark.h" namespace ingvio { void AnchoredLandmark::switchRepXyz2Invd(const Eigen::Vector3d& xyz, Eigen::Vector3d& invd) { assert(xyz.z() != 0.0); if (xyz.z() < 0) std::cout << "[AnchoredLandmark]: Warning xyz.z() < 0 !" << std::endl; invd.z() = 1.0/xyz.z(); invd.x() = xyz.x()*invd.z(); invd.y() = xyz.y()*invd.z(); } void AnchoredLandmark::switchRepInvd2Xyz(const Eigen::Vector3d& invd, Eigen::Vector3d& xyz) { assert(invd.z() != 0.0); if (invd.z() < 0) std::cout << "[AnchoredLandmark]: Warning invd.z() < 0 !" << std::endl; xyz.z() = 1.0/invd.z(); xyz.x() = invd.x()*xyz.z(); xyz.y() = invd.y()*xyz.z(); } void AnchoredLandmark::switchRepXyz2Bear(const Eigen::Vector3d& xyz, Eigen::Vector3d& bear) { double r = xyz.norm(); assert(r != 0.0); double rho = 1.0/r; bear.z() = rho; bear.x() = std::atan2(xyz.y(), xyz.x()); bear.y() = std::acos(rho*xyz.z()); } void AnchoredLandmark::switchRepBear2Xyz(const Eigen::Vector3d& bear, Eigen::Vector3d& xyz) { assert(bear.z() != 0.0); if (bear.z() < 0) std::cout << "[AnchoredLandmark]: Warning bear.z() < 0 !" << std::endl; double r = 1.0/bear.z(); xyz.z() = r*std::cos(bear.y()); xyz.x() = r*std::cos(bear.x())*std::sin(bear.y()); xyz.y() = r*std::sin(bear.x())*std::sin(bear.y()); } void AnchoredLandmark::setRepType(const AnchoredLandmark::LmRep& other_rep_type) { if (!_has_rep_set) { _landmark_param_type = other_rep_type; _has_rep_set = true; } else std::cout << "[AnchoredLandmark]: Representation type has been set once! Cannot change!" << std::endl; } void AnchoredLandmark::resetAnchoredPose(std::shared_ptr new_anchored_pose, bool isUpdateRepValue) { if (new_anchored_pose != nullptr) { _anchored_pose = new_anchored_pose; if (isUpdateRepValue) { this->setValuePosXyz(this->_pos_xyz); // TODO: The anchored pose may not take fej? this->setFejPosXyz(this->_pos_xyz_fej); } } else _anchored_pose.reset(); } void AnchoredLandmark::setValuePosXyz(const Eigen::Vector3d& xyz_world) { _pos_xyz = xyz_world; if (_anchored_pose != nullptr) { Eigen::Vector3d xyz_body = _anchored_pose->valueLinearAsMat().transpose()*(_pos_xyz - _anchored_pose->valueTrans()); switch (_landmark_param_type) { case XYZ: _pos_rep = xyz_body; break; case INV_DEPTH: switchRepXyz2Invd(xyz_body, _pos_rep); break; case BEARING: switchRepXyz2Bear(xyz_body, _pos_rep); break; default: assert(false); break; } } else std::cout << "[AnchoredLandmark]: Cannot set body rep, no anchored pose!" << std::endl; } void AnchoredLandmark::setValuePosRep(const Eigen::Vector3d& pos_rep_body) { _pos_rep = pos_rep_body; Eigen::Vector3d xyz_body; switch (_landmark_param_type) { case XYZ: xyz_body = _pos_rep; break; case INV_DEPTH: switchRepInvd2Xyz(_pos_rep, xyz_body); break; case BEARING: switchRepBear2Xyz(_pos_rep, xyz_body); break; default: assert(false); break; } if (_anchored_pose != nullptr) _pos_xyz = _anchored_pose->valueLinearAsMat()*xyz_body + _anchored_pose->valueTrans(); else std::cout << "[AnchoredLandmark]: Cannot set world xyz pos, no anchored pose!" << std::endl; } void AnchoredLandmark::setFejPosXyz(const Eigen::Vector3d& xyz_world_fej) { _pos_xyz_fej = xyz_world_fej; if (_anchored_pose != nullptr) { Eigen::Vector3d xyz_body_fej = _anchored_pose->fejLinearAsMat().transpose()*(_pos_xyz_fej - _anchored_pose->fejTrans()); switch (_landmark_param_type) { case XYZ: _pos_rep_fej = xyz_body_fej; break; case INV_DEPTH: switchRepXyz2Invd(xyz_body_fej, _pos_rep_fej); break; case BEARING: switchRepXyz2Bear(xyz_body_fej, _pos_rep_fej); break; default: assert(false); break; } } else std::cout << "[AnchoredLandmark]: Cannot set body rep fej, no anchored pose!" << std::endl; } void AnchoredLandmark::setFejPosRep(const Eigen::Vector3d& pos_rep_body_fej) { _pos_rep_fej = pos_rep_body_fej; Eigen::Vector3d xyz_body_fej; switch (_landmark_param_type) { case XYZ: xyz_body_fej = _pos_rep_fej; break; case INV_DEPTH: switchRepInvd2Xyz(_pos_rep_fej, xyz_body_fej); break; case BEARING: switchRepBear2Xyz(_pos_rep_fej, xyz_body_fej); break; default: assert(false); break; } if (_anchored_pose != nullptr) _pos_xyz_fej = _anchored_pose->fejLinearAsMat()*xyz_body_fej + _anchored_pose->fejTrans(); else std::cout << "[AnchoredLandmark]: Cannot set world xyz pos fej, no anchored pose!" << std::endl; } std::shared_ptr AnchoredLandmark::clone() { std::shared_ptr tmp(new AnchoredLandmark()); tmp->setRepType(this->checkLmRepType()); tmp->resetAnchoredPose(this->getAnchoredPose()); tmp->setValuePosXyz(this->valuePosXyz()); tmp->setFejPosXyz(this->fejPosXyz()); return tmp; } void AnchoredLandmark::update(const Eigen::VectorXd& dx) { assert(dx.rows() >= this->idx() + this->size()); const Eigen::Vector3d& delta_p = dx.block<3, 1>(this->idx(), 0); if (_anchored_pose != nullptr) { assert(_anchored_pose->idx() + _anchored_pose->size() <= dx.rows()); const Eigen::Vector3d& delta_theta = dx.block<3, 1>(_anchored_pose->idx(), 0); this->setValuePosXyz(GammaFunc(delta_theta, 0)*this->valuePosXyz() + GammaFunc(delta_theta, 1)*delta_p); } else { std::cout << "[AnchoredLandmark]: Warning! Update without anchored pose!" << std::endl; this->setValuePosXyz(this->valuePosXyz() + delta_p); } } void AnchoredLandmark::setIdentity() { _pos_rep.setZero(); _pos_rep_fej.setZero(); _pos_xyz.setZero(); _pos_xyz_fej.setZero(); } void AnchoredLandmark::setRandom() { setValuePosXyz(Eigen::Vector3d::Random()); } } ================================================ FILE: ingvio_estimator/src/AnchoredLandmark.h ================================================ /** This File is part of InGVIO, an invariant filter for mono/stereo visual- * inertial-raw GNSS navigation. * * Copyright (C) 2022 Changwu Liu (cwliu529@163.com, * lcw18@mails.tsinghua.edu.cn (valid until 2023)) * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program. If not, see . */ #pragma once #include #include "PoseState.h" namespace ingvio { class AnchoredLandmark : public Type { public: enum LmRep {XYZ, INV_DEPTH, BEARING}; AnchoredLandmark() : Type(3) { _pos_rep.setZero(); _pos_rep_fej.setZero(); _pos_xyz.setZero(); _pos_xyz_fej.setZero(); _landmark_param_type = INV_DEPTH; _has_rep_set = false; _anchored_pose.reset(); } ~AnchoredLandmark() {} void update(const Eigen::VectorXd& dx) override; void setIdentity() override; void setRandom() override; void setRepType(const AnchoredLandmark::LmRep& other_rep_type); static void switchRepXyz2Invd(const Eigen::Vector3d& xyz, Eigen::Vector3d& invd); static void switchRepInvd2Xyz(const Eigen::Vector3d& invd, Eigen::Vector3d& xyz); static void switchRepXyz2Bear(const Eigen::Vector3d& xyz, Eigen::Vector3d& bear); static void switchRepBear2Xyz(const Eigen::Vector3d& bear, Eigen::Vector3d& xyz); void resetAnchoredPose(std::shared_ptr new_anchored_pose = nullptr, bool isUpdateRepValue = false); const std::shared_ptr getAnchoredPose() const { return _anchored_pose; } const Eigen::Vector3d& valuePosRep() const { return _pos_rep; } const Eigen::Vector3d& valuePosXyz() const { return _pos_xyz; } const Eigen::Vector3d& fejPosRep() const { return _pos_rep_fej; } const Eigen::Vector3d& fejPosXyz() const { return _pos_xyz_fej; } const LmRep& checkLmRepType() const { return _landmark_param_type; } void setValuePosXyz(const Eigen::Vector3d& xyz_world); void setValuePosRep(const Eigen::Vector3d& pos_rep_body); void setFejPosXyz(const Eigen::Vector3d& xyz_world_fej); void setFejPosRep(const Eigen::Vector3d& pos_rep_body_fej); std::shared_ptr clone(); protected: LmRep _landmark_param_type; Eigen::Vector3d _pos_rep; Eigen::Vector3d _pos_rep_fej; Eigen::Vector3d _pos_xyz; Eigen::Vector3d _pos_xyz_fej; std::shared_ptr _anchored_pose; bool _has_rep_set; }; } ================================================ FILE: ingvio_estimator/src/AuxGammaFunc.cpp ================================================ /** This File is part of InGVIO, an invariant filter for mono/stereo visual- * inertial-raw GNSS navigation. * * Copyright (C) 2022 Changwu Liu (cwliu529@163.com, * lcw18@mails.tsinghua.edu.cn (valid until 2023)) * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program. If not, see . */ #include #include #include "AuxGammaFunc.h" namespace ingvio { Eigen::Matrix3d skew(const Eigen::Vector3d& vec) { Eigen::Matrix3d result; result << 0.0, -vec.z(), vec.y(), vec.z(), 0.0, -vec.x(), -vec.y(), vec.x(), 0.0; return result; } Eigen::Vector3d vee(const Eigen::Matrix3d& mat) { Eigen::Vector3d result; result.z() = mat(1, 0); result.y() = mat(0, 2); result.x() = mat(2, 1); return result; } Eigen::Matrix3d GammaFunc(const Eigen::Vector3d& vec, int m) { assert(m >= 0 && m <= 3); Eigen::Matrix3d result; double theta = vec.norm(); if (std::fabs(theta) < 1e-06) { double factor = 1.0; switch (m) { case 3: factor = 1.0/6.0; break; case 2: factor = 0.5; break; default: break; } return factor*Eigen::Matrix3d::Identity(); } Eigen::Vector3d n = vec.normalized(); Eigen::Matrix3d n_cross = skew(n); Eigen::Matrix3d n_cross2 = n_cross*n_cross; double factor0, factor1, factor2; double sintheta = std::sin(theta); double costheta = std::cos(theta); switch (m) { case 1: { factor0 = 1.0; factor1 = (1.0 - costheta)/theta; factor2 = (theta - sintheta)/theta; break; } case 2: { factor0 = 0.5; factor1 = (theta-sintheta)/std::pow(theta, 2); factor2 = (std::pow(theta, 2)+2.0*costheta-2.0)/(2.0*std::pow(theta, 2)); break; } case 3: { factor0 = 1.0/6.0; double theta3 = std::pow(theta, 3); factor1 = (std::pow(theta, 2)+2.0*costheta-2.0)/(2.0*theta3); factor2 = (theta3-6.0*theta+6.0*sintheta)/(6.0*theta3); break; } default: { factor0 = 1.0; factor1 = sintheta; factor2 = 1.0 - costheta; break; } } result = factor0*Eigen::Matrix3d::Identity() + factor1*n_cross + factor2*n_cross2; return result; } Eigen::Matrix3d Psi1Func(const Eigen::Vector3d& tilde_omega, const Eigen::Vector3d& tilde_acc, double dt) { if ((tilde_omega*dt).norm() < 1e-08) return Eigen::Matrix3d::Zero(); Eigen::Matrix3d M1 = skew(tilde_acc)*GammaFunc(-tilde_omega*dt, 2)*std::pow(dt, 2.0); Eigen::Matrix3d WA = skew(tilde_omega)*skew(tilde_acc); Eigen::Matrix3d WAW = WA*skew(tilde_omega); Eigen::Matrix3d WAW2 = WAW*skew(tilde_omega); Eigen::Matrix3d W2A = skew(tilde_omega)*WA; Eigen::Matrix3d W2AW = W2A*skew(tilde_omega); Eigen::Matrix3d W2AW2 = W2AW*skew(tilde_omega); double eta = tilde_omega.norm(); double xi = eta*dt; double xi2 = std::pow(xi, 2.0); double sin_xi = std::sin(xi); double cos_xi = std::cos(xi); double sin_2xi = std::sin(2*xi); double cos_2xi = std::cos(2*xi); double eta3 = std::pow(eta, 3); double eta4 = eta*eta3; double eta5 = eta*eta4; double eta6 = eta*eta5; double c1 = (sin_xi-xi*cos_xi)/eta3; double c2 = (cos_2xi-4*cos_xi+3)/(4*eta4); double c3 = (4*sin_xi+sin_2xi-4*xi*cos_xi-2*xi)/(4*eta5); double c4 = (xi2-2*xi*sin_xi-2*cos_xi+2)/(2*eta4); double c5 = (6*xi-8*sin_xi+sin_2xi)/(4*eta5); double c6 = (2*xi2-4*xi*sin_xi-cos_2xi+1)/(4*eta6); Eigen::Matrix3d result = M1*(c1*WA + c2*WAW + c3*WAW2 + c4*W2A + c5*W2AW + c6*W2AW2); return result; } Eigen::Matrix3d Psi2Func(const Eigen::Vector3d& tilde_omega, const Eigen::Vector3d& tilde_acc, double dt) { if ((tilde_omega*dt).norm() < 1e-07) return Eigen::Matrix3d::Zero(); Eigen::Matrix3d M1 = skew(tilde_acc)*GammaFunc(-tilde_omega*dt, 3)*std::pow(dt, 3); Eigen::Matrix3d WA = skew(tilde_omega)*skew(tilde_acc); Eigen::Matrix3d WAW = WA*skew(tilde_omega); Eigen::Matrix3d WAW2 = WAW*skew(tilde_omega); Eigen::Matrix3d W2A = skew(tilde_omega)*WA; Eigen::Matrix3d W2AW = W2A*skew(tilde_omega); Eigen::Matrix3d W2AW2 = W2AW*skew(tilde_omega); double eta = tilde_omega.norm(); double xi = eta*dt; double xi2 = std::pow(xi, 2.0); double xi3 = xi*xi2; double sin_xi = std::sin(xi); double cos_xi = std::cos(xi); double sin_2xi = std::sin(2*xi); double cos_2xi = std::cos(2*xi); double eta3 = std::pow(eta, 3); double eta4 = eta*eta3; double eta5 = eta*eta4; double eta6 = eta*eta5; double eta7 = eta*eta6; double c1 = (xi*sin_xi+2*cos_xi-2)/eta4; double c2 = (6*xi-8*sin_xi+sin_2xi)/(8*eta5); double c3 = (2*xi2+8*xi*sin_xi+16*cos_xi+cos_2xi-17)/(8*eta6); double c4 = (xi3+6*xi-12*sin_xi+6*xi*cos_xi)/(6*eta5); double c5 = (6*xi2+16*cos_xi-cos_2xi-15)/(8*eta6); double c6 = (4*xi3+6*xi-24*sin_xi-3*sin_2xi+24*xi*cos_xi)/(24*eta7); Eigen::Matrix3d result; result = M1*(c1*WA + c2*WAW + c3*WAW2 + c4*W2A + c5*W2AW + c6*W2AW2); return result; } } ================================================ FILE: ingvio_estimator/src/AuxGammaFunc.h ================================================ /** This File is part of InGVIO, an invariant filter for mono/stereo visual- * inertial-raw GNSS navigation. * * Copyright (C) 2022 Changwu Liu (cwliu529@163.com, * lcw18@mails.tsinghua.edu.cn (valid until 2023)) * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program. If not, see . */ #pragma once #include namespace ingvio { extern Eigen::Matrix3d skew(const Eigen::Vector3d& vec); extern Eigen::Vector3d vee(const Eigen::Matrix3d& mat); extern Eigen::Matrix3d GammaFunc(const Eigen::Vector3d& vec, int m = 0); extern Eigen::Matrix3d Psi1Func(const Eigen::Vector3d& tilde_omega, const Eigen::Vector3d& tilde_acc, double dt); extern Eigen::Matrix3d Psi2Func(const Eigen::Vector3d& tilde_omega, const Eigen::Vector3d& tilde_acc, double dt); } ================================================ FILE: ingvio_estimator/src/Color.h ================================================ /** This File is part of InGVIO, an invariant filter for mono/stereo visual- * inertial-raw GNSS navigation. * * Copyright (C) 2022 Changwu Liu (cwliu529@163.com, * lcw18@mails.tsinghua.edu.cn (valid until 2023)) * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program. If not, see . */ #pragma once #include namespace color { inline std::ostream& setBlue(std::ostream& s) { s << "\033[0;1;34m"; return s; } inline std::ostream& setRed(std::ostream& s) { s << "\033[0;1;31m"; return s; } inline std::ostream& setGreen(std::ostream& s) { s << "\033[0;1;32m"; return s; } inline std::ostream& setYellow(std::ostream& s) { s << "\033[0;1;33m"; return s; } inline std::ostream& setWhite(std::ostream& s) { s << "\033[0;1;37m"; return s; } inline std::ostream& resetColor(std::ostream& s) { s << "\033[0m"; return s; } } ================================================ FILE: ingvio_estimator/src/GnssData.cpp ================================================ /** This File is part of InGVIO, an invariant filter for mono/stereo visual- * inertial-raw GNSS navigation. * * Copyright (C) 2022 Changwu Liu (cwliu529@163.com, * lcw18@mails.tsinghua.edu.cn (valid until 2023)) * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program. If not, see . */ #include "GnssData.h" namespace ingvio { void GnssData::inputEphem(gnss_comm::EphemBasePtr ephem_ptr) { double toe = gnss_comm::time2sec(ephem_ptr->toe); if (this->sat2time_index.count(ephem_ptr->sat) == 0 || this->sat2time_index.at(ephem_ptr->sat).count(toe) == 0) { this->sat2ephem[ephem_ptr->sat].emplace_back(ephem_ptr); this->sat2time_index[ephem_ptr->sat].emplace(toe, sat2ephem.at(ephem_ptr->sat).size()-1); } } } ================================================ FILE: ingvio_estimator/src/GnssData.h ================================================ /** This File is part of InGVIO, an invariant filter for mono/stereo visual- * inertial-raw GNSS navigation. * * Copyright (C) 2022 Changwu Liu (cwliu529@163.com, * lcw18@mails.tsinghua.edu.cn (valid until 2023)) * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program. If not, see . */ #pragma once #include namespace ingvio { class GnssData { public: GnssData() : _isIono(false) {} ~GnssData() = default; GnssData operator=(const GnssData&) = delete; std::vector latest_gnss_iono_params; std::map> sat2ephem; std::map> sat2time_index; std::map sat_track_status; void inputEphem(gnss_comm::EphemBasePtr ephem_ptr); bool _isIono; private: GnssData(const GnssData&) {} }; } ================================================ FILE: ingvio_estimator/src/GnssManager.cpp ================================================ /** This File is part of InGVIO, an invariant filter for mono/stereo visual- * inertial-raw GNSS navigation. * * Copyright (C) 2022 Changwu Liu (cwliu529@163.com, * lcw18@mails.tsinghua.edu.cn (valid until 2023)) * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program. If not, see . */ #include "GnssManager.h" namespace ingvio { const std::unordered_map GnssManager::satsys_to_gnsstype = { {SYS_GPS, State::GNSSType::GPS}, {SYS_BDS, State::GNSSType::BDS}, {SYS_GAL, State::GNSSType::GAL}, {SYS_GLO, State::GNSSType::GLO} }; const std::unordered_map GnssManager::gnsstype_to_satsys = { {State::GNSSType::GPS, SYS_GPS}, {State::GNSSType::BDS, SYS_BDS}, {State::GNSSType::GAL, SYS_GAL}, {State::GNSSType::GLO, SYS_GLO} }; const std::unordered_map GnssManager::idx_to_gnsstype = { {0, State::GNSSType::GPS}, {1, State::GNSSType::GLO}, {2, State::GNSSType::GAL}, {3, State::GNSSType::BDS} }; State::GNSSType GnssManager::convertSatsys2GnssType(const uint32_t& sat_sys) { if (GnssManager::satsys_to_gnsstype.find(sat_sys) == GnssManager::satsys_to_gnsstype.end()) { std::cout << "[GnssManager]: Sat sys not in index map, cannot convert to gnss type!" << std::endl; std::exit(EXIT_FAILURE); } return GnssManager::satsys_to_gnsstype.at(sat_sys); } Eigen::Matrix3d GnssManager::calcRw2enu(const double& yaw_offset) { return Eigen::AngleAxisd(yaw_offset, Eigen::Vector3d::UnitZ()).toRotationMatrix(); } Eigen::Isometry3d GnssManager::calcTw2enu(const double& yaw_offset) { Eigen::Isometry3d result = Eigen::Isometry3d::Identity(); result.linear() = GnssManager::calcRw2enu(yaw_offset); return result; } Eigen::Vector4d GnssManager::getClockbiasVec(const std::shared_ptr state) { Eigen::Vector4d cb = Eigen::Vector4d::Zero(); for (const auto& gnss_item : state->_gnss) if (gnss_item.first == State::YOF || gnss_item.first == State::FS) continue; else cb(gnss_comm::sys2idx.at(GnssManager::gnsstype_to_satsys.at(gnss_item.first))) = gnss_item.second->value(); return cb; } bool GnssManager::checkGnssStates(const std::shared_ptr state) { if (state->_gnss.find(State::GNSSType::YOF) == state->_gnss.end()) return false; if (state->_gnss.find(State::GNSSType::FS) == state->_gnss.end()) return false; for (int i = 0; i < 4; ++i) if (state->_gnss.find(static_cast(i)) != state->_gnss.end()) return true; return false; } Eigen::Matrix3d GnssManager::dotRw2enu(const std::shared_ptr state) { if (state->_gnss.find(State::GNSSType::YOF) == state->_gnss.end()) return Eigen::Matrix3d::Zero(); const double yo = state->_gnss.at(State::GNSSType::YOF)->value(); Eigen::Matrix3d result; result << -std::sin(yo), -std::cos(yo), 0.0, std::cos(yo), -std::sin(yo), 0.0, 0.0, 0.0, 0.0; return result; } Eigen::Matrix3d GnssManager::calcRw2enu(const std::shared_ptr state) { if (state->_gnss.find(State::GNSSType::YOF) == state->_gnss.end()) return Eigen::Matrix3d::Identity(); const double yo = state->_gnss.at(State::GNSSType::YOF)->value(); return GnssManager::calcRw2enu(yo); } Eigen::Isometry3d GnssManager::calcTw2enu(const std::shared_ptr state) { if (state->_gnss.find(State::GNSSType::YOF) == state->_gnss.end()) return Eigen::Isometry3d::Identity(); const double yo = state->_gnss.at(State::GNSSType::YOF)->value(); return GnssManager::calcTw2enu(yo); } void GnssManager::printYOF(const std::shared_ptr state) { if (state->_gnss.find(State::GNSSType::YOF) == state->_gnss.end()) return; std::cout << "[GnssManager]: Local to ENU yaw offset = " << state->_gnss.at(State::GNSSType::YOF)->value()*180.0/M_PI << " (deg)" << std::endl; } } ================================================ FILE: ingvio_estimator/src/GnssManager.h ================================================ /** This File is part of InGVIO, an invariant filter for mono/stereo visual- * inertial-raw GNSS navigation. * * Copyright (C) 2022 Changwu Liu (cwliu529@163.com, * lcw18@mails.tsinghua.edu.cn (valid until 2023)) * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program. If not, see . */ #pragma once #include #include #include "State.h" namespace ingvio { class GnssManager { public: GnssManager() = delete; GnssManager(const GnssManager&) = delete; GnssManager operator=(const GnssManager&) = delete; static const std::unordered_map satsys_to_gnsstype; static const std::unordered_map gnsstype_to_satsys; static const std::unordered_map idx_to_gnsstype; static State::GNSSType convertSatsys2GnssType(const uint32_t& sat_sys); static Eigen::Matrix3d calcRw2enu(const double& yaw_offset); static Eigen::Isometry3d calcTw2enu(const double& yaw_offset); static Eigen::Matrix3d calcRw2enu(const std::shared_ptr state); static Eigen::Isometry3d calcTw2enu(const std::shared_ptr state); static Eigen::Vector4d getClockbiasVec(const std::shared_ptr state); static bool checkGnssStates(const std::shared_ptr state); static Eigen::Matrix3d dotRw2enu(const std::shared_ptr state); static void printYOF(const std::shared_ptr state); }; } ================================================ FILE: ingvio_estimator/src/GnssProcessor.cpp ================================================ /** This File is part of InGVIO, an invariant filter for mono/stereo visual- * inertial-raw GNSS navigation. * * Copyright (C) 2022 Changwu Liu (cwliu529@163.com, * lcw18@mails.tsinghua.edu.cn (valid until 2023)) * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program. If not, see . */ #include #include "IngvioFilter.h" #include #include #include "GnssData.h" #include "GnssSync.h" #include "GvioAligner.h" namespace ingvio { void IngvioFilter::callbackRtkGroundTruth( const sensor_msgs::NavSatFix::ConstPtr& nav_sat_msg) { if (!_filter_params._enable_gnss || !_gnss_sync->isSync()) return; if (_gvio_aligner->isAlign() && nav_sat_msg->status.status >= sensor_msgs::NavSatStatus::STATUS_FIX) { Eigen::Vector3d gt_geo, gt_ecef; gt_geo.x() = nav_sat_msg->latitude; gt_geo.y() = nav_sat_msg->longitude; gt_geo.z() = nav_sat_msg->altitude; gt_ecef = gnss_comm::geo2ecef(gt_geo); Eigen::Vector3d gt_w = _gvio_aligner->getTecef2w()*gt_ecef; if (gt_w.hasNaN()) return; geometry_msgs::PoseStamped pose_gt; pose_gt.header.stamp = ros::Time().fromSec(nav_sat_msg->header.stamp.toSec() + _gnss_sync->getUnsyncTime()); pose_gt.header.frame_id = "world"; pose_gt.pose.position.x = gt_w.x(); pose_gt.pose.position.y = gt_w.y(); pose_gt.pose.position.z = gt_w.z(); pose_gt.pose.orientation.x = 0.0; pose_gt.pose.orientation.y = 0.0; pose_gt.pose.orientation.z = 0.0; pose_gt.pose.orientation.w = 0.0; _path_gt_msg.header.stamp = pose_gt.header.stamp; _path_gt_msg.header.frame_id = "world"; _path_gt_msg.poses.push_back(pose_gt); _path_gt_pub.publish(_path_gt_msg); Eigen::Isometry3d T_i2w = Eigen::Isometry3d::Identity(); T_i2w.translation() = gt_w; nav_msgs::Odometry odom_gt_msg; odom_gt_msg.header.stamp = pose_gt.header.stamp; odom_gt_msg.header.frame_id = "world"; odom_gt_msg.child_frame_id = "gt"; tf::poseEigenToMsg(T_i2w, odom_gt_msg.pose.pose); tf::vectorEigenToMsg(Eigen::Vector3d::Zero(), odom_gt_msg.twist.twist.linear); _odom_gt_pub.publish(odom_gt_msg); } } void IngvioFilter::callbackEphem(const gnss_comm::GnssEphemMsgConstPtr& ephem_msg) { if (!_filter_params._enable_gnss) return; gnss_comm::EphemPtr ephem = gnss_comm::msg2ephem(ephem_msg); _gnss_data->inputEphem(ephem); } void IngvioFilter::callbackGloEphem(const gnss_comm::GnssGloEphemMsgConstPtr& glo_ephem_msg) { if (!_filter_params._enable_gnss) return; gnss_comm::GloEphemPtr glo_ephem = gnss_comm::msg2glo_ephem(glo_ephem_msg); _gnss_data->inputEphem(glo_ephem); } void IngvioFilter::callbackIonoParams(const gnss_comm::StampedFloat64ArrayConstPtr& iono_msg) { if (!_filter_params._enable_gnss) return; std::vector iono_params; std::copy(iono_msg->data.begin(), iono_msg->data.end(), std::back_inserter(iono_params)); if (iono_params.size() != 8) return; _gnss_data->latest_gnss_iono_params.clear(); _gnss_data->latest_gnss_iono_params.assign(iono_params.begin(), iono_params.end()); if (!_gnss_data->_isIono) _gnss_data->_isIono = true; } void IngvioFilter::callbackGnssMeas(const gnss_comm::GnssMeasMsgConstPtr& gnss_meas_msg) { double aux_time = ros::Time::now().toSec(); if (!_filter_params._enable_gnss || !_gnss_data->_isIono) return; std::vector gnss_meas = gnss_comm::msg2meas(gnss_meas_msg); if (gnss_meas.size() <= 0) return; _gnss_sync->storeTimePair(aux_time, gnss_meas[0]->time); if (!_gnss_sync->isSync()) return; std::vector valid_meas; std::vector valid_ephems; for (auto obs : gnss_meas) { uint32_t sys = gnss_comm::satsys(obs->sat, NULL); if (sys != SYS_GPS && sys != SYS_GLO && sys != SYS_GAL && sys != SYS_BDS) continue; if (_gnss_data->sat2ephem.count(obs->sat) == 0) continue; if (obs->freqs.empty()) continue; int freq_idx = -1; gnss_comm::L1_freq(obs, &freq_idx); if (freq_idx < 0) continue; double obs_time = gnss_comm::time2sec(obs->time); std::map time2index = _gnss_data->sat2time_index.at(obs->sat); double ephem_time = EPH_VALID_SECONDS; size_t ephem_index = -1; for (auto ti : time2index) { if (std::fabs(ti.first - obs_time) < ephem_time) { ephem_time = std::fabs(ti.first - obs_time); ephem_index = ti.second; } } if (ephem_time >= EPH_VALID_SECONDS || ephem_index == -1) continue; const gnss_comm::EphemBasePtr& best_ephem = _gnss_data->sat2ephem.at(obs->sat).at(ephem_index); if (obs->psr_std[freq_idx] > _filter_params._gnss_psr_std_thres || obs->psr_std[freq_idx] > _filter_params._gnss_dopp_std_thres) { _gnss_data->sat_track_status[obs->sat] = 0; continue; } else { if (_gnss_data->sat_track_status.count(obs->sat) == 0) _gnss_data->sat_track_status[obs->sat] = 0; ++_gnss_data->sat_track_status.at(obs->sat); } if (_gnss_data->sat_track_status[obs->sat] < _filter_params._gnss_track_num_thres) continue; valid_meas.push_back(obs); valid_ephems.push_back(best_ephem); } if (valid_meas.size() <= 0) return; else _gnss_sync->bufferGnssMeas(valid_meas, valid_ephems); if (valid_meas.size() >= 4) { Eigen::Matrix pos_raw_spp = gnss_comm::psr_pos(valid_meas, valid_ephems, _gnss_data->latest_gnss_iono_params); if (pos_raw_spp.hasNaN() || pos_raw_spp.block<3, 1>(0, 0).norm() < 1e-03) return; Eigen::Vector4d vel_raw_spp; Eigen::Vector3d pos_ecef = pos_raw_spp.block<3, 1>(0, 0); vel_raw_spp = gnss_comm::dopp_vel(valid_meas, valid_ephems, pos_ecef); if (vel_raw_spp.hasNaN()) return; _gnss_sync->bufferSppMeas(valid_meas[0]->time, pos_raw_spp, vel_raw_spp); } } } ================================================ FILE: ingvio_estimator/src/GnssSync.cpp ================================================ /** This File is part of InGVIO, an invariant filter for mono/stereo visual- * inertial-raw GNSS navigation. * * Copyright (C) 2022 Changwu Liu (cwliu529@163.com, * lcw18@mails.tsinghua.edu.cn (valid until 2023)) * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program. If not, see . */ #include #include "GnssSync.h" namespace ingvio { void GnssSync::bufferGnssMeas(const std::vector& valid_meas, const std::vector& valid_ephems) { if (!this->isSync()) return; bool flag = false; while (_gnss_meas_buffer.size() > 100) { _gnss_meas_buffer.pop(); if (!flag) flag = true; } if (flag) std::cout << "[GnssSync]: Throw previous Gnss Meas in buffer due to exceeding max size!" << std::endl; _gnss_meas_buffer.push(std::make_pair(valid_meas, valid_ephems)); } void GnssSync::bufferSppMeas(const gnss_comm::gtime_t& gtime, const Eigen::Matrix& pos_spp, const Eigen::Vector4d& vel_spp) { if (!this->isSync()) return; bool flag = false; while (_spp_meas_buffer.size() > 100) { _spp_meas_buffer.pop(); if (!flag) flag = true; } if (flag) std::cout << "[GnssSync]: Throw previous Spp Meas in buffer due to exceeding max size!" << std::endl; _spp_meas_buffer.push(SppMeas(gtime, pos_spp, vel_spp)); } void GnssSync::storeTimePair(const double& curr_time, const gnss_comm::gtime_t& gnss_time) { if (this->isSync()) return; if (_rosidx_gnsstime.find(curr_time) == _rosidx_gnsstime.end()) _rosidx_gnsstime[curr_time] = gnss_time; if (_rosidx_gnsstime.size() >= 3 && _rosidx_header.size() >= 3) { double delta_time = INFINITY; double idx_g, idx_h; for (const auto& item_g : _rosidx_gnsstime) for (const auto& item_h : _rosidx_header) if (std::fabs(item_g.first - item_h.first) < delta_time) { idx_g = item_g.first; idx_h = item_h.first; delta_time = std::fabs(idx_g - idx_h); } if (delta_time < _unsync_thres) { _gnss2local_time_offset = _rosidx_header.at(idx_h) - gnss_comm::time2sec(_rosidx_gnsstime.at(idx_g)) - (idx_h - idx_g); _isSync = true; std::cout << "[GnssSync]: gnss time to local time offset = " << std::setprecision(10) << _gnss2local_time_offset << " (s) " << std::endl; } _rosidx_gnsstime.clear(); _rosidx_header.clear(); } } void GnssSync::storeTimePair(const double& curr_time, const double& header_time) { if (this->isSync()) return; if (_rosidx_header.find(curr_time) == _rosidx_header.end()) _rosidx_header[curr_time] = header_time; if (_rosidx_gnsstime.size() >= 3 && _rosidx_header.size() >= 3) { double delta_time = INFINITY; double idx_g, idx_h; for (const auto& item_g : _rosidx_gnsstime) for (const auto& item_h : _rosidx_header) if (std::fabs(item_g.first - item_h.first) < delta_time) { idx_g = item_g.first; idx_h = item_h.first; delta_time = std::fabs(idx_g - idx_h); } if (delta_time < _unsync_thres) { _gnss2local_time_offset = _rosidx_header.at(idx_h) - gnss_comm::time2sec(_rosidx_gnsstime.at(idx_g)) - (idx_h - idx_g); _isSync = true; std::cout << "[GnssSync]: gnss time to local time offset = " << std::setprecision(10) << _gnss2local_time_offset << " (s) " << std::endl; } _rosidx_gnsstime.clear(); _rosidx_header.clear(); } } bool GnssSync::getGnssMeasAt(const std_msgs::Header& header, GnssMeas& gnss_meas) { if (!isSync()) return false; double target_time = header.stamp.toSec(); bool flag = false; while (!_gnss_meas_buffer.empty()) { auto& g_m = _gnss_meas_buffer.front(); double g_time = gnss_comm::time2sec(g_m.first[0]->time) + _gnss2local_time_offset; if (g_time < target_time - _unsync_thres) { _gnss_meas_buffer.pop(); continue; } if (g_time >= target_time + _unsync_thres) break; std::swap(gnss_meas, g_m); _gnss_meas_buffer.pop(); flag = true; break; } return flag; } bool GnssSync::getSppAt(const std_msgs::Header& header, SppMeas& spp_meas) { if (!isSync()) return false; double target_time = header.stamp.toSec(); bool flag = false; while (!_spp_meas_buffer.empty()) { auto& s_m = _spp_meas_buffer.front(); double g_time = gnss_comm::time2sec(s_m.gtime) + _gnss2local_time_offset; if (g_time < target_time - _unsync_thres) { _spp_meas_buffer.pop(); continue; } if (g_time >= target_time + _unsync_thres) break; std::swap(spp_meas, s_m); _spp_meas_buffer.pop(); flag = true; break; } return flag; } void GnssSync::clearMeasBuffer() { this->clear(this->_gnss_meas_buffer); this->clear(this->_spp_meas_buffer); } void GnssSync::clearMeasBufferUntil(const std_msgs::Header& header) { if (!isSync()) return; double time_end = header.stamp.toSec(); while (!_gnss_meas_buffer.empty()) { auto& gnss_meas = _gnss_meas_buffer.front(); double meas_time = gnss_comm::time2sec(gnss_meas.first[0]->time) + _gnss2local_time_offset; if (meas_time > time_end) break; _gnss_meas_buffer.pop(); } while (!_spp_meas_buffer.empty()) { auto& spp_meas = _spp_meas_buffer.front(); double meas_time = gnss_comm::time2sec(spp_meas.gtime) + _gnss2local_time_offset; if (meas_time > time_end) break; _spp_meas_buffer.pop(); } } } ================================================ FILE: ingvio_estimator/src/GnssSync.h ================================================ /** This File is part of InGVIO, an invariant filter for mono/stereo visual- * inertial-raw GNSS navigation. * * Copyright (C) 2022 Changwu Liu (cwliu529@163.com, * lcw18@mails.tsinghua.edu.cn (valid until 2023)) * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program. If not, see . */ #pragma once #include #include #include "IngvioParams.h" namespace ingvio { typedef std::pair, std::vector> GnssMeas; struct SppMeas { SppMeas() { posSpp.setZero(); velSpp.setZero(); } SppMeas(const gnss_comm::gtime_t& time, const Eigen::Matrix& pos_spp, const Eigen::Vector4d& vel_spp) : gtime(time), posSpp(pos_spp), velSpp(vel_spp) {} ~SppMeas() = default; gnss_comm::gtime_t gtime; Eigen::Matrix posSpp; Eigen::Vector4d velSpp; }; class GnssSync { public: GnssSync(const IngvioParams& filter_params) : _unsync_thres(0.13) { _isSync = false; if (!filter_params._enable_gnss) return; if (filter_params._use_fix_time_offset) { _isSync = true; _gnss2local_time_offset = filter_params._gnss_local_offset; } } ~GnssSync() = default; GnssSync(const GnssSync&) = delete; GnssSync operator=(const GnssSync&) = delete; void bufferGnssMeas(const std::vector& valid_meas, const std::vector& valid_ephems); void bufferSppMeas(const gnss_comm::gtime_t& gtime, const Eigen::Matrix& pos_spp, const Eigen::Vector4d& vel_spp); bool isSync() const { return _isSync; } const double& getUnsyncTime() const { return _gnss2local_time_offset; } void storeTimePair(const double& curr_time, const gnss_comm::gtime_t& gnss_time); void storeTimePair(const double& curr_time, const double& header_time); bool getGnssMeasAt(const std_msgs::Header& header, GnssMeas& gnss_meas); bool getSppAt(const std_msgs::Header& header, SppMeas& spp_meas); void clearMeasBuffer(); void clearMeasBufferUntil(const std_msgs::Header& header); protected: bool _isSync; double _gnss2local_time_offset; double _unsync_thres; std::queue _gnss_meas_buffer; std::queue _spp_meas_buffer; std::map _rosidx_gnsstime; std::map _rosidx_header; template static void clear(std::queue& qu) { std::queue empty; std::swap(empty, qu); } }; } ================================================ FILE: ingvio_estimator/src/GnssUpdate.cpp ================================================ /** This File is part of InGVIO, an invariant filter for mono/stereo visual- * inertial-raw GNSS navigation. * * Copyright (C) 2022 Changwu Liu (cwliu529@163.com, * lcw18@mails.tsinghua.edu.cn (valid until 2023)) * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program. If not, see . */ #include #include "StateManager.h" #include "GvioAligner.h" #include "GnssUpdate.h" #include "GnssManager.h" namespace ingvio { void GnssUpdate::checkYofStatus(std::shared_ptr state, std::shared_ptr gvio_aligner) { if (!state->_state_params._enable_gnss || !gvio_aligner->isAlign()) return; if (state->_gnss.find(State::GNSSType::YOF) == state->_gnss.end()) StateManager::addGNSSVariable(state, State::GNSSType::YOF, gvio_aligner->getYawOffset(), state->_state_params._init_cov_yof); } void GnssUpdate::getSysInGnssMeas(const GnssMeas& gnss_meas) { _gnss_sys.clear(); _gnss_sys.insert(State::GNSSType::YOF); bool flag = false; for (const gnss_comm::ObsPtr& obs : gnss_meas.first) if (_gnss_sys.find(GnssManager::convertSatsys2GnssType(gnss_comm::satsys(obs->sat, NULL))) == _gnss_sys.end()) { flag = true; _gnss_sys.insert(GnssManager::convertSatsys2GnssType(gnss_comm::satsys(obs->sat, NULL))); } else continue; if (flag) _gnss_sys.insert(State::GNSSType::FS); } void GnssUpdate::removeUntrackedSys(std::shared_ptr state, const GnssMeas& gnss_meas) { if (!state->_state_params._enable_gnss) return; this->getSysInGnssMeas(gnss_meas); std::vector gstate_to_marg; for (auto& gnss_state : state->_gnss) if (_gnss_sys.find(gnss_state.first) == _gnss_sys.end()) gstate_to_marg.push_back(gnss_state.first); for (const auto& gs : gstate_to_marg) StateManager::margGNSSVariable(state, gs); } void GnssUpdate::updateTrackedSys(std::shared_ptr state, const GnssMeas& gnss_meas, std::shared_ptr gvio_aligner, const std::vector& iono_params) { if (!state->_state_params._enable_gnss || !gvio_aligner->isAlign()) return; if (gnss_meas.first.size() <= 0 || iono_params.size() != 8) return; if (!GnssManager::checkGnssStates(state)) return; std::vector all_sat_states = gnss_comm::sat_states(gnss_meas.first, gnss_meas.second); Eigen::Matrix xyzt; xyzt.block<3, 1>(0, 0) = gvio_aligner->getTenu2ecef()*GnssManager::calcTw2enu(state->_gnss.at(State::GNSSType::YOF)->value())*state->_extended_pose->valueTrans1(); xyzt.block<4, 1>(3, 0) = GnssManager::getClockbiasVec(state); Eigen::Matrix dopp; dopp.block<3, 1>(0, 0) = gvio_aligner->getRenu2ecef()*GnssManager::calcRw2enu(state->_gnss.at(State::GNSSType::YOF)->value())*state->_extended_pose->valueTrans2(); dopp(3, 0) = state->_gnss.at(State::GNSSType::FS)->value(); std::vector atmos_delay; std::vector all_sv_azel; Eigen::VectorXd res_pos; Eigen::VectorXd res_vel; Eigen::MatrixXd J_pos_ecef; Eigen::MatrixXd J_vel_ecef; gnss_comm::psr_res(xyzt, gnss_meas.first, all_sat_states, iono_params, res_pos, J_pos_ecef, atmos_delay, all_sv_azel); gnss_comm::dopp_res(dopp, xyzt.block<3, 1>(0, 0), gnss_meas.first, all_sat_states, res_vel, J_vel_ecef); std::vector> var_order; std::map, int> local_var_index; var_order.push_back(state->_extended_pose); local_var_index[state->_extended_pose] = 0; var_order.push_back(state->_gnss.at(State::GNSSType::YOF)); local_var_index[state->_gnss.at(State::GNSSType::YOF)] = 9; const int max_possible_rows = 2*gnss_meas.first.size(); const int max_possible_cols = state->_extended_pose->size() + 6; const Eigen::Matrix3d Rw2ecef = gvio_aligner->getRenu2ecef()*GnssManager::calcRw2enu(state); Eigen::VectorXd res(max_possible_rows); Eigen::MatrixXd H(max_possible_rows, max_possible_cols); Eigen::MatrixXd R(max_possible_rows, max_possible_rows); res.setZero(); H.setZero(); R.setZero(); int row_cnt = 0; int col_cnt = 10; for (int i = 0; i < res_pos.rows(); ++i) { const Eigen::Vector3d unit_rv2sv = -J_pos_ecef.block<1, 3>(i, 0).transpose(); const uint32_t sys = gnss_comm::satsys(all_sat_states[i]->sat_id, NULL); if (state->_gnss.find(GnssManager::satsys_to_gnsstype.at(sys)) == state->_gnss.end()) continue; auto cb_state = state->_gnss.at(GnssManager::satsys_to_gnsstype.at(sys)); Eigen::MatrixXd H_i(1, state->_extended_pose->size()+2); H_i.setZero(); H_i.block<1, 3>(0, 0) = unit_rv2sv.transpose()*Rw2ecef*skew(state->_extended_pose->valueTrans1()); H_i.block<1, 3>(0, 3) = -unit_rv2sv.transpose()*Rw2ecef; if (_is_adjust_yof) { H_i(0, 9) = -unit_rv2sv.transpose()*gvio_aligner->getRenu2ecef()*GnssManager::dotRw2enu(state)*state->_extended_pose->valueTrans1(); } H_i(0, 10) = 1.0; Eigen::VectorXd res_i(1); res_i(0) = -res_pos(i); std::vector> sub_order(3); sub_order[0] = state->_extended_pose; sub_order[1] = state->_gnss.at(State::GNSSType::YOF); sub_order[2] = cb_state; double ns = gnss_meas.second[i]->ura; int l1_idx = -1; gnss_comm::L1_freq(gnss_meas.first[i], &l1_idx); double npr = gnss_meas.first[i]->psr_std[l1_idx]; double sin_el = std::sin(all_sv_azel[i].y()); if (std::fabs(sin_el) < 1e-6) sin_el = 1e-6; double psr_noise = _psr_noise_amp*std::pow(ns*npr/(sin_el*sin_el), 0.5); if (_is_gnss_chi2_test && !testChiSquared(state, res_i, H_i, sub_order, psr_noise)) continue; res(row_cnt, 0) = res_i(0); R(row_cnt, row_cnt) = psr_noise*psr_noise; H.block<1, 9>(row_cnt, local_var_index.at(state->_extended_pose)) = H_i.block<1, 9>(0, 0); H(row_cnt, local_var_index.at(state->_gnss.at(State::GNSSType::YOF))) = H_i(0, 9); if (local_var_index.find(cb_state) == local_var_index.end()) { local_var_index[cb_state] = col_cnt; col_cnt += cb_state->size(); var_order.push_back(cb_state); } H(row_cnt, local_var_index.at(cb_state)) = 1.0; ++row_cnt; } auto cs_state = state->_gnss.at(State::GNSSType::FS); local_var_index[cs_state] = col_cnt; col_cnt += cs_state->size(); var_order.push_back(cs_state); for (int i = 0; i < res_vel.rows(); ++i) { const Eigen::Vector3d unit_rv2sv = -J_vel_ecef.block<1, 3>(i, 0).transpose(); const uint32_t sys = gnss_comm::satsys(all_sat_states[i]->sat_id, NULL); if (state->_gnss.find(GnssManager::satsys_to_gnsstype.at(sys)) == state->_gnss.end()) continue; std::vector> sub_order(3); sub_order[0] = state->_extended_pose; sub_order[1] = state->_gnss.at(State::GNSSType::YOF); sub_order[2] = state->_gnss.at(State::GNSSType::FS); Eigen::MatrixXd H_i(1, 11); H_i.setZero(); H_i.block<1, 3>(0, 0) = unit_rv2sv.transpose()*Rw2ecef*skew(state->_extended_pose->valueTrans2()); H_i.block<1, 3>(0, 6) = -unit_rv2sv.transpose()*Rw2ecef; if (_is_adjust_yof) { H_i(0, 9) = -unit_rv2sv.transpose()*gvio_aligner->getRenu2ecef()*GnssManager::dotRw2enu(state)*state->_extended_pose->valueTrans2(); } H_i(0, 10) = 1.0; Eigen::VectorXd res_i(1); res_i(0) = -res_vel(i); double ns = gnss_meas.second[i]->ura; int l1_idx = -1; const double obs_freq = gnss_comm::L1_freq(gnss_meas.first[i], &l1_idx); double ndp = gnss_meas.first[i]->dopp_std[l1_idx]*LIGHT_SPEED/obs_freq; double sin_el = std::sin(all_sv_azel[i].y()); if (std::fabs(sin_el) < 1e-6) sin_el = 1e-6; double dopp_noise = _dopp_noise_amp*std::pow(ns*ndp/(sin_el*sin_el), 0.5); if (_is_gnss_chi2_test && !testChiSquared(state, res_i, H_i, sub_order, dopp_noise)) continue; res(row_cnt, 0) = res_i(0); R(row_cnt, row_cnt) = dopp_noise*dopp_noise; H.block<1, 9>(row_cnt, local_var_index.at(state->_extended_pose)) = H_i.block<1, 9>(0, 0); H(row_cnt, local_var_index.at(state->_gnss.at(State::GNSSType::YOF))) = H_i(0, 9); H(row_cnt, local_var_index.at(state->_gnss.at(State::GNSSType::FS))) = H_i(0, 10); ++row_cnt; } if (row_cnt < max_possible_rows) { res.conservativeResize(row_cnt, 1); R.conservativeResize(row_cnt, row_cnt); H.conservativeResize(row_cnt, H.cols()); } if (col_cnt < max_possible_cols) H.conservativeResize(H.rows(), col_cnt); if (res.rows() <= 14 && _is_gnss_strong_reject && !testChiSquared(state, res, H, var_order, R, res.rows())) return; StateManager::ekfUpdate(state, var_order, H, res, R); // GnssManager::printYOF(state); } void GnssUpdate::getSysInSppMeas(const SppMeas& spp_meas) { _spp_sys.clear(); if (std::fabs(spp_meas.velSpp(3, 0)) > 1e-03) _spp_sys.insert(State::GNSSType::FS); for (int i = 0; i < 4; ++i) if (std::fabs(spp_meas.posSpp(3+i, 0)) > 1e-03) _spp_sys.insert(GnssManager::idx_to_gnsstype.at(i)); } void GnssUpdate::calcSysToAdd(const std::shared_ptr state, std::unordered_set& sys_to_add) { sys_to_add.clear(); for (const auto& gtype : _spp_sys) if (state->_gnss.find(gtype) == state->_gnss.end()) sys_to_add.insert(gtype); } void GnssUpdate::addNewTrackedSys(std::shared_ptr state, const GnssMeas& gnss_meas, const SppMeas& spp_meas, std::shared_ptr gvio_aligner, const std::vector& iono_params) { if (!state->_state_params._enable_gnss || !gvio_aligner->isAlign()) return; if (gnss_meas.first.size() <= 0 || iono_params.size() != 8) return; if (state->_gnss.find(State::GNSSType::YOF) == state->_gnss.end()) return; const Eigen::Matrix3d Rw2ecef = gvio_aligner->getRenu2ecef()*GnssManager::calcRw2enu(state); this->getSysInSppMeas(spp_meas); std::unordered_set sys_to_add; this->calcSysToAdd(state, sys_to_add); if (sys_to_add.size() == 0) return; std::vector all_sat_states = gnss_comm::sat_states(gnss_meas.first, gnss_meas.second); Eigen::Matrix xyzt; xyzt.block<3, 1>(0, 0) = gvio_aligner->getTenu2ecef()*GnssManager::calcTw2enu(state->_gnss.at(State::GNSSType::YOF)->value())*state->_extended_pose->valueTrans1(); xyzt.block<4, 1>(3, 0) = GnssManager::getClockbiasVec(state); for (const auto& item : sys_to_add) if (item != State::GNSSType::FS) { int idx_to_add = 3 + gnss_comm::sys2idx.at(GnssManager::gnsstype_to_satsys.at(item)); xyzt(idx_to_add, 0) = spp_meas.posSpp(idx_to_add, 0); } Eigen::Matrix dopp = Eigen::Matrix::Zero(); dopp.block<3, 1>(0, 0) = gvio_aligner->getRenu2ecef()*GnssManager::calcRw2enu(state->_gnss.at(State::GNSSType::YOF)->value())*state->_extended_pose->valueTrans2(); if (sys_to_add.find(State::GNSSType::FS) == sys_to_add.end()) { if (state->_gnss.find(State::GNSSType::FS) == state->_gnss.end()) std::cout << "[GnssUpdate]: Fs is either added or in the state!" << std::endl; else dopp(3, 0) = state->_gnss.at(State::GNSSType::FS)->value(); } else dopp(3, 0) = spp_meas.velSpp(3, 0); std::vector atmos_delay; std::vector all_sv_azel; Eigen::VectorXd res_pos; Eigen::VectorXd res_vel; Eigen::MatrixXd J_pos_ecef; Eigen::MatrixXd J_vel_ecef; gnss_comm::psr_res(xyzt, gnss_meas.first, all_sat_states, iono_params, res_pos, J_pos_ecef, atmos_delay, all_sv_azel); gnss_comm::dopp_res(dopp, xyzt.block<3, 1>(0, 0), gnss_meas.first, all_sat_states, res_vel, J_vel_ecef); for (const auto& gtype : sys_to_add) if (gtype == State::GNSSType::FS) { Eigen::VectorXd res = -res_vel; Eigen::MatrixXd Hf(res.rows(), 1); Hf.setOnes(); Eigen::MatrixXd Hx(res.rows(), state->_extended_pose->size()+1); Hx.setZero(); Eigen::MatrixXd batch_unit_rv2sv_T = -J_vel_ecef.block(0, 0, J_vel_ecef.rows(), 3); Hx.block(0, 0, Hx.rows(), 3) = batch_unit_rv2sv_T*Rw2ecef*skew(state->_extended_pose->valueTrans2()); Hx.block(0, 6, Hx.rows(), 3) = -batch_unit_rv2sv_T*Rw2ecef; if (_is_adjust_yof) { Hx.block(0, 9, Hx.rows(), 1) = -batch_unit_rv2sv_T*gvio_aligner->getRecef2enu()*GnssManager::dotRw2enu(state)*state->_extended_pose->valueTrans2(); } std::vector> x_order(2); x_order[0] = state->_extended_pose; x_order[1] = state->_gnss.at(State::GNSSType::YOF); std::shared_ptr fs = std::make_shared(); fs->setValue(spp_meas.velSpp(3, 0)); double avg_noise = 0.0; for (int i = 0; i < gnss_meas.second.size(); ++i) { double ns = gnss_meas.second[i]->ura; int l1_idx = -1; const double obs_freq = gnss_comm::L1_freq(gnss_meas.first[i], &l1_idx); double ndp = gnss_meas.first[i]->dopp_std[l1_idx]*LIGHT_SPEED/obs_freq; double sin_el = std::sin(all_sv_azel[i].y()); if (std::fabs(sin_el) < 1e-6) sin_el = 1e-6; avg_noise += ns*ndp/(sin_el*sin_el); } avg_noise /= gnss_meas.second.size(); avg_noise = _dopp_noise_amp*std::sqrt(avg_noise); if (!StateManager::addVariableDelayed(state, fs, x_order, Hx, Hf, res, avg_noise, 0.95, true)) continue; state->_gnss[State::GNSSType::FS] = fs; } else { Eigen::VectorXd res; Eigen::MatrixXd batch_unit_rv2sv_T; double avg_noise; this->getResJacobianOfSys(gtype, gnss_meas, all_sv_azel, res_pos, J_pos_ecef, res, batch_unit_rv2sv_T, avg_noise); avg_noise *= _psr_noise_amp; Eigen::MatrixXd Hf(res.rows(), 1); Hf.setOnes(); std::vector> x_order(2); x_order[0] = state->_extended_pose; x_order[1] = state->_gnss.at(State::GNSSType::YOF); Eigen::MatrixXd Hx(res.rows(), 10); Hx.setZero(); Hx.block(0, 0, Hx.rows(), 3) = batch_unit_rv2sv_T*Rw2ecef*skew(state->_extended_pose->valueTrans1()); Hx.block(0, 3, Hx.rows(), 3) = -batch_unit_rv2sv_T*Rw2ecef; if (_is_adjust_yof) { Hx.block(0, 9, Hx.rows(), 1) = -batch_unit_rv2sv_T*gvio_aligner->getRecef2enu()*GnssManager::dotRw2enu(state)*state->_extended_pose->valueTrans1(); } std::shared_ptr cb = std::make_shared(); cb->setValue(spp_meas.posSpp(3+gnss_comm::sys2idx.at(GnssManager::gnsstype_to_satsys.at(gtype)), 0)); if (!StateManager::addVariableDelayed(state, cb, x_order, Hx, Hf, res, avg_noise, 0.95, true)) continue; state->_gnss[gtype] = cb; } } void GnssUpdate::getResJacobianOfSys(const State::GNSSType& gtype, const GnssMeas& gnss_meas, const std::vector& all_sv_azel, const Eigen::VectorXd& res_total, const Eigen::MatrixXd& H_total, Eigen::VectorXd& res_sys, Eigen::MatrixXd& batch_unit_rv2sv_T_sys, double& noise) { assert(H_total.cols() == 7); const int col_idx = 3 + gnss_comm::sys2idx.at(GnssManager::gnsstype_to_satsys.at(gtype)); std::vector this_sys_row; for (int i = 0; i < H_total.rows(); ++i) if (H_total(i, col_idx) == 1.0) this_sys_row.push_back(i); res_sys = Eigen::VectorXd::Zero(this_sys_row.size()); batch_unit_rv2sv_T_sys = Eigen::MatrixXd::Zero(this_sys_row.size(), 3); int row_cnt = 0; noise = 0.0; for (const int& row_idx : this_sys_row) { res_sys(row_cnt) = -res_total(row_idx); batch_unit_rv2sv_T_sys.row(row_cnt) = -H_total.block<1, 3>(row_idx, 0); ++row_cnt; double ns = gnss_meas.second[row_idx]->ura; int l1_idx = -1; gnss_comm::L1_freq(gnss_meas.first[row_idx], &l1_idx); double npr = gnss_meas.first[row_idx]->psr_std[l1_idx]; double sin_el = std::sin(all_sv_azel[row_idx].y()); if (std::fabs(sin_el) < 1e-6) sin_el = 1e-6; noise += ns*npr/(sin_el*sin_el); } noise /= this_sys_row.size(); noise = std::sqrt(noise); } } ================================================ FILE: ingvio_estimator/src/GnssUpdate.h ================================================ /** This File is part of InGVIO, an invariant filter for mono/stereo visual- * inertial-raw GNSS navigation. * * Copyright (C) 2022 Changwu Liu (cwliu529@163.com, * lcw18@mails.tsinghua.edu.cn (valid until 2023)) * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program. If not, see . */ #pragma once #include #include #include "State.h" #include "Update.h" #include "IngvioParams.h" #include "GnssSync.h" namespace ingvio { class State; class GvioAligner; class GnssUpdate : public UpdateBase { public: using UpdateBase::UpdateBase; GnssUpdate(const IngvioParams& filter_params) : UpdateBase(filter_params._chi2_max_dof, filter_params._chi2_thres), _psr_noise_amp(filter_params._psr_noise_amp), _dopp_noise_amp(filter_params._dopp_noise_amp), _is_adjust_yof(filter_params._is_adjust_yof), _is_gnss_chi2_test(filter_params._is_gnss_chi2_test), _is_gnss_strong_reject(filter_params._is_gnss_strong_reject) {} virtual ~GnssUpdate() {} GnssUpdate(const GnssUpdate&) = delete; GnssUpdate operator=(const GnssUpdate&) = delete; void checkYofStatus(std::shared_ptr state, std::shared_ptr gvio_aligner); void removeUntrackedSys(std::shared_ptr state, const GnssMeas& gnss_meas); void updateTrackedSys(std::shared_ptr state, const GnssMeas& gnss_meas, std::shared_ptr gvio_aligner, const std::vector& iono_params); void addNewTrackedSys(std::shared_ptr state, const GnssMeas& gnss_meas, const SppMeas& spp_meas, std::shared_ptr gvio_aligner, const std::vector& iono_params); protected: double _psr_noise_amp = 1.0; double _dopp_noise_amp = 1.0; int _is_adjust_yof = 0; int _is_gnss_chi2_test = 0; int _is_gnss_strong_reject = 0; void getSysInGnssMeas(const GnssMeas& gnss_meas); void getSysInSppMeas(const SppMeas& spp_meas); void calcSysToAdd(const std::shared_ptr state, std::unordered_set& sys_to_add); void getResJacobianOfSys(const State::GNSSType& gtype, const GnssMeas& gnss_meas, const std::vector& all_sv_azel, const Eigen::VectorXd& res_total, const Eigen::MatrixXd& H_total, Eigen::VectorXd& res_sys, Eigen::MatrixXd& batch_unit_rv2sv_T_sys, double& noise); std::unordered_set _gnss_sys; std::unordered_set _spp_sys; }; } ================================================ FILE: ingvio_estimator/src/GvioAligner.cpp ================================================ /** This File is part of InGVIO, an invariant filter for mono/stereo visual- * inertial-raw GNSS navigation. * * Copyright (C) 2022 Changwu Liu (cwliu529@163.com, * lcw18@mails.tsinghua.edu.cn (valid until 2023)) * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program. If not, see . */ #include #include "PoseState.h" #include "Color.h" #include "GvioAligner.h" namespace ingvio { bool GvioAligner::isAlign() const { return _isAligned; } Eigen::Isometry3d GvioAligner::getTecef2w() const { Eigen::Isometry3d T_enu2w = Eigen::Isometry3d::Identity(); T_enu2w.linear() = this->getRenu2w(); return T_enu2w*this->getTecef2enu(); } Eigen::Isometry3d GvioAligner::getTw2ecef() const { Eigen::Isometry3d T_w2enu = Eigen::Isometry3d::Identity(); T_w2enu.linear() = this->getRw2enu(); return this->getTenu2ecef()*T_w2enu; } double GvioAligner::getYawOffset() const { return _yaw_offset; } Eigen::Isometry3d GvioAligner::getTecef2enu() const { return _T_enu2ecef.inverse(); } Eigen::Isometry3d GvioAligner::getTenu2ecef() const { return _T_enu2ecef; } Eigen::Matrix3d GvioAligner::getRecef2enu() const { return _T_enu2ecef.linear().transpose(); } Eigen::Matrix3d GvioAligner::getRenu2ecef() const { return _T_enu2ecef.linear(); } Eigen::Matrix3d GvioAligner::getRw2enu() const { return Eigen::AngleAxisd(_yaw_offset, Eigen::Vector3d::UnitZ()).toRotationMatrix(); } Eigen::Matrix3d GvioAligner::getRenu2w() const { return this->getRw2enu().transpose(); } void GvioAligner::batchAlign(const GnssMeas& gnss_meas, const std::shared_ptr epose, const std::vector& iono) { if (_isAligned) return; _iono_params.clear(); std::copy(iono.begin(), iono.end(), std::back_inserter(_iono_params)); if (_align_buffer.size() < _batch_size) _align_buffer.push_back(std::make_tuple(epose->valueTrans1(), epose->valueTrans2(), gnss_meas.first, gnss_meas.second)); else { Eigen::Vector2d hor_vel = Eigen::Vector2d::Zero(); std::for_each(_align_buffer.begin(), _align_buffer.end(), [&hor_vel](const AlignBufferItem& item) { hor_vel += std::get(item).head<2>().cwiseAbs(); } ); hor_vel /= _align_buffer.size(); if (hor_vel.norm() <= _vel_thres) { std::cout << color::setYellow << "[GvioAligner]: Horizontal velocity excitation not enough, waiting and restart ..." << color::resetColor << std::endl; _align_buffer.clear(); _isAligned = false; _num_all_meas = 0; _all_sat_states.clear(); return; } std::cout << color::setBlue << "[GvioAligner]: Start batch alignment ..." << color::resetColor << std::endl; _num_all_meas = 0; _all_sat_states.clear(); for (const auto& align_item : _align_buffer) { _all_sat_states.push_back( gnss_comm::sat_states(std::get(align_item), std::get(align_item))); _num_all_meas += std::get(align_item).size(); } Eigen::Matrix rough_anchor_ecef; rough_anchor_ecef.setZero(); if (!coarseLocalization(rough_anchor_ecef)) { _align_buffer.clear(); _isAligned = false; _num_all_meas = 0; _all_sat_states.clear(); return; } double yaw_offset = 0.0; double rcv_ddt = 0.0; if (!yawAlignment(rough_anchor_ecef.head<3>(), yaw_offset, rcv_ddt)) { _align_buffer.clear(); _isAligned = false; _num_all_meas = 0; _all_sat_states.clear(); return; } Eigen::Matrix refined_anchor_ecef; refined_anchor_ecef.setZero(); if (!anchorRefinement(yaw_offset, rcv_ddt, rough_anchor_ecef, refined_anchor_ecef)) { _align_buffer.clear(); _isAligned = false; _num_all_meas = 0; _all_sat_states.clear(); return; } _align_buffer.clear(); _num_all_meas = 0; _all_sat_states.clear(); _isAligned = true; Eigen::Matrix3d R_enu2ecef = gnss_comm::ecef2rotation(refined_anchor_ecef.head<3>()); _T_enu2ecef.linear() = R_enu2ecef; _T_enu2ecef.translation() = refined_anchor_ecef.head<3>(); _yaw_offset = yaw_offset; std::cout << color::setGreen << "[GvioAligner]: Yaw offset from north = " << yaw_offset*180.0/M_PI << " (deg)" << color::resetColor << std::endl; std::cout << color::setGreen << "[GvioAligner]: Refined anchor in ECEF = " << refined_anchor_ecef.head<3>().transpose() << " (m)" << color::resetColor << std::endl; } } bool GvioAligner::coarseLocalization(Eigen::Matrix& rough_anchor_ecef) { rough_anchor_ecef.setZero(); std::vector accumObs; std::vector accumEphems; for (const auto& align_item : _align_buffer) { std::copy(std::get(align_item).begin(), std::get(align_item).end(), std::back_inserter(accumObs)); std::copy(std::get(align_item).begin(), std::get(align_item).end(), std::back_inserter(accumEphems)); } Eigen::Matrix xyzt = gnss_comm::psr_pos(accumObs, accumEphems, this->_iono_params); if (xyzt.hasNaN() || xyzt.topRows<3>().norm() < 1e-06) { std::cout << color::setRed << "[GvioAligner]: Coarse anchor localization failed!" << color::resetColor << std::endl; return false; } for (int i = 0; i < 4; ++i) if (std::fabs(xyzt(i+3, 0)) < 1.0) xyzt(i+3, 0) = 0.0; rough_anchor_ecef = xyzt; return true; } bool GvioAligner::yawAlignment(const Eigen::Vector3d& rough_anchor_ecef, double& yaw_offset, double& rcv_ddt) { yaw_offset = 0.0; rcv_ddt = 0.0; double estYaw = 0.0; double estRcvDdt = 0.0; const Eigen::Matrix3d roughRenu2ecef = gnss_comm::ecef2rotation(rough_anchor_ecef); int iter = 0; double delta_yaw_norm = 1.0; while (iter <= _max_iter && delta_yaw_norm > _conv_epsilon) { Eigen::MatrixXd A(_num_all_meas, 2); A.setZero(); A.col(1).setOnes(); Eigen::VectorXd b(_num_all_meas); b.setZero(); Eigen::Matrix3d Rw2enu(Eigen::AngleAxisd(estYaw, Eigen::Vector3d::UnitZ())); Eigen::Matrix3d dotC3; dotC3 << -std::sin(estYaw), -std::cos(estYaw), 0.0, std::cos(estYaw), -std::sin(estYaw), 0.0, 0.0, 0.0, 0.0; int row_cnt = 0; for (int i = 0; i < _align_buffer.size(); ++i) { auto& align_item = _align_buffer[i]; Eigen::Vector4d vel_ddt_ecef; vel_ddt_ecef.head<3>() = roughRenu2ecef*Rw2enu*std::get(align_item); vel_ddt_ecef(3) = estRcvDdt; Eigen::VectorXd epochRes; Eigen::MatrixXd epochJ; gnss_comm::dopp_res(vel_ddt_ecef, rough_anchor_ecef, std::get(align_item), _all_sat_states[i], epochRes, epochJ); b.segment(row_cnt, std::get(align_item).size()) = epochRes; A.block(row_cnt, 0, std::get(align_item).size(), 1) = epochJ.leftCols(3)*roughRenu2ecef*dotC3*std::get(align_item); row_cnt += std::get(align_item).size(); } Eigen::VectorXd delta_yaw = -(A.transpose()*A).inverse()*A.transpose()*b; estYaw += delta_yaw(0); estRcvDdt += delta_yaw(1); delta_yaw_norm = delta_yaw.norm(); ++iter; } if (iter > _max_iter) { std::cout << color::setRed << "[GvioAligner]: Yaw alignment reaches max iter, failed!" << color::resetColor << std::endl; return false; } yaw_offset = estYaw; if (yaw_offset > M_PI) yaw_offset -= std::floor(estYaw/(2.0*M_PI)+0.5)*(2.0*M_PI); else if (yaw_offset < -M_PI) yaw_offset -= std::ceil(estYaw/(2.0*M_PI)-0.5)*(2.0*M_PI); rcv_ddt = estRcvDdt; return true; } bool GvioAligner::anchorRefinement(const double& yaw_offset, const double& rcv_ddt, const Eigen::Matrix& rough_anchor_ecef, Eigen::Matrix& refined_anchor_ecef) { refined_anchor_ecef = rough_anchor_ecef; const Eigen::Matrix3d alignedRw2enu = Eigen::AngleAxisd(yaw_offset, Eigen::Vector3d::UnitZ()).toRotationMatrix(); auto getRefinedRw2ecef = [&alignedRw2enu](const Eigen::Matrix& anchor_ecef) { return gnss_comm::ecef2rotation(anchor_ecef.head<3>())*alignedRw2enu; }; std::vector> spp(_align_buffer.size()); for (int i = 0; i < _align_buffer.size(); ++i) { const auto& align_item = _align_buffer[i]; Eigen::Matrix xyzt = gnss_comm::psr_pos( std::get(align_item), std::get(align_item), this->_iono_params); if (xyzt.hasNaN() || xyzt.head<3>().norm() < 1e-03) { std::cout << color::setRed << "[GvioAligner]: Anchor refinement failure due to unable to conduct SPP!" << color::resetColor << std::endl; return false; } spp[i] = xyzt; } int iter_refine = 0; while (iter_refine <= _max_iter) { Eigen::Vector3d anchor_pos = Eigen::Vector3d::Zero(); for (int i = 0; i < _align_buffer.size(); ++i) anchor_pos += spp[i].head<3>() - getRefinedRw2ecef(refined_anchor_ecef)*std::get(_align_buffer[i]); anchor_pos /= _align_buffer.size(); Eigen::Vector3d dx = anchor_pos - refined_anchor_ecef.head<3>(); refined_anchor_ecef.head<3>() = anchor_pos; if (dx.norm() > _conv_epsilon) break; ++iter_refine; } if (iter_refine > _max_iter) { std::cout << color::setRed << "[GvioAligner]: Anchor refinement failure reaching max iter!" << color::resetColor << std::endl; return false; } refined_anchor_ecef.tail<4>() = spp.rbegin()->tail<4>(); return true; } } ================================================ FILE: ingvio_estimator/src/GvioAligner.h ================================================ /** This File is part of InGVIO, an invariant filter for mono/stereo visual- * inertial-raw GNSS navigation. * * Copyright (C) 2022 Changwu Liu (cwliu529@163.com, * lcw18@mails.tsinghua.edu.cn (valid until 2023)) * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program. If not, see . */ #pragma once #include #include #include #include "IngvioParams.h" #include "GnssData.h" #include "GnssSync.h" namespace ingvio { class SE23; class GvioAligner { public: GvioAligner(const IngvioParams& filter_params) { _batch_size = filter_params._gv_align_batch_size; _max_iter = filter_params._gv_align_max_iter; _conv_epsilon = filter_params._gv_align_conv_epsilon; _vel_thres = filter_params._gv_align_vel_thres; } ~GvioAligner() = default; GvioAligner(const GvioAligner&) = delete; GvioAligner operator=(const GvioAligner&) = delete; bool isAlign() const; Eigen::Isometry3d getTecef2w() const; Eigen::Isometry3d getTw2ecef() const; Eigen::Isometry3d getTecef2enu() const; Eigen::Isometry3d getTenu2ecef() const; double getYawOffset() const; Eigen::Matrix3d getRecef2enu() const; Eigen::Matrix3d getRenu2ecef() const; Eigen::Matrix3d getRw2enu() const; Eigen::Matrix3d getRenu2w() const; void batchAlign(const GnssMeas& gnss_meas, const std::shared_ptr epose, const std::vector& iono); protected: int _batch_size; int _max_iter; double _conv_epsilon; double _vel_thres; bool _isAligned = false; Eigen::Isometry3d _T_enu2ecef; double _yaw_offset; typedef std::tuple, std::vector> AlignBufferItem; enum AlignBufferIndex {posVIO, velVIO, obsGNSS, ephemGNSS}; std::vector _align_buffer; std::vector _iono_params; int _num_all_meas; std::vector> _all_sat_states; bool coarseLocalization(Eigen::Matrix& rough_anchor_ecef); bool yawAlignment(const Eigen::Vector3d& rough_anchor_ecef, double& yaw_offset, double& rcv_ddt); bool anchorRefinement(const double& yaw_offset, const double& rcv_ddt, const Eigen::Matrix& rough_anchor_ecef, Eigen::Matrix& refined_anchor_ecef); }; } ================================================ FILE: ingvio_estimator/src/ImuPropagator.cpp ================================================ /** This File is part of InGVIO, an invariant filter for mono/stereo visual- * inertial-raw GNSS navigation. * * Copyright (C) 2022 Changwu Liu (cwliu529@163.com, * lcw18@mails.tsinghua.edu.cn (valid until 2023)) * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program. If not, see . */ #include #include "StateManager.h" #include "State.h" #include "ImuPropagator.h" namespace ingvio { void ImuPropagator::storeImu(const ImuCtrl& imu_ctrl) { if (_imu_ctrl_buffer.size() > _max_imu_buffer_size) { std::cout << "[ImuPropagator]: Exceeding imu max buffer size, throw curr imu ctrl!" << std::endl; return; } else _imu_ctrl_buffer.push_back(imu_ctrl); if (!_has_gravity_set && _init_imu_buffer_sp > 0) { if (_imu_ctrl_buffer.size() < _init_imu_buffer_sp) return; std::cout << "[ImuPropagator]: Start init gravity norm ..." << std::endl; Eigen::Vector3d sum_gravity = Eigen::Vector3d::Zero(); for (const auto& item : _imu_ctrl_buffer) sum_gravity += item._accel_raw; sum_gravity /= _imu_ctrl_buffer.size(); if (std::fabs(sum_gravity.norm()-_init_gravity)/_init_gravity > 0.02) { std::cout << "[ImuPropagator]: Keep Camera STEADY!! Reinit gravity ..." << std::endl; _imu_ctrl_buffer.clear(); _has_gravity_set = false; } else { _gravity = Eigen::Vector3d(0.0, 0.0, -sum_gravity.norm()); _quat_init = Eigen::Quaterniond::FromTwoVectors(-sum_gravity, _gravity); _has_gravity_set = true; std::cout << "[ImuPropagator]: Gravity in body init: gx = " << -sum_gravity.x() << " gy = " << -sum_gravity.y() << " gz = " << -sum_gravity.z() << std::endl; } } } bool ImuPropagator::getAvgQuat(Eigen::Quaterniond& quat_avg, int num_ctrls) { if (_imu_ctrl_buffer.size() == 0 || num_ctrls <= 0) { quat_avg.setIdentity(); return false; } Eigen::Vector3d sum_sf = Eigen::Vector3d::Zero(); int idx = 0; for (int i = _imu_ctrl_buffer.size()-1; i >= std::max(0, (int)_imu_ctrl_buffer.size()-num_ctrls); --i) { sum_sf += _imu_ctrl_buffer[i]._accel_raw; ++idx; } sum_sf /= idx; sum_sf.normalize(); quat_avg = Eigen::Quaterniond::FromTwoVectors(-sum_sf, Eigen::Vector3d(0, 0, -1)); return true; } void ImuPropagator::stateAndCovTransition(std::shared_ptr state, const ImuCtrl& imu_ctrl, double dt, Eigen::Matrix& Phi, Eigen::Matrix& G, bool isAnalytic) { Phi.setIdentity(); G.setZero(); Eigen::Matrix3d R_hat = state->_extended_pose->valueLinearAsMat(); Eigen::Vector3d p_hat = state->_extended_pose->valueTrans1(); Eigen::Vector3d v_hat = state->_extended_pose->valueTrans2(); G.block<3, 3>(0, 0) = R_hat; G.block<3, 3>(3, 0) = skew(p_hat)*R_hat; G.block<3, 3>(6, 0) = skew(v_hat)*R_hat; G.block<3, 3>(6, 3) = R_hat; G.block<3, 3>(9, 6) = Eigen::Matrix3d::Identity(); G.block<3, 3>(12, 9) = Eigen::Matrix3d::Identity(); if (isAnalytic) { const Eigen::Vector3d gyro_unbiased = imu_ctrl._gyro_raw - state->_bg->value(); const Eigen::Vector3d acc_unbiased = imu_ctrl._accel_raw - state->_ba->value(); state->_timestamp += dt; Eigen::Matrix3d Gamma0 = GammaFunc(dt*gyro_unbiased, 0); Eigen::Matrix3d Gamma1 = GammaFunc(dt*gyro_unbiased, 1); Eigen::Matrix3d Gamma2 = GammaFunc(dt*gyro_unbiased, 2); Eigen::Matrix3d R_hat_new = R_hat*Gamma0; state->_extended_pose->setValueLinearByMat(R_hat_new); Eigen::Vector3d v_hat_new = v_hat + _gravity*dt + R_hat*Gamma1*acc_unbiased*dt; state->_extended_pose->setValueTrans2(v_hat_new); Eigen::Vector3d p_hat_new = p_hat + v_hat*dt + 0.5*_gravity*std::pow(dt, 2) + R_hat*Gamma2*acc_unbiased*std::pow(dt, 2); state->_extended_pose->setValueTrans1(p_hat_new); if (state->_state_params._enable_gnss) for (int i = 0; i < 4; ++i) if (state->_gnss.find(static_cast(i)) != state->_gnss.end() && state->_gnss.find(State::GNSSType::FS) != state->_gnss.end()) { double clockbias_hat = state->_gnss.at(static_cast(i))->value(); double clockbias_hat_new = clockbias_hat + dt*state->_gnss.at(State::GNSSType::FS)->value(); state->_gnss.at(static_cast(i))->setValue(clockbias_hat_new); } Phi.block<3, 3>(3, 0) = 0.5*skew(_gravity)*std::pow(dt, 2); Phi.block<3, 3>(3, 6) = dt*Eigen::Matrix3d::Identity(); Phi.block<3, 3>(6, 0) = skew(_gravity)*dt; Phi.block<3, 3>(0, 9) = -R_hat*Gamma1*dt; Phi.block<3, 3>(6, 12) = -R_hat*Gamma1*dt; Phi.block<3, 3>(3, 12) = -R_hat*Gamma2*std::pow(dt, 2); Phi.block<3, 3>(6, 9) = -skew(v_hat_new)*R_hat*Gamma1*dt + R_hat*Psi1Func(gyro_unbiased, acc_unbiased, dt); Phi.block<3, 3>(3, 9) = -skew(p_hat_new)*R_hat*Gamma1*dt + R_hat*Psi2Func(gyro_unbiased, acc_unbiased, dt); } else { const Eigen::Vector3d gyro_unbiased = imu_ctrl._gyro_raw - state->_bg->value(); const Eigen::Vector3d acc_unbiased = imu_ctrl._accel_raw - state->_ba->value(); state->_timestamp += dt; Eigen::Quaterniond q_hat = state->_extended_pose->valueLinearAsQuat(); Eigen::Vector3d delta_angle = dt*gyro_unbiased; Eigen::Quaterniond q_hat_dt2 = q_hat*Eigen::AngleAxisd(delta_angle.norm()/2, delta_angle.normalized()); Eigen::Quaterniond q_hat_dt = q_hat*Eigen::AngleAxisd(delta_angle.norm(), delta_angle.normalized()); // k1 = f(tk, xk) Eigen::Vector3d k1_v = R_hat*acc_unbiased + _gravity; Eigen::Vector3d k1_p = v_hat; // k2 = f(tk+dt/2, xk+k1*dt/2) Eigen::Vector3d k2_v = q_hat_dt2.toRotationMatrix()*acc_unbiased + _gravity; Eigen::Vector3d k2_p = v_hat + k1_v*dt/2.0; // k3 = f(tk+dt/2, xk+k2*dt/2) Eigen::Vector3d k3_v = q_hat_dt2.toRotationMatrix()*acc_unbiased + _gravity; Eigen::Vector3d k3_p = v_hat + k2_v*dt/2.0; // k4 = f(tk+dt, xk+k3*dt) Eigen::Vector3d k4_v = q_hat_dt.toRotationMatrix()*acc_unbiased + _gravity; Eigen::Vector3d k4_p = v_hat + k3_v*dt; // update p and v nominal states using RK4 Eigen::Vector3d v_hat_new = v_hat + dt/6.0*(k1_v + 2.0*k2_v + 2.0*k3_v + k4_v); Eigen::Vector3d p_hat_new = p_hat + dt/6.0*(k1_p + 2.0*k2_p + 2.0*k3_p + k4_p); state->_extended_pose->setValueLinearByQuat(q_hat_dt); state->_extended_pose->setValueTrans1(p_hat_new); state->_extended_pose->setValueTrans2(v_hat_new); if (state->_state_params._enable_gnss) for (int i = 0; i < 4; ++i) if (state->_gnss.find(static_cast(i)) != state->_gnss.end() && state->_gnss.find(State::GNSSType::FS) != state->_gnss.end()) { double clockbias_hat = state->_gnss.at(static_cast(i))->value(); double clockbias_hat_new = clockbias_hat + dt*state->_gnss.at(State::GNSSType::FS)->value(); state->_gnss.at(static_cast(i))->setValue(clockbias_hat_new); } Eigen::Matrix F = Eigen::Matrix::Zero(); F.block<3, 3>(3, 6) = Eigen::Matrix3d::Identity(); F.block<3, 3>(6, 0) = skew(_gravity); F.block<3, 3>(0, 9) = -R_hat; F.block<3, 3>(3, 9) = -skew(p_hat)*R_hat; F.block<3, 3>(6, 9) = -skew(v_hat)*R_hat; F.block<3, 3>(6, 12) = -R_hat; Eigen::Matrix F2 = F*F/2.0; Eigen::Matrix F3 = F2*F/3.0; Phi = Eigen::Matrix::Identity() + F*dt + F2*dt*dt + F3*std::pow(dt, 3.0); } } void ImuPropagator::propagateUntil(std::shared_ptr state, double t_end, bool isAnalytic) { if (!_has_gravity_set || t_end <= state->_timestamp) return; if (_imu_ctrl_buffer.size() == 0) return; if (_imu_ctrl_buffer[0]._timestamp > t_end) return; int propa_cnt = 0; ImuCtrl last_imu_ctrl = _imu_ctrl_buffer[_imu_ctrl_buffer.size()-1]; for (int i = 0; i < _imu_ctrl_buffer.size(); ++i) { double ctrl_time = _imu_ctrl_buffer[i]._timestamp; if (ctrl_time < state->_timestamp) { ++propa_cnt; continue; } if (ctrl_time > t_end) break; ++propa_cnt; double dt = ctrl_time - state->_timestamp; if (dt < 1e-6) continue; Eigen::Matrix Phi; Eigen::Matrix G; last_imu_ctrl = _imu_ctrl_buffer[i]; this->stateAndCovTransition(state, _imu_ctrl_buffer[i], dt, Phi, G, isAnalytic); StateManager::propagateStateCov(state, Phi, G, dt); } if (state->_timestamp < t_end) { double dt_last = t_end - state->_timestamp; if (dt_last > 1e-06) { Eigen::Matrix Phi; Eigen::Matrix G; this->stateAndCovTransition(state, last_imu_ctrl, dt_last, Phi, G, isAnalytic); StateManager::propagateStateCov(state, Phi, G, dt_last); } else state->_timestamp = t_end; } _imu_ctrl_buffer.erase(_imu_ctrl_buffer.begin(), _imu_ctrl_buffer.begin()+propa_cnt); } void ImuPropagator::propagateAugmentAtEnd(std::shared_ptr state, double t_end, bool isAnalytic) { if (!_has_gravity_set) return; this->propagateUntil(state, t_end, isAnalytic); if (state->_timestamp < t_end) { std::cout << "[ImuPropagator]: Cannot propa to t_end due to no imu ctrl!" << std::endl; return; } else if (state->_timestamp > t_end) { std::cout << "[IMUPropagator]: Cannot propa because t_end < curr state time!" << std::endl; return; } StateManager::augmentSlidingWindowPose(state); } void ImuPropagator::propagateToExpectedPoseAndAugment(std::shared_ptr state, double t_end, const Eigen::Isometry3d& T_i2w) { if (!_has_gravity_set) return; state->_extended_pose->setValueLinearByMat(T_i2w.linear()); state->_extended_pose->setValueTrans1(T_i2w.translation()); state->_extended_pose->setValueTrans2(Eigen::Vector3d::Zero()); state->_bg->setIdentity(); state->_ba->setIdentity(); state->_camleft_imu_extrinsics->setIdentity(); state->_timestamp = t_end; StateManager::augmentSlidingWindowPose(state); } } ================================================ FILE: ingvio_estimator/src/ImuPropagator.h ================================================ /** This File is part of InGVIO, an invariant filter for mono/stereo visual- * inertial-raw GNSS navigation. * * Copyright (C) 2022 Changwu Liu (cwliu529@163.com, * lcw18@mails.tsinghua.edu.cn (valid until 2023)) * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program. If not, see . */ #pragma once #include #include #include #include #include #include #include #include "IngvioParams.h" #include "AuxGammaFunc.h" namespace ingvio { class State; class ImuCtrl { public: ImuCtrl() : _timestamp(-1), _accel_raw(Eigen::Vector3d::Zero()), _gyro_raw(Eigen::Vector3d::Zero()) {} ImuCtrl(const sensor_msgs::Imu::ConstPtr& imu_msg) { _timestamp = imu_msg->header.stamp.toSec(); tf::vectorMsgToEigen(imu_msg->angular_velocity, _gyro_raw); tf::vectorMsgToEigen(imu_msg->linear_acceleration, _accel_raw); } double _timestamp; Eigen::Vector3d _accel_raw; Eigen::Vector3d _gyro_raw; ImuCtrl operator+(const ImuCtrl& other_imu_ctrl) { ImuCtrl result; result._accel_raw = this->_accel_raw + other_imu_ctrl._accel_raw; result._gyro_raw = this->_gyro_raw + other_imu_ctrl._gyro_raw; return result; } ImuCtrl& operator=(const ImuCtrl& other_imu_ctrl) { this->_timestamp = other_imu_ctrl._timestamp; this->_accel_raw = other_imu_ctrl._accel_raw; this->_gyro_raw = other_imu_ctrl._gyro_raw; return *this; } void setRandom() { _accel_raw.setRandom(); _gyro_raw.setRandom(); } }; class ImuPropagator { public: ImuPropagator() : _has_gravity_set(true) { _init_imu_buffer_sp = -1; _init_gravity = 9.8; _gravity = Eigen::Vector3d(0.0, 0.0, -9.8); _quat_init.setIdentity(); } ImuPropagator(const IngvioParams& filter_params) : _has_gravity_set(false), _init_gravity(filter_params._init_gravity), _max_imu_buffer_size(filter_params._max_imu_buffer_size), _init_imu_buffer_sp(filter_params._init_imu_buffer_sp) { if (_init_imu_buffer_sp < 0) { _has_gravity_set = true; _gravity = Eigen::Vector3d(0.0, 0.0, -_init_gravity); _quat_init.setIdentity(); } } void reset(bool isResetGravity = false) { _has_gravity_set = !isResetGravity; _imu_ctrl_buffer.clear(); if (_init_imu_buffer_sp < 0) { _has_gravity_set = true; _gravity = Eigen::Vector3d(0.0, 0.0, -_init_gravity); _quat_init.setIdentity(); } } void storeImu(const ImuCtrl& imu_ctrl); void storeImu(const sensor_msgs::Imu::ConstPtr& imu_msg) { ImuCtrl tmp(imu_msg); this->storeImu(tmp); } bool getInitQuat(Eigen::Quaterniond& quat_init) const { if (!_has_gravity_set) { quat_init.setIdentity(); return false; } else { quat_init = _quat_init; return true; } } bool getAvgQuat(Eigen::Quaterniond& quat_avg, int num_ctrls = 1); const Eigen::Vector3d& getGravity() const { return _gravity; } const bool& isInit() const { return _has_gravity_set; } void stateAndCovTransition(std::shared_ptr state, const ImuCtrl& imu_ctrl, double dt, Eigen::Matrix& Phi, Eigen::Matrix& G, bool isAnalytic = true); void propagateUntil(std::shared_ptr state, double t_end, bool isAnalytic = true); void propagateAugmentAtEnd(std::shared_ptr state, double t_end, bool isAnalytic = true); void propagateToExpectedPoseAndAugment(std::shared_ptr state, double t_end, const Eigen::Isometry3d& T_i2w); protected: bool _has_gravity_set; double _init_gravity; int _max_imu_buffer_size; int _init_imu_buffer_sp; Eigen::Vector3d _gravity; Eigen::Quaterniond _quat_init; std::vector _imu_ctrl_buffer; }; } ================================================ FILE: ingvio_estimator/src/IngvioFilter.cpp ================================================ /** This File is part of InGVIO, an invariant filter for mono/stereo visual- * inertial-raw GNSS navigation. * * Copyright (C) 2022 Changwu Liu (cwliu529@163.com, * lcw18@mails.tsinghua.edu.cn (valid until 2023)) * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program. If not, see . */ #include #include #include "IngvioFilter.h" #include "State.h" #include "StateManager.h" #include "ImuPropagator.h" #include "Triangulator.h" #include "MapServerManager.h" #include "RemoveLostUpdate.h" #include "SwMargUpdate.h" #include "KeyframeUpdate.h" #include "LandmarkUpdate.h" #include "TicToc.h" #include "Color.h" #include "GnssData.h" #include "GnssSync.h" #include "GvioAligner.h" #include "GnssUpdate.h" namespace ingvio { void IngvioFilter::initIO() { _filter_params.readParams(_nh); if (_filter_params._cam_nums == 2) { _sub_stereo_frame = _nh.subscribe(_filter_params._feature_topic, 100, &IngvioFilter::callbackStereoFrame, this); } else if (_filter_params._cam_nums == 1) { _sub_mono_frame = _nh.subscribe(_filter_params._feature_topic, 100, &IngvioFilter::callbackMonoFrame, this); } else { std::cout << "[IngvioParams]: Cam num " << _filter_params._cam_nums << " not supported! Init as mono config!" << std::endl; _filter_params._cam_nums = 1; _sub_mono_frame = _nh.subscribe(_filter_params._feature_topic, 100, &IngvioFilter::callbackMonoFrame, this); } _sub_imu = _nh.subscribe(_filter_params._imu_topic, 500, &IngvioFilter::callbackIMU, this); _odom_w_pub = _nh.advertise("pose_w", 5); _path_w_pub = _nh.advertise("path_w", 1); _state = std::make_shared(_filter_params); _imu_propa = std::make_shared(_filter_params); _tri = std::make_shared(_filter_params); _map_server = std::make_shared(); _remove_lost_update = std::make_shared(_filter_params); _keyframe_update = std::make_shared(_filter_params); _sw_marg_update = std::make_shared(_filter_params); _landmark_update = std::make_shared(_filter_params); _gnss_data = std::make_shared(); _gnss_sync = std::make_shared(_filter_params); _gvio_aligner = std::make_shared(_filter_params); _gnss_update = std::make_shared(_filter_params); if (_filter_params._enable_gnss) { _sub_ephem = _nh.subscribe(_filter_params._gnss_ephem_topic, 100, &IngvioFilter::callbackEphem, this); _sub_glo_ephem = _nh.subscribe(_filter_params._gnss_glo_ephem_topic, 100, &IngvioFilter::callbackGloEphem, this); _sub_gnss_meas = _nh.subscribe(_filter_params._gnss_meas_topic, 100, &IngvioFilter::callbackGnssMeas, this); _sub_iono_params = _nh.subscribe(_filter_params._gnss_iono_params_topic, 100, &IngvioFilter::callbackIonoParams, this); _sub_rtk_gt = _nh.subscribe(_filter_params._rtk_gt_topic, 50, &IngvioFilter::callbackRtkGroundTruth, this); _odom_spp_pub = _nh.advertise("pose_spp", 5); _path_spp_pub = _nh.advertise("path_spp", 1); _odom_gt_pub = _nh.advertise("pose_gt", 5); _path_gt_pub = _nh.advertise("path_gt", 1); } _filter_params.printParams(); } void IngvioFilter::callbackMonoFrame(const feature_tracker::MonoFrameConstPtr& mono_frame_ptr) { if (_filter_params._enable_gnss && !_gnss_sync->isSync()) return; if (!_hasImageCome) { _hasImageCome = true; return; } if (!_hasInitState) return; const double target_time = mono_frame_ptr->header.stamp.toSec(); if (_state->_timestamp >= target_time) return; TicToc timer_mono; _imu_propa->propagateAugmentAtEnd(_state, target_time); if (_state->_timestamp < target_time) return; MapServerManager::collectMonoMeas(_map_server, _state, mono_frame_ptr); _remove_lost_update->updateStateMono(_state, _map_server, _tri); if (_filter_params._is_key_frame) { _keyframe_update->updateStateMono(_state, _map_server, _tri); if (_filter_params._max_lm_feats > 0) { _landmark_update->updateLandmarkMono(_state, _map_server); _landmark_update->initNewLandmarkMono(_state, _map_server, _tri, _filter_params._max_sw_clones); } _keyframe_update->cleanMonoObsAtMargTime(_state, _map_server); _keyframe_update->changeMSCKFAnchor(_state, _map_server); if (_filter_params._max_lm_feats > 0) { std::vector marg_kfs; _keyframe_update->getMargKfs(_state, marg_kfs); _landmark_update->changeLandmarkAnchor(_state, _map_server, marg_kfs); } _keyframe_update->margSwPose(_state); } else { _sw_marg_update->updateStateMono(_state, _map_server, _tri); if (_filter_params._max_lm_feats > 0) { _landmark_update->updateLandmarkMono(_state, _map_server); _landmark_update->initNewLandmarkMono(_state, _map_server, _tri, _filter_params._max_sw_clones); } _sw_marg_update->cleanMonoObsAtMargTime(_state, _map_server); _sw_marg_update->changeMSCKFAnchor(_state, _map_server); if (_filter_params._max_lm_feats > 0) _landmark_update->changeLandmarkAnchor(_state, _map_server); _sw_marg_update->margSwPose(_state); } MapServerManager::eraseInvalidFeatures(_map_server, _state); if (_filter_params._enable_gnss) { GnssMeas gnss_meas; SppMeas spp_meas; bool flag = false; if (_gnss_sync->getSppAt(mono_frame_ptr->header, spp_meas)) { flag = true; visualizeSpp(mono_frame_ptr->header, spp_meas); } if (_gnss_sync->getGnssMeasAt(mono_frame_ptr->header, gnss_meas)) { if (flag && !_gvio_aligner->isAlign()) _gvio_aligner->batchAlign(gnss_meas, _state->_extended_pose, _gnss_data->latest_gnss_iono_params); if (_gvio_aligner->isAlign()) { _gnss_update->checkYofStatus(_state, _gvio_aligner); // _gnss_update->removeUntrackedSys(_state, gnss_meas); _gnss_update->updateTrackedSys(_state, gnss_meas, _gvio_aligner, _gnss_data->latest_gnss_iono_params); if (flag) _gnss_update->addNewTrackedSys(_state, gnss_meas, spp_meas, _gvio_aligner, _gnss_data->latest_gnss_iono_params); } } } visualize(mono_frame_ptr->header); double time_mono = timer_mono.toc(); static double total_time = 0.0; static int total_cnt = 0; ++total_cnt; total_time += time_mono; if (time_mono < 50.0) std::cout << color::setGreen << "[IngvioFilter]: One loop mono callback: " << total_time/total_cnt << " (ms) " << color::resetColor << std::endl; else std::cout << color::setRed << "[IngvioFilter]: One loop mono callback: " << total_time/total_cnt << " (ms) " << color::resetColor << std::endl; } void IngvioFilter::callbackStereoFrame(const feature_tracker::StereoFrameConstPtr& stereo_frame_ptr) { if (_filter_params._enable_gnss && !_gnss_sync->isSync()) return; if (!_hasImageCome) { _hasImageCome = true; return; } if (!_hasInitState) return; const double target_time = stereo_frame_ptr->header.stamp.toSec(); if (_state->_timestamp >= target_time) return; TicToc timer_stereo; _imu_propa->propagateAugmentAtEnd(_state, target_time); if (_state->_timestamp < target_time) return; MapServerManager::collectStereoMeas(_map_server, _state, stereo_frame_ptr); _remove_lost_update->updateStateStereo(_state, _map_server, _tri); if (_filter_params._is_key_frame) { _keyframe_update->updateStateStereo(_state, _map_server, _tri); if (_filter_params._max_lm_feats > 0) { _landmark_update->updateLandmarkStereo(_state, _map_server); _landmark_update->initNewLandmarkStereo(_state, _map_server, _tri, _filter_params._max_sw_clones); } _keyframe_update->cleanStereoObsAtMargTime(_state, _map_server); _keyframe_update->changeMSCKFAnchor(_state, _map_server); if (_filter_params._max_lm_feats > 0) { std::vector marg_kfs; _keyframe_update->getMargKfs(_state, marg_kfs); _landmark_update->changeLandmarkAnchor(_state, _map_server, marg_kfs); } _keyframe_update->margSwPose(_state); } else { _sw_marg_update->updateStateStereo(_state, _map_server, _tri); if (_filter_params._max_lm_feats > 0) { _landmark_update->updateLandmarkStereo(_state, _map_server); _landmark_update->initNewLandmarkStereo(_state, _map_server, _tri, _filter_params._max_sw_clones); } _sw_marg_update->cleanStereoObsAtMargTime(_state, _map_server); _sw_marg_update->changeMSCKFAnchor(_state, _map_server); if (_filter_params._max_lm_feats > 0) _landmark_update->changeLandmarkAnchor(_state, _map_server); _sw_marg_update->margSwPose(_state); } MapServerManager::eraseInvalidFeatures(_map_server, _state); if (_filter_params._enable_gnss) { GnssMeas gnss_meas; SppMeas spp_meas; bool flag = false; if (_gnss_sync->getSppAt(stereo_frame_ptr->header, spp_meas)) { flag = true; visualizeSpp(stereo_frame_ptr->header, spp_meas); } if (_gnss_sync->getGnssMeasAt(stereo_frame_ptr->header, gnss_meas)) { if (flag && !_gvio_aligner->isAlign()) _gvio_aligner->batchAlign(gnss_meas, _state->_extended_pose, _gnss_data->latest_gnss_iono_params); if (_gvio_aligner->isAlign()) { _gnss_update->checkYofStatus(_state, _gvio_aligner); // _gnss_update->removeUntrackedSys(_state, gnss_meas); _gnss_update->updateTrackedSys(_state, gnss_meas, _gvio_aligner, _gnss_data->latest_gnss_iono_params); if (flag) _gnss_update->addNewTrackedSys(_state, gnss_meas, spp_meas, _gvio_aligner, _gnss_data->latest_gnss_iono_params); } } } visualize(stereo_frame_ptr->header); double time_stereo = timer_stereo.toc(); static double total_time = 0.0; static int total_cnt = 0; ++total_cnt; total_time += time_stereo; if (time_stereo < 50.0) std::cout << color::setGreen << "[IngvioFilter]: One loop stereo callback: " << total_time/total_cnt << " (ms) " << color::resetColor << std::endl; else std::cout << color::setRed << "[IngvioFilter]: One loop stereo callback: " << total_time/total_cnt << " (ms) " << color::resetColor << std::endl; } void IngvioFilter::callbackIMU(sensor_msgs::Imu::ConstPtr imu_msg) { double aux_time = ros::Time::now().toSec(); if (_filter_params._enable_gnss) { _gnss_sync->storeTimePair(aux_time, imu_msg->header.stamp.toSec()); if (!_gnss_sync->isSync()) return; } if (!_hasImageCome) return; _imu_propa->storeImu(imu_msg); if (_imu_propa->isInit() && !_hasInitState) { Eigen::Quaterniond init_quat; if (_imu_propa->getInitQuat(init_quat)) { _state->initStateAndCov(imu_msg->header.stamp.toSec(), init_quat); _hasInitState = true; } } } void IngvioFilter::visualize(const std_msgs::Header& header) { ros::Time time = header.stamp; Eigen::Isometry3d T_i2w = Eigen::Isometry3d::Identity(); T_i2w.linear() = _state->_extended_pose->valueLinearAsMat(); T_i2w.translation() = _state->_extended_pose->valueTrans1(); Eigen::Vector3d vel = _state->_extended_pose->valueTrans2(); if (T_i2w.linear().hasNaN() || T_i2w.translation().hasNaN() || vel.hasNaN()) return; tf::Transform T_i2w_tf; tf::transformEigenToTF(T_i2w, T_i2w_tf); _tf_pub.sendTransform(tf::StampedTransform(T_i2w_tf, time, "world", "uav")); nav_msgs::Odometry odom_w_msg; odom_w_msg.header.stamp = time; odom_w_msg.header.frame_id = "world"; odom_w_msg.child_frame_id = "uav"; tf::poseEigenToMsg(T_i2w, odom_w_msg.pose.pose); tf::vectorEigenToMsg(vel, odom_w_msg.twist.twist.linear); // ROS_INFO("Velocity vx = %f vy = %f vz = %f", vel(0), vel(1), vel(2)); // ROS_INFO("Relative global position x = %f y = %f z = %f", odom_msg.pose.pose.position.x, odom_msg.pose.pose.position.y, odom_msg.pose.pose.position.z); _odom_w_pub.publish(odom_w_msg); geometry_msgs::PoseStamped pose_w; pose_w.header.stamp = time; pose_w.header.frame_id = "world"; pose_w.pose.position.x = odom_w_msg.pose.pose.position.x; pose_w.pose.position.y = odom_w_msg.pose.pose.position.y; pose_w.pose.position.z = odom_w_msg.pose.pose.position.z; pose_w.pose.orientation.x = odom_w_msg.pose.pose.orientation.x; pose_w.pose.orientation.y = odom_w_msg.pose.pose.orientation.y; pose_w.pose.orientation.z = odom_w_msg.pose.pose.orientation.z; pose_w.pose.orientation.w = odom_w_msg.pose.pose.orientation.w; _path_w_msg.header.stamp = time; _path_w_msg.header.frame_id = "world"; _path_w_msg.poses.push_back(pose_w); _path_w_pub.publish(_path_w_msg); } void IngvioFilter::visualizeSpp(const std_msgs::Header& header, const SppMeas& spp_meas) { if (!_filter_params._enable_gnss || !_gnss_sync->isSync() || !_gvio_aligner->isAlign()) return; Eigen::Vector3d pos_spp_w = _gvio_aligner->getTecef2w()*spp_meas.posSpp.block<3, 1>(0, 0); if (pos_spp_w.hasNaN()) return; geometry_msgs::PoseStamped pose_spp; pose_spp.header.stamp = header.stamp; pose_spp.header.frame_id = "world"; pose_spp.pose.position.x = pos_spp_w.x(); pose_spp.pose.position.y = pos_spp_w.y(); pose_spp.pose.position.z = pos_spp_w.z(); pose_spp.pose.orientation.x = 0.0; pose_spp.pose.orientation.y = 0.0; pose_spp.pose.orientation.z = 0.0; pose_spp.pose.orientation.w = 1.0; _path_spp_msg.header.stamp = pose_spp.header.stamp; _path_spp_msg.header.frame_id = "world"; _path_spp_msg.poses.push_back(pose_spp); _path_spp_pub.publish(_path_spp_msg); Eigen::Isometry3d T_i2w = Eigen::Isometry3d::Identity(); T_i2w.translation() = pos_spp_w; Eigen::Vector3d vel_spp_w = _gvio_aligner->getRecef2enu()*spp_meas.velSpp.block<3, 1>(0, 0); if (vel_spp_w.hasNaN()) return; nav_msgs::Odometry odom_spp_msg; odom_spp_msg.header.stamp = header.stamp; odom_spp_msg.header.frame_id = "world"; odom_spp_msg.child_frame_id = "spp"; tf::poseEigenToMsg(T_i2w, odom_spp_msg.pose.pose); tf::vectorEigenToMsg(vel_spp_w, odom_spp_msg.twist.twist.linear); _odom_spp_pub.publish(odom_spp_msg); } } ================================================ FILE: ingvio_estimator/src/IngvioFilter.h ================================================ /** This File is part of InGVIO, an invariant filter for mono/stereo visual- * inertial-raw GNSS navigation. * * Copyright (C) 2022 Changwu Liu (cwliu529@163.com, * lcw18@mails.tsinghua.edu.cn (valid until 2023)) * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program. If not, see . */ #pragma once #include #include #include #include #include #include #include #include #include #include #include "IngvioParams.h" #include "MapServer.h" namespace ingvio { class State; class ImuPropagator; class Triangulator; class RemoveLostUpdate; class SwMargUpdate; class KeyframeUpdate; class LandmarkUpdate; class GnssData; class GnssSync; class GvioAligner; struct SppMeas; class GnssUpdate; class IngvioFilter { public: IngvioFilter(ros::NodeHandle& n) : _nh(n) {} ~IngvioFilter() = default; IngvioFilter(const IngvioFilter&) = delete; IngvioFilter operator=(const IngvioFilter&) = delete; void initIO(); protected: IngvioParams _filter_params; ros::NodeHandle _nh; ros::Subscriber _sub_mono_frame, _sub_stereo_frame, _sub_imu; ros::Publisher _odom_w_pub, _path_w_pub; tf::TransformBroadcaster _tf_pub; ros::Subscriber _sub_ephem, _sub_glo_ephem, _sub_gnss_meas, _sub_iono_params, _sub_rtk_gt; ros::Publisher _odom_spp_pub, _odom_gt_pub; ros::Publisher _path_spp_pub, _path_gt_pub; nav_msgs::Path _path_w_msg, _path_spp_msg, _path_gt_msg; bool _hasImageCome = false; bool _hasInitState = false; std::shared_ptr _state; std::shared_ptr _imu_propa; std::shared_ptr _tri; std::shared_ptr _map_server; std::shared_ptr _remove_lost_update; std::shared_ptr _sw_marg_update; std::shared_ptr _keyframe_update; std::shared_ptr _landmark_update; std::shared_ptr _gnss_data; std::shared_ptr _gnss_sync; std::shared_ptr _gvio_aligner; std::shared_ptr _gnss_update; void callbackMonoFrame(const feature_tracker::MonoFrameConstPtr& mono_frame_ptr); void callbackStereoFrame(const feature_tracker::StereoFrameConstPtr& stereo_frame_ptr); void callbackIMU(sensor_msgs::Imu::ConstPtr imu_msg); void callbackEphem(const gnss_comm::GnssEphemMsgConstPtr& ephem_msg); void callbackGloEphem(const gnss_comm::GnssGloEphemMsgConstPtr& glo_ephem_msg); void callbackIonoParams(const gnss_comm::StampedFloat64ArrayConstPtr& iono_msg); void callbackGnssMeas(const gnss_comm::GnssMeasMsgConstPtr& gnss_meas_msg); void callbackRtkGroundTruth(const sensor_msgs::NavSatFix::ConstPtr& nav_sat_msg); void visualize(const std_msgs::Header& header); void visualizeSpp(const std_msgs::Header& header, const SppMeas& spp_meas); }; typedef std::shared_ptr IngvioFilterPtr; } ================================================ FILE: ingvio_estimator/src/IngvioNode.cpp ================================================ /** This File is part of InGVIO, an invariant filter for mono/stereo visual- * inertial-raw GNSS navigation. * * Copyright (C) 2022 Changwu Liu (cwliu529@163.com, * lcw18@mails.tsinghua.edu.cn (valid until 2023)) * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program. If not, see . */ #include "IngvioFilter.h" using namespace ingvio; int main(int argc, char** argv) { ros::init(argc, argv, "ingvio_estimator"); ros::NodeHandle n("~"); ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info); IngvioFilterPtr ingvio_filter_ptr = std::make_shared(n); ingvio_filter_ptr->initIO(); ros::spin(); return 0; } ================================================ FILE: ingvio_estimator/src/IngvioParams.cpp ================================================ /** This File is part of InGVIO, an invariant filter for mono/stereo visual- * inertial-raw GNSS navigation. * * Copyright (C) 2022 Changwu Liu (cwliu529@163.com, * lcw18@mails.tsinghua.edu.cn (valid until 2023)) * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program. If not, see . */ #include "IngvioParams.h" #include namespace ingvio { void IngvioParams::readParams(ros::NodeHandle& nh) { _config_path = readParams(nh, "config_file"); this->readParams(_config_path); } void IngvioParams::readParams(const std::string& config_path) { cv::FileStorage fs(config_path, cv::FileStorage::READ); if (!fs.isOpened()) ROS_ERROR("[IngvioParams]: Wrong path to config file!"); _cam_nums = fs["cam_nums"]; if (_cam_nums >= 3) { ROS_WARN("[IngvioParams]: Camera number not available! Switch to mono!"); _cam_nums = 1; } fs["cam_left_file_path"] >> _cam_left_config_path; if (_cam_nums == 2) fs["cam_right_file_path"] >> _cam_right_config_path; fs["feature_topic"] >> _feature_topic; fs["imu_topic"] >> _imu_topic; _max_sw_clones = fs["max_sliding_window_poses"]; _max_lm_feats = fs["max_landmark_features"]; _is_key_frame = fs["is_key_frame"]; _enable_gnss = fs["enable_gnss"]; _noise_g = fs["noise_gyro"]; _noise_a = fs["noise_accel"]; _noise_bg = fs["noise_bias_gyro"]; _noise_ba = fs["noise_bias_accel"]; _init_cov_rot = fs["init_cov_rot"]; _init_cov_pos = fs["init_cov_pos"]; _init_cov_vel = fs["init_cov_vel"]; _init_cov_bg = fs["init_cov_bg"]; _init_cov_ba = fs["init_cov_ba"]; _init_cov_ext_rot = fs["init_cov_ext_rot"]; _init_cov_ext_pos = fs["init_cov_ext_pos"]; _init_gravity = fs["gravity_norm"]; _max_imu_buffer_size = fs["max_imu_buffer_size"]; _init_imu_buffer_sp = fs["init_imu_buffer_sp"]; _trans_thres = fs["trans_thres"]; _huber_epsilon = fs["huber_epsilon"]; _conv_precision = fs["conv_precision"]; _init_damping = fs["init_damping"]; _outer_loop_max_iter = fs["outer_loop_max_iter"]; _inner_loop_max_iter = fs["inner_loop_max_iter"]; _max_depth = fs["max_depth"]; _min_depth = fs["min_depth"]; _max_baseline_ratio = fs["max_baseline_ratio"]; _chi2_max_dof = fs["chi2_max_dof"]; _chi2_thres = fs["chi2_thres"]; _visual_noise = fs["visual_noise"]; _frame_select_interval = fs["frame_select_interval"]; if (_enable_gnss) { _noise_clockbias = fs["noise_rcv_clockbias"]; _noise_cb_rw = fs["noise_rcv_clockbias_randomwalk"]; _init_cov_rcv_clockbias = fs["init_cov_rcv_clockbias"]; _init_cov_rcv_clockbias_randomwalk = fs["init_cov_rcv_clockbias_randomwalk"]; _init_cov_yof = fs["init_cov_yof"]; fs["gnss_ephem_topic"] >> _gnss_ephem_topic; fs["gnss_glo_ephem_topic"] >> _gnss_glo_ephem_topic; fs["gnss_meas_topic"] >> _gnss_meas_topic; fs["gnss_iono_params_topic"] >> _gnss_iono_params_topic; fs["rtk_gt_topic"] >> _rtk_gt_topic; _gnss_elevation_thres = fs["gnss_elevation_thres"]; _gnss_psr_std_thres = fs["gnss_psr_std_thres"]; _gnss_dopp_std_thres = fs["gnss_dopp_std_thres"]; _gnss_track_num_thres = fs["gnss_track_num_thres"]; _use_fix_time_offset = fs["use_fix_time_offset"]; if (_use_fix_time_offset) _gnss_local_offset = fs["gnss_local_offset"]; _is_gnss_chi2_test = fs["gnss_chi2_test"]; _is_gnss_strong_reject = fs["gnss_strong_reject"]; _gv_align_batch_size = fs["gv_align_batch_size"]; _gv_align_max_iter = fs["gv_align_max_iter"]; _gv_align_conv_epsilon = fs["gv_align_conv_epsilon"]; _gv_align_vel_thres = fs["gv_align_vel_thres"]; _psr_noise_amp = fs["psr_noise_amp"]; _dopp_noise_amp = fs["dopp_noise_amp"]; _is_adjust_yof = fs["is_adjust_yof"]; } fs.release(); cv::FileStorage fs_cam_left(_cam_left_config_path, cv::FileStorage::READ); if (!fs_cam_left.isOpened()) ROS_ERROR("[IngvioParams]: Wrong path to left cam config file!"); cv::Mat cv_R1, cv_T1; fs_cam_left["extrinsicRotation"] >> cv_R1; fs_cam_left["extrinsicTranslation"] >> cv_T1; Eigen::Matrix3d eigen_R1; Eigen::Vector3d eigen_T1; cv::cv2eigen(cv_R1, eigen_R1); cv::cv2eigen(cv_T1, eigen_T1); _T_cl2i.linear() = eigen_R1; _T_cl2i.translation() = eigen_T1; fs_cam_left.release(); if (_cam_nums == 2) { cv::FileStorage fs_cam_right(_cam_right_config_path, cv::FileStorage::READ); if (!fs_cam_right.isOpened()) ROS_ERROR("[IngvioParams]: Wrong path to right cam config file!"); cv::Mat cv_R2, cv_T2; fs_cam_right["extrinsicRotation"] >> cv_R2; fs_cam_right["extrinsicTranslation"] >> cv_T2; Eigen::Matrix3d eigen_R2; Eigen::Vector3d eigen_T2; cv::cv2eigen(cv_R2, eigen_R2); cv::cv2eigen(cv_T2, eigen_T2); _T_cr2i.linear() = eigen_R2; _T_cr2i.translation() = eigen_T2; fs_cam_right.release(); } } void IngvioParams::printParams() { std::cout << "===== Ingvio Parameter List =====" << std::endl; std::cout << "Number of CAMS: " << _cam_nums << std::endl; std::cout << "InEKF_config_path: " << _config_path << std::endl; std::cout << "cam_left_file_path: " << _cam_left_config_path << std::endl; if (_cam_nums == 2) std::cout << "cam_right_file_path: " << _cam_right_config_path << std::endl; std::cout << "feature_topic: " << _feature_topic << std::endl; std::cout << "imu_topic: " << _imu_topic << std::endl; std::cout << "max_sliding_window_poses: " << _max_sw_clones << std::endl; std::cout << "is_key_frame: " << _is_key_frame << std::endl; std::cout << "max_landmark_features: " << _max_lm_feats << std::endl; std::cout << "enable_gnss: " << _enable_gnss << std::endl; std::cout << "noise_gyro: " << _noise_g << std::endl; std::cout << "noise_accel: " << _noise_a << std::endl; std::cout << "noise_bias_gyro: " << _noise_bg << std::endl; std::cout << "noise_bias_accel: " << _noise_ba << std::endl; if (_enable_gnss) { std::cout << "noise_rcv_clockbias: " << _noise_clockbias << std::endl; std::cout << "noise_rcv_clockbias_randomwalk: " << _noise_cb_rw << std::endl; } std::cout << "init_cov_rot: " << _init_cov_rot << std::endl; std::cout << "init_cov_pos: " << _init_cov_pos << std::endl; std::cout << "init_cov_vel: " << _init_cov_vel << std::endl; std::cout << "init_cov_bg: " << _init_cov_bg << std::endl; std::cout << "init_cov_ba: " << _init_cov_ba << std::endl; std::cout << "init_cov_ext_rot: " << _init_cov_ext_rot << std::endl; std::cout << "init_cov_ext_pos: " << _init_cov_ext_pos << std::endl; std::cout << "gravity_norm: " << _init_gravity << std::endl; std::cout << "max_imu_buffer_size: " << _max_imu_buffer_size << std::endl; std::cout << "init_imu_buffer_sp: " << _init_imu_buffer_sp << std::endl; std::cout << "trans_thres: " << _trans_thres << std::endl; std::cout << "huber_epsilon: " << _huber_epsilon << std::endl; std::cout << "conv_precision: " << _conv_precision << std::endl; std::cout << "init_damping: " << _init_damping << std::endl; std::cout << "outer_loop_max_iter: " << _outer_loop_max_iter << std::endl; std::cout << "inner_loop_max_iter: " << _inner_loop_max_iter << std::endl; std::cout << "max depth: " << _max_depth << std::endl; std::cout << "min_depth: " << _min_depth << std::endl; std::cout << "max_baseline_ratio: " << _max_baseline_ratio << std::endl; std::cout << "chi2_max_dof: " << _chi2_max_dof << std::endl; std::cout << "chi2_thres: " << _chi2_thres << std::endl; std::cout << "visual_noise: " << _visual_noise << std::endl; std::cout << "frame_select_interval: " << _frame_select_interval << std::endl; if (_enable_gnss) { std::cout << "init_cov_rcv_clockbias: " << _init_cov_rcv_clockbias << std::endl; std::cout << "init_cov_rcv_clockbias_randomwalk: " << _init_cov_rcv_clockbias_randomwalk << std::endl; std::cout << "init_cov_yof: " << _init_cov_yof << std::endl; std::cout << "gnss_ephem_topic: " << _gnss_ephem_topic << std::endl; std::cout << "gnss_glo_ephem_topic: " << _gnss_glo_ephem_topic << std::endl; std::cout << "gnss_meas_topic: " << _gnss_meas_topic << std::endl; std::cout << "gnss_iono_params_topic: " << _gnss_iono_params_topic << std::endl; std::cout << "rtk_gt_topic: " << _rtk_gt_topic << std::endl; std::cout << "gnss_elevation_thres: " << _gnss_elevation_thres << std::endl; std::cout << "gnss_psr_std_thres: " << _gnss_psr_std_thres << std::endl; std::cout << "gnss_dopp_std_thres: " << _gnss_dopp_std_thres << std::endl; std::cout << "gnss_track_num_thres: " << _gnss_track_num_thres << std::endl; std::cout << "use_fix_time_offset: " << _use_fix_time_offset << std::endl; if (_use_fix_time_offset) std::cout << "gnss_local_offset: " << _gnss_local_offset << std::endl; std::cout << "gnss_chi2_test: " << _is_gnss_chi2_test << std::endl; std::cout << "gnss_strong_reject: " << _is_gnss_strong_reject << std::endl; std::cout << "gv_align_batch_size: " << _gv_align_batch_size << std::endl; std::cout << "gv_align_max_iter: " << _gv_align_max_iter << std::endl; std::cout << "gv_align_conv_epsilon: " << _gv_align_conv_epsilon << std::endl; std::cout << "gv_align_vel_thres: " << _gv_align_vel_thres << std::endl; std::cout << "psr_noise_amp: " << _psr_noise_amp << std::endl; std::cout << "dopp_noise_amp: " << _dopp_noise_amp << std::endl; std::cout << "is_adjust_yof: " << _is_adjust_yof << std::endl; } std::cout << "===============================" << std::endl; } } ================================================ FILE: ingvio_estimator/src/IngvioParams.h ================================================ /** This File is part of InGVIO, an invariant filter for mono/stereo visual- * inertial-raw GNSS navigation. * * Copyright (C) 2022 Changwu Liu (cwliu529@163.com, * lcw18@mails.tsinghua.edu.cn (valid until 2023)) * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program. If not, see . */ #pragma once #include #include #include #include #include namespace ingvio { class IngvioParams { public: IngvioParams() : _T_cl2i(Eigen::Isometry3d::Identity()), _T_cr2i(Eigen::Isometry3d::Identity()) {} IngvioParams(const IngvioParams&) = delete; IngvioParams operator=(const IngvioParams&) = delete; ~IngvioParams() = default; std::string _config_path; int _cam_nums; std::string _cam_left_config_path; std::string _cam_right_config_path; Eigen::Isometry3d _T_cl2i, _T_cr2i; std::string _feature_topic; std::string _imu_topic; int _max_sw_clones; int _is_key_frame; int _max_lm_feats; int _enable_gnss; double _noise_g; double _noise_a; double _noise_bg; double _noise_ba; double _noise_clockbias; double _noise_cb_rw; double _init_cov_rot; double _init_cov_pos; double _init_cov_vel; double _init_cov_bg; double _init_cov_ba; double _init_cov_ext_rot; double _init_cov_ext_pos; double _init_cov_rcv_clockbias; double _init_cov_rcv_clockbias_randomwalk; double _init_cov_yof; double _init_gravity; int _max_imu_buffer_size; int _init_imu_buffer_sp; double _trans_thres; double _huber_epsilon; double _conv_precision; double _init_damping; int _outer_loop_max_iter; int _inner_loop_max_iter; double _max_depth; double _min_depth; double _max_baseline_ratio; int _chi2_max_dof; double _chi2_thres; double _visual_noise; int _frame_select_interval; std::string _gnss_ephem_topic; std::string _gnss_glo_ephem_topic; std::string _gnss_meas_topic; std::string _gnss_iono_params_topic; std::string _rtk_gt_topic; double _gnss_elevation_thres; double _gnss_psr_std_thres; double _gnss_dopp_std_thres; int _gnss_track_num_thres; int _use_fix_time_offset; double _gnss_local_offset; int _is_gnss_chi2_test; int _is_gnss_strong_reject; int _gv_align_batch_size; int _gv_align_max_iter; double _gv_align_conv_epsilon; double _gv_align_vel_thres; double _psr_noise_amp; double _dopp_noise_amp; int _is_adjust_yof; void readParams(ros::NodeHandle& nh); void readParams(const std::string& config_path); void printParams(); template static T readParams(ros::NodeHandle& n, std::string name); }; template T IngvioParams::readParams(ros::NodeHandle& n, std::string name) { T ans; if (n.getParam(name, ans)) { ROS_INFO_STREAM("Loaded " << name << ": " << ans); } else { ROS_ERROR_STREAM("Failed to load: " << name); n.shutdown(); } return ans; } } ================================================ FILE: ingvio_estimator/src/KeyframeUpdate.cpp ================================================ /** This File is part of InGVIO, an invariant filter for mono/stereo visual- * inertial-raw GNSS navigation. * * Copyright (C) 2022 Changwu Liu (cwliu529@163.com, * lcw18@mails.tsinghua.edu.cn (valid until 2023)) * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program. If not, see . */ #include #include #include #include #include #include "State.h" #include "StateManager.h" #include "MapServer.h" #include "MapServerManager.h" #include "Triangulator.h" #include "KeyframeUpdate.h" namespace ingvio { int KeyframeUpdate::_select_cnt = 0; void KeyframeUpdate::getMargKfs(const std::shared_ptr state, std::vector& marg_kfs) { if (state->_sw_camleft_poses.size() < _max_sw_poses || _max_sw_poses < 3) { marg_kfs.clear(); return; } if (state->_timestamp == _timestamp && _kfs.size() > 0) { assert(_kfs.size() == 2); marg_kfs = _kfs; return; } if (state->_sw_camleft_poses.size() > _max_sw_poses) { std::cout << "[KeyframeUpdate]: Current sw poses larger than max size!" << std::endl; std::exit(EXIT_FAILURE); } _timestamp = state->_timestamp; _kfs.clear(); const int rem = _max_sw_poses - 2; const int idx1 = 2 + KeyframeUpdate::_select_cnt; ++KeyframeUpdate::_select_cnt; KeyframeUpdate::_select_cnt = KeyframeUpdate::_select_cnt % rem; auto item1 = state->_sw_camleft_poses.rbegin(); for (int i = 0; i < idx1; ++i) ++item1; auto item2 = state->_sw_camleft_poses.rbegin(); ++item2; _kfs.push_back(item1->first); _kfs.push_back(item2->first); /* auto item2 = state->_sw_camleft_poses.rbegin(); for (int i = 0; i < 2; ++i) ++item2; double time1; if (KeyframeUpdate::_select_cnt == 0) { auto item1 = state->_sw_camleft_poses.begin(); for (int i = 0; i < 2; ++i) ++item1; time1 = item1->first; } else { auto item1 = state->_sw_camleft_poses.rbegin(); for (int i = 0; i < 3; ++i) ++item1; time1 = item1->first; } _kfs.push_back(time1); _kfs.push_back(item2->first); ++KeyframeUpdate::_select_cnt; KeyframeUpdate::_select_cnt = KeyframeUpdate::_select_cnt % 2; */ marg_kfs = _kfs; } void KeyframeUpdate::margSwPose(std::shared_ptr state) { std::vector marg_kfs; this->getMargKfs(state, marg_kfs); if (marg_kfs.size() == 0) return; for (const double& marg_time : marg_kfs) StateManager::margSlidingWindowPose(state, marg_time); } void KeyframeUpdate::generateSwVarOrder(const std::shared_ptr state, const std::vector& selected_timestamps, std::vector>& sw_var_order, std::map, int>& sw_index, std::vector>& sw_var_type) { sw_var_order.clear(); sw_index.clear(); sw_var_type.clear(); int cnt = 0; const auto& sw_poses = state->_sw_camleft_poses; for (const double& ts : selected_timestamps) { if (sw_poses.find(ts) == sw_poses.end()) { std::cout << "[KeyframeUpdate]: selected timestamp not in sw!" << std::endl; std::exit(EXIT_FAILURE); } sw_var_order.push_back(sw_poses.at(ts)); sw_var_type.push_back(sw_poses.at(ts)); sw_index[sw_poses.at(ts)] = 6*cnt; ++cnt; } } std::shared_ptr KeyframeUpdate::calcResJacobianSingleFeatSelectedMonoObs( const std::shared_ptr feature_info, const std::map>& sw_poses, const std::vector>& sw_var_order, const std::map, int>& sw_index_map, const std::vector& selected_timestamps, Eigen::VectorXd& res_block, Eigen::MatrixXd& H_block, Eigen::MatrixXd& H_anchor_block) { const int num_of_cols = 6*sw_var_order.size(); const int num_of_rows = 2*selected_timestamps.size(); Eigen::VectorXd res_block_tmp = Eigen::VectorXd::Zero(num_of_rows); Eigen::MatrixXd H_block_tmp = Eigen::MatrixXd::Zero(num_of_rows, num_of_cols); Eigen::MatrixXd H_anchor_block_tmp = Eigen::MatrixXd::Zero(num_of_rows, 6); Eigen::MatrixXd Hf_block_tmp = Eigen::MatrixXd::Zero(num_of_rows, 3); const Eigen::Vector3d pf_w = feature_info->_landmark->valuePosXyz(); const std::shared_ptr pose_anchor_ptr = feature_info->_landmark->getAnchoredPose(); int row_cnt = 0; for (const auto& time_of_obs : selected_timestamps) { if (sw_poses.find(time_of_obs) == sw_poses.end() || feature_info->_mono_obs.find(time_of_obs) == feature_info->_mono_obs.end()) continue; const std::shared_ptr& mono_obs_ptr = feature_info->_mono_obs.at(time_of_obs); const std::shared_ptr& pose_obs_ptr = sw_poses.at(time_of_obs); const Eigen::Matrix3d R_cm2w = pose_obs_ptr->valueLinearAsMat(); const Eigen::Vector3d p_cm = pose_obs_ptr->valueTrans(); const Eigen::Vector3d pf_cm = R_cm2w.transpose()*(pf_w-p_cm); Eigen::Matrix H_proj = Eigen::Matrix::Zero(); H_proj(0, 0) = 1.0/pf_cm.z(); H_proj(0, 2) = -pf_cm.x()/std::pow(pf_cm.z(), 2); H_proj(1, 1) = 1.0/pf_cm.z(); H_proj(1, 2) = -pf_cm.y()/std::pow(pf_cm.z(), 2); Eigen::MatrixXd H_pf2x = Eigen::MatrixXd::Zero(3, num_of_cols); Eigen::Matrix H_pf2anchor = Eigen::Matrix::Zero(); if (pose_obs_ptr != pose_anchor_ptr) { H_pf2x.block<3, 3>(0, sw_index_map.at(pose_obs_ptr)) = R_cm2w.transpose()*skew(pf_w); H_pf2anchor.block<3, 3>(0, 0) = -H_pf2x.block<3, 3>(0, sw_index_map.at(pose_obs_ptr)); } H_pf2x.block<3, 3>(0, sw_index_map.at(pose_obs_ptr)+3) = -R_cm2w.transpose(); Eigen::Matrix3d H_pf2pf = R_cm2w.transpose(); if (H_proj.hasNaN() || H_pf2x.hasNaN()) continue; H_block_tmp.block(row_cnt, 0, 2, num_of_cols) = H_proj*H_pf2x; H_anchor_block_tmp.block(row_cnt, 0, 2, 6) = H_proj*H_pf2anchor; Hf_block_tmp.block(row_cnt, 0, 2, 3) = H_proj*H_pf2pf; res_block_tmp.block(row_cnt, 0, 2, 1) = mono_obs_ptr->asVec()-Eigen::Vector2d(pf_cm.x()/pf_cm.z(), pf_cm.y()/pf_cm.z()); row_cnt += 2; } if (row_cnt < res_block.rows()) { res_block_tmp.conservativeResize(row_cnt, 1); H_block_tmp.conservativeResize(row_cnt, num_of_cols); H_anchor_block_tmp.conservativeResize(row_cnt, 6); Hf_block_tmp.conservativeResize(row_cnt, 3); } if (row_cnt <= 3) std::cout << "[KeyframeUpdate]: Warning! in calc one feat selected jacobian, num of rows <= 3 !" << std::endl; Eigen::JacobiSVD svd_helper(Hf_block_tmp, Eigen::ComputeFullU | Eigen::ComputeThinV); Eigen::MatrixXd V = svd_helper.matrixU().rightCols(row_cnt-3); H_block = V.transpose()*H_block_tmp; res_block = V.transpose()*res_block_tmp; H_anchor_block = V.transpose()*H_anchor_block_tmp; return pose_anchor_ptr; } void KeyframeUpdate::cleanMonoObsAtMargTime(std::shared_ptr state, std::shared_ptr map_server) { std::vector marg_kfs; this->getMargKfs(state, marg_kfs); if (marg_kfs.size() == 0) return; std::vector all_ids; for (const auto& item : *map_server) all_ids.push_back(item.first); std::vector ids_to_clean; for (const int& id : all_ids) for (const double& marg_time : marg_kfs) { if (map_server->at(id)->_mono_obs.find(marg_time) != map_server->at(id)->_mono_obs.end()) map_server->at(id)->_mono_obs.erase(marg_time); if (map_server->at(id)->_mono_obs.size() == 0) ids_to_clean.push_back(id); } for (int id : ids_to_clean) map_server->erase(id); } void KeyframeUpdate::changeMSCKFAnchor(std::shared_ptr state, std::shared_ptr map_server) { std::vector marg_kfs; this->getMargKfs(state, marg_kfs); if (marg_kfs.size() == 0) return; std::unordered_set> old_anchor_set; for (const double& marg_time : marg_kfs) old_anchor_set.insert(state->_sw_camleft_poses.at(marg_time)); const std::shared_ptr new_anchor = state->_sw_camleft_poses.rbegin()->second; std::vector ids_to_marg; for (auto& item : *map_server) { if (item.second->_ftype != FeatureInfo::FeatureType::MSCKF) continue; if (old_anchor_set.find(item.second->_landmark->getAnchoredPose()) != old_anchor_set.end()) { if (item.second->_isTri) { const Eigen::Vector3d pf = item.second->_landmark->valuePosXyz(); const Eigen::Vector3d body = new_anchor->valueLinearAsMat().transpose()*(pf - new_anchor->valueTrans()); if (body.z() <= 0.3) { ids_to_marg.push_back(item.first); continue; } item.second->_landmark->resetAnchoredPose(new_anchor, true); } else ids_to_marg.push_back(item.first); } } for (const int& id: ids_to_marg) map_server->erase(id); } std::shared_ptr KeyframeUpdate::calcResJacobianSingleFeatSelectedStereoObs( const std::shared_ptr feature_info, const std::map>& sw_poses, const std::vector>& sw_var_order, const std::map, int>& sw_index_map, const std::vector& selected_timestamps, const Eigen::Isometry3d& T_cl2cr, Eigen::VectorXd& res_block, Eigen::MatrixXd& H_block, Eigen::MatrixXd& H_anchor_block) { const Eigen::Matrix3d& R_cl2cr = T_cl2cr.linear(); const int num_of_cols = 6*sw_var_order.size(); const int num_of_rows = 4*selected_timestamps.size(); Eigen::VectorXd res_block_tmp = Eigen::VectorXd::Zero(num_of_rows); Eigen::MatrixXd H_block_tmp = Eigen::MatrixXd::Zero(num_of_rows, num_of_cols); Eigen::MatrixXd H_anchor_block_tmp = Eigen::MatrixXd::Zero(num_of_rows, 6); Eigen::MatrixXd Hf_block_tmp = Eigen::MatrixXd::Zero(num_of_rows, 3); const Eigen::Vector3d pf_w = feature_info->_landmark->valuePosXyz(); const std::shared_ptr pose_anchor_ptr = feature_info->_landmark->getAnchoredPose(); int row_cnt = 0; for (const auto& time_of_obs : selected_timestamps) { if (sw_poses.find(time_of_obs) == sw_poses.end() || feature_info->_stereo_obs.find(time_of_obs) == feature_info->_stereo_obs.end()) continue; const std::shared_ptr& stereo_obs_ptr = feature_info->_stereo_obs.at(time_of_obs); const std::shared_ptr& pose_obs_ptr = sw_poses.at(time_of_obs); const Eigen::Matrix3d R_cm2w = pose_obs_ptr->valueLinearAsMat(); const Eigen::Vector3d p_cm = pose_obs_ptr->valueTrans(); const Eigen::Vector3d pf_cm = R_cm2w.transpose()*(pf_w-p_cm); Eigen::Matrix H_proj = Eigen::Matrix::Zero(); H_proj(0, 0) = 1.0/pf_cm.z(); H_proj(0, 2) = -pf_cm.x()/std::pow(pf_cm.z(), 2); H_proj(1, 1) = 1.0/pf_cm.z(); H_proj(1, 2) = -pf_cm.y()/std::pow(pf_cm.z(), 2); const Eigen::Vector3d pf_cm_r = T_cl2cr*pf_cm; Eigen::Matrix H_proj_r = Eigen::Matrix::Zero(); H_proj_r(0, 0) = 1.0/pf_cm_r.z(); H_proj_r(0, 2) = -pf_cm_r.x()/std::pow(pf_cm_r.z(), 2); H_proj_r(1, 1) = 1.0/pf_cm_r.z(); H_proj_r(1, 2) = -pf_cm_r.y()/std::pow(pf_cm_r.z(), 2); Eigen::MatrixXd H_pf2x = Eigen::MatrixXd::Zero(3, num_of_cols); Eigen::Matrix H_pf2anchor = Eigen::Matrix::Zero(); if (pose_anchor_ptr != pose_obs_ptr) { H_pf2x.block<3, 3>(0, sw_index_map.at(pose_obs_ptr)) = R_cm2w.transpose()*skew(pf_w); H_pf2anchor.block<3, 3>(0, 0) = -H_pf2x.block<3, 3>(0, sw_index_map.at(pose_obs_ptr)); } H_pf2x.block<3, 3>(0, sw_index_map.at(pose_obs_ptr)+3) = -R_cm2w.transpose(); Eigen::Matrix3d H_pf2pf = R_cm2w.transpose(); if (H_proj.hasNaN() || H_pf2x.hasNaN()) continue; H_block_tmp.block(row_cnt, 0, 2, num_of_cols) = H_proj*H_pf2x; H_anchor_block_tmp.block(row_cnt, 0, 2, 6) = H_proj*H_pf2anchor; Hf_block_tmp.block(row_cnt, 0, 2, 3) = H_proj*H_pf2pf; H_block_tmp.block(row_cnt+2, 0, 2, num_of_cols) = H_proj_r*R_cl2cr*H_pf2x; H_anchor_block_tmp.block(row_cnt+2, 0, 2, 6) = H_proj_r*R_cl2cr*H_pf2anchor; Hf_block_tmp.block(row_cnt+2, 0, 2, 3) = H_proj_r*R_cl2cr*H_pf2pf; res_block_tmp.block(row_cnt, 0, 4, 1) = stereo_obs_ptr->asVec()-Eigen::Vector4d(pf_cm.x()/pf_cm.z(), pf_cm.y()/pf_cm.z(), pf_cm_r.x()/pf_cm_r.z(), pf_cm_r.y()/pf_cm_r.z()); row_cnt += 4; } if (row_cnt < res_block.rows()) { res_block_tmp.conservativeResize(row_cnt, 1); H_block_tmp.conservativeResize(row_cnt, num_of_cols); H_anchor_block_tmp.conservativeResize(row_cnt, 6); Hf_block_tmp.conservativeResize(row_cnt, 3); } if (row_cnt <= 3) std::cout << "[KeyframeUpdate]: Warning! in calc one feat selected jacobian, num of rows <= 3 !" << std::endl; Eigen::JacobiSVD svd_helper(Hf_block_tmp, Eigen::ComputeFullU | Eigen::ComputeThinV); Eigen::MatrixXd V = svd_helper.matrixU().rightCols(row_cnt-3); H_block = V.transpose()*H_block_tmp; H_anchor_block = V.transpose()*H_anchor_block_tmp; res_block = V.transpose()*res_block_tmp; return pose_anchor_ptr; } void KeyframeUpdate::updateStateMono(std::shared_ptr state, std::shared_ptr map_server, std::shared_ptr tri) { std::vector selected_timestamps; this->getMargKfs(state, selected_timestamps); if (selected_timestamps.size() == 0) return; std::vector> sw_var_order; std::map, int> sw_index_map; std::vector> sw_var_type; this->generateSwVarOrder(state, selected_timestamps, sw_var_order, sw_index_map, sw_var_type); std::vector update_ids; for (const auto& item : *map_server) { const auto& feat_info_ptr = item.second; if (feat_info_ptr->_ftype != FeatureInfo::FeatureType::MSCKF) continue; bool flag = false; const auto& mono_obs = item.second->_mono_obs; for (const double& ts : selected_timestamps) if (mono_obs.find(ts) == mono_obs.end()) { flag = true; break; } if (flag) continue; if (FeatureInfoManager::triangulateFeatureInfoMono(feat_info_ptr, tri, state)) update_ids.push_back(item.first); } if (update_ids.size() == 0) return; const int max_possible_cols = 6*state->_sw_camleft_poses.size(); const int max_possible_rows = update_ids.size()*(2*selected_timestamps.size()-3); Eigen::VectorXd res_large(max_possible_rows); Eigen::MatrixXd H_large(max_possible_rows, max_possible_cols); res_large.setZero(); H_large.setZero(); int row_cnt = 0; int col_cnt = 6*sw_var_type.size(); int feats_cnt = 0; for (int i = 0; i < update_ids.size(); ++i) { Eigen::VectorXd res_block; Eigen::MatrixXd H_block; Eigen::MatrixXd H_anchor_block; const auto& anchor_pose_ptr = this->calcResJacobianSingleFeatSelectedMonoObs( map_server->at(update_ids[i]), state->_sw_camleft_poses, sw_var_order, sw_index_map, selected_timestamps, res_block, H_block, H_anchor_block); bool flag = false; if (sw_index_map.find(anchor_pose_ptr) == sw_index_map.end()) { flag = true; sw_var_order.push_back(anchor_pose_ptr); sw_var_type.push_back(anchor_pose_ptr); sw_index_map[anchor_pose_ptr] = col_cnt; col_cnt += anchor_pose_ptr->size(); H_block.conservativeResize(H_block.rows(), H_block.cols() + 6); } H_block.block(0, sw_index_map.at(anchor_pose_ptr), H_block.rows(), 6) = H_anchor_block; if (!testChiSquared(state, res_block, H_block, sw_var_type, this->_noise, 2)) { if (flag) { sw_var_order.pop_back(); sw_var_type.pop_back(); sw_index_map.erase(anchor_pose_ptr); col_cnt -= anchor_pose_ptr->size(); } continue; } H_large.block(row_cnt, 0, H_block.rows(), col_cnt) = H_block; res_large.block(row_cnt, 0, res_block.rows(), 1) = res_block; row_cnt += res_block.rows(); ++feats_cnt; } if (row_cnt < max_possible_rows || col_cnt < max_possible_cols) { H_large.conservativeResize(row_cnt, col_cnt); res_large.conservativeResize(row_cnt, 1); } Eigen::MatrixXd H_thin; Eigen::VectorXd res_thin; if (H_large.rows() > H_large.cols()) { Eigen::SparseMatrix H_sparse = H_large.sparseView(); Eigen::SPQR> spqr_helper; spqr_helper.setSPQROrdering(SPQR_ORDERING_NATURAL); spqr_helper.compute(H_sparse); Eigen::MatrixXd H_temp; Eigen::VectorXd r_temp; (spqr_helper.matrixQ().transpose()*H_large).evalTo(H_temp); (spqr_helper.matrixQ().transpose()*res_large).evalTo(r_temp); H_thin = H_temp.topRows(col_cnt); res_thin = r_temp.head(col_cnt); } else { H_thin = H_large; res_thin = res_large; } /* std::cout << "[KeyframeUpdate]: Num of mono feats used in selected pose = " << feats_cnt << std::endl; * */ StateManager::ekfUpdate(state, sw_var_type, H_thin, res_thin, std::pow(this->_noise, 2)*Eigen::MatrixXd::Identity(res_thin.rows(), res_thin.rows())); } void KeyframeUpdate::updateStateStereo(std::shared_ptr state, std::shared_ptr map_server, std::shared_ptr tri) { std::vector selected_timestamps; this->getMargKfs(state, selected_timestamps); if (selected_timestamps.size() == 0) return; std::vector> sw_var_order; std::map, int> sw_index_map; std::vector> sw_var_type; this->generateSwVarOrder(state, selected_timestamps, sw_var_order, sw_index_map, sw_var_type); std::vector update_ids; for (const auto& item : *map_server) { const auto& feat_info_ptr = item.second; if (feat_info_ptr->_ftype != FeatureInfo::FeatureType::MSCKF) continue; bool flag = false; const auto& stereo_obs = item.second->_stereo_obs; for (const double& ts : selected_timestamps) if (stereo_obs.find(ts) == stereo_obs.end()) { flag = true; break; } if (flag) continue; if (FeatureInfoManager::triangulateFeatureInfoStereo(feat_info_ptr, tri, state)) update_ids.push_back(item.first); } if (update_ids.size() == 0) return; const int max_possible_cols = 6*state->_sw_camleft_poses.size(); const int max_possible_rows = update_ids.size()*(4*selected_timestamps.size()-3); Eigen::VectorXd res_large(max_possible_rows); Eigen::MatrixXd H_large(max_possible_rows, max_possible_cols); res_large.setZero(); H_large.setZero(); int row_cnt = 0; int col_cnt = 6*sw_var_type.size(); int feats_cnt = 0; for (int i = 0; i < update_ids.size(); ++i) { Eigen::VectorXd res_block; Eigen::MatrixXd H_block; Eigen::MatrixXd H_anchor_block; const auto& anchor_pose_ptr = this->calcResJacobianSingleFeatSelectedStereoObs( map_server->at(update_ids[i]), state->_sw_camleft_poses, sw_var_order, sw_index_map, selected_timestamps, state->_state_params._T_cl2cr, res_block, H_block, H_anchor_block); bool flag = false; if (sw_index_map.find(anchor_pose_ptr) == sw_index_map.end()) { flag = true; sw_var_order.push_back(anchor_pose_ptr); sw_var_type.push_back(anchor_pose_ptr); sw_index_map[anchor_pose_ptr] = col_cnt; col_cnt += anchor_pose_ptr->size(); H_block.conservativeResize(H_block.rows(), H_block.cols() + 6); } H_block.block(0, sw_index_map.at(anchor_pose_ptr), H_block.rows(), 6) = H_anchor_block; if (!testChiSquared(state, res_block, H_block, sw_var_type, this->_noise, 2)) { if (flag) { sw_var_order.pop_back(); sw_var_type.pop_back(); sw_index_map.erase(anchor_pose_ptr); col_cnt -= anchor_pose_ptr->size(); } continue; } H_large.block(row_cnt, 0, H_block.rows(), col_cnt) = H_block; res_large.block(row_cnt, 0, res_block.rows(), 1) = res_block; row_cnt += res_block.rows(); ++feats_cnt; } if (row_cnt < max_possible_rows || col_cnt < max_possible_cols) { H_large.conservativeResize(row_cnt, col_cnt); res_large.conservativeResize(row_cnt, 1); } Eigen::MatrixXd H_thin; Eigen::VectorXd res_thin; if (H_large.rows() > H_large.cols()) { Eigen::SparseMatrix H_sparse = H_large.sparseView(); Eigen::SPQR> spqr_helper; spqr_helper.setSPQROrdering(SPQR_ORDERING_NATURAL); spqr_helper.compute(H_sparse); Eigen::MatrixXd H_temp; Eigen::VectorXd r_temp; (spqr_helper.matrixQ().transpose()*H_large).evalTo(H_temp); (spqr_helper.matrixQ().transpose()*res_large).evalTo(r_temp); H_thin = H_temp.topRows(col_cnt); res_thin = r_temp.head(col_cnt); } else { H_thin = H_large; res_thin = res_large; } /* std::cout << "[KeyframeUpdate]: Num of stereo feats used in selected pose = " << feats_cnt << std::endl; */ StateManager::ekfUpdate(state, sw_var_type, H_thin, res_thin, std::pow(this->_noise, 2)*Eigen::MatrixXd::Identity(res_thin.rows(), res_thin.rows())); } void KeyframeUpdate::cleanStereoObsAtMargTime(std::shared_ptr state, std::shared_ptr map_server) { std::vector marg_kfs; this->getMargKfs(state, marg_kfs); std::vector all_ids; for (const auto& item : *map_server) all_ids.push_back(item.first); std::vector ids_to_clean; for (const int& id : all_ids) for (const double& marg_time : marg_kfs) { if (map_server->at(id)->_stereo_obs.find(marg_time) != map_server->at(id)->_stereo_obs.end()) map_server->at(id)->_stereo_obs.erase(marg_time); if (map_server->at(id)->_stereo_obs.size() == 0) ids_to_clean.push_back(id); } for (int id : ids_to_clean) map_server->erase(id); } } ================================================ FILE: ingvio_estimator/src/KeyframeUpdate.h ================================================ /** This File is part of InGVIO, an invariant filter for mono/stereo visual- * inertial-raw GNSS navigation. * * Copyright (C) 2022 Changwu Liu (cwliu529@163.com, * lcw18@mails.tsinghua.edu.cn (valid until 2023)) * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program. If not, see . */ #pragma once #include "IngvioParams.h" #include "MapServer.h" #include "Update.h" namespace ingvio { class State; class Triangulator; class KeyframeUpdate : public UpdateBase { public: using UpdateBase::UpdateBase; KeyframeUpdate(const IngvioParams& filter_params) : UpdateBase(filter_params._chi2_max_dof, filter_params._chi2_thres), _noise(filter_params._visual_noise), _max_sw_poses(filter_params._max_sw_clones) {} KeyframeUpdate(const KeyframeUpdate&) = delete; KeyframeUpdate operator=(const KeyframeUpdate&) = delete; virtual ~KeyframeUpdate() {} void getMargKfs(const std::shared_ptr state, std::vector& marg_kfs); void margSwPose(std::shared_ptr state); void changeMSCKFAnchor(std::shared_ptr state, std::shared_ptr map_server); void updateStateMono(std::shared_ptr state, std::shared_ptr map_server, std::shared_ptr tri); void cleanMonoObsAtMargTime(std::shared_ptr state, std::shared_ptr map_server); void updateStateStereo(std::shared_ptr state, std::shared_ptr map_server, std::shared_ptr tri); void cleanStereoObsAtMargTime(std::shared_ptr state, std::shared_ptr map_server); protected: double _noise; int _max_sw_poses; double _timestamp = -1.0; std::vector _kfs; static int _select_cnt; void generateSwVarOrder(const std::shared_ptr state, const std::vector& selected_timestamps, std::vector>& sw_var_order, std::map, int>& sw_index, std::vector>& sw_var_type); std::shared_ptr calcResJacobianSingleFeatSelectedMonoObs( const std::shared_ptr feature_info, const std::map>& sw_poses, const std::vector>& sw_var_order, const std::map, int>& sw_index_map, const std::vector& selected_timestamps, Eigen::VectorXd& res_block, Eigen::MatrixXd& H_block, Eigen::MatrixXd& H_anchor_block); std::shared_ptr calcResJacobianSingleFeatSelectedStereoObs( const std::shared_ptr feature_info, const std::map>& sw_poses, const std::vector>& sw_var_order, const std::map, int>& sw_index_map, const std::vector& selected_timestamps, const Eigen::Isometry3d& T_cl2cr, Eigen::VectorXd& res_block, Eigen::MatrixXd& H_block, Eigen::MatrixXd& H_anchor_block); }; } ================================================ FILE: ingvio_estimator/src/LandmarkUpdate.cpp ================================================ /** This File is part of InGVIO, an invariant filter for mono/stereo visual- * inertial-raw GNSS navigation. * * Copyright (C) 2022 Changwu Liu (cwliu529@163.com, * lcw18@mails.tsinghua.edu.cn (valid until 2023)) * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program. If not, see . */ #include #include "StateManager.h" #include "MapServer.h" #include "MapServerManager.h" #include "LandmarkUpdate.h" namespace ingvio { void LandmarkUpdate::updateLandmarkMono(std::shared_ptr state, std::shared_ptr map_server) { if (state->_anchored_landmarks.size() == 0) return; std::vector> var_order; var_order.push_back(state->_extended_pose); var_order.push_back(state->_camleft_imu_extrinsics); std::map, int> sub_var_col_idx; sub_var_col_idx[state->_extended_pose] = 0; sub_var_col_idx[state->_camleft_imu_extrinsics] = 9; const int max_possible_rows = 2*state->_anchored_landmarks.size(); const int max_possible_cols = 15 + 6*state->_sw_camleft_poses.size() + 3*state->_anchored_landmarks.size(); Eigen::MatrixXd H_large(max_possible_rows, max_possible_cols); Eigen::VectorXd res_large(max_possible_rows); H_large.setZero(); res_large.setZero(); int row_cnt = 0; int col_cnt = 15; for (const auto& item : state->_anchored_landmarks) { const int& id = item.first; if (map_server->find(id) == map_server->end()) { std::cout << "[LandmarkUpdate]: Landmark in state not in map server!" << std::endl; std::exit(EXIT_FAILURE); } else if (map_server->at(id)->_ftype != FeatureInfo::FeatureType::SLAM) { std::cout << "[LandmarkUpdate]: Landmark in state not marked SLAM type in map server!" << std::endl; std::exit(EXIT_FAILURE); } else if (map_server->at(id)->_mono_obs.find(state->_timestamp) == map_server->at(id)->_mono_obs.end()) { std::cout << "[LandmarkUpdate]: Landmark in state not tracked to curr time! Should have been marged before!" << std::endl; std::exit(EXIT_FAILURE); } if (map_server->at(id)->_landmark != state->_anchored_landmarks.at(id)) { std::cout << "[LandmarkUpdate]: Landmark ptr in state not the same as that in map server!" << std::endl; std::exit(EXIT_FAILURE); } std::shared_ptr anchored_pose_ptr = item.second->getAnchoredPose(); std::shared_ptr lm_ptr = item.second; Eigen::Vector2d res; Eigen::Matrix H_fj_epose; Eigen::Matrix H_fj_ext; Eigen::Matrix H_fj_anch; Eigen::Matrix H_fj_pf; this->calcResJacobianSingleLandmarkMono(map_server->at(id), state, res, H_fj_epose, H_fj_ext, H_fj_anch, H_fj_pf); std::vector> var_order_chi2(4); var_order_chi2[0] = state->_extended_pose; var_order_chi2[1] = state->_camleft_imu_extrinsics; var_order_chi2[2] = anchored_pose_ptr; var_order_chi2[3] = lm_ptr; Eigen::MatrixXd H_chi2(2, 24); H_chi2.block<2, 9>(0, 0) = H_fj_epose; H_chi2.block<2, 6>(0, 9) = H_fj_ext; H_chi2.block<2, 6>(0, 15) = H_fj_anch; H_chi2.block<2, 3>(0, 21) = H_fj_pf; if (!testChiSquared(state, res, H_chi2, var_order_chi2, this->_noise)) continue; res_large.block<2, 1>(row_cnt, 0) = res; H_large.block<2, 9>(row_cnt, sub_var_col_idx.at(state->_extended_pose)) = H_fj_epose; H_large.block<2, 6>(row_cnt, sub_var_col_idx.at(state->_camleft_imu_extrinsics)) = H_fj_ext; if (sub_var_col_idx.find(anchored_pose_ptr) == sub_var_col_idx.end()) { sub_var_col_idx[anchored_pose_ptr] = col_cnt; col_cnt += 6; var_order.push_back(anchored_pose_ptr); } H_large.block<2, 6>(row_cnt, sub_var_col_idx.at(anchored_pose_ptr)) = H_fj_anch; if (sub_var_col_idx.find(lm_ptr) == sub_var_col_idx.end()) { sub_var_col_idx[lm_ptr] = col_cnt; col_cnt += 3; var_order.push_back(lm_ptr); } H_large.block<2, 3>(row_cnt, sub_var_col_idx.at(lm_ptr)) = H_fj_pf; row_cnt += 2; } if (row_cnt == 0) return; /* std::cout << "[LandmarkUpdate]: row cnt = " << row_cnt << std::endl; std::cout << "[LandmarkUpdate]: col cnt = " << col_cnt << std::endl; */ if (max_possible_rows > row_cnt || max_possible_cols > col_cnt) { H_large.conservativeResize(row_cnt, col_cnt); res_large.conservativeResize(row_cnt, 1); } StateManager::ekfUpdate(state, var_order, H_large, res_large, std::pow(this->_noise, 2)*Eigen::MatrixXd::Identity(res_large.rows(), res_large.rows())); } void LandmarkUpdate::updateLandmarkMonoSw(std::shared_ptr state, std::shared_ptr map_server) { if (state->_anchored_landmarks.size() == 0) return; if (state->_timestamp != state->_sw_camleft_poses.rbegin()->first) { std::cout << "[LandmarkUpdate]: Last sw pose is not at curr time!" << std::endl; std::exit(EXIT_FAILURE); } const std::shared_ptr curr_pose = state->_sw_camleft_poses.rbegin()->second; std::vector> var_order; var_order.push_back(curr_pose); std::map, int> sub_var_col_idx; sub_var_col_idx[curr_pose] = 0; const int max_possible_rows = 2*state->_anchored_landmarks.size(); const int max_possible_cols = 6*state->_sw_camleft_poses.size() + 3*state->_anchored_landmarks.size(); Eigen::MatrixXd H_large(max_possible_rows, max_possible_cols); Eigen::VectorXd res_large(max_possible_rows); H_large.setZero(); res_large.setZero(); int row_cnt = 0; int col_cnt = 6; for (const auto& item : state->_anchored_landmarks) { const int& id = item.first; if (map_server->find(id) == map_server->end()) { std::cout << "[LandmarkUpdate]: Landmark in state not in map server!" << std::endl; std::exit(EXIT_FAILURE); } else if (map_server->at(id)->_ftype != FeatureInfo::FeatureType::SLAM) { std::cout << "[LandmarkUpdate]: Landmark in state not marked SLAM type in map server!" << std::endl; std::exit(EXIT_FAILURE); } else if (map_server->at(id)->_mono_obs.find(state->_timestamp) == map_server->at(id)->_mono_obs.end()) { std::cout << "[LandmarkUpdate]: Landmark in state not tracked to curr time! Should have been marged before!" << std::endl; std::exit(EXIT_FAILURE); } if (map_server->at(id)->_landmark != state->_anchored_landmarks.at(id)) { std::cout << "[LandmarkUpdate]: Landmark ptr in state not the same as that in map server!" << std::endl; std::exit(EXIT_FAILURE); } std::shared_ptr anchored_pose_ptr = item.second->getAnchoredPose(); std::shared_ptr lm_ptr = item.second; Eigen::Vector2d res; Eigen::Matrix H_fj_pose; Eigen::Matrix H_fj_anch; Eigen::Matrix H_fj_pf; this->calcResJacobianSingleLandmarkMono(map_server->at(id), state, res, H_fj_pose, H_fj_anch, H_fj_pf); if (curr_pose == anchored_pose_ptr) { std::cout << "[LandmarkUpdate]: Warning! Current pose is the same as anchored pose!" << std::endl; continue; } std::vector> var_order_chi2(3); var_order_chi2[0] = curr_pose; var_order_chi2[1] = anchored_pose_ptr; var_order_chi2[2] = lm_ptr; Eigen::MatrixXd H_chi2(2, 15); H_chi2.block<2, 6>(0, 0) = H_fj_pose; H_chi2.block<2, 6>(0, 6) = H_fj_anch; H_chi2.block<2, 3>(0, 12) = H_fj_pf; if (!testChiSquared(state, res, H_chi2, var_order_chi2, this->_noise)) continue; res_large.block<2, 1>(row_cnt, 0) = res; H_large.block<2, 6>(row_cnt, sub_var_col_idx.at(curr_pose)) = H_fj_pose; if (sub_var_col_idx.find(anchored_pose_ptr) == sub_var_col_idx.end()) { sub_var_col_idx[anchored_pose_ptr] = col_cnt; col_cnt += 6; var_order.push_back(anchored_pose_ptr); } H_large.block<2, 6>(row_cnt, sub_var_col_idx.at(anchored_pose_ptr)) = H_fj_anch; if (sub_var_col_idx.find(lm_ptr) == sub_var_col_idx.end()) { sub_var_col_idx[lm_ptr] = col_cnt; col_cnt += 3; var_order.push_back(lm_ptr); } H_large.block<2, 3>(row_cnt, sub_var_col_idx.at(lm_ptr)) = H_fj_pf; row_cnt += 2; } if (row_cnt == 0) return; if (max_possible_rows > row_cnt || max_possible_cols > col_cnt) { H_large.conservativeResize(row_cnt, col_cnt); res_large.conservativeResize(row_cnt, 1); } StateManager::ekfUpdate(state, var_order, H_large, res_large, std::pow(this->_noise, 2)*Eigen::MatrixXd::Identity(res_large.rows(), res_large.rows())); } void LandmarkUpdate::changeLandmarkAnchor(std::shared_ptr state, std::shared_ptr map_server) { double marg_time = state->nextMargTime(); if (marg_time == INFINITY || state->_sw_camleft_poses.find(marg_time) == state->_sw_camleft_poses.end()) return; const std::shared_ptr old_anchor = state->_sw_camleft_poses.at(marg_time); const double latest_sw = state->_sw_camleft_poses.rbegin()->first; const std::shared_ptr new_anchor = state->_sw_camleft_poses.at(latest_sw); std::vector ids_to_marg; for (auto& item : *map_server) { if (item.second->_ftype != FeatureInfo::FeatureType::SLAM) continue; if (item.second->_landmark->getAnchoredPose() == old_anchor) { const Eigen::Vector3d pf = item.second->_landmark->valuePosXyz(); const Eigen::Vector3d body = new_anchor->valueLinearAsMat().transpose()*(pf - new_anchor->valueTrans()); if (body.z() <= 0) { ids_to_marg.push_back(item.first); continue; } FeatureInfoManager::changeAnchoredPose(item.second, state, latest_sw); if (item.second->_landmark->getAnchoredPose() != new_anchor) ids_to_marg.push_back(item.first); } } for (const int& id: ids_to_marg) { StateManager::margAnchoredLandmarkInState(state, id); map_server->erase(id); } } void LandmarkUpdate::changeLandmarkAnchor(std::shared_ptr state, std::shared_ptr map_server, const std::vector& marg_kfs) { if (marg_kfs.size() == 0) return; std::unordered_set> old_anchor_set; for (const double& marg : marg_kfs) old_anchor_set.insert(state->_sw_camleft_poses.at(marg)); const std::shared_ptr new_anchor = state->_sw_camleft_poses.rbegin()->second; std::vector ids_to_marg; for (auto& item : *map_server) { if (item.second->_ftype != FeatureInfo::FeatureType::SLAM) continue; if (old_anchor_set.find(item.second->_landmark->getAnchoredPose()) != old_anchor_set.end()) { const Eigen::Vector3d pf = item.second->_landmark->valuePosXyz(); const Eigen::Vector3d body = new_anchor->valueLinearAsMat().transpose()*(pf - new_anchor->valueTrans()); if (body.z() <= 0) { ids_to_marg.push_back(item.first); continue; } FeatureInfoManager::changeAnchoredPose(item.second, state, state->_sw_camleft_poses.rbegin()->first); if (item.second->_landmark->getAnchoredPose() != new_anchor) ids_to_marg.push_back(item.first); } } for (const int& id: ids_to_marg) { StateManager::margAnchoredLandmarkInState(state, id); map_server->erase(id); } } void LandmarkUpdate::initNewLandmarkMono(std::shared_ptr state, std::shared_ptr map_server, std::shared_ptr tri, int min_init_poses) { if (state->_sw_camleft_poses.size() < min_init_poses) return; const int vac_num_lm = state->_state_params._max_landmarks - state->_anchored_landmarks.size(); if (vac_num_lm <= 0) return; std::vector ids_to_init; for (const auto& item : *map_server) { if (ids_to_init.size() >= vac_num_lm) break; if (item.second->_mono_obs.size() < min_init_poses || item.second->_ftype == FeatureInfo::FeatureType::SLAM) continue; if (!FeatureInfoManager::triangulateFeatureInfoMono(item.second, tri, state)) continue; ids_to_init.push_back(item.first); } if (ids_to_init.size() == 0) return; std::vector> sw_var_order; std::map, int> sw_index_map; std::vector> sw_var_type; this->generateSwVarOrder(state, sw_var_order, sw_index_map, sw_var_type); for (const int& id : ids_to_init) { Eigen::VectorXd res_block; Eigen::MatrixXd Hx_block; Eigen::MatrixXd Hf_block; this->calcResJacobianSingleFeatAllMonoObs(map_server->at(id), state->_sw_camleft_poses, sw_var_order, sw_index_map, res_block, Hx_block, Hf_block); if (!StateManager::addVariableDelayed(state, map_server->at(id)->_landmark, sw_var_type, Hx_block, Hf_block, res_block, this->_noise, 0.95, true)) continue; if (state->_anchored_landmarks.find(id) != state->_anchored_landmarks.end()) { std::cout << "[LandmarkUpdate]: The id intended to add already in state!" << std::endl; std::exit(EXIT_FAILURE); } state->_anchored_landmarks[id] = map_server->at(id)->_landmark; map_server->at(id)->_ftype = FeatureInfo::FeatureType::SLAM; } } void LandmarkUpdate::calcResJacobianSingleFeatAllMonoObs( const std::shared_ptr feature_info, const std::map>& sw_poses, const std::vector>& sw_var_order, const std::map, int>& sw_index_map, Eigen::VectorXd& res_block, Eigen::MatrixXd& Hx_block, Eigen::MatrixXd& Hf_block) { const int num_of_cols = 6*sw_var_order.size(); const int num_of_rows = 2*feature_info->numOfMonoFrames(); Eigen::VectorXd res_block_tmp = Eigen::VectorXd::Zero(num_of_rows); Eigen::MatrixXd H_block_tmp = Eigen::MatrixXd::Zero(num_of_rows, num_of_cols); Eigen::MatrixXd Hf_block_tmp = Eigen::MatrixXd::Zero(num_of_rows, 3); const Eigen::Vector3d pf_w = feature_info->_landmark->valuePosXyz(); int row_cnt = 0; for (const auto& item : feature_info->_mono_obs) { const double& time_of_obs = item.first; if (sw_poses.find(time_of_obs) == sw_poses.end()) continue; const std::shared_ptr& mono_obs_ptr = item.second; const std::shared_ptr& pose_obs_ptr = sw_poses.at(time_of_obs); const std::shared_ptr pose_anchor_ptr = feature_info->_landmark->getAnchoredPose(); const Eigen::Matrix3d R_cm2w = pose_obs_ptr->valueLinearAsMat(); const Eigen::Vector3d p_cm = pose_obs_ptr->valueTrans(); const Eigen::Vector3d pf_cm = R_cm2w.transpose()*(pf_w-p_cm); Eigen::Matrix H_proj = Eigen::Matrix::Zero(); H_proj(0, 0) = 1.0/pf_cm.z(); H_proj(0, 2) = -pf_cm.x()/std::pow(pf_cm.z(), 2); H_proj(1, 1) = 1.0/pf_cm.z(); H_proj(1, 2) = -pf_cm.y()/std::pow(pf_cm.z(), 2); Eigen::MatrixXd H_pf2x = Eigen::MatrixXd::Zero(3, num_of_cols); if (pose_obs_ptr != pose_anchor_ptr) { H_pf2x.block<3, 3>(0, sw_index_map.at(pose_obs_ptr)) = R_cm2w.transpose()*skew(pf_w); H_pf2x.block<3, 3>(0, sw_index_map.at(pose_anchor_ptr)) = -H_pf2x.block<3, 3>(0, sw_index_map.at(pose_obs_ptr)); } H_pf2x.block<3, 3>(0, sw_index_map.at(pose_obs_ptr)+3) = -R_cm2w.transpose(); Eigen::Matrix3d H_pf2pf = R_cm2w.transpose(); if (H_proj.hasNaN() || H_pf2x.hasNaN()) continue; H_block_tmp.block(row_cnt, 0, 2, num_of_cols) = H_proj*H_pf2x; Hf_block_tmp.block(row_cnt, 0, 2, 3) = H_proj*H_pf2pf; res_block_tmp.block(row_cnt, 0, 2, 1) = mono_obs_ptr->asVec()-Eigen::Vector2d(pf_cm.x()/pf_cm.z(), pf_cm.y()/pf_cm.z()); row_cnt += 2; } if (row_cnt < res_block.rows()) { res_block_tmp.conservativeResize(row_cnt, 1); H_block_tmp.conservativeResize(row_cnt, num_of_cols); Hf_block_tmp.conservativeResize(row_cnt, 3); } res_block = res_block_tmp; Hx_block = H_block_tmp; Hf_block = Hf_block_tmp; } void LandmarkUpdate::generateSwVarOrder(const std::shared_ptr state, std::vector>& sw_var_order, std::map, int>& sw_index, std::vector>& sw_var_type) { sw_var_order.clear(); sw_index.clear(); sw_var_type.clear(); int cnt = 0; for (const auto& item : state->_sw_camleft_poses) { sw_var_order.push_back(item.second); sw_var_type.push_back(item.second); sw_index[item.second] = 6*cnt; ++cnt; } } void LandmarkUpdate::calcResJacobianSingleLandmarkMono( const std::shared_ptr feature_info, const std::shared_ptr state, Eigen::Vector2d& res, Eigen::Matrix& H_fj_epose, Eigen::Matrix& H_fj_ext, Eigen::Matrix& H_fj_anch, Eigen::Matrix& H_fj_pf) { const Eigen::Vector3d pf_w = feature_info->_landmark->valuePosXyz(); const Eigen::Matrix3d R_i2w_T = state->_extended_pose->valueLinearAsMat().transpose(); const Eigen::Matrix3d R_cl2i_T = state->_camleft_imu_extrinsics->valueLinearAsMat().transpose(); const Eigen::Vector3d p_i2w = state->_extended_pose->valueTrans1(); const Eigen::Vector3d p_c2i = state->_camleft_imu_extrinsics->valueTrans(); const Eigen::Vector3d pf_i = R_i2w_T*(pf_w-p_i2w); const Eigen::Vector3d pf_cl = R_cl2i_T*(pf_i-p_c2i); const double curr_time = state->_timestamp; if (feature_info->_mono_obs.find(curr_time) == feature_info->_mono_obs.end() || feature_info->_ftype != FeatureInfo::FeatureType::SLAM) { std::cout << "[LandmarkUpdate]: Cannot calc curr slam feature mono res and jacobi!" << std::endl; std::exit(EXIT_FAILURE); } res = feature_info->_mono_obs.at(curr_time)->asVec() - Eigen::Vector2d(pf_cl.x()/pf_cl.z(), pf_cl.y()/pf_cl.z()); Eigen::Matrix H_proj = Eigen::Matrix::Zero(); H_proj(0, 0) = 1.0/pf_cl.z(); H_proj(1, 1) = 1.0/pf_cl.z(); H_proj(0, 2) = -pf_cl.x()/std::pow(pf_cl.z(), 2); H_proj(1, 2) = -pf_cl.y()/std::pow(pf_cl.z(), 2); Eigen::Matrix3d R_w2cl = R_cl2i_T*R_i2w_T; H_fj_epose.setZero(); H_fj_epose.block<2, 3>(0, 0) = H_proj*R_w2cl*skew(pf_w); H_fj_epose.block<2, 3>(0, 3) = -H_proj*R_w2cl; H_fj_ext.setZero(); H_fj_ext.block<2, 3>(0, 0) = H_proj*R_cl2i_T*skew(pf_i); H_fj_ext.block<2, 3>(0, 3) = -H_proj*R_cl2i_T; H_fj_anch.setZero(); H_fj_anch.block<2, 3>(0, 0) = -H_proj*R_w2cl*skew(pf_w); H_fj_pf = H_proj*R_w2cl; } void LandmarkUpdate::calcResJacobianSingleLandmarkMono( const std::shared_ptr feature_info, const std::shared_ptr state, Eigen::Vector2d& res, Eigen::Matrix& H_fj_pose, Eigen::Matrix& H_fj_anch, Eigen::Matrix& H_fj_pf) { const Eigen::Vector3d pf_w = feature_info->_landmark->valuePosXyz(); const double curr_time = state->_timestamp; const Eigen::Matrix3d R_cm_T = state->_sw_camleft_poses.at(curr_time)->valueLinearAsMat().transpose(); const Eigen::Vector3d p_cm = state->_sw_camleft_poses.at(curr_time)->valueTrans(); const Eigen::Vector3d pf_cl = R_cm_T*(pf_w-p_cm); if (feature_info->_mono_obs.find(curr_time) == feature_info->_mono_obs.end() || feature_info->_ftype != FeatureInfo::FeatureType::SLAM) { std::cout << "[LandmarkUpdate]: Cannot calc curr slam feature mono res and jacobi!" << std::endl; std::exit(EXIT_FAILURE); } res = feature_info->_mono_obs.at(curr_time)->asVec() - Eigen::Vector2d(pf_cl.x()/pf_cl.z(), pf_cl.y()/pf_cl.z()); Eigen::Matrix H_proj = Eigen::Matrix::Zero(); H_proj(0, 0) = 1.0/pf_cl.z(); H_proj(1, 1) = 1.0/pf_cl.z(); H_proj(0, 2) = -pf_cl.x()/std::pow(pf_cl.z(), 2); H_proj(1, 2) = -pf_cl.y()/std::pow(pf_cl.z(), 2); H_fj_pose.setZero(); H_fj_anch.setZero(); if (state->_sw_camleft_poses.at(curr_time) != feature_info->_landmark->getAnchoredPose()) { H_fj_pose.block<2, 3>(0, 0) = H_proj*R_cm_T*skew(pf_w); H_fj_anch.block<2, 3>(0, 0) = -H_fj_pose.block<2, 3>(0, 0); } H_fj_pose.block<2, 3>(0, 3) = -H_proj*R_cm_T; H_fj_pf = -H_fj_pose.block<2, 3>(0, 3); } void LandmarkUpdate::calcResJacobianSingleLandmarkStereo( const std::shared_ptr feature_info, const std::shared_ptr state, Eigen::Vector4d& res, Eigen::Matrix& H_fj_epose, Eigen::Matrix& H_fj_ext, Eigen::Matrix& H_fj_anch, Eigen::Matrix& H_fj_pf) { const Eigen::Vector3d pf_w = feature_info->_landmark->valuePosXyz(); const Eigen::Matrix3d R_i2w_T = state->_extended_pose->valueLinearAsMat().transpose(); const Eigen::Matrix3d R_cl2i_T = state->_camleft_imu_extrinsics->valueLinearAsMat().transpose(); const Eigen::Vector3d p_i2w = state->_extended_pose->valueTrans1(); const Eigen::Vector3d p_c2i = state->_camleft_imu_extrinsics->valueTrans(); const Eigen::Vector3d pf_i = R_i2w_T*(pf_w-p_i2w); const Eigen::Vector3d pf_cl = R_cl2i_T*(pf_i-p_c2i); const Eigen::Isometry3d& T_cl2cr = state->_state_params._T_cl2cr; const Eigen::Vector3d pf_cr = T_cl2cr*pf_cl; const double curr_time = state->_timestamp; if (feature_info->_stereo_obs.find(curr_time) == feature_info->_stereo_obs.end() || feature_info->_ftype != FeatureInfo::FeatureType::SLAM) { std::cout << "[LandmarkUpdate]: Cannot calc curr slam feature stereo res and jacobi!" << std::endl; std::exit(EXIT_FAILURE); } res = feature_info->_stereo_obs.at(curr_time)->asVec() - Eigen::Vector4d(pf_cl.x()/pf_cl.z(), pf_cl.y()/pf_cl.z(), pf_cr.x()/pf_cr.z(), pf_cr.y()/pf_cr.z()); Eigen::Matrix H_proj = Eigen::Matrix::Zero(); H_proj(0, 0) = 1.0/pf_cl.z(); H_proj(1, 1) = 1.0/pf_cl.z(); H_proj(0, 2) = -pf_cl.x()/std::pow(pf_cl.z(), 2); H_proj(1, 2) = -pf_cl.y()/std::pow(pf_cl.z(), 2); Eigen::Matrix H_proj_r = Eigen::Matrix::Zero(); H_proj_r(0, 0) = 1.0/pf_cr.z(); H_proj_r(1, 1) = 1.0/pf_cr.z(); H_proj_r(0, 2) = -pf_cr.x()/std::pow(pf_cr.z(), 2); H_proj_r(1, 2) = -pf_cr.y()/std::pow(pf_cr.z(), 2); Eigen::Matrix3d R_w2cl = R_cl2i_T*R_i2w_T; H_fj_epose.setZero(); H_fj_epose.block<2, 3>(0, 0) = H_proj*R_w2cl*skew(pf_w); H_fj_epose.block<2, 3>(0, 3) = -H_proj*R_w2cl; H_fj_epose.block<2, 3>(2, 0) = H_proj_r*T_cl2cr.linear()*R_w2cl*skew(pf_w); H_fj_epose.block<2, 3>(2, 3) = -H_proj_r*T_cl2cr.linear()*R_w2cl; H_fj_ext.setZero(); H_fj_ext.block<2, 3>(0, 0) = H_proj*R_cl2i_T*skew(pf_i); H_fj_ext.block<2, 3>(0, 3) = -H_proj*R_cl2i_T; H_fj_ext.block<2, 3>(2, 0) = H_proj_r*T_cl2cr.linear()*R_cl2i_T*skew(pf_i); H_fj_ext.block<2, 3>(2, 3) = -H_proj_r*T_cl2cr.linear()*R_cl2i_T; H_fj_anch.setZero(); H_fj_anch.block<2, 3>(0, 0) = -H_proj*R_w2cl*skew(pf_w); H_fj_anch.block<2, 3>(2, 0) = -H_proj_r*T_cl2cr.linear()*skew(pf_w); H_fj_pf.setZero(); H_fj_pf.block<2, 3>(0, 0) = H_proj*R_w2cl; H_fj_pf.block<2, 3>(2, 0) = H_proj_r*T_cl2cr.linear()*R_w2cl; } void LandmarkUpdate::updateLandmarkStereo(std::shared_ptr state, std::shared_ptr map_server) { if (state->_anchored_landmarks.size() == 0) return; std::vector> var_order; var_order.push_back(state->_extended_pose); var_order.push_back(state->_camleft_imu_extrinsics); std::map, int> sub_var_col_idx; sub_var_col_idx[state->_extended_pose] = 0; sub_var_col_idx[state->_camleft_imu_extrinsics] = 9; const int max_possible_rows = 4*state->_anchored_landmarks.size(); const int max_possible_cols = 15 + 6*state->_sw_camleft_poses.size() + 3*state->_anchored_landmarks.size(); Eigen::MatrixXd H_large(max_possible_rows, max_possible_cols); Eigen::VectorXd res_large(max_possible_rows); H_large.setZero(); res_large.setZero(); int row_cnt = 0; int col_cnt = 15; for (const auto& item : state->_anchored_landmarks) { const int& id = item.first; if (map_server->find(id) == map_server->end()) { std::cout << "[LandmarkUpdate]: Landmark in state not in map server!" << std::endl; std::exit(EXIT_FAILURE); } else if (map_server->at(id)->_ftype != FeatureInfo::FeatureType::SLAM) { std::cout << "[LandmarkUpdate]: Landmark in state not marked SLAM type in map server!" << std::endl; std::exit(EXIT_FAILURE); } else if (map_server->at(id)->_stereo_obs.find(state->_timestamp) == map_server->at(id)->_stereo_obs.end()) { std::cout << "[LandmarkUpdate]: Landmark in state not tracked to curr time! Should have been marged before!" << std::endl; std::exit(EXIT_FAILURE); } if (map_server->at(id)->_landmark != state->_anchored_landmarks.at(id)) { std::cout << "[LandmarkUpdate]: Landmark ptr in state not the same as that in map server!" << std::endl; std::exit(EXIT_FAILURE); } std::shared_ptr anchored_pose_ptr = item.second->getAnchoredPose(); std::shared_ptr lm_ptr = item.second; Eigen::Vector4d res; Eigen::Matrix H_fj_epose; Eigen::Matrix H_fj_ext; Eigen::Matrix H_fj_anch; Eigen::Matrix H_fj_pf; this->calcResJacobianSingleLandmarkStereo(map_server->at(id), state, res, H_fj_epose, H_fj_ext, H_fj_anch, H_fj_pf); std::vector> var_order_chi2(4); var_order_chi2[0] = state->_extended_pose; var_order_chi2[1] = state->_camleft_imu_extrinsics; var_order_chi2[2] = anchored_pose_ptr; var_order_chi2[3] = lm_ptr; Eigen::MatrixXd H_chi2(4, 24); H_chi2.block<4, 9>(0, 0) = H_fj_epose; H_chi2.block<4, 6>(0, 9) = H_fj_ext; H_chi2.block<4, 6>(0, 15) = H_fj_anch; H_chi2.block<4, 3>(0, 21) = H_fj_pf; if (!testChiSquared(state, res, H_chi2, var_order_chi2, this->_noise)) continue; res_large.block<4, 1>(row_cnt, 0) = res; H_large.block<4, 9>(row_cnt, sub_var_col_idx.at(state->_extended_pose)) = H_fj_epose; H_large.block<4, 6>(row_cnt, sub_var_col_idx.at(state->_camleft_imu_extrinsics)) = H_fj_ext; if (sub_var_col_idx.find(anchored_pose_ptr) == sub_var_col_idx.end()) { sub_var_col_idx[anchored_pose_ptr] = col_cnt; col_cnt += 6; var_order.push_back(anchored_pose_ptr); } H_large.block<4, 6>(row_cnt, sub_var_col_idx.at(anchored_pose_ptr)) = H_fj_anch; if (sub_var_col_idx.find(lm_ptr) == sub_var_col_idx.end()) { sub_var_col_idx[lm_ptr] = col_cnt; col_cnt += 3; var_order.push_back(lm_ptr); } H_large.block<4, 3>(row_cnt, sub_var_col_idx.at(lm_ptr)) = H_fj_pf; row_cnt += 4; } if (row_cnt == 0) return; if (max_possible_rows > row_cnt || max_possible_cols > col_cnt) { H_large.conservativeResize(row_cnt, col_cnt); res_large.conservativeResize(row_cnt, 1); } StateManager::ekfUpdate(state, var_order, H_large, res_large, std::pow(this->_noise, 2)*Eigen::MatrixXd::Identity(res_large.rows(), res_large.rows())); } void LandmarkUpdate::calcResJacobianSingleFeatAllStereoObs( const std::shared_ptr feature_info, const std::map>& sw_poses, const std::vector>& sw_var_order, const std::map, int>& sw_index_map, const Eigen::Isometry3d& T_cl2cr, Eigen::VectorXd& res_block, Eigen::MatrixXd& Hx_block, Eigen::MatrixXd& Hf_block) { const int num_of_cols = 6*sw_var_order.size(); const int num_of_rows = 4*feature_info->numOfStereoFrames(); Eigen::VectorXd res_block_tmp = Eigen::VectorXd::Zero(num_of_rows); Eigen::MatrixXd H_block_tmp = Eigen::MatrixXd::Zero(num_of_rows, num_of_cols); Eigen::MatrixXd Hf_block_tmp = Eigen::MatrixXd::Zero(num_of_rows, 3); const Eigen::Vector3d pf_w = feature_info->_landmark->valuePosXyz(); int row_cnt = 0; for (const auto& item : feature_info->_stereo_obs) { const double& time_of_obs = item.first; if (sw_poses.find(time_of_obs) == sw_poses.end()) continue; const std::shared_ptr& stereo_obs_ptr = item.second; const std::shared_ptr& pose_obs_ptr = sw_poses.at(time_of_obs); const std::shared_ptr pose_anchor_ptr = feature_info->_landmark->getAnchoredPose(); const Eigen::Matrix3d R_cm2w = pose_obs_ptr->valueLinearAsMat(); const Eigen::Vector3d p_cm = pose_obs_ptr->valueTrans(); const Eigen::Vector3d pf_cm = R_cm2w.transpose()*(pf_w-p_cm); const Eigen::Vector3d pf_cm_r = T_cl2cr*pf_cm; Eigen::Matrix H_proj = Eigen::Matrix::Zero(); H_proj(0, 0) = 1.0/pf_cm.z(); H_proj(0, 2) = -pf_cm.x()/std::pow(pf_cm.z(), 2); H_proj(1, 1) = 1.0/pf_cm.z(); H_proj(1, 2) = -pf_cm.y()/std::pow(pf_cm.z(), 2); Eigen::Matrix H_proj_r = Eigen::Matrix::Zero(); H_proj_r(0, 0) = 1.0/pf_cm_r.z(); H_proj_r(1, 1) = 1.0/pf_cm_r.z(); H_proj_r(0, 2) = -pf_cm_r.x()/std::pow(pf_cm_r.z(), 2); H_proj_r(1, 2) = -pf_cm_r.y()/std::pow(pf_cm_r.z(), 2); Eigen::MatrixXd H_pf2x = Eigen::MatrixXd::Zero(3, num_of_cols); if (pose_anchor_ptr != pose_obs_ptr) { H_pf2x.block<3, 3>(0, sw_index_map.at(pose_obs_ptr)) = R_cm2w.transpose()*skew(pf_w); H_pf2x.block<3, 3>(0, sw_index_map.at(pose_anchor_ptr)) = -H_pf2x.block<3, 3>(0, sw_index_map.at(pose_obs_ptr)); } H_pf2x.block<3, 3>(0, sw_index_map.at(pose_obs_ptr)+3) = -R_cm2w.transpose(); Eigen::Matrix3d H_pf2pf = R_cm2w.transpose(); if (H_proj.hasNaN() || H_pf2x.hasNaN()) continue; H_block_tmp.block(row_cnt, 0, 2, num_of_cols) = H_proj*H_pf2x; H_block_tmp.block(row_cnt+2, 0, 2, num_of_cols) = H_proj_r*T_cl2cr.linear()*H_pf2x; Hf_block_tmp.block(row_cnt, 0, 2, 3) = H_proj*H_pf2pf; Hf_block_tmp.block(row_cnt+2, 0, 2, 3) = H_proj_r*T_cl2cr.linear()*H_pf2pf; res_block_tmp.block(row_cnt, 0, 4, 1) = stereo_obs_ptr->asVec()-Eigen::Vector4d(pf_cm.x()/pf_cm.z(), pf_cm.y()/pf_cm.z(), pf_cm_r.x()/pf_cm_r.z(), pf_cm_r.y()/pf_cm_r.z()); row_cnt += 4; } if (row_cnt < res_block.rows()) { res_block_tmp.conservativeResize(row_cnt, 1); H_block_tmp.conservativeResize(row_cnt, num_of_cols); Hf_block_tmp.conservativeResize(row_cnt, 3); } res_block = res_block_tmp; Hx_block = H_block_tmp; Hf_block = Hf_block_tmp; } void LandmarkUpdate::initNewLandmarkStereo(std::shared_ptr state, std::shared_ptr map_server, std::shared_ptr tri, int min_init_poses) { if (state->_sw_camleft_poses.size() < min_init_poses) return; const int vac_num_lm = state->_state_params._max_landmarks - state->_anchored_landmarks.size(); if (vac_num_lm <= 0) return; std::vector ids_to_init; for (const auto& item : *map_server) { if (ids_to_init.size() >= vac_num_lm) break; if (item.second->_stereo_obs.size() < min_init_poses || item.second->_ftype == FeatureInfo::FeatureType::SLAM) continue; if (!FeatureInfoManager::triangulateFeatureInfoStereo(item.second, tri, state)) continue; ids_to_init.push_back(item.first); } if (ids_to_init.size() == 0) return; std::vector> sw_var_order; std::map, int> sw_index_map; std::vector> sw_var_type; this->generateSwVarOrder(state, sw_var_order, sw_index_map, sw_var_type); for (const int& id : ids_to_init) { Eigen::VectorXd res_block; Eigen::MatrixXd Hx_block; Eigen::MatrixXd Hf_block; this->calcResJacobianSingleFeatAllStereoObs(map_server->at(id), state->_sw_camleft_poses, sw_var_order, sw_index_map, state->_state_params._T_cl2cr, res_block, Hx_block, Hf_block); if (!StateManager::addVariableDelayed(state, map_server->at(id)->_landmark, sw_var_type, Hx_block, Hf_block, res_block, this->_noise, 0.95, true)) continue; if (state->_anchored_landmarks.find(id) != state->_anchored_landmarks.end()) { std::cout << "[LandmarkUpdate]: The id intended to add already in state!" << std::endl; std::exit(EXIT_FAILURE); } state->_anchored_landmarks[id] = map_server->at(id)->_landmark; map_server->at(id)->_ftype = FeatureInfo::FeatureType::SLAM; } } } ================================================ FILE: ingvio_estimator/src/LandmarkUpdate.h ================================================ /** This File is part of InGVIO, an invariant filter for mono/stereo visual- * inertial-raw GNSS navigation. * * Copyright (C) 2022 Changwu Liu (cwliu529@163.com, * lcw18@mails.tsinghua.edu.cn (valid until 2023)) * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program. If not, see . */ #pragma once #include #include "IngvioParams.h" #include "MapServer.h" #include "Update.h" namespace ingvio { class State; class Triangulator; class LandmarkUpdate : public UpdateBase { public: using UpdateBase::UpdateBase; LandmarkUpdate(const IngvioParams& filter_params) : UpdateBase(filter_params._chi2_max_dof, filter_params._chi2_thres), _noise(filter_params._visual_noise) {} LandmarkUpdate(const LandmarkUpdate&) = delete; LandmarkUpdate operator=(const LandmarkUpdate&) = delete; virtual ~LandmarkUpdate() {} void updateLandmarkMono(std::shared_ptr state, std::shared_ptr map_server); void updateLandmarkMonoSw(std::shared_ptr state, std::shared_ptr map_server); void initNewLandmarkMono(std::shared_ptr state, std::shared_ptr map_server, std::shared_ptr tri, int min_init_poses); void changeLandmarkAnchor(std::shared_ptr state, std::shared_ptr map_server); void changeLandmarkAnchor(std::shared_ptr state, std::shared_ptr map_server, const std::vector& marg_kfs); void updateLandmarkStereo(std::shared_ptr state, std::shared_ptr map_server); void initNewLandmarkStereo(std::shared_ptr state, std::shared_ptr map_server, std::shared_ptr tri, int min_init_poses); protected: double _noise; void calcResJacobianSingleLandmarkMono(const std::shared_ptr feature_info, const std::shared_ptr state, Eigen::Vector2d& res, Eigen::Matrix& H_fj_epose, Eigen::Matrix& H_fj_ext, Eigen::Matrix& H_fj_anch, Eigen::Matrix& H_fj_pf); void calcResJacobianSingleFeatAllMonoObs( const std::shared_ptr feature_info, const std::map>& sw_poses, const std::vector>& sw_var_order, const std::map, int>& sw_index_map, Eigen::VectorXd& res_block, Eigen::MatrixXd& Hx_block, Eigen::MatrixXd& Hf_block); void calcResJacobianSingleLandmarkMono(const std::shared_ptr feature_info, const std::shared_ptr state, Eigen::Vector2d& res, Eigen::Matrix& H_fj_pose, Eigen::Matrix& H_fj_anch, Eigen::Matrix& H_fj_pf); void generateSwVarOrder(const std::shared_ptr state, std::vector>& sw_var_order, std::map, int>& sw_index, std::vector>& sw_var_type); void calcResJacobianSingleLandmarkStereo(const std::shared_ptr feature_info, const std::shared_ptr state, Eigen::Vector4d& res, Eigen::Matrix& H_fj_epose, Eigen::Matrix& H_fj_ext, Eigen::Matrix& H_fj_anch, Eigen::Matrix& H_fj_pf); void calcResJacobianSingleFeatAllStereoObs( const std::shared_ptr feature_info, const std::map>& sw_poses, const std::vector>& sw_var_order, const std::map, int>& sw_index_map, const Eigen::Isometry3d& T_cl2cr, Eigen::VectorXd& res_block, Eigen::MatrixXd& Hx_block, Eigen::MatrixXd& Hf_block); }; } ================================================ FILE: ingvio_estimator/src/MapServer.cpp ================================================ /** This File is part of InGVIO, an invariant filter for mono/stereo visual- * inertial-raw GNSS navigation. * * Copyright (C) 2022 Changwu Liu (cwliu529@163.com, * lcw18@mails.tsinghua.edu.cn (valid until 2023)) * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program. If not, see . */ #include "MapServer.h" namespace ingvio { void MonoMeas::setFromMonoMsg(const feature_tracker::MonoMeas::ConstPtr& mono_msg) { _id = mono_msg->id; _u0 = mono_msg->u0; _v0 = mono_msg->v0; } void MonoMeas::setFromMonoMsg(const feature_tracker::MonoMeas& mono_msg) { _id = mono_msg.id; _u0 = mono_msg.u0; _v0 = mono_msg.v0; } void StereoMeas::setFromStereoMsg(const feature_tracker::StereoMeas::ConstPtr& stereo_msg) { _id = stereo_msg->id; _u0 = stereo_msg->u0; _v0 = stereo_msg->v0; _u1 = stereo_msg->u1; _v1 = stereo_msg->v1; } void StereoMeas::setFromStereoMsg(const feature_tracker::StereoMeas& stereo_msg) { _id = stereo_msg.id; _u0 = stereo_msg.u0; _v0 = stereo_msg.v0; _u1 = stereo_msg.u1; _v1 = stereo_msg.v1; } bool FeatureInfo::hasMonoObsAt(double timestamp) const { return (_mono_obs.find(timestamp) != _mono_obs.end()); } Eigen::Vector2d FeatureInfo::monoMeasAt(double timestamp) const { if (hasMonoObsAt(timestamp)) return _mono_obs.at(timestamp)->asVec(); else return Eigen::Vector2d::Zero(); } bool FeatureInfo::hasStereoObsAt(double timestamp) const { return (_stereo_obs.find(timestamp) != _stereo_obs.end()); } Eigen::Vector4d FeatureInfo::stereoMeasAt(double timestamp) const { if (hasStereoObsAt(timestamp)) return _stereo_obs.at(timestamp)->asVec(); else return Eigen::Vector4d::Zero(); } } ================================================ FILE: ingvio_estimator/src/MapServer.h ================================================ /** This File is part of InGVIO, an invariant filter for mono/stereo visual- * inertial-raw GNSS navigation. * * Copyright (C) 2022 Changwu Liu (cwliu529@163.com, * lcw18@mails.tsinghua.edu.cn (valid until 2023)) * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program. If not, see . */ #pragma once #include #include #include #include #include "AnchoredLandmark.h" namespace ingvio { struct MonoMeas { int _id; double _u0; double _v0; Eigen::Vector2d asVec() const { return Eigen::Vector2d(_u0, _v0); } void setFromMonoMsg(const feature_tracker::MonoMeas::ConstPtr& mono_msg); void setFromMonoMsg(const feature_tracker::MonoMeas& mono_msg); }; struct StereoMeas { int _id; double _u0; double _v0; double _u1; double _v1; Eigen::Vector2d asLeftVec() const { return Eigen::Vector2d(_u0, _v0); } Eigen::Vector2d asRightVec() const { return Eigen::Vector2d(_u1, _v1); } Eigen::Vector4d asVec() const { return Eigen::Vector4d(_u0, _v0, _u1, _v1); } void setFromStereoMsg(const feature_tracker::StereoMeas::ConstPtr& stereo_msg); void setFromStereoMsg(const feature_tracker::StereoMeas& stereo_msg); }; class FeatureInfo { public: enum FeatureType {MSCKF = 0, SLAM}; FeatureInfo() : _id(-1), _ftype(MSCKF), _isToMarg(false), _isTri(false), _numOfTri(0) { _mono_obs.clear(); _stereo_obs.clear(); _landmark = std::make_shared(); } ~FeatureInfo() = default; const int& getId() const { return _id; } const FeatureType& getFeatureType() const { return _ftype; } const bool& isToMarg() const { return _isToMarg; } const bool& isTri() const { return _isTri; } bool hasMonoObsAt(double timestamp) const; Eigen::Vector2d monoMeasAt(double timestamp) const; bool hasStereoObsAt(double timestamp) const; Eigen::Vector4d stereoMeasAt(double timestamp) const; int numOfMonoFrames() const { return _mono_obs.size(); } int numOfStereoFrames() const { return _stereo_obs.size(); } const std::shared_ptr anchor() const { return _landmark->getAnchoredPose(); } const std::shared_ptr landmark() const { return _landmark; } /* protected: friend class FeatureInfoManager; friend class MapServerManager; */ int _id; FeatureType _ftype; bool _isToMarg; bool _isTri; int _numOfTri; std::shared_ptr _landmark; std::map> _mono_obs; std::map> _stereo_obs; }; typedef std::map> MapServer; } ================================================ FILE: ingvio_estimator/src/MapServerManager.cpp ================================================ /** This File is part of InGVIO, an invariant filter for mono/stereo visual- * inertial-raw GNSS navigation. * * Copyright (C) 2022 Changwu Liu (cwliu529@163.com, * lcw18@mails.tsinghua.edu.cn (valid until 2023)) * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program. If not, see . */ #include #include #include #include #include #include "State.h" #include "StateManager.h" #include "MapServer.h" #include "MapServerManager.h" #include "Triangulator.h" namespace ingvio { std::shared_ptr MonoMeasManager::convertFromMsg( const feature_tracker::MonoMeas::ConstPtr& mono_msg) { std::shared_ptr mono_meas = std::make_shared(); mono_meas->setFromMonoMsg(mono_msg); return mono_meas; } std::shared_ptr MonoMeasManager::convertFromMsg( const feature_tracker::MonoMeas& mono_msg) { std::shared_ptr mono_meas = std::make_shared(); mono_meas->setFromMonoMsg(mono_msg); return mono_meas; } std::shared_ptr MonoMeasManager::random(int id) { std::shared_ptr mono_meas = std::make_shared(); Eigen::Vector2d vec = Eigen::Vector2d::Random(); mono_meas->_id = id; mono_meas->_u0 = vec.x(); mono_meas->_v0 = vec.y(); return mono_meas; } std::shared_ptr StereoMeasManager::convertFromMsg( const feature_tracker::StereoMeas::ConstPtr& stereo_msg) { std::shared_ptr stereo_meas = std::make_shared(); stereo_meas->setFromStereoMsg(stereo_msg); return stereo_meas; } std::shared_ptr StereoMeasManager::convertFromMsg( const feature_tracker::StereoMeas& stereo_msg) { std::shared_ptr stereo_meas = std::make_shared(); stereo_meas->setFromStereoMsg(stereo_msg); return stereo_meas; } std::shared_ptr StereoMeasManager::random(int id) { std::shared_ptr stereo_meas = std::make_shared(); Eigen::Vector4d vec = Eigen::Vector4d::Random(); stereo_meas->_id = id; stereo_meas->_u0 = vec.x(); stereo_meas->_v0 = vec.y(); stereo_meas->_u1 = vec.z(); stereo_meas->_v1 = vec.w(); return stereo_meas; } void FeatureInfoManager::collectMonoMeas(std::shared_ptr feature_info, std::shared_ptr state, std::shared_ptr mono_meas) { const double& timestamp = state->_timestamp; if (state->_sw_camleft_poses.find(timestamp) == state->_sw_camleft_poses.end()) { std::cout << "[FeatureInfoManager]: Meas timestamp not in sw!" << std::endl; assert(false); } if (feature_info->_mono_obs.size() == 0) { feature_info->_mono_obs[timestamp] = mono_meas; feature_info->_id = mono_meas->_id; feature_info->_ftype = FeatureInfo::FeatureType::MSCKF; feature_info->_isToMarg = false; feature_info->_isTri = false; feature_info->_landmark->resetAnchoredPose(state->_sw_camleft_poses.at(timestamp)); } else { if (feature_info->hasMonoObsAt(timestamp)) { std::cout << "[FeatureInfoManager]: Meas timestamp already in mono obs, skip adding! " << std::endl; return; } if (feature_info->_ftype == FeatureInfo::FeatureType::SLAM) feature_info->_mono_obs.clear(); feature_info->_mono_obs[timestamp] = mono_meas; assert(feature_info->_id == mono_meas->_id); feature_info->_isToMarg = false; } } void FeatureInfoManager::collectStereoMeas(std::shared_ptr feature_info, std::shared_ptr state, std::shared_ptr stereo_meas) { const double& timestamp = state->_timestamp; if (state->_sw_camleft_poses.find(timestamp) == state->_sw_camleft_poses.end()) { std::cout << "[FeatureInfoManager]: Meas timestamp not in sw!" << std::endl; assert(false); } if (feature_info->_stereo_obs.size() == 0) { feature_info->_stereo_obs[timestamp] = stereo_meas; feature_info->_id = stereo_meas->_id; feature_info->_ftype = FeatureInfo::FeatureType::MSCKF; feature_info->_isToMarg = false; feature_info->_isTri = false; feature_info->_landmark->resetAnchoredPose(state->_sw_camleft_poses.at(timestamp)); } else { if (feature_info->hasStereoObsAt(timestamp)) { std::cout << "[FeatureInfoManager]: Meas timestamp already in stereo obs, skip adding! " << std::endl; return; } if (feature_info->_ftype == FeatureInfo::FeatureType::SLAM) feature_info->_stereo_obs.clear(); feature_info->_stereo_obs[timestamp] = stereo_meas; assert(feature_info->_id == stereo_meas->_id); feature_info->_isToMarg = false; } } void MapServerManager::collectMonoMeas(std::shared_ptr map_server, std::shared_ptr state, const feature_tracker::MonoFrame::ConstPtr& mono_frame_msg) { assert(mono_frame_msg->header.stamp.toSec() == state->_timestamp); for (const auto& item : mono_frame_msg->mono_features) { if (map_server->find(item.id) == map_server->end()) map_server->insert(std::make_pair(item.id, std::make_shared())); FeatureInfoManager::collectMonoMeas(map_server->at(item.id), state, MonoMeasManager::convertFromMsg(item)); } } void MapServerManager::collectStereoMeas(std::shared_ptr map_server, std::shared_ptr state, const feature_tracker::StereoFrame::ConstPtr& stereo_frame_msg) { assert(stereo_frame_msg->header.stamp.toSec() == state->_timestamp); for (const auto& item : stereo_frame_msg->stereo_features) { if (map_server->find(item.id) == map_server->end()) map_server->insert(std::make_pair(item.id, std::make_shared())); FeatureInfoManager::collectStereoMeas(map_server->at(item.id), state, StereoMeasManager::convertFromMsg(item)); } } void MapServerManager::markMargMonoFeatures(std::shared_ptr map_server, std::shared_ptr state) { const double& curr_timestamp = state->_timestamp; std::vector marg_ids; for (auto& item : *map_server) if (!item.second->hasMonoObsAt(curr_timestamp)) { item.second->_isToMarg = true; if (item.second->_ftype == FeatureInfo::FeatureType::SLAM) { assert(item.second->_id == item.first); marg_ids.push_back(item.second->_id); } } for (int i = 0; i < marg_ids.size(); ++i) { StateManager::margAnchoredLandmarkInState(state, marg_ids[i]); map_server->erase(marg_ids[i]); } } void MapServerManager::markMargStereoFeatures(std::shared_ptr map_server, std::shared_ptr state) { const double& curr_timestamp = state->_timestamp; std::vector marg_ids; for (auto& item : *map_server) { if (!item.second->hasStereoObsAt(curr_timestamp)) { item.second->_isToMarg = true; if (item.second->_ftype == FeatureInfo::FeatureType::SLAM) { assert(item.second->_id == item.first); marg_ids.push_back(item.second->_id); } } } for (int i = 0; i < marg_ids.size(); ++i) { StateManager::margAnchoredLandmarkInState(state, marg_ids[i]); map_server->erase(marg_ids[i]); } } bool FeatureInfoManager::triangulateFeatureInfoMono( std::shared_ptr feature_info, const std::shared_ptr tri, const std::shared_ptr state) { Eigen::Vector3d pf; bool flag = tri->triangulateMonoObs(feature_info->_mono_obs, state->_sw_camleft_poses, pf); if (flag && !pf.hasNaN()) { ++feature_info->_numOfTri; Eigen::Vector3d body_anchor = feature_info->_landmark->getAnchoredPose()->copyValueAsIso().inverse()*pf; if (body_anchor.z() <= 0) return false; if (!feature_info->_isTri) { feature_info->_landmark->setFejPosXyz(pf); feature_info->_landmark->setValuePosXyz(pf); feature_info->_isTri = true; return true; } feature_info->_landmark->setValuePosXyz(pf); return true; } else return false; } bool FeatureInfoManager::triangulateFeatureInfoStereo( std::shared_ptr feature_info, const std::shared_ptr tri, const std::shared_ptr state) { Eigen::Vector3d pf; bool flag = tri->triangulateStereoObs(feature_info->_stereo_obs, state->_sw_camleft_poses, state->_state_params._T_cl2cr, pf); if (flag && !pf.hasNaN()) { ++feature_info->_numOfTri; Eigen::Vector3d body_anchor = feature_info->_landmark->getAnchoredPose()->copyValueAsIso().inverse()*pf; if (body_anchor.z() <= 0) return false; if (!feature_info->_isTri) { feature_info->_landmark->setFejPosXyz(pf); feature_info->_landmark->setValuePosXyz(pf); feature_info->_isTri = true; return true; } feature_info->_landmark->setValuePosXyz(pf); return true; } else return false; } void FeatureInfoManager::changeAnchoredPose(std::shared_ptr feature_info, std::shared_ptr state, double target_sw_timestamp) { if (state->_sw_camleft_poses.size() < 2) return; if (state->_sw_camleft_poses.find(target_sw_timestamp) == state->_sw_camleft_poses.end() || state->_anchored_landmarks.find(feature_info->getId()) == state->_anchored_landmarks.end()) return; if (feature_info->_ftype != FeatureInfo::FeatureType::SLAM) return; bool notFoundcurr = true; for (const auto& item : state->_sw_camleft_poses) if (item.second == feature_info->anchor()) { notFoundcurr = false; break; } if (notFoundcurr || state->_anchored_landmarks.at(feature_info->getId()) != feature_info->landmark()) return; std::vector> var_order; var_order.push_back(feature_info->anchor()); var_order.push_back(state->_sw_camleft_poses.at(target_sw_timestamp)); var_order.push_back(state->_anchored_landmarks.at(feature_info->getId())); Eigen::MatrixXd H = Eigen::MatrixXd::Zero(3, 15); const Eigen::Vector3d& pf = feature_info->landmark()->valuePosXyz(); H.block<3, 3>(0, 0) = -skew(pf); H.block<3, 3>(0, 6) = skew(pf); H.block<3, 3>(0, 12) = Eigen::Matrix3d::Identity(); StateManager::replaceVarLinear(state, feature_info->_landmark, var_order, H); feature_info->_landmark->resetAnchoredPose(state->_sw_camleft_poses.at(target_sw_timestamp), true); } void FeatureInfoManager::changeAnchoredPose(std::shared_ptr feature_info, std::shared_ptr state) { if (state->_sw_camleft_poses.size() < 2) return; FeatureInfoManager::changeAnchoredPose(feature_info, state, state->_sw_camleft_poses.rbegin()->first); } void MapServerManager::mapStatistics(const std::shared_ptr map_server) { int num_of_msckf = 0; int num_of_slam = 0; int num_of_err = 0; for (const auto& item : *map_server) { if (item.second->_ftype == FeatureInfo::FeatureType::SLAM) ++num_of_slam; else if (item.second->_ftype == FeatureInfo::FeatureType::MSCKF) ++num_of_msckf; if (item.second->_mono_obs.size() == 0 && item.second->_stereo_obs.size() == 0) ++num_of_err; } std::cout << "[MapServerManager]: Num of msckf feats = " << num_of_msckf << " Num of slam feats = " << num_of_slam << std::endl; if (num_of_err > 0) std::cout << "[MapServerManager]: Warning!! " << num_of_err << " feats in map server is null without obs!" << std::endl; } void MapServerManager::checkMapStateConsistent(const std::shared_ptr map_server, const std::shared_ptr state) { std::vector ids_slam; for (const auto& item : *map_server) if (item.second->_ftype == FeatureInfo::FeatureType::SLAM) { const int& id = item.first; ids_slam.push_back(id); if (state->_anchored_landmarks.find(id) == state->_anchored_landmarks.end()) std::cout << "[MapServerManager]: Map server slam feat id = " << id << " not in the state! " << std::endl; else { if (state->_anchored_landmarks.at(id) != item.second->_landmark) std::cout << "[MapServerManager]: Map server slam feat id = " << id << " lm ptr not the same with that in state! " << std::endl; if (state->_anchored_landmarks.at(id)->getAnchoredPose() != item.second->_landmark->getAnchoredPose()) std::cout << "[MapServerManager]: Map server slam feat id = " << id << " anchor pose not the same with that of in state! " << std::endl; } } if (ids_slam.size() != state->_anchored_landmarks.size()) std::cout << "[MapServerManager]: Some state lm are no longer in map server!" << std::endl; } void MapServerManager::checkAnchorStatus(const std::shared_ptr map_server, const std::shared_ptr state) { std::unordered_map, double> sw_poses; for (const auto& item : state->_sw_camleft_poses) sw_poses.insert(std::make_pair(item.second, item.first)); for (const auto& item : *map_server) if (sw_poses.find(item.second->_landmark->getAnchoredPose()) == sw_poses.end()) { std::cout << "[MapServerManager]: Anchor of Feat id = " << item.first << " not in the sw! " << std::endl; } } void MapServerManager::eraseInvalidFeatures(std::shared_ptr map_server, std::shared_ptr state) { std::vector ids_to_remove; for (const auto& item : *map_server) { const int& id = item.first; const auto& feat_info_ptr = item.second; if (!feat_info_ptr->_isTri) continue; if (feat_info_ptr->_landmark->getAnchoredPose() == nullptr) { ids_to_remove.push_back(id); continue; } const auto anchor_ptr = feat_info_ptr->_landmark->getAnchoredPose(); Eigen::Vector3d body = anchor_ptr->copyValueAsIso().inverse()*feat_info_ptr->_landmark->valuePosXyz(); if (body.z() <= 0.2) ids_to_remove.push_back(id); } if (ids_to_remove.size() > 0) std::cout << "[MapServerManager]: Num of feats have negative depth in anchor pose = " << ids_to_remove.size() << std::endl; for (const int& id : ids_to_remove) { if (map_server->at(id)->_ftype == FeatureInfo::FeatureType::SLAM) StateManager::margAnchoredLandmarkInState(state, id); map_server->erase(id); } } } ================================================ FILE: ingvio_estimator/src/MapServerManager.h ================================================ /** This File is part of InGVIO, an invariant filter for mono/stereo visual- * inertial-raw GNSS navigation. * * Copyright (C) 2022 Changwu Liu (cwliu529@163.com, * lcw18@mails.tsinghua.edu.cn (valid until 2023)) * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program. If not, see . */ #pragma once #include #include #include #include #include namespace ingvio { struct MonoMeas; struct StereoMeas; class FeatureInfo; class State; class Triangulator; class MonoMeasManager { private: MonoMeasManager() {} MonoMeasManager(const MonoMeasManager&) {} MonoMeasManager operator=(const MonoMeasManager&) = delete; public: static std::shared_ptr convertFromMsg( const feature_tracker::MonoMeas::ConstPtr& mono_msg); static std::shared_ptr convertFromMsg( const feature_tracker::MonoMeas& mono_msg); static std::shared_ptr random(int id); }; class StereoMeasManager { private: StereoMeasManager() {} StereoMeasManager(const StereoMeasManager&) {} StereoMeasManager operator=(const StereoMeasManager&) = delete; public: static std::shared_ptr convertFromMsg(const feature_tracker::StereoMeas::ConstPtr& stereo_msg); static std::shared_ptr convertFromMsg( const feature_tracker::StereoMeas& stereo_msg); static std::shared_ptr random(int id); }; class FeatureInfoManager { private: FeatureInfoManager () {} FeatureInfoManager(const FeatureInfoManager&) {} FeatureInfoManager operator=(const FeatureInfoManager&) = delete; public: static void collectMonoMeas(std::shared_ptr feature_info, std::shared_ptr state, std::shared_ptr mono_meas); static void collectStereoMeas(std::shared_ptr feature_info, std::shared_ptr state, std::shared_ptr stereo_meas); static bool triangulateFeatureInfoMono(std::shared_ptr feature_info, const std::shared_ptr tri, const std::shared_ptr state); static bool triangulateFeatureInfoStereo(std::shared_ptr feature_info, const std::shared_ptr tri, const std::shared_ptr state); static void changeAnchoredPose(std::shared_ptr feature_info, std::shared_ptr state, double target_sw_timestamp); static void changeAnchoredPose(std::shared_ptr feature_info, std::shared_ptr state); }; class MapServerManager { private: MapServerManager () {} MapServerManager(const MapServerManager&) {} MapServerManager operator=(const MapServerManager&) = delete; public: static void collectMonoMeas(std::shared_ptr map_server, std::shared_ptr state, const feature_tracker::MonoFrame::ConstPtr& mono_frame_msg); static void collectStereoMeas(std::shared_ptr map_server, std::shared_ptr state, const feature_tracker::StereoFrame::ConstPtr& stereo_frame_msg); static void markMargMonoFeatures(std::shared_ptr map_server, std::shared_ptr state); static void markMargStereoFeatures(std::shared_ptr map_server, std::shared_ptr state); static void mapStatistics(const std::shared_ptr map_server); static void checkMapStateConsistent(const std::shared_ptr map_server, const std::shared_ptr state); static void checkAnchorStatus(const std::shared_ptr map_server, const std::shared_ptr state); static void eraseInvalidFeatures(std::shared_ptr map_server, std::shared_ptr state); }; } ================================================ FILE: ingvio_estimator/src/PoseState.cpp ================================================ /** This File is part of InGVIO, an invariant filter for mono/stereo visual- * inertial-raw GNSS navigation. * * Copyright (C) 2022 Changwu Liu (cwliu529@163.com, * lcw18@mails.tsinghua.edu.cn (valid until 2023)) * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program. If not, see . */ #include "PoseState.h" namespace ingvio { void SO3::update(const Eigen::VectorXd& dx) { assert(dx.rows() >= this->size() + this->idx()); _rot = GammaFunc(dx.block<3, 1>(this->idx(), 0), 0)*_rot; _q = Eigen::Quaterniond(_rot); } void SO3::setIdentity() { _q.setIdentity(); _rot.setIdentity(); } void SO3::setRandom() { _q = Eigen::Quaterniond(Eigen::Vector4d::Random()); _q.normalize(); _rot = _q.toRotationMatrix(); } void SO3::setValueByQuat(const Eigen::Quaterniond& other_quat) { _q = other_quat.normalized(); _rot = _q.toRotationMatrix(); } void SO3::setValueByMat(const Eigen::Matrix3d& other_mat) { _rot = other_mat; _q = Eigen::Quaterniond(_rot); } void SO3::setFejByQuat(const Eigen::Quaterniond& other_quat) { _q_fej = other_quat.normalized(); _rot_fej = _q_fej.toRotationMatrix(); } void SO3::setFejByMat(const Eigen::Matrix3d& other_mat) { _rot_fej = other_mat; _q_fej = Eigen::Quaterniond(_rot_fej); } std::shared_ptr SO3::clone() { auto tmp = std::shared_ptr(new SO3()); tmp->setFejByQuat(this->fejAsQuat()); tmp->setValueByQuat(this->valueAsQuat()); return tmp; } void SE3::update(const Eigen::VectorXd& dx) { assert(dx.rows() >= this->idx() + this->size()); Eigen::Matrix3d Gamma0 = GammaFunc(dx.block<3, 1>(this->idx(), 0), 0); _rot = Gamma0*_rot; _q = Eigen::Quaterniond(_rot); _vec = Gamma0*_vec + GammaFunc(dx.block<3, 1>(this->idx(), 0), 1)*dx.block<3, 1>(this->idx()+3, 0); } void SE3::setIdentity() { _q.setIdentity(); _rot.setIdentity(); _vec.setZero(); } void SE3::setRandom() { _q = Eigen::Quaterniond(Eigen::Vector4d::Random()); _q.normalize(); _rot = _q.toRotationMatrix(); _vec.setRandom(); } Eigen::Isometry3d SE3::copyValueAsIso() const { Eigen::Isometry3d result; result.setIdentity(); result.linear() = _rot; result.translation() = _vec; return result; } Eigen::Isometry3d SE3::copyFejAsIso() const { Eigen::Isometry3d result; result.setIdentity(); result.linear() = _rot_fej; result.translation() = _vec_fej; return result; } void SE3::setValueLinearByQuat(const Eigen::Quaterniond& other_quat) { _q = other_quat.normalized(); _rot = _q.toRotationMatrix(); } void SE3::setValueLinearByMat(const Eigen::Matrix3d& other_mat) { _rot = other_mat; _q = Eigen::Quaterniond(_rot); } void SE3::setValueByIso(const Eigen::Isometry3d& other_iso) { this->setValueLinearByMat(other_iso.linear()); this->setValueTrans(other_iso.translation()); } void SE3::setFejLinearByQuat(const Eigen::Quaterniond& other_quat) { _q_fej = other_quat.normalized(); _rot_fej = _q_fej.toRotationMatrix(); } void SE3::setFejLinearByMat(const Eigen::Matrix3d& other_mat) { _rot_fej = other_mat; _q_fej = Eigen::Quaterniond(_rot_fej); } void SE3::setFejByIso(const Eigen::Isometry3d& other_iso) { this->setFejLinearByMat(other_iso.linear()); this->setFejTrans(other_iso.translation()); } std::shared_ptr SE3::clone() { auto tmp = std::shared_ptr(new SE3()); tmp->setValueLinearByQuat(this->valueLinearAsQuat()); tmp->setValueTrans(this->valueTrans()); tmp->setFejLinearByQuat(this->fejLinearAsQuat()); tmp->setFejTrans(this->fejTrans()); return tmp; } void SE23::update(const Eigen::VectorXd& dx) { assert(dx.rows() >= this->idx() + this->size()); Eigen::Matrix3d Gamma0 = GammaFunc(dx.block<3, 1>(this->idx(), 0), 0); Eigen::Matrix3d Gamma1 = GammaFunc(dx.block<3, 1>(this->idx(), 0), 1); _rot = Gamma0*_rot; _q = Eigen::Quaterniond(_rot); _vec1 = Gamma0*_vec1 + Gamma1*dx.block<3, 1>(this->idx()+3, 0); _vec2 = Gamma0*_vec2 + Gamma1*dx.block<3, 1>(this->idx()+6, 0); } void SE23::setIdentity() { _q.setIdentity(); _rot.setIdentity(); _vec1.setZero(); _vec2.setZero(); } void SE23::setRandom() { _q = Eigen::Quaterniond(Eigen::Vector4d::Random()); _q.normalize(); _rot = _q.toRotationMatrix(); _vec1.setRandom(); _vec2.setRandom(); } Eigen::Isometry3d SE23::copyValueAsIso() const { Eigen::Isometry3d result; result.setIdentity(); result.linear() = _rot; result.translation() = _vec1; return result; } Eigen::Isometry3d SE23::copyFejAsIso() const { Eigen::Isometry3d result; result.setIdentity(); result.linear() = _rot_fej; result.translation() = _vec1_fej; return result; } void SE23::setValueLinearByQuat(const Eigen::Quaterniond& other_quat) { _q = other_quat.normalized(); _rot = _q.toRotationMatrix(); } void SE23::setValueLinearByMat(const Eigen::Matrix3d& other_mat) { _rot = other_mat; _q = Eigen::Quaterniond(_rot); } void SE23::setFejLinearByQuat(const Eigen::Quaterniond& other_quat) { _q_fej = other_quat.normalized(); _rot_fej = _q_fej.toRotationMatrix(); } void SE23::setFejLinearByMat(const Eigen::Matrix3d& other_mat) { _rot_fej = other_mat; _q_fej = Eigen::Quaterniond(_rot_fej); } std::shared_ptr SE23::clone() { auto tmp = std::shared_ptr(new SE23()); tmp->setValueLinearByQuat(this->valueLinearAsQuat()); tmp->setValueTrans1(this->valueTrans1()); tmp->setValueTrans2(this->valueTrans2()); tmp->setFejLinearByQuat(this->fejLinearAsQuat()); tmp->setFejTrans1(this->fejTrans1()); tmp->setFejTrans2(this->fejTrans2()); return tmp; } } ================================================ FILE: ingvio_estimator/src/PoseState.h ================================================ /** This File is part of InGVIO, an invariant filter for mono/stereo visual- * inertial-raw GNSS navigation. * * Copyright (C) 2022 Changwu Liu (cwliu529@163.com, * lcw18@mails.tsinghua.edu.cn (valid until 2023)) * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program. If not, see . */ #pragma once #include #include "AuxGammaFunc.h" #include "VecState.h" namespace ingvio { class SO3 : public Type { public: SO3() : Type(3) { _q.setIdentity(); _q_fej.setIdentity(); _rot.setIdentity(); _rot_fej.setIdentity(); } ~SO3() {} void update(const Eigen::VectorXd& dx) override; void setIdentity() override; void setRandom() override; const Eigen::Quaterniond& valueAsQuat() const { return _q; } const Eigen::Matrix3d& valueAsMat() const { return _rot; } const Eigen::Quaterniond& fejAsQuat() const { return _q_fej; } const Eigen::Matrix3d& fejAsMat() const { return _rot_fej; } void setValueByQuat(const Eigen::Quaterniond& other_quat); void setValueByMat(const Eigen::Matrix3d& other_mat); void setFejByQuat(const Eigen::Quaterniond& other_quat); void setFejByMat(const Eigen::Matrix3d& other_mat); std::shared_ptr clone(); protected: Eigen::Quaterniond _q; Eigen::Matrix3d _rot; Eigen::Quaterniond _q_fej; Eigen::Matrix3d _rot_fej; }; class SE3 : public Type { public: SE3() : Type(6) { _q.setIdentity(); _q_fej = _q; _rot.setIdentity(); _rot_fej = _rot; _vec.setZero(); _vec_fej = _vec; } ~SE3() {} void update(const Eigen::VectorXd& dx) override; void setIdentity() override; void setRandom() override; const Eigen::Quaterniond& valueLinearAsQuat() const { return _q; } const Eigen::Matrix3d& valueLinearAsMat() const { return _rot; } const Eigen::Vector3d& valueTrans() const { return _vec; } Eigen::Isometry3d copyValueAsIso() const; const Eigen::Quaterniond& fejLinearAsQuat() const { return _q_fej; } const Eigen::Matrix3d& fejLinearAsMat() const { return _rot_fej; } const Eigen::Vector3d& fejTrans() const { return _vec_fej; } Eigen::Isometry3d copyFejAsIso() const; void setValueLinearByQuat(const Eigen::Quaterniond& other_quat); void setValueLinearByMat(const Eigen::Matrix3d& other_mat); void setValueTrans(const Eigen::Vector3d& other_vec) { _vec = other_vec; } void setValueByIso(const Eigen::Isometry3d& other_iso); void setFejLinearByQuat(const Eigen::Quaterniond& other_quat); void setFejLinearByMat(const Eigen::Matrix3d& other_mat); void setFejTrans(const Eigen::Vector3d& other_vec) { _vec_fej = other_vec; } void setFejByIso(const Eigen::Isometry3d& other_iso); std::shared_ptr clone(); protected: Eigen::Quaterniond _q; Eigen::Matrix3d _rot; Eigen::Quaterniond _q_fej; Eigen::Matrix3d _rot_fej; Eigen::Vector3d _vec; Eigen::Vector3d _vec_fej; }; class SE23 : public Type { public: SE23() : Type(9) { _q.setIdentity(); _q_fej = _q; _rot.setIdentity(); _rot_fej = _rot; _vec1.setZero(); _vec1_fej = _vec1; _vec2.setZero(); _vec2_fej = _vec2; } ~SE23() {} void update(const Eigen::VectorXd& dx) override; void setIdentity() override; void setRandom() override; const Eigen::Quaterniond& valueLinearAsQuat() const { return _q; } const Eigen::Matrix3d& valueLinearAsMat() const { return _rot; } const Eigen::Vector3d& valueTrans1() const { return _vec1; } const Eigen::Vector3d& valueTrans2() const { return _vec2; } Eigen::Isometry3d copyValueAsIso() const; const Eigen::Quaterniond& fejLinearAsQuat() const { return _q_fej; } const Eigen::Matrix3d& fejLinearAsMat() const { return _rot_fej; } const Eigen::Vector3d& fejTrans1() const { return _vec1_fej; } const Eigen::Vector3d& fejTrans2() const { return _vec2_fej; } Eigen::Isometry3d copyFejAsIso() const; void setValueLinearByQuat(const Eigen::Quaterniond& other_quat); void setValueLinearByMat(const Eigen::Matrix3d& other_mat); void setValueTrans1(const Eigen::Vector3d& other_vec) { _vec1 = other_vec; } void setValueTrans2(const Eigen::Vector3d& other_vec) { _vec2 = other_vec; } void setFejLinearByQuat(const Eigen::Quaterniond& other_quat); void setFejLinearByMat(const Eigen::Matrix3d& other_mat); void setFejTrans1(const Eigen::Vector3d& other_vec) { _vec1_fej = other_vec; } void setFejTrans2(const Eigen::Vector3d& other_vec) { _vec2_fej = other_vec; } std::shared_ptr clone(); protected: Eigen::Quaterniond _q; Eigen::Quaterniond _q_fej; Eigen::Matrix3d _rot; Eigen::Matrix3d _rot_fej; Eigen::Vector3d _vec1; Eigen::Vector3d _vec1_fej; Eigen::Vector3d _vec2; Eigen::Vector3d _vec2_fej; }; } ================================================ FILE: ingvio_estimator/src/RemoveLostUpdate.cpp ================================================ /** This File is part of InGVIO, an invariant filter for mono/stereo visual- * inertial-raw GNSS navigation. * * Copyright (C) 2022 Changwu Liu (cwliu529@163.com, * lcw18@mails.tsinghua.edu.cn (valid until 2023)) * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program. If not, see . */ #include #include #include #include #include "IngvioParams.h" #include "StateManager.h" #include "MapServer.h" #include "MapServerManager.h" #include "Triangulator.h" #include "RemoveLostUpdate.h" namespace ingvio { void RemoveLostUpdate::updateStateMono(std::shared_ptr state, std::shared_ptr map_server, std::shared_ptr tri) { MapServerManager::markMargMonoFeatures(map_server, state); std::vector update_ids, direct_marg_ids; for (auto& item : *map_server) if (item.second->_ftype == FeatureInfo::FeatureType::MSCKF && item.second->_isToMarg) { if (FeatureInfoManager::triangulateFeatureInfoMono(item.second, tri, state) && item.second->_mono_obs.size() >= 4) update_ids.push_back(item.first); else direct_marg_ids.push_back(item.first); } for (const auto& item : direct_marg_ids) map_server->erase(item); if (update_ids.size() == 0) return; std::map, int> sw_index_map; std::vector> sw_var_order; const int max_possible_cols = 6*state->_sw_camleft_poses.size(); int row_cnt = 0; for (int i = 0; i < update_ids.size(); ++i) row_cnt += 2*map_server->at(update_ids[i])->numOfMonoFrames()-3; const int max_possible_rows = row_cnt; Eigen::VectorXd res_large(max_possible_rows); Eigen::MatrixXd H_large(max_possible_rows, max_possible_cols); res_large.setZero(); H_large.setZero(); int valid_id_cnt = 0; row_cnt = 0; int col_cnt = 0; for (int i = 0; i < update_ids.size(); ++i) { Eigen::VectorXd res_block; Eigen::MatrixXd H_block; std::map, int> block_index_map; std::vector> block_var_order; this->calcResJacobianSingleFeatAllMonoObs(map_server->at(update_ids[i]), state->_sw_camleft_poses, block_var_order, block_index_map, res_block, H_block); if (!testChiSquared(state, res_block, H_block, block_var_order, this->_noise, map_server->at(update_ids[i])->_mono_obs.size()-1)) continue; Eigen::MatrixXd H_large_row_block = Eigen::MatrixXd::Zero(res_block.rows(), max_possible_cols); for (const auto& item : block_index_map) { const std::shared_ptr& pose_ptr = item.first; const int& sub_idx = item.second; if (sw_index_map.find(pose_ptr) == sw_index_map.end()) { sw_index_map[pose_ptr] = col_cnt; sw_var_order.push_back(pose_ptr); col_cnt += pose_ptr->size(); } H_large_row_block.block(0, sw_index_map.at(pose_ptr), H_block.rows(), pose_ptr->size()) = H_block.block(0, sub_idx, H_block.rows(), pose_ptr->size()); } H_large.block(row_cnt, 0, H_block.rows(), max_possible_cols) = H_large_row_block; res_large.block(row_cnt, 0, res_block.rows(), 1) = res_block; row_cnt += res_block.rows(); ++valid_id_cnt; if (valid_id_cnt >= _max_valid_ids) break; } /* if (valid_id_cnt > 0) std::cout << "[RemoveLostUpdate]: Features used in remove update = " << valid_id_cnt << std::endl; */ if (row_cnt < max_possible_rows || col_cnt < max_possible_cols) { H_large.conservativeResize(row_cnt, col_cnt); res_large.conservativeResize(row_cnt, 1); } Eigen::MatrixXd H_thin; Eigen::VectorXd res_thin; if (H_large.rows() > H_large.cols()) { Eigen::SparseMatrix H_sparse = H_large.sparseView(); Eigen::SPQR> spqr_helper; spqr_helper.setSPQROrdering(SPQR_ORDERING_NATURAL); spqr_helper.compute(H_sparse); Eigen::MatrixXd H_temp; Eigen::VectorXd r_temp; (spqr_helper.matrixQ().transpose()*H_large).evalTo(H_temp); (spqr_helper.matrixQ().transpose()*res_large).evalTo(r_temp); H_thin = H_temp.topRows(row_cnt); res_thin = r_temp.head(row_cnt); } else { H_thin = H_large; res_thin = res_large; } if (res_thin.rows() > 0) StateManager::ekfUpdate(state, sw_var_order, H_thin, res_thin, std::pow(this->_noise, 2)*Eigen::MatrixXd::Identity(res_thin.rows(), res_thin.rows())); for (const auto& item : update_ids) map_server->erase(item); } void RemoveLostUpdate::calcResJacobianSingleFeatAllMonoObs( const std::shared_ptr feature_info, const std::map>& sw_poses, std::vector>& block_var_order, std::map, int>& block_index_map, Eigen::VectorXd& res_block, Eigen::MatrixXd& H_block) { const int max_possible_rows = 2*feature_info->numOfMonoFrames(); const int max_possible_cols = 6*sw_poses.size(); Eigen::VectorXd res_block_tmp = Eigen::VectorXd::Zero(max_possible_rows); Eigen::MatrixXd H_block_tmp = Eigen::MatrixXd::Zero(max_possible_rows, max_possible_cols); Eigen::MatrixXd Hf_block_tmp = Eigen::MatrixXd::Zero(max_possible_rows, 3); const Eigen::Vector3d pf_w = feature_info->_landmark->valuePosXyz(); const std::shared_ptr pose_anchor_ptr = feature_info->_landmark->getAnchoredPose(); block_var_order.clear(); block_index_map.clear(); block_index_map[pose_anchor_ptr] = 0; block_var_order.push_back(pose_anchor_ptr); int row_cnt = 0; int col_cnt = 6; for (const auto& item : feature_info->_mono_obs) { const double& time_of_obs = item.first; if (sw_poses.find(time_of_obs) == sw_poses.end()) continue; const std::shared_ptr& mono_obs_ptr = item.second; const std::shared_ptr& pose_obs_ptr = sw_poses.at(time_of_obs); const Eigen::Matrix3d R_cm2w = pose_obs_ptr->valueLinearAsMat(); const Eigen::Vector3d p_cm = pose_obs_ptr->valueTrans(); const Eigen::Vector3d pf_cm = R_cm2w.transpose()*(pf_w-p_cm); Eigen::Matrix H_proj = Eigen::Matrix::Zero(); H_proj(0, 0) = 1.0/pf_cm.z(); H_proj(0, 2) = -pf_cm.x()/std::pow(pf_cm.z(), 2); H_proj(1, 1) = 1.0/pf_cm.z(); H_proj(1, 2) = -pf_cm.y()/std::pow(pf_cm.z(), 2); bool flag = false; if (block_index_map.find(pose_obs_ptr) == block_index_map.end()) { flag = true; block_index_map[pose_obs_ptr] = col_cnt; block_var_order.push_back(pose_obs_ptr); col_cnt += pose_obs_ptr->size(); } Eigen::MatrixXd H_pf2x = Eigen::MatrixXd::Zero(3, max_possible_cols); if (pose_obs_ptr != pose_anchor_ptr) { H_pf2x.block<3, 3>(0, block_index_map.at(pose_obs_ptr)) = R_cm2w.transpose()*skew(pf_w); H_pf2x.block<3, 3>(0, block_index_map.at(pose_anchor_ptr)) = -H_pf2x.block<3, 3>(0, block_index_map.at(pose_obs_ptr)); } H_pf2x.block<3, 3>(0, block_index_map.at(pose_obs_ptr)+3) = -R_cm2w.transpose(); Eigen::Matrix3d H_pf2pf = R_cm2w.transpose(); if (H_proj.hasNaN() || H_pf2x.hasNaN()) { if (flag) { block_index_map.erase(pose_obs_ptr); block_var_order.pop_back(); col_cnt -= pose_obs_ptr->size(); } continue; } H_block_tmp.block(row_cnt, 0, 2, max_possible_cols) = H_proj*H_pf2x; Hf_block_tmp.block(row_cnt, 0, 2, 3) = H_proj*H_pf2pf; res_block_tmp.block(row_cnt, 0, 2, 1) = mono_obs_ptr->asVec()-Eigen::Vector2d(pf_cm.x()/pf_cm.z(), pf_cm.y()/pf_cm.z()); row_cnt += 2; } if (row_cnt < max_possible_rows || col_cnt < max_possible_cols) { res_block_tmp.conservativeResize(row_cnt, 1); H_block_tmp.conservativeResize(row_cnt, col_cnt); Hf_block_tmp.conservativeResize(row_cnt, 3); } if (row_cnt <= 3) std::cout << "[RemoveLostUpdate]: Warning in mono feats removal, row cnt <= 3! " << std::endl; Eigen::JacobiSVD svd_helper(Hf_block_tmp, Eigen::ComputeFullU | Eigen::ComputeThinV); Eigen::MatrixXd V = svd_helper.matrixU().rightCols(row_cnt-3); H_block = V.transpose()*H_block_tmp; res_block = V.transpose()*res_block_tmp; } void RemoveLostUpdate::updateStateStereo(std::shared_ptr state, std::shared_ptr map_server, std::shared_ptr tri) { MapServerManager::markMargStereoFeatures(map_server, state); std::vector update_ids, direct_marg_ids; for (auto& item : *map_server) if (item.second->_ftype == FeatureInfo::FeatureType::MSCKF && item.second->_isToMarg) { if (FeatureInfoManager::triangulateFeatureInfoStereo(item.second, tri, state) && item.second->numOfStereoFrames() >= 3) update_ids.push_back(item.first); else direct_marg_ids.push_back(item.first); } for (const auto& item : direct_marg_ids) map_server->erase(item); if (update_ids.size() == 0) return; std::map, int> sw_index_map; std::vector> sw_var_order; const int max_possible_cols = 6*state->_sw_camleft_poses.size(); int row_cnt = 0; for (int i = 0; i < update_ids.size(); ++i) row_cnt += 4*map_server->at(update_ids[i])->numOfStereoFrames()-3; const int max_possible_rows = row_cnt; Eigen::VectorXd res_large(max_possible_rows); Eigen::MatrixXd H_large(max_possible_rows, max_possible_cols); res_large.setZero(); H_large.setZero(); int valid_id_cnt = 0; row_cnt = 0; int col_cnt = 0; for (int i = 0; i < update_ids.size(); ++i) { Eigen::VectorXd res_block; Eigen::MatrixXd H_block; std::map, int> block_index_map; std::vector> block_var_order; this->calcResJacobianSingleFeatAllStereoObs(map_server->at(update_ids[i]), state->_sw_camleft_poses, state->_state_params._T_cl2cr, block_var_order, block_index_map, res_block, H_block); if (!testChiSquared(state, res_block, H_block, block_var_order, this->_noise, map_server->at(update_ids[i])->_stereo_obs.size()-1)) continue; Eigen::MatrixXd H_large_row_block = Eigen::MatrixXd::Zero(res_block.rows(), max_possible_cols); for (const auto& item : block_index_map) { const std::shared_ptr& pose_ptr = item.first; const int& sub_idx = item.second; if (sw_index_map.find(pose_ptr) == sw_index_map.end()) { sw_index_map[pose_ptr] = col_cnt; sw_var_order.push_back(pose_ptr); col_cnt += pose_ptr->size(); } H_large_row_block.block(0, sw_index_map.at(pose_ptr), H_block.rows(), pose_ptr->size()) = H_block.block(0, sub_idx, H_block.rows(), pose_ptr->size()); } H_large.block(row_cnt, 0, H_block.rows(), max_possible_cols) = H_large_row_block; res_large.block(row_cnt, 0, res_block.rows(), 1) = res_block; row_cnt += res_block.rows(); ++valid_id_cnt; if (valid_id_cnt >= _max_valid_ids) break; } /* if (valid_id_cnt > 0) std::cout << "[RemoveLostUpdate]: Features used in remove update = " << valid_id_cnt << std::endl; */ if (row_cnt < max_possible_rows || col_cnt < max_possible_cols) { H_large.conservativeResize(row_cnt, col_cnt); res_large.conservativeResize(row_cnt, 1); } Eigen::MatrixXd H_thin; Eigen::VectorXd res_thin; if (H_large.rows() > H_large.cols()) { Eigen::SparseMatrix H_sparse = H_large.sparseView(); Eigen::SPQR> spqr_helper; spqr_helper.setSPQROrdering(SPQR_ORDERING_NATURAL); spqr_helper.compute(H_sparse); Eigen::MatrixXd H_temp; Eigen::VectorXd r_temp; (spqr_helper.matrixQ().transpose()*H_large).evalTo(H_temp); (spqr_helper.matrixQ().transpose()*res_large).evalTo(r_temp); H_thin = H_temp.topRows(row_cnt); res_thin = r_temp.head(row_cnt); } else { H_thin = H_large; res_thin = res_large; } if (res_thin.rows() > 0) StateManager::ekfUpdate(state, sw_var_order, H_thin, res_thin, std::pow(this->_noise, 2)*Eigen::MatrixXd::Identity(res_thin.rows(), res_thin.rows())); for (const auto& item : update_ids) map_server->erase(item); } void RemoveLostUpdate::calcResJacobianSingleFeatAllStereoObs( const std::shared_ptr feature_info, const std::map>& sw_poses, const Eigen::Isometry3d& T_cl2cr, std::vector>& block_var_order, std::map, int>& block_index_map, Eigen::VectorXd& res_block, Eigen::MatrixXd& H_block) { const int max_possible_rows = 4*feature_info->numOfStereoFrames(); const int max_possible_cols = 6*sw_poses.size(); Eigen::VectorXd res_block_tmp = Eigen::VectorXd::Zero(max_possible_rows); Eigen::MatrixXd H_block_tmp = Eigen::MatrixXd::Zero(max_possible_rows, max_possible_cols); Eigen::MatrixXd Hf_block_tmp = Eigen::MatrixXd::Zero(max_possible_rows, 3); const Eigen::Vector3d pf_w = feature_info->_landmark->valuePosXyz(); const std::shared_ptr pose_anchor_ptr = feature_info->_landmark->getAnchoredPose(); block_var_order.clear(); block_index_map.clear(); block_index_map[pose_anchor_ptr] = 0; block_var_order.push_back(pose_anchor_ptr); int row_cnt = 0; int col_cnt = 6; for (const auto& item : feature_info->_stereo_obs) { const double& time_of_obs = item.first; if (sw_poses.find(time_of_obs) == sw_poses.end()) continue; const std::shared_ptr& stereo_obs_ptr = item.second; const std::shared_ptr& pose_obs_ptr = sw_poses.at(time_of_obs); const Eigen::Matrix3d R_cm2w = pose_obs_ptr->valueLinearAsMat(); const Eigen::Vector3d p_cm = pose_obs_ptr->valueTrans(); const Eigen::Vector3d pf_cm = R_cm2w.transpose()*(pf_w-p_cm); const Eigen::Vector3d pf_cm_r = T_cl2cr*pf_cm; Eigen::Matrix H_proj = Eigen::Matrix::Zero(); H_proj(0, 0) = 1.0/pf_cm.z(); H_proj(0, 2) = -pf_cm.x()/std::pow(pf_cm.z(), 2); H_proj(1, 1) = 1.0/pf_cm.z(); H_proj(1, 2) = -pf_cm.y()/std::pow(pf_cm.z(), 2); Eigen::Matrix H_proj_r = Eigen::Matrix::Zero(); H_proj_r(0, 0) = 1.0/pf_cm_r.z(); H_proj_r(0, 2) = -pf_cm_r.x()/std::pow(pf_cm_r.z(), 2); H_proj_r(1, 1) = 1.0/pf_cm_r.z(); H_proj_r(1, 2) = -pf_cm_r.y()/std::pow(pf_cm_r.z(), 2); bool flag = false; if (block_index_map.find(pose_obs_ptr) == block_index_map.end()) { flag = true; block_index_map[pose_obs_ptr] = col_cnt; block_var_order.push_back(pose_obs_ptr); col_cnt += pose_obs_ptr->size(); } Eigen::MatrixXd H_pf2x = Eigen::MatrixXd::Zero(3, max_possible_cols); if (pose_obs_ptr != pose_anchor_ptr) { H_pf2x.block<3, 3>(0, block_index_map.at(pose_obs_ptr)) = R_cm2w.transpose()*skew(pf_w); H_pf2x.block<3, 3>(0, block_index_map.at(pose_anchor_ptr)) = -H_pf2x.block<3, 3>(0, block_index_map.at(pose_obs_ptr)); } H_pf2x.block<3, 3>(0, block_index_map.at(pose_obs_ptr)+3) = -R_cm2w.transpose(); Eigen::Matrix3d H_pf2pf = R_cm2w.transpose(); if (H_proj.hasNaN() || H_pf2x.hasNaN()) { if (flag) { block_index_map.erase(pose_obs_ptr); block_var_order.pop_back(); col_cnt -= pose_obs_ptr->size(); } continue; } H_block_tmp.block(row_cnt, 0, 2, max_possible_cols) = H_proj*H_pf2x; Hf_block_tmp.block(row_cnt, 0, 2, 3) = H_proj*H_pf2pf; H_block_tmp.block(row_cnt+2, 0, 2, max_possible_cols) = H_proj_r*T_cl2cr.linear()*H_pf2x; Hf_block_tmp.block(row_cnt+2, 0, 2, 3) = H_proj_r*T_cl2cr.linear()*H_pf2pf; res_block_tmp.block(row_cnt, 0, 4, 1) = stereo_obs_ptr->asVec()-Eigen::Vector4d(pf_cm.x()/pf_cm.z(), pf_cm.y()/pf_cm.z(), pf_cm_r.x()/pf_cm_r.z(), pf_cm_r.y()/pf_cm_r.z()); row_cnt += 4; } if (row_cnt < max_possible_rows || col_cnt < max_possible_cols) { res_block_tmp.conservativeResize(row_cnt, 1); H_block_tmp.conservativeResize(row_cnt, col_cnt); Hf_block_tmp.conservativeResize(row_cnt, 3); } if (row_cnt <= 3) std::cout << "[RemoveLostUpdate]: Warning in stereo feats removal, row cnt <= 3! " << std::endl; Eigen::JacobiSVD svd_helper(Hf_block_tmp, Eigen::ComputeFullU | Eigen::ComputeThinV); Eigen::MatrixXd V = svd_helper.matrixU().rightCols(row_cnt-3); H_block = V.transpose()*H_block_tmp; res_block = V.transpose()*res_block_tmp; } } ================================================ FILE: ingvio_estimator/src/RemoveLostUpdate.h ================================================ /** This File is part of InGVIO, an invariant filter for mono/stereo visual- * inertial-raw GNSS navigation. * * Copyright (C) 2022 Changwu Liu (cwliu529@163.com, * lcw18@mails.tsinghua.edu.cn (valid until 2023)) * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program. If not, see . */ #pragma once #include "MapServer.h" #include "Update.h" namespace ingvio { class Triangulator; class IngvioParams; class RemoveLostUpdate : public UpdateBase { public: using UpdateBase::UpdateBase; RemoveLostUpdate(const IngvioParams& filter_params) : UpdateBase(filter_params._chi2_max_dof, filter_params._chi2_thres), _max_valid_ids(20), _noise(filter_params._visual_noise) {} RemoveLostUpdate(const RemoveLostUpdate&) = delete; RemoveLostUpdate operator=(const RemoveLostUpdate&) = delete; virtual ~RemoveLostUpdate() {} void updateStateMono(std::shared_ptr state, std::shared_ptr map_server, std::shared_ptr tri); void updateStateStereo(std::shared_ptr state, std::shared_ptr map_server, std::shared_ptr tri); protected: void calcResJacobianSingleFeatAllMonoObs( const std::shared_ptr feature_info, const std::map>& sw_poses, std::vector>& block_var_order, std::map, int>& block_index_map, Eigen::VectorXd& res_block, Eigen::MatrixXd& H_block); void calcResJacobianSingleFeatAllStereoObs( const std::shared_ptr feature_info, const std::map>& sw_poses, const Eigen::Isometry3d& T_cl2cr, std::vector>& block_var_order, std::map, int>& block_index_map, Eigen::VectorXd& res_block, Eigen::MatrixXd& H_block); int _max_valid_ids; double _noise; }; } ================================================ FILE: ingvio_estimator/src/State.cpp ================================================ /** This File is part of InGVIO, an invariant filter for mono/stereo visual- * inertial-raw GNSS navigation. * * Copyright (C) 2022 Changwu Liu (cwliu529@163.com, * lcw18@mails.tsinghua.edu.cn (valid until 2023)) * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program. If not, see . */ #include "State.h" namespace ingvio { StateParams::StateParams(const IngvioParams& filter_params) { _cam_nums = filter_params._cam_nums; _max_sw_poses = filter_params._max_sw_clones; _max_landmarks = filter_params._max_lm_feats; _T_cl2cr = filter_params._T_cr2i.inverse()*filter_params._T_cl2i; _T_cl2i = filter_params._T_cl2i; _enable_gnss = static_cast(filter_params._enable_gnss); _noise_a = filter_params._noise_a; _noise_g = filter_params._noise_g; _noise_ba = filter_params._noise_ba; _noise_bg = filter_params._noise_bg; _init_cov_rot = filter_params._init_cov_rot; _init_cov_pos = filter_params._init_cov_pos; _init_cov_vel = filter_params._init_cov_vel; _init_cov_bg = filter_params._init_cov_bg; _init_cov_ba = filter_params._init_cov_ba; _init_cov_ext_rot = filter_params._init_cov_ext_rot; _init_cov_ext_pos = filter_params._init_cov_ext_pos; if (_enable_gnss) { _noise_clockbias = filter_params._noise_clockbias; _noise_clockbias = filter_params._noise_cb_rw; _init_cov_rcv_clockbias = filter_params._init_cov_rcv_clockbias; _init_cov_rcv_clockbias_randomwalk = filter_params._init_cov_rcv_clockbias_randomwalk; _init_cov_yof = filter_params._init_cov_yof; } } State::State(const IngvioParams& filter_params) : _state_params(filter_params) { int idx = 0; _extended_pose = std::make_shared(); _extended_pose->set_cov_idx(idx); _err_variables.push_back(_extended_pose); idx += _extended_pose->size(); _bg = std::make_shared(); _bg->set_cov_idx(idx); _err_variables.push_back(_bg); idx += _bg->size(); _ba = std::make_shared(); _ba->set_cov_idx(idx); _err_variables.push_back(_ba); idx += _ba->size(); _camleft_imu_extrinsics = std::make_shared(); _camleft_imu_extrinsics->set_cov_idx(idx); _err_variables.push_back(_camleft_imu_extrinsics); idx += _camleft_imu_extrinsics->size(); _gnss.clear(); _sw_camleft_poses.clear(); _anchored_landmarks.clear(); _cov = std::pow(1e-03, 2)*Eigen::MatrixXd::Identity(idx, idx); _camleft_imu_extrinsics->setValueByIso(filter_params._T_cl2i); } State::State() : _state_params() { int idx = 0; _extended_pose = std::make_shared(); _extended_pose->set_cov_idx(idx); _err_variables.push_back(_extended_pose); idx += _extended_pose->size(); _bg = std::make_shared(); _bg->set_cov_idx(idx); _err_variables.push_back(_bg); idx += _bg->size(); _ba = std::make_shared(); _ba->set_cov_idx(idx); _err_variables.push_back(_ba); idx += _ba->size(); _camleft_imu_extrinsics = std::make_shared(); _camleft_imu_extrinsics->set_cov_idx(idx); _err_variables.push_back(_camleft_imu_extrinsics); idx += _camleft_imu_extrinsics->size(); _gnss.clear(); _sw_camleft_poses.clear(); _anchored_landmarks.clear(); _cov = std::pow(1e-03, 2)*Eigen::MatrixXd::Identity(idx, idx); _camleft_imu_extrinsics->setValueByIso(Eigen::Isometry3d::Identity()); } void State::initStateAndCov(double init_timestamp, const Eigen::Quaterniond& init_quat_i2w, const Eigen::Vector3d& init_pos, const Eigen::Vector3d& init_vel, const Eigen::Vector3d& init_bg, const Eigen::Vector3d& init_ba) { _timestamp = init_timestamp; for (int i = _extended_pose->idx(); i < _extended_pose->idx()+3; ++i) _cov(i, i) = std::pow(_state_params._init_cov_rot, 2.0); for (int i = _extended_pose->idx()+3; i < _extended_pose->idx()+6; ++i) _cov(i, i) = std::pow(_state_params._init_cov_pos, 2.0); for (int i = _extended_pose->idx()+6; i < _extended_pose->idx()+9; ++i) _cov(i, i) = std::pow(_state_params._init_cov_vel, 2.0); for (int i = _bg->idx(); i < _bg->idx()+_bg->size(); ++i) _cov(i, i) = std::pow(_state_params._init_cov_bg, 2.0); for (int i = _ba->idx(); i < _ba->idx()+_ba->size(); ++i) _cov(i, i) = std::pow(_state_params._init_cov_ba, 2.0); for (int i = _camleft_imu_extrinsics->idx(); i < _camleft_imu_extrinsics->idx()+3; ++i) _cov(i, i) = std::pow(_state_params._init_cov_ext_rot, 2.0); for (int i = _camleft_imu_extrinsics->idx()+3; i < _camleft_imu_extrinsics->idx()+6; ++i) _cov(i, i) = std::pow(_state_params._init_cov_ext_pos, 2.0); this->_extended_pose->setValueLinearByQuat(init_quat_i2w); this->_extended_pose->setValueTrans1(init_pos); this->_extended_pose->setValueTrans2(init_vel); this->_bg->setValue(init_bg); this->_ba->setValue(init_ba); this->_camleft_imu_extrinsics->setValueByIso(_state_params._T_cl2i); } void State::initStateAndCov(double init_timestamp, const Eigen::Quaterniond& init_quat_i2w, const Eigen::Vector3d& init_pos) { this->initStateAndCov(init_timestamp, init_quat_i2w, init_pos, Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero()); } void State::initStateAndCov(double init_timestamp, const Eigen::Quaterniond& init_quat_i2w) { this->initStateAndCov(init_timestamp, init_quat_i2w, Eigen::Vector3d::Zero()); } double distance(std::shared_ptr state1, std::shared_ptr state2) { if (state1->_timestamp != state2->_timestamp) std::cout << "[State]: Timestamp not the same when calc distance!" << std::endl; double dist = 0.0; dist += (state1->_extended_pose->valueLinearAsMat()-state2->_extended_pose->valueLinearAsMat()).norm(); dist += (state1->_extended_pose->valueTrans1()-state2->_extended_pose->valueTrans1()).norm(); dist += (state1->_extended_pose->valueTrans2()-state2->_extended_pose->valueTrans2()).norm(); dist += (state1->_bg->value()-state2->_bg->value()).norm(); dist += (state1->_ba->value()-state2->_ba->value()).norm(); dist += (state1->_camleft_imu_extrinsics->valueLinearAsMat()-state2->_camleft_imu_extrinsics->valueLinearAsMat()).norm(); dist += (state1->_camleft_imu_extrinsics->valueTrans()-state2->_camleft_imu_extrinsics->valueTrans()).norm(); for (const auto& item : state1->_gnss) { assert(state2->_gnss.find(item.first) != state2->_gnss.end()); dist += std::fabs(item.second->value() - state2->_gnss.at(item.first)->value()); } for (const auto& item : state1->_anchored_landmarks) { assert(state2->_anchored_landmarks.find(item.first) != state2->_anchored_landmarks.end()); dist += (item.second->valuePosXyz()-state2->_anchored_landmarks.at(item.first)->valuePosXyz()).norm(); } for (const auto& item : state2->_sw_camleft_poses) { assert(state2->_sw_camleft_poses.find(item.first) != state2->_sw_camleft_poses.end()); dist += (item.second->valueLinearAsMat()-state2->_sw_camleft_poses.at(item.first)->valueLinearAsMat()).norm(); dist += (item.second->valueTrans()-state2->_sw_camleft_poses.at(item.first)->valueTrans()).norm(); } return dist; } } ================================================ FILE: ingvio_estimator/src/State.h ================================================ /** This File is part of InGVIO, an invariant filter for mono/stereo visual- * inertial-raw GNSS navigation. * * Copyright (C) 2022 Changwu Liu (cwliu529@163.com, * lcw18@mails.tsinghua.edu.cn (valid until 2023)) * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program. If not, see . */ #pragma once #include #include #include #include #include #include "VecState.h" #include "PoseState.h" #include "AnchoredLandmark.h" #include "IngvioParams.h" namespace ingvio { class StateParams { public: StateParams() = default; StateParams(const IngvioParams& filter_params); int _cam_nums = 2; int _max_sw_poses = 20; int _max_landmarks = 25; double _noise_g = 0.005; double _noise_a = 0.05; double _noise_bg = 0.001; double _noise_ba = 0.01; double _noise_clockbias = 2.0; double _noise_cb_rw = 0.2; double _init_cov_rot; double _init_cov_pos; double _init_cov_vel; double _init_cov_bg; double _init_cov_ba; double _init_cov_ext_rot; double _init_cov_ext_pos; double _init_cov_rcv_clockbias; double _init_cov_rcv_clockbias_randomwalk; double _init_cov_yof; bool _enable_gnss = true; Eigen::Isometry3d _T_cl2cr = Eigen::Isometry3d::Identity(); Eigen::Isometry3d _T_cl2i = Eigen::Isometry3d::Identity(); }; class State { public: enum GNSSType {GPS = 0, GLO, GAL, BDS, FS, YOF}; State(); State(const IngvioParams& filter_params); ~State() {} double nextMargTime() { double time = INFINITY; if (_sw_camleft_poses.size() > _state_params._max_sw_poses) for (const auto& item: _sw_camleft_poses) if (item.first < time) time = item.first; return time; } void initStateAndCov(double init_timestamp, const Eigen::Quaterniond& init_quat_i2w); void initStateAndCov(double init_timestamp, const Eigen::Quaterniond& init_quat_i2w, const Eigen::Vector3d& init_pos); void initStateAndCov(double init_timestamp, const Eigen::Quaterniond& init_quat_i2w, const Eigen::Vector3d& init_pos, const Eigen::Vector3d& init_vel, const Eigen::Vector3d& init_bg, const Eigen::Vector3d& init_ba); int curr_cov_size() { return _cov.rows(); } int curr_err_variable_size() { return _err_variables.size();} double _timestamp = -1; StateParams _state_params; std::shared_ptr _extended_pose; std::shared_ptr _bg; std::shared_ptr _ba; std::shared_ptr _camleft_imu_extrinsics; std::unordered_map> _gnss; std::unordered_map> _anchored_landmarks; std::map, std::less> _sw_camleft_poses; private: friend class StateManager; Eigen::MatrixXd _cov; std::vector> _err_variables; }; extern double distance(std::shared_ptr state1, std::shared_ptr state2); } ================================================ FILE: ingvio_estimator/src/StateManager.cpp ================================================ /** This File is part of InGVIO, an invariant filter for mono/stereo visual- * inertial-raw GNSS navigation. * * Copyright (C) 2022 Changwu Liu (cwliu529@163.com, * lcw18@mails.tsinghua.edu.cn (valid until 2023)) * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program. If not, see . */ #include #include "StateManager.h" namespace ingvio { bool StateManager::checkStateContinuity(const std::shared_ptr state) { int idx = 0; for (int i = 0; i < state->_err_variables.size(); ++i) if (state->_err_variables[i]->idx() == idx) idx += state->_err_variables[i]->size(); else return false; if (state->_cov.rows() == idx && state->_cov.cols() == idx) return true; else return false; } void StateManager::propagateStateCov(std::shared_ptr state, const Eigen::Matrix& Phi_imu, const Eigen::Matrix& G_imu, double dt) { assert(StateManager::checkStateContinuity(state)); Eigen::MatrixXd cov_tmp(state->curr_cov_size(), state->curr_cov_size()); cov_tmp.block<15, 15>(0, 0) = Phi_imu*state->_cov.block<15, 15>(0, 0)*Phi_imu.transpose(); Eigen::MatrixXd cov21 = state->_cov.block(15, 0, state->_cov.rows()-15, 15)*Phi_imu.transpose(); Eigen::MatrixXd cov22 = state->_cov.block(15, 15, state->_cov.rows()-15, state->_cov.cols()-15); if (state->_state_params._enable_gnss && state->_gnss.find(State::GNSSType::FS) != state->_gnss.end()) { Eigen::MatrixXd cov21_tmp = cov21; Eigen::MatrixXd cov22_tmp = cov22; const int local_col_idx = state->_gnss.find(State::GNSSType::FS)->second->idx() - 15; for (int i = 0; i < 4; ++i) { auto item = state->_gnss.find(static_cast(i)); if (item != state->_gnss.end()) { const int local_row_idx = item->second->idx() - 15; cov21_tmp.row(local_row_idx) += dt*cov21.row(local_col_idx); cov22_tmp.row(local_row_idx) += dt*cov22.row(local_col_idx); } } cov21 = cov21_tmp; cov22 = cov22_tmp; for (int i = 0; i < 4; ++i) { auto item = state->_gnss.find(static_cast(i)); if (item != state->_gnss.end()) { const int local_row_idx = item->second->idx() - 15; cov22_tmp.col(local_row_idx) += dt*cov22.col(local_col_idx); } } cov22 = cov22_tmp; } cov_tmp.block(15, 0, cov_tmp.rows()-15, 15) = cov21; cov_tmp.block(0, 15, 15, cov_tmp.cols()-15) = cov21.transpose(); cov_tmp.block(15, 15, cov_tmp.rows()-15, cov_tmp.cols()-15) = cov22; Eigen::Matrix G_tmp = G_imu; G_tmp.middleCols<3>(0) *= state->_state_params._noise_g; G_tmp.middleCols<3>(3) *= state->_state_params._noise_a; G_tmp.middleCols<3>(6) *= state->_state_params._noise_bg; G_tmp.middleCols<3>(9) *= state->_state_params._noise_ba; cov_tmp.block<15, 15>(0, 0) += dt*Phi_imu*G_tmp*G_tmp.transpose()*Phi_imu.transpose(); if (state->_state_params._enable_gnss) for (int i = 0; i < 5; ++i) { auto item1 = state->_gnss.find(static_cast(i)); if (item1 == state->_gnss.end()) continue; for (int j = 0; j < 5; ++j) { auto item2 = state->_gnss.find(static_cast(j)); if (item2 == state->_gnss.end()) continue; if (item1->first != State::GNSSType::FS && item2->first != State::GNSSType::FS) cov_tmp(item1->second->idx(), item2->second->idx()) += dt*std::pow(state->_state_params._noise_clockbias, 2) + std::pow(dt, 3)*std::pow(state->_state_params._noise_cb_rw, 2); else if (item1->first == State::GNSSType::FS && item2->first == State::GNSSType::FS) cov_tmp(item1->second->idx(), item2->second->idx()) += dt*std::pow(state->_state_params._noise_cb_rw, 2); else cov_tmp(item1->second->idx(), item2->second->idx()) += std::pow(dt, 2)*std::pow(state->_state_params._noise_cb_rw, 2); } } state->_cov = 0.5*(cov_tmp + cov_tmp.transpose()); } Eigen::MatrixXd StateManager::getFullCov(std::shared_ptr state) { Eigen::MatrixXd cov(state->curr_cov_size(), state->curr_cov_size()); cov = state->_cov; return cov; } Eigen::MatrixXd StateManager::getMarginalCov(std::shared_ptr state, const std::vector>& small_variables) { int small_cov_size = 0; for (int i = 0; i < small_variables.size(); ++i) small_cov_size += small_variables[i]->size(); Eigen::MatrixXd small_cov(small_cov_size, small_cov_size); int row_idx = 0; for (int i = 0; i < small_variables.size(); ++i) { int col_idx = 0; for(int j = 0; j < small_variables.size(); ++j) { small_cov.block(row_idx, col_idx, small_variables[i]->size(), small_variables[j]->size()) = state->_cov.block(small_variables[i]->idx(), small_variables[j]->idx(), small_variables[i]->size(), small_variables[j]->size()); col_idx += small_variables[j]->size(); } row_idx += small_variables[i]->size(); } return small_cov; } void StateManager::marginalize(std::shared_ptr state, std::shared_ptr marg) { if (std::find(state->_err_variables.begin(), state->_err_variables.end(), marg) == state->_err_variables.end() ) { std::cout << "[StateManager]: Marg is not in the current state!" << std::endl; std::exit(EXIT_FAILURE); } int marg_size = marg->size(); int marg_start = marg->idx(); int marg_end = marg->idx() + marg->size(); Eigen::MatrixXd cov_tmp(state->_cov.rows() - marg_size, state->_cov.cols() - marg_size); cov_tmp.block(0, 0, marg_start, marg_start) = state->_cov.block(0, 0, marg_start, marg_start); cov_tmp.block(marg_start, 0, cov_tmp.rows()-marg_start, marg_start) = state->_cov.block(marg_end, 0, cov_tmp.rows()-marg_start, marg_start); cov_tmp.block(0, marg_start, marg_start, cov_tmp.cols()-marg_start) = state->_cov.block(0, marg_end, marg_start, cov_tmp.cols()-marg_start); cov_tmp.block(marg_start, marg_start, cov_tmp.rows()-marg_start, cov_tmp.cols()-marg_start) = state->_cov.block(marg_end, marg_end, cov_tmp.rows()-marg_start, cov_tmp.cols()-marg_start); state->_cov = cov_tmp; std::vector> remaining_variables; for (int i = 0; i < state->_err_variables.size(); ++i) if (state->_err_variables[i] != marg) { if (state->_err_variables[i]->idx() > marg_start) state->_err_variables[i]->set_cov_idx(state->_err_variables[i]->idx()-marg_size); remaining_variables.push_back(state->_err_variables[i]); } marg->set_cov_idx(-1); state->_err_variables = remaining_variables; } void StateManager::addVariableIndependent(std::shared_ptr state, std::shared_ptr new_state, const Eigen::MatrixXd& new_state_cov_block) { assert(new_state->size() == new_state_cov_block.rows()); int old_cov_size = state->curr_cov_size(); Eigen::MatrixXd new_cov(old_cov_size+new_state->size(), old_cov_size+new_state->size()); new_cov.setZero(); new_cov.block(0, 0, old_cov_size, old_cov_size) = state->_cov; new_cov.block(old_cov_size, old_cov_size, new_state->size(), new_state->size()) = new_state_cov_block; new_state->set_cov_idx(old_cov_size); state->_cov = new_cov; state->_err_variables.push_back(new_state); assert(StateManager::checkStateContinuity(state)); } void StateManager::addGNSSVariable(std::shared_ptr state, const State::GNSSType& gtype, double value, double cov) { if (state->_gnss.find(gtype) != state->_gnss.end()) std::cout << "[StateManager]: GNSS variable already in the state, adding operation will rewrite such var!" << std::endl; state->_gnss[gtype] = std::make_shared(); state->_gnss[gtype]->setValue(value); Eigen::MatrixXd scalar_cov(1, 1); scalar_cov(0, 0) = cov; StateManager::addVariableIndependent(state, state->_gnss[gtype], scalar_cov); } void StateManager::margGNSSVariable(std::shared_ptr state, const State::GNSSType& gtype) { if (state->_gnss.find(gtype) == state->_gnss.end()) std::cout << "[StateManager]: GNSS variable not in the state, no need to marg!" << std::endl; StateManager::marginalize(state, state->_gnss.at(gtype)); state->_gnss.erase(gtype); } void StateManager::boxPlus(std::shared_ptr state, const Eigen::VectorXd& dx) { assert(dx.rows() == state->curr_cov_size()); assert(StateManager::checkStateContinuity(state)); for (int i = 0; i < state->_err_variables.size(); ++i) state->_err_variables[i]->update(dx); } void StateManager::augmentSlidingWindowPose(std::shared_ptr state) { if (state->_sw_camleft_poses.find(state->_timestamp) != state->_sw_camleft_poses.end()) { std::cout << "[StateManager]: Curr pose already in the sw, cannot clone!" << std::endl; return; } std::shared_ptr leftcam_pose_clone = std::shared_ptr(new SE3()); Eigen::Isometry3d T_i2w = Eigen::Isometry3d::Identity(); T_i2w.linear() = state->_extended_pose->valueLinearAsMat(); T_i2w.translation() = state->_extended_pose->valueTrans1(); Eigen::Isometry3d T_cl2i = Eigen::Isometry3d::Identity(); T_cl2i.linear() = state->_camleft_imu_extrinsics->valueLinearAsMat(); T_cl2i.translation() = state->_camleft_imu_extrinsics->valueTrans(); leftcam_pose_clone->setValueByIso(T_i2w*T_cl2i); leftcam_pose_clone->setFejByIso(T_i2w*T_cl2i); leftcam_pose_clone->set_cov_idx(state->curr_cov_size()); state->_sw_camleft_poses[state->_timestamp] = leftcam_pose_clone; state->_err_variables.push_back(leftcam_pose_clone); Eigen::Matrix J = Eigen::Matrix::Zero(); J.block<6, 6>(0, 0).setIdentity(); J.block<3, 3>(0, 15) = state->_extended_pose->valueLinearAsMat(); J.block<3, 3>(3, 18) = state->_extended_pose->valueLinearAsMat(); Eigen::MatrixXd cov_new(state->_cov.rows()+leftcam_pose_clone->size(), state->_cov.cols()+leftcam_pose_clone->size()); cov_new.setZero(); cov_new.block(0, 0, state->curr_cov_size(), state->curr_cov_size()) = state->_cov; cov_new.block(state->curr_cov_size(), state->curr_cov_size(), leftcam_pose_clone->size(), leftcam_pose_clone->size()) = J*state->_cov.block<21, 21>(0, 0)*J.transpose(); cov_new.block(state->curr_cov_size(), 0, leftcam_pose_clone->size(), state->curr_cov_size()) = J*state->_cov.block(0, 0, 21, state->curr_cov_size()); cov_new.block(0, state->curr_cov_size(), state->curr_cov_size(), leftcam_pose_clone->size()) = cov_new.block(state->curr_cov_size(), 0, leftcam_pose_clone->size(), state->curr_cov_size()).transpose(); state->_cov = 0.5*(cov_new+cov_new.transpose()); assert(checkStateContinuity(state)); } void StateManager::addAnchoredLandmarkInState(std::shared_ptr state, std::shared_ptr anchored_landmark, int lm_id, const Eigen::Matrix3d& cov) { if (state->_anchored_landmarks.find(lm_id) != state->_anchored_landmarks.end()) { std::cout << "[StateManager]: Landmark already in the state, cannot add!" << std::endl; return; } state->_anchored_landmarks[lm_id] = anchored_landmark; StateManager::addVariableIndependent(state, anchored_landmark, cov); assert(StateManager::checkStateContinuity(state)); } void StateManager::margSlidingWindowPose(std::shared_ptr state, double marg_time) { if (state->_sw_camleft_poses.find(marg_time) == state->_sw_camleft_poses.end()) std::cout << "[StateManager]: Marg pose time not exists! Cannot marg!" << std::endl; StateManager::marginalize(state, state->_sw_camleft_poses.at(marg_time)); state->_sw_camleft_poses.erase(marg_time); assert(StateManager::checkStateContinuity(state)); } void StateManager::margSlidingWindowPose(std::shared_ptr state) { double marg_time = state->nextMargTime(); if (marg_time == INFINITY) { std::cout << "[StateManager]: Auto marg pose gives inf time! Cannot marg!" << std::endl; return; } StateManager::margSlidingWindowPose(state, marg_time); } void StateManager::margAnchoredLandmarkInState(std::shared_ptr state, int lm_id) { if (state->_anchored_landmarks.find(lm_id) == state->_anchored_landmarks.end()) { std::cout << "[StateManager]: Landmark id not exists in state! Cannot marg!" << std::endl; return; } StateManager::marginalize(state, state->_anchored_landmarks.at(lm_id)); state->_anchored_landmarks.erase(lm_id); assert(StateManager::checkStateContinuity(state)); } // Acknowledgements: This function is inspired by the realization of OpenVINS // Delayed-Initialization // See https://github.com/rpng/open_vins void StateManager::ekfUpdate(std::shared_ptr state, const std::vector>& var_order, const Eigen::MatrixXd& H, const Eigen::VectorXd& res, const Eigen::MatrixXd& R) { assert(res.rows() == R.rows()); assert(H.rows() == res.rows()); assert(R.rows() == R.cols()); assert(StateManager::checkSubOrder(state, var_order)); int small_idx = 0; std::vector H_idx; for (int i = 0; i < var_order.size(); ++i) { H_idx.push_back(small_idx); small_idx += var_order[i]->size(); } Eigen::MatrixXd PH_T(state->_cov.rows(), H.rows()); PH_T.setZero(); for (const auto& total_var : state->_err_variables) { Eigen::MatrixXd PH_T_i = Eigen::MatrixXd::Zero(total_var->size(), res.rows()); for (int i = 0; i < var_order.size(); ++i) { std::shared_ptr meas_var = var_order[i]; PH_T_i.noalias() += state->_cov.block(total_var->idx(), meas_var->idx(), total_var->size(), meas_var->size())*H.block(0, H_idx[i], H.rows(), meas_var->size()).transpose(); } PH_T.block(total_var->idx(), 0, total_var->size(), res.rows()) = PH_T_i; } Eigen::MatrixXd small_cov = StateManager::getMarginalCov(state, var_order); Eigen::MatrixXd S(R.rows(), R.cols()); S = H*small_cov*H.transpose() + R; Eigen::MatrixXd K = PH_T*S.inverse(); Eigen::MatrixXd cov_tmp = state->_cov; cov_tmp -= K*PH_T.transpose(); state->_cov = 0.5*(cov_tmp + cov_tmp.transpose()); Eigen::VectorXd diags = state->_cov.diagonal(); for (int i = 0; i < diags.rows(); ++i) if (diags(i) < 0.0) { std::cout << "[StateManager]: EKF Update and found negative diag cov elements! " << std::endl; assert(false); } Eigen::VectorXd dx = K*res; StateManager::boxPlus(state, dx); } bool StateManager::checkSubOrder(std::shared_ptr state, const std::vector>& sub_order) { auto isFound = [&state] (const std::shared_ptr& var) { return std::find(state->_err_variables.begin(), state->_err_variables.end(), var) != state->_err_variables.end(); }; for (const auto& item : sub_order) if (!isFound(item)) { std::cout << "[StateManager]: Existing sub order var not in state! " << std::endl; return false; } return true; } int StateManager::calcSubVarSize(const std::vector>& sub_var) { int total_size = 0; for (const auto& item : sub_var) if (item != nullptr) total_size += item->size(); return total_size; } // Acknowledgements: This function is inspired by the realization of OpenVINS // Delayed-Initialization // See https://github.com/rpng/open_vins void StateManager::addVariableDelayedInvertible(std::shared_ptr state, std::shared_ptr var_new, const std::vector>& var_old_order, const Eigen::MatrixXd& H_old, const Eigen::MatrixXd& H_new, const Eigen::VectorXd& res, double noise_iso_meas) { if (std::find(state->_err_variables.begin(), state->_err_variables.end(), var_new) != state->_err_variables.end()) { std::cout << "[StateManager]: New var already in state! Cannot perform add var delayed inv!" << std::endl; return; } assert(StateManager::checkSubOrder(state, var_old_order)); int old_sub_var_size = StateManager::calcSubVarSize(var_old_order); assert(res.rows() == H_old.rows()); assert(res.rows() == H_new.rows()); assert(H_new.rows() == H_new.cols()); assert(H_new.cols() == var_new->size()); assert(H_old.cols() == old_sub_var_size); assert(H_new.determinant() != 0); int small_idx = 0; std::vector H_idx; for (int i = 0; i < var_old_order.size(); ++i) { H_idx.push_back(small_idx); small_idx += var_old_order[i]->size(); } Eigen::MatrixXd PH_T = Eigen::MatrixXd::Zero(state->_cov.rows(), res.rows()); for (int i = 0; i < state->_err_variables.size(); ++i) { Eigen::MatrixXd PH_T_i = Eigen::MatrixXd::Zero(state->_err_variables[i]->size(), res.rows()); for (int j = 0; j < var_old_order.size(); ++j) { std::shared_ptr meas_var = var_old_order[j]; PH_T_i += state->_cov.block(state->_err_variables[i]->idx(), meas_var->idx(), state->_err_variables[i]->size(), meas_var->size())*H_old.block(0, H_idx[j], H_old.rows(), meas_var->size()).transpose(); } PH_T.block(state->_err_variables[i]->idx(), 0, state->_err_variables[i]->size(), res.rows()) = PH_T_i; } Eigen::MatrixXd small_cov = StateManager::getMarginalCov(state, var_old_order); Eigen::MatrixXd S(res.rows(), res.rows()); S = H_old*small_cov*H_old.transpose(); for (int i = 0; i < S.cols(); ++i) S(i, i) += std::pow(noise_iso_meas, 2.0); Eigen::MatrixXd H_new_inv = H_new.inverse(); Eigen::MatrixXd cov_newnew = H_new_inv*S*H_new_inv.transpose(); Eigen::MatrixXd cov_tmp = Eigen::MatrixXd::Zero(state->_cov.rows() + var_new->size(), state->_cov.cols() + var_new->size()); cov_tmp.block(0, 0, state->_cov.rows(), state->_cov.cols()) = state->_cov; cov_tmp.block(state->_cov.rows(), state->_cov.cols(), var_new->size(), var_new->size()) = cov_newnew; cov_tmp.block(0, state->_cov.cols(), state->_cov.rows(), var_new->size()) = -PH_T*H_new_inv.transpose(); cov_tmp.block(state->_cov.rows(), 0, var_new->size(), state->_cov.cols()) = cov_tmp.block(0, state->_cov.cols(), state->_cov.rows(), var_new->size()).transpose(); var_new->set_cov_idx(state->_cov.cols()); state->_err_variables.push_back(var_new); state->_cov = 0.5*(cov_tmp + cov_tmp.transpose()); } // Acknowledgements: This function is inspired by the realization of OpenVINS // Delayed-Initialization // See https://github.com/rpng/open_vins bool StateManager::addVariableDelayed(std::shared_ptr state, std::shared_ptr var_new, const std::vector>& var_old_order, Eigen::MatrixXd& H_old, Eigen::MatrixXd& H_new, Eigen::VectorXd& res, double noise_iso_meas, double chi2_mult_factor, bool do_chi2) { if (std::find(state->_err_variables.begin(), state->_err_variables.end(), var_new) != state->_err_variables.end()) { std::cout << "[StateManager]: New var already in state! Cannot perform add var delayed inv!" << std::endl; return false; } int old_sub_var_size = StateManager::calcSubVarSize(var_old_order); int new_var_size = var_new->size(); assert(StateManager::checkSubOrder(state, var_old_order)); assert(res.rows() == H_old.rows()); assert(res.rows() == H_new.rows()); assert(H_new.cols() == new_var_size); assert(H_old.cols() == old_sub_var_size); if (H_new.rows() <= H_new.cols()) { std::cout << "[StateManager]: H_new rows should be larger than H_new cols!" << std::endl; return false; } Eigen::JacobiRotation tmpG; for (int n = 0; n < H_new.cols(); ++n) for (int m = H_new.rows()-1; m > n; --m) { tmpG.makeGivens(H_new(m-1, n), H_new(m, n)); (H_new.block(m-1, n, 2, H_new.cols()-n)).applyOnTheLeft(0, 1,tmpG.adjoint()); (res.block(m-1, 0, 2, 1)).applyOnTheLeft(0, 1, tmpG.adjoint()); (H_old.block(m-1, 0, 2, H_old.cols())).applyOnTheLeft(0, 1, tmpG.adjoint()); } Eigen::MatrixXd Hxinit = H_old.block(0, 0, new_var_size, H_old.cols()); Eigen::MatrixXd Hfinit = H_new.block(0, 0, new_var_size, new_var_size); Eigen::VectorXd resinit = res.block(0, 0, new_var_size, 1); Eigen::MatrixXd Hup = H_old.block(new_var_size, 0, H_old.rows()-new_var_size, H_old.cols()); Eigen::VectorXd resup = res.block(new_var_size, 0, res.rows()-new_var_size, 1); Eigen::MatrixXd small_cov = StateManager::getMarginalCov(state, var_old_order); Eigen::MatrixXd S = Hup*small_cov*Hup.transpose(); for (int i = 0; i < S.rows(); ++i) S(i, i) += std::pow(noise_iso_meas, 2.0); double chi2 = resup.dot(S.llt().solve(resup)); boost::math::chi_squared chi_squared_dist(res.rows()); double chi2_check = boost::math::quantile(chi_squared_dist, 0.95); if (chi2 > chi2_mult_factor * chi2_check && do_chi2) { std::cout << "[StateManager]: Cannot add variable due to chi2 test failure!" << std::endl; return false; } StateManager::addVariableDelayedInvertible(state, var_new, var_old_order, Hxinit, Hfinit, resinit, noise_iso_meas); if (Hup.rows() > 0) StateManager::ekfUpdate(state, var_old_order, Hup, resup, std::pow(noise_iso_meas, 2.0)*Eigen::MatrixXd::Identity(resup.rows(), resup.rows())); return true; } void StateManager::replaceVarLinear(std::shared_ptr state, const std::shared_ptr target_var, const std::vector>& dependence_order, const Eigen::MatrixXd& H) { bool notFound = true; for (const auto& item : state->_err_variables) if (target_var == item) { notFound = false; break; } if (notFound) { std::cout << "[StateManager]: Target var not in state, cannot linearly replace!" << std::endl; return; } assert(StateManager::checkSubOrder(state, dependence_order)); assert(target_var->size() == H.rows()); int small_idx = 0; std::vector H_idx; for (int i = 0; i < dependence_order.size(); ++i) { H_idx.push_back(small_idx); small_idx += dependence_order[i]->size(); } assert(small_idx == H.cols()); Eigen::MatrixXd PH_T(state->_cov.rows(), H.rows()); PH_T.setZero(); for (const auto& total_var : state->_err_variables) { Eigen::MatrixXd PH_T_i = Eigen::MatrixXd::Zero(total_var->size(), H.rows()); for (int i = 0; i < dependence_order.size(); ++i) { std::shared_ptr meas_var = dependence_order[i]; PH_T_i.noalias() += state->_cov.block(total_var->idx(), meas_var->idx(), total_var->size(), meas_var->size())*H.block(0, H_idx[i], H.rows(), meas_var->size()).transpose(); } PH_T.block(total_var->idx(), 0, total_var->size(), H.rows()) = PH_T_i; } Eigen::MatrixXd small_cov = StateManager::getMarginalCov(state, dependence_order); Eigen::MatrixXd HPH_T = H*small_cov*H.transpose(); state->_cov.block(0, target_var->idx(), state->curr_cov_size(), target_var->size()) = PH_T; state->_cov.block(target_var->idx(), 0, target_var->size(), state->curr_cov_size()) = PH_T.transpose(); state->_cov.block(target_var->idx(), target_var->idx(), target_var->size(), target_var->size()) = HPH_T; } } ================================================ FILE: ingvio_estimator/src/StateManager.h ================================================ /** This File is part of InGVIO, an invariant filter for mono/stereo visual- * inertial-raw GNSS navigation. * * Copyright (C) 2022 Changwu Liu (cwliu529@163.com, * lcw18@mails.tsinghua.edu.cn (valid until 2023)) * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program. If not, see . */ #pragma once #include #include #include #include "State.h" namespace ingvio { class State; class StateManager { public: StateManager() = delete; static bool checkStateContinuity(const std::shared_ptr state); static void propagateStateCov(std::shared_ptr state, const Eigen::Matrix& Phi_imu, const Eigen::Matrix& G_imu, double dt); static Eigen::MatrixXd getFullCov(std::shared_ptr state); static Eigen::MatrixXd getMarginalCov( std::shared_ptr state, const std::vector>& small_variables); static void marginalize(std::shared_ptr state, std::shared_ptr marg); static void addVariableIndependent(std::shared_ptr state, std::shared_ptr new_state, const Eigen::MatrixXd& new_state_cov_block); static void addGNSSVariable(std::shared_ptr state, const State::GNSSType& gtype, double value, double cov); static void margGNSSVariable(std::shared_ptr state, const State::GNSSType& gtype); static void boxPlus(std::shared_ptr state, const Eigen::VectorXd& dx); static void augmentSlidingWindowPose(std::shared_ptr state); static void addAnchoredLandmarkInState( std::shared_ptr state, std::shared_ptr anchored_landmark, int lm_id, const Eigen::Matrix3d& cov); static void margSlidingWindowPose(std::shared_ptr state, double marg_time); static void margSlidingWindowPose(std::shared_ptr state); static void margAnchoredLandmarkInState(std::shared_ptr state, int lm_id); // Acknowledgements: This function is inspired by the realization of OpenVINS // Delayed-Initialization // See https://github.com/rpng/open_vins static void ekfUpdate(std::shared_ptr state, const std::vector>& var_order, const Eigen::MatrixXd& H, const Eigen::VectorXd& res, const Eigen::MatrixXd& R); static bool checkSubOrder(std::shared_ptr state, const std::vector>& sub_order); static int calcSubVarSize(const std::vector>& sub_var); // Acknowledgements: This function is inspired by the realization of OpenVINS // Delayed-Initialization // See https://github.com/rpng/open_vins static void addVariableDelayedInvertible( std::shared_ptr state, std::shared_ptr var_new, const std::vector>& var_old_order, const Eigen::MatrixXd& H_old, const Eigen::MatrixXd& H_new, const Eigen::VectorXd& res, double noise_iso_meas); // Acknowledgements: This function is inspired by the realization of OpenVINS // Delayed-Initialization // See https://github.com/rpng/open_vins static bool addVariableDelayed( std::shared_ptr state, std::shared_ptr var_new, const std::vector>& var_old_order, Eigen::MatrixXd& H_old, Eigen::MatrixXd& H_new, Eigen::VectorXd& res, double noise_iso_meas, double chi2_mult_factor, bool do_chi2 = true); static void replaceVarLinear( std::shared_ptr state, const std::shared_ptr target_var, const std::vector>& dependence_order, const Eigen::MatrixXd& H); }; } ================================================ FILE: ingvio_estimator/src/SwMargUpdate.cpp ================================================ /** This File is part of InGVIO, an invariant filter for mono/stereo visual- * inertial-raw GNSS navigation. * * Copyright (C) 2022 Changwu Liu (cwliu529@163.com, * lcw18@mails.tsinghua.edu.cn (valid until 2023)) * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program. If not, see . */ #include #include #include #include #include "IngvioParams.h" #include "StateManager.h" #include "MapServer.h" #include "MapServerManager.h" #include "Triangulator.h" #include "SwMargUpdate.h" #include "TicToc.h" namespace ingvio { void SwMargUpdate::updateStateMono(std::shared_ptr state, std::shared_ptr map_server, std::shared_ptr tri) { double marg_time = state->nextMargTime(); if (marg_time == INFINITY) return; std::vector selected_timestamps; this->selectSwTimestamps(state->_sw_camleft_poses, marg_time, selected_timestamps); std::vector> sw_var_order; std::map, int> sw_index_map; std::vector> sw_var_type; this->generateSwVarOrder(state, selected_timestamps, sw_var_order, sw_index_map, sw_var_type); std::vector update_ids; for (const auto& item : *map_server) { const auto& feat_info_ptr = item.second; if (feat_info_ptr->_ftype != FeatureInfo::FeatureType::MSCKF) continue; bool flag = false; const auto& mono_obs = item.second->_mono_obs; for (const double& ts : selected_timestamps) if (mono_obs.find(ts) == mono_obs.end()) { flag = true; break; } if (flag) continue; if (FeatureInfoManager::triangulateFeatureInfoMono(feat_info_ptr, tri, state)) update_ids.push_back(item.first); } if (update_ids.size() == 0) return; const int max_possible_cols = 6*state->_sw_camleft_poses.size(); const int max_possible_rows = update_ids.size()*(2*selected_timestamps.size()-3); Eigen::VectorXd res_large(max_possible_rows); Eigen::MatrixXd H_large(max_possible_rows, max_possible_cols); res_large.setZero(); H_large.setZero(); int row_cnt = 0; int col_cnt = 6*sw_var_type.size(); int feats_cnt = 0; for (int i = 0; i < update_ids.size(); ++i) { Eigen::VectorXd res_block; Eigen::MatrixXd H_block; Eigen::MatrixXd H_anchor_block; const auto& anchor_pose_ptr = this->calcResJacobianSingleFeatSelectedMonoObs( map_server->at(update_ids[i]), state->_sw_camleft_poses, sw_var_order, sw_index_map, selected_timestamps, res_block, H_block, H_anchor_block); bool flag = false; if (sw_index_map.find(anchor_pose_ptr) == sw_index_map.end()) { flag = true; sw_var_order.push_back(anchor_pose_ptr); sw_var_type.push_back(anchor_pose_ptr); sw_index_map[anchor_pose_ptr] = col_cnt; col_cnt += anchor_pose_ptr->size(); H_block.conservativeResize(H_block.rows(), H_block.cols() + 6); } H_block.block(0, sw_index_map.at(anchor_pose_ptr), H_block.rows(), 6) = H_anchor_block; if (!testChiSquared(state, res_block, H_block, sw_var_type, this->_noise, selected_timestamps.size()-1)) { if (flag) { sw_var_order.pop_back(); sw_var_type.pop_back(); sw_index_map.erase(anchor_pose_ptr); col_cnt -= anchor_pose_ptr->size(); } continue; } H_large.block(row_cnt, 0, H_block.rows(), col_cnt) = H_block; res_large.block(row_cnt, 0, res_block.rows(), 1) = res_block; row_cnt += res_block.rows(); ++feats_cnt; } if (row_cnt < max_possible_rows || col_cnt < max_possible_cols) { H_large.conservativeResize(row_cnt, col_cnt); res_large.conservativeResize(row_cnt, 1); } Eigen::MatrixXd H_thin; Eigen::VectorXd res_thin; if (H_large.rows() > H_large.cols()) { Eigen::SparseMatrix H_sparse = H_large.sparseView(); Eigen::SPQR> spqr_helper; spqr_helper.setSPQROrdering(SPQR_ORDERING_NATURAL); spqr_helper.compute(H_sparse); Eigen::MatrixXd H_temp; Eigen::VectorXd r_temp; (spqr_helper.matrixQ().transpose()*H_large).evalTo(H_temp); (spqr_helper.matrixQ().transpose()*res_large).evalTo(r_temp); H_thin = H_temp.topRows(col_cnt); res_thin = r_temp.head(col_cnt); } else { H_thin = H_large; res_thin = res_large; } /* std::cout << "[SwMargUpdate]: Num of mono feats used in selected pose = " << feats_cnt << std::endl; */ StateManager::ekfUpdate(state, sw_var_type, H_thin, res_thin, std::pow(this->_noise, 2)*Eigen::MatrixXd::Identity(res_thin.rows(), res_thin.rows())); } void SwMargUpdate::cleanMonoObsAtMargTime(std::shared_ptr state, std::shared_ptr map_server) { double marg_time = state->nextMargTime(); if (marg_time == INFINITY) return; std::vector all_ids; for (const auto& item : *map_server) all_ids.push_back(item.first); std::vector ids_to_clean; for (int i = 0; i < all_ids.size(); ++i) { if (map_server->at(all_ids[i])->_mono_obs.find(marg_time) != map_server->at(all_ids[i])->_mono_obs.end()) map_server->at(all_ids[i])->_mono_obs.erase(marg_time); if (map_server->at(all_ids[i])->_mono_obs.size() == 0) ids_to_clean.push_back(all_ids[i]); } for (int id : ids_to_clean) map_server->erase(id); } void SwMargUpdate::updateStateStereo(std::shared_ptr state, std::shared_ptr map_server, std::shared_ptr tri) { double marg_time = state->nextMargTime(); if (marg_time == INFINITY) return; std::vector selected_timestamps; this->selectSwTimestamps(state->_sw_camleft_poses, marg_time, selected_timestamps); std::vector> sw_var_order; std::map, int> sw_index_map; std::vector> sw_var_type; this->generateSwVarOrder(state, selected_timestamps, sw_var_order, sw_index_map, sw_var_type); std::vector update_ids; for (const auto& item : *map_server) { const auto& feat_info_ptr = item.second; if (feat_info_ptr->_ftype != FeatureInfo::FeatureType::MSCKF) continue; bool flag = false; const auto& stereo_obs = item.second->_stereo_obs; for (const double& ts : selected_timestamps) if (stereo_obs.find(ts) == stereo_obs.end()) { flag = true; break; } if (flag) continue; if (FeatureInfoManager::triangulateFeatureInfoStereo(feat_info_ptr, tri, state)) update_ids.push_back(item.first); } if (update_ids.size() == 0) return; const int max_possible_cols = 6*state->_sw_camleft_poses.size(); const int max_possible_rows = update_ids.size()*(4*selected_timestamps.size()-3); Eigen::VectorXd res_large(max_possible_rows); Eigen::MatrixXd H_large(max_possible_rows, max_possible_cols); res_large.setZero(); H_large.setZero(); int row_cnt = 0; int col_cnt = 6*sw_var_type.size(); int feats_cnt = 0; for (int i = 0; i < update_ids.size(); ++i) { Eigen::VectorXd res_block; Eigen::MatrixXd H_block; Eigen::MatrixXd H_anchor_block; const auto& anchor_pose_ptr = this->calcResJacobianSingleFeatSelectedStereoObs( map_server->at(update_ids[i]), state->_sw_camleft_poses, sw_var_order, sw_index_map, selected_timestamps, state->_state_params._T_cl2cr, res_block, H_block, H_anchor_block); bool flag = false; if (sw_index_map.find(anchor_pose_ptr) == sw_index_map.end()) { flag = true; sw_var_order.push_back(anchor_pose_ptr); sw_var_type.push_back(anchor_pose_ptr); sw_index_map[anchor_pose_ptr] = col_cnt; col_cnt += anchor_pose_ptr->size(); H_block.conservativeResize(H_block.rows(), H_block.cols() + 6); } H_block.block(0, sw_index_map.at(anchor_pose_ptr), H_block.rows(), 6) = H_anchor_block; if (!testChiSquared(state, res_block, H_block, sw_var_type, this->_noise, selected_timestamps.size()-1)) { if (flag) { sw_var_order.pop_back(); sw_var_type.pop_back(); sw_index_map.erase(anchor_pose_ptr); col_cnt -= anchor_pose_ptr->size(); } continue; } H_large.block(row_cnt, 0, H_block.rows(), col_cnt) = H_block; res_large.block(row_cnt, 0, res_block.rows(), 1) = res_block; row_cnt += res_block.rows(); ++feats_cnt; } if (row_cnt < max_possible_rows || col_cnt < max_possible_cols) { H_large.conservativeResize(row_cnt, col_cnt); res_large.conservativeResize(row_cnt, 1); } Eigen::MatrixXd H_thin; Eigen::VectorXd res_thin; if (H_large.rows() > H_large.cols()) { Eigen::SparseMatrix H_sparse = H_large.sparseView(); Eigen::SPQR> spqr_helper; spqr_helper.setSPQROrdering(SPQR_ORDERING_NATURAL); spqr_helper.compute(H_sparse); Eigen::MatrixXd H_temp; Eigen::VectorXd r_temp; (spqr_helper.matrixQ().transpose()*H_large).evalTo(H_temp); (spqr_helper.matrixQ().transpose()*res_large).evalTo(r_temp); H_thin = H_temp.topRows(col_cnt); res_thin = r_temp.head(col_cnt); } else { H_thin = H_large; res_thin = res_large; } /* std::cout << "[SwMargUpdate]: Num of stereo feats used in selected pose = " << feats_cnt << std::endl; */ StateManager::ekfUpdate(state, sw_var_type, H_thin, res_thin, std::pow(this->_noise, 2)*Eigen::MatrixXd::Identity(res_thin.rows(), res_thin.rows())); } void SwMargUpdate::changeMSCKFAnchor(std::shared_ptr state, std::shared_ptr map_server) { double marg_time = state->nextMargTime(); if (marg_time == INFINITY || state->_sw_camleft_poses.find(marg_time) == state->_sw_camleft_poses.end()) return; const std::shared_ptr old_anchor = state->_sw_camleft_poses.at(marg_time); const double latest_sw = state->_sw_camleft_poses.rbegin()->first; const std::shared_ptr new_anchor = state->_sw_camleft_poses.at(latest_sw); std::vector ids_to_marg; for (auto& item : *map_server) { if (item.second->_ftype != FeatureInfo::FeatureType::MSCKF) continue; if (item.second->_landmark->getAnchoredPose() == old_anchor) { if (item.second->_isTri) { const Eigen::Vector3d pf = item.second->_landmark->valuePosXyz(); const Eigen::Vector3d body = new_anchor->valueLinearAsMat().transpose()*(pf - new_anchor->valueTrans()); if (body.z() <= 0) { ids_to_marg.push_back(item.first); continue; } item.second->_landmark->resetAnchoredPose(new_anchor, true); } else ids_to_marg.push_back(item.first); } } for (const int& id: ids_to_marg) map_server->erase(id); } void SwMargUpdate::margSwPose(std::shared_ptr state) { double marg_time = state->nextMargTime(); if (marg_time == INFINITY) return; StateManager::margSlidingWindowPose(state, marg_time); } void SwMargUpdate::cleanStereoObsAtMargTime(std::shared_ptr state, std::shared_ptr map_server) { double marg_time = state->nextMargTime(); if (marg_time == INFINITY) return; std::vector all_ids; for (const auto& item : *map_server) all_ids.push_back(item.first); std::vector ids_to_clean; for (int i = 0; i < all_ids.size(); ++i) { if (map_server->at(all_ids[i])->_stereo_obs.find(marg_time) != map_server->at(all_ids[i])->_stereo_obs.end()) map_server->at(all_ids[i])->_stereo_obs.erase(marg_time); if (map_server->at(all_ids[i])->_stereo_obs.size() == 0) ids_to_clean.push_back(all_ids[i]); } for (int id : ids_to_clean) map_server->erase(id); } void SwMargUpdate::generateSwVarOrder(const std::shared_ptr state, const std::vector& selected_timestamps, std::vector>& sw_var_order, std::map, int>& sw_index, std::vector>& sw_var_type) { sw_var_order.clear(); sw_index.clear(); sw_var_type.clear(); int cnt = 0; const auto& sw_poses = state->_sw_camleft_poses; for (const double& ts : selected_timestamps) { if (sw_poses.find(ts) == sw_poses.end()) { std::cout << "[SwMargUpdate]: selected timestamp not in sw!" << std::endl; std::exit(EXIT_FAILURE); } sw_var_order.push_back(sw_poses.at(ts)); sw_var_type.push_back(sw_poses.at(ts)); sw_index[sw_poses.at(ts)] = 6*cnt; ++cnt; } } void SwMargUpdate::selectSwTimestamps( const std::map>& sw_poses, const double& marg_time, std::vector& selected_timestamps) { selected_timestamps.clear(); if (marg_time == INFINITY || sw_poses.find(marg_time) == sw_poses.end()) return; int cnt = 1; selected_timestamps.push_back(marg_time); for (const auto& item : sw_poses) { if (item.first <= marg_time) continue; if (cnt % _frame_select_interval == 0) selected_timestamps.push_back(item.first); ++cnt; } } std::shared_ptr SwMargUpdate::calcResJacobianSingleFeatSelectedMonoObs( const std::shared_ptr feature_info, const std::map>& sw_poses, const std::vector>& sw_var_order, const std::map, int>& sw_index_map, const std::vector& selected_timestamps, Eigen::VectorXd& res_block, Eigen::MatrixXd& H_block, Eigen::MatrixXd& H_anchor_block) { const int num_of_cols = 6*sw_var_order.size(); const int num_of_rows = 2*selected_timestamps.size(); Eigen::VectorXd res_block_tmp = Eigen::VectorXd::Zero(num_of_rows); Eigen::MatrixXd H_block_tmp = Eigen::MatrixXd::Zero(num_of_rows, num_of_cols); Eigen::MatrixXd H_anchor_block_tmp = Eigen::MatrixXd::Zero(num_of_rows, 6); Eigen::MatrixXd Hf_block_tmp = Eigen::MatrixXd::Zero(num_of_rows, 3); const Eigen::Vector3d pf_w = feature_info->_landmark->valuePosXyz(); const std::shared_ptr pose_anchor_ptr = feature_info->_landmark->getAnchoredPose(); int row_cnt = 0; for (const auto& time_of_obs : selected_timestamps) { if (sw_poses.find(time_of_obs) == sw_poses.end() || feature_info->_mono_obs.find(time_of_obs) == feature_info->_mono_obs.end()) continue; const std::shared_ptr& mono_obs_ptr = feature_info->_mono_obs.at(time_of_obs); const std::shared_ptr& pose_obs_ptr = sw_poses.at(time_of_obs); const Eigen::Matrix3d R_cm2w = pose_obs_ptr->valueLinearAsMat(); const Eigen::Vector3d p_cm = pose_obs_ptr->valueTrans(); const Eigen::Vector3d pf_cm = R_cm2w.transpose()*(pf_w-p_cm); Eigen::Matrix H_proj = Eigen::Matrix::Zero(); H_proj(0, 0) = 1.0/pf_cm.z(); H_proj(0, 2) = -pf_cm.x()/std::pow(pf_cm.z(), 2); H_proj(1, 1) = 1.0/pf_cm.z(); H_proj(1, 2) = -pf_cm.y()/std::pow(pf_cm.z(), 2); Eigen::MatrixXd H_pf2x = Eigen::MatrixXd::Zero(3, num_of_cols); Eigen::Matrix H_pf2anchor = Eigen::Matrix::Zero(); if (pose_obs_ptr != pose_anchor_ptr) { H_pf2x.block<3, 3>(0, sw_index_map.at(pose_obs_ptr)) = R_cm2w.transpose()*skew(pf_w); H_pf2anchor.block<3, 3>(0, 0) = -H_pf2x.block<3, 3>(0, sw_index_map.at(pose_obs_ptr)); } H_pf2x.block<3, 3>(0, sw_index_map.at(pose_obs_ptr)+3) = -R_cm2w.transpose(); Eigen::Matrix3d H_pf2pf = R_cm2w.transpose(); if (H_proj.hasNaN() || H_pf2x.hasNaN()) continue; H_block_tmp.block(row_cnt, 0, 2, num_of_cols) = H_proj*H_pf2x; H_anchor_block_tmp.block(row_cnt, 0, 2, 6) = H_proj*H_pf2anchor; Hf_block_tmp.block(row_cnt, 0, 2, 3) = H_proj*H_pf2pf; res_block_tmp.block(row_cnt, 0, 2, 1) = mono_obs_ptr->asVec()-Eigen::Vector2d(pf_cm.x()/pf_cm.z(), pf_cm.y()/pf_cm.z()); row_cnt += 2; } if (row_cnt < res_block.rows()) { res_block_tmp.conservativeResize(row_cnt, 1); H_block_tmp.conservativeResize(row_cnt, num_of_cols); H_anchor_block_tmp.conservativeResize(row_cnt, 6); Hf_block_tmp.conservativeResize(row_cnt, 3); } if (row_cnt <= 3) std::cout << "[SwMargUpdate]: Warning! in calc one feat selected jacobian, num of rows <= 3 !" << std::endl; Eigen::JacobiSVD svd_helper(Hf_block_tmp, Eigen::ComputeFullU | Eigen::ComputeThinV); Eigen::MatrixXd V = svd_helper.matrixU().rightCols(row_cnt-3); H_block = V.transpose()*H_block_tmp; res_block = V.transpose()*res_block_tmp; H_anchor_block = V.transpose()*H_anchor_block_tmp; return pose_anchor_ptr; } std::shared_ptr SwMargUpdate::calcResJacobianSingleFeatSelectedStereoObs( const std::shared_ptr feature_info, const std::map>& sw_poses, const std::vector>& sw_var_order, const std::map, int>& sw_index_map, const std::vector& selected_timestamps, const Eigen::Isometry3d& T_cl2cr, Eigen::VectorXd& res_block, Eigen::MatrixXd& H_block, Eigen::MatrixXd& H_anchor_block) { const Eigen::Matrix3d& R_cl2cr = T_cl2cr.linear(); const int num_of_cols = 6*sw_var_order.size(); const int num_of_rows = 4*selected_timestamps.size(); Eigen::VectorXd res_block_tmp = Eigen::VectorXd::Zero(num_of_rows); Eigen::MatrixXd H_block_tmp = Eigen::MatrixXd::Zero(num_of_rows, num_of_cols); Eigen::MatrixXd H_anchor_block_tmp = Eigen::MatrixXd::Zero(num_of_rows, 6); Eigen::MatrixXd Hf_block_tmp = Eigen::MatrixXd::Zero(num_of_rows, 3); const Eigen::Vector3d pf_w = feature_info->_landmark->valuePosXyz(); const std::shared_ptr pose_anchor_ptr = feature_info->_landmark->getAnchoredPose(); int row_cnt = 0; for (const auto& time_of_obs : selected_timestamps) { if (sw_poses.find(time_of_obs) == sw_poses.end() || feature_info->_stereo_obs.find(time_of_obs) == feature_info->_stereo_obs.end()) continue; const std::shared_ptr& stereo_obs_ptr = feature_info->_stereo_obs.at(time_of_obs); const std::shared_ptr& pose_obs_ptr = sw_poses.at(time_of_obs); const Eigen::Matrix3d R_cm2w = pose_obs_ptr->valueLinearAsMat(); const Eigen::Vector3d p_cm = pose_obs_ptr->valueTrans(); const Eigen::Vector3d pf_cm = R_cm2w.transpose()*(pf_w-p_cm); Eigen::Matrix H_proj = Eigen::Matrix::Zero(); H_proj(0, 0) = 1.0/pf_cm.z(); H_proj(0, 2) = -pf_cm.x()/std::pow(pf_cm.z(), 2); H_proj(1, 1) = 1.0/pf_cm.z(); H_proj(1, 2) = -pf_cm.y()/std::pow(pf_cm.z(), 2); const Eigen::Vector3d pf_cm_r = T_cl2cr*pf_cm; Eigen::Matrix H_proj_r = Eigen::Matrix::Zero(); H_proj_r(0, 0) = 1.0/pf_cm_r.z(); H_proj_r(0, 2) = -pf_cm_r.x()/std::pow(pf_cm_r.z(), 2); H_proj_r(1, 1) = 1.0/pf_cm_r.z(); H_proj_r(1, 2) = -pf_cm_r.y()/std::pow(pf_cm_r.z(), 2); Eigen::MatrixXd H_pf2x = Eigen::MatrixXd::Zero(3, num_of_cols); Eigen::Matrix H_pf2anchor = Eigen::Matrix::Zero(); if (pose_obs_ptr != pose_anchor_ptr) { H_pf2x.block<3, 3>(0, sw_index_map.at(pose_obs_ptr)) = R_cm2w.transpose()*skew(pf_w); H_pf2anchor.block<3, 3>(0, 0) = -H_pf2x.block<3, 3>(0, sw_index_map.at(pose_obs_ptr)); } H_pf2x.block<3, 3>(0, sw_index_map.at(pose_obs_ptr)+3) = -R_cm2w.transpose(); Eigen::Matrix3d H_pf2pf = R_cm2w.transpose(); Eigen::MatrixXd H_pf2x_r = R_cl2cr*H_pf2x; Eigen::Matrix H_pf2anchor_r = R_cl2cr*H_pf2anchor; Eigen::MatrixXd H_pf2pf_r = R_cl2cr*H_pf2pf; if (H_proj.hasNaN() || H_pf2x.hasNaN()) continue; H_block_tmp.block(row_cnt, 0, 2, num_of_cols) = H_proj*H_pf2x; H_anchor_block_tmp.block(row_cnt, 0, 2, 6) = H_proj*H_pf2anchor; Hf_block_tmp.block(row_cnt, 0, 2, 3) = H_proj*H_pf2pf; H_block_tmp.block(row_cnt+2, 0, 2, num_of_cols) = H_proj_r*H_pf2x_r; H_anchor_block_tmp.block(row_cnt+2, 0, 2, 6) = H_proj_r*H_pf2anchor_r; Hf_block_tmp.block(row_cnt+2, 0, 2, 3) = H_proj_r*H_pf2pf_r; res_block_tmp.block(row_cnt, 0, 4, 1) = stereo_obs_ptr->asVec()-Eigen::Vector4d(pf_cm.x()/pf_cm.z(), pf_cm.y()/pf_cm.z(), pf_cm_r.x()/pf_cm_r.z(), pf_cm_r.y()/pf_cm_r.z()); row_cnt += 4; } if (row_cnt < res_block.rows()) { res_block_tmp.conservativeResize(row_cnt, 1); H_block_tmp.conservativeResize(row_cnt, num_of_cols); H_anchor_block_tmp.conservativeResize(row_cnt, 6); Hf_block_tmp.conservativeResize(row_cnt, 3); } if (row_cnt <= 3) std::cout << "[SwMargUpdate]: Warning! in calc one feat selected jacobian, num of rows <= 3 !" << std::endl; Eigen::JacobiSVD svd_helper(Hf_block_tmp, Eigen::ComputeFullU | Eigen::ComputeThinV); Eigen::MatrixXd V = svd_helper.matrixU().rightCols(row_cnt-3); H_block = V.transpose()*H_block_tmp; H_anchor_block = V.transpose()*H_anchor_block_tmp; res_block = V.transpose()*res_block_tmp; return pose_anchor_ptr; } void SwMargUpdate::removeMonoMSCKFinMargPose(std::shared_ptr state, std::shared_ptr map_server) { const double marg_time = state->nextMargTime(); if (marg_time == INFINITY) return; std::vector ids_to_marg; for (const auto& item : *map_server) if (item.second->_mono_obs.find(marg_time) != item.second->_mono_obs.end() && item.second->_ftype == FeatureInfo::FeatureType::MSCKF) ids_to_marg.push_back(item.first); for (const int& id : ids_to_marg) map_server->erase(id); } void SwMargUpdate::removeStereoMSCKFinMargPose(std::shared_ptr state, std::shared_ptr map_server) { const double marg_time = state->nextMargTime(); if (marg_time == INFINITY) return; std::vector ids_to_marg; for (const auto& item : *map_server) if (item.second->_stereo_obs.find(marg_time) != item.second->_stereo_obs.end() && item.second->_ftype == FeatureInfo::FeatureType::MSCKF) ids_to_marg.push_back(item.first); for (const int& id : ids_to_marg) map_server->erase(id); } } ================================================ FILE: ingvio_estimator/src/SwMargUpdate.h ================================================ /** This File is part of InGVIO, an invariant filter for mono/stereo visual- * inertial-raw GNSS navigation. * * Copyright (C) 2022 Changwu Liu (cwliu529@163.com, * lcw18@mails.tsinghua.edu.cn (valid until 2023)) * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program. If not, see . */ #pragma once #include "MapServer.h" #include "Update.h" namespace ingvio { class Triangulator; class IngvioParams; class SwMargUpdate : public UpdateBase { public: using UpdateBase::UpdateBase; SwMargUpdate(const IngvioParams& filter_params) : UpdateBase(filter_params._chi2_max_dof, filter_params._chi2_thres), _noise(filter_params._visual_noise), _frame_select_interval(filter_params._frame_select_interval) {} SwMargUpdate(const SwMargUpdate&) = delete; SwMargUpdate operator=(const SwMargUpdate&) = delete; virtual ~SwMargUpdate() {} void updateStateMono(std::shared_ptr state, std::shared_ptr map_server, std::shared_ptr tri); void cleanMonoObsAtMargTime(std::shared_ptr state, std::shared_ptr map_server); void updateStateStereo(std::shared_ptr state, std::shared_ptr map_server, std::shared_ptr tri); void cleanStereoObsAtMargTime(std::shared_ptr state, std::shared_ptr map_server); void changeMSCKFAnchor(std::shared_ptr state, std::shared_ptr map_server); void margSwPose(std::shared_ptr state); void removeMonoMSCKFinMargPose(std::shared_ptr state, std::shared_ptr map_server); void removeStereoMSCKFinMargPose(std::shared_ptr state, std::shared_ptr map_server); protected: double _noise; int _frame_select_interval; void generateSwVarOrder(const std::shared_ptr state, const std::vector& selected_timestamps, std::vector>& sw_var_order, std::map, int>& sw_index, std::vector>& sw_var_type); std::shared_ptr calcResJacobianSingleFeatSelectedMonoObs( const std::shared_ptr feature_info, const std::map>& sw_poses, const std::vector>& sw_var_order, const std::map, int>& sw_index_map, const std::vector& selected_timestamps, Eigen::VectorXd& res_block, Eigen::MatrixXd& H_block, Eigen::MatrixXd& H_anchor_block); std::shared_ptr calcResJacobianSingleFeatSelectedStereoObs( const std::shared_ptr feature_info, const std::map>& sw_poses, const std::vector>& sw_var_order, const std::map, int>& sw_index_map, const std::vector& selected_timestamps, const Eigen::Isometry3d& T_cl2cr, Eigen::VectorXd& res_block, Eigen::MatrixXd& H_block, Eigen::MatrixXd& H_anchor_block); void selectSwTimestamps(const std::map>& sw_poses, const double& marg_time, std::vector& selected_timestamps); }; } ================================================ FILE: ingvio_estimator/src/TicToc.h ================================================ /** This File is part of InGVIO, an invariant filter for mono/stereo visual- * inertial-raw GNSS navigation. * * Copyright (C) 2022 Changwu Liu (cwliu529@163.com, * lcw18@mails.tsinghua.edu.cn (valid until 2023)) * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program. If not, see . */ // The TicToc Class is inspired by such realization in GVINS. // see https://github.com/HKUST-Aerial-Robotics/GVINS #pragma once #include #include #include namespace ingvio { class TicToc { public: TicToc() { tic(); } void tic() { start = std::chrono::system_clock::now(); } double toc() { end = std::chrono::system_clock::now(); std::chrono::duration elapsed_seconds = end - start; return elapsed_seconds.count() * 1000; } private: std::chrono::time_point start, end; }; } ================================================ FILE: ingvio_estimator/src/Triangulator.cpp ================================================ /** This File is part of InGVIO, an invariant filter for mono/stereo visual- * inertial-raw GNSS navigation. * * Copyright (C) 2022 Changwu Liu (cwliu529@163.com, * lcw18@mails.tsinghua.edu.cn (valid until 2023)) * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program. If not, see . */ #include #include "Triangulator.h" #include "MapServer.h" #include "PoseState.h" namespace ingvio { double Triangulator::findLongestTrans( const std::map, std::less>& sw_poses, const std::map>& mobs, double& max_length) const { assert(sw_poses.size() >= 2); const auto& item_last = sw_poses.rbegin(); const double timestamp_last = item_last->first; const std::shared_ptr pose_last = item_last->second; Eigen::Vector3d unit_feat_last; unit_feat_last.head<2>() = mobs.at(timestamp_last)->asVec(); unit_feat_last.z() = 1.0; unit_feat_last.normalize(); const Eigen::Vector3d unit_feat_w = pose_last->valueLinearAsMat()*unit_feat_last; const Eigen::Matrix3d proj = Eigen::Matrix3d::Identity()-unit_feat_w*unit_feat_w.transpose(); max_length = -INFINITY; double max_timestamp = timestamp_last; for (const auto& item : sw_poses) { if (item.first == timestamp_last) continue; const Eigen::Vector3d trans_vec = proj*(item.second->valueTrans()-pose_last->valueTrans()); double trans = std::abs(trans_vec.norm()); if (trans > max_length) { max_length = trans; max_timestamp = item.first; } } return max_timestamp; } void Triangulator::calcRelaSwPose( const std::map, std::less>& sw_poses, std::map, std::less>& rel_sw_poses) const { const std::shared_ptr pose_last = sw_poses.rbegin()->second; const double timestamp_last = sw_poses.rbegin()->first; rel_sw_poses.clear(); for (const auto& item : sw_poses) { rel_sw_poses[item.first] = std::make_shared(); if (item.first == timestamp_last) { rel_sw_poses[timestamp_last]->setIdentity(); continue; } rel_sw_poses[item.first]->setValueByIso(item.second->copyValueAsIso().inverse()*pose_last->copyValueAsIso()); } } double Triangulator::initDepth(const Eigen::Vector2d& m1, const Eigen::Vector2d& m2, const std::shared_ptr T12) const { Eigen::Vector3d tilde_m1 = T12->valueLinearAsMat()*Eigen::Vector3d(m1.x(), m1.y(), 1.0); Eigen::Vector3d t = T12->valueTrans(); Eigen::Vector2d A, b; A(0, 0) = tilde_m1.x() - m2.x()*tilde_m1.z(); A(1, 0) = tilde_m1.y() - m2.y()*tilde_m1.z(); b(0, 0) = m2.x()*t.z()-t.x(); b(1, 0) = m2.y()*t.z()-t.y(); return (A.transpose()*A).inverse()*A.transpose()*b; } double Triangulator::calcUnitCost(const Eigen::Vector2d& meas, const std::shared_ptr rel_pose, const Eigen::Vector3d& solution) const { Eigen::Vector3d pf0; pf0.z() = 1.0/solution.z(); pf0.x() = solution.x()*pf0.z(); pf0.y() = solution.y()*pf0.z(); Eigen::Vector3d pf = rel_pose->valueLinearAsMat()*pf0 + rel_pose->valueTrans(); Eigen::Vector2d meas_hat; meas_hat.x() = pf.x()/pf.z(); meas_hat.y() = pf.y()/pf.z(); return (meas-meas_hat).squaredNorm(); } double Triangulator::calcTotalCost(const std::map>& mobs, const std::map>& rel_poses, const Eigen::Vector3d& solution) const { double total_cost = 0.0; for (const auto& item : mobs) total_cost += calcUnitCost(item.second->asVec(), rel_poses.at(item.first), solution); return total_cost; } void Triangulator::calcResJacobian(const Eigen::Vector2d& meas, const std::shared_ptr rel_pose, const Eigen::Vector3d& solution, Eigen::Vector2d& res, Eigen::Matrix& J, double& w) const { Eigen::Vector3d tilde_pf_hat = rel_pose->valueLinearAsMat()*Eigen::Vector3d(solution.x(), solution.y(), 1.0)+rel_pose->valueTrans()*solution.z(); Eigen::Vector2d meas_hat; meas_hat.x() = tilde_pf_hat.x()/tilde_pf_hat.z(); meas_hat.y() = tilde_pf_hat.y()/tilde_pf_hat.z(); res = meas_hat - meas; Eigen::Matrix W; W.setZero(); W(0, 0) = 1.0/tilde_pf_hat.z(); W(0, 2) = -tilde_pf_hat.x()/std::pow(tilde_pf_hat.z(), 2); W(1, 1) = 1.0/tilde_pf_hat.z(); W(1, 2) = -tilde_pf_hat.y()/std::pow(tilde_pf_hat.z(), 2); Eigen::Matrix U; U.rightCols<1>() = rel_pose->valueTrans(); U.leftCols<2>() = rel_pose->valueLinearAsMat().leftCols<2>(); J = W*U; double e = res.norm(); if (e <= _huber_epsilon) w = 1.0; else w = std::sqrt(2.0*_huber_epsilon/e); } bool Triangulator::triangulateMonoObs( const std::map>& mono_obs, const std::map, std::less>& sw_poses_raw, Eigen::Vector3d& pf) const { std::map> mobs; std::map, std::less> sw_poses; this->filterCommonTimestamp(sw_poses_raw, mono_obs, sw_poses, mobs); if (mobs.size() <= 4) { pf.setZero(); return false; } double max_trans; double max_timestamp = findLongestTrans(sw_poses, mobs, max_trans); if (max_trans < _trans_thres) return false; std::map> rel_sw_poses; calcRelaSwPose(sw_poses, rel_sw_poses); Eigen::Vector3d solution; const double timestamp_last = rel_sw_poses.rbegin()->first; const std::shared_ptr pose_last = sw_poses.rbegin()->second; solution.x() = mobs[timestamp_last]->_u0; solution.y() = mobs[timestamp_last]->_v0; solution.z() = 1.0/initDepth(mobs[timestamp_last]->asVec(), mobs[max_timestamp]->asVec(), rel_sw_poses[max_timestamp]); /* std::cout << "init pose = " << (pose0->copyValueAsIso().inverse()*Eigen::Vector3d(solution.x()/solution.z(), solution.y()/solution.z(), 1.0/solution.z())).transpose() << std::endl; */ double total_cost = calcTotalCost(mobs, rel_sw_poses, solution); double lambda = _init_damping; int inner_loop_cnt = 0; int outer_loop_cnt = 0; bool is_cost_reduced = false; double delta_norm = INFINITY; do { Eigen::Matrix3d A = Eigen::Matrix3d::Zero(); Eigen::Vector3d b = Eigen::Vector3d::Zero(); for (const auto& item : rel_sw_poses) { Eigen::Matrix J; Eigen::Vector2d res; double w; calcResJacobian(mobs.at(item.first)->asVec(), item.second, solution, res, J, w); if (w == 1.0) { A += J.transpose()*J; b -= J.transpose()*res; } else { A += std::pow(w, 2)*J.transpose()*J; b -= std::pow(w, 2)*J.transpose()*res; } } do { Eigen::Matrix3d Damper = lambda*Eigen::Matrix3d::Identity(); Eigen::Vector3d delta = (A + Damper).ldlt().solve(b); Eigen::Vector3d new_solution = solution + delta; delta_norm = delta.norm(); double new_total_cost = calcTotalCost(mobs, rel_sw_poses, new_solution); if (new_total_cost < total_cost) { total_cost = new_total_cost; solution = new_solution; is_cost_reduced = true; lambda = lambda/10.0 > 1e-10 ? lambda/10.0 : 1e-10; } else { is_cost_reduced = false; lambda = lambda*10 < 1e12 ? lambda*10 : 1e12; } } while (inner_loop_cnt++ < _inner_loop_max_iter && !is_cost_reduced); inner_loop_cnt = 0; } while (outer_loop_cnt++ < _outer_loop_max_iter && delta_norm > _conv_precision); Eigen::Vector3d pf_last; pf_last.z() = 1.0/solution.z(); pf_last.x() = solution.x()*pf_last.z(); pf_last.y() = solution.y()*pf_last.z(); pf.setZero(); if ((outer_loop_cnt >= _outer_loop_max_iter && inner_loop_cnt >= _inner_loop_max_iter) || delta_norm > _conv_precision) return false; for (const auto& item : rel_sw_poses) { Eigen::Vector3d tmp = item.second->copyValueAsIso()*pf_last; if (tmp.z() <= _min_depth) return false; } Eigen::HouseholderQR qr(pf_last); Eigen::MatrixXd Q = qr.householderQ(); double base_line_max = 0.0; for (const auto& item : rel_sw_poses) { Eigen::Vector3d p_ci_A = item.second->copyValueAsIso().inverse().translation(); double base_line = ((Q.block(0, 1, 3, 2)).transpose()*p_ci_A).norm(); if (base_line > base_line_max) base_line_max = base_line; } if (pf_last.z() < _min_depth || pf_last.z() > _max_depth) return false; pf = pose_last->copyValueAsIso()*pf_last; if (pf.hasNaN()) { pf.setZero(); return false; } return true; } bool Triangulator::triangulateStereoObs( const std::map>& stereo_obs, const std::map, std::less>& sw_poses_raw, const Eigen::Isometry3d& T_cl2cr, Eigen::Vector3d& pf) const { std::map> sobs; std::map, std::less> sw_poses; this->filterCommonTimestamp(sw_poses_raw, stereo_obs, sw_poses, sobs); std::map> mono_obs; std::map, std::less> sw_mono_poses; double cnt = 0; for (const auto& item : sobs) { cnt = cnt + 1.0; mono_obs[cnt] = std::make_shared(); mono_obs.at(cnt)->_u0 = item.second->_u0; mono_obs.at(cnt)->_v0 = item.second->_v0; sw_mono_poses[cnt] = sw_poses.at(item.first)->clone(); cnt = cnt + 1.0; mono_obs[cnt] = std::make_shared(); mono_obs.at(cnt)->_u0 = item.second->_u1; mono_obs.at(cnt)->_v0 = item.second->_v1; sw_mono_poses[cnt] = std::make_shared(); sw_mono_poses.at(cnt)->setValueByIso(sw_poses.at(item.first)->copyValueAsIso()*T_cl2cr.inverse()); } bool flag = this->triangulateMonoObs(mono_obs, sw_mono_poses, pf); return flag; } } ================================================ FILE: ingvio_estimator/src/Triangulator.h ================================================ /** This File is part of InGVIO, an invariant filter for mono/stereo visual- * inertial-raw GNSS navigation. * * Copyright (C) 2022 Changwu Liu (cwliu529@163.com, * lcw18@mails.tsinghua.edu.cn (valid until 2023)) * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program. If not, see . */ #pragma once #include #include #include "IngvioParams.h" namespace ingvio { struct MonoMeas; struct StereoMeas; class SE3; class Triangulator { public: Triangulator() {} Triangulator(const IngvioParams& filter_params) { _trans_thres = filter_params._trans_thres; _huber_epsilon = filter_params._huber_epsilon; _conv_precision = filter_params._conv_precision; _init_damping = filter_params._init_damping; _outer_loop_max_iter = filter_params._outer_loop_max_iter; _inner_loop_max_iter = filter_params._inner_loop_max_iter; _max_depth = filter_params._max_depth; _min_depth = filter_params._min_depth; _max_baseline_ratio = filter_params._max_baseline_ratio; } ~Triangulator() {} bool triangulateMonoObs( const std::map>& mono_obs, const std::map, std::less>& sw_poses_raw, Eigen::Vector3d& pf) const; bool triangulateStereoObs( const std::map>& stereo_obs, const std::map, std::less>& sw_poses_raw, const Eigen::Isometry3d& T_cl2cr, Eigen::Vector3d& pf) const; protected: double _trans_thres = 0.1; double _huber_epsilon = 0.01; double _conv_precision = 5e-7; double _init_damping = 1e-3; int _outer_loop_max_iter = 10; int _inner_loop_max_iter = 10; double _max_depth = 60.0; double _min_depth = 0.2; double _max_baseline_ratio = 40.0; double findLongestTrans( const std::map, std::less>& sw_poses, const std::map>& mobs, double& max_length) const; double initDepth(const Eigen::Vector2d& m1, const Eigen::Vector2d& m2, const std::shared_ptr T12) const; void calcRelaSwPose( const std::map, std::less>& sw_poses, std::map, std::less>& rel_sw_poses) const; double calcUnitCost(const Eigen::Vector2d& meas, const std::shared_ptr rel_pose, const Eigen::Vector3d& solution) const; double calcTotalCost(const std::map>& mobs, const std::map>& rel_poses, const Eigen::Vector3d& solution) const; void calcResJacobian(const Eigen::Vector2d& meas, const std::shared_ptr rel_pose, const Eigen::Vector3d& solution, Eigen::Vector2d& res, Eigen::Matrix& J, double& w) const; template void filterCommonTimestamp( const std::map, std::less>& poses, const std::map>& obs, std::map>& common_poses, std::map>& common_obs) const { common_obs.clear(); common_poses.clear(); for (const auto& item : obs) { if (poses.find(item.first) == poses.end()) continue; common_obs[item.first] = item.second; common_poses[item.first] = poses.at(item.first); } } }; } ================================================ FILE: ingvio_estimator/src/Update.cpp ================================================ /** This File is part of InGVIO, an invariant filter for mono/stereo visual- * inertial-raw GNSS navigation. * * Copyright (C) 2022 Changwu Liu (cwliu529@163.com, * lcw18@mails.tsinghua.edu.cn (valid until 2023)) * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program. If not, see . */ #include "StateManager.h" #include "Update.h" namespace ingvio { void UpdateBase::setChiSquaredTable(const int& max_dof, const double& thres) { for (int i = 1; i <= max_dof; ++i) { boost::math::chi_squared chi_squared_dist(i); this->_chi_squared_table[i] = boost::math::quantile(chi_squared_dist, thres); } } double UpdateBase::whitenResidual(const std::shared_ptr state, const Eigen::VectorXd& res, const Eigen::MatrixXd& H, const std::vector>& var_order, double noise) { assert(StateManager::checkSubOrder(state, var_order)); int idx = 0; for (int i = 0; i < var_order.size(); ++i) idx += var_order[i]->size(); assert(res.rows() == H.rows()); assert(idx == H.cols()); Eigen::MatrixXd small_cov = StateManager::getMarginalCov(state, var_order); Eigen::MatrixXd S = H*small_cov*H.transpose() + std::pow(noise, 2.0)*Eigen::MatrixXd::Identity(H.rows(), H.rows()); return res.transpose()*S.ldlt().solve(res); } double UpdateBase::whitenResidual(const std::shared_ptr state, const Eigen::VectorXd& res, const Eigen::MatrixXd& H, const std::vector>& var_order, const Eigen::MatrixXd& R) { assert(StateManager::checkSubOrder(state, var_order)); assert(R.rows() == R.cols()); int idx = 0; for (int i = 0; i < var_order.size(); ++i) idx += var_order[i]->size(); assert(res.rows() == H.rows()); assert(idx == H.cols()); Eigen::MatrixXd small_cov = StateManager::getMarginalCov(state, var_order); Eigen::MatrixXd S = H*small_cov*H.transpose() + R; return res.transpose()*S.ldlt().solve(res); } bool UpdateBase::testChiSquared(const std::shared_ptr state, const Eigen::VectorXd& res, const Eigen::MatrixXd& H, const std::vector>& var_order, double noise) { double prob = this->whitenResidual(state, res, H, var_order, noise); int dof = res.rows(); if (_chi_squared_table.find(dof) == _chi_squared_table.end()) for (int i = _chi_squared_table.rbegin()->first+1; i <= dof; ++i) { boost::math::chi_squared chi_squared_dist(i); this->_chi_squared_table[i] = boost::math::quantile(chi_squared_dist, _thres); } if (prob < _chi_squared_table.at(dof)) return true; else return false; } bool UpdateBase::testChiSquared(const std::shared_ptr state, const Eigen::VectorXd& res, const Eigen::MatrixXd& H, const std::vector>& var_order, double noise, int dof) { double prob = this->whitenResidual(state, res, H, var_order, noise); if (_chi_squared_table.find(dof) == _chi_squared_table.end()) for (int i = _chi_squared_table.rbegin()->first+1; i <= dof; ++i) { boost::math::chi_squared chi_squared_dist(i); this->_chi_squared_table[i] = boost::math::quantile(chi_squared_dist, _thres); } if (prob < _chi_squared_table.at(dof)) return true; else return false; } bool UpdateBase::testChiSquared(const std::shared_ptr state, const Eigen::VectorXd& res, const Eigen::MatrixXd& H, const std::vector>& var_order, const Eigen::MatrixXd& R, int dof) { if (dof <= 0) return false; double prob = this->whitenResidual(state, res, H, var_order, R); if (_chi_squared_table.find(dof) == _chi_squared_table.end()) for (int i = _chi_squared_table.rbegin()->first+1; i <= dof; ++i) { boost::math::chi_squared chi_squared_dist(i); this->_chi_squared_table[i] = boost::math::quantile(chi_squared_dist, _thres); } if (prob < _chi_squared_table.at(dof)) return true; else return false; } } ================================================ FILE: ingvio_estimator/src/Update.h ================================================ /** This File is part of InGVIO, an invariant filter for mono/stereo visual- * inertial-raw GNSS navigation. * * Copyright (C) 2022 Changwu Liu (cwliu529@163.com, * lcw18@mails.tsinghua.edu.cn (valid until 2023)) * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program. If not, see . */ #pragma once #include #include #include #include #include namespace ingvio { class Type; class State; class UpdateBase { public: UpdateBase(const int& max_dof, const double& thres) : _thres(thres) { this->setChiSquaredTable(max_dof, _thres); } UpdateBase(const double& thres) : _thres(thres) { this->setChiSquaredTable(150, _thres); } UpdateBase() : _thres(0.95) { this->setChiSquaredTable(150, _thres); } virtual ~UpdateBase() {} UpdateBase(const UpdateBase&) = delete; protected: double _thres; std::map _chi_squared_table; void setChiSquaredTable(const int& max_dof, const double& thres); double whitenResidual(const std::shared_ptr state, const Eigen::VectorXd& res, const Eigen::MatrixXd& H, const std::vector>& var_order, double noise); double whitenResidual(const std::shared_ptr state, const Eigen::VectorXd& res, const Eigen::MatrixXd& H, const std::vector>& var_order, const Eigen::MatrixXd& R); virtual bool testChiSquared(const std::shared_ptr state, const Eigen::VectorXd& res, const Eigen::MatrixXd& H, const std::vector>& var_order, double noise); virtual bool testChiSquared(const std::shared_ptr state, const Eigen::VectorXd& res, const Eigen::MatrixXd& H, const std::vector>& var_order, double noise, int dof); virtual bool testChiSquared(const std::shared_ptr state, const Eigen::VectorXd& res, const Eigen::MatrixXd& H, const std::vector>& var_order, const Eigen::MatrixXd& R, int dof); }; } ================================================ FILE: ingvio_estimator/src/VecState.cpp ================================================ /** This File is part of InGVIO, an invariant filter for mono/stereo visual- * inertial-raw GNSS navigation. * * Copyright (C) 2022 Changwu Liu (cwliu529@163.com, * lcw18@mails.tsinghua.edu.cn (valid until 2023)) * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program. If not, see . */ #include "VecState.h" namespace ingvio { void Vec3::update(const Eigen::VectorXd& dx) { assert(dx.rows() >= this->idx() + this->size()); _vec += dx.block<3, 1>(this->idx(), 0); } std::shared_ptr Vec3::clone() { auto tmp = std::shared_ptr(new Vec3()); tmp->setValue(this->value()); tmp->setFej(this->fej()); return tmp; } void Scalar::update(const Eigen::VectorXd& dx) { assert(dx.rows() >= this->idx() + this->size()); _scalar += dx(this->idx()); } void Scalar::setRandom() { Eigen::Vector2d tmp; tmp.setRandom(); _scalar = tmp(0); } std::shared_ptr Scalar::clone() { auto tmp = std::shared_ptr(new Scalar()); tmp->setValue(this->value()); tmp->setFej(this->fej()); return tmp; } } ================================================ FILE: ingvio_estimator/src/VecState.h ================================================ /** This File is part of InGVIO, an invariant filter for mono/stereo visual- * inertial-raw GNSS navigation. * * Copyright (C) 2022 Changwu Liu (cwliu529@163.com, * lcw18@mails.tsinghua.edu.cn (valid until 2023)) * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program. If not, see . */ #pragma once #include #include namespace ingvio { // Acknowledgements: // The idea of type-based index system is inspired by the realization of OpenVINS // see https://github.com/rpng/open_vins class Type { public: Type(int size) { _size = size; } virtual ~Type(){} virtual void set_cov_idx(int new_cov_idx) { _idx = new_cov_idx; } int idx() const { return _idx; } int size() const {return _size; } virtual void update(const Eigen::VectorXd& dx) = 0; virtual void setIdentity() = 0; virtual void setRandom() = 0; protected: int _idx = -1; int _size = -1; }; class Vec3 : public Type { public: Vec3() : Type(3) { _vec.setZero(); _vec_fej.setZero(); } ~Vec3() {} void update(const Eigen::VectorXd& dx) override; void setIdentity() override { _vec.setZero(); } void setRandom() override { _vec.setRandom(); } const Eigen::Vector3d& value() const { return _vec; } const Eigen::Vector3d& fej() const { return _vec_fej; } void setValue(const Eigen::Vector3d& other_vec) { _vec = other_vec; } void setFej(const Eigen::Vector3d& other_vec_fej) { _vec_fej = other_vec_fej; } std::shared_ptr clone(); protected: Eigen::Vector3d _vec; Eigen::Vector3d _vec_fej; }; class Scalar : public Type { public: Scalar() : Type(1) { _scalar = 0.0; _scalar_fej = 0.0; } ~Scalar() {} void update(const Eigen::VectorXd& dx) override; void setIdentity() override { _scalar = 0.0; } void setRandom() override; const double& value() const { return _scalar; } const double& fej() const { return _scalar_fej; } void setValue(const double& other_scalar) { _scalar = other_scalar; } void setFej(const double& other_scalar_fej) { _scalar_fej = other_scalar_fej; } std::shared_ptr clone(); protected: double _scalar; double _scalar_fej; }; } ================================================ FILE: ingvio_estimator/test/GenerateNoise.cpp ================================================ /** This File is part of InGVIO, an invariant filter for mono/stereo visual- * inertial-raw GNSS navigation. * * Copyright (C) 2022 Changwu Liu (cwliu529@163.com, * lcw18@mails.tsinghua.edu.cn (valid until 2023)) * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program. If not, see . */ #include "GenerateNoise.h" #include namespace ingvio_test { double generateGaussRandom(const double& mean, const double& std) { double z1; do { int x1, x2; x1 = rand(); x2 = rand(); double y1 = static_cast(x1 % 100)/100.0; double y2 = static_cast(x2 % 100)/100.0; z1 = std::sqrt(-2.0*std::log(y1))*std::sin(2.0*M_PI*y2); } while (std::isnan(mean+z1*std) || std::isinf(mean+z1*std)); return mean+z1*std; } bool isSPD(const Eigen::MatrixXd& cov) { assert(cov.rows() == cov.cols()); Eigen::VectorXd diags = cov.diagonal(); for (int i = 0; i < diags.rows(); ++i) if (diags(i, 0) < 0.0) return false; return true; } } ================================================ FILE: ingvio_estimator/test/GenerateNoise.h ================================================ /** This File is part of InGVIO, an invariant filter for mono/stereo visual- * inertial-raw GNSS navigation. * * Copyright (C) 2022 Changwu Liu (cwliu529@163.com, * lcw18@mails.tsinghua.edu.cn (valid until 2023)) * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program. If not, see . */ #pragma once #include namespace ingvio_test { extern double generateGaussRandom(const double& mean, const double& std); extern bool isSPD(const Eigen::MatrixXd& cov); } ================================================ FILE: ingvio_estimator/test/TestMapServer.cpp ================================================ /** This File is part of InGVIO, an invariant filter for mono/stereo visual- * inertial-raw GNSS navigation. * * Copyright (C) 2022 Changwu Liu (cwliu529@163.com, * lcw18@mails.tsinghua.edu.cn (valid until 2023)) * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program. If not, see . */ #include #include #include #include "MapServer.h" #include "MapServerManager.h" #include "State.h" #include "StateManager.h" #include "ImuPropagator.h" #include #include #include "GenerateNoise.h" #include "Triangulator.h" using namespace ingvio; namespace ingvio_test { class TestMapServer : public virtual ::testing::Test { public: TestMapServer () { T_cl2cr.linear() = Eigen::Matrix3d::Identity(); T_cl2cr.translation() = Eigen::Vector3d(0.001, -0.12, 0.003); filter_params.readParams("/home/lcw/VIO/ws_ingvio/src/config/sportsfield/ingvio_stereo.yaml"); filter_params._init_imu_buffer_sp = -1; state = std::make_shared(filter_params); state->_state_params._T_cl2cr = T_cl2cr; imu_propa = std::make_shared(filter_params); state->initStateAndCov(0.0, Eigen::Quaterniond::Identity()); map_server = std::make_shared(); } virtual ~TestMapServer() = default; protected: Eigen::Isometry3d T_cl2cr; IngvioParams filter_params; std::shared_ptr state; std::shared_ptr imu_propa; std::shared_ptr map_server; feature_tracker::MonoFrameConstPtr generateRandomMonoFrame(const std::vector& ids, double timestamp) { feature_tracker::MonoFramePtr mono_frame(new feature_tracker::MonoFrame()); mono_frame->header.stamp = ros::Time(timestamp); for (int i = 0; i < ids.size(); ++i) { feature_tracker::MonoMeas::Ptr mono_meas(new feature_tracker::MonoMeas()); mono_meas->id = ids[i]; Eigen::Vector2d vec = Eigen::Vector2d::Random(); mono_meas->u0 = vec.x(); mono_meas->v0 = vec.y(); mono_frame->mono_features.push_back(*mono_meas); } return mono_frame; } feature_tracker::MonoMeas::Ptr calcMonoMeas(int id, const Eigen::Vector3d& pf, const std::shared_ptr pose) { feature_tracker::MonoMeas::Ptr mono_meas(new feature_tracker::MonoMeas()); Eigen::Vector3d body = pose->copyValueAsIso().inverse()*pf; mono_meas->id = id; mono_meas->u0 = body.x()/body.z() + generateGaussRandom(0.0, 0.02); mono_meas->v0 = body.y()/body.z() + generateGaussRandom(0.0, 0.02); return mono_meas; } feature_tracker::MonoFrameConstPtr generateMonoFrame(double timestamp, const std::vector& ids, const std::vector& pfs, const std::shared_ptr pose) { feature_tracker::MonoFramePtr mono_frame(new feature_tracker::MonoFrame()); mono_frame->header.stamp = ros::Time(timestamp); assert(ids.size() == pfs.size()); for (int i = 0; i < ids.size(); ++i) mono_frame->mono_features.push_back(*(calcMonoMeas(ids[i], pfs[i], pose))); return mono_frame; } feature_tracker::StereoFrameConstPtr generateRandomStereoFrame(const std::vector& ids, double timestamp) { feature_tracker::StereoFramePtr stereo_frame(new feature_tracker::StereoFrame()); stereo_frame->header.stamp = ros::Time(timestamp); for (int i = 0; i < ids.size(); ++i) { feature_tracker::StereoMeas::Ptr stereo_meas(new feature_tracker::StereoMeas()); stereo_meas->id = ids[i]; Eigen::Vector4d vec = Eigen::Vector4d::Random(); stereo_meas->u0 = vec.x(); stereo_meas->v0 = vec.y(); stereo_meas->u1 = vec.z(); stereo_meas->v1 = vec.w(); stereo_frame->stereo_features.push_back(*stereo_meas); } return stereo_frame; } feature_tracker::StereoMeas::Ptr calcStereoMeas(int id, const Eigen::Vector3d& pf, const std::shared_ptr pose, const Eigen::Isometry3d& T_cl2cr) { feature_tracker::StereoMeas::Ptr stereo_meas(new feature_tracker::StereoMeas()); Eigen::Vector3d body_left = pose->copyValueAsIso().inverse()*pf; Eigen::Vector3d body_right = T_cl2cr*pose->copyValueAsIso().inverse()*pf; stereo_meas->id = id; stereo_meas->u0 = body_left.x()/body_left.z() + generateGaussRandom(0.0, 0.02); stereo_meas->v0 = body_left.y()/body_left.z() + generateGaussRandom(0.0, 0.02); stereo_meas->u1 = body_right.x()/body_right.z() + generateGaussRandom(0.0, 0.02); stereo_meas->v1 = body_right.y()/body_right.z() + generateGaussRandom(0.0, 0.02); return stereo_meas; } feature_tracker::StereoFrameConstPtr generateStereoFrame(double timestamp, const std::vector& ids, const std::vector& pfs, const std::shared_ptr pose, const Eigen::Isometry3d& T_cl2cr) { feature_tracker::StereoFramePtr stereo_frame(new feature_tracker::StereoFrame()); stereo_frame->header.stamp = ros::Time(timestamp); assert(ids.size() == pfs.size()); for (int i = 0; i < ids.size(); ++i) stereo_frame->stereo_features.push_back(*(calcStereoMeas(ids[i], pfs[i], pose, T_cl2cr))); return stereo_frame; } }; TEST_F(TestMapServer, collectFeatureAndMarg) { std::vector id1(4); for (int i = 0; i < id1.size(); ++i) id1[i] = i+1; std::vector id2(4); for (int i = 0; i < id2.size(); ++i) id2[i] = i+2; Eigen::Isometry3d T; Eigen::Vector3d rot = Eigen::Vector3d::Random(); Eigen::Vector3d trans = Eigen::Vector3d::Random(); T.linear() = Eigen::AngleAxisd(rot.norm(), rot.normalized()).toRotationMatrix(); T.translation() = trans; imu_propa->propagateToExpectedPoseAndAugment(state, 2.0, T); MapServerManager::collectMonoMeas(map_server, state, generateRandomMonoFrame(id1, 2.0)); ASSERT_EQ(map_server->size(), 4); for (int i = 1; i < 5; ++i) { ASSERT_EQ(map_server->at(i)->getId(), i); ASSERT_EQ(map_server->at(i)->getFeatureType(), FeatureInfo::FeatureType::MSCKF); ASSERT_EQ(map_server->at(i)->numOfMonoFrames(), 1); ASSERT_TRUE(map_server->at(i)->hasMonoObsAt(2.0)); } imu_propa->propagateToExpectedPoseAndAugment(state, 4.0, T); MapServerManager::collectMonoMeas(map_server, state, generateRandomMonoFrame(id2, 4.0)); ASSERT_EQ(map_server->size(), 5); for (int i = 1; i < 6; ++i) { ASSERT_EQ(map_server->at(i)->getId(), i); ASSERT_EQ(map_server->at(i)->getFeatureType(), FeatureInfo::FeatureType::MSCKF); if (i == 1 || i == 5) ASSERT_EQ(map_server->at(i)->numOfMonoFrames(), 1); else ASSERT_EQ(map_server->at(i)->numOfMonoFrames(), 2); if (i > 1) ASSERT_TRUE(map_server->at(i)->hasMonoObsAt(4.0)); ASSERT_TRUE(!map_server->at(i)->isToMarg()); } MapServerManager::markMargMonoFeatures(map_server, state); ASSERT_EQ(map_server->size(), 5); for (int i = 1; i < 6; ++i) { if (i == 1) ASSERT_TRUE(map_server->at(i)->isToMarg()); else ASSERT_TRUE(!map_server->at(i)->isToMarg()); } state.reset(new State(filter_params)); state->initStateAndCov(0.0, Eigen::Quaterniond::Identity()); imu_propa.reset(new ImuPropagator(filter_params)); map_server.reset(new MapServer()); imu_propa->propagateToExpectedPoseAndAugment(state, 2.0, T); MapServerManager::collectStereoMeas(map_server, state, generateRandomStereoFrame(id1, 2.0)); ASSERT_EQ(map_server->size(), 4); for (int i = 1; i < 5; ++i) { ASSERT_EQ(map_server->at(i)->getId(), i); ASSERT_EQ(map_server->at(i)->getFeatureType(), FeatureInfo::FeatureType::MSCKF); ASSERT_EQ(map_server->at(i)->numOfStereoFrames(), 1); ASSERT_TRUE(map_server->at(i)->hasStereoObsAt(2.0)); } imu_propa->propagateToExpectedPoseAndAugment(state, 4.0, T); MapServerManager::collectStereoMeas(map_server, state, generateRandomStereoFrame(id2, 4.0)); ASSERT_EQ(map_server->size(), 5); for (int i = 1; i < 6; ++i) { ASSERT_EQ(map_server->at(i)->getId(), i); ASSERT_EQ(map_server->at(i)->getFeatureType(), FeatureInfo::FeatureType::MSCKF); if (i == 1 || i == 5) ASSERT_EQ(map_server->at(i)->numOfStereoFrames(), 1); else ASSERT_EQ(map_server->at(i)->numOfStereoFrames(), 2); if (i > 1) ASSERT_TRUE(map_server->at(i)->hasStereoObsAt(4.0)); ASSERT_TRUE(!map_server->at(i)->isToMarg()); } MapServerManager::markMargStereoFeatures(map_server, state); ASSERT_EQ(map_server->size(), 5); for (int i = 1; i < 6; ++i) { if (i == 1) ASSERT_TRUE(map_server->at(i)->isToMarg()); else ASSERT_TRUE(!map_server->at(i)->isToMarg()); } for (int i = 1; i < 6; ++i) if (i < 5) ASSERT_TRUE(map_server->at(i)->anchor() == static_cast>(state->_sw_camleft_poses.at(2.0))); else ASSERT_TRUE((map_server->at(i)->anchor() == static_cast>(state->_sw_camleft_poses.at(4.0)))); } TEST_F(TestMapServer, featureInfoTriMono) { std::vector ids1(2); ids1[0] = 1; ids1[1] = 2; std::vector pfs1(2); pfs1[0] = Eigen::Vector3d(1.0, 1.5, 2.0); pfs1[1] = Eigen::Vector3d(1.0, 2.0, 3.0); std::vector ids2(2); ids2[0] = 2; ids2[1] = 3; std::vector pfs2(2); pfs2[0] = pfs1[1]; pfs2[1] = Eigen::Vector3d(1.5, 2.0, 3.0); std::vector time(9); for (int i = 0; i < time.size(); ++i) time[i] = 0.5*(i+1); std::vector T(9); for (int i = 0; i < T.size(); ++i) { T[i].linear() = Eigen::AngleAxisd(generateGaussRandom(0.0, 0.1), Eigen::Vector3d(0, 0, 1)).toRotationMatrix(); T[i].translation() = Eigen::Vector3d(2*i-5.0, 2*i-6.0, 0.0); } std::vector> poses(9); for (int i = 0; i < 9; ++i) { poses[i] = std::make_shared(); poses[i]->setValueByIso(T[i]); } imu_propa->propagateToExpectedPoseAndAugment(state, time[0], T[0]); MapServerManager::collectMonoMeas(map_server, state, generateMonoFrame(time[0], ids1, pfs1, poses[0])); imu_propa->propagateToExpectedPoseAndAugment(state, time[1], T[1]); MapServerManager::collectMonoMeas(map_server, state, generateMonoFrame(time[1], ids1, pfs1, poses[1])); imu_propa->propagateToExpectedPoseAndAugment(state, time[2], T[2]); MapServerManager::collectMonoMeas(map_server, state, generateMonoFrame(time[2], ids1, pfs1, poses[2])); imu_propa->propagateToExpectedPoseAndAugment(state, time[3], T[3]); MapServerManager::collectMonoMeas(map_server, state, generateMonoFrame(time[3], ids1, pfs1, poses[3])); imu_propa->propagateToExpectedPoseAndAugment(state, time[4], T[4]); MapServerManager::collectMonoMeas(map_server, state, generateMonoFrame(time[4], ids2, pfs2, poses[4])); imu_propa->propagateToExpectedPoseAndAugment(state, time[5], T[5]); MapServerManager::collectMonoMeas(map_server, state, generateMonoFrame(time[5], ids2, pfs2, poses[5])); std::shared_ptr tri(new Triangulator()); Eigen::Vector3d result; bool flag; flag = FeatureInfoManager::triangulateFeatureInfoMono(map_server->at(1), tri, state); std::cout << "pf1 ref = " << pfs1[0].transpose() << std::endl; std::cout << "pf1 mono estimated = " << map_server->at(1)->landmark()->valuePosXyz().transpose() << std::endl; ASSERT_TRUE(map_server->at(1)->isTri() == flag); flag = FeatureInfoManager::triangulateFeatureInfoMono(map_server->at(2), tri, state); std::cout << "pf2 ref = " << pfs1[1].transpose() << std::endl; std::cout << "pf2 mono estimated = " << map_server->at(2)->landmark()->valuePosXyz().transpose() << std::endl; ASSERT_TRUE(map_server->at(2)->isTri() == flag); imu_propa->propagateToExpectedPoseAndAugment(state, time[6], T[6]); MapServerManager::collectMonoMeas(map_server, state, generateMonoFrame(time[6], ids2, pfs2, poses[6])); imu_propa->propagateToExpectedPoseAndAugment(state, time[7], T[7]); MapServerManager::collectMonoMeas(map_server, state, generateMonoFrame(time[7], ids2, pfs2, poses[7])); imu_propa->propagateToExpectedPoseAndAugment(state, time[8], T[8]); MapServerManager::collectMonoMeas(map_server, state, generateMonoFrame(time[8], ids2, pfs2, poses[8])); flag = FeatureInfoManager::triangulateFeatureInfoMono(map_server->at(3), tri, state); std::cout << "pf3 ref = " << pfs2[1].transpose() << std::endl; std::cout << "pf3 mono estimated = " << map_server->at(3)->landmark()->valuePosXyz().transpose() << std::endl; ASSERT_TRUE(map_server->at(3)->isTri() == flag); } TEST_F(TestMapServer, featureInfoTriStereo) { std::vector ids1(2); ids1[0] = 1; ids1[1] = 2; std::vector pfs1(2); pfs1[0] = Eigen::Vector3d(1.0, 1.5, 2.0); pfs1[1] = Eigen::Vector3d(1.0, 2.0, 3.0); std::vector ids2(2); ids2[0] = 2; ids2[1] = 3; std::vector pfs2(2); pfs2[0] = pfs1[1]; pfs2[1] = Eigen::Vector3d(1.5, 2.0, 3.0); std::vector time(9); for (int i = 0; i < time.size(); ++i) time[i] = 0.5*(i+1); std::vector T(9); for (int i = 0; i < T.size(); ++i) { T[i].linear() = Eigen::AngleAxisd(generateGaussRandom(0.0, 0.1), Eigen::Vector3d(0, 0, 1)).toRotationMatrix(); T[i].translation() = Eigen::Vector3d(2*i-5.0, 2*i-6.0, 0.0); } std::vector> poses(9); for (int i = 0; i < 9; ++i) { poses[i] = std::make_shared(); poses[i]->setValueByIso(T[i]); } imu_propa->propagateToExpectedPoseAndAugment(state, time[0], T[0]); MapServerManager::collectStereoMeas(map_server, state, generateStereoFrame(time[0], ids1, pfs1, poses[0], T_cl2cr)); imu_propa->propagateToExpectedPoseAndAugment(state, time[1], T[1]); MapServerManager::collectStereoMeas(map_server, state, generateStereoFrame(time[1], ids1, pfs1, poses[1], T_cl2cr)); imu_propa->propagateToExpectedPoseAndAugment(state, time[2], T[2]); MapServerManager::collectStereoMeas(map_server, state, generateStereoFrame(time[2], ids1, pfs1, poses[2], T_cl2cr)); imu_propa->propagateToExpectedPoseAndAugment(state, time[3], T[3]); MapServerManager::collectStereoMeas(map_server, state, generateStereoFrame(time[3], ids1, pfs1, poses[3], T_cl2cr)); imu_propa->propagateToExpectedPoseAndAugment(state, time[4], T[4]); MapServerManager::collectStereoMeas(map_server, state, generateStereoFrame(time[4], ids2, pfs2, poses[4], T_cl2cr)); imu_propa->propagateToExpectedPoseAndAugment(state, time[5], T[5]); MapServerManager::collectStereoMeas(map_server, state, generateStereoFrame(time[5], ids2, pfs2, poses[5], T_cl2cr)); std::shared_ptr tri(new Triangulator()); Eigen::Vector3d result; bool flag; flag = FeatureInfoManager::triangulateFeatureInfoStereo(map_server->at(1), tri, state); std::cout << "pf1 ref = " << pfs1[0].transpose() << std::endl; std::cout << "pf1 stereo estimated = " << map_server->at(1)->landmark()->valuePosXyz().transpose() << std::endl; ASSERT_TRUE(map_server->at(1)->isTri() == flag); flag = FeatureInfoManager::triangulateFeatureInfoStereo(map_server->at(2), tri, state); std::cout << "pf2 ref = " << pfs1[1].transpose() << std::endl; std::cout << "pf2 stereo estimated = " << map_server->at(2)->landmark()->valuePosXyz().transpose() << std::endl; ASSERT_TRUE(map_server->at(2)->isTri() == flag); imu_propa->propagateToExpectedPoseAndAugment(state, time[6], T[6]); MapServerManager::collectStereoMeas(map_server, state, generateStereoFrame(time[6], ids2, pfs2, poses[6], T_cl2cr)); imu_propa->propagateToExpectedPoseAndAugment(state, time[7], T[7]); MapServerManager::collectStereoMeas(map_server, state, generateStereoFrame(time[7], ids2, pfs2, poses[7], T_cl2cr)); imu_propa->propagateToExpectedPoseAndAugment(state, time[8], T[8]); MapServerManager::collectStereoMeas(map_server, state, generateStereoFrame(time[8], ids2, pfs2, poses[8], T_cl2cr)); flag = FeatureInfoManager::triangulateFeatureInfoStereo(map_server->at(3), tri, state); std::cout << "pf3 ref = " << pfs2[1].transpose() << std::endl; std::cout << "pf3 stereo estimated = " << map_server->at(3)->landmark()->valuePosXyz().transpose() << std::endl; ASSERT_TRUE(map_server->at(3)->isTri() == flag); } TEST_F(TestMapServer, changeAnchor) { for (int i = 0; i < 500; ++i) { ImuCtrl imu_ctrl; imu_ctrl._gyro_raw = Eigen::Vector3d(0.0, 0.0, 0.0); imu_ctrl._accel_raw = Eigen::Vector3d(0.0, 0.0, 9.8); imu_ctrl._timestamp = 0.01*i; imu_propa->storeImu(imu_ctrl); } std::vector ids(1); ids[0] = 1; std::vector pfs(1); pfs[0] = Eigen::Vector3d(1.0, 1.0, 6.0); imu_propa->propagateAugmentAtEnd(state, 1.8); MapServerManager::collectMonoMeas(map_server, state, generateMonoFrame(1.8, ids, pfs, state->_sw_camleft_poses.at(1.8))); StateManager::addAnchoredLandmarkInState(state, map_server->at(1)->landmark(), map_server->at(1)->getId(), 2.21*Eigen::Matrix3d::Identity()); imu_propa->propagateAugmentAtEnd(state, 3.6); MapServerManager::collectMonoMeas(map_server, state, generateMonoFrame(3.6, ids, pfs, state->_sw_camleft_poses.at(3.6))); map_server->at(1)->_ftype = FeatureInfo::FeatureType::SLAM; map_server->at(1)->_landmark->setValuePosXyz(pfs[0]); ASSERT_EQ(map_server->at(1)->getId(), 1); ASSERT_EQ(map_server->at(1)->isTri(), false); ASSERT_EQ(map_server->at(1)->isToMarg(), false); ASSERT_EQ(map_server->at(1)->_landmark->getAnchoredPose(), state->_sw_camleft_poses.at(1.8)); ASSERT_EQ(map_server->at(1)->numOfMonoFrames(), 2); ASSERT_EQ(state->_anchored_landmarks.at(1)->getAnchoredPose(), state->_sw_camleft_poses.at(1.8)); ASSERT_NEAR((state->_anchored_landmarks.at(1)->valuePosXyz()-pfs[0]).norm(), 0.0, 1e-06); ASSERT_NEAR((map_server->at(1)->_landmark->valuePosXyz()-pfs[0]).norm(), 0.0, 1e-06); ASSERT_TRUE(isSPD(StateManager::getFullCov(state))); Eigen::MatrixXd cov_orig = StateManager::getFullCov(state); Eigen::MatrixXd cov = StateManager::getFullCov(state); Eigen::MatrixXd H = Eigen::MatrixXd::Zero(3, 36); // std::cout << "cov ref = " << cov << std::endl; H.block<3, 3>(0, 27) = Eigen::Matrix3d::Identity(); H.block<3, 3>(0, 21) = -skew(pfs[0]); H.block<3, 3>(0, 30) = skew(pfs[0]); FeatureInfoManager::changeAnchoredPose(map_server->at(1), state, 3.6); ASSERT_EQ(map_server->at(1)->_landmark->getAnchoredPose(), state->_sw_camleft_poses.at(3.6)); ASSERT_EQ(state->_anchored_landmarks.at(1)->getAnchoredPose(), state->_sw_camleft_poses.at(3.6)); ASSERT_TRUE(isSPD(StateManager::getFullCov(state))); cov.block<36, 3>(0, 27) = cov_orig*H.transpose(); cov.block<3, 36>(27, 0) = H*cov_orig; cov.block<3, 3>(27, 27) = H*cov_orig*H.transpose(); // std::cout << "cov ref = " << cov << std::endl; // std::cout << "cov diff = " << cov - StateManager::getFullCov(state) << std::endl; ASSERT_NEAR((cov-StateManager::getFullCov(state)).norm(), 0.0, 1e-10); } } int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } ================================================ FILE: ingvio_estimator/test/TestPropagator.cpp ================================================ /** This File is part of InGVIO, an invariant filter for mono/stereo visual- * inertial-raw GNSS navigation. * * Copyright (C) 2022 Changwu Liu (cwliu529@163.com, * lcw18@mails.tsinghua.edu.cn (valid until 2023)) * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program. If not, see . */ #include #include "IngvioParams.h" #include "ImuPropagator.h" #include "State.h" #include "StateManager.h" #include "TicToc.h" using namespace ingvio; namespace ingvio_test { class TestPropagator : public virtual ::testing::Test { public: TestPropagator() : _N(300) { sf = Eigen::Vector3d::Random().normalized(); sf *= 9.75; } virtual ~TestPropagator() {} protected: int _N; IngvioParams filter_params; Eigen::Vector3d sf; }; TEST_F(TestPropagator, initGravity) { filter_params.readParams("/home/lcw/VIO/ws_ingvio/src/config/sportsfield/ingvio_stereo.yaml"); filter_params.printParams(); ImuPropagator ip2; ImuPropagator ip1(filter_params); for (int i = 0; i < _N; ++i) { ImuCtrl imu_ctrl; imu_ctrl._accel_raw = sf; imu_ctrl._gyro_raw.setZero(); imu_ctrl._timestamp = 0.01*i; ip1.storeImu(imu_ctrl); ip2.storeImu(imu_ctrl); } auto isMatNear = [](const Eigen::MatrixXd& mat1, const Eigen::MatrixXd& mat2) { if ((mat1-mat2).norm() < 1e-08) return true; else return false; }; auto isQuatNear = [](const Eigen::Quaterniond& quat1, const Eigen::Quaterniond& quat2) { if ((quat1.toRotationMatrix()-quat2.toRotationMatrix()).norm() < 1e-08) return true; else return false; }; ASSERT_TRUE(isMatNear(Eigen::Vector3d(0, 0, -sf.norm()), ip1.getGravity())); ASSERT_TRUE(isMatNear(Eigen::Vector3d(0, 0, -9.8), ip2.getGravity())); Eigen::Quaterniond init_quat1, init_quat2; ASSERT_TRUE(ip1.getInitQuat(init_quat1)); ASSERT_TRUE(ip2.getInitQuat(init_quat2)); ASSERT_TRUE(isQuatNear(init_quat2, Eigen::Quaterniond::Identity())); Eigen::Quaterniond quat_ref = Eigen::Quaterniond::FromTwoVectors(-sf, Eigen::Vector3d(0, 0, -sf.norm())); ASSERT_TRUE(isQuatNear(init_quat1, quat_ref)); Eigen::Quaterniond tmp_quat1, tmp_quat2; for (int i = 1; i < 400; i += 20) { ASSERT_TRUE(ip1.getAvgQuat(tmp_quat1, i)); ASSERT_TRUE(ip2.getAvgQuat(tmp_quat2, i)); ASSERT_TRUE(isQuatNear(quat_ref, tmp_quat1)); ASSERT_TRUE(isQuatNear(quat_ref, tmp_quat2)); } } TEST_F(TestPropagator, oneStepProp) { filter_params.readParams("/home/lcw/VIO/ws_ingvio/src/config/sportsfield/ingvio_stereo.yaml"); filter_params._init_imu_buffer_sp = -1; ImuPropagator ip(filter_params); ImuCtrl imu_ctrl; imu_ctrl.setRandom(); std::shared_ptr state1, state2; auto reset = [](std::shared_ptr& state1, std::shared_ptr& state2, const IngvioParams& filter_params) { state1.reset(new State(filter_params)); state2.reset(new State(filter_params)); state1->_timestamp = 1.0; state2->_timestamp = 1.0; state1->_extended_pose->setRandom(); state2->_extended_pose = state1->_extended_pose->clone(); state1->_bg->setRandom(); state2->_bg = state1->_bg->clone(); state1->_ba->setRandom(); state2->_ba = state1->_ba->clone(); StateManager::addGNSSVariable(state1, State::GNSSType::GPS, 5000, 9.0); StateManager::addGNSSVariable(state1, State::GNSSType::FS, 20, 2.0); StateManager::addGNSSVariable(state2, State::GNSSType::GPS, 5000, 9.0); StateManager::addGNSSVariable(state2, State::GNSSType::FS, 20, 2.0); }; reset(state1, state2, filter_params); ASSERT_NEAR(distance(state1, state2), 0.0, 1e-12); double err1 = INFINITY; double err2 = INFINITY; Eigen::Matrix Phi1, Phi2; Eigen::Matrix G1, G2; double t_analytic = 0.0; double t_numerical = 0.0; for (int i = 0; i < 5; ++i) { reset(state1, state2, filter_params); double dt = std::pow(10.0, -i); TicToc analytic; ip.stateAndCovTransition(state1, imu_ctrl, dt, Phi1, G1, true); t_analytic += analytic.toc(); TicToc numerical; ip.stateAndCovTransition(state2, imu_ctrl, dt, Phi2, G2, false); t_numerical += numerical.toc(); double err_state = distance(state1, state2); double err_Phi = (Phi1-Phi2).norm(); ASSERT_TRUE(err_state < err1); ASSERT_TRUE(err_Phi < err2); err1 = err_state; err2 = err_Phi; } std::cout << "Analytic one step prop: " << t_analytic << " (ms)" << std::endl; std::cout << "Numerical one step prop: " << t_numerical << " (ms)" << std::endl; } TEST_F(TestPropagator, propaUntil) { filter_params.readParams("/home/lcw/VIO/ws_ingvio/src/config/sportsfield/ingvio_stereo.yaml"); filter_params._init_imu_buffer_sp = -1; std::shared_ptr state = std::make_shared(filter_params); ImuPropagator ip(filter_params); ASSERT_TRUE(ip.isInit()); state->initStateAndCov(0.0, Eigen::Quaterniond::Identity()); for (int i = -20; i <= 100; ++i) { ImuCtrl imu_ctrl; imu_ctrl.setRandom(); imu_ctrl._timestamp = 0.1*i; ip.storeImu(imu_ctrl); } ip.propagateUntil(state, 1.0); ASSERT_EQ(state->_timestamp, 1.0); ip.propagateUntil(state, 8.35); ASSERT_EQ(state->_timestamp, 8.35); ip.propagateUntil(state, 7.1); ASSERT_EQ(state->_timestamp, 8.35); ip.propagateUntil(state, 9.5); ASSERT_EQ(state->_timestamp, 9.5); ip.propagateUntil(state, 10.5); ASSERT_EQ(state->_timestamp, 10.5); ip.propagateUntil(state, 11.0); ASSERT_EQ(state->_timestamp, 10.5); } TEST_F(TestPropagator, propagateAugment) { filter_params.readParams("/home/lcw/VIO/ws_ingvio/src/config/sportsfield/ingvio_stereo.yaml"); filter_params._init_imu_buffer_sp = -1; std::shared_ptr state = std::make_shared(filter_params); ImuPropagator ip(filter_params); ASSERT_TRUE(ip.isInit()); state->initStateAndCov(0.0, Eigen::Quaterniond::Identity()); for (int i = -20; i <= 100; ++i) { ImuCtrl imu_ctrl; imu_ctrl.setRandom(); imu_ctrl._timestamp = 0.1*i; ip.storeImu(imu_ctrl); } for (int i = 0; i < 15; ++i) ip.propagateAugmentAtEnd(state, (i+1)*0.5); ASSERT_EQ(state->_timestamp, 7.5); ASSERT_EQ(state->curr_cov_size(), 21 + 6*15); } } int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } ================================================ FILE: ingvio_estimator/test/TestStateManager.cpp ================================================ /** This File is part of InGVIO, an invariant filter for mono/stereo visual- * inertial-raw GNSS navigation. * * Copyright (C) 2022 Changwu Liu (cwliu529@163.com, * lcw18@mails.tsinghua.edu.cn (valid until 2023)) * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program. If not, see . */ #include #include #include "State.h" #include "StateManager.h" using namespace ingvio; namespace ingvio_test { TEST(testState, BasicFuncs) { for (int i = 0; i < 20; ++i) { Eigen::Vector3d vec; vec.setRandom(); ASSERT_TRUE(skew(vec) == -skew(vec).transpose()); ASSERT_TRUE(vee(skew(vec)) == vec); Eigen::Matrix3d rot_mat = Eigen::AngleAxisd(vec.norm(), vec.normalized()).toRotationMatrix(); ASSERT_NEAR((GammaFunc(vec)-rot_mat).norm(), 0.0, 1e-08); } ASSERT_NEAR((GammaFunc(Eigen::Vector3d::Zero(), 1)-Eigen::Matrix3d::Identity()).norm(), 0.0, 1e-08); ASSERT_NEAR((GammaFunc(Eigen::Vector3d::Zero(), 2)-0.5*Eigen::Matrix3d::Identity()).norm(), 0.0, 1e-08); ASSERT_NEAR((GammaFunc(Eigen::Vector3d::Zero(), 3)-1.0/6.0*Eigen::Matrix3d::Identity()).norm(), 0.0, 1e-08); } TEST(testState, StateAddMargProp) { IngvioParams filter_params; filter_params.readParams("/home/lcw/VIO/ws_ingvio/src/config/sportsfield/ingvio_stereo.yaml"); filter_params._enable_gnss = 1; filter_params.printParams(); std::shared_ptr state = std::make_shared(filter_params); ASSERT_TRUE(StateManager::checkStateContinuity(state)); ASSERT_EQ(state->curr_cov_size(), 21); ASSERT_NEAR((state->_camleft_imu_extrinsics->valueLinearAsMat()-filter_params._T_cl2i.linear()).norm(), 0.0, 1e-08); ASSERT_NEAR((state->_camleft_imu_extrinsics->valueTrans()-filter_params._T_cl2i.translation()).norm(), 0.0, 1e-08); StateManager::addGNSSVariable(state, State::GNSSType::GPS, 20.0, 4.0); ASSERT_TRUE(state->curr_cov_size() == 22); ASSERT_TRUE(state->curr_err_variable_size() == 5); StateManager::addGNSSVariable(state, State::GNSSType::BDS, 16.0, 4.0); ASSERT_TRUE(state->curr_cov_size() == 23); ASSERT_TRUE(state->curr_err_variable_size() == 6); StateManager::addGNSSVariable(state, State::GNSSType::YOF, 123.0, 1.0); ASSERT_TRUE(state->curr_cov_size() == 24); ASSERT_TRUE(state->curr_err_variable_size() == 7); StateManager::addGNSSVariable(state, State::GNSSType::FS, 2.0, 1.0); ASSERT_TRUE(state->curr_cov_size() == 25); ASSERT_TRUE(state->curr_err_variable_size() == 8); StateManager::margGNSSVariable(state, State::GNSSType::GPS); ASSERT_TRUE(state->curr_cov_size() == 24); ASSERT_TRUE(state->curr_err_variable_size() == 7); StateManager::addGNSSVariable(state, State::GNSSType::GLO, 3.0, 6.0); ASSERT_TRUE(state->curr_cov_size() == 25); ASSERT_TRUE(state->curr_err_variable_size() == 8); Eigen::MatrixXd cov = StateManager::getFullCov(state); const Eigen::Matrix Phi_imu = Eigen::Matrix::Random(); const Eigen::Matrix G_imu = Eigen::Matrix::Random(); const double dt = 1.5; Eigen::Matrix Q_imu; Q_imu.setIdentity(); Q_imu.middleCols<3>(0) *= std::pow(state->_state_params._noise_g, 2.0); Q_imu.middleCols<3>(3) *= std::pow(state->_state_params._noise_a, 2.0); Q_imu.middleCols<3>(6) *= std::pow(state->_state_params._noise_bg, 2.0); Q_imu.middleCols<3>(9) *= std::pow(state->_state_params._noise_ba, 2.0); Eigen::Matrix2d Q_gnss; Q_gnss.setZero(); Q_gnss(0, 0) = std::pow(state->_state_params._noise_clockbias, 2.0); Q_gnss(1, 1) = std::pow(state->_state_params._noise_cb_rw, 2.0); Eigen::Matrix Q = Eigen::Matrix::Zero(); Q.block<12, 12>(0, 0) = Q_imu; Q.block<2, 2>(12, 12) = Q_gnss; Eigen::Matrix4d Phi_gnss = Eigen::Matrix4d::Identity(); Phi_gnss(0, 2) = dt; Phi_gnss(3, 2) = dt; Eigen::Matrix Phi = Eigen::Matrix::Identity(); Phi.block<15, 15>(0, 0) = Phi_imu; Phi.block<4, 4>(21, 21) = Phi_gnss; Eigen::Matrix G = Eigen::Matrix::Zero(); G.block<15, 12>(0, 0) = G_imu; Eigen::Matrix G_gnss = Eigen::Matrix::Zero(); G_gnss(0, 0) = 1; G_gnss(2, 1) = 1; G_gnss(3, 0) = 1; G.block<4, 2>(21, 12) = G_gnss; Eigen::MatrixXd cov_result = Phi*cov*Phi.transpose() + dt*Phi*G*Q*G.transpose()*Phi.transpose(); StateManager::propagateStateCov(state, Phi_imu, G_imu, dt); ASSERT_NEAR((StateManager::getFullCov(state)-cov_result).norm(), 0.0, 1e-10); // std::cout << "propagateStateCov = " << StateManager::getFullCov(state) << std::endl; // std::cout << "=====================" << std::endl; // std::cout << "calculated ref cov = " << cov_result << std::endl; std::vector> small_var; small_var.push_back(state->_extended_pose); small_var.push_back(state->_ba); small_var.push_back(state->_gnss.at(State::GNSSType::YOF)); small_var.push_back(state->_gnss.at(State::GNSSType::GLO)); Eigen::MatrixXd small_cov = StateManager::getMarginalCov(state, small_var); ASSERT_NEAR((small_cov.block<9, 9>(0, 0) - StateManager::getFullCov(state).block<9, 9>(0, 0)).norm(), 0.0, 1e-06); ASSERT_NEAR((small_cov.block<3, 3>(9, 9) - StateManager::getFullCov(state).block<3, 3>(12, 12)).norm(), 0.0, 1e-06); } class StateUpdateTest : public virtual ::testing::Test { protected: StateUpdateTest() { filter_params.readParams("/home/lcw/VIO/ws_ingvio/src/config/sportsfield/ingvio_stereo.yaml"); state = std::make_shared(filter_params); state->_extended_pose->setRandom(); state->_bg->setRandom(); state->_ba->setRandom(); state->_camleft_imu_extrinsics->setRandom(); StateManager::addGNSSVariable(state, State::GNSSType::GPS, 20.0, 4.0); StateManager::addGNSSVariable(state, State::GNSSType::YOF, 123.0, 1.0); StateManager::addGNSSVariable(state, State::GNSSType::FS, 2.0, 1.0); StateManager::addGNSSVariable(state, State::GNSSType::BDS, 16.0, 4.0); Phi_imu.setRandom(); G_imu.setRandom(); } virtual ~StateUpdateTest() {} IngvioParams filter_params; std::shared_ptr state; Eigen::Matrix Phi_imu; Eigen::Matrix G_imu; }; TEST_F(StateUpdateTest, augmentPose) { auto constructJ = [](const Eigen::Matrix3d& C_i2w) { Eigen::Matrix tmp; tmp.setZero(); tmp.block<3, 3>(0, 0).setIdentity(); tmp.block<3, 3>(3, 3).setIdentity(); tmp.block<3, 3>(0, 15) = C_i2w; tmp.block<3, 3>(3, 18) = C_i2w; return tmp; }; auto constructLargeJ = [&](int orig_row, const Eigen::Matrix3d& C_i2w) { Eigen::MatrixXd tmp(orig_row+6, orig_row); tmp.setZero(); tmp.block(0, 0, orig_row, orig_row) = Eigen::MatrixXd::Identity(orig_row, orig_row); tmp.block<6, 21>(orig_row, 0) = constructJ(C_i2w); return tmp; }; this->state->_timestamp = 1.0; StateManager::propagateStateCov(this->state, this->Phi_imu, this->G_imu, 1.5); state->_timestamp = 2.5; Eigen::MatrixXd cov1 = StateManager::getFullCov(state); Eigen::MatrixXd largeJ1 = constructLargeJ(cov1.rows(), this->state->_extended_pose->valueLinearAsMat()); StateManager::augmentSlidingWindowPose(state); ASSERT_NEAR((StateManager::getFullCov(state)-largeJ1*cov1*largeJ1.transpose()).norm(), 0.0, 1e-08); ASSERT_EQ(state->curr_cov_size(), cov1.rows() + 6); ASSERT_NEAR((state->_extended_pose->valueLinearAsMat()*state->_camleft_imu_extrinsics->valueLinearAsMat()-state->_sw_camleft_poses[2.5]->valueLinearAsMat()).norm(), 0.0, 1e-08); ASSERT_NEAR((state->_extended_pose->valueTrans1()+state->_extended_pose->valueLinearAsMat()*state->_camleft_imu_extrinsics->valueTrans()-state->_sw_camleft_poses[2.5]->valueTrans()).norm(), 0.0, 1e-08); StateManager::propagateStateCov(this->state, this->Phi_imu, this->G_imu, 0.5); state->_timestamp = 3.0; Eigen::MatrixXd cov2 = StateManager::getFullCov(state); Eigen::MatrixXd largeJ2 = constructLargeJ(cov2.rows(), this->state->_extended_pose->valueLinearAsMat()); StateManager::augmentSlidingWindowPose(state); ASSERT_NEAR((StateManager::getFullCov(state)-largeJ2*cov2*largeJ2.transpose()).norm(), 0.0, 1e-08); ASSERT_EQ(state->curr_cov_size(), cov2.rows() + 6); ASSERT_NEAR((state->_extended_pose->valueLinearAsMat()*state->_camleft_imu_extrinsics->valueLinearAsMat()-state->_sw_camleft_poses[3.0]->valueLinearAsMat()).norm(), 0.0, 1e-08); ASSERT_NEAR((state->_extended_pose->valueTrans1()+state->_extended_pose->valueLinearAsMat()*state->_camleft_imu_extrinsics->valueTrans()-state->_sw_camleft_poses[3.0]->valueTrans()).norm(), 0.0, 1e-08); } TEST_F(StateUpdateTest, stateBoxPlus) { Eigen::Vector3d x1, x2, x3; x1.setRandom(); x1.z() = std::fabs(x1.z()); x1.head<2>().normalize(); AnchoredLandmark::switchRepBear2Xyz(x1, x2); AnchoredLandmark::switchRepXyz2Bear(x2, x3); ASSERT_NEAR(x1.z()-x3.z(), 0.0, 1e-08); ASSERT_NEAR(std::fabs(x1.y()), std::fabs(x3.y()), 1e-08); ASSERT_TRUE(std::fabs(x1.x()-x3.x()) < 1e-08 || std::fabs(std::fabs(x1.x()-x3.x())-M_PI) < 1e-08); x1.setRandom(); x1.z() = std::fabs(x1.z()); AnchoredLandmark::switchRepXyz2Bear(x1, x2); AnchoredLandmark::switchRepBear2Xyz(x2, x3); ASSERT_NEAR((x1-x3).norm(), 0.0, 1e-08); x1.setRandom(); x1.z() = std::fabs(x1.z()); AnchoredLandmark::switchRepXyz2Invd(x1, x2); AnchoredLandmark::switchRepInvd2Xyz(x2, x3); ASSERT_NEAR((x1-x3).norm(), 0.0, 1e-08); x1.setRandom(); x1.z() = std::fabs(x1.z()); AnchoredLandmark::switchRepInvd2Xyz(x1, x2); AnchoredLandmark::switchRepXyz2Invd(x2, x3); ASSERT_NEAR((x1-x3).norm(), 0.0, 1e-08); this->state->_timestamp = 1.0; StateManager::propagateStateCov(this->state, this->Phi_imu, this->G_imu, 1.5); state->_timestamp = 2.5; StateManager::augmentSlidingWindowPose(state); std::shared_ptr lm1(new AnchoredLandmark()); lm1->resetAnchoredPose(state->_sw_camleft_poses.at(2.5)); lm1->setRepType(AnchoredLandmark::LmRep::BEARING); Eigen::Vector3d tmp = Eigen::Vector3d::Random(); tmp.z() = std::fabs(tmp.z()); tmp.topRows<2>().normalize(); lm1->setValuePosRep(tmp); StateManager::addAnchoredLandmarkInState(state, lm1, 5, 10.0*Eigen::Matrix3d::Identity()); Eigen::Vector3d bodyXYZ; bodyXYZ.z() = 1.0/tmp.z()*std::cos(tmp.y()); bodyXYZ.x() = 1.0/tmp.z()*std::cos(tmp.x())*std::sin(tmp.y()); bodyXYZ.y() = 1.0/tmp.z()*std::sin(tmp.x())*std::sin(tmp.y()); Eigen::Vector3d worldXYZ = state->_sw_camleft_poses.at(2.5)->valueLinearAsMat()*bodyXYZ + state->_sw_camleft_poses.at(2.5)->valueTrans(); ASSERT_NEAR((worldXYZ-lm1->valuePosXyz()).norm(), 0.0, 1e-8); ASSERT_NEAR((worldXYZ-state->_anchored_landmarks.at(5)->valuePosXyz()).norm(), 0.0, 1e-08); StateManager::propagateStateCov(this->state, this->Phi_imu, this->G_imu, 0.5); state->_timestamp = 3.0; StateManager::augmentSlidingWindowPose(state); std::shared_ptr lm2(new AnchoredLandmark()); lm2->resetAnchoredPose(state->_sw_camleft_poses.at(3.0)); lm2->setRepType(AnchoredLandmark::LmRep::INV_DEPTH); tmp = Eigen::Vector3d::Random(); tmp.z() = std::fabs(tmp.z()); lm2->setValuePosRep(tmp); StateManager::addAnchoredLandmarkInState(state, lm2, 6, 12.0*Eigen::Matrix3d::Identity()); bodyXYZ.x() = tmp.x()/tmp.z(); bodyXYZ.y() = tmp.y()/tmp.z(); bodyXYZ.z() = 1.0/tmp.z(); worldXYZ = state->_sw_camleft_poses.at(3.0)->copyValueAsIso()*bodyXYZ; ASSERT_NEAR((worldXYZ-lm2->valuePosXyz()).norm(), 0.0, 1e-08); ASSERT_NEAR((worldXYZ-state->_anchored_landmarks.at(6)->valuePosXyz()).norm(), 0.0, 1e-08); Eigen::Matrix3d R_hat = state->_extended_pose->valueLinearAsMat(); Eigen::Vector3d p_hat = state->_extended_pose->valueTrans1(); Eigen::Vector3d v_hat = state->_extended_pose->valueTrans2(); Eigen::Vector3d bg_hat = state->_bg->value(); Eigen::Vector3d ba_hat = state->_ba->value(); Eigen::Matrix3d R_ext_hat = state->_camleft_imu_extrinsics->valueLinearAsMat(); Eigen::Vector3d p_ext_hat = state->_camleft_imu_extrinsics->valueTrans(); double t_gps_hat = state->_gnss.at(State::GNSSType::GPS)->value(); double t_bds_hat = state->_gnss.at(State::GNSSType::BDS)->value(); double fs_hat = state->_gnss.at(State::GNSSType::FS)->value(); double yof_hat = state->_gnss.at(State::GNSSType::YOF)->value(); Eigen::Matrix3d R_c1_hat = state->_sw_camleft_poses.at(2.5)->valueLinearAsMat(); Eigen::Vector3d p_c1_hat = state->_sw_camleft_poses.at(2.5)->valueTrans(); Eigen::Matrix3d R_c2_hat = state->_sw_camleft_poses.at(3.0)->valueLinearAsMat(); Eigen::Vector3d p_c2_hat = state->_sw_camleft_poses.at(3.0)->valueTrans(); Eigen::Vector3d pf1_hat = state->_anchored_landmarks.at(5)->valuePosXyz(); Eigen::Vector3d pf2_hat = state->_anchored_landmarks.at(6)->valuePosXyz(); const int rows = state->curr_cov_size(); Eigen::VectorXd dx(rows); dx.setRandom(); const Eigen::Vector3d& delta_theta = dx.block<3, 1>(0, 0); const Eigen::Vector3d& delta_p = dx.block<3, 1>(3, 0); const Eigen::Vector3d& delta_v = dx.block<3, 1>(6, 0); const Eigen::Vector3d& delta_bg = dx.block<3, 1>(9, 0); const Eigen::Vector3d& delta_ba = dx.block<3, 1>(12, 0); const Eigen::Vector3d& delta_theta_ext = dx.block<3, 1>(15, 0); const Eigen::Vector3d& delta_p_ext = dx.block<3, 1>(18, 0); const double& delta_t_gps = dx(21); const double& delta_yof = dx(22); const double& delta_fs = dx(23); const double& delta_t_bds = dx(24); const Eigen::Vector3d& delta_theta_c1 = dx.block<3, 1>(25, 0); const Eigen::Vector3d& delta_p_c1 = dx.block<3, 1>(28, 0); const Eigen::Vector3d& delta_pf1 = dx.block<3, 1>(31, 0); const Eigen::Vector3d& delta_theta_c2 = dx.block<3, 1>(34, 0); const Eigen::Vector3d& delta_p_c2 = dx.block<3, 1>(37, 0); const Eigen::Vector3d& delta_pf2 = dx.block<3, 1>(40, 0); Eigen::Matrix3d R = GammaFunc(delta_theta, 0)*R_hat; Eigen::Vector3d p = GammaFunc(delta_theta, 0)*p_hat + GammaFunc(delta_theta, 1)*delta_p; Eigen::Vector3d v = GammaFunc(delta_theta, 0)*v_hat + GammaFunc(delta_theta, 1)*delta_v; Eigen::Vector3d bg = bg_hat + delta_bg; Eigen::Vector3d ba = ba_hat + delta_ba; Eigen::Matrix3d R_ext = GammaFunc(delta_theta_ext, 0)*R_ext_hat; Eigen::Vector3d p_ext = GammaFunc(delta_theta_ext, 0)*p_ext_hat + GammaFunc(delta_theta_ext, 1)*delta_p_ext; double t_gps = t_gps_hat + delta_t_gps; double t_bds = t_bds_hat + delta_t_bds; double yof = yof_hat + delta_yof; double fs = fs_hat + delta_fs; Eigen::Matrix3d R_c1 = GammaFunc(delta_theta_c1, 0)*R_c1_hat; Eigen::Vector3d p_c1 = GammaFunc(delta_theta_c1, 0)*p_c1_hat + GammaFunc(delta_theta_c1, 1)*delta_p_c1; Eigen::Matrix3d R_c2 = GammaFunc(delta_theta_c2, 0)*R_c2_hat; Eigen::Vector3d p_c2 = GammaFunc(delta_theta_c2, 0)*p_c2_hat + GammaFunc(delta_theta_c2, 1)*delta_p_c2; Eigen::Vector3d pf1 = GammaFunc(delta_theta_c1, 0)*pf1_hat + GammaFunc(delta_theta_c1, 1)*delta_pf1; Eigen::Vector3d pf2 = GammaFunc(delta_theta_c2, 0)*pf2_hat + GammaFunc(delta_theta_c2, 1)*delta_pf2; StateManager::boxPlus(state, dx); auto isMatNear = [](const Eigen::MatrixXd& mat1, const Eigen::MatrixXd& mat2) { assert(mat1.rows() == mat2.rows()); assert(mat1.cols() == mat2.cols()); if ((mat1-mat2).norm() < 1e-08) return true; else return false; }; ASSERT_TRUE(isMatNear(R, state->_extended_pose->valueLinearAsMat())); ASSERT_TRUE(isMatNear(p, state->_extended_pose->valueTrans1())); ASSERT_TRUE(isMatNear(v, state->_extended_pose->valueTrans2())); ASSERT_TRUE(isMatNear(bg, state->_bg->value())); ASSERT_TRUE(isMatNear(ba, state->_ba->value())); ASSERT_TRUE(isMatNear(R_ext, state->_camleft_imu_extrinsics->valueLinearAsMat())); ASSERT_TRUE(isMatNear(p_ext, state->_camleft_imu_extrinsics->valueTrans())); ASSERT_NEAR(t_gps, state->_gnss.at(State::GNSSType::GPS)->value(), 1e-08); ASSERT_NEAR(t_bds, state->_gnss.at(State::GNSSType::BDS)->value(), 1e-08); ASSERT_NEAR(yof, state->_gnss.at(State::GNSSType::YOF)->value(), 1e-08); ASSERT_NEAR(fs, state->_gnss.at(State::GNSSType::FS)->value(), 1e-08); ASSERT_TRUE(isMatNear(R_c1, state->_sw_camleft_poses.at(2.5)->valueLinearAsMat())); ASSERT_TRUE(isMatNear(p_c1, state->_sw_camleft_poses.at(2.5)->valueTrans())); ASSERT_TRUE(isMatNear(R_c2, state->_sw_camleft_poses.at(3.0)->valueLinearAsMat())); ASSERT_TRUE(isMatNear(p_c2, state->_sw_camleft_poses.at(3.0)->valueTrans())); ASSERT_TRUE(isMatNear(pf1, state->_anchored_landmarks.at(5)->valuePosXyz())); ASSERT_TRUE(isMatNear(pf2, state->_anchored_landmarks.at(6)->valuePosXyz())); pf1 += delta_pf1; pf2 += delta_pf2; state->_anchored_landmarks.at(5)->resetAnchoredPose(); state->_anchored_landmarks.at(6)->resetAnchoredPose(); StateManager::boxPlus(state, dx); ASSERT_TRUE(isMatNear(pf1, state->_anchored_landmarks.at(5)->valuePosXyz())); ASSERT_TRUE(isMatNear(pf2, state->_anchored_landmarks.at(6)->valuePosXyz())); StateManager::margAnchoredLandmarkInState(state, 5); StateManager::margAnchoredLandmarkInState(state, 6); StateManager::margSlidingWindowPose(state); StateManager::margSlidingWindowPose(state, 3.0); StateManager::margSlidingWindowPose(state, 2.5); ASSERT_TRUE(state->curr_cov_size() == 25); ASSERT_TRUE(StateManager::checkStateContinuity(state)); } TEST_F(StateUpdateTest, stateCovUpdate) { this->state->_timestamp = 1.0; StateManager::propagateStateCov(this->state, this->Phi_imu, this->G_imu, 1.5); state->_timestamp = 2.5; StateManager::augmentSlidingWindowPose(state); std::shared_ptr lm1(new AnchoredLandmark()); lm1->resetAnchoredPose(state->_sw_camleft_poses.at(2.5)); lm1->setRepType(AnchoredLandmark::LmRep::BEARING); Eigen::Vector3d tmp = Eigen::Vector3d::Random(); tmp.z() = std::fabs(tmp.z()); tmp.topRows<2>().normalize(); lm1->setValuePosRep(tmp); StateManager::addAnchoredLandmarkInState(state, lm1, 5, 10.0*Eigen::Matrix3d::Identity()); std::vector> var_order; var_order.push_back(state->_extended_pose); var_order.push_back(state->_gnss.at(State::GNSSType::GPS)); var_order.push_back(state->_gnss.at(State::GNSSType::BDS)); var_order.push_back(state->_gnss.at(State::GNSSType::FS)); int var_size = StateManager::calcSubVarSize(var_order); Eigen::VectorXd res(6); res.setRandom(); Eigen::MatrixXd H(res.rows(), var_size); H.setZero(); H.block<6, 3>(0, 0).setRandom(); H.block<3, 3>(0, 3).setRandom(); H.block<3, 3>(3, 6).setRandom(); H(0, 9) = 1.0; H(1, 9) = 1.0; H(2, 10) = 1.0; H.block<3, 1>(3, 11).setOnes(); Eigen::MatrixXd H_large(res.rows(), state->curr_cov_size()); H_large.setZero(); H_large.block<6, 9>(0, 0) = H.block<6, 9>(0, 0); H_large(0, state->_gnss.at(State::GNSSType::GPS)->idx()) = 1.0; H_large(1, state->_gnss.at(State::GNSSType::GPS)->idx()) = 1.0; H_large(2, state->_gnss.at(State::GNSSType::BDS)->idx()) = 1.0; H_large.block<3, 1>(3, state->_gnss.at(State::GNSSType::FS)->idx()).setOnes(); Eigen::MatrixXd cov_orig = StateManager::getFullCov(state); auto isMatNear = [](const Eigen::MatrixXd& mat1, const Eigen::MatrixXd& mat2) { assert(mat1.rows() == mat2.rows()); assert(mat1.cols() == mat2.cols()); if ((mat1-mat2).norm() < 1e-08) return true; else return false; }; Eigen::MatrixXd R = 0.5*Eigen::MatrixXd::Identity(res.rows(), res.rows()); StateManager::ekfUpdate(state, var_order, H, res, R); Eigen::MatrixXd K = cov_orig*H_large.transpose()*(H_large*cov_orig*H_large.transpose() + R).inverse(); Eigen::MatrixXd cov_ref = (Eigen::MatrixXd::Identity(cov_orig.rows(), cov_orig.cols())-K*H_large)*cov_orig; ASSERT_TRUE(isMatNear(cov_ref, StateManager::getFullCov(state))); } class AddDelayedTest : public virtual ::testing::Test { protected: AddDelayedTest() { filter_params.readParams("/home/lcw/VIO/ws_ingvio/src/config/sportsfield/ingvio_stereo.yaml"); state = std::make_shared(filter_params); state->_extended_pose->setRandom(); state->_bg->setRandom(); state->_ba->setRandom(); state->_camleft_imu_extrinsics->setRandom(); Phi_imu.setRandom(); G_imu.setRandom(); state->initStateAndCov(1.0, Eigen::Quaterniond::FromTwoVectors(Eigen::Vector3d::Random().normalized(), Eigen::Vector3d::Random().normalized())); this->state->_timestamp = 1.0; StateManager::propagateStateCov(this->state, this->Phi_imu, this->G_imu, 1.5); state->_timestamp = 2.5; } virtual ~AddDelayedTest() {} IngvioParams filter_params; std::shared_ptr state; Eigen::Matrix Phi_imu; Eigen::Matrix G_imu; }; TEST_F(AddDelayedTest, addVarInv) { Eigen::VectorXd res(1); res = Eigen::Vector2d::Random().head<1>(); std::vector> sub_var_old; sub_var_old.push_back(state->_extended_pose); Eigen::MatrixXd H_old(1, state->_extended_pose->size()); H_old.setRandom(); Eigen::MatrixXd H_new(1, 1); H_new(0, 0) = 1.0; std::shared_ptr tgps = std::make_shared(); tgps->setRandom(); state->_gnss[State::GNSSType::GPS] = tgps; Eigen::MatrixXd cov_orig = StateManager::getFullCov(state); double noise_meas_iso = 2.0; StateManager::addVariableDelayedInvertible(state, tgps, sub_var_old, H_old, H_new, res, noise_meas_iso); ASSERT_TRUE(StateManager::checkStateContinuity(state)); ASSERT_EQ(state->curr_cov_size(), cov_orig.cols()+1); ASSERT_EQ(state->_gnss[State::GNSSType::GPS]->idx(), cov_orig.rows()); Eigen::MatrixXd cov_new = StateManager::getFullCov(state); Eigen::MatrixXd x1 = H_old*cov_orig.block<9, 9>(0, 0)*H_old.transpose(); ASSERT_NEAR(cov_new(cov_new.rows()-1, cov_new.cols()-1), (x1(0, 0)+std::pow(noise_meas_iso, 2.0))/std::pow(H_new(0, 0), 2.0), 1e-8); auto isMatNear = [](const Eigen::MatrixXd& mat1, const Eigen::MatrixXd& mat2) { if ((mat1-mat2).norm() < 1e-08) return true; else return false; }; Eigen::Matrix H_old_large = Eigen::Matrix::Zero(); H_old_large.block<1, 9>(0, 0) = H_old.block<1, 9>(0, 0); ASSERT_TRUE(isMatNear(cov_new.block<21, 1>(0, 21), -cov_orig*H_old_large.transpose()/H_new(0, 0))); } TEST_F(AddDelayedTest, addVar) { Eigen::VectorXd res(2); res.setRandom(); std::vector> sub_var_old; sub_var_old.push_back(state->_extended_pose); Eigen::MatrixXd H_old(2, state->_extended_pose->size()); H_old.setRandom(); Eigen::MatrixXd H_new(2, 1); H_new(0, 0) = 1.0; H_new(1, 0) = 1.0; Eigen::MatrixXd H_old_copy = H_old; Eigen::MatrixXd H_new_copy = H_new; Eigen::VectorXd res_copy = res; std::shared_ptr tgps = std::make_shared(); tgps->setRandom(); state->_gnss[State::GNSSType::GPS] = tgps; Eigen::MatrixXd cov_orig = StateManager::getFullCov(state); double noise_meas_iso = 2.0; StateManager::addVariableDelayed(state, tgps, sub_var_old, H_old, H_new, res, noise_meas_iso, 1.0, false); ASSERT_TRUE(StateManager::checkStateContinuity(state)); ASSERT_EQ(state->curr_cov_size(), cov_orig.cols()+1); ASSERT_EQ(state->_gnss[State::GNSSType::GPS]->idx(), cov_orig.rows()); Eigen::MatrixXd cov_new = StateManager::getFullCov(state); Eigen::Matrix Q_T; Q_T(0, 0) = std::sqrt(2.0)/2.0; Q_T(0, 1) = std::sqrt(2.0)/2.0; Q_T(1, 0) = -std::sqrt(2.0)/2.0; Q_T(1, 1) = std::sqrt(2.0)/2.0; res_copy = Q_T*res_copy; H_old_copy = Q_T*H_old_copy; H_new_copy = Q_T*H_new_copy; Eigen::MatrixXd x1 = H_old_copy.block<1, 9>(0, 0)*cov_orig.block<9, 9>(0, 0)*H_old_copy.block<1, 9>(0, 0).transpose(); auto isMatNear = [](const Eigen::MatrixXd& mat1, const Eigen::MatrixXd& mat2) { if ((mat1-mat2).norm() < 1e-08) return true; else return false; }; Eigen::MatrixXd cov_before_update(22, 22); cov_before_update.block<21, 21>(0, 0) = cov_orig; cov_before_update(21, 21) = (x1(0, 0)+std::pow(noise_meas_iso, 2.0))/std::pow(H_new_copy(0, 0), 2.0); Eigen::Matrix H_old_copy_large = Eigen::Matrix::Zero(); H_old_copy_large.block<1, 9>(0, 0) = H_old_copy.block<1, 9>(0, 0); cov_before_update.block<21, 1>(0, 21) = -cov_orig*H_old_copy_large.transpose()/H_new_copy(0, 0); cov_before_update.block<1, 21>(21, 0) = cov_before_update.block<21, 1>(0, 21).transpose(); Eigen::MatrixXd H_update = Eigen::MatrixXd::Zero(1, 22); H_update.block<1, 9>(0, 0) = H_old_copy.block<1, 9>(1, 0); Eigen::MatrixXd S = H_update*cov_before_update*H_update.transpose() + std::pow(noise_meas_iso, 2.0)*Eigen::MatrixXd::Identity(1, 1); Eigen::MatrixXd Kalman_gain = cov_before_update*H_update.transpose()*S.inverse(); Eigen::MatrixXd cov_after_update = cov_before_update; cov_after_update -= Kalman_gain*H_update*cov_before_update; ASSERT_TRUE(isMatNear(cov_after_update, cov_new)); } } int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } ================================================ FILE: ingvio_estimator/test/TestTriangulator.cpp ================================================ /** This File is part of InGVIO, an invariant filter for mono/stereo visual- * inertial-raw GNSS navigation. * * Copyright (C) 2022 Changwu Liu (cwliu529@163.com, * lcw18@mails.tsinghua.edu.cn (valid until 2023)) * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program. If not, see . */ #include #include #include "MapServer.h" #include "Triangulator.h" #include "GenerateNoise.h" using namespace ingvio; namespace ingvio_test { class TestTriangulator : public virtual ::testing::Test { public: TestTriangulator() : pf(Eigen::Vector3d(1.0, 2.0, 3.0)) { T_cl2cr.linear() = Eigen::Matrix3d::Identity(); T_cl2cr.translation() = Eigen::Vector3d(0.001, -0.12, 0.003); for (int i = 0; i < 10; ++i) { sw_poses1[0.1*i] = std::make_shared(); sw_poses1[0.1*i]->setValueLinearByMat(Eigen::AngleAxisd(generateGaussRandom(0.0, 0.1), Eigen::Vector3d(0, 0, 1)).toRotationMatrix()); sw_poses1[0.1*i]->setValueTrans(Eigen::Vector3d(2*i-9.0, 2*i-9.0, 0.0)); mono_obs1[0.1*i] = calcMonoMeas(pf, sw_poses1.at(0.1*i)); stereo_obs1[0.1*i] = calcStereoMeas(pf, sw_poses1.at(0.1*i)); } sw_poses2[0.1] = std::make_shared(); sw_poses2[0.1]->setValueLinearByQuat(Eigen::Quaterniond::FromTwoVectors(Eigen::Vector3d(0, 0, 1), Eigen::Vector3d(0, 0, 1))); sw_poses2[0.1]->setValueTrans(Eigen::Vector3d(0, 0, 0)); mono_obs2[0.1] = calcMonoMeas(pf, sw_poses2.at(0.1)); stereo_obs2[0.1] = calcStereoMeas(pf, sw_poses2.at(0.1)); sw_poses2[0.2] = std::make_shared(); sw_poses2[0.2]->setValueLinearByQuat(Eigen::Quaterniond::FromTwoVectors(Eigen::Vector3d(0, 0, 1), Eigen::Vector3d(0, -1, 0))); sw_poses2[0.2]->setValueTrans(Eigen::Vector3d(0, 5, 0)); mono_obs2[0.2] = calcMonoMeas(pf, sw_poses2.at(0.2)); stereo_obs2[0.2] = calcStereoMeas(pf, sw_poses2.at(0.2)); sw_poses2[0.3] = std::make_shared(); sw_poses2[0.3]->setValueLinearByQuat(Eigen::Quaterniond::FromTwoVectors(Eigen::Vector3d(0, 0, 1), Eigen::Vector3d(0, 0, -1))); sw_poses2[0.3]->setValueTrans(Eigen::Vector3d(0, 0, 8)); mono_obs2[0.3] = calcMonoMeas(pf, sw_poses2.at(0.3)); stereo_obs2[0.3] = calcStereoMeas(pf, sw_poses2.at(0.3)); sw_poses2[0.4] = std::make_shared(); sw_poses2[0.4]->setValueLinearByQuat(Eigen::Quaterniond::FromTwoVectors(Eigen::Vector3d(0, 0, 1), Eigen::Vector3d(0, 1, 0))); sw_poses2[0.4]->setValueTrans(Eigen::Vector3d(0, -6, 0)); mono_obs2[0.4] = calcMonoMeas(pf, sw_poses2.at(0.4)); stereo_obs2[0.4] = calcStereoMeas(pf, sw_poses2.at(0.4)); sw_poses2[0.5] = std::make_shared(); sw_poses2[0.5]->setValueLinearByQuat(Eigen::Quaterniond::FromTwoVectors(Eigen::Vector3d(0, 0, 1), Eigen::Vector3d(-1, 0, 0))); sw_poses2[0.5]->setValueTrans(Eigen::Vector3d(5.5, 0, 0)); mono_obs2[0.5] = calcMonoMeas(pf, sw_poses2.at(0.5)); stereo_obs2[0.5] = calcStereoMeas(pf, sw_poses2.at(0.5)); sw_poses2[0.6] = std::make_shared(); sw_poses2[0.6]->setValueLinearByQuat(Eigen::Quaterniond::FromTwoVectors(Eigen::Vector3d(0, 0, 1), Eigen::Vector3d(1, 0, 0))); sw_poses2[0.6]->setValueTrans(Eigen::Vector3d(-10, 0, 0)); mono_obs2[0.6] = calcMonoMeas(pf, sw_poses2.at(0.6)); stereo_obs2[0.6] = calcStereoMeas(pf, sw_poses2.at(0.6)); } virtual ~TestTriangulator() {} protected: Eigen::Vector3d pf; Eigen::Isometry3d T_cl2cr; std::map> sw_poses1, sw_poses2; std::map> mono_obs1, mono_obs2; std::map> stereo_obs1, stereo_obs2; std::shared_ptr calcMonoMeas(const Eigen::Vector3d& pf, const std::shared_ptr pose) { std::shared_ptr mono_meas(new MonoMeas()); Eigen::Vector3d body = pose->copyValueAsIso().inverse()*pf; mono_meas->_u0 = body.x()/body.z() + generateGaussRandom(0.0, 0.02); mono_meas->_v0 = body.y()/body.z() + generateGaussRandom(0.0, 0.02); return mono_meas; } std::shared_ptr calcStereoMeas(const Eigen::Vector3d& pf, const std::shared_ptr pose) { std::shared_ptr stereo_meas(new StereoMeas()); Eigen::Vector3d body_left = pose->copyValueAsIso().inverse()*pf; Eigen::Vector3d body_right = T_cl2cr*pose->copyValueAsIso().inverse()*pf; stereo_meas->_u0 = body_left.x()/body_left.z() + generateGaussRandom(0.0, 0.02); stereo_meas->_v0 = body_left.y()/body_left.z() + generateGaussRandom(0.0, 0.02); stereo_meas->_u1 = body_right.x()/body_right.z() + generateGaussRandom(0.0, 0.02); stereo_meas->_v1 = body_right.y()/body_right.z() + generateGaussRandom(0.0, 0.02); // std::cout << " u0 = " << stereo_meas->_u0 << " v0 = " << stereo_meas->_v0 << " u1 = " << stereo_meas->_u1 << " v1 = " << stereo_meas->_v1 << std::endl; return stereo_meas; } }; TEST_F(TestTriangulator, monoTriangulate) { std::shared_ptr tri(new Triangulator()); Eigen::Vector3d result; bool flag = tri->triangulateMonoObs(mono_obs1, sw_poses1, result); std::cout << "pf ref = " << pf.transpose() << std::endl; std::cout << "mono triangulation result = " << result.transpose() << std::endl; ASSERT_NEAR((pf-result).norm(), 0.0, 0.05); ASSERT_TRUE(flag); flag = tri->triangulateMonoObs(mono_obs2, sw_poses2, result); std::cout << "pf ref = " << pf.transpose() << std::endl; std::cout << "mono triangulation result = " << result.transpose() << std::endl; ASSERT_NEAR((pf-result).norm(), 0.0, 0.15); ASSERT_TRUE(flag); } TEST_F(TestTriangulator, stereoTriangulate) { std::shared_ptr tri(new Triangulator()); Eigen::Vector3d result; bool flag = tri->triangulateStereoObs(stereo_obs1, sw_poses1, T_cl2cr, result); std::cout << "pf ref = " << pf.transpose() << std::endl; std::cout << "stereo triangulation result = " << result.transpose() << std::endl; ASSERT_NEAR((pf-result).norm(), 0.0, 0.05); ASSERT_TRUE(flag); flag = tri->triangulateStereoObs(stereo_obs2, sw_poses2, T_cl2cr, result); std::cout << "pf ref = " << pf.transpose() << std::endl; std::cout << "stereo triangulation result = " << result.transpose() << std::endl; ASSERT_NEAR((pf-result).norm(), 0.0, 0.15); ASSERT_TRUE(flag); } } int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); }